void updateHaptics(void)
{
// reset clock
simClock.reset();
// main haptic simulation loop
while(simulationRunning)
{
// stop the simulation clock
simClock.stop();
// read the time increment in seconds
double timeInterval = simClock.getCurrentTimeSeconds();
if (timeInterval > 0.001) { timeInterval = 0.001; }
// restart the simulation clock
simClock.reset();
simClock.start();
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
for (int y=0; y<10; y++)
{
for (int x=0; x<10; x++)
{
cVector3d nodePos = nodes[x][y]->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
cVector3d tmpfrc = cNegate(f);
nodes[x][y]->setExternalForce(tmpfrc);
force.add(f);
}
}
// integrate dynamics
defWorld->updateDynamics(timeInterval);
// scale force
force.mul(deviceForceScale);
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// reset simulation clock
simClock.reset();
simClock.start();
// main haptic simulation loop
while(simulationRunning)
{
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
list<cGELMesh*>::iterator i;
// model water level
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
if (nodePos.z < level)
{
double depth = nodePos.z - level;
force.add(cVector3d(0,0,-100*depth));
}
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
if ((nodePos.z-radius) < level)
{
double depth = (nodePos.z-radius) - level;
force.add(cVector3d(0,0,-1.0 * depth));
node->m_vel.mul(0.95);
}
node->setExternalForce(force);
}
}
}
// compute haptic feedback
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
}
force.add(cMul(1.0, f));
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
node->setExternalForce(tmpfrc);
}
force.add(cMul(4.0, f));
}
}
}
// integrate dynamics
double interval = simClock.stop();
simClock.reset();
simClock.start();
if (interval > 0.001) { interval = 0.001; }
defWorld->updateDynamics(interval);
// scale force
force.mul(0.6 * deviceForceScale);
// water viscosity
if ((pos.z - deviceRadius) < level)
{
// read damping properties of haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
double Kv = 0.8 * info.m_maxLinearDamping;
// read device velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// compute a scale factor [0,1] proportional to percentage
// of tool volume immersed in the water
double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
double scale = cClamp(val, 0.1, 1.0);
// compute force
cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
force.add(forceDamping);
}
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
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;
}
void updateHaptics(void)
{
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
// reset haptics activation clock
startHapticsClock.reset();
startHapticsClock.start();
bool hapticsReady = false;
// main haptic simulation loop
while(simulationRunning)
{
// wait for some time before enabling haptics
if (!hapticsReady)
{
if (startHapticsClock.getCurrentTimeSeconds() > 3.0)
{
hapticsReady = true;
}
}
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// check if the tool is touching an object
cGenericObject* object = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
// read user switch status
bool userSwitch = tool->getUserSwitch(0);
// if the tool is currently grasping an object we simply update the interaction grasp force
// between the tool and the object (modeled by a virtual spring)
if (graspActive && userSwitch)
{
// retrieve latest position and orientation of grasped ODE object in world coordinates
cMatrix3d globalGraspObjectRot = graspObject->getGlobalRot();
cVector3d globalGraspObjectPos = graspObject->getGlobalPos();
// compute the position of the grasp point on object in global coordinates
cVector3d globalGraspPos = globalGraspObjectPos + cMul(globalGraspObjectRot, graspPos);
// retrieve the position of the tool in global coordinates
cVector3d globalToolPos = tool->getProxyGlobalPos();
// compute the offset between the tool and grasp point on the object
cVector3d offset = globalToolPos - globalGraspPos;
// model a spring between both points
double STIFFNESS = 4;
cVector3d force = STIFFNESS * offset;
// apply attraction force (grasp) onto object
graspObject->addGlobalForceAtGlobalPos(force, globalGraspPos);
// scale magnitude and apply opposite force to haptic device
tool->m_lastComputedGlobalForce.add(cMul(-1.0, force));
// update both end points of the line which is used for display purposes only
graspLine->m_pointA = globalGraspPos;
graspLine->m_pointB = globalToolPos;
}
// the user is not or no longer currently grasping the object
else
{
// was the user grasping the object at the previous simulation loop
if (graspActive)
{
// we disable grasping
graspActive = false;
// we hide the virtual line between the tool and the grasp point
graspLine->setShowEnabled(false);
// we enable haptics interaction between the tool and the previously grasped object
if (graspObject != NULL)
{
graspObject->m_imageModel->setHapticEnabled(true, true);
}
}
// the user is touching an object
if (object != NULL)
{
// check if object is attached to an external ODE parent
cGenericType* externalParent = object->getExternalParent();
cODEGenericBody* ODEobject = dynamic_cast<cODEGenericBody*>(externalParent);
if (ODEobject != NULL)
{
// get position of tool
cVector3d pos = tool->m_proxyPointForceModel->m_contactPoint0->m_globalPos;
// check if user has enabled the user switch to gras the object
if (userSwitch)
{
// a new object is being grasped
graspObject = ODEobject;
// retrieve the grasp position on the object in local coordinates
graspPos = tool->m_proxyPointForceModel->m_contactPoint0->m_localPos;
// grasp in now active!
graspActive = true;
// enable small line which display the offset between the tool and the grasp point
graspLine->setShowEnabled(true);
// disable haptic interaction between the tool and the grasped device.
// this is performed for stability reasons.
graspObject->m_imageModel->setHapticEnabled(false, true);
}
// retrieve the haptic interaction force being applied to the tool
cVector3d force = tool->m_lastComputedGlobalForce;
// apply haptic force to ODE object
cVector3d tmpfrc = cNegate(force);
if (hapticsReady)
{
ODEobject->addGlobalForceAtGlobalPos(tmpfrc, pos);
}
}
}
}
// send forces to device
tool->applyForces();
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = cClamp(time, 0.001, 0.004);
// reset clock
simClock.reset();
simClock.start();
// update simulation
ODEWorld->updateDynamics(nextSimInterval);
}
// exit haptics thread
simulationFinished = true;
}
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;
}
//===========================================================================
bool cCamera::select(const int a_windowPosX,
const int a_windowPosY,
const int a_windowWidth,
const int a_windowHeight,
cCollisionRecorder& a_collisionRecorder,
cCollisionSettings& a_collisionSettings)
{
// sanity check
if ((a_windowWidth <= 0) || (a_windowHeight <= 0)) return (false);
// clear collision recorder
a_collisionRecorder.clear();
// update my m_globalPos and m_globalRot variables
m_parentWorld->computeGlobalPositions(false);
// init variable to store result
bool result = false;
if (m_perspectiveMode)
{
// make sure we have a legitimate field of view
if (fabs(m_fieldViewAngle) < 0.001f) { return (false); }
// compute the ray that leaves the eye point at the appropriate angle
//
// m_fieldViewAngle / 2.0 would correspond to the _top_ of the window
double distCam = (a_windowHeight / 2.0f) / cTanDeg(m_fieldViewAngle / 2.0f);
cVector3d selectRay;
selectRay.set(-distCam,
(a_windowPosX - (a_windowWidth / 2.0f)),
((a_windowHeight / 2.0f) - a_windowPosY));
selectRay.normalize();
selectRay = cMul(m_globalRot, selectRay);
// create a point that's way out along that ray
cVector3d selectPoint = cAdd(m_globalPos, cMul(100000, selectRay));
// search for intersection between the ray and objects in the world
result = m_parentWorld->computeCollisionDetection(
m_globalPos,
selectPoint,
a_collisionRecorder,
a_collisionSettings);
}
else
{
double hw = (double)(a_windowWidth) * 0.5;
double hh = (double)(a_windowHeight)* 0.5;
double aspect = hw / hh;
double offsetX = ((a_windowPosX - hw) / hw) * 0.5 * m_orthographicWidth;
double offsetY =-((a_windowPosY - hh) / hh) * 0.5 * (m_orthographicWidth / aspect);
cVector3d pos = cAdd(m_globalPos,
cMul(offsetX, m_globalRot.getCol1()),
cMul(offsetY, m_globalRot.getCol2()));
// create a point that's way out along that ray
cVector3d selectPoint = cAdd(pos, cMul(100000, cNegate(m_globalRot.getCol0())));
result = m_parentWorld->computeCollisionDetection(pos,
selectPoint,
a_collisionRecorder,
a_collisionSettings);
}
// return result
return (result);
}