function [dp]=dV(q)
    dp = q/(q(1)^2+q(2)^2+q(3)^2)^(1.5)        
endfunction
function Om=Omega(q)
    Om=[0,-1,0;1,0,0;0,0,0]
endfunction
function e=energie(q,p)
    e=0.5*(p(1)^2+p(2)^2+p(3)^2)-1/(q(1)^2+q(2)^2+q(3)^2)^0.5
endfunction
function [q,p]=pasVerletMag(dV,Omega,h,qn,pn)
    pi=pn-0.5*h*dV(qn);
    Om=Omega(qn);
    Om2=Om*Om;
    w=(Om(1,2)^2+Om(1,3)^2+Om(2,3)^2)^0.5;
    Id=[1,0,0;0,1,0;0,0,1];
    F=Id*h+(1-cos(w*h))/w^2*Om-(sin(w*h)-w*h)/w^3*Om2;
    q=qn+F*pi;
    E=Id+sin(w*h)/w*Om+2*(sin(w*h*0.5)/w)^2*Om2;
    p=E*pi-0.5*h*dV(q);
endfunction
function [mq,mp,e,t] = verletMag(q0,p0,T,n1,n2,dV,Omega,energie)
    h=T/(n1*n2);
    mq=zeros(3,n2+1);
    mp=zeros(3,n2+1);
    e=zeros(1,n2+1);
    t=zeros(1,n2+1);
    mq(:,1)=q0;
    mp(:,1)=p0;
    e(:,1)=energie(q0,p0);
    q=q0; 
    p=p0;
    t(1)=0;
    for k2=1:n2,
        for k1=1:n1,
            [q,p]=pasVerletMag(dV,Omega,h,q,p);
        end;
        mq(:,k2+1)=q;
        mp(:,k2+1)=p;
        e(:,k2+1)=energie(q,p);
        t(k2+1)=t(k2)+n1*h;
    end;
endfunction

function [dp]=dV(q)
    dp =  q/(q(1)^2+q(2)^2+q(3)^2)^(1.5)
endfunction
function Om=Omega(q)
    Om=[0,-1,0;1,0,0;0,0,0]
endfunction
function e=energie(q,p)
    e=0.5*(p(1)^2+p(2)^2+p(3)^2)-1/(q(1)^2+q(2)^2+q(3)^2)^0.5
endfunction
q0=[1;1;1];
p0=[0;0;0];
T=100;
n1=10;
n2=1000;
[mq,mp,e,t]=verletMag(q0,p0,T,n1,n2,dV,Omega,energie);
                
figA=scf();
plot2d(mq(1,:),mq(2,:))
xtitle('','x','y')
figB=scf();
plot2d(t,e)
xtitle('','t','E')
