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(); }