void updateGraphics(void)
{
int px, py;
// update haptic rate label
labelHapticRate->setString ("haptic rate: "+cStr(frequencyCounter.getFrequency(), 0) + " [Hz]");
px = (int)(0.5 * (displayW - labelHapticRate->getWidth()));
labelHapticRate->setLocalPos(px, 15);
// update other widgets
py = (int)(0.5 * (displayH - level->getHeight()));
level->setLocalPos(50, py);
level->setValue(tool->m_lastComputedGlobalForce.length());
px = displayW - 80;
py = (int)(0.5 * displayH);
dial->setLocalPos(px, py);
dial->setValue1(angVel.length());
// render world
camera->renderView(displayW, displayH);
// swap buffers
glutSwapBuffers();
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));
}
//===========================================================================
void cShapeSphere::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
// compute distance from center of sphere to tool
double distance = a_toolPos.length();
// from the position of the tool, search for the nearest point located
// on the surface of the sphere
if (distance > 0)
{
m_interactionProjectedPoint = cMul( (m_radius/distance), a_toolPos);
}
else
{
m_interactionProjectedPoint = a_toolPos;
}
// check if tool is located inside or outside of the sphere
if (distance <= m_radius)
{
m_interactionInside = true;
}
else
{
m_interactionInside = false;
}
}
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;
}
// ch lab ---to be filled in by the students---
// Overriden from class cProxyPointForceAlgo
// Here, a_goal has been computed by collision detection above as the constrained goal towards which
// the proxy should be moved. But before moving it freely to that location, let us check if friction allows
// us to do so. we answer the question: to what extent along the proxy-goal segment can we forward the proxy?
void ch_proxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal, const cVector3d& a_proxy, cVector3d& a_normal, cGenericObject* a_parent, const cVector3d& a_toolVel)
{
// In this method, our aim is to calculate the the next best position for the proxy (m_nextBestProxyGlobalPos), considering friction.
// Among other things, we are given the goal position (a_goal), the current proxy position (a_proxy), the parent object (a_parent),
// the tool velocity (a_toolVel)
// We will use this variable to determine if we should use the static or the dynamic
// friction coeff to compute current frictional force.
static double last_device_vel;
// friction coefficients assigned to object surface
cMesh* parent_mesh = dynamic_cast<cMesh*>(a_parent);
// Right now we can only work with cMesh's
if (parent_mesh == NULL)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// read friction coefficients here
// -----------------------your code here------------------------------------
double dynamic_coeff = parent_mesh->m_material.getDynamicFriction();
double static_coeff = parent_mesh->m_material.getStaticFriction();
// find the penetration depth of the actual device position from the nominal object surface
double pen_depth = cDistance(a_goal, m_deviceGlobalPos);
// shall we use the static or the dynamic friction coeff. for the cone radius calculation?
double cone_radius;
if(last_device_vel < SMALL_VEL)
{
//// the radius of the friction cone
//-----------------------your code here------------------------------------
cone_radius = static_coeff*pen_depth;
}
else
{
//// the radius of the friction cone
//-----------------------your code here------------------------------------
cone_radius = dynamic_coeff*pen_depth;
}
// vector from the current proxy position to the new sub-goal
cVector3d a_proxyGoal, a_proxyGoalNormalized;
a_goal.subr(a_proxy, a_proxyGoal);
// normalize the proxy goal vector
a_proxyGoal.normalizer(a_proxyGoalNormalized);
double a_proxyGoalLength = a_proxyGoal.length();
if(a_proxyGoalLength < cone_radius)
// The proxy is inside the friction cone already.
return;
else
{
// The proxy is outside the friction cone, we should advance it towards the cone circumference,
// along the vector from the current proxy position to the current goal position
//-----------------------your code here------------------------------------
// calculate a value for m_nextBestProxyGlobalPos
m_nextBestProxyGlobalPos=a_proxy+(a_proxyGoalLength-cone_radius)*a_proxyGoalNormalized;
}
// record last velocity in order to decide if static or dynamic friction is to be applied during the
// next iteration
last_device_vel = a_toolVel.length();
}
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();
}
