void updateHaptics(void)
{
// precion clock
cPrecisionClock clock;
int i = 0;
// main haptic simulation loop
while(simulationRunning)
{
bool blIO = false;
blIO = deltaCtrl.SetFGetPosStatues(dF,dPos,statu,dT, blOutRange); // changing point
dPos[2]+=0.1;
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// update my pose************MINE***************** we get the force in
tool->updateMyPose(dPos);
// compute interaction forces
tool->computeInteractionForces();
// we need to get the force out to our device**************** MINE
tool->getLastComputedForce(dF);
// send forces to device
tool->applyForces();
// stop clock
double time = clock.stop();
// check if contact occured with cylinder
if(tool->isInContact(cylinder))
{
// get force applied on the y axis
double force = tool->m_lastComputedGlobalForce.y();
// shift cylinderaccording to force
const double K = 0.5;
double pos = cylinder->getLocalPos().y();
pos = cClamp(pos - K * time * force,-0.5, 0.6);
cylinder->setLocalPos(0.0, pos, 0.0);
}
// restart clock
clock.start(true);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
void cMaterial::setTransparencyLevel(float a_levelTransparency)
{
// make sur value is in range [0.0 - 1.0]
float level = cClamp(a_levelTransparency, 0.0f, 1.0f);
// apply new value
m_ambient.setA(level);
m_diffuse.setA(level);
m_specular.setA(level);
m_emission.setA(level);
}
// --------------------------------------------------------------------------
// Helper function to compute the intensity of a point on an image given
// (u,v) coordinates in the range [0,1], using bilinear interpolation
double MyForceAlgorithm::imageIntensityAt(cImageLoader *image, double u, double v)
{
// get image width and height
int w = image->getWidth();
int h = image->getHeight();
// get the four integer coordinates that bound our query point (u,v)
int ux0 = cClamp(int(w*u), 0, w-1), ux1 = cClamp(int(w*u)+1, 0, w-1);
int vx0 = cClamp(int(h*v), 0, h-1), vx1 = cClamp(int(h*v)+1, 0, h-1);
// retrieve the four pixel values from the image
int bytes = image->getFormat() == GL_RGB ? 3 : 4;
int offset[2][2] = {
{ (vx0*w + ux0)*bytes, (vx1*w + ux0)*bytes },
{ (vx0*w + ux1)*bytes, (vx1*w + ux1)*bytes }
};
double intensity[2][2];
for (int i = 0; i < 2; ++i)
for (int j = 0; j < 2; ++j) {
// assumes RGB image format, and average
unsigned char *pixel = image->getData() + offset[i][j];
intensity[i][j] = (double(pixel[0]) + double(pixel[1]) + double(pixel[2])) / 765.0;
}
// compute interpolation weights
int uw = cClamp(w*u - ux0, 0.0, 1.0);
int vw = cClamp(h*v - vx0, 0.0, 1.0);
// perform bilinear interpolation and return value
double i0 = cLerp(uw, intensity[0][0], intensity[1][0]);
double i1 = cLerp(uw, intensity[0][1], intensity[1][1]);
return cLerp(vw, i0, i1);
}
void updateHaptics(void)
{
// reset simulation clock
simClock.reset();
simClock.start();
// main haptic simulation loop
while(simulationRunning)
{
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
list<cGELMesh*>::iterator i;
// model water level
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
if (nodePos.z < level)
{
double depth = nodePos.z - level;
force.add(cVector3d(0,0,-100*depth));
}
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
if ((nodePos.z-radius) < level)
{
double depth = (nodePos.z-radius) - level;
force.add(cVector3d(0,0,-1.0 * depth));
node->m_vel.mul(0.95);
}
node->setExternalForce(force);
}
}
}
// compute haptic feedback
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
}
force.add(cMul(1.0, f));
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
node->setExternalForce(tmpfrc);
}
force.add(cMul(4.0, f));
}
}
}
// integrate dynamics
double interval = simClock.stop();
simClock.reset();
simClock.start();
if (interval > 0.001) { interval = 0.001; }
defWorld->updateDynamics(interval);
// scale force
force.mul(0.6 * deviceForceScale);
// water viscosity
if ((pos.z - deviceRadius) < level)
{
// read damping properties of haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
double Kv = 0.8 * info.m_maxLinearDamping;
// read device velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// compute a scale factor [0,1] proportional to percentage
// of tool volume immersed in the water
double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
double scale = cClamp(val, 0.1, 1.0);
// compute force
cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
force.add(forceDamping);
}
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
// 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();
// for each interaction point of the tool we look for any contact events
// with the environment and apply forces accordingly
int numInteractionPoints = tool->getNumInteractionPoints();
for (int i=0; i<numInteractionPoints; i++)
{
// get pointer to next interaction point of tool
cHapticPoint* interactionPoint = tool->getInteractionPoint(i);
// check primary contact point if available
if (interactionPoint->getNumCollisionEvents() > 0)
{
cCollisionEvent* collisionEvent = interactionPoint->getCollisionEvent(0);
// given the mesh object we may be touching, we search for its owner which
// could be the mesh itself or a multi-mesh object. Once the owner found, we
// look for the parent that will point to the ODE object itself.
cGenericObject* object = collisionEvent->m_object->getOwner()->getOwner();
// cast to ODE object
cODEGenericBody* ODEobject = dynamic_cast<cODEGenericBody*>(object);
// if ODE object, we apply interaction forces
if (ODEobject != NULL)
{
ODEobject->addExternalForceAtPoint(-0.3 * interactionPoint->getLastComputedForce(),
collisionEvent->m_globalPos);
}
}
}
// send forces to device
tool->applyForces();
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = cClamp(time, 0.00001, 0.001);
// 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 cMaterial::setShininess(GLuint a_shininess)
{
m_shininess = cClamp(a_shininess, (GLuint)0, (GLuint)128);
}
//===========================================================================
void cCamera::setFieldViewAngle(double a_fieldViewAngle)
{
m_fieldViewAngle = cClamp(a_fieldViewAngle, 0.0, 180.0);
}
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;
}
//===========================================================================
void cCamera::setFieldViewAngle(double a_fieldViewAngle)
{
m_fieldViewAngle = cClamp(a_fieldViewAngle, 0.0, 180.0);
m_orthographicWidth = 0.0;
m_perspectiveMode = true;
}
//===========================================================================
void cSpotLight::setCutOffAngleDeg(const GLfloat& a_angleDeg)
{
m_cutOffAngleDEG = cClamp(a_angleDeg, (GLfloat)0.0, (GLfloat)90.0);
}
//===========================================================================
void cShapeBox::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
// temp variables
bool inside;
cVector3d projectedPoint;
// sign
double signX = cSign(a_toolPos(0) );
double signY = cSign(a_toolPos(1) );
double signZ = cSign(a_toolPos(2) );
// check if tool is located inside box
double tx = (signX * a_toolPos(0) );
double ty = (signY * a_toolPos(1) );
double tz = (signZ * a_toolPos(2) );
inside = false;
if (cContains(tx, 0.0, m_hSizeX))
{
if (cContains(ty, 0.0, m_hSizeY))
{
if (cContains(tz, 0.0, m_hSizeZ))
{
inside = true;
}
}
}
if (inside)
{
// tool is located inside box, compute distance from tool to surface
double m_distanceX = m_hSizeX - (signX * a_toolPos(0) );
double m_distanceY = m_hSizeY - (signY * a_toolPos(1) );
double m_distanceZ = m_hSizeZ - (signZ * a_toolPos(2) );
// search nearest surface
if (m_distanceX < m_distanceY)
{
if (m_distanceX < m_distanceZ)
{
projectedPoint(0) = signX * m_hSizeX;
projectedPoint(1) = a_toolPos(1) ;
projectedPoint(2) = a_toolPos(2) ;
}
else
{
projectedPoint(0) = a_toolPos(0) ;
projectedPoint(1) = a_toolPos(1) ;
projectedPoint(2) = signZ * m_hSizeZ;
}
}
else
{
if (m_distanceY < m_distanceZ)
{
projectedPoint(0) = a_toolPos(0) ;
projectedPoint(1) = signY * m_hSizeY;
projectedPoint(2) = a_toolPos(2) ;
}
else
{
projectedPoint(0) = a_toolPos(0) ;
projectedPoint(1) = a_toolPos(1) ;
projectedPoint(2) = signZ * m_hSizeZ;
}
}
}
else
{
projectedPoint(0) = cClamp(a_toolPos(0) , -m_hSizeX, m_hSizeX);
projectedPoint(1) = cClamp(a_toolPos(1) , -m_hSizeY, m_hSizeY);
projectedPoint(2) = cClamp(a_toolPos(2) , -m_hSizeZ, m_hSizeZ);
}
// return results
projectedPoint.copyto(m_interactionProjectedPoint);
m_interactionInside = inside;
}
