//  Persistence of Vision Ray Tracer version 3.5 Include File
//  File: quaternions.inc
//  Description: Quaternion macros (primarily used for rotations)
//  Last updated: 2003.10.8
//  Created by: Alain Ducharme
//
//  Notes:
//    - Angles are in Radians, except for RotSimulXYZ(An)
//    - Might need to normalize quaternions more often if rounding errors creep
//      in because quaternions must be unit lenght for rotation operations to work
//
//  If you expand/fix/enhance, please let me know: Alain_Ducharme@hotmail.com
//

#ifndef(QUATERNIONS_INC_TEMP)
#declare QUATERNIONS_INC_TEMP = version;
#version 3.5;

#macro QToMatrix(Q)
  // Convert a quaternion to a Povray transformation matrix (4x3)
  // Use: matrix <M[0].x,M[0].y,M[0].z,M[1].x,M[1].y,M[1].z,M[2].x,M[2].y,M[2].z,M[3].x,M[3].y,M[3].z>
  #local X2 = Q.x + Q.x;
  #local Y2 = Q.y + Q.y;
  #local Z2 = Q.z + Q.z;
  #local XX = Q.x * X2;
  #local XY = Q.x * Y2;
  #local XZ = Q.x * Z2;
  #local YY = Q.y * Y2;
  #local YZ = Q.y * Z2;
  #local ZZ = Q.z * Z2;
  #local TX = Q.t * X2;
  #local TY = Q.t * Y2;
  #local TZ = Q.t * Z2;
  array[4] {<1.0 - (YY + ZZ),XY + TZ,XZ - TY>,<XY - TZ,1.0 - (XX + ZZ),YZ + TX>,<XZ + TY,YZ - TX,1.0 - (XX + YY)>,<0,0,0>}
#end

#macro QMatrix(Q)
  // Use quaternion directly as an object modifier
  #local M = QToMatrix(Q)
  transform { matrix <M[0].x,M[0].y,M[0].z,M[1].x,M[1].y,M[1].z,M[2].x,M[2].y,M[2].z,M[3].x,M[3].y,M[3].z> }
#end

#macro QFromMatrix(M)
  // Convert a Povray matrix (same array format as above) to a quaternion
  // Note: you should normalize the resulting quaternion if you want to use it for rotation
  #local TR = M[0].x+M[1].y+M[2].z+1;
  #if (TR>0)
    #local S=sqrt(TR)*2;
    #local R = <(M[1].z-M[2].y)/S,(M[2].x-M[0].z)/S,(M[0].y-M[1].x)/S,0.25*S>;
  #else
    #if (M[0].x > M[1].y & M[0].x > M[2].z) // Column 0
      #local S = sqrt(1+M[0].x-M[1].y-M[2].z) * 2;
      #local R = <0.25*S,(M[0].y+M[1].x)/S,(M[2].x+M[0].z)/S,(M[1].z-M[2].y)/S>;
    #else
      #if (M[1].y > M[2].z)                 // Column 1
        #local S = sqrt(1+M[1].y-M[0].x-M[2].z) * 2;
        #local R = <(M[0].y+M[1].x)/S,0.25*S,(M[1].z+M[2].y)/S,(M[2].x-M[0].z)/S>;
      #else                                 // Column 2
        #local S = sqrt(1+M[2].z-M[0].x-M[1].y) * 2;
        #local R = <(M[2].x+M[0].z)/S,(M[1].z+M[2].y)/S,0.25*S,(M[0].y-M[1].x)/S>;
      #end
    #end
  #end
  R      
#end

#macro Qsc(Q)
  // Square the quaternion components
  (Q.x*Q.x+Q.y*Q.y+Q.z*Q.z+Q.t*Q.t)
#end  

#macro QMagnitude(Q)
  // Magnitude of quaternion
  sqrt(Qsc(Q))
#end

#macro QNormalize(Q)
  // Normalize quaternion
  #local M = QMagnitude(Q);
  <Q.x/M,Q.y/M,Q.z/M,Q.t/M>
#end

#macro QInverse(Q)
  // Q^-1
  <-Q.x,-Q.y,-Q.z,Q.t>
#end

#macro QMultiply(Qa, Qb)
  // qa * qb (can effectively be used to add two rotations)
  <Qa.x*Qb.t + Qa.t*Qb.x + Qa.y*Qb.z - Qa.z*Qb.y,
   Qa.y*Qb.t + Qa.t*Qb.y + Qa.z*Qb.x - Qa.x*Qb.z,
   Qa.z*Qb.t + Qa.t*Qb.z + Qa.x*Qb.y - Qa.y*Qb.x,
   Qa.t*Qb.t - Qa.x*Qb.x - Qa.y*Qb.y - Qa.z*Qb.z>
#end

#macro QRotate(Ax,An)
  // Returns a quaternion that represents a rotation around an axis at specified angle
  // linear interpolation from origin: pass An*i where i is between 0 and 1
  #local Ax = vnormalize(Ax);
  <Ax.x*sin(An/2),Ax.y*sin(An/2),Ax.z*sin(An/2),cos(An/2)>
#end

#macro QAxAn(Q,An)
  // Return the rotation axis and angle (in parameter) of a quaternion
  #declare An = acos(Q.t)*2; 
  #if (An)
    #local SA = sqrt(1-Q.t*Q.t);
    #local R = <Q.x/SA,Q.y/SA,Q.z/SA>;
  #else
    #local R = x;  // No rotation, I prefer to return x
  #end
  R  
#end

#macro VQRotate(Vec,Q)
  // Rotate a vector with a quaternion
  #local P = <Vec.x,Vec.y,Vec.z,0>;
  #local R = QMultiply(QMultiply(Q,P),QInverse(Q));
  <R.x,R.y,R.z>
#end

/* Use Pov's built-in vaxis_rotate(), it's much faster
#macro VQARotate(Vec, Ax, An)
  // Use quaternion to rotate a vector around an axis at specified angle
  #local Q = QRotate(Ax,An);
  VRotateQ(Vec,Q)
#end */

#macro QDiff(Qa, Qb)
  // In effect returns the quaternion required to go from qa to qb
  #local R = QMultiply(Qb,QInverse(Qa));
  #if (R.t < 0)  // Make sure we take the shortest route...
    #local R = -R;
  #end
  R
#end

#macro QADiff(Qa, Qb)
  // Returns the angle difference between two quaternians
  #local An = 0;
  #local Ax = QAxAn(QDiff(Qa, Qb),An);
  An
#end

#macro EulerToQ(An)
  // Rotate three (xyz in a 3D vector) Euler angles into a quaternion
  // Note: Like a regular rotate x,y,z : can suffer from Gimbal Lock
  #local cr = cos(An.x/2);
  #local cp = cos(An.y/2);
  #local cy = cos(An.z/2);
  #local sr = sin(An.x/2);
  #local sp = sin(An.y/2);
  #local sy = sin(An.z/2);
  #local cpcy = cp * cy;
  #local spsy = sp * sy;
  <sr * cpcy - cr * spsy,cr * sp * cy + sr * cp * sy,cr * cp * sy - sr * sp * cy, cr * cpcy + sr * spsy>
#end

#macro QToEuler(Q)
  // Quaternion to Euler angles
  // FAILS with difficult situations (Euler problems), this needs work to avoid errors...
  #local Q2 = Q*Q;
  <atan2 (2*(Q.y*Q.z+Q.x*Q.t), (-Q2.x-Q2.y+Q2.z+Q2.t)),
   asin (-2*(Q.x*Q.z-Q.y*Q.t)),
   atan2 (2*(Q.x*Q.y+Q.z*Q.t), (Q2.x-Q2.y-Q2.z+Q2.t))>
#end

#macro Qln(Q)
  // ln(Q)
  #local R = sqrt(Q.x*Q.x+Q.y*Q.y+Q.z*Q.z);
  #if (R>0)
    #local AT = atan2(R,Q.t)/R;
  #else
    #local AT = 0;
  #end    
  <AT*Q.x,AT*Q.y,AT*Q.z,0.5*log(Qsc(Q))>
#end

#macro QExp(Q)
  // e^Q
  #local R = sqrt(Q.x*Q.x+Q.y*Q.y+Q.z*Q.z);
  #local ET = exp(Q.t);
  #if (R>0)
    #local S = ET*sin(R)/R;
  #else
    #local S = 0;
  #end
  <S*Q.x,S*Q.y,S*Q.z,ET*cos(R)>
#end

#macro QLinear(Qa,Qb,I)
  // Linear interpolation from Qa to Qb (I = 0 to 1)
  QExp((1-I)*Qln(Qa)+I*Qln(Qb))
#end

#macro QHermite(Qa,Qb,Qat,Qbt,I)
  // Hermite interpolation from qa to qb using tangents qat & qbt (I = 0 to 1)
  QExp((+2*I*I*I-3*I*I+1)*Qln(Qa)+
       (-2*I*I*I+3*I*I)*Qln(Qb)+
       (+1*I*I*I-2*I*I +I)*Qln(Qat)+
       (+1*I*I*I-1*I*I)*Qln(Qbt))
#end

#macro QBezier(Qa,Qb,Qc,Qd,I)
  // Bezier interpolation from qa to qd (I = 0 to 1)
  QExp((-1*I*I*I+3*I*I-3*I+1)*Qln(Qa)+
       (+3*I*I*I-6*I*I+3*I)*Qln(Qb)+
       (-3*I*I*I+3*I*I)*Qln(Qc)+
       (+1*I*I*I)*Qln(Qd))
#end

// Example of rotation transform useable as object modifier

#macro RotSimulXYZ(An)
  // Rotate simultaneously around XYZ at specified XYZ Euler angles (in Degrees here!)
  // Note: does not suffer from Gimbal Lock...
  // But rotations add up, they are not independant, think of a rolling sphere
  #local An = <radians(An.x),radians(An.y),radians(An.z)>;
  #local RL = sqrt(An.x*An.x+An.y*An.y+An.z*An.z);
  #if (RL) // If Zero, no rotation
    #local RA = RL * 0.5;
    #local S = sin(RA)/RL;
    #local Q = <An.x*S,An.y*S,An.z*S,cos(RA)>;
    QMatrix(Q)
  #end  
#end

#version QUATERNIONS_INC_TEMP;
#end //quaternions.inc