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