Point3F & QuatF::mulP(const Point3F& p, Point3F* r) { QuatF qq; QuatF qi = *this; QuatF qv( p.x, p.y, p.z, 0.0f); qi.inverse(); qq.mul(qi, qv); qv.mul(qq, *this); r->set(qv.x, qv.y, qv.z); return *r; }
F32 Rigid::getKineticEnergy() { Point3F w; QuatF qmat = angPosition; qmat.inverse(); qmat.mulP(angVelocity,&w); const F32* f = invObjectInertia; return 0.5f * ((mass * mDot(linVelocity,linVelocity)) + w.x * w.x / f[0] + w.y * w.y / f[5] + w.z * w.z / f[10]); }
Point3F& m_mul( const Point3F &p, const QuatF &q, Point3F *r ) { QuatF qq; QuatF qi = q; QuatF qv( p.x, p.y, p.z, 0.0f); qi.inverse(); m_mul(qi, qv, &qq ); m_mul(qq, q, &qv ); r->set(qv.x, qv.y, qv.z); return ( *r ); }
Point3F& m_mul( const Point3F &p, const TQuatF &q, Point3F *r ) { //rotate a point by a Quaternion QuatF a; QuatF i = q; QuatF v( p.x, p.y, p.z, 0.0f); i.inverse(); m_mul(i, v, &a ); m_mul(a, q, &v ); v.normalize(); r->set(v.x, v.y, v.z); *r += q.p; return ( *r ); }
QuatF& QuatF::operator /=( const QuatF & c ) { QuatF temp = c; return ( (*this) *= temp.inverse() ); }