bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints11(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// The proxy is now constrained on a plane; we now calculate the nearest
// point to the original goal (device position) on this plane; this point
// is computed by projecting the ideal goal onto the plane defined by the
// intersected triangle
cVector3d goalGlobalPos = cProjectPointOnPlane(a_goalGlobalPos,
m_proxyGlobalPos,
m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal);
// A vector from the proxy to the goal (ch lab -projection of the original goal onto the collided triangle)
cVector3d vProxyToGoal;
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
// ch lab - If the distance between the proxy and the projected goal position is
// very small then we can be considered done.
// original comment - If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(m_proxyGlobalPos, goalGlobalPos))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 1;
return (false);
}
// ch lab - compute the normalized form of the vector going from the
// current proxy position to the projected goal position
// original comment - compute the normalized form of the vector going from the
// current proxy position to the desired goal position
cVector3d vProxyToGoalNormalized;
vProxyToGoal.normalizer(vProxyToGoalNormalized);
// Test whether the path from the proxy to the goal is obstructed.
// For this we create a segment that goes from the proxy position to
// the goal position plus a little extra to take into account the
// physical radius of the proxy.
cVector3d targetPos = goalGlobalPos +
cMul(m_epsilonCollisionDetection, vProxyToGoalNormalized);
// setup collision detector
m_collisionSettings.m_collisionRadius = m_radius;
// search for collision
m_collisionSettings.m_adjustObjectMotion = false;
m_collisionRecorderConstraint1.clear();
bool hit = m_world->computeCollisionDetection( m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint1,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint1.m_nearestCollision.m_squareDistance);
if (collisionDistance > (cDistance(m_proxyGlobalPos, goalGlobalPos) + CHAI_SMALL))
{
hit = false;
}
else
{
// a collision has occurred and we check if the distance from the
// proxy to the collision is smaller than epsilon. If yes, then
// we reduce the epsilon term in order to avoid possible "pop through"
// effect if we suddenly push the proxy "up" again.
if (collisionDistance < m_epsilon)
{
m_epsilon = collisionDistance;
if (m_epsilon < m_epsilonMinimalValue)
{
m_epsilon = m_epsilonMinimalValue;
}
}
}
}
// If no collision occurs, we move the proxy to its goal, unless
// friction prevents us from doing so.
if (!hit)
{
m_numContacts = 1;
testFrictionAndMoveProxy(goalGlobalPos,
m_proxyGlobalPos,
m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal,
m_collisionRecorderConstraint0.m_nearestCollision.m_object, a_toolVel);
m_algoCounter = 0;
return (false);
}
// a second collision has occurred
m_algoCounter = 2;
//-----------------------------------------------------------------------
// SECOND COLLISION OCCURES:
//-----------------------------------------------------------------------
// We want the center of the proxy to move as far toward the triangle as it can,
// but we want it to stop when the _sphere_ representing the proxy hits the
// triangle. We want to compute how far the proxy center will have to
// be pushed _away_ from the collision point - along the vector from the proxy
// to the goal - to keep a distance m_radius between the proxy center and the
// triangle.
//
// So we compute the cosine of the angle between the normal and proxy-goal vector...
double cosAngle = vProxyToGoalNormalized.dot(m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal);
// Now we compute how far away from the collision point - _backwards_
// along vProxyGoal - we have to put the proxy to keep it from penetrating
// the triangle.
//
// If only ASCII art were a little more expressive...
double distanceTriangleProxy = m_epsilon / cAbs(cosAngle);
if (distanceTriangleProxy > collisionDistance) {
distanceTriangleProxy = cMax(collisionDistance, m_epsilon);
}
// We compute the projection of the vector between the proxy and the collision
// point onto the normal of the triangle. This is the direction in which
// we'll move the _goal_ to "push it away" from the triangle (to account for
// the radius of the proxy).
// A vector from the most recent collision point to the proxy
cVector3d vCollisionToProxy;
m_proxyGlobalPos.subr(m_contactPoint1->m_globalPos, vCollisionToProxy);
// Move the proxy to the collision point, minus the distance along the
// movement vector that we computed above.
//
// Note that we're adjusting the 'proxy' variable, which is just a local
// copy of the proxy position. We still might decide not to move the
// 'real' proxy due to friction.
cVector3d vColNextGoal;
vProxyToGoalNormalized.mulr(-distanceTriangleProxy, vColNextGoal);
cVector3d nextProxyPos;
m_contactPoint1->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(goalGlobalPos, nextProxyPos))
{
m_numContacts = 2;
m_algoCounter = 0;
return (true);
}
return (true);
}
//===========================================================================
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;
}
//===========================================================================
void cProxyPointForceAlgo::updateForce()
{
// initialize variables
double stiffness = 0.0;
cVector3d normal;
normal.zero();
// if there are no contacts between proxy and environment, no force is applied
if (m_numContacts == 0)
{
m_lastGlobalForce.zero();
return;
}
//---------------------------------------------------------------------
// stiffness and surface normal estimation
//---------------------------------------------------------------------
else if (m_numContacts == 1)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() );
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
}
// if there are two contact points, the stiffness is the average of the
// stiffnesses of the two intersected triangles' meshes
else if (m_numContacts == 2)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() ) / 2.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.mul(1.0/2.0);
}
// if there are three contact points, the stiffness is the average of the
// stiffnesses of the three intersected triangles' meshes
else if (m_numContacts == 3)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint2->m_triangle->getParent()->m_material.getStiffness() ) / 3.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.add(m_contactPoint2->m_globalNormal);
normal.mul(1.0/3.0);
}
//---------------------------------------------------------------------
// computing a force (Hooke's law)
//---------------------------------------------------------------------
// compute the force by modeling a spring between the proxy and the device
cVector3d force;
m_proxyGlobalPos.subr(m_deviceGlobalPos, force);
force.mul(stiffness);
m_lastGlobalForce = force;
// compute tangential and normal forces
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
//---------------------------------------------------------------------
// force shading (optional)
//---------------------------------------------------------------------
if ((m_useForceShading) && (m_numContacts == 1))
{
// get vertices and normals related to contact triangle
cVector3d vertex0 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getPos()));
cVector3d vertex1 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getPos()));
cVector3d vertex2 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getPos()));
cVector3d normal0 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getNormal());
cVector3d normal1 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getNormal());
cVector3d normal2 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getNormal());
// compute angles between normals. If the angles are very different, then do not apply shading.
double angle01 = cAngle(normal0, normal1);
double angle02 = cAngle(normal0, normal2);
double angle12 = cAngle(normal1, normal2);
if ((angle01 < m_forceShadingAngleThreshold) || (angle02 < m_forceShadingAngleThreshold) || (angle12 < m_forceShadingAngleThreshold))
{
double a0 = 0;
double a1 = 0;
cProjectPointOnPlane(m_contactPoint0->m_globalPos, vertex0, vertex1, vertex2, a0, a1);
cVector3d normalShaded = cAdd(
cMul(0.5, cAdd(cMul(a0, normal1), cMul((1-a0), normal0))),
cMul(0.5, cAdd(cMul(a1, normal2), cMul((1-a1), normal0)))
);
normalShaded.normalize();
if (cAngle(normalShaded, normal) > 1.0)
{
normalShaded.negate();
}
if (cAngle(normal, normalShaded) < m_forceShadingAngleThreshold)
{
double forceMagnitude = m_normalForce.length();
force = cAdd( cMul(forceMagnitude, normalShaded), m_tangentialForce);
m_lastGlobalForce = force;
normal = normalShaded;
// update tangential and normal forces again
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
}
}
}
}
void hapticsLoop(void* a_pUserData)
{
// read the position of the haptic device
cursor->updatePose();
// compute forces between the cursor and the environment
cursor->computeForces();
// stop the simulation clock
g_clock.stop();
// read the time increment in seconds
double increment = g_clock.getCurrentTime() / 1000000.0;
// restart the simulation clock
g_clock.initialize();
g_clock.start();
// get position of cursor in global coordinates
cVector3d cursorPos = cursor->m_deviceGlobalPos;
// compute velocity of cursor;
timeCounter = timeCounter + increment;
if (timeCounter > 0.01)
{
cursorVel = (cursorPos - lastCursorPos) / timeCounter;
lastCursorPos = cursorPos;
timeCounter = 0;
}
// get position of torus in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute the velocity of the sphere at the contact point
cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
if (rotVelocity.length() > CHAI_SMALL)
{
cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
cVector3d vpc = cursorPos - projection;
if (vpc.length() > CHAI_SMALL)
{
contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
}
}
// get the last force applied to the cursor in global coordinates
cVector3d cursorForce = cursor->m_lastComputedGlobalForce;
// compute friction force
cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));
// add friction force to cursor
cursor->m_lastComputedGlobalForce.add(friction);
// update rotational velocity
if (friction.length() > CHAI_SMALL)
{
rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
}
// add some damping...
//rotVelocity.mul(1.0 - increment);
// compute the next rotation of the torus
if (rotVelocity.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
}
// send forces to haptic device
cursor->applyForces();
}
