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() );
}