//==============================================================================
void cLookAt(const cVector3d& a_eye, const cVector3d& a_at, const cVector3d& a_up)
{
#ifdef C_USE_OPENGL
// Define our look vector (z axis)
cVector3d look = a_at - a_eye;
look.normalize();
// Define our new x axis
cVector3d xaxis;
xaxis = cCross(look,a_up);
xaxis.normalize();
// Define our new y axis as the cross of the x and z axes
cVector3d upv = cCross(xaxis,look);
// Turn around the z axis
look.mul(-1.0);
// Put it all into a GL-friendly matrix
double matrix[16];
matrix[0] = xaxis(0);
matrix[1] = xaxis(1);
matrix[2] = xaxis(2);
matrix[3] = 0.0;
matrix[4] = upv(0);
matrix[5] = upv(1);
matrix[6] = upv(2);
matrix[7] = 0.0;
matrix[8] = look(0);
matrix[9] = look(1);
matrix[10] = look(2);
matrix[11] = 0.0;
matrix[12] = a_eye(0);
matrix[13] = a_eye(1);
matrix[14] = a_eye(2);
matrix[15] = 1.0;
// Push it onto the matrix stack
glMultMatrixd(matrix);
#endif
}
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;
}
//===========================================================================
void cCamera::renderView(const int a_windowWidth, const int a_windowHeight,
const int a_imageIndex)
{
// store most recent size of display
m_lastDisplayWidth = a_windowWidth;
m_lastDisplayHeight = a_windowHeight;
// set background color
cColorf color = getParentWorld()->getBackgroundColor();
glClearColor(color.getR(), color.getG(), color.getB(), color.getA());
// clear the color and depth buffers
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// compute global pose
computeGlobalCurrentObjectOnly(true);
// check window size
if (a_windowHeight == 0) { return; }
// render the 'back' 2d object layer; it will set up its own
// projection matrix
if (m_back_2Dscene.getNumChildren())
render2dSceneGraph(&m_back_2Dscene,a_windowWidth,a_windowHeight);
// set up perspective projection
double glAspect = ((double)a_windowWidth / (double)a_windowHeight);
// set the perspective up for monoscopic rendering
if (a_imageIndex == CHAI_MONO || a_imageIndex == CHAI_STEREO_DEFAULT)
{
// Set up the projection matrix
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(
m_fieldViewAngle, // Field of View Angle.
glAspect, // Aspect ratio of viewing volume.
m_distanceNear, // Distance to Near clipping plane.
m_distanceFar); // Distance to Far clipping plane.
// Now set up the view matrix
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// render pose
cVector3d lookAt = m_globalRot.getCol0();
cVector3d lookAtPos;
m_globalPos.subr(lookAt, lookAtPos);
cVector3d up = m_globalRot.getCol2();
gluLookAt( m_globalPos.x, m_globalPos.y, m_globalPos.z,
lookAtPos.x, lookAtPos.y, lookAtPos.z,
up.x, up.y, up.z );
}
// set the perspective up for stereoscopic rendering
else
{
// Based on Paul Bourke's stereo rendering tutorial:
//
// http://astronomy.swin.edu.au/~pbourke/opengl/stereogl/
double radians = ((CHAI_PI / 180.0) * m_fieldViewAngle / 2.0f);
double wd2 = m_distanceNear * tan(radians);
double ndfl = m_distanceNear / m_stereoFocalLength;
// compute the look, up, and cross vectors
cVector3d lookv = m_globalRot.getCol0();
lookv.mul(-1.0);
cVector3d upv = m_globalRot.getCol2();
cVector3d offsetv = cCross(lookv,upv);
offsetv.mul(m_stereoEyeSeparation / 2.0);
if (a_imageIndex == CHAI_STEREO_LEFT) offsetv.mul(-1.0);
// decide whether to offset left or right
double stereo_multiplier = (a_imageIndex == CHAI_STEREO_LEFT) ? 1.0f : -1.0f;
double left = -1.0 * glAspect * wd2 + stereo_multiplier * 0.5 * m_stereoEyeSeparation * ndfl;
double right = glAspect * wd2 + stereo_multiplier * 0.5 * m_stereoEyeSeparation * ndfl;
double top = wd2;
double bottom = -1.0 * wd2;
// Set up the projection matrix
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glFrustum(left,right,bottom,top,m_distanceNear,m_distanceFar);
// Now set up the view matrix
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// compute the offset we should apply to the current camera position
cVector3d pos = cAdd(m_globalPos,offsetv);
// compute the shifted camera position
cVector3d lookAtPos;
pos.addr(lookv, lookAtPos);
// set up the view matrix
gluLookAt(pos.x, pos.y, pos.z,
lookAtPos.x, lookAtPos.y, lookAtPos.z,
upv.x, upv.y, upv.z
);
}
for(unsigned int i=0; i<CHAI_MAX_CLIP_PLANES; i++) {
if (m_clipPlanes[i].enabled==1) {
glEnable(GL_CLIP_PLANE0+i);
glClipPlane(GL_CLIP_PLANE0+i,m_clipPlanes[i].peqn);
}
else if (m_clipPlanes[i].enabled==0) {
glDisable(GL_CLIP_PLANE0+i);
}
else if (m_clipPlanes[i].enabled==-1) {
// Don't touch
}
}
// Back up the projection matrix for future reference
glGetDoublev(GL_PROJECTION_MATRIX,m_projectionMatrix);
// Set up reasonable default OpenGL state
glEnable(GL_LIGHTING);
glDisable(GL_BLEND);
glDepthMask(GL_TRUE);
glEnable(GL_DEPTH_TEST);
// optionally perform multiple rendering passes for transparency
if (m_useMultipassTransparency) {
m_parentWorld->renderSceneGraph(CHAI_RENDER_MODE_NON_TRANSPARENT_ONLY);
m_parentWorld->renderSceneGraph(CHAI_RENDER_MODE_TRANSPARENT_BACK_ONLY);
m_parentWorld->renderSceneGraph(CHAI_RENDER_MODE_TRANSPARENT_FRONT_ONLY);
}
else
{
m_parentWorld->renderSceneGraph(CHAI_RENDER_MODE_RENDER_ALL);
}
// render the 'front' 2d object layer; it will set up its own
// projection matrix
if (m_front_2Dscene.getNumChildren())
render2dSceneGraph(&m_front_2Dscene,a_windowWidth,a_windowHeight);
}
//===========================================================================
cCollisionSpheresTri::cCollisionSpheresTri(cVector3d a, cVector3d b, cVector3d c)
{
// Calculate the center of the circumscribing sphere for this triangle:
// First compute the normal to the plane of this triangle
cVector3d plane_normal = cCross(a-b, a-c);
// Compute the perpendicular bisector of the edge between points a and b
cVector3d bisector1_dir = cCross(a-b, plane_normal);
cVector3d bisector1_pt = (a + b) / 2.0;
// Compute the perpendicular bisector of the edge between points b and c
cVector3d bisector2_dir = cCross(b-c, plane_normal);
cVector3d bisector2_pt = (b + c) / 2.0;
// Find the intersection of the perpendicular bisectors to find the center
// of the circumscribed sphere, using the formula for 3D line-line
// intersection given at
// http://cglab.snu.ac.kr/research/seminar/data01-1/RealTimeRendering10_1.ppt
cVector3d mat[3];
mat[0] = bisector2_pt - bisector1_pt;
mat[1] = bisector2_dir;
mat[2] = cCross(bisector1_dir, bisector2_dir);
float det = (float)(
mat[0].x*mat[1].y*mat[2].z + mat[1].x*mat[2].y*mat[0].z +
mat[2].x*mat[0].y*mat[1].z - mat[0].z*mat[1].y*mat[2].z -
mat[1].z*mat[2].y*mat[0].x - mat[2].z*mat[0].y*mat[1].x);
cVector3d cp = cCross(bisector1_dir, bisector2_dir);
if (cp.lengthsq() > CHAI_SMALL)
{
float s = (float)(det / cp.lengthsq());
m_center = bisector1_pt + s * bisector1_dir;
}
else
{
m_center = (a + b)/2.0;
}
// set the vertices (corners) of the triangle
m_corner[0].m_pos = a;
m_corner[1].m_pos = b;
m_corner[2].m_pos = c;
// Calculate a radius of the bounding sphere as the largest distance between
// the sphere center calculated above and any vertex of the triangle
m_radius = 0;
unsigned int i, j, k;
for (i = 0; i < 3; i++)
{
double curRadius = m_corner[i].m_pos.distance(m_center);
if (curRadius > m_radius)
m_radius = curRadius;
}
// See if we could get a smaller bounding sphere by just taking one of the edges
// of the triangle as a diameter (this may be better for long, skinny triangles)
for (i=0; i<3; i++)
{
for (j=i; j<3; j++)
{
// Calculate the center for this edge, and determine necessary sphere radius
cVector3d candidate_center;
candidate_center.x = (m_corner[i].m_pos.x + m_corner[j].m_pos.x) / 2.0;
candidate_center.y = (m_corner[i].m_pos.y + m_corner[j].m_pos.y) / 2.0;
candidate_center.z = (m_corner[i].m_pos.z + m_corner[j].m_pos.z) / 2.0;
double candidate_radius = 0.0;
for (k = 0; k < 3; k++)
{
double curRad = m_corner[k].m_pos.distance(candidate_center);
if (curRad > candidate_radius) candidate_radius = curRad;
}
// If this results in a smaller sphere, use it
if (candidate_radius < m_radius)
{
m_radius = candidate_radius;
m_center = candidate_center;
}
}
}
}
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 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)
{
// 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 cCamera::renderView(const int a_windowWidth,
const int a_windowHeight)
{
//-----------------------------------------------------------------------
// (1) COMPUTE SHADOW MAPS
//-----------------------------------------------------------------------
// initialize a temporary variable that will inform us if shadow casting is used
// by our application and also supported by the hardware.
bool useShadowCasting = false;
list<cShadowMap*> shadowMaps;
shadowMaps.clear();
// shadow casting has been requested, we will first need to verify if the hardware
// can support it
if (m_useShadowCasting)
{
// we verify if shadow casting is supported by hardware
if (isShadowCastingSupported())
{
// we check every light source. if it is a spot light we verify if shadow casting is enabled
for (unsigned int i=0; i<m_parentWorld->m_lights.size(); i++)
{
cSpotLight* light = dynamic_cast<cSpotLight*>(m_parentWorld->getLightSource(i));
if (light != NULL)
{
if (light->getShadowMapEnabled())
{
// update shadow map
light->updateShadowMap();
// add shadowmap to list
shadowMaps.push_back(light->m_shadowMap);
// shadow mapping is used by at least one light source!
useShadowCasting = true;
}
}
}
}
}
//-----------------------------------------------------------------------
// (2) INITIALIZE CURRENT VIEWPORT
//-----------------------------------------------------------------------
// check window size
if (a_windowHeight == 0) { return; }
// store most recent size of display
m_lastDisplayWidth = a_windowWidth;
m_lastDisplayHeight = a_windowHeight;
// setup viewport
glViewport(0, 0, a_windowWidth, a_windowHeight);
// compute aspect ratio
double glAspect = ((double)a_windowWidth / (double)a_windowHeight);
// compute global pose
computeGlobalPositionsFromRoot(true);
// set background color
glClearColor(m_parentWorld->getBackgroundColor().getR(),
m_parentWorld->getBackgroundColor().getG(),
m_parentWorld->getBackgroundColor().getB(),
1.0);
//-----------------------------------------------------------------------
// (3) VERIFY IF STEREO DISPLAY IS ENABLED
//-----------------------------------------------------------------------
GLboolean stereo = false;
unsigned int numStereoPass = 1;
if (m_useStereo)
{
// verify if stereo is available by the graphics hardware and camera is of perspective model
glGetBooleanv(GL_STEREO, &stereo);
if (stereo && m_perspectiveMode)
{
// stereo is available - we shall perform 2 rendering passes for LEFT and RIGHT eye.
numStereoPass = 2;
}
else
{
stereo = false;
}
}
//-----------------------------------------------------------------------
// (4) RENDER THE ENTIRE SCENE
//-----------------------------------------------------------------------
for (unsigned int i=0; i<numStereoPass; i++)
{
//-------------------------------------------------------------------
// (4.1) SELECTING THE DISPLAY BUFFER (MONO / STEREO)
//-------------------------------------------------------------------
if (stereo)
{
if (i == 0)
{
glDrawBuffer(GL_BACK_LEFT);
}
else
{
glDrawBuffer(GL_BACK_RIGHT);
}
}
else
{
glDrawBuffer(GL_BACK);
}
//-------------------------------------------------------------------
// (4.2) CLEAR CURRENT BUFFER
//-------------------------------------------------------------------
// clear the color and depth buffers
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
glShadeModel(GL_SMOOTH);
//-------------------------------------------------------------------
// (4.3) RENDER BACK PLANE
//-------------------------------------------------------------------
// render the 2D backlayer
// it will set up its own projection matrix
if (m_backLayer->getNumChildren())
{
renderLayer(m_backLayer,
a_windowWidth,
a_windowHeight);
}
// clear depth buffer
glClear(GL_DEPTH_BUFFER_BIT);
//-------------------------------------------------------------------
// (4.4a) SETUP CAMERA (MONO RENDERING)
//-------------------------------------------------------------------
if (!stereo)
{
// init projection matrix
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
// create projection matrix depending of camera mode
if (m_perspectiveMode)
{
// setup perspective camera
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(
m_fieldViewAngle, // Field of View angle.
glAspect, // Aspect ratio of viewing volume.
m_distanceNear, // Distance to Near clipping plane.
m_distanceFar); // Distance to Far clipping plane.
}
else
{
// setup orthographic camera
double left = -m_orthographicWidth / 2.0;
double right = -left;
double bottom = left / glAspect;
double top = -bottom;
glOrtho(left, // Left vertical clipping plane.
right, // Right vertical clipping plane.
bottom, // Bottom vertical clipping plane.
top, // Top vertical clipping plane.
m_distanceNear, // Distance to Near clipping plane.
m_distanceFar // Distance to Far clipping plane.
);
}
// setup cameta position
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// compute camera location
cVector3d lookAt = m_globalRot.getCol0();
cVector3d lookAtPos;
m_globalPos.subr(lookAt, lookAtPos);
cVector3d up = m_globalRot.getCol2();
// setup modelview matrix
gluLookAt( m_globalPos(0) , m_globalPos(1) , m_globalPos(2) ,
lookAtPos(0) , lookAtPos(1) , lookAtPos(2) ,
up(0) , up(1) , up(2) );
}
//-------------------------------------------------------------------
// (4.4b) SETUP CAMERA (STEREO RENDERING)
//-------------------------------------------------------------------
else
{
//-----------------------------------------------------------------
// Based on Paul Bourke's stereo rendering tutorial:
// http://local.wasp.uwa.edu.au/~pbourke/miscellaneous/stereographics/stereorender/
//-----------------------------------------------------------------
double radians = ((C_PI / 180.0) * m_fieldViewAngle / 2.0f);
double wd2 = m_distanceNear * tan(radians);
double ndfl = m_distanceNear / m_stereoFocalLength;
// compute the look, up, and cross vectors
cVector3d lookv = m_globalRot.getCol0();
lookv.mul(-1.0);
cVector3d upv = m_globalRot.getCol2();
cVector3d offsetv = cCross(lookv,upv);
offsetv.mul(m_stereoEyeSeparation / 2.0);
if (i == 0) offsetv.mul(-1.0);
// decide whether to offset left or right
double stereo_multiplier = (i == 0) ? 1.0f : -1.0f; // (i == 0) correspond to LEFT IMAGE, (i == 1) correspond to RIGHT IMAGE
double left = -1.0 * glAspect * wd2 + stereo_multiplier * 0.5 * m_stereoEyeSeparation * ndfl;
double right = glAspect * wd2 + stereo_multiplier * 0.5 * m_stereoEyeSeparation * ndfl;
double top = wd2;
double bottom = -1.0 * wd2;
// setup projection matrix
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glFrustum(left,right,bottom,top,m_distanceNear,m_distanceFar);
// initialize modelview matrix
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// compute the offset we should apply to the current camera position
cVector3d pos = cAdd(m_globalPos,offsetv);
// compute the shifted camera position
cVector3d lookAtPos;
pos.addr(lookv, lookAtPos);
// setup modelview matrix
gluLookAt(pos(0) , pos(1) , pos(2) ,
lookAtPos(0) , lookAtPos(1) , lookAtPos(2) ,
upv(0) , upv(1) , upv(2)
);
}
// Backup the view and projection matrix for future reference
glGetDoublev(GL_PROJECTION_MATRIX,m_projectionMatrix.getData());
glGetDoublev(GL_MODELVIEW_MATRIX, m_modelviewMatrix.getData());
//-------------------------------------------------------------------
// (4.5) RENDER THE 3D WORLD
//-------------------------------------------------------------------
// Set up reasonable default OpenGL state
glEnable(GL_LIGHTING);
glDisable(GL_BLEND);
glDepthMask(GL_TRUE);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
// rendering options
cRenderOptions options;
// optionally perform multiple rendering passes for transparency
if (m_useMultipassTransparency)
{
////////////////////////////////////////////////////////////////////
// MULTI PASS - USING SHADOW CASTING
////////////////////////////////////////////////////////////////////
if (useShadowCasting)
{
// setup rendering options
options.m_camera = this;
options.m_single_pass_only = false;
options.m_render_opaque_objects_only = true;
options.m_render_transparent_front_faces_only = false;
options.m_render_transparent_back_faces_only = false;
options.m_enable_lighting = true;
options.m_render_materials = true;
options.m_render_textures = true;
options.m_creating_shadow_map = false;
options.m_rendering_shadow = true;
options.m_shadow_light_level = 1.0 - m_parentWorld->getShadowIntensity();
options.m_storeObjectPositions = false;
options.m_resetDisplay = m_resetDisplay;
// render 1st pass (opaque objects - shadowed regions)
m_parentWorld->renderSceneGraph(options);
// setup rendering options
options.m_rendering_shadow = false;
options.m_shadow_light_level = 1.0;
// render 2nd pass (opaque objects - non shadowed regions)
list<cShadowMap*>::iterator i;
for(i = shadowMaps.begin(); i !=shadowMaps.end(); ++i)
{
cShadowMap *shadowMap = *i;
shadowMap->render(options);
glEnable(GL_POLYGON_OFFSET_FILL);
m_parentWorld->renderSceneGraph(options);
glDisable(GL_POLYGON_OFFSET_FILL);
// restore states
glActiveTexture(GL_TEXTURE0_ARB);
glDisable(GL_TEXTURE_2D);
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glDisable(GL_TEXTURE_GEN_R);
glDisable(GL_TEXTURE_GEN_Q);
glDisable(GL_ALPHA_TEST);
}
// setup rendering options
options.m_render_opaque_objects_only = false;
options.m_render_transparent_back_faces_only = true;
// render 3rd pass (transparent objects - back faces only)
m_parentWorld->renderSceneGraph(options);
// modify rendering options for third pass
options.m_render_transparent_back_faces_only = false;
options.m_render_transparent_front_faces_only = true;
// render 4th pass (transparent objects - back faces only)
m_parentWorld->renderSceneGraph(options);
}
////////////////////////////////////////////////////////////////////
// MULTI PASS - WITHOUT SHADOWS
////////////////////////////////////////////////////////////////////
else
{
// setup rendering options for first pass
options.m_camera = this;
options.m_single_pass_only = false;
options.m_render_opaque_objects_only = true;
options.m_render_transparent_front_faces_only = false;
options.m_render_transparent_back_faces_only = false;
options.m_enable_lighting = true;
options.m_render_materials = true;
options.m_render_textures = true;
options.m_creating_shadow_map = false;
options.m_rendering_shadow = false;
options.m_shadow_light_level = 1.0;
options.m_storeObjectPositions = true;
options.m_resetDisplay = m_resetDisplay;
// render 1st pass (opaque objects - all faces)
m_parentWorld->renderSceneGraph(options);
// modify rendering options
options.m_render_opaque_objects_only = false;
options.m_render_transparent_back_faces_only = true;
options.m_storeObjectPositions = false;
// render 2nd pass (transparent objects - back faces only)
m_parentWorld->renderSceneGraph(options);
// modify rendering options
options.m_render_transparent_back_faces_only = false;
options.m_render_transparent_front_faces_only = true;
// render 3rd pass (transparent objects - front faces only)
m_parentWorld->renderSceneGraph(options);
}
}
else
{
////////////////////////////////////////////////////////////////////
// SINGLE PASS - USING SHADOW CASTING
////////////////////////////////////////////////////////////////////
if (useShadowCasting)
{
// setup rendering options for single pass
options.m_camera = this;
options.m_single_pass_only = true;
options.m_render_opaque_objects_only = false;
options.m_render_transparent_front_faces_only = false;
options.m_render_transparent_back_faces_only = false;
options.m_enable_lighting = true;
options.m_render_materials = true;
options.m_render_textures = true;
options.m_creating_shadow_map = false;
options.m_rendering_shadow = true;
options.m_shadow_light_level = 1.0 - m_parentWorld->getShadowIntensity();
options.m_storeObjectPositions = false;
options.m_resetDisplay = m_resetDisplay;
// render 1st pass (opaque objects - all faces - shadowed regions)
m_parentWorld->renderSceneGraph(options);
// setup rendering options
options.m_rendering_shadow = false;
options.m_shadow_light_level = 1.0;
// render 2nd pass (opaque objects - all faces - non shadowed regions)
list<cShadowMap*>::iterator i;
for(i = shadowMaps.begin(); i !=shadowMaps.end(); ++i)
{
cShadowMap *shadowMap = *i;
shadowMap->render(options);
glEnable(GL_POLYGON_OFFSET_FILL);
m_parentWorld->renderSceneGraph(options);
glDisable(GL_POLYGON_OFFSET_FILL);
// restore states
glActiveTexture(GL_TEXTURE0_ARB);
glDisable(GL_TEXTURE_2D);
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glDisable(GL_TEXTURE_GEN_R);
glDisable(GL_TEXTURE_GEN_Q);
glDisable(GL_ALPHA_TEST);
}
// restore states
glActiveTexture(GL_TEXTURE0_ARB);
glDisable(GL_TEXTURE_2D);
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glDisable(GL_TEXTURE_GEN_R);
glDisable(GL_TEXTURE_GEN_Q);
glDisable(GL_ALPHA_TEST);
}
////////////////////////////////////////////////////////////////////
// SINGLE PASS - WITHOUT SHADOWS
////////////////////////////////////////////////////////////////////
else
{
// setup rendering options for single pass
options.m_camera = this;
options.m_single_pass_only = true;
options.m_render_opaque_objects_only = false;
options.m_render_transparent_front_faces_only = false;
options.m_render_transparent_back_faces_only = false;
options.m_enable_lighting = true;
options.m_render_materials = true;
options.m_render_textures = true;
options.m_creating_shadow_map = false;
options.m_rendering_shadow = false;
options.m_shadow_light_level = 1.0;
options.m_storeObjectPositions = true;
options.m_resetDisplay = m_resetDisplay;
// render single pass (all objects)
m_parentWorld->renderSceneGraph(options);
}
}
//-------------------------------------------------------------------
// (4.6) RENDER FRONT PLANE
//-------------------------------------------------------------------
// clear depth buffer
glClear(GL_DEPTH_BUFFER_BIT);
// render the 'front' 2d object layer; it will set up its own
// projection matrix
if (m_frontLayer->getNumChildren() > 0)
{
renderLayer(m_frontLayer,
a_windowWidth,
a_windowHeight);
}
// if requested, display reset has now been completed
m_resetDisplay = false;
}
}
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();
}
