------------------------------------------------------------------------------
--------------- SPATIAL ROTATION ABOUT AN ARBITRARY VECTOR -------------------
------------------------------------------------------------------------------
-- by Jean-Marc Ottorini  ottorini@nancy.inra.fr
-- Institut National de la Recherche Agronomique - Centre de Nancy

-- Definitions --
 
dot(u,v) = sum(u[i]*v[i],i,1,count(u))
norm(u)  = sqrt(dot(u, u))

cross(u,v) = {u[2]*v[3] - u[3]*v[2],
              u[3]*v[1] - u[1]*v[3],
              u[1]*v[2] - u[2]*v[1]}

tspos(A)[i,j] = A[j,i] dim[count(A[1]),countA]

mult(A,B)[i,j] = sum(A[i,k]*B[k,j],k,1,count(B))dim[count(A),count(B[1])]


-- Application --

-- Let u be a given vector

u = {1.0,2.0,2.0} 

-- let v be another vector such that {u, v} is linearly independant
-- (for instance, we may locate a component of u  with  maximal 
-- absolute value, and define v with 0  in place of this component, 
-- and 1 elsewhere)

v = {1.0,1.0,0.0} 

-- A two step Gram-Schmidt algorithm brings an orthonormal 
-- basis {e1,e2} :

e1 = u/norm(u)
e2 = (v-dot(e1,v)*e1)/norm(v-dot(e1,v)*e1)

-- Cross product of e1 and e2 completes {e1, e2} to an orthonormal
-- basis E = {e1,e2,e3} 

e3 = cross(e1,e2)

-- Let P be the transition matrix from basis E to canonical basis
 
P = tspos({e1,e2,e3})

-- P is orthogonal:

mult(tspos(P), P):{{1.000,0.000,0.000},{0.000,1.000,0.000},{0.000,0.000,1.000}}
mult(P, tspos(P)):{{1.000,0.000,0.000},{0.000,1.000,0.000},{0.000,0.000,1.000}}

-- That is,  tspos(P) is the inverse of P, thus it is the transition 
-- matrix from canonical basis to basis E  

-- Let t be a given angle:

t = 45

-- Let R0 be the matrix of rotation about u through t,
-- expressed in E coordinates:

R0 = {{1,     0,      0},
      {0,cos(t),-sin(t)},
      {0,sin(t),cos(t)}}

-- Then, the matrix of rotation about u expressed in canonical 
-- coordinates is R, such that:

R = mult(P,mult(R0,tspos(P)))

R:{{0.740,-0.406,0.536},{0.536,0.837,-0.106},{-0.406,0.366,0.837}}

-- Adequately, u is an invariant of the transformation:

u1 = {{u[1]}, {u[2]}, {u[3]}}

mult(R, u1):{{1.000},{2.000},{2.000}}

-- Five successive rotations about the y and the z-axis would have  
-- done the job, but this solution seems computationally better.
------------------------------------------------------------------------------

-------------------------  POLAR ROTATION SOLUTION ---------------------------

-- Let b be the angle between the projection of u on the (x, z) plane,
-- and a the angle between  u and the y-axis  (a and b are the  polar
-- rotation angles)

b = acos(u[1]/norm({u[1],u[3]}))*(u[3]/abs(u[3]))

a = acos(u[2]/norm(u))

-- Let R1 be the matrix of rotation through b about y axis
-- "   R2 "   "    "    "   "        "      a  "    z axis
-- "   R3 "   "    "    "   "        "      t  "    y axis
-- "   R4 "   "    "    "   "        "     -a  "    z axis
-- "   R5 "   "    "    "   "        "     -b  "    y axis

R1 = {{ cos(b), 0, sin(b)},
      {      0, 1,      0},
      {-sin(b), 0, cos(b)}}

R2 = {{cos(a), -sin(a), 0},
      {sin(a),  cos(a), 0},
      {     0,       0, 1}}

R3 = {{ cos(t), 0, sin(t)},
      {      0, 1,      0},
      {-sin(t), 0, cos(t)}}

R4 = {{cos(-a), -sin(-a), 0},
      {sin(-a),  cos(-a), 0},
      {      0,        0, 1}}

R5 = {{ cos(-b), 0, sin(-b)},
      {       0, 1,       0},
      {-sin(-b), 0, cos(-b)}}

-- As expected, R2R1u is located along the y axis and has
-- the same length (3) as u:

R2R1u = mult(R2,mult(R1,u1))

R2R1u:{{0.000},{3.000},{0.000}}

-- The rotation matrix we find is  newR such that:

newR = mult(R5, mult(R4, mult(R3, mult(R2, R1))))

newR:{{0.740,-0.406,0.536},{0.536,0.837,-0.106},{-0.406,0.366,0.837}}

-- It is the matrix that we already found:

R:{{0.740,-0.406,0.536},{0.536,0.837,-0.106},{-0.406,0.366,0.837}}

                       -------------------------------------

-- Useful references:

-- Howard Anton. "Elementary Linear Algebra". Second edition, 1977. 
--   John Wiley.
-- Chris Rorres and Howard Anton. "Applications of Linear Algebra".  
--   Second edition, 1979, John Wiley.