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