//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
cVector3d toolProjection = a_toolPos;
toolProjection.z(0.0);
m_interactionNormal.set(0,0,1);
// search for the nearest point on the torus medial axis
if (a_toolPos.lengthsq() > C_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));
// compute eventual penetration of tool inside the torus
cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);
double distance = vectTorusTool.length();
// normal
if (distance > 0.0)
{
m_interactionNormal = vectTorusTool;
m_interactionNormal.normalize();
}
// tool is located inside the torus
if ((distance < m_innerRadius) && (distance > 0.001))
{
m_interactionInside = true;
}
// tool is located outside the torus
else
{
m_interactionInside = false;
}
// compute surface point
double dist = vectTorusTool.length();
if (dist > 0)
{
vectTorusTool.mul(1/dist);
}
vectTorusTool.mul(m_innerRadius);
pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
}
else
{
m_interactionInside = false;
m_interactionPoint = a_toolPos;
}
}
//===========================================================================
cVector3d cShapeTorus::computeLocalForce(const cVector3d& a_localPosition)
{
// In the following we compute the reaction forces between the tool and the
// sphere.
cVector3d localForce;
// project pointer on torus plane (z=0)
cVector3d fingerProjection = a_localPosition;
fingerProjection.z = 0;
// search for the nearest point on the torus medial axis
if (a_localPosition.lengthsq() > CHAI_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(fingerProjection));
// compute eventual penetration of finger inside the torus
cVector3d vectTorusFinger = cSub(a_localPosition, pointAxisTorus);
double distance = vectTorusFinger.length();
// finger inside torus, compute forces
if ((distance < m_innerRadius) && (distance > 0.001))
{
localForce = cMul((m_innerRadius - distance) * (m_material.getStiffness()), cNormalize(vectTorusFinger));
}
// finger is outside torus
else
{
localForce.zero();
}
}
else
{
localForce.zero();
}
return (localForce);
}
