let elementary_move_precise constraints =
let (m,b) = equations constraints in
let a = Matrix.solve m b in
let oldpos = Array.copy pos in
print_rotations a ;
for k = 0 to nb_bones - 1 do
let (i,j,d) = bones.(k) in
let (xi,yi,zi) = pos.(i) in
let (xj,yj,zj) = pos.(j) in
let old = angles.(k) in
let (theta,phi) = (mod2pi (old.theta +. a.(2*k)),
mod2pi (old.phi +. a.(2*k+1))) in
let (nxj,nyj,nzj) =
(xi +. d*.(sin phi)*.(cos theta),
yi +. d*.(sin phi)*.(sin theta),
zi +. d*.(cos phi)) in
pos.(j) <- (nxj,nyj,nzj) ;
angles.(k) <- {theta=theta;phi=phi} ;
Printf.printf
"dM_%d :\n\tEffectif :\t%f %f %f\n\tAnnonc\233 :\t%f %f %f\n%!"
j (nxj-.xj) (nyj-.yj) (nzj-.zj)
a.(2*nb_bones+3*j+0)
a.(2*nb_bones+3*j+1)
a.(2*nb_bones+3*j+2) ;
done