Handedness

x = cross(z,up);
y = cross(z,x);
 


Vec cross ( const Vec& v1, const Vec& v2 )
 {
   return Vec ( v1.y*v2.z - v1.z*v2.y,
                v1.z*v2.x - v1.x*v2.z,
                v1.x*v2.y - v1.y*v2.x  );
 }

# include <math.h>
# include "math.h"

const float srpi = 3.14159265f;
const float srtiny = 1.0E-6f;
# define SRNEXTZ(a,eps) ( (a)>-(eps) && (a)<(eps) )

float Vec::len () const
 {
   float f = x*x + y*y + z*z;
   if ( f>0 ) f = sqrtf ( f );
   return f;
 }

float Vec::len ( float n )
 {
   float f = sqrtf (x*x + y*y + z*z);
   if ( f>0 ) { n/=f; x*=n; y*=n; z*=n; }
   return f;
 }

Vec cross ( const Vec& v1, const Vec& v2 )
 {
   return Vec ( v1.y*v2.z - v1.z*v2.y,
                v1.z*v2.x - v1.x*v2.z,
                v1.x*v2.y - v1.y*v2.x  );
 }

void Quat::rot ( const Vec& axis, float radians )
 { 
   float f;
   
   // normalize axis:
   x=axis.x; y=axis.y; z=axis.z;
   f = x*x + y*y + z*z;
   if ( f>0 )
    { f = sqrtf ( f );
      x/=f; y/=f; z/=f;
    }
    
   // set the quaternion:
   radians/=2;
   f = sinf ( radians );
   x*=f; y*=f; z*=f;
   w = cosf ( radians );
 }

// above ok jacky

Quat operator * ( const Quat &q1, const Quat &q2 )
 {
   Quat q;

   q.w = (q1.w*q2.w) - (q1.x*q2.x + q1.y*q2.y + q1.z*q2.z); // w1*w2-dot(v1,v2)
   q.x = q1.y*q2.z - q1.z*q2.y; // cross (q1.v,q2.v)
   q.y = q1.z*q2.x - q1.x*q2.z;
   q.z = q1.x*q2.y - q1.y*q2.x;
   q.x += (q1.x*q2.w) + (q2.x*q1.w);
   q.y += (q1.y*q2.w) + (q2.y*q1.w);
   q.z += (q1.z*q2.w) + (q2.z*q1.w);

   return q;
 }

void quat_to_ts ( const Quat& q, float& tw, float& sx, float& sy )
 {
   // First test if the swing is in the singularity:
   if ( SRNEXTZ(q.z,srtiny) && SRNEXTZ(q.w,srtiny) ) { sx=sy=srpi; tw=0; return; }

   // Decompose q into twist-swing
   // by solving the equation Qtwist(t*2) * Qswing(s*2) = q
   // note: (x,y) is the normalized swing axis (x*x+y*y=1)
   // ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
   // ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )  (1)
   // From (1) CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
   // The swing rotation/2 s comes from:
   // From (1) (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>  Cs = sqrt ( qw^2 + qz^2 ) (3)
   // From (1) (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 => Ss = sqrt ( qx^2 + qy^2 ) (4)
   // From (1) : |SsCt -StSs| |x| = |qx|
   //            |StSs +SsCt| |y|   |qy| (5)

   double qw, qx, qy, qz;
   if ( q.w<0 )
    { qw=-q.w; qx=-q.x; qy=-q.y; qz=-q.z; }
   else
    { qw=q.w; qx=q.x; qy=q.y; qz=q.z; }
      
   double t = atan2 ( qz, qw ); // from (2)
   double s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3) and (4)

   double x=0, y=0, sins=sin(s);

   if ( !SRNEXTZ(sins,srtiny) )
    { double sint = sin(t);
      double cost = cos(t);

      // by solving the linear system in (5):
      y = (-qx*sint + qy*cost)/sins;
      x = ( qx*cost + qy*sint)/sins;
    }

   tw = float(2.0*t);
   sx = float(x*2.0*s);
   sy = float(y*2.0*s);
 }

void quat_to_st ( const Quat& q, float& sx, float& sy, float& tw )
 {
   // Decompose q into swing-twist using a similar development as in
   // function quat_to_ts
   if ( SRNEXTZ(q.z,srtiny) && SRNEXTZ(q.w,srtiny) ) { sx=sy=srpi; tw=0; }

   double qw, qx, qy, qz;
   if ( q.w<0 )
    { qw=-q.w; qx=-q.x; qy=-q.y; qz=-q.z; }
   else
    { qw=q.w; qx=q.x; qy=q.y; qz=q.z; }

   // Get the twist t:
   double t = 2.0 * atan2(qz,qw);

   double bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
   double gam = t/2.0;
   double sinc = SRNEXTZ(bet,srtiny)? 1.0 : sin(bet)/bet;
   double singam = sin(gam);
   double cosgam = cos(gam);

   sx = float( (2.0/sinc) * (cosgam*qx - singam*qy) );
   sy = float( (2.0/sinc) * (singam*qx + cosgam*qy) );
   tw = float( t );
 }

Vec operator * ( const Vec &v, const Quat &q )
 {
   Quat qv ( 0, v.x, v.y, v.z );
   qv = q * qv * q.inverse();
   return Vec ( qv.x, qv.y, qv.z );
 }

void Mat::set ( const Quat& q, const Vec& v )
 {
   // convert quaternion to rotation matrix:
   float x2  = q.x+q.x;
   float x2x = x2*q.x;
   float x2y = x2*q.y;
   float x2z = x2*q.z;
   float x2w = x2*q.w;
   float y2  = q.y+q.y;
   float y2y = y2*q.y;
   float y2z = y2*q.z;
   float y2w = y2*q.w;
   float z2  = q.z+q.z;
   float z2z = z2*q.z;
   float z2w = z2*q.w;

   e[0] = 1.0f - y2y - z2z; e[1] = x2y + z2w;        e[2]  = x2z - y2w;        e[3]=0;
   e[4] = x2y - z2w;        e[5] = 1.0f - x2x - z2z; e[6]  = y2z + x2w;        e[7]=0;
   e[8] = x2z + y2w;        e[9] = y2z - x2w;        e[10] = 1.0f - x2x - y2y; e[11]=0;

   // now update translation:
   e[12] = v.x;
   e[13] = v.y;
   e[14] = v.z;
   e[15] = 1.0f;
 }

void Mat::lookat ( const Vec& eye, const Vec& center, const Vec& up )
 {
   Vec z = eye-center; z.normalize();
   Vec x = cross ( up, z );  // Jacky for left-handed system, Vec x = cross(z,up);
   Vec y = cross ( z,  x );  // Jacky this is correct for both systems

   x.normalize();
   y.normalize();

   e[0]=x.x; e[1]=y.x; e[2]=z.x; e[3]=0;  // jacky need transpose? it's a column vector where dx is row based
   e[4]=x.y; e[5]=y.y; e[6]=z.y; e[7]=0;
   e[8]=x.z; e[9]=y.z; e[10]=z.z; e[11]=0;
   e[15]=1.0f;

   Vec v = eye*-1.0f;
   // left combine translation:
   e[12] = v.x*e[0] + v.y*e[4] + v.z*e[8];
   e[13] = v.x*e[1] + v.y*e[5] + v.z*e[9];
   e[14] = v.x*e[2] + v.y*e[6] + v.z*e[10];
 }

# define E11 e[0]
# define E12 e[1]
# define E13 e[2]
# define E14 e[3]
# define E21 e[4]
# define E22 e[5]
# define E23 e[6]
# define E24 e[7]
# define E31 e[8]
# define E32 e[9]
# define E33 e[10]
# define E34 e[11]
# define E41 e[12]
# define E42 e[13]
# define E43 e[14]
# define E44 e[15]

void Mat::inverse ( Mat &inv ) const
 {
   float m12 = E21*E32 - E22*E31;
   float m13 = E21*E33 - E23*E31;
   float m14 = E21*E34 - E24*E31;
   float m23 = E22*E33 - E23*E32;
   float m24 = E22*E34 - E24*E32;
   float m34 = E23*E34 - E24*E33;

   float d1 = E12*m34 - E13*m24 + E14*m23;
   float d2 = E11*m34 - E13*m14 + E14*m13;
   float d3 = E11*m24 - E12*m14 + E14*m12;
   float d4 = E11*m23 - E12*m13 + E13*m12;
   float d = -E41*d1 + E42*d2 - E43*d3 + E44*d4;
   if (d==0.0) return;
   d = 1.0f/d;

   inv.E11 = (E42*m34 - E43*m24 + E44*m23) * d;
   inv.E21 = (E43*m14 - E41*m34 - E44*m13) * d;
   inv.E31 = (E41*m24 - E42*m14 + E44*m12) * d;
   inv.E41 = (E42*m13 - E41*m23 - E43*m12) * d;
   inv.E14 = (E13*m24 - E12*m34 - E14*m23) * d;
   inv.E24 = (E11*m34 - E13*m14 + E14*m13) * d;
   inv.E34 = (E12*m14 - E11*m24 - E14*m12) * d;
   inv.E44 = (E11*m23 - E12*m13 + E13*m12) * d;

   m12 = E11*E42 - E12*E41;
   m13 = E11*E43 - E13*E41;
   m14 = E11*E44 - E14*E41;
   m23 = E12*E43 - E13*E42;
   m24 = E12*E44 - E14*E42;
   m34 = E13*E44 - E14*E43;
   inv.E12 = (E32*m34 - E33*m24 + E34*m23) * d;
   inv.E22 = (E33*m14 - E31*m34 - E34*m13) * d;
   inv.E32 = (E31*m24 - E32*m14 + E34*m12) * d;
   inv.E42 = (E32*m13 - E31*m23 - E33*m12) * d;
   inv.E13 = (E23*m24 - E22*m34 - E24*m23) * d;
   inv.E23 = (E21*m34 - E23*m14 + E24*m13) * d;
   inv.E33 = (E22*m14 - E21*m24 - E24*m12) * d;
   inv.E43 = (E21*m23 - E22*m13 + E23*m12) * d;
 }

Vec operator * ( const Vec &v, const Mat &m )
 {
   Vec r ( m.E11*v.x + m.E21*v.y + m.E31*v.z + m.E41,
             m.E12*v.x + m.E22*v.y + m.E32*v.z + m.E42,
             m.E13*v.x + m.E23*v.y + m.E33*v.z + m.E43  );

   float w = m.E14*v.x + m.E24*v.y + m.E34*v.z + m.E44;
   if ( w!=0.0 && w!=1.0 ) { r.x/=w; r.y/=w; r.z/=w; }
   return r;
 }

Vec operator * ( const Mat &m, const Vec &v )
 {
   Vec r ( m.E11*v.x + m.E12*v.y + m.E13*v.z + m.E14,
             m.E21*v.x + m.E22*v.y + m.E23*v.z + m.E24,
             m.E31*v.x + m.E32*v.y + m.E33*v.z + m.E34  );

   float w = m.E41*v.x + m.E42*v.y + m.E43*v.z + m.E44;
   if ( w!=0.0 && w!=1.0 ) { r.x/=w; r.y/=w; r.z/=w; }
   return r;
 }