cVector3d computeForce(const cVector3d& a_cursor,
double a_cursorRadius,
const cVector3d& a_spherePos,
double a_radius,
double a_stiffness)
{
// compute the reaction forces between the tool and the ith sphere.
cVector3d force;
force.zero();
cVector3d vSphereCursor = a_cursor - a_spherePos;
// check if both objects are intersecting
if (vSphereCursor.length() < 0.0000001)
{
return (force);
}
if (vSphereCursor.length() > (a_cursorRadius + a_radius))
{
return (force);
}
// compute penetration distance between tool and surface of sphere
double penetrationDistance = (a_cursorRadius + a_radius) - vSphereCursor.length();
cVector3d forceDirection = cNormalize(vSphereCursor);
force = cMul( penetrationDistance * a_stiffness, forceDirection);
// return result
return (force);
}
//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
cVector3d toolProjection = a_toolPos;
toolProjection.z(0.0);
m_interactionNormal.set(0,0,1);
// search for the nearest point on the torus medial axis
if (a_toolPos.lengthsq() > C_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));
// compute eventual penetration of tool inside the torus
cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);
double distance = vectTorusTool.length();
// normal
if (distance > 0.0)
{
m_interactionNormal = vectTorusTool;
m_interactionNormal.normalize();
}
// tool is located inside the torus
if ((distance < m_innerRadius) && (distance > 0.001))
{
m_interactionInside = true;
}
// tool is located outside the torus
else
{
m_interactionInside = false;
}
// compute surface point
double dist = vectTorusTool.length();
if (dist > 0)
{
vectTorusTool.mul(1/dist);
}
vectTorusTool.mul(m_innerRadius);
pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
}
else
{
m_interactionInside = false;
m_interactionPoint = a_toolPos;
}
}
//===========================================================================
cVector3d cShapeTorus::computeLocalForce(const cVector3d& a_localPosition)
{
// In the following we compute the reaction forces between the tool and the
// sphere.
cVector3d localForce;
// project pointer on torus plane (z=0)
cVector3d fingerProjection = a_localPosition;
fingerProjection.z = 0;
// search for the nearest point on the torus medial axis
if (a_localPosition.lengthsq() > CHAI_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(fingerProjection));
// compute eventual penetration of finger inside the torus
cVector3d vectTorusFinger = cSub(a_localPosition, pointAxisTorus);
double distance = vectTorusFinger.length();
// finger inside torus, compute forces
if ((distance < m_innerRadius) && (distance > 0.001))
{
localForce = cMul((m_innerRadius - distance) * (m_material.getStiffness()), cNormalize(vectTorusFinger));
}
// finger is outside torus
else
{
localForce.zero();
}
}
else
{
localForce.zero();
}
return (localForce);
}
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);
}
}
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;
}
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();
}
