//==============================================================================
void cToolGripper::computeInteractionForces()
{
// convert the angle of the gripper into a position in device coordinates.
// this value is device dependent.
double gripperPositionFinger = 0.0;
double gripperPositionThumb = 0.0;
if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
// compute new position of thumb and finger
cVector3d lineFingerThumb = getGlobalRot().getCol1();
cVector3d pFinger = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionFinger * lineFingerThumb;
cVector3d pThumb = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionThumb * lineFingerThumb;
cVector3d posFinger, posThumb;
if (m_hapticDevice->m_specifications.m_rightHand)
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pThumb));
}
else
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pThumb));
}
// compute forces
cVector3d forceThumb = m_hapticPointThumb->computeInteractionForces(posThumb,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
cVector3d forceFinger = m_hapticPointFinger->computeInteractionForces(posFinger,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
// compute torques
double scl = 0.0;
double factor = m_gripperWorkspaceScale * m_workspaceScaleFactor;
if (factor > 0.0)
{
scl = 1.0 / factor;
}
cVector3d torque = scl * cAdd(cCross(cSub(posThumb, m_deviceGlobalPos), forceThumb), cCross(cSub(posFinger, m_deviceGlobalPos), forceFinger));
// compute gripper force
double gripperForce = 0.0;
if ((m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7) ||
(m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7))
{
cVector3d dir = posFinger - posThumb;
if (dir.length() > 0.00001)
{
dir.normalize ();
cVector3d force = cProject (forceFinger, dir);
gripperForce = force.length();
if (force.length() > 0.001)
{
double angle = cAngle(dir, force);
if ((angle > C_PI/2.0) || (angle < -C_PI/2.0)) gripperForce = -gripperForce;
}
}
}
// gripper damping
double gripperAngularVelocity = 0.0;
m_hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
double gripperDamping = -0.1 * m_hapticDevice->m_specifications.m_maxGripperAngularDamping * gripperAngularVelocity;
// finalize forces, torques and gripper force
m_lastComputedGlobalForce = forceThumb + forceFinger;
m_lastComputedGlobalTorque = torque;
m_lastComputedGripperForce = gripperForce + gripperDamping;
}
void updateHaptics(void)
{
double timeV = 0.0;
cLabel* label = new cLabel();
cLabel* label2 = new cLabel();
rootLabels->addChild(label);
rootLabels->addChild(label2);
label->setPos(0, 0, 0);
label2->setPos(0, -20, 0);
label->m_fontColor.set(1.0, 1.0, 1.0);
label2->m_fontColor.set(1.0, 1.0, 1.0);
// main haptic simulation loop
while(simulationRunning)
{
float deltaTime = pClock.getCurrentTimeSeconds() - lastTime;
lastTime = pClock.getCurrentTimeSeconds();
cVector3d equilibrium (0.0, 0.0, 0.0);
cVector3d pos0 (0.0, 0.0, 0.0), pos1 (0.0, 0.0, 0.0);
// for each device
int i=0;
while (i < numHapticDevices)
{
// read position of haptic device
cVector3d newPosition;
hapticDevices[i]->getPosition(newPosition);
newPosition = deviceToWorld(newPosition, i);
((i == 0)? pos0 : pos1) = newPosition;
// read orientation of haptic device
cMatrix3d newRotation;
hapticDevices[i]->getRotation(newRotation);
// update position and orientation of cursor
cursors[i]->setPos(newPosition);
cursors[i]->setRot(newRotation);
// read linear velocity from device
cVector3d linearVelocity;
hapticDevices[i]->getLinearVelocity(linearVelocity);
// update arrow
// velocityVectors[i]->m_pointA = newPosition;
// velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity);
// read user button status
bool buttonStatus;
hapticDevices[i]->getUserSwitch(0, buttonStatus);
// adjustthe color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (i == 0)
cursors[i]->m_material = matCursor2;
else
cursors[i]->m_material = matCursor1;
// increment counter
i++;
}
double f0, f1;
f0 = pos0.length();
f1 = pos1.length();
f0 = f0/(f0 + f1);
f1 = 1.0 - f0;
equilibrium = pos1 + (pos0 - pos1)*f0;
// Update the position of the sun
cVector3d dir = pos1 - pos0;
double dist = dir.length();
dir.normalize();
double vibrationSpeed = 20.0;
double vibrationAmount = 0.02;
//sun->setPos(sun->getPos()*(1.0 - deltaTime*speed) + equilibrium*(deltaTime*speed));
//timeV += deltaTime*vibrationSpeed*(0.7 - cAbs(f0 - 0.5)*2.0);
sun->setPos(equilibrium /*+ vibrationAmount*dir*cSinRad(timeV)*/);
// Update logic
if (!calibrationFinished) {
label->m_string = "Calibrating, please move the haptic devices around in order to determine their limitations in movement.";
label2->m_string = "Press 'c' to finish calibration.";
if (sun->getPos().x < min.x)
min.x = sun->getPos().x;
if (sun->getPos().y < min.y)
min.y = sun->getPos().y;
if (sun->getPos().z < min.z)
min.z = sun->getPos().z;
if (sun->getPos().x > max.x)
max.x = sun->getPos().x;
if (sun->getPos().y > max.y)
max.y = sun->getPos().y;
if (sun->getPos().z > max.z)
max.z = sun->getPos().z;
} else if (logic->isReady()) {
if (logic->gameIsOver() && !scoreDisplayed) {
std::stringstream strs;
strs << logic->playTime();
std::string playString = strs.str();
// define its position, color and string message
label->m_string = "Congratulation! Your Time: " + playString;
label2->m_string = "Press 'r' to restart!";
for (i = 0; i < numHapticDevices; i++) {
cVector3d zero(0.0, 0.0, 0.0);
hapticDevices[i]->setForce(zero);
}
scoreDisplayed = true;
} else if (!scoreDisplayed) {
label->m_string = "";
label2->m_string = "";
logic->update(deltaTime);
}
for (i = 0; i < numHapticDevices; i++) {
// compute a reaction force
cVector3d newForce (0,0,0);
cVector3d devicePosition;
hapticDevices[i]->getPosition(devicePosition);
devicePosition = deviceToWorld(devicePosition, i);
double k = 0.4;
if (test == 1) k = 0.3;
double dist = (devicePosition - sun->getPos()).length();
//dist=dist-0.1;
newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist);
//double intensity = (newForce.length())*1.0;
//newForce.normalize();
//newForce *= intensity;
// newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist);
if (i == 0) { // Device on positive X (RIGHT)
newForce.x *= -1.0;
newForce.y *= -1.0;
}
// send computed force to haptic device
// bool status = true;
// if (hapticDevices[i]->getUserSwitch(0))
// printf("button pressed\n");
// Check if the sphere is in the target area. If so, vibrate
cVector3d vibrationForce(0.0, 0.0, 0.0);
if (logic->sphereInTarget() && !logic->gameIsOver()) {
Cube* target = logic->getTarget();
double dist = target->getPos().distance(sun->getPos());
double factor = 1.0 - dist/(target->size/2.0);
timeV += deltaTime * (0.5 + factor/2.0);
double f = 2*cSinRad(40*timeV);
vibrationForce = cVector3d(f, f, f);
}
newForce += vibrationForce;
if (test <= 2 || i == 0)
hapticDevices[i]->setForce(newForce);
else {
cVector3d zero;
zero.zero();
hapticDevices[i]->setForce(zero);
}
}
}
}
// exit haptics thread
simulationFinished = true;
}
