//============================================================================== 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(); }