void BodyRigid::forceAndTorqueAccum(Vect3 const &f, bool forceInLocal, Vect3 const &forceAppLocal, Vect3 const &t, bool torqueInLocal)
{
Vect3 force; Vect3 torque;
if(forceInLocal)
{
force = m_q*f;
torque = forceAppLocal.cross(f);
}
else
{
force = f;
torque = forceAppLocal.cross(m_q.inverse()*f);
}
if(torqueInLocal)
{
torque += t;
}
else
{
torque += m_q.inverse()*t;
}
m_appliedForce += force;
m_appliedTorque += torque;
}
static Vect3 equator_map_inv(const Vect3& ref, const Vect3& p) {
Vect3 xmult = ref.Hat();
Vect3 ymult = vect3_orthog_toy(ref).Hat();
Vect3 zmult = ref.cross(vect3_orthog_toy(ref)).Hat();
Vect3 xmultInv = Vect3(xmult.x, ymult.x, zmult.x);
Vect3 ymultInv = Vect3(xmult.y, ymult.y, zmult.y);
Vect3 zmultInv = Vect3(xmult.z, ymult.z, zmult.z);
return Vect3(xmultInv.dot(p), ymultInv.dot(p), zmultInv.dot(p));
}
void BodyRigid::forceAccumGlobal(Vect3 const &globalForce, Vect3 const &globalPnt)
{
Vect3 fLocal = m_q.inverse() * globalForce;
Vect3 pntLocal = m_q.inverse() * (globalPnt-m_pos);
Vect3 tLocal = pntLocal.cross(fLocal);
//std::cout << "dfadsfads" << std::endl;
m_appliedForce += globalForce;
m_appliedTorque += tLocal;
}
static Vect3 equator_map(const Vect3& ref, const Vect3& p) {
Vect3 xmult = ref.Hat();
Vect3 ymult = vect3_orthog_toy(ref).Hat();
Vect3 zmult = ref.cross(vect3_orthog_toy(ref)).Hat();
return Vect3(xmult.dot(p), ymult.dot(p), zmult.dot(p));
}
