void updateHaptics(void) { // reset clock cPrecisionClock clock; clock.reset(); // main haptic simulation loop while(simulationRunning) { ///////////////////////////////////////////////////////////////////// // SIMULATION TIME ///////////////////////////////////////////////////////////////////// // stop the simulation clock clock.stop(); // read the time increment in seconds double timeInterval = clock.getCurrentTimeSeconds(); // restart the simulation clock clock.reset(); clock.start(); // update frequency counter frequencyCounter.signal(1); ///////////////////////////////////////////////////////////////////// // HAPTIC FORCE COMPUTATION ///////////////////////////////////////////////////////////////////// // compute global reference frames for each object world->computeGlobalPositions(true); // update position and orientation of tool tool->updatePose(); // compute interaction forces tool->computeInteractionForces(); // send forces to device tool->applyForces(); ///////////////////////////////////////////////////////////////////// // HAPTIC SIMULATION ///////////////////////////////////////////////////////////////////// // get position of cursor in global coordinates cVector3d toolPos = tool->getDeviceGlobalPos(); // get position of object in global coordinates cVector3d objectPos = object->getGlobalPos(); // compute a vector from the center of mass of the object (point of rotation) to the tool cVector3d v = cSub(toolPos, objectPos); // compute angular acceleration based on the interaction forces // between the tool and the object cVector3d angAcc(0,0,0); if (v.length() > 0.0) { // get the last force applied to the cursor in global coordinates // we negate the result to obtain the opposite force that is applied on the // object cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce); // compute the effective force that contributes to rotating the object. cVector3d force = toolForce - cProject(toolForce, v); // compute the resulting torque cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force)); // update rotational acceleration const double INERTIA = 0.4; angAcc = (1.0 / INERTIA) * torque; } // update rotational velocity angVel.add(timeInterval * angAcc); // set a threshold on the rotational velocity term const double MAX_ANG_VEL = 10.0; double vel = angVel.length(); if (vel > MAX_ANG_VEL) { angAcc.mul(MAX_ANG_VEL / vel); } // add some damping too const double DAMPING = 0.1; angVel.mul(1.0 - DAMPING * timeInterval); // if user switch is pressed, set velocity to zero if (tool->getUserSwitch(0) == 1) { angVel.zero(); } // compute the next rotation configuration of the object if (angVel.length() > C_SMALL) { object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length()); } } // exit haptics thread simulationFinished = true; }
//=========================================================================== bool cEffectMagnet::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { // compute distance from object to tool double distance = cDistance(a_toolPos, m_parent->m_interactionProjectedPoint); // get parameters of magnet double magnetMaxForce = m_parent->m_material.getMagnetMaxForce(); double magnetMaxDistance = m_parent->m_material.getMagnetMaxDistance(); double stiffness = m_parent->m_material.getStiffness(); double forceMagnitude = 0; if ((distance > 0) && (distance < magnetMaxDistance) && (stiffness > 0)) { double limitLinearModel = magnetMaxForce / stiffness; cClamp(limitLinearModel, 0.0, 0.5 * distance); if (distance < limitLinearModel) { // apply local linear model near magnet forceMagnitude = stiffness * distance; } else { // compute quadratic model cMatrix3d sys; sys.m[0][0] = limitLinearModel * limitLinearModel; sys.m[0][1] = limitLinearModel; sys.m[0][2] = 1.0; sys.m[1][0] = magnetMaxDistance * magnetMaxDistance; sys.m[1][1] = magnetMaxDistance; sys.m[1][2] = 1.0; sys.m[2][0] = 2.0 * limitLinearModel; sys.m[2][1] = 1.0; sys.m[2][2] = 0.0; sys.invert(); cVector3d param; sys.mulr(cVector3d(magnetMaxForce, 0.0, -1.0), param); // apply quadratic model double val = distance - limitLinearModel; forceMagnitude = param.x * val * val + param.y * val + param.z; } // compute magnetic force a_reactionForce = cMul(forceMagnitude, cNormalize(cSub(m_parent->m_interactionProjectedPoint, a_toolPos))); // add damping component double viscosity = m_parent->m_material.getViscosity(); cVector3d viscousForce = cMul(-viscosity, a_toolVel); a_reactionForce.add(viscousForce); return (true); } else { // the tool is located outside the magnet zone a_reactionForce.zero(); return (false); } }
//=========================================================================== bool cWorld::computeCollisionDetection( cVector3d& a_segmentPointA, const cVector3d& a_segmentPointB, cGenericObject*& a_colObject, cTriangle*& a_colTriangle, cVector3d& a_colPoint, double& a_colDistance, const bool a_visibleObjectsOnly, int a_proxyCall) { // initialize objects for collision detection calls cGenericObject* t_colObject; cTriangle *t_colTriangle; cVector3d t_colPoint; bool hit = false; double colSquareDistance = CHAI_LARGE; double t_colSquareDistance = colSquareDistance; // get the transpose of the local rotation matrix cMatrix3d transLocalRot; m_localRot.transr(transLocalRot); // convert second endpoint of the segment into local coordinate frame cVector3d localSegmentPointB = a_segmentPointB; localSegmentPointB.sub(m_localPos); transLocalRot.mul(localSegmentPointB); // r_segmentPointA is the value that we will return in a_segmentPointA // at the end; it should be unchanged from the received value of // a_segmentPointA, unless the collision that will be returned is with // a moving object, in which case it will be adjusted so that it is in the // same location relative to the moving object as it was at the previous // haptic iteration; this is necessary so that the proxy algorithm gets the // correct new proxy position cVector3d r_segmentPointA = a_segmentPointA; // check for collisions with all children of this world unsigned int nChildren = m_children.size(); for (unsigned int i=0; i<nChildren; i++) { // start with the first segment point as it was received cVector3d l_segmentPointA = a_segmentPointA; // convert first endpoint of the segment into local coordinate frame cVector3d localSegmentPointA = l_segmentPointA; localSegmentPointA.sub(m_localPos); transLocalRot.mul(localSegmentPointA); // if this is a first call from the proxy algorithm, and the current // child is a dynamic object, adjust the first segment endpoint so that // it is in the same position relative to the moving object as it was // at the previous haptic iteration if ((a_proxyCall == 1) && (m_children[i]->m_historyValid)) AdjustCollisionSegment(l_segmentPointA,localSegmentPointA,m_children[i]); // call this child's collision detection function to see if it (or any // of its descendants) are intersected by the segment int coll = m_children[i]->computeCollisionDetection(localSegmentPointA, localSegmentPointB, t_colObject, t_colTriangle, t_colPoint, t_colSquareDistance, a_visibleObjectsOnly, a_proxyCall); // if a collision was found with this child, and this collision is // closer than any others found so far... if ((coll == 1) && (t_colSquareDistance < colSquareDistance)) { // record that there has been a collision hit = true; // set the return parameters with information about this collision // (they may be overwritten if a closer collision is found later // on in this loop) a_colObject = t_colObject; a_colTriangle = t_colTriangle; a_colPoint = t_colPoint; // this is now the shortest distance to a collision found so far colSquareDistance = t_colSquareDistance; // convert collision point into parent coordinate frame m_localRot.mul(a_colPoint); a_colPoint.add(m_localPos); // localSegmentPointA's position (as possibly modified in the // call to the child's collision detector), converted back to // the global coordinate frame, is currently the proxy position // we will want to return (unless we find a closer collision later on) r_segmentPointA = cAdd(cMul(m_localRot,localSegmentPointA), m_localPos); } } // for optimization reasons, the collision detectors only computes the // squared distance between a_segmentA and collision point; this // computes a square root to obtain the actual distance. a_colDistance = sqrt(colSquareDistance); // set the value of the actual parameter for the first segment point; this // is the proxy position when called by the proxy algorithm, and may be // different from the value passed in this parameter if the closest collision // was with a moving object a_segmentPointA = r_segmentPointA; // return whether there was a collision between the segment and this world return (hit); }
void updateHaptics(void) { // reset clock simClock.reset(); // main haptic simulation loop while(simulationRunning) { // compute global reference frames for each object world->computeGlobalPositions(true); // update position and orientation of tool tool->updatePose(); // compute interaction forces tool->computeInteractionForces(); // send forces to device tool->applyForces(); // stop the simulation clock simClock.stop(); // read the time increment in seconds double timeInterval = simClock.getCurrentTimeSeconds(); // restart the simulation clock simClock.reset(); simClock.start(); // temp variable to compute rotational acceleration cVector3d rotAcc(0,0,0); // check if tool is touching an object cGenericObject* objectContact = tool->m_proxyPointForceModel->m_contactPoint0->m_object; if (objectContact != NULL) { // retrieve the root of the object mesh cGenericObject* obj = objectContact->getSuperParent(); // get position of cursor in global coordinates cVector3d toolPos = tool->m_deviceGlobalPos; // get position of object in global coordinates cVector3d objectPos = obj->getGlobalPos(); // compute a vector from the center of mass of the object (point of rotation) to the tool cVector3d vObjectCMToTool = cSub(toolPos, objectPos); // compute acceleration based on the interaction forces // between the tool and the object if (vObjectCMToTool.length() > 0.0) { // get the last force applied to the cursor in global coordinates // we negate the result to obtain the opposite force that is applied on the // object cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce); // compute effective force to take into account the fact the object // can only rotate around a its center mass and not translate cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool); // compute the resulting torque cVector3d torque = cMul(vObjectCMToTool.length(), cCross( cNormalize(vObjectCMToTool), effectiveForce)); // update rotational acceleration const double OBJECT_INERTIA = 0.4; rotAcc = (1.0 / OBJECT_INERTIA) * torque; } } // update rotational velocity rotVel.add(timeInterval * rotAcc); // set a threshold on the rotational velocity term const double ROT_VEL_MAX = 10.0; double velMag = rotVel.length(); if (velMag > ROT_VEL_MAX) { rotVel.mul(ROT_VEL_MAX / velMag); } // add some damping too const double DAMPING_GAIN = 0.1; rotVel.mul(1.0 - DAMPING_GAIN * timeInterval); // if user switch is pressed, set velocity to zero if (tool->getUserSwitch(0) == 1) { rotVel.zero(); } // compute the next rotation configuration of the object if (rotVel.length() > CHAI_SMALL) { object->rotate(cNormalize(rotVel), timeInterval * rotVel.length()); } } // exit haptics thread simulationFinished = 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(); }