void updateHaptics(void)
{
// initialize frequency counter
frequencyCounter.reset();
// initialize precision clock
cPrecisionClock clock;
clock.reset();
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// 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 haptic device
tool->applyForces();
// stop clock
double time = clock.stop();
// scroll text according to the haptic device position
const double K = 4000;
double command = tool->m_hapticPoint->getGlobalPosGoal().y();
double pos = textPosition + K * time * command;
textPosition = cClamp(pos, -text->getWidth(), (double)(windowW));
// restart clock
clock.start(true);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// initialize frequency counter
frequencyCounter.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// READ HAPTIC DEVICE
/////////////////////////////////////////////////////////////////////
// read position
cVector3d position;
hapticDevice->getPosition(position);
// read orientation
cMatrix3d rotation;
hapticDevice->getRotation(rotation);
// read gripper position
double gripperAngle;
hapticDevice->getGripperAngleRad(gripperAngle);
// read linear velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// read angular velocity
cVector3d angularVelocity;
hapticDevice->getAngularVelocity(angularVelocity);
// read gripper angular velocity
double gripperAngularVelocity;
hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
// read userswitch status (button 0)
bool button0, button1, button2, button3;
button0 = false;
button1 = false;
button2 = false;
button3 = false;
hapticDevice->getUserSwitch(0, button0);
hapticDevice->getUserSwitch(1, button1);
hapticDevice->getUserSwitch(2, button2);
hapticDevice->getUserSwitch(3, button3);
/////////////////////////////////////////////////////////////////////
// UPDATE 3D MODELS
/////////////////////////////////////////////////////////////////////
// update arrow
velocity->m_pointA = position;
velocity->m_pointB = cAdd(position, linearVelocity);
// update position and orientation of cursor
cursor->setLocalPos(position);
cursor->setLocalRot(rotation);
// adjust the color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (button0)
{
cursor->m_material->setGreenMediumAquamarine();
}
else if (button1)
{
cursor->m_material->setYellowGold();
}
else if (button2)
{
cursor->m_material->setOrangeCoral();
}
else if (button3)
{
cursor->m_material->setPurpleLavender();
}
else
{
cursor->m_material->setBlueRoyal();
}
// update global variable for graphic display update
hapticDevicePosition = position;
/////////////////////////////////////////////////////////////////////
// COMPUTE AND SEND FORCE AND TORQUE
/////////////////////////////////////////////////////////////////////
cVector3d force (0,0,0);
cVector3d torque (0,0,0);
double gripperForce = 0.0;
// apply force field
if (useForceField)
{
// compute linear force
double Kp = 20; // [N/m]
cVector3d forceField = -Kp * position;
force.add(forceField);
// compute angular torque
double Kr = 0.05; // [N/m.rad]
cVector3d axis;
double angle;
rotation.toAngleAxis(angle, axis);
torque = (-Kr * angle) * axis;
}
// apply damping term
if (useDamping)
{
cHapticDeviceInfo info = hapticDevice->getSpecifications();
// compute linear damping force
double Kv = 1.0 * info.m_maxLinearDamping;
cVector3d forceDamping = -Kv * linearVelocity;
force.add(forceDamping);
// compute angluar damping force
double Kvr = 1.0 * info.m_maxAngularDamping;
cVector3d torqueDamping = -Kvr * angularVelocity;
torque.add(torqueDamping);
// compute gripper angular damping force
double Kvg = 1.0 * info.m_maxGripperAngularDamping;
gripperForce = gripperForce - Kvg * gripperAngularVelocity;
}
// send computed force, torque and gripper force to haptic device
hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}