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