void updateHaptics(void)
{
// start haptic device
hapticDevice->open();
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
double kp, kr;
kp = 0.0;
kr = 0.0;
cMatrix3d prevRotTool;
prevRotTool.identity();
// main haptic simulation loop
while(simulationRunning)
{
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = simClock.stop();//0.0005;//cClamp(time, 0.00001, 0.0002);
// reset clock
simClock.reset();
simClock.start();
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
cVector3d posDevice;
cMatrix3d rotDevice;
hapticDevice->getPosition(posDevice);
hapticDevice->getRotation(rotDevice);
// scale position of device
posDevice.mul(workspaceScaleFactor);
// read position of tool
cVector3d posTool = ODETool->getLocalPos();
cMatrix3d rotTool = ODETool->getLocalRot();
// compute position and angular error between tool and haptic device
cVector3d deltaPos = (posDevice - posTool);
cMatrix3d deltaRot = cMul(cTranspose(rotTool), rotDevice);
double angle;
cVector3d axis;
deltaRot.toAngleAxis(angle, axis);
// compute force and torque to apply to tool
cVector3d force, torque;
force = linStiffness * deltaPos;
ODETool->addExternalForce(force);
torque = cMul((angStiffness * angle), axis);
rotTool.mul(torque);
ODETool->addExternalTorque(torque);
// update data for display
frame->setLocalRot(deltaRot);
// compute force and torque to apply to haptic device
force = -linG * force;
torque = -angG * torque;
line->m_pointB = torque;
// send forces to device
hapticDevice->setForceAndTorqueAndGripperForce(force, torque, 0.0);
if (linG < linGain)
{
linG = linG + 0.1 * time * linGain;
}
else
{
linG = linGain;
}
if (angG < angGain)
{
angG = angG + 0.1 * time * angGain;
}
else
{
angG = angGain;
}
// update simulation
ODEWorld->updateDynamics(nextSimInterval);
}
// 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 updateHaptics(void)
{
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
// reset haptics activation clock
startHapticsClock.reset();
startHapticsClock.start();
bool hapticsReady = false;
// main haptic simulation loop
while(simulationRunning)
{
// wait for some time before enabling haptics
if (!hapticsReady)
{
if (startHapticsClock.getCurrentTimeSeconds() > 3.0)
{
hapticsReady = true;
}
}
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// check if the tool is touching an object
cGenericObject* object = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
// read user switch status
bool userSwitch = tool->getUserSwitch(0);
// if the tool is currently grasping an object we simply update the interaction grasp force
// between the tool and the object (modeled by a virtual spring)
if (graspActive && userSwitch)
{
// retrieve latest position and orientation of grasped ODE object in world coordinates
cMatrix3d globalGraspObjectRot = graspObject->getGlobalRot();
cVector3d globalGraspObjectPos = graspObject->getGlobalPos();
// compute the position of the grasp point on object in global coordinates
cVector3d globalGraspPos = globalGraspObjectPos + cMul(globalGraspObjectRot, graspPos);
// retrieve the position of the tool in global coordinates
cVector3d globalToolPos = tool->getProxyGlobalPos();
// compute the offset between the tool and grasp point on the object
cVector3d offset = globalToolPos - globalGraspPos;
// model a spring between both points
double STIFFNESS = 4;
cVector3d force = STIFFNESS * offset;
// apply attraction force (grasp) onto object
graspObject->addGlobalForceAtGlobalPos(force, globalGraspPos);
// scale magnitude and apply opposite force to haptic device
tool->m_lastComputedGlobalForce.add(cMul(-1.0, force));
// update both end points of the line which is used for display purposes only
graspLine->m_pointA = globalGraspPos;
graspLine->m_pointB = globalToolPos;
}
// the user is not or no longer currently grasping the object
else
{
// was the user grasping the object at the previous simulation loop
if (graspActive)
{
// we disable grasping
graspActive = false;
// we hide the virtual line between the tool and the grasp point
graspLine->setShowEnabled(false);
// we enable haptics interaction between the tool and the previously grasped object
if (graspObject != NULL)
{
graspObject->m_imageModel->setHapticEnabled(true, true);
}
}
// the user is touching an object
if (object != NULL)
{
// check if object is attached to an external ODE parent
cGenericType* externalParent = object->getExternalParent();
cODEGenericBody* ODEobject = dynamic_cast<cODEGenericBody*>(externalParent);
if (ODEobject != NULL)
{
// get position of tool
cVector3d pos = tool->m_proxyPointForceModel->m_contactPoint0->m_globalPos;
// check if user has enabled the user switch to gras the object
if (userSwitch)
{
// a new object is being grasped
graspObject = ODEobject;
// retrieve the grasp position on the object in local coordinates
graspPos = tool->m_proxyPointForceModel->m_contactPoint0->m_localPos;
// grasp in now active!
graspActive = true;
// enable small line which display the offset between the tool and the grasp point
graspLine->setShowEnabled(true);
// disable haptic interaction between the tool and the grasped device.
// this is performed for stability reasons.
graspObject->m_imageModel->setHapticEnabled(false, true);
}
// retrieve the haptic interaction force being applied to the tool
cVector3d force = tool->m_lastComputedGlobalForce;
// apply haptic force to ODE object
cVector3d tmpfrc = cNegate(force);
if (hapticsReady)
{
ODEobject->addGlobalForceAtGlobalPos(tmpfrc, pos);
}
}
}
}
// send forces to device
tool->applyForces();
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = cClamp(time, 0.001, 0.004);
// reset clock
simClock.reset();
simClock.start();
// update simulation
ODEWorld->updateDynamics(nextSimInterval);
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
bool cEffectMagnet::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
// compute distance from object to tool
double distance = cDistance(a_toolPos, m_parent->m_interactionProjectedPoint);
// get parameters of magnet
double magnetMaxForce = m_parent->m_material.getMagnetMaxForce();
double magnetMaxDistance = m_parent->m_material.getMagnetMaxDistance();
double stiffness = m_parent->m_material.getStiffness();
double forceMagnitude = 0;
if ((distance > 0) && (distance < magnetMaxDistance) && (stiffness > 0))
{
double limitLinearModel = magnetMaxForce / stiffness;
cClamp(limitLinearModel, 0.0, 0.5 * distance);
if (distance < limitLinearModel)
{
// apply local linear model near magnet
forceMagnitude = stiffness * distance;
}
else
{
// compute quadratic model
cMatrix3d sys;
sys.m[0][0] = limitLinearModel * limitLinearModel;
sys.m[0][1] = limitLinearModel;
sys.m[0][2] = 1.0;
sys.m[1][0] = magnetMaxDistance * magnetMaxDistance;
sys.m[1][1] = magnetMaxDistance;
sys.m[1][2] = 1.0;
sys.m[2][0] = 2.0 * limitLinearModel;
sys.m[2][1] = 1.0;
sys.m[2][2] = 0.0;
sys.invert();
cVector3d param;
sys.mulr(cVector3d(magnetMaxForce, 0.0, -1.0), param);
// apply quadratic model
double val = distance - limitLinearModel;
forceMagnitude = param.x * val * val + param.y * val + param.z;
}
// compute magnetic force
a_reactionForce = cMul(forceMagnitude, cNormalize(cSub(m_parent->m_interactionProjectedPoint, a_toolPos)));
// add damping component
double viscosity = m_parent->m_material.getViscosity();
cVector3d viscousForce = cMul(-viscosity, a_toolVel);
a_reactionForce.add(viscousForce);
return (true);
}
else
{
// the tool is located outside the magnet zone
a_reactionForce.zero();
return (false);
}
}
void updateHaptics(void)
{
// 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();
// retrieve and update the force that is applied on each object
// stop the simulation clock
simClock.stop();
// read the time increment in seconds
double timeInterval = simClock.getCurrentTimeSeconds();
// restart the simulation clock
simClock.reset();
simClock.start();
// get position of cursor in global coordinates
cVector3d toolPos = tool->m_deviceGlobalPos;
// 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 vObjectCMToTool = cSub(toolPos, objectPos);
// compute acceleration based on the interaction forces
// between the tool and the object
cVector3d rotAcc(0,0,0);
if (vObjectCMToTool.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute effective force to take into account the fact the object
// can only rotate around a its center mass and not translate
cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool);
// compute the resulting torque
cVector3d torque = cMul(vObjectCMToTool.length(), cCross( cNormalize(vObjectCMToTool), effectiveForce));
// update rotational acceleration
const double OBJECT_INERTIA = 0.4;
rotAcc = (1.0 / OBJECT_INERTIA) * torque;
}
// update rotational velocity
rotVel.add(timeInterval * rotAcc);
// set a threshold on the rotational velocity term
const double ROT_VEL_MAX = 10.0;
double velMag = rotVel.length();
if (velMag > ROT_VEL_MAX)
{
rotVel.mul(ROT_VEL_MAX / velMag);
}
// add some damping too
const double DAMPING_GAIN = 0.1;
rotVel.mul(1.0 - DAMPING_GAIN * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
rotVel.zero();
}
// compute the next rotation configuration of the object
if (rotVel.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVel), timeInterval * rotVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
void hapticsLoop(void* a_pUserData)
{
// read the position of the haptic device
cursor->updatePose();
// compute forces between the cursor and the environment
cursor->computeForces();
// stop the simulation clock
g_clock.stop();
// read the time increment in seconds
double increment = g_clock.getCurrentTime() / 1000000.0;
// restart the simulation clock
g_clock.initialize();
g_clock.start();
// get position of cursor in global coordinates
cVector3d cursorPos = cursor->m_deviceGlobalPos;
// compute velocity of cursor;
timeCounter = timeCounter + increment;
if (timeCounter > 0.01)
{
cursorVel = (cursorPos - lastCursorPos) / timeCounter;
lastCursorPos = cursorPos;
timeCounter = 0;
}
// get position of torus in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute the velocity of the sphere at the contact point
cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
if (rotVelocity.length() > CHAI_SMALL)
{
cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
cVector3d vpc = cursorPos - projection;
if (vpc.length() > CHAI_SMALL)
{
contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
}
}
// get the last force applied to the cursor in global coordinates
cVector3d cursorForce = cursor->m_lastComputedGlobalForce;
// compute friction force
cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));
// add friction force to cursor
cursor->m_lastComputedGlobalForce.add(friction);
// update rotational velocity
if (friction.length() > CHAI_SMALL)
{
rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
}
// add some damping...
//rotVelocity.mul(1.0 - increment);
// compute the next rotation of the torus
if (rotVelocity.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
}
// send forces to haptic device
cursor->applyForces();
}
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);
#ifdef _WIN32
Sleep(1);
#else
usleep(1000);
#endif
}
// exit haptics thread
simulationFinished = true;
}
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);
}
//===========================================================================
void cGELMesh::connectVerticesToSkeleton(bool a_connectToNodesOnly)
{
// get number of vertices
int numVertices = m_gelVertices.size();
// for each deformable vertex we search for the nearest sphere or link
for (int i=0; i<numVertices; i++)
{
// get current deformable vertex
cGELVertex* curVertex = &m_gelVertices[i];
// get current vertex position
cVector3d pos = curVertex->m_vertex->getPos();
// initialize constant
double min_distance = 99999999999999999.0;
cGELSkeletonNode* nearest_node = NULL;
cGELSkeletonLink* nearest_link = NULL;
// search for the nearest node
list<cGELSkeletonNode*>::iterator itr;
for(itr = m_nodes.begin(); itr != m_nodes.end(); ++itr)
{
cGELSkeletonNode* nextNode = *itr;
double distance = cDistance(pos, nextNode->m_pos);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = nextNode;
nearest_link = NULL;
}
}
// search for the nearest link if any
if (!a_connectToNodesOnly)
{
list<cGELSkeletonLink*>::iterator j;
for(j = m_links.begin(); j != m_links.end(); ++j)
{
cGELSkeletonLink* nextLink = *j;
double angle0 = cAngle(nextLink->m_wLink01, cSub(pos, nextLink->m_node0->m_pos));
double angle1 = cAngle(nextLink->m_wLink10, cSub(pos, nextLink->m_node1->m_pos));
if ((angle0 < (CHAI_PI / 2.0)) && (angle1 < (CHAI_PI / 2.0)))
{
cVector3d p = cProjectPointOnLine(pos,
nextLink->m_node0->m_pos,
nextLink->m_wLink01);
double distance = cDistance(pos, p);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = NULL;
nearest_link = nextLink;
}
}
}
}
// attach vertex to nearest node if it exists
if (nearest_node != NULL)
{
curVertex->m_node = nearest_node;
curVertex->m_link = NULL;
cVector3d posRel = cSub(pos, nearest_node->m_pos);
curVertex->m_massParticle->m_pos = cMul(cTrans(nearest_node->m_rot), posRel);
}
// attach vertex to nearest link if it exists
else if (nearest_link != NULL)
{
curVertex->m_node = NULL;
curVertex->m_link = nearest_link;
cMatrix3d rot;
rot.setCol( nearest_link->m_A0,
nearest_link->m_B0,
nearest_link->m_wLink01);
cVector3d posRel = cSub(pos, nearest_link->m_node0->m_pos);
curVertex->m_massParticle->m_pos = cMul(cInv(rot), posRel);
}
}
}
//==============================================================================
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);
}
}
