void updateCameraPosition()
{
// check values
if (cameraDistance < 0.1) { cameraDistance = 0.1; }
if (cameraAngleV > 89) { cameraAngleV = 89; }
if (cameraAngleV < 10) { cameraAngleV = 10; }
// compute position of camera in space
cVector3d pos = cAdd(
cameraPosition,
cVector3d(
cameraDistance * cCosDeg(cameraAngleH) * cCosDeg(cameraAngleV),
cameraDistance * cSinDeg(cameraAngleH) * cCosDeg(cameraAngleV),
cameraDistance * cSinDeg(cameraAngleV)
)
);
// compute lookat position
cVector3d lookat = cameraPosition;
// define role orientation of camera
cVector3d up(0.0, 0.0, 1.0);
// set new position to camera
camera->set(pos, lookat, up);
// recompute global positions
world->computeGlobalPositions(true);
}
int main()
{
char s[100];
double a, b;
struct complex C1, C2;
printf("Введите комплексное число в формате a, b: ");
scanf("%lf%lf", &a, &b);
C1 = cRead(a, b);
cPrint(C1);
printf("Введите комплексное число в формате a,b: ");
scanf("%lf%lf", &a, &b);
C2 = cRead(a,b);
cPrint(C2);
printf("РЎСѓРјРјР°: ");
cPrint(cAdd(C1, C2));
printf("Разность: ");
cPrint(cSub(C1, C2));
printf("Произведение: ");
cPrint(cMul(C1, C2));
printf("Частное: ");
cPrint(cDiv(C1, C2));
printf("Модуль 1-ого: %f: \n", cAbs(C1));
printf("Аргумент 1-ого: %f: \n", cArg(C1));
printf("Сопряжённое 1-ого: "); cPrint(cConj(C1));
printf("Re 1-РѕРіРѕ: %f: \n", cReal(C1));
printf("Im 1-РѕРіРѕ: %f: \n", cImag(C1));
return 0;
}
int main(){
Complex C1, C2, CADD;
C1 = cRead();
C2 = cRead();
Complex_Output(C1);
Complex_Output(C2);
CADD = cAdd(C1, C2);
Complex_Output(CADD);
Complex_Output(cMul(C1,C2));
printf("\nArg(C1) = %lf", Arg(C1));
printf("\nMODUL(C2) = %lf", Modul(C1));
return 0;
}
//===========================================================================
cCollisionSpheresLine::cCollisionSpheresLine(cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB)
{
// calculate the center of the line segment
m_center = cMul(0.5, cAdd(a_segmentPointA, a_segmentPointB));
// calculate the radius of the bounding sphere as the distance from the
// center of the segment (calculated above) to an endpoint
m_radius = cDistance(m_center, a_segmentPointA);
// store segment
m_segmentPointA = a_segmentPointA;
m_segmentPointB = a_segmentPointB;
}
//===========================================================================
void cSpotLight::updateShadowMap()
{
// sanity check
if ((!m_enabled) || (m_shadowMap == NULL)) { return; }
// update shadow map
cVector3d tmp0 = getGlobalPos();
cVector3d tmp1 = cAdd(getGlobalPos(), getGlobalRot().getCol0());
cVector3d tmp2 = getGlobalRot().getCol2();
m_shadowMap->updateMap(m_worldParent,
tmp0,
tmp1,
tmp2,
m_cutOffAngleDEG,
m_shadowNearClippingPlane,
m_shadowFarClippingPlane);
}
//===========================================================================
bool cCamera::select(const int a_windowPosX,
const int a_windowPosY,
const int a_windowWidth,
const int a_windowHeight,
cCollisionRecorder& a_collisionRecorder,
cCollisionSettings& a_collisionSettings)
{
// clear collision recorder
a_collisionRecorder.clear();
// update my m_globalPos and m_globalRot variables
m_parentWorld->computeGlobalPositions(false);
// make sure we have a legitimate field of view
if (fabs(m_fieldViewAngle) < 0.001f) { return (false); }
// compute the ray that leaves the eye point at the appropriate angle
//
// m_fieldViewAngle / 2.0 would correspond to the _top_ of the window
double distCam = (a_windowHeight / 2.0f) / cTanDeg(m_fieldViewAngle / 2.0f);
cVector3d selectRay;
selectRay.set(-distCam,
(a_windowPosX - (a_windowWidth / 2.0f)),
((a_windowHeight / 2.0f) - a_windowPosY));
selectRay.normalize();
selectRay = cMul(m_globalRot, selectRay);
// create a point that's way out along that ray
cVector3d selectPoint = cAdd(m_globalPos, cMul(100000, selectRay));
// search for intersection between the ray and objects in the world
bool result = m_parentWorld->computeCollisionDetection(
m_globalPos,
selectPoint,
a_collisionRecorder,
a_collisionSettings);
// return result
return result;
}
//===========================================================================
cCollisionSpheresLine::cCollisionSpheresLine(cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB)
{
// calculate the center of the line segment
m_center = cAdd(a_segmentPointA, a_segmentPointB);
m_center.x *= 0.5;
m_center.y *= 0.5;
m_center.z *= 0.5;
// calculate the radius of the bounding sphere as the distance from the
// center of the segment (calculated above) to an endpoint
cVector3d rad = cSub(m_center, a_segmentPointA);
m_radius = sqrt(rad.x*rad.x + rad.y*rad.y + rad.z*rad.z);
// set origin and direction of the line segment; i.e., redefine the segment
// as a ray from the first endpoint (presumably the proxy position when
// the collision detection is being used with the proxy force algorithm) to
// the second endpoint (presumably the goal position)
m_segmentPointA = a_segmentPointA;
a_segmentPointB.subr(a_segmentPointA, m_dir);
}
//===========================================================================
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);
}
bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints22(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// The proxy is now constrained by two triangles and can only move along
// a virtual line; we now calculate the nearest point to the original
// goal (device position) along this line by projecting the ideal
// goal onto the line.
//
// The line is expressed by the cross product of both surface normals,
// which have both been oriented to point away from the device
cVector3d line;
m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal.crossr(m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal, line);
// check result.
if (line.equals(cVector3d(0,0,0)))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 2;
return (false);
}
line.normalize();
// Compute the projection of the device position (goal) onto the line; this
// gives us the new goal position.
cVector3d goalGlobalPos = cProjectPointOnLine(a_goalGlobalPos, m_proxyGlobalPos, line);
// A vector from the proxy to the goal
cVector3d vProxyToGoal;
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(m_proxyGlobalPos, goalGlobalPos))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
m_numContacts = 2;
return (false);
}
// compute the normalized form of the vector going from the
// current proxy position to the desired goal position
cVector3d vProxyToGoalNormalized;
vProxyToGoal.normalizer(vProxyToGoalNormalized);
// Test whether the path from the proxy to the goal is obstructed.
// For this we create a segment that goes from the proxy position to
// the goal position plus a little extra to take into account the
// physical radius of the proxy.
cVector3d targetPos = goalGlobalPos +
cMul(m_epsilonCollisionDetection, vProxyToGoalNormalized);
// setup collision detector
m_collisionSettings.m_collisionRadius = m_radius;
// search for collision
m_collisionSettings.m_adjustObjectMotion = false;
m_collisionRecorderConstraint2.clear();
bool hit = m_world->computeCollisionDetection( m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint2,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint2.m_nearestCollision.m_squareDistance);
if (collisionDistance > (cDistance(m_proxyGlobalPos, goalGlobalPos) + CHAI_SMALL))
{
hit = false;
}
else
{
// a collision has occurred and we check if the distance from the
// proxy to the collision is smaller than epsilon. If yes, then
// we reduce the epsilon term in order to avoid possible "pop through"
// effect if we suddenly push the proxy "up" again.
if (collisionDistance < m_epsilon)
{
m_epsilon = collisionDistance;
if (m_epsilon < m_epsilonMinimalValue)
{
m_epsilon = m_epsilonMinimalValue;
}
}
}
}
// If no collision occurs, we move the proxy to its goal, unless
// friction prevents us from doing so
if (!hit)
{
cVector3d normal = cMul(0.5,cAdd(m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal,
m_collisionRecorderConstraint1.m_nearestCollision.m_globalNormal));
testFrictionAndMoveProxy(goalGlobalPos,
m_proxyGlobalPos,
normal,
m_collisionRecorderConstraint1.m_nearestCollision.m_triangle->getParent(), a_toolVel);
m_numContacts = 2;
m_algoCounter = 0;
return (false);
}
//-----------------------------------------------------------------------
// THIRD COLLISION OCCURES:
//-----------------------------------------------------------------------
// We want the center of the proxy to move as far toward the triangle as it can,
// but we want it to stop when the _sphere_ representing the proxy hits the
// triangle. We want to compute how far the proxy center will have to
// be pushed _away_ from the collision point - along the vector from the proxy
// to the goal - to keep a distance m_radius between the proxy center and the
// triangle.
//
// So we compute the cosine of the angle between the normal and proxy-goal vector...
double cosAngle = vProxyToGoalNormalized.dot(m_collisionRecorderConstraint2.m_nearestCollision.m_globalNormal);
// Now we compute how far away from the collision point - _backwards_
// along vProxyGoal - we have to put the proxy to keep it from penetrating
// the triangle.
//
// If only ASCII art were a little more expressive...
double distanceTriangleProxy = m_epsilon / cAbs(cosAngle);
if (distanceTriangleProxy > collisionDistance) {
distanceTriangleProxy = cMax(collisionDistance, m_epsilon);
}
// We compute the projection of the vector between the proxy and the collision
// point onto the normal of the triangle. This is the direction in which
// we'll move the _goal_ to "push it away" from the triangle (to account for
// the radius of the proxy).
// A vector from the most recent collision point to the proxy
cVector3d vCollisionToProxy;
m_proxyGlobalPos.subr(m_contactPoint2->m_globalPos, vCollisionToProxy);
// Move the proxy to the collision point, minus the distance along the
// movement vector that we computed above.
//
// Note that we're adjusting the 'proxy' variable, which is just a local
// copy of the proxy position. We still might decide not to move the
// 'real' proxy due to friction.
cVector3d vColNextGoal;
vProxyToGoalNormalized.mulr(-distanceTriangleProxy, vColNextGoal);
cVector3d nextProxyPos;
m_contactPoint2->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
m_algoCounter = 0;
m_numContacts = 3;
// TODO: There actually should be a third friction test to see if we
// can make it to our new goal position, but this is generally such a
// small movement in one iteration that it's irrelevant...
return (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);
}
//-----------------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;
}
//===========================================================================
bool cWorld::computeCollisionDetection(
cVector3d& a_segmentPointA, const cVector3d& a_segmentPointB,
cGenericObject*& a_colObject, cTriangle*& a_colTriangle, cVector3d& a_colPoint,
double& a_colDistance, const bool a_visibleObjectsOnly, int a_proxyCall)
{
// initialize objects for collision detection calls
cGenericObject* t_colObject;
cTriangle *t_colTriangle;
cVector3d t_colPoint;
bool hit = false;
double colSquareDistance = CHAI_LARGE;
double t_colSquareDistance = colSquareDistance;
// get the transpose of the local rotation matrix
cMatrix3d transLocalRot;
m_localRot.transr(transLocalRot);
// convert second endpoint of the segment into local coordinate frame
cVector3d localSegmentPointB = a_segmentPointB;
localSegmentPointB.sub(m_localPos);
transLocalRot.mul(localSegmentPointB);
// r_segmentPointA is the value that we will return in a_segmentPointA
// at the end; it should be unchanged from the received value of
// a_segmentPointA, unless the collision that will be returned is with
// a moving object, in which case it will be adjusted so that it is in the
// same location relative to the moving object as it was at the previous
// haptic iteration; this is necessary so that the proxy algorithm gets the
// correct new proxy position
cVector3d r_segmentPointA = a_segmentPointA;
// check for collisions with all children of this world
unsigned int nChildren = m_children.size();
for (unsigned int i=0; i<nChildren; i++)
{
// start with the first segment point as it was received
cVector3d l_segmentPointA = a_segmentPointA;
// convert first endpoint of the segment into local coordinate frame
cVector3d localSegmentPointA = l_segmentPointA;
localSegmentPointA.sub(m_localPos);
transLocalRot.mul(localSegmentPointA);
// if this is a first call from the proxy algorithm, and the current
// child is a dynamic object, adjust the first segment endpoint so that
// it is in the same position relative to the moving object as it was
// at the previous haptic iteration
if ((a_proxyCall == 1) && (m_children[i]->m_historyValid))
AdjustCollisionSegment(l_segmentPointA,localSegmentPointA,m_children[i]);
// call this child's collision detection function to see if it (or any
// of its descendants) are intersected by the segment
int coll = m_children[i]->computeCollisionDetection(localSegmentPointA, localSegmentPointB,
t_colObject, t_colTriangle, t_colPoint, t_colSquareDistance, a_visibleObjectsOnly, a_proxyCall);
// if a collision was found with this child, and this collision is
// closer than any others found so far...
if ((coll == 1) && (t_colSquareDistance < colSquareDistance))
{
// record that there has been a collision
hit = true;
// set the return parameters with information about this collision
// (they may be overwritten if a closer collision is found later
// on in this loop)
a_colObject = t_colObject;
a_colTriangle = t_colTriangle;
a_colPoint = t_colPoint;
// this is now the shortest distance to a collision found so far
colSquareDistance = t_colSquareDistance;
// convert collision point into parent coordinate frame
m_localRot.mul(a_colPoint);
a_colPoint.add(m_localPos);
// localSegmentPointA's position (as possibly modified in the
// call to the child's collision detector), converted back to
// the global coordinate frame, is currently the proxy position
// we will want to return (unless we find a closer collision later on)
r_segmentPointA = cAdd(cMul(m_localRot,localSegmentPointA), m_localPos);
}
}
// for optimization reasons, the collision detectors only computes the
// squared distance between a_segmentA and collision point; this
// computes a square root to obtain the actual distance.
a_colDistance = sqrt(colSquareDistance);
// set the value of the actual parameter for the first segment point; this
// is the proxy position when called by the proxy algorithm, and may be
// different from the value passed in this parameter if the closest collision
// was with a moving object
a_segmentPointA = r_segmentPointA;
// return whether there was a collision between the segment and this world
return (hit);
}
//===========================================================================
void cProxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal,
const cVector3d& a_proxy,
cVector3d& a_normal,
cGenericObject* a_parent)
{
// check if friction is enabled
if (m_useFriction == false)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// Compute penetration depth; how far is the device "behind" the
// plane of the obstructing surface
cVector3d projectedGoal = cProjectPointOnPlane(m_deviceGlobalPos, a_proxy, a_normal);
double penetrationDepth = cSub(m_deviceGlobalPos,projectedGoal).length();
// Find the appropriate friction coefficient
// Our dynamic and static coefficients...
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;
}
double mud = parent_mesh->m_material.getDynamicFriction();
double mus = parent_mesh->m_material.getStaticFriction();
// No friction; don't try to compute friction cones
if ((mud == 0) && (mus == 0))
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// The corresponding friction cone radii
double atmd = atan(mud);
double atms = atan(mus);
// Compute a vector from the device to the proxy, for computing
// the angle of the friction cone
cVector3d vDeviceProxy = cSub(a_proxy, m_deviceGlobalPos);
vDeviceProxy.normalize();
// Now compute the angle of the friction cone...
double theta = acos(vDeviceProxy.dot(a_normal));
// Manage the "slip-friction" state machine
// If the dynamic friction radius is for some reason larger than the
// static friction radius, always slip
if (mud > mus)
{
m_slipping = true;
}
// If we're slipping...
else if (m_slipping)
{
if (theta < (atmd * m_frictionDynHysteresisMultiplier))
{
m_slipping = false;
}
else
{
m_slipping = true;
}
}
// If we're not slipping...
else
{
if (theta > atms)
{
m_slipping = true;
}
else
{
m_slipping = false;
}
}
// The friction coefficient we're going to use...
double mu;
if (m_slipping) mu = mud;
else mu = mus;
// Calculate the friction radius as the absolute value of the penetration
// depth times the coefficient of friction
double frictionRadius = fabs(penetrationDepth * mu);
// Calculate the distance between the proxy position and the current
// goal position.
double r = a_proxy.distance(a_goal);
// If this distance is smaller than CHAI_SMALL, we consider the proxy
// to be at the same position as the goal, and we're done...
if (r < CHAI_SMALL)
{
m_nextBestProxyGlobalPos = a_proxy;
}
// If the proxy is outside the friction cone, update its position to
// be on the perimeter of the friction cone...
else if (r > frictionRadius)
{
m_nextBestProxyGlobalPos = cAdd(a_goal, cMul(frictionRadius/r, cSub(a_proxy, a_goal)));
}
// Otherwise, if the proxy is inside the friction cone, the proxy
// should not be moved (set next best position to current position)
else
{
m_nextBestProxyGlobalPos = a_proxy;
}
// We're done; record the fact that we're still touching an object...
return;
}
//===========================================================================
void cProxyPointForceAlgo::updateForce()
{
// initialize variables
double stiffness = 0.0;
cVector3d normal;
normal.zero();
// if there are no contacts between proxy and environment, no force is applied
if (m_numContacts == 0)
{
m_lastGlobalForce.zero();
return;
}
//---------------------------------------------------------------------
// stiffness and surface normal estimation
//---------------------------------------------------------------------
else if (m_numContacts == 1)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() );
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
}
// if there are two contact points, the stiffness is the average of the
// stiffnesses of the two intersected triangles' meshes
else if (m_numContacts == 2)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() ) / 2.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.mul(1.0/2.0);
}
// if there are three contact points, the stiffness is the average of the
// stiffnesses of the three intersected triangles' meshes
else if (m_numContacts == 3)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint2->m_triangle->getParent()->m_material.getStiffness() ) / 3.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.add(m_contactPoint2->m_globalNormal);
normal.mul(1.0/3.0);
}
//---------------------------------------------------------------------
// computing a force (Hooke's law)
//---------------------------------------------------------------------
// compute the force by modeling a spring between the proxy and the device
cVector3d force;
m_proxyGlobalPos.subr(m_deviceGlobalPos, force);
force.mul(stiffness);
m_lastGlobalForce = force;
// compute tangential and normal forces
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
//---------------------------------------------------------------------
// force shading (optional)
//---------------------------------------------------------------------
if ((m_useForceShading) && (m_numContacts == 1))
{
// get vertices and normals related to contact triangle
cVector3d vertex0 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getPos()));
cVector3d vertex1 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getPos()));
cVector3d vertex2 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getPos()));
cVector3d normal0 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getNormal());
cVector3d normal1 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getNormal());
cVector3d normal2 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getNormal());
// compute angles between normals. If the angles are very different, then do not apply shading.
double angle01 = cAngle(normal0, normal1);
double angle02 = cAngle(normal0, normal2);
double angle12 = cAngle(normal1, normal2);
if ((angle01 < m_forceShadingAngleThreshold) || (angle02 < m_forceShadingAngleThreshold) || (angle12 < m_forceShadingAngleThreshold))
{
double a0 = 0;
double a1 = 0;
cProjectPointOnPlane(m_contactPoint0->m_globalPos, vertex0, vertex1, vertex2, a0, a1);
cVector3d normalShaded = cAdd(
cMul(0.5, cAdd(cMul(a0, normal1), cMul((1-a0), normal0))),
cMul(0.5, cAdd(cMul(a1, normal2), cMul((1-a1), normal0)))
);
normalShaded.normalize();
if (cAngle(normalShaded, normal) > 1.0)
{
normalShaded.negate();
}
if (cAngle(normal, normalShaded) < m_forceShadingAngleThreshold)
{
double forceMagnitude = m_normalForce.length();
force = cAdd( cMul(forceMagnitude, normalShaded), m_tangentialForce);
m_lastGlobalForce = force;
normal = normalShaded;
// update tangential and normal forces again
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
}
}
}
}
Carro::Carro(dynamicWorld* world):dynamicObject(world,0.5*LARGURACARRO*COMPRIMENTOCARRO,true) {
steeringAngle = 0;
//Instancia o chassi e o meio
meio = new cGenericObject();
chassi = new cGenericObject();
meio->addChild(chassi);
//Adiciona o meio ao carro propriamente dito
this->addChild(meio);
//Inicializa a roda1
roda1 = new cMesh(world);
roda1->loadFromFile("roda_simples.obj");
roda1->computeBoundaryBox(true);
cVector3d min = roda1->getBoundaryMin();
cVector3d max = roda1->getBoundaryMax();
cVector3d meio = cMul(-0.5,cAdd(min,max));
cVector3d span = cSub(max, min);
for(int i=0;i<roda1->getNumVertices(true);i++)
roda1->getVertex(i,true)->translate(meio);
double size = cMax(span.x, cMax(span.y, span.z));
double scaleFactor = 2*RAIORODA / size;
roda1->scale(scaleFactor);
chassi->addChild(roda1);
roda1->translate(0,RAIORODA,LARGURACARRO/2.0);
roda1->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda2
roda2 = new cMesh(world);
roda2->loadFromFile("roda_simples.obj");
for(int i=0;i<roda2->getNumVertices(true);i++)
roda2->getVertex(i,true)->translate(meio);
roda2->scale(scaleFactor);
chassi->addChild(roda2);
roda2->translate(0,RAIORODA,-LARGURACARRO/2.0);
roda2->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instanci os eixos do carro
eixo1 = new cGenericObject();
chassi->addChild(eixo1);
eixo2 = new cGenericObject();
chassi->addChild(eixo2);
//Instancia e inicializa a roda3
roda3 = new cMesh(world);
roda3->loadFromFile("roda_simples.obj");
for(int i=0;i<roda3->getNumVertices(true);i++)
roda3->getVertex(i,true)->translate(meio);
roda3->scale(scaleFactor);
eixo1->addChild(roda3);
eixo1->translate(DISTANCIAEIXOS,RAIORODA,LARGURACARRO/2.0);
roda3->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda4
roda4 = new cMesh(world);
roda4->loadFromFile("roda_simples.obj");
for(int i=0;i<roda4->getNumVertices(true);i++)
roda4->getVertex(i,true)->translate(meio);
roda4->scale(scaleFactor);
eixo2->addChild(roda4);
eixo2->translate(DISTANCIAEIXOS,RAIORODA,-LARGURACARRO/2.0);
roda4->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instancia e inicializa a carroceria
carroceria = new cMesh(world);
carroceria->loadFromFile("ferrari.3ds");
chassi->addChild(carroceria);
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
meio = cMul(-0.5,cAdd(min,max));
for(int i=0;i<carroceria->getNumVertices(true);i++)
carroceria->getVertex(i,true)->translate(meio);
size = cMax(span.x, cMax(span.y, span.z));
scaleFactor = COMPRIMENTOCARRO / size;
carroceria->scale(scaleFactor);
carroceria->rotate(cVector3d(0,1,0),3.1415/2.0);
carroceria->rotate(cVector3d(0,0,1),3.1415/2.0);
//recalcula dimensões
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
double altura= cMin(span.x, cMin(span.y, span.z));
// posiciona carroceria: assumindo que altura do chão é 0.3* raio da roda
// levanta meia altura para chao ficar no plano x,z
carroceria->translate(DISTANCIAEIXOS/1.85,altura*0.5+RAIORODA*0.3,0);
carroceria->useColors(true, true);
carroceria->useMaterial(false,true);
}
//===========================================================================
bool cCamera::select(const int a_windowPosX,
const int a_windowPosY,
const int a_windowWidth,
const int a_windowHeight,
cCollisionRecorder& a_collisionRecorder,
cCollisionSettings& a_collisionSettings)
{
// sanity check
if ((a_windowWidth <= 0) || (a_windowHeight <= 0)) return (false);
// clear collision recorder
a_collisionRecorder.clear();
// update my m_globalPos and m_globalRot variables
m_parentWorld->computeGlobalPositions(false);
// init variable to store result
bool result = false;
if (m_perspectiveMode)
{
// make sure we have a legitimate field of view
if (fabs(m_fieldViewAngle) < 0.001f) { return (false); }
// compute the ray that leaves the eye point at the appropriate angle
//
// m_fieldViewAngle / 2.0 would correspond to the _top_ of the window
double distCam = (a_windowHeight / 2.0f) / cTanDeg(m_fieldViewAngle / 2.0f);
cVector3d selectRay;
selectRay.set(-distCam,
(a_windowPosX - (a_windowWidth / 2.0f)),
((a_windowHeight / 2.0f) - a_windowPosY));
selectRay.normalize();
selectRay = cMul(m_globalRot, selectRay);
// create a point that's way out along that ray
cVector3d selectPoint = cAdd(m_globalPos, cMul(100000, selectRay));
// search for intersection between the ray and objects in the world
result = m_parentWorld->computeCollisionDetection(
m_globalPos,
selectPoint,
a_collisionRecorder,
a_collisionSettings);
}
else
{
double hw = (double)(a_windowWidth) * 0.5;
double hh = (double)(a_windowHeight)* 0.5;
double aspect = hw / hh;
double offsetX = ((a_windowPosX - hw) / hw) * 0.5 * m_orthographicWidth;
double offsetY =-((a_windowPosY - hh) / hh) * 0.5 * (m_orthographicWidth / aspect);
cVector3d pos = cAdd(m_globalPos,
cMul(offsetX, m_globalRot.getCol1()),
cMul(offsetY, m_globalRot.getCol2()));
// create a point that's way out along that ray
cVector3d selectPoint = cAdd(pos, cMul(100000, cNegate(m_globalRot.getCol0())));
result = m_parentWorld->computeCollisionDetection(pos,
selectPoint,
a_collisionRecorder,
a_collisionSettings);
}
// return result
return (result);
}
bool ch_proxyPointForceAlgo::computeNextProxyPositionWithContraints00(const cVector3d& a_goalGlobalPos, const cVector3d& a_toolVel)
{
// We define the goal position of the proxy.
cVector3d goalGlobalPos = a_goalGlobalPos;
// To address numerical errors of the computer, we make sure to keep the proxy
// slightly above any triangle and not directly on it. If we are using a radius of
// zero, we need to define a default small value for epsilon
m_epsilonInitialValue = cAbs(0.0001 * m_radius);
if (m_epsilonInitialValue < m_epsilonBaseValue)
{
m_epsilonInitialValue = m_epsilonBaseValue;
}
// The epsilon value is dynamic (can be reduced). We set it to its initial
// value if the proxy is not touching any triangle.
if (m_numContacts == 0)
{
m_epsilon = m_epsilonInitialValue;
m_slipping = true;
}
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (!m_useDynamicProxy)
{
if (goalAchieved(m_proxyGlobalPos, goalGlobalPos))
{
m_nextBestProxyGlobalPos = m_proxyGlobalPos;
m_algoCounter = 0;
return (false);
}
}
// compute the normalized form of the vector going from the
// current proxy position to the desired goal position
// compute the distance between the proxy and the goal positions
double distanceProxyGoal = cDistance(m_proxyGlobalPos, goalGlobalPos);
// A vector from the proxy to the goal
cVector3d vProxyToGoal;
cVector3d vProxyToGoalNormalized;
bool proxyAndDeviceEqual;
if (distanceProxyGoal > m_epsilon)
{
// proxy and goal are sufficiently distant from each other
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
vProxyToGoal.normalizer(vProxyToGoalNormalized);
proxyAndDeviceEqual = false;
}
else
{
// proxy and goal are very close to each other
vProxyToGoal.zero();
vProxyToGoalNormalized.zero();
proxyAndDeviceEqual = true;
}
// Test whether the path from the proxy to the goal is obstructed.
// For this we create a segment that goes from the proxy position to
// the goal position plus a little extra to take into account the
// physical radius of the proxy.
cVector3d targetPos;
if (m_useDynamicProxy)
{
targetPos = goalGlobalPos;
}
else
{
targetPos = goalGlobalPos +
cMul(m_epsilonCollisionDetection, vProxyToGoalNormalized);
}
// setup collision detector
// m_radius is the radius of the proxy
m_collisionSettings.m_collisionRadius = m_radius;
// Search for a collision between the first segment (proxy-device)
// and the environment.
m_collisionSettings.m_adjustObjectMotion = m_useDynamicProxy;
m_collisionRecorderConstraint0.clear();
bool hit = m_world->computeCollisionDetection(m_proxyGlobalPos,
targetPos,
m_collisionRecorderConstraint0,
m_collisionSettings);
// check if collision occurred between proxy and goal positions.
double collisionDistance;
if (hit)
{
collisionDistance = sqrt(m_collisionRecorderConstraint0.m_nearestCollision.m_squareDistance);
if (m_useDynamicProxy)
{
// retrieve new position of proxy
cVector3d posLocal = m_collisionRecorderConstraint0.m_nearestCollision.m_adjustedSegmentAPoint;
cGenericObject* obj = m_collisionRecorderConstraint0.m_nearestCollision.m_object;
cVector3d posGlobal = cAdd(obj->getGlobalPos(), cMul( obj->getGlobalRot(), posLocal ));
m_proxyGlobalPos = posGlobal;
distanceProxyGoal = cDistance(m_proxyGlobalPos, goalGlobalPos);
goalGlobalPos.subr(m_proxyGlobalPos, vProxyToGoal);
vProxyToGoal.normalizer(vProxyToGoalNormalized);
}
if (collisionDistance > (distanceProxyGoal + CHAI_SMALL))
{
// just to make sure that the collision point lies on the proxy-goal segment and not outside of it
hit = false;
}
if (hit)
{
// a collision has occurred and we check if the distance from the
// proxy to the collision is smaller than epsilon. If yes, then
// we reduce the epsilon term in order to avoid possible "pop through"
// effect if we suddenly push the proxy "up" again.
if (collisionDistance < m_epsilon)
{
m_epsilon = collisionDistance;
if (m_epsilon < m_epsilonMinimalValue)
{
m_epsilon = m_epsilonMinimalValue;
}
}
}
}
// If no collision occurs, then we move the proxy to its goal, and we're done
if (!hit)
{
m_numContacts = 0;
m_algoCounter = 0;
m_slipping = true;
m_nextBestProxyGlobalPos = goalGlobalPos;
return (false);
}
// a first collision has occurred
m_algoCounter = 1;
//-----------------------------------------------------------------------
// FIRST COLLISION OCCURES:
//-----------------------------------------------------------------------
// We want the center of the proxy to move as far toward the triangle as it can,
// but we want it to stop when the _sphere_ representing the proxy hits the
// triangle. We want to compute how far the proxy center will have to
// be pushed _away_ from the collision point - along the vector from the proxy
// to the goal - to keep a distance m_radius between the proxy center and the
// triangle.
//
// So we compute the cosine of the angle between the normal and proxy-goal vector...
double cosAngle = vProxyToGoalNormalized.dot(m_collisionRecorderConstraint0.m_nearestCollision.m_globalNormal);
// Now we compute how far away from the collision point - _backwards_
// along vProxyGoal - we have to put the proxy to keep it from penetrating
// the triangle.
//
// If only ASCII art were a little more expressive...
double distanceTriangleProxy = m_epsilon / cAbs(cosAngle);
if (distanceTriangleProxy > collisionDistance) {
distanceTriangleProxy = cMax(collisionDistance, m_epsilon);
}
// We compute the projection of the vector between the proxy and the collision
// point onto the normal of the triangle. This is the direction in which
// we'll move the _goal_ to "push it away" from the triangle (to account for
// the radius of the proxy).
// A vector from the most recent collision point to the proxy
cVector3d vCollisionToProxy;
m_proxyGlobalPos.subr(m_contactPoint0->m_globalPos, vCollisionToProxy);
// Move the proxy to the collision point, minus the distance along the
// movement vector that we computed above.
//
// Note that we're adjusting the 'proxy' variable, which is just a local
// copy of the proxy position. We still might decide not to move the
// 'real' proxy due to friction.
cVector3d vColNextGoal;
vProxyToGoalNormalized.mulr(-distanceTriangleProxy, vColNextGoal);
cVector3d nextProxyPos;
m_contactPoint0->m_globalPos.addr(vColNextGoal, nextProxyPos);
// we can now set the next position of the proxy
m_nextBestProxyGlobalPos = nextProxyPos;
// If the distance between the proxy and the goal position (device) is
// very small then we can be considered done.
if (goalAchieved(goalGlobalPos, nextProxyPos))
{
m_numContacts = 1;
m_algoCounter = 0;
return (true);
}
return (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)
{
// initialize frequency counter
frequencyCounter.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// READ HAPTIC DEVICE
/////////////////////////////////////////////////////////////////////
// read position
cVector3d position;
hapticDevice->getPosition(position);
// read orientation
cMatrix3d rotation;
hapticDevice->getRotation(rotation);
// read gripper position
double gripperAngle;
hapticDevice->getGripperAngleRad(gripperAngle);
// read linear velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// read angular velocity
cVector3d angularVelocity;
hapticDevice->getAngularVelocity(angularVelocity);
// read gripper angular velocity
double gripperAngularVelocity;
hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
// read userswitch status (button 0)
bool button0, button1, button2, button3;
button0 = false;
button1 = false;
button2 = false;
button3 = false;
hapticDevice->getUserSwitch(0, button0);
hapticDevice->getUserSwitch(1, button1);
hapticDevice->getUserSwitch(2, button2);
hapticDevice->getUserSwitch(3, button3);
/////////////////////////////////////////////////////////////////////
// UPDATE 3D MODELS
/////////////////////////////////////////////////////////////////////
// update arrow
velocity->m_pointA = position;
velocity->m_pointB = cAdd(position, linearVelocity);
// update position and orientation of cursor
cursor->setLocalPos(position);
cursor->setLocalRot(rotation);
// adjust the color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (button0)
{
cursor->m_material->setGreenMediumAquamarine();
}
else if (button1)
{
cursor->m_material->setYellowGold();
}
else if (button2)
{
cursor->m_material->setOrangeCoral();
}
else if (button3)
{
cursor->m_material->setPurpleLavender();
}
else
{
cursor->m_material->setBlueRoyal();
}
// update global variable for graphic display update
hapticDevicePosition = position;
/////////////////////////////////////////////////////////////////////
// COMPUTE AND SEND FORCE AND TORQUE
/////////////////////////////////////////////////////////////////////
cVector3d force (0,0,0);
cVector3d torque (0,0,0);
double gripperForce = 0.0;
// apply force field
if (useForceField)
{
// compute linear force
double Kp = 20; // [N/m]
cVector3d forceField = -Kp * position;
force.add(forceField);
// compute angular torque
double Kr = 0.05; // [N/m.rad]
cVector3d axis;
double angle;
rotation.toAngleAxis(angle, axis);
torque = (-Kr * angle) * axis;
}
// apply damping term
if (useDamping)
{
cHapticDeviceInfo info = hapticDevice->getSpecifications();
// compute linear damping force
double Kv = 1.0 * info.m_maxLinearDamping;
cVector3d forceDamping = -Kv * linearVelocity;
force.add(forceDamping);
// compute angluar damping force
double Kvr = 1.0 * info.m_maxAngularDamping;
cVector3d torqueDamping = -Kvr * angularVelocity;
torque.add(torqueDamping);
// compute gripper angular damping force
double Kvg = 1.0 * info.m_maxGripperAngularDamping;
gripperForce = gripperForce - Kvg * gripperAngularVelocity;
}
// send computed force, torque and gripper force to haptic device
hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);
// update frequency counter
frequencyCounter.signal(1);
}
// 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 updateHaptics(void)
{
// main haptic simulation loop
while(simulationRunning)
{
// for each device
int i=0;
while (i < numHapticDevices)
{
// read position of haptic device
cVector3d newPosition;
hapticDevices[i]->getPosition(newPosition);
// read orientation of haptic device
cMatrix3d newRotation;
hapticDevices[i]->getRotation(newRotation);
// update position and orientation of cursor
cursors[i]->setPos(newPosition);
cursors[i]->setRot(newRotation);
// read linear velocity from device
cVector3d linearVelocity;
hapticDevices[i]->getLinearVelocity(linearVelocity);
// update arrow
velocityVectors[i]->m_pointA = newPosition;
velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity);
// read user button status
bool buttonStatus;
hapticDevices[i]->getUserSwitch(0, buttonStatus);
// adjustthe color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (buttonStatus)
{
cursors[i]->m_material = matCursorButtonON;
}
else
{
cursors[i]->m_material = matCursorButtonOFF;
}
// compute a reaction force
cVector3d newForce (0,0,0);
// apply force field
if (useForceField)
{
double Kp = 20.0; // [N/m]
cVector3d force = cMul(-Kp, newPosition);
newForce.add(force);
}
// apply viscosity
if (useDamping)
{
cHapticDeviceInfo info = hapticDevices[i]->getSpecifications();
double Kv = info.m_maxLinearDamping;
cVector3d force = cMul(-Kv, linearVelocity);
newForce.add(force);
}
// send computed force to haptic device
hapticDevices[i]->setForce(newForce);
// increment counter
i++;
}
}
// exit haptics thread
simulationFinished = true;
}
//==============================================================================
bool cTriangleArray::computeCollision(const unsigned int a_triangleIndex,
cGenericObject* a_object,
cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB,
cCollisionRecorder& a_recorder,
cCollisionSettings& a_settings) const
{
// verify that triangle is active
if (!m_allocated[a_triangleIndex]) { return (false); }
// temp variables
bool hit = false;
cVector3d collisionPoint;
cVector3d collisionNormal;
double collisionDistanceSq = C_LARGE;
double collisionPointV01 = 0.0;
double collisionPointV02 = 0.0;
// retrieve information about which side of the triangles need to be checked
cMaterialPtr material = a_object->m_material;
bool checkFrontSide = material->getHapticTriangleFrontSide();
bool checkBackSide = material->getHapticTriangleBackSide();
// retrieve vertex positions
cVector3d vertex0 = m_vertices->getLocalPos(getVertexIndex0(a_triangleIndex));
cVector3d vertex1 = m_vertices->getLocalPos(getVertexIndex1(a_triangleIndex));
cVector3d vertex2 = m_vertices->getLocalPos(getVertexIndex2(a_triangleIndex));
// If m_collisionRadius == 0, we search for a possible intersection between
// the segment AB and the triangle defined by its three vertices V0, V1, V2.
if (a_settings.m_collisionRadius == 0.0)
{
// check for collision between segment and triangle only
if (cIntersectionSegmentTriangle(a_segmentPointA,
a_segmentPointB,
vertex0,
vertex1,
vertex2,
checkFrontSide,
checkBackSide,
collisionPoint,
collisionNormal,
collisionPointV01,
collisionPointV02))
{
hit = true;
collisionDistanceSq = cDistanceSq(a_segmentPointA, collisionPoint);
}
}
// If m_collisionRadius > 0, we search for a possible intersection between
// the segment AB and the shell of the selected triangle which is described
// by its three vertices and m_collisionRadius.
else
{
cVector3d t_collisionPoint, t_collisionNormal;
double t_collisionDistanceSq;
cVector3d normal = cComputeSurfaceNormal(vertex0, vertex1, vertex2);
cVector3d offset; normal.mulr(a_settings.m_collisionRadius, offset);
cVector3d t_vertex0, t_vertex1, t_vertex2;
double t_collisionPointV01, t_collisionPointV02, t_collisionPointV12;
// check for collision between segment and triangle upper shell
vertex0.addr(offset, t_vertex0);
vertex1.addr(offset, t_vertex1);
vertex2.addr(offset, t_vertex2);
if (cIntersectionSegmentTriangle(a_segmentPointA,
a_segmentPointB,
t_vertex0,
t_vertex1,
t_vertex2,
checkFrontSide,
false,
collisionPoint,
collisionNormal,
collisionPointV01,
collisionPointV02))
{
hit = true;
collisionDistanceSq = cDistanceSq(a_segmentPointA, collisionPoint);
}
// check for collision between segment and triangle lower shell
vertex0.subr(offset, t_vertex0);
vertex1.subr(offset, t_vertex1);
vertex2.subr(offset, t_vertex2);
if (cIntersectionSegmentTriangle(a_segmentPointA,
a_segmentPointB,
t_vertex0,
t_vertex1,
t_vertex2,
false,
checkBackSide,
t_collisionPoint,
t_collisionNormal,
t_collisionPointV01,
t_collisionPointV02))
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = t_collisionPointV01;
collisionPointV02 = t_collisionPointV02;
}
}
// check for collision between sphere located at vertex 0.
// if the starting point (a_segmentPointA) is located inside
// the sphere, we ignore the collision to avoid remaining
// stuck inside the triangle.
cVector3d t_p, t_n;
double t_c;
if (cIntersectionSegmentSphere(a_segmentPointA,
a_segmentPointB,
vertex0,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_p,
t_n) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = 0.0;
collisionPointV02 = 0.0;
}
}
// check for collision between sphere located at vertex 1.
// if the starting point (a_segmentPointA) is located inside
// the sphere, we ignore the collision to avoid remaining
// stuck inside the triangle.
if (cIntersectionSegmentSphere(a_segmentPointA,
a_segmentPointB,
vertex1,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_p,
t_n) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = 1.0;
collisionPointV02 = 0.0;
}
}
// check for collision between sphere located at vertex 2.
// if the starting point (a_segmentPointA) is located inside
// the sphere, we ignore the collision to avoid remaining
// stuck inside the triangle.
if (cIntersectionSegmentSphere(a_segmentPointA,
a_segmentPointB,
vertex2,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_p,
t_n) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = 0.0;
collisionPointV02 = 1.0;
}
}
// check for collision between segment and triangle edge01 shell.
// if the starting point (a_segmentPointA) is located inside
// the cylinder, we ignore the collision to avoid remaining
// stuck inside the triangle.
if (cIntersectionSegmentToplessCylinder(a_segmentPointA,
a_segmentPointB,
vertex0,
vertex1,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_collisionPointV01,
t_p,
t_n,
t_c) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = t_collisionPointV01;
collisionPointV02 = 0.0;
}
}
// check for collision between segment and triangle edge02 shell.
// if the starting point (a_segmentPointA) is located inside
// the cylinder, we ignore the collision to avoid remaining
// stuck inside the triangle.
if (cIntersectionSegmentToplessCylinder(a_segmentPointA,
a_segmentPointB,
vertex0,
vertex2,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_collisionPointV02,
t_p,
t_n,
t_c) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = 0.0;
collisionPointV02 = t_collisionPointV02;
}
}
// check for collision between segment and triangle edge12 shell.
// if the starting point (a_segmentPointA) is located inside
// the cylinder, we ignore the collision to avoid remaining
// stuck inside the triangle.
if (cIntersectionSegmentToplessCylinder(a_segmentPointA,
a_segmentPointB,
vertex1,
vertex2,
a_settings.m_collisionRadius,
t_collisionPoint,
t_collisionNormal,
t_collisionPointV12,
t_p,
t_n,
t_c) > 0)
{
hit = true;
t_collisionDistanceSq = cDistanceSq(a_segmentPointA, t_collisionPoint);
if (t_collisionDistanceSq <= collisionDistanceSq)
{
collisionPoint = t_collisionPoint;
collisionNormal = t_collisionNormal;
collisionDistanceSq = t_collisionDistanceSq;
collisionPointV01 = 1.0 - t_collisionPointV12;
collisionPointV02 = t_collisionPointV12;
}
}
}
// report collision
if (hit)
{
// before reporting the new collision, we need to check if
// the collision settings require us to verify the side of the
// triangle which has been hit.
bool hit_confirmed = false;
if (checkFrontSide && checkBackSide)
{
// settings specify that a collision can occur on both sides
// of the triangle, so the new collision is reported.
hit_confirmed = true;
}
else
{
// we need check on which side of the triangle the collision occurred
// and see if it needs to be reported.
cVector3d segmentAB;
a_segmentPointB.subr(a_segmentPointA, segmentAB);
cVector3d v01, v02, triangleNormal;
vertex2.subr(vertex0, v02);
vertex1.subr(vertex0, v01);
v01.crossr(v02, triangleNormal);
double value = cCosAngle(segmentAB, triangleNormal);
if (value <= 0.0)
{
if (checkFrontSide)
hit_confirmed = true;
}
else
{
if (checkBackSide)
hit_confirmed = true;
}
}
// here we finally report the new collision to the collision event handler.
if (hit_confirmed)
{
// we verify if anew collision needs to be created or if we simply
// need to update the nearest collision.
if (a_settings.m_checkForNearestCollisionOnly)
{
// no new collision event is create. We just check if we need
// to update the nearest collision
if(collisionDistanceSq <= a_recorder.m_nearestCollision.m_squareDistance)
{
// report basic collision data
a_recorder.m_nearestCollision.m_object = a_object;
a_recorder.m_nearestCollision.m_triangles = ((cMesh*)(a_object))->m_triangles;
a_recorder.m_nearestCollision.m_triangleIndex = a_triangleIndex;
a_recorder.m_nearestCollision.m_localPos = collisionPoint;
a_recorder.m_nearestCollision.m_localNormal = collisionNormal;
a_recorder.m_nearestCollision.m_squareDistance = collisionDistanceSq;
a_recorder.m_nearestCollision.m_adjustedSegmentAPoint = a_segmentPointA;
a_recorder.m_nearestCollision.m_trianglePosV01 = collisionPointV01;
a_recorder.m_nearestCollision.m_trianglePosV02 = collisionPointV02;
// report advanced collision data
if (!a_settings.m_returnMinimalCollisionData)
{
a_recorder.m_nearestCollision.m_globalPos = cAdd(a_object->getGlobalPos(),
cMul(a_object->getGlobalRot(),
a_recorder.m_nearestCollision.m_localPos));
a_recorder.m_nearestCollision.m_globalNormal = cMul(a_object->getGlobalRot(),
a_recorder.m_nearestCollision.m_localNormal);
}
}
}
else
{
cCollisionEvent newCollisionEvent;
// report basic collision data
newCollisionEvent.m_object = a_object;
newCollisionEvent.m_triangles = ((cMesh*)(a_object))->m_triangles;
newCollisionEvent.m_triangleIndex = a_triangleIndex;
newCollisionEvent.m_localPos = collisionPoint;
newCollisionEvent.m_localNormal = collisionNormal;
newCollisionEvent.m_squareDistance = collisionDistanceSq;
newCollisionEvent.m_adjustedSegmentAPoint = a_segmentPointA;
newCollisionEvent.m_trianglePosV01 = collisionPointV01;
newCollisionEvent.m_trianglePosV02 = collisionPointV02;
// report advanced collision data
if (!a_settings.m_returnMinimalCollisionData)
{
newCollisionEvent.m_globalPos = cAdd(a_object->getGlobalPos(),
cMul(a_object->getGlobalRot(),
newCollisionEvent.m_localPos));
newCollisionEvent.m_globalNormal = cMul(a_object->getGlobalRot(),
newCollisionEvent.m_localNormal);
}
// add new collision even to collision list
a_recorder.m_collisions.push_back(newCollisionEvent);
// check if this new collision is a candidate for "nearest one"
if(collisionDistanceSq <= a_recorder.m_nearestCollision.m_squareDistance)
{
a_recorder.m_nearestCollision = newCollisionEvent;
}
}
}
// return result
return (hit_confirmed);
}
else
{
return (false);
}
}