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 Force2BodySpringDamp::apply(double t) { // pt1 on body body 1 & pt2 on body 2 Vect3 pt1Pos = m_body1->getPosition(m_body1Offset); Vect3 pt1Vel = m_body1->getVelocityGlobal(m_body1Offset); Vect3 pt2Pos = m_body2->getPosition(m_body2Offset); Vect3 pt2Vel = m_body2->getVelocityGlobal(m_body2Offset); // distance between 2 points in global Vect3 dist = pt2Pos-pt1Pos; // Relative Velocity Vect3 revVel = pt2Vel - pt1Vel; // just to have meaningfull names Vect3 normalForceVect = dist; //normailze normalForceVector in place and get magnitude double dist_mag = magnitude(normalForceVect); normalForceVect.normalize(); double springForceMag = (dist_mag-m_fl)*m_k; double velAlongSpring = normalForceVect.dot(revVel); double dampForceMag = velAlongSpring * m_c; double totalForceMag = springForceMag + dampForceMag; Vect3 totalForceVect = normalForceVect * totalForceMag; m_body1->forceAccum(totalForceVect,false,m_body1Offset); m_body2->forceAccum(-totalForceVect,false,m_body2Offset); }
// time of closest approach double VectFuns::tau(const Vect3& s, const Vect3& vo, const Vect3& vi) { double rtn; Vect3 v = vo.Sub(vi); double nv = v.norm(); if (Util::almost_equals(nv,0.0)) { rtn = std::numeric_limits<double>::max(); // pseudo infinity } else rtn = -s.dot(v)/(nv*nv); return rtn; }// tau
MatX jointFrame(const Vect3 &loc, const Vect3 &axis_, const Vect3 &ref_) { Vect3 axis, ref, other; Mat3 R; axis.normalize(axis_); ref.displace(ref_, axis, - ref_.dot(axis)); // in case ref & axis not perp ref.normalize(ref); other.cross(axis, ref); R.setXcol(ref); R.setYcol(other); R.setZcol(axis); return MatX(R, loc); }
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)); }