include/crbn/basic/quat.hpp

#ifndef __quat_hpp__
#define __quat_hpp__

#include <crbn/basic/scalar.hpp>
#include <crbn/basic/vec3.hpp>

struct quat {
  union {
    float data[ 4 ];
    struct { float x, y, z, w; };
  };

  quat() : x(0.0), y(0.0), z(0.0), w(1.0) {}
  quat( const float& i, const float& r = 1.0f ) :
    x(i), y(i), z(i), w(r)
  {}
  quat( const float& a, const float& b, const float& c, const float& d ) :
    x(a), y(b), z(c), w(d)
  {}
  quat( const quat& q ) : x(q.w), y(q.y), z(q.z), w(q.w) {}
  // Rotation about unit vector v by an angle t
  quat( const vec3& v, const float& t ) {
    float s = sin(t * 0.5);
    x = s * v.x;
    y = s * v.y;
    z = s * v.z;
    w = cos(t * 0.5);
  }
};

// a = b + c
inline void qadd( quat& a, const quat& b, const quat& c ) {
  a.x = b.x + c.x;
  a.y = b.y + c.y;
  a.z = b.z + c.z;
  a.w = b.w + c.w;
}
// a = a + b
inline void qadd( quat& a, const quat& b ) {
  a.x += b.x;
  a.y += b.y;
  a.z += b.z;
  a.w += b.w;
}

// a = b - c
inline void qsub( quat& a, const quat& b, const quat& c ) {
  a.x = b.x - c.x;
  a.y = b.y - c.y;
  a.z = b.z - c.z;
  a.w = b.w - c.w;
}
// a = a - b
inline void qsub( quat& a, const quat& b ) {
  a.x -= b.x;
  a.y -= b.y;
  a.z -= b.z;
  a.w -= b.w;
}

// a = conjugate( b )
inline void qconj( quat& a, const quat& b ) {
  a.x = -b.x;
  a.y = -b.y;
  a.z = -b.z;
  a.w = b.w;
}
// a = conjugate( a )
inline void qconj( quat& a ) {
  a.x = -a.x;
  a.y = -a.y;
  a.z = -a.z;
}

// a = f * b = b * f
inline void qmul( quat& a, const float f, const quat& b ) {
  a.x = f * b.x;
  a.y = f * b.y;
  a.z = f * b.z;
  a.w = f * b.w;
}
inline void qmul( quat& a, const float f ) {
  a.x *= f;
  a.y *= f;
  a.z *= f;
  a.w *= f;
}

// a = b / f componentwise
inline void qdiv( quat& a, const quat& b, const float f ) {
  float d = 1.0f / f;
  a.x = b.x * d;
  a.y = b.y * d;
  a.z = b.z * d;
  a.w = b.w * d;
}
// a = a / f componentwise
inline void qdiv( quat& a, const float f ) {
  float d = 1.0f / f;
  a.x *= d;
  a.y *= d;
  a.z *= d;
  a.w *= d;
}

// a = b * c
inline void qmul( quat& a, const quat& b, const quat& c ) {
  float bx = b.x, by = b.y, bz = b.z, bw = b.w;
  float cx = c.x, cy = c.y, cz = c.z, cw = c.w;

  a.x = by * cz - bz * cy + cw * bx + bw * cx;
  a.y = bz * cx - bx * cz + cw * by + bw * cy;
  a.z = bx * cy - by * cx + cw * bz + bw * cz;
  a.w = bw * cw - bx * cx - by * cy - bz * cz;
}
// a = a * b
inline void qmul( quat& a, const quat& b ) {
  float bx = a.x, by = a.y, bz = a.z, bw = a.w;
  float cx = b.x, cy = b.y, cz = b.z, cw = b.w;

  a.x = by * cz - bz * cy + cw * bx + bw * cx;
  a.y = bz * cx - bx * cz + cw * by + bw * cy;
  a.z = bx * cy - by * cx + cw * bz + bw * cz;
  a.w = bw * cw - bx * cx - by * cy - bz * cz;
}

// a = b * b
inline void qsqr( quat& a, const quat& b ) {
  a.w = b.w * b.w - b.x * b.x - b.y * b.y - b.z * b.z;
  a.x = 2.0 * b.w * b.x;     
  a.y = 2.0 * b.w * b.y;     
  a.z = 2.0 * b.w * b.z;     
}

// a = a * a
inline void qsqr( quat& a ) {
  float w = a.w;
  a.w = w * w - a.x * a.x - a.y * a.y - a.z * a.z;
  a.x *= 2.0 * w;     
  a.y *= 2.0 * w;     
  a.z *= 2.0 * w;     
}

// ret = norm( a )
inline float qnorm( const quat& a ) {
  return sqr( a.x ) + sqr( a.y ) + sqr( a.z ) + sqr( a.w );
}

// ret = length( a )
inline float qnorm( const quat& a ) {
  return sqrt(sqr( a.x ) + sqr( a.y ) + sqr( a.z ) + sqr( a.w ));
}

// ret = dot product of a & b
inline float qdot( const quat& a, const quat& b ) {
  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

inline void qinverse( quat& a, const quat& b ) {
  float f = sqr( b.x ) + sqr( b.y ) + sqr( b.z ) + sqr( b.w );
  if( f < 1e-6f ) {
    f = -1.0f / f;
    a.x = b.x * f;
    a.y = b.y * f;
    a.z = b.z * f;
    a.w = b.w * -f;
  }
}
inline void qinverse( quat& a ) {
  float f = sqr( a.x ) + sqr( a.y ) + sqr( a.z ) + sqr( a.w );
  if( f < 1e-6f ) {
    f = -1.0f / f;
    a.x *= f;
    a.y *= f;
    a.z *= f;
    a.w *= -f;
  }
}

// a = b + f * c
inline void qaddfmul( quat& a, const quat& b, const float f, const quat& c ) {
  a.x = b.x + f * c.x;
  a.y = b.y + f * c.y;
  a.z = b.z + f * c.z;
  a.w = b.w + f * c.w;
}
// a += f * b
inline void qaddfmul( quat& a, const float f, const quat& b )
{
  a.x += f * b.x;
  a.y += f * b.y;
  a.z += f * b.z;
  a.w += f * b.w;
}

// spherical linear interpolation of 2 quaternion
inline void qslerp( quat& a, const float& f, const quat& b, const quat& c ) {
  float ps = qdot( b, c );
  float phi = (float)acos( (double)ps );
  float sphi = 1.0f / sinf( phi );

  qmul( a, sphi * sinf( (1.0f-f) * phi ), b );
  qaddfmul( a, sphi * sinf( phi * f ) , c );
}
// version 2 or slerp
inline void qslerp2( quat& a, const float& f, const quat& b, const quat& c ) {
  quat ib;
  qinverse( ib, b );
  qmul( ib, c );

  float theta = f * (float)acos( (double)ib.w );
  float l = sqr( ib.x ) + sqr( ib.y ) + sqr( ib.z );
  qmul( ib, sinf( theta ) / sqrtf( l ) );
  ib.w = cosf( theta );
  qmul( a, b, ib );
}

#endif // __quat_hpp__

---