MATRIX3 SPHERE::springJacobian(const VEC3F& collisionPoint)
{
VEC3F direction = collisionPoint - _center;
Real dot = direction.dot(direction);
Real sqrtDot = sqrt(dot);
//Real invSqrtDot = 1.0 / sqrtDot;
MATRIX3 final;
final.setZero();
//////////////////////////////////////////////////////////////////////
// compute the tet volume
//////////////////////////////////////////////////////////////////////
Real TET::volume()
{
// formula for a tet volume with vertices (a,b,c,d) is:
// |(a - d) dot ((b - d) cross (c - d))| / 6
VEC3F a = (*vertices[1]) - (*vertices[0]);
VEC3F b = (*vertices[2]) - (*vertices[0]);
VEC3F c = (*vertices[3]) - (*vertices[0]);
return fabs(a.dot(b.cross(c)) / 6.0);
}
VEC3F SPHERE::force(const VEC3F& collisionPoint, const VEC3F& collisionVelocity)
{
VEC3F direction = collisionPoint - _center;
VEC3F normal = direction;
normal.normalize();
Real velocityDot = normal.dot(collisionVelocity);
VEC3F springForce = _collisionStiffness * (direction - (normal * _radius));
VEC3F dampingForce = _collisionDamping * normal * velocityDot;
return springForce + dampingForce;
}