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