struct Bivector3 { float b01 = 0; float b02 = 0; float b12 = 0; Bivector3( float b01, float b02, float b12 ); }; Bivector3::Bivector3( float b01, float b02, float b12 ) : b01(b01), b02(b02), b12(b12) {} // Wedge product inline Bivector3 Wedge( const Vector3& u, const Vector3& v ) { Bivector3 ret( u[0]*v[1] - u[1]*v[0], // XY u[0]*v[2] - u[2]*v[0], // XZ u[1]*v[2] - u[2]*v[1] // YZ ); return ret; } // --------------------------------------------------------------------- struct Rotor3 { // scalar part float a = 1; // bivector part float b01 = 0; float b02 = 0; float b12 = 0; // default ctor Rotor3() {} Rotor3( float a, float b01, float b02, float b12 ); Rotor3( float a, const Bivector3& bv ); // construct the rotor that rotates one vector to another Rotor3( const Vector3& vFrom, const Vector3& vTo ); // angle+axis, or rather angle+plane Rotor3( const Bivector3& bvPlane, float angleRadian ); // rotate a vector by the rotor Vector3 rotate( const Vector3& v ) const; // multiply Rotor3 operator*( const Rotor3& r ) const; Rotor3 operator*=( const Rotor3& r ); Rotor3 rotate( const Rotor3& r ) const; // length utility Rotor3 reverse() const; // equivalent to conjugate float lengthsqrd() const; float length() const; void normalize(); Rotor3 normal() const; // convert to matrix Matrix3 toMatrix3() const; }; // default ctor Rotor3::Rotor3( float a, float b01, float b02, float b12 ) : a(a), b01(b01), b02(b02), b12(b12) {} Rotor3::Rotor3( float a, const Bivector3& bv ) : a(a), b01(bv.b01), b02(bv.b02), b12(bv.b12) {} // construct the rotor that rotates one unit vector to another // uses the usual trick to get the half angle Rotor3::Rotor3( const Vector3& vFrom, const Vector3& vTo ) { a = 1 + Dot( vTo, vFrom ); // the left side of the products have b a, not a b, so flip Bivector3 minusb = Wedge( vTo, vFrom ); b01 = minusb.b01; b02 = minusb.b02; b12 = minusb.b12; normalize(); } // angle+plane, plane must be normalized Rotor3::Rotor3( const Bivector3& bvPlane, float angleRadian ) { float sina = sin(angleRadian / 2.f); a = cos(angleRadian / 2.f); // the left side of the products have b a, not a b b01 = -sina * bvPlane.b01; b02 = -sina * bvPlane.b02; b12 = -sina * bvPlane.b12; } // Rotor3-Rotor3 product // non-optimized inline Rotor3 Rotor3::operator*( const Rotor3& q ) const { const Rotor3& p = *this; Rotor3 r; // here we just expanded the geometric product rules r.a = p.a * q.a - p.b01 * q.b01 - p.b02 * q.b02 - p.b12 * q.b12; r.b01 = p.b01 * q.a + p.a * q.b01 + p.b12 * q.b02 - p.b02 * q.b12; r.b02 = p.b02 * q.a + p.a * q.b02 - p.b12 * q.b01 + p.b01 * q.b12; r.b12 = p.b12 * q.a + p.a * q.b12 + p.b02 * q.b01 - p.b01 * q.b02; return r; } /// R x R* // non-optimized Vector3 Rotor3::rotate( const Vector3& x ) const { const Rotor3& p = *this; // q = P x Vector3 q; q[0] = p.a * x[0] + x[1] * p.b01 + x[2] * p.b02; q[1] = p.a * x[1] - x[0] * p.b01 + x[2] * p.b12; q[2] = p.a * x[2] - x[0] * p.b02 - x[1] * p.b12; float q012 = x[0] * p.b12 - x[1] * p.b02 + x[2] * p.b01; // trivector // r = q P* Vector3 r; r[0] = p.a * q[0] + q[1] * p.b01 + q[2] * p.b02 + q012 * p.b12; r[1] = p.a * q[1] - q[0] * p.b01 - q012 * p.b02 + q[2] * p.b12; r[2] = p.a * q[2] + q012 * p.b01 - q[0] * p.b02 - q[1] * p.b12; // trivector part of the result is always zero! return r; } // Rotor3-Rotor3 product inline Rotor3 Rotor3::operator*=( const Rotor3& r ) { (*this) = (*this) * r; return *this; } // rotate a rotor by another inline Rotor3 Rotor3::rotate( const Rotor3& r ) const { // should unwrap this for efficiency return (*this) * r * (*this).reverse(); } // Equivalent to conjugate inline Rotor3 Rotor3::reverse() const { return Rotor3( a, -b01, -b02, -b12 ); } // Length Squared inline float Rotor3::lengthsqrd() const { return Sqr(a) + Sqr(b01) + Sqr(b02) + Sqr(b12); } // Length inline float Rotor3::length() const { return sqrt( lengthsqrd() ); } // Normalize inline void Rotor3::normalize() { float l = length(); a /= l; b01 /= l; b02 /= l; b12 /= l; } // Normalized rotor inline Rotor3 Rotor3::normal() const { Rotor3 r = *this; r.normalize(); return r; } // convert to matrix // non-optimized inline Matrix3 Rotor3::toMatrix3() const { Vector3 v0 = rotate( Vector3(1,0,0) ); Vector3 v1 = rotate( Vector3(0,1,0) ); Vector3 v2 = rotate( Vector3(0,0,1) ); return Matrix3( v0, v1, v2 ); } // geometric product (for reference), produces twice the angle, negative direction inline Rotor3 Geo( const Vector3 & a, const Vector3 & b ) { return Rotor3( Dot(a,b), Wedge(a,b) ); }
// Cross product inline Vector3 Cross( const Vector3& u, const Vector3& v ) { Vector3 ret( u[1]*v[2] - u[2]*v[1], u[2]*v[0] - u[0]*v[2], u[0]*v[1] - u[1]*v[0] ); return ret; } // --------------------------------------------------------------------- struct Quaternion { // real part float a = 1; // imaginary part float v0 = 0; float v1 = 0; float v2 = 0; // default ctor Quaternion() {} Quaternion( float a, float v0, float v1, float v2 ); // construct the quaternion that rotates one vector to another Quaternion( const Vector3& vFrom, const Vector3& vTo ); // angle+axis, axis must be normalized Quaternion( float angleRadian, const Vector3& axis ); // rotate a vector by the quaternion Vector3 rotate( const Vector3& v ) const; // multiply Quaternion operator*( const Quaternion& q ) const; Quaternion operator*=( const Quaternion& q ); Quaternion rotate( const Quaternion& q ) const; // length utility Quaternion conjugate() const; float lengthsqrd() const; float length() const; void normalize(); Quaternion normal() const; // convert to matrix Matrix3 toMatrix3() const; }; // default ctor Quaternion::Quaternion( float a, float v0, float v1, float v2 ) : a(a), v0(v0), v1(v1), v2(v2) {} // construct the quaternion that rotates one unit vector to another // uses the usual trick to get the half angle Quaternion::Quaternion( const Vector3& vFrom, const Vector3& vTo ) { a = 1 + Dot( vTo, vFrom ); Vector3 vec = Cross( vFrom, vTo ); v0 = vec[0]; v1 = vec[1]; v2 = vec[2]; normalize(); } // angle+axis, axis must be normalized Quaternion::Quaternion( float angleRadian, const Vector3& axis ) { float sina = sin(angleRadian / 2.f); a = cos(angleRadian / 2.f); v0 = sina * axis[0]; v1 = sina * axis[1]; v2 = sina * axis[2]; } // Quaternion-Quaternion product // non-optimized inline Quaternion Quaternion::operator*( const Quaternion& q ) const { const Quaternion& p = *this; Quaternion r; // Quaternion formulas: // r.a = p.a * q.a - Dot(p.v,q.v); // r.v = p.a * q.v + q.a * p.v + Cross(p.v,q.v); r.a = p.a * q.a - p.v0 * q.v0 - p.v1 * q.v1 - p.v2 * q.v2; r.v0 = p.a * q.v0 + q.a * p.v0 + p.v1 * q.v2 - p.v2 * q.v1; r.v1 = p.a * q.v1 + q.a * p.v1 + p.v2 * q.v0 - p.v0 * q.v2; r.v2 = p.a * q.v2 + q.a * p.v2 + p.v0 * q.v1 - p.v1 * q.v0; return r; } /// Q (x,0) Q* // non-optimized Vector3 Quaternion::rotate( const Vector3& x ) const { const Quaternion& p = *this; // q = P (x,0) Quaternion q; q.v0 = p.a * x[0] + p.v1 * x[2] - p.v2 * x[1]; q.v1 = p.a * x[1] + p.v2 * x[0] - p.v0 * x[2]; q.v2 = p.a * x[2] + p.v0 * x[1] - p.v1 * x[0]; q.a = - p.v0 * x[0] - p.v1 * x[1] - p.v2 * x[2]; // r = q P* Vector3 r; r[0] = q.a * -p.v0 + p.a * q.v0 - q.v1 * p.v2 + q.v2 * p.v1; r[1] = q.a * -p.v1 + p.a * q.v1 - q.v2 * p.v0 + q.v0 * p.v2; r[2] = q.a * -p.v2 + p.a * q.v2 - q.v0 * p.v1 + q.v1 * p.v0; return r; } // Quaternion-Quaternion product inline Quaternion Quaternion::operator*=( const Quaternion& r ) { (*this) = (*this) * r; return *this; } // rotate a rotor by another inline Quaternion Quaternion::rotate( const Quaternion& r ) const { // should unwrap this for efficiency return (*this) * r * (*this).conjugate(); } // C inline Quaternion Quaternion::conjugate() const { return Quaternion( a, -v0, -v1, -v2 ); } // Length Squared inline float Quaternion::lengthsqrd() const { return Sqr(a) + Sqr(v0) + Sqr(v1) + Sqr(v2); } // Length inline float Quaternion::length() const { return sqrt( lengthsqrd() ); } // Normalize inline void Quaternion::normalize() { float l = length(); a /= l; v0 /= l; v1 /= l; v2 /= l; } // Normalized rotor inline Quaternion Quaternion::normal() const { Quaternion r = *this; r.normalize(); return r; } // convert to matrix // non-optimized inline Matrix3 Quaternion::toMatrix3() const { Vector3 v0 = rotate( Vector3(1,0,0) ); Vector3 v1 = rotate( Vector3(0,1,0) ); Vector3 v2 = rotate( Vector3(0,0,1) ); return Matrix3( v0, v1, v2 ); }