//===========================================================================
void cGELMesh::connectVerticesToSkeleton(bool a_connectToNodesOnly)
{
// get number of vertices
int numVertices = m_gelVertices.size();
// for each deformable vertex we search for the nearest sphere or link
for (int i=0; i<numVertices; i++)
{
// get current deformable vertex
cGELVertex* curVertex = &m_gelVertices[i];
// get current vertex position
cVector3d pos = curVertex->m_vertex->getPos();
// initialize constant
double min_distance = 99999999999999999.0;
cGELSkeletonNode* nearest_node = NULL;
cGELSkeletonLink* nearest_link = NULL;
// search for the nearest node
list<cGELSkeletonNode*>::iterator itr;
for(itr = m_nodes.begin(); itr != m_nodes.end(); ++itr)
{
cGELSkeletonNode* nextNode = *itr;
double distance = cDistance(pos, nextNode->m_pos);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = nextNode;
nearest_link = NULL;
}
}
// search for the nearest link if any
if (!a_connectToNodesOnly)
{
list<cGELSkeletonLink*>::iterator j;
for(j = m_links.begin(); j != m_links.end(); ++j)
{
cGELSkeletonLink* nextLink = *j;
double angle0 = cAngle(nextLink->m_wLink01, cSub(pos, nextLink->m_node0->m_pos));
double angle1 = cAngle(nextLink->m_wLink10, cSub(pos, nextLink->m_node1->m_pos));
if ((angle0 < (CHAI_PI / 2.0)) && (angle1 < (CHAI_PI / 2.0)))
{
cVector3d p = cProjectPointOnLine(pos,
nextLink->m_node0->m_pos,
nextLink->m_wLink01);
double distance = cDistance(pos, p);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = NULL;
nearest_link = nextLink;
}
}
}
}
// attach vertex to nearest node if it exists
if (nearest_node != NULL)
{
curVertex->m_node = nearest_node;
curVertex->m_link = NULL;
cVector3d posRel = cSub(pos, nearest_node->m_pos);
curVertex->m_massParticle->m_pos = cMul(cTrans(nearest_node->m_rot), posRel);
}
// attach vertex to nearest link if it exists
else if (nearest_link != NULL)
{
curVertex->m_node = NULL;
curVertex->m_link = nearest_link;
cMatrix3d rot;
rot.setCol( nearest_link->m_A0,
nearest_link->m_B0,
nearest_link->m_wLink01);
cVector3d posRel = cSub(pos, nearest_link->m_node0->m_pos);
curVertex->m_massParticle->m_pos = cMul(cInv(rot), posRel);
}
}
}
bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints22(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// The proxy is now constrained by two triangles and can only move along
// a virtual line; we now calculate the nearest point to the original
// goal (device position) along this line by projecting the ideal
// goal onto the line.
//
// The line is expressed by the cross product of both surface normals,
// which have both been oriented to point away from the device
cVector3d line;
m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal.crossr(m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal, line);
// check result.
if (line.equals(cVector3d(0,0,0)))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 2;
return (false);
}
line.normalize();
// Compute the projection of the device position (goal) onto the line; this
// gives us the new goal position.
cVector3d goalGlobalPos = cProjectPointOnLine(a_goalGlobalPos, m_proxyGlobalPos, line);
// A vector from the proxy to the goal
cVector3d vProxyToGoal;
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
// 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 = 2;
return (false);
}
// 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_collisionRecorderConstraint2.clear();
bool hit = m_world->computeCollisionDetection( m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint2,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint2.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)
{
cVector3d normal = cMul(0.5,cAdd(m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal,
m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal));
testFrictionAndMoveProxy(goalGlobalPos,
m_proxyGlobalPos,
normal,
m_collisionRecorderConstraint1.m_nearestCollision.m_triangle->getParent(), a_toolVel);
m_numContacts = 2;
m_algoCounter = 0;
return (false);
}
//-----------------------------------------------------------------------
// THIRD 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_collisionRecorderConstraint2.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_contactPoint2->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_contactPoint2->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
m_algoCounter = 0;
m_numContacts = 3;
// TODO: There actually should be a third friction test to see if we
// can make it to our new goal position, but this is generally such a
// small movement in one iteration that it's irrelevant...
return (true);
}
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();
}
