//===========================================================================
bool cProxyPointForceAlgo::goalAchieved(const cVector3d& a_proxy, const cVector3d& a_goal) const
{
if (m_useDynamicProxy)
{
return (!(a_proxy.distance(a_goal) > 0.0));
}
else
{
return (a_proxy.distance(a_goal) < (m_epsilonBaseValue));
}
}
//===========================================================================
void cProxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal,
const cVector3d& a_proxy,
cVector3d& a_normal,
cGenericObject* a_parent)
{
// check if friction is enabled
if (m_useFriction == false)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// Compute penetration depth; how far is the device "behind" the
// plane of the obstructing surface
cVector3d projectedGoal = cProjectPointOnPlane(m_deviceGlobalPos, a_proxy, a_normal);
double penetrationDepth = cSub(m_deviceGlobalPos,projectedGoal).length();
// Find the appropriate friction coefficient
// Our dynamic and static coefficients...
cMesh* parent_mesh = dynamic_cast<cMesh*>(a_parent);
// Right now we can only work with cMesh's
if (parent_mesh == NULL)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
double mud = parent_mesh->m_material.getDynamicFriction();
double mus = parent_mesh->m_material.getStaticFriction();
// No friction; don't try to compute friction cones
if ((mud == 0) && (mus == 0))
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// The corresponding friction cone radii
double atmd = atan(mud);
double atms = atan(mus);
// Compute a vector from the device to the proxy, for computing
// the angle of the friction cone
cVector3d vDeviceProxy = cSub(a_proxy, m_deviceGlobalPos);
vDeviceProxy.normalize();
// Now compute the angle of the friction cone...
double theta = acos(vDeviceProxy.dot(a_normal));
// Manage the "slip-friction" state machine
// If the dynamic friction radius is for some reason larger than the
// static friction radius, always slip
if (mud > mus)
{
m_slipping = true;
}
// If we're slipping...
else if (m_slipping)
{
if (theta < (atmd * m_frictionDynHysteresisMultiplier))
{
m_slipping = false;
}
else
{
m_slipping = true;
}
}
// If we're not slipping...
else
{
if (theta > atms)
{
m_slipping = true;
}
else
{
m_slipping = false;
}
}
// The friction coefficient we're going to use...
double mu;
if (m_slipping) mu = mud;
else mu = mus;
// Calculate the friction radius as the absolute value of the penetration
// depth times the coefficient of friction
double frictionRadius = fabs(penetrationDepth * mu);
// Calculate the distance between the proxy position and the current
// goal position.
double r = a_proxy.distance(a_goal);
// If this distance is smaller than CHAI_SMALL, we consider the proxy
// to be at the same position as the goal, and we're done...
if (r < CHAI_SMALL)
{
m_nextBestProxyGlobalPos = a_proxy;
}
// If the proxy is outside the friction cone, update its position to
// be on the perimeter of the friction cone...
else if (r > frictionRadius)
{
m_nextBestProxyGlobalPos = cAdd(a_goal, cMul(frictionRadius/r, cSub(a_proxy, a_goal)));
}
// Otherwise, if the proxy is inside the friction cone, the proxy
// should not be moved (set next best position to current position)
else
{
m_nextBestProxyGlobalPos = a_proxy;
}
// We're done; record the fact that we're still touching an object...
return;
}
