void updateHaptics(void)
{
// main haptic simulation loop
while(simulationRunning)
{
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// if the haptic device does track orientations, we automatically
// oriente the drill to remain perpendicular to the tooth
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
cMatrix3d rot = tool->m_deviceGlobalRot;
if (info.m_sensedRotation == false)
{
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
rot.identity();
cVector3d vx, vy, vz;
cVector3d zUp (0,0,1);
cVector3d yUp (0,1,0);
vx = pos - tooth->getPos();
if (vx.length() > 0.001)
{
vx.normalize();
if (cAngle(vx,zUp) > 0.001)
{
vy = cCross(zUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
else
{
vy = cCross(yUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
rot.setCol(vx, vy, vz);
drill->setRot(rot);
}
}
int button = tool->getUserSwitch(0);
if (button == 0)
{
lastPosDevice = pos;
lastRotDevice = rot;
lastPosObject = tooth->getPos();
lastRotObject = tooth->getRot();
lastDeviceObjectPos = cTrans(lastRotDevice) * ((lastPosObject - lastPosDevice) + 0.01*cNormalize(lastPosObject - lastPosDevice));
lastDeviceObjectRot = cMul(cTrans(lastRotDevice), lastRotObject);
tooth->setHapticEnabled(true, true);
tool->setShowEnabled(true, true);
drill->setShowEnabled(true, true);
}
else
{
tool->setShowEnabled(false, true);
drill->setShowEnabled(false, true);
cMatrix3d rotDevice01 = cMul(cTrans(lastRotDevice), rot);
cMatrix3d newRot = cMul(rot, lastDeviceObjectRot);
cVector3d newPos = cAdd(pos, cMul(rot, lastDeviceObjectPos));
tooth->setPos(newPos);
tooth->setRot(newRot);
world->computeGlobalPositions(true);
tooth->setHapticEnabled(false, true);
}
//-----------------Jake--------------//
char serialData[50];
double pos1 = 0;
double pos2 = 0;
if (sp->ReadData(serialData, strlen(serialData)) > 0)
{
if (sp->parse_num(serialData, pos1))
{
Sleep(50);
if (pos1 > 0.3)
{
pos1 = 0.126*pow(pos1,-1.07);
pos1 = (pos1+pos1_1+pos1_2)/3;
//pos1 = (pos1 + pos1_1 + pos1_2 + pos1_3 + pos1_4)/5;
pos1_5 = pos1_4;
pos1_4 = pos1_3;
pos1_3 = pos1_2;
pos1_2 = pos1_1;
pos1_1 = pos1;
pos2 = 0.126*pow(pos2,-1.07);
pos2 = (pos2 + pos2_1 + pos2_2 + pos2_3 + pos2_4)/5;
pos2_5 = pos2_4;
pos2_4 = pos2_3;
pos2_3 = pos2_2;
pos2_2 = pos2_1;
pos2_1 = pos2;
}
else
{
pos1 = previous_pos1;
pos2 = previous_pos2;
}
printf("Received data %f and %f \n", pos1, pos2);
} else
{
printf("Conversion failed\n");
}
} else {
printf("Receive failed\n");
}
if (pos_counter > 2)
{
double dx1 = pos1 - previous_pos1;
double dx2 = pos2 - previous_pos2;
if (abs(dx1) < 0.1)
{
overall_pos1 -= dx1*10;
if (overall_pos1 < -0.1)
{
overall_pos1 = -0.1;
} else if(overall_pos1 > 0)
{
overall_pos1 = 0;
}
}
if (abs(dx2) < 0.1)
{
overall_pos2 -= dx2;
if (overall_pos2 < -0.1)
{
overall_pos2 = -0.1;
} else if(overall_pos2 > 0)
{
overall_pos2 = 0;
}
}
} else {
pos_counter++;
}
previous_pos1 = pos1;
previous_pos2 = pos2;
index_finger->setPos(pos.x - 0.15, pos.y, pos.z + overall_pos1);
index_finger->setRot(rot);
index_finger->computeInteractionForces();
index_finger->updatePose();
thumb->setPos(pos.x + 0.1, pos.y - 0.15 + overall_pos2, pos.z - 0.05);
thumb->setRot(rot);
thumb->computeInteractionForces();
thumb->updatePose();
// compute global reference frames for each object
world->computeGlobalPositions(true);
std::stringstream torque_str_tmp;
double torque_temp1 = sqrt(pow(index_finger->m_lastComputedGlobalForce.x,2) + pow(index_finger->m_lastComputedGlobalForce.y,2) + pow(index_finger->m_lastComputedGlobalForce.z,2));
double torque_temp2 = sqrt(pow(thumb->m_lastComputedGlobalForce.x,2) + pow(thumb->m_lastComputedGlobalForce.y,2) + pow(thumb->m_lastComputedGlobalForce.z,2));
/*torque_str_tmp << std::setprecision(2) << torque_temp;
const std::string& torque_to_send = torque_str_tmp.str();
char to_send[50];
strcpy(to_send, "T");
strcat(to_send, torque_to_send.c_str());
strcat(to_send, ";");
*/
char to_send[50];
if (torque_temp1 > 0 && torque_temp2 > 0)
{
strcpy(to_send, "T");
strcat(to_send, "1/1");
strcat(to_send, ";");
} else if (torque_temp1 == 0 && torque_temp2 > 0)
{
strcpy(to_send, "T");
strcat(to_send, "0/1");
strcat(to_send, ";");
} else if (torque_temp1 > 0 && torque_temp2 == 0)
{
strcpy(to_send, "T");
strcat(to_send, "1/0");
strcat(to_send, ";");
} else
{
strcpy(to_send, "T");
strcat(to_send, "0/0");
strcat(to_send, ";");
}
if (sp->WriteData(to_send, strlen(to_send)))
{
printf("Sent %s\n", to_send);
} else
{
printf("Force data %s could not be sent\n", to_send);
}
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// main haptic simulation loop
while(simulationRunning)
{
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// if the haptic device does track orientations, we automatically
// oriente the drill to remain perpendicular to the tooth
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
cMatrix3d rot = tool->m_deviceGlobalRot;
if (info.m_sensedRotation == false)
{
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
rot.identity();
cVector3d vx, vy, vz;
cVector3d zUp (0,0,1);
cVector3d yUp (0,1,0);
vx = pos - tooth->getPos();
if (vx.length() > 0.001)
{
vx.normalize();
if (cAngle(vx,zUp) > 0.001)
{
vy = cCross(zUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
else
{
vy = cCross(yUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
rot.setCol(vx, vy, vz);
drill->setRot(rot);
}
}
int button = tool->getUserSwitch(0);
if (button == 0)
{
lastPosDevice = pos;
lastRotDevice = rot;
lastPosObject = tooth->getPos();
lastRotObject = tooth->getRot();
lastDeviceObjectPos = cTrans(lastRotDevice) * ((lastPosObject - lastPosDevice) + 0.01*cNormalize(lastPosObject - lastPosDevice));
lastDeviceObjectRot = cMul(cTrans(lastRotDevice), lastRotObject);
tooth->setHapticEnabled(true, true);
tool->setShowEnabled(true, true);
drill->setShowEnabled(true, true);
}
else
{
tool->setShowEnabled(false, true);
drill->setShowEnabled(false, true);
cMatrix3d rotDevice01 = cMul(cTrans(lastRotDevice), rot);
cMatrix3d newRot = cMul(rot, lastDeviceObjectRot);
cVector3d newPos = cAdd(pos, cMul(rot, lastDeviceObjectPos));
tooth->setPos(newPos);
tooth->setRot(newRot);
world->computeGlobalPositions(true);
tooth->setHapticEnabled(false, true);
}
// compute global reference frames for each object
world->computeGlobalPositions(true);
}
// exit haptics thread
simulationFinished = true;
}
