//===========================================================================
bool cEffectStickSlip::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
// check if history for this IDN exists
if (a_toolID < CHAI_EFFECT_MAX_IDN)
{
if (m_parent->m_interactionInside)
{
// check if a recent valid point has been stored previously
if (!m_history[a_toolID].m_valid)
{
m_history[a_toolID].m_currentStickPosition = a_toolPos;
m_history[a_toolID].m_valid = true;
}
// read parameters for stick and slip model
double stiffness = m_parent->m_material.getStickSlipStiffness();
double forceMax = m_parent->m_material.getStickSlipForceMax();
// compute current force between last stick position and current tool position
double distance = cDistance(a_toolPos, m_history[a_toolID].m_currentStickPosition);
double forceMag = distance * stiffness;
if (forceMag > 0)
{
// if force above threshold, slip...
if (forceMag > forceMax)
{
m_history[a_toolID].m_currentStickPosition = a_toolPos;
a_reactionForce.zero();
}
// ...otherwise stick
else
{
a_reactionForce = (forceMag / distance) * cSub(m_history[a_toolID].m_currentStickPosition, a_toolPos);
}
}
else
{
a_reactionForce.zero();
}
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
m_history[a_toolID].m_valid = false;
a_reactionForce.zero();
return (false);
}
}
else
{
a_reactionForce.zero();
return (false);
}
}
// get PHANTOM state and update shared data
void updatePhantom(void) {
// initialize frequency counter
p_sharedData->phantomFreqCounter.reset();
while(p_sharedData->simulationRunning) {
if (p_sharedData->m_phantomLoopTimer.timeoutOccurred()) {
p_sharedData->m_phantomLoopTimer.stop();
if (p_sharedData->input == PHANTOM) {
// get PHANTOM position and velocity vectors
p_sharedData->p_Phantom->getPosition(pos);
p_sharedData->p_Phantom->getLinearVelocity(vel);
// extract X elements from vectors
p_sharedData->phantomPos = pos.y();
p_sharedData->phantomVel = vel.y();
// update frequency counter
p_sharedData->phantomFreqCounter.signal(1);
}
p_sharedData->m_phantomLoopTimer.start(true);
Sleep(1);
}
}
}
void updateGraphics(void)
{
int px;
// update position of label
labelHapticDeviceModel->setLocalPos(10, displayH - 30, 0.0);
// update position of label and content
double posX = 1000 * hapticDevicePosition.x();
double posY = 1000 * hapticDevicePosition.y();
double posZ = 1000 * hapticDevicePosition.z();
labelHapticDevicePosition->setString("position [mm]: " + cStr(posX, 0) + " " +
cStr(posY, 0) + " " +
cStr(posZ, 0));
labelHapticDevicePosition->setLocalPos(10, displayH - 50, 0.0);
// update haptic rate label
labelHapticRate->setString ("haptic rate: "+cStr(frequencyCounter.getFrequency(), 0) + " [Hz]");
px = (int)(0.5 * (displayW - labelHapticRate->getWidth()));
labelHapticRate->setLocalPos(px, 15);
// render world
camera->renderView(displayW, displayH);
// swap buffers
glutSwapBuffers();
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));
}
//===========================================================================
bool cProxyPointForceAlgo::goalAchieved(const cVector3d& a_proxy, const cVector3d& a_goal) const
{
if (m_useDynamicProxy)
{
return (!(a_proxy.distance(a_goal) > 0.0));
}
else
{
return (a_proxy.distance(a_goal) < (m_epsilonBaseValue));
}
}
//===========================================================================
void cLight::setDir(const cVector3d& a_direction)
{
// We arbitrarily point lights along the x axis of the stored
// rotation matrix... this allows matrix transformations
// to apply to lights.
// m_localRot.setCol0(a_direction);
cVector3d c0, c1, c2, t;
a_direction.copyto(c0);
// check vector
if (c0.lengthsq() < 0.0001) { return; }
// normalize direction vector
c0.normalize();
// compute 2 vector perpendicular to a_direction
t.set(a_direction.y, a_direction.z, a_direction.x);
t.crossr(c0, c1);
c1.crossr(c0, c2);
// c0.negate();
c1.negate();
c2.negate();
// update rotation matrix
m_localRot.setCol(c0,c1,c2);
}
//===========================================================================
bool cEffectVibration::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// read vibration parameters
double vibrationFrequency = m_parent->m_material.getVibrationFrequency();
double vibrationAmplitude = m_parent->m_material.getVibrationAmplitude();
// read time
double time = clock.getCurrentTimeSeconds();
// compute force magnitude
double forceMag = vibrationAmplitude * sin(2.0 * CHAI_PI *vibrationFrequency * time);
a_reactionForce = cMul(forceMag, cVector3d(1, 0, 0));
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
}
//===========================================================================
void cShapeSphere::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
// compute distance from center of sphere to tool
double distance = a_toolPos.length();
// from the position of the tool, search for the nearest point located
// on the surface of the sphere
if (distance > 0)
{
m_interactionProjectedPoint = cMul( (m_radius/distance), a_toolPos);
}
else
{
m_interactionProjectedPoint = a_toolPos;
}
// check if tool is located inside or outside of the sphere
if (distance <= m_radius)
{
m_interactionInside = true;
}
else
{
m_interactionInside = false;
}
}
//===========================================================================
int cVirtualDevice::getPosition(cVector3d& a_position)
{
if (!m_systemReady)
{
a_position.set(0, 0, 0);
return (-1);
}
double x,y,z;
x = (double)(*m_pDevice).PosX;
y = (double)(*m_pDevice).PosY;
z = (double)(*m_pDevice).PosZ;
a_position.set(x, y, z);
return (0);
}
void updateGraphics(void)
{
int px, py;
// update haptic rate label
labelHapticRate->setString ("haptic rate: "+cStr(frequencyCounter.getFrequency(), 0) + " [Hz]");
px = (int)(0.5 * (displayW - labelHapticRate->getWidth()));
labelHapticRate->setLocalPos(px, 15);
// update other widgets
py = (int)(0.5 * (displayH - level->getHeight()));
level->setLocalPos(50, py);
level->setValue(tool->m_lastComputedGlobalForce.length());
px = displayW - 80;
py = (int)(0.5 * displayH);
dial->setLocalPos(px, py);
dial->setValue1(angVel.length());
// render world
camera->renderView(displayW, displayH);
// swap buffers
glutSwapBuffers();
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));
}
//==============================================================================
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_interactionPoint);
// 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 (m_enabledInside || (!m_parent->m_interactionInside))
{
if ((distance < magnetMaxDistance) && (stiffness > 0))
{
double d = magnetMaxForce / stiffness;
if (distance < d)
{
forceMagnitude = stiffness * distance;
}
else
{
double dx = (magnetMaxDistance - d);
if (dx > 0)
{
double k = magnetMaxForce / dx;
forceMagnitude = k * (magnetMaxDistance - distance);
}
}
// compute reaction force
int sign = -1;
if (m_parent->m_interactionInside)
{
sign = 1;
}
a_reactionForce = cMul(sign * forceMagnitude, m_parent->m_interactionNormal);
return (true);
}
else
{
return (false);
}
}
else
{
// the tool is located outside the magnet zone
a_reactionForce.zero();
return (false);
}
}
//===========================================================================
int cPhantomDevice::getPosition(cVector3d& a_position)
{
// check if drivers are installed
if (!m_driverInstalled) return (-1);
double x,y,z;
int error = hdPhantomGetPosition(m_deviceID, &x, &y, &z);
a_position.set(x, y, z);
estimateLinearVelocity(a_position);
return (error);
}
void VirtualHapticDevice::GetCursorVelocity(cVector3d& velocity) {
double currentTime = clock.getCurrentTimeSeconds();
double ellapsedTime = currentTime - lastClock;
lastClock = currentTime;
cVector3d currentPos;
chaiDevice->getPosition(currentPos);
velocity = currentPos - lastPosition;
velocity.mul(1.0 / ellapsedTime );
lastPosition.copyfrom(currentPos);
}
//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
cVector3d toolProjection = a_toolPos;
toolProjection.z(0.0);
m_interactionNormal.set(0,0,1);
// search for the nearest point on the torus medial axis
if (a_toolPos.lengthsq() > C_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));
// compute eventual penetration of tool inside the torus
cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);
double distance = vectTorusTool.length();
// normal
if (distance > 0.0)
{
m_interactionNormal = vectTorusTool;
m_interactionNormal.normalize();
}
// tool is located inside the torus
if ((distance < m_innerRadius) && (distance > 0.001))
{
m_interactionInside = true;
}
// tool is located outside the torus
else
{
m_interactionInside = false;
}
// compute surface point
double dist = vectTorusTool.length();
if (dist > 0)
{
vectTorusTool.mul(1/dist);
}
vectTorusTool.mul(m_innerRadius);
pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
}
else
{
m_interactionInside = false;
m_interactionPoint = a_toolPos;
}
}
//===========================================================================
int cVirtualDevice::getForce(cVector3d& a_force)
{
if (!m_systemReady)
{
a_force.set(0,0,0);
return (-1);
}
a_force.x = ((*m_pDevice).ForceX);
a_force.y = ((*m_pDevice).ForceY);
a_force.z = ((*m_pDevice).ForceZ);
return (0);
}
//===========================================================================
int cHydraDevice::getPosition(cVector3d& a_position)
{
//std::cout << "get pos";
/************************************************************************
STEP 7:
Here you may implement code which reads the position (X,Y,Z) from
your haptic device. Read the values from your device and modify
the local variable (x,y,z) accordingly.
If the operation fails return an error code such as -1 for instance.
Note:
For consistency, units must be in meters.
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
*************************************************************************/
int error = 0;
double x,y,z;
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
y = acd.controllers[a_deviceNumber].pos[0] / 1000.0f;
z = acd.controllers[a_deviceNumber].pos[1] / 1000.0f - 0.3;
x = acd.controllers[a_deviceNumber].pos[2] / 1000.0f - 0.3;
// offset
cMatrix3d r;
getRotation(r);
cVector3d v(-0.08,0,0);
v = r * v;
// store new position values
a_position.set(x+v.x(), y+v.y(), z+v.z());
// estimate linear velocity
estimateLinearVelocity(a_position);
// exit
return (error);
}
//===========================================================================
bool cCollisionBrute::computeCollision(cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB,
cGenericObject*& a_colObject,
cTriangle*& a_colTriangle,
cVector3d& a_colPoint,
double& a_colSquareDistance,
int a_proxyCall)
{
// temp variables for storing results
cGenericObject* colObject;
cTriangle* colTriangle;
cVector3d colPoint;
bool hit = false;
// convert two point segment into a segment described by a point and
// a directional vector
cVector3d dir;
a_segmentPointB.subr(a_segmentPointA, dir);
// compute the squared length of the segment
double colSquareDistance = dir.lengthsq();
// check all triangles for collision and return the nearest one
unsigned int ntriangles = m_triangles->size();
for (unsigned int i=0; i<ntriangles; i++)
{
// check for a collision between this triangle and the segment by
// calling the triangle's collision detection method; it will only
// return true if the distance between the segment origin and this
// triangle is less than the current closest intersecting triangle
// (whose distance squared is kept in colSquareDistance)
if ((*m_triangles)[i].computeCollision(
a_segmentPointA, dir, colObject, colTriangle, colPoint, colSquareDistance))
{
a_colObject = colObject;
a_colTriangle = colTriangle;
a_colPoint = colPoint;
a_colSquareDistance = colSquareDistance;
hit = true;
}
}
// return result
return (hit);
}
//===========================================================================
bool cEffectViscosity::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// the tool is located inside the object.
double viscosity = m_parent->m_material.getViscosity();
a_reactionForce = cMul(-viscosity, a_toolVel);
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force.
a_reactionForce.zero();
return (false);
}
}
//===========================================================================
void cDirectionalLight::setDir(const cVector3d& a_direction)
{
// We arbitrarily point lights along the x axis of the stored
// rotation matrix... this allows matrix transformations
// to apply to lights.
// m_localRot.setCol0(a_direction);
cVector3d c0, c1, c2, t0, t1;
a_direction.copyto(c0);
// check vector
if (c0.lengthsq() < 0.0001) {
return;
}
// normalize direction vector
c0.normalize();
// compute 2 vector perpendicular to a_direction
t0.set(0.0, 0.0, 1.0);
t1.set(0.0, 1.0, 0.0);
double a0 = cAngle(c0, t0);
double a1 = cAngle(c0, t1);
if (sin(a0) > sin(a1))
{
c0.crossr(t0, c1);
c0.crossr(c1, c2);
}
else
{
c0.crossr(t1, c1);
c0.crossr(c1, c2);
}
c1.normalize();
c2.normalize();
// update rotation matrix
m_localRot.setCol(c0,c1,c2);
}
//===========================================================================
bool cEffectSurface::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// the tool is located inside the object,
// we compute a reaction force using Hooke's law
double stiffness = m_parent->m_material.getStiffness();
a_reactionForce = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
}
//===========================================================================
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);
}
//===========================================================================
cVector3d cShapeTorus::computeLocalForce(const cVector3d& a_localPosition)
{
// In the following we compute the reaction forces between the tool and the
// sphere.
cVector3d localForce;
// project pointer on torus plane (z=0)
cVector3d fingerProjection = a_localPosition;
fingerProjection.z = 0;
// search for the nearest point on the torus medial axis
if (a_localPosition.lengthsq() > CHAI_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(fingerProjection));
// compute eventual penetration of finger inside the torus
cVector3d vectTorusFinger = cSub(a_localPosition, pointAxisTorus);
double distance = vectTorusFinger.length();
// finger inside torus, compute forces
if ((distance < m_innerRadius) && (distance > 0.001))
{
localForce = cMul((m_innerRadius - distance) * (m_material.getStiffness()), cNormalize(vectTorusFinger));
}
// finger is outside torus
else
{
localForce.zero();
}
}
else
{
localForce.zero();
}
return (localForce);
}
//==============================================================================
bool cMyCustomDevice::getPosition(cVector3d& a_position)
{
////////////////////////////////////////////////////////////////////////////
/*
STEP 7:
Here you shall implement code that reads the position (X,Y,Z) from
your haptic device. Read the values from your device and modify
the local variable (x,y,z) accordingly.
If the operation fails return an C_ERROR, C_SUCCESS otherwise
Note:
For consistency, units must be in meters.
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
*/
////////////////////////////////////////////////////////////////////////////
bool result = C_SUCCESS;
double x,y,z;
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
x = 0.0; // x = getMyDevicePositionX()
y = 0.0; // y = getMyDevicePositionY()
z = 0.0; // z = getMyDevicePositionZ()
// store new position values
a_position.set(x, y, z);
// estimate linear velocity
estimateLinearVelocity(a_position);
// exit
return (result);
}
//===========================================================================
bool cEffectPotentialField::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// the tool is located inside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
else
{
// the tool is located outside the object,
// we compute a reaction force using Hooke's law
double stiffness = m_parent->m_material->getStiffness();
double maxForce = m_parent->m_material->getMagnetMaxForce();
cVector3d force = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));
if (maxForce > 0.0)
{
double ratio = force.length() / maxForce;
if (ratio > 1.0)
{
force = (1.0 / ratio) * force;
}
}
else
{
force.zero();
}
a_reactionForce = force;
return (true);
}
}
//===========================================================================
int cMyCustomDevice::getPosition(cVector3d& a_position)
{
/************************************************************************
STEP 7:
Here you may implement code which reads the position (X,Y,Z) from
your haptic device. Read the values from your device and modify
the local variable (x,y,z) accordingly.
If the operation fails return an error code such as -1 for instance.
Note:
For consistency, units must be in meters.
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
*************************************************************************/
int error = 0;
double x,y,z;
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
x = 0.0; // x = getMyDevicePositionX()
y = 0.0; // y = getMyDevicePositionY()
z = 0.0; // z = getMyDevicePositionZ()
// store new position values
a_position.set(x, y, z);
// estimate linear velocity
estimateLinearVelocity(a_position);
// exit
return (error);
}
void updateHaptics(void)
{
// reset clock
cPrecisionClock clock;
clock.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
// update frequency counter
frequencyCounter.signal(1);
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
/////////////////////////////////////////////////////////////////////
// HAPTIC SIMULATION
/////////////////////////////////////////////////////////////////////
// get position of cursor in global coordinates
cVector3d toolPos = tool->getDeviceGlobalPos();
// get position of object in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute a vector from the center of mass of the object (point of rotation) to the tool
cVector3d v = cSub(toolPos, objectPos);
// compute angular acceleration based on the interaction forces
// between the tool and the object
cVector3d angAcc(0,0,0);
if (v.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute the effective force that contributes to rotating the object.
cVector3d force = toolForce - cProject(toolForce, v);
// compute the resulting torque
cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));
// update rotational acceleration
const double INERTIA = 0.4;
angAcc = (1.0 / INERTIA) * torque;
}
// update rotational velocity
angVel.add(timeInterval * angAcc);
// set a threshold on the rotational velocity term
const double MAX_ANG_VEL = 10.0;
double vel = angVel.length();
if (vel > MAX_ANG_VEL)
{
angAcc.mul(MAX_ANG_VEL / vel);
}
// add some damping too
const double DAMPING = 0.1;
angVel.mul(1.0 - DAMPING * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
angVel.zero();
}
// compute the next rotation configuration of the object
if (angVel.length() > C_SMALL)
{
object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
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);
}
}
// ch lab ---to be filled in by the students---
// Overriden from class cProxyPointForceAlgo
// Here, a_goal has been computed by collision detection above as the constrained goal towards which
// the proxy should be moved. But before moving it freely to that location, let us check if friction allows
// us to do so. we answer the question: to what extent along the proxy-goal segment can we forward the proxy?
void ch_proxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal, const cVector3d& a_proxy, cVector3d& a_normal, cGenericObject* a_parent, const cVector3d& a_toolVel)
{
// In this method, our aim is to calculate the the next best position for the proxy (m_nextBestProxyGlobalPos), considering friction.
// Among other things, we are given the goal position (a_goal), the current proxy position (a_proxy), the parent object (a_parent),
// the tool velocity (a_toolVel)
// We will use this variable to determine if we should use the static or the dynamic
// friction coeff to compute current frictional force.
static double last_device_vel;
// friction coefficients assigned to object surface
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;
}
// read friction coefficients here
// -----------------------your code here------------------------------------
double dynamic_coeff = parent_mesh->m_material.getDynamicFriction();
double static_coeff = parent_mesh->m_material.getStaticFriction();
// find the penetration depth of the actual device position from the nominal object surface
double pen_depth = cDistance(a_goal, m_deviceGlobalPos);
// shall we use the static or the dynamic friction coeff. for the cone radius calculation?
double cone_radius;
if(last_device_vel < SMALL_VEL)
{
//// the radius of the friction cone
//-----------------------your code here------------------------------------
cone_radius = static_coeff*pen_depth;
}
else
{
//// the radius of the friction cone
//-----------------------your code here------------------------------------
cone_radius = dynamic_coeff*pen_depth;
}
// vector from the current proxy position to the new sub-goal
cVector3d a_proxyGoal, a_proxyGoalNormalized;
a_goal.subr(a_proxy, a_proxyGoal);
// normalize the proxy goal vector
a_proxyGoal.normalizer(a_proxyGoalNormalized);
double a_proxyGoalLength = a_proxyGoal.length();
if(a_proxyGoalLength < cone_radius)
// The proxy is inside the friction cone already.
return;
else
{
// The proxy is outside the friction cone, we should advance it towards the cone circumference,
// along the vector from the current proxy position to the current goal position
//-----------------------your code here------------------------------------
// calculate a value for m_nextBestProxyGlobalPos
m_nextBestProxyGlobalPos=a_proxy+(a_proxyGoalLength-cone_radius)*a_proxyGoalNormalized;
}
// record last velocity in order to decide if static or dynamic friction is to be applied during the
// next iteration
last_device_vel = a_toolVel.length();
}
//===========================================================================
bool cCollisionAABB::computeCollision(cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB,
cGenericObject*& a_colObject,
cTriangle*& a_colTriangle,
cVector3d& a_colPoint,
double& a_colSquareDistance,
int a_proxyCall)
{
// convert two point segment into a segment described by a point and
// a directional vector
cVector3d dir;
a_segmentPointB.subr(a_segmentPointA, dir);
// if this is a subsequent call from the proxy algorithm after detecting
// an initial collision, and if the flag to use neighbor checking is set,
// only neighbors of the triangle from the first collision detection
// need to be checked
if ((m_useNeighbors) && (a_proxyCall > 1) && (m_root != NULL) &&
(m_lastCollision != NULL) && (m_lastCollision->m_neighbors != NULL))
{
// initialize temp variables for output parameters
cGenericObject* colObject;
cTriangle* colTriangle;
cVector3d colPoint;
double colSquareDistance = dir.lengthsq();
bool firstHit = true;
// check each neighbor, and find the closest for which there is a
// collision, if any
unsigned int ntris = m_lastCollision->m_neighbors->size();
std::vector<cTriangle*>* neighbors = m_lastCollision->m_neighbors;
for (unsigned int i=0; i<ntris; i++)
{
cTriangle* tri = (*neighbors)[i];
if (tri == 0) {
CHAI_DEBUG_PRINT("Oops... invalid neighbor\n");
continue;
}
if (tri->computeCollision(
a_segmentPointA, dir, colObject, colTriangle,
colPoint, colSquareDistance))
{
// if this intersected triangle is closer to the segment origin
// than any other found so far, set the output parameters
if (firstHit || (colSquareDistance < a_colSquareDistance))
{
m_lastCollision = colTriangle;
a_colObject = colObject;
a_colTriangle = colTriangle;
a_colPoint = colPoint;
a_colSquareDistance = colSquareDistance;
firstHit = false;
}
}
}
// if at least one neighbor triangle was intersected, return true
if (!firstHit) return true;
// otherwise there was no collision; return false
if (a_proxyCall != -1) m_lastCollision = NULL;
return false;
}
// otherwise, if this is the first call in an iteration of the proxy
// algorithm (or a call from any other algorithm), check the AABB tree
// if the root is null, the tree is empty, so there can be no collision
if (m_root == NULL)
{
if (a_proxyCall != -1) m_lastCollision = NULL;
return (false);
}
// create an axis-aligned bounding box for the line
cCollisionAABBBox lineBox;
lineBox.setEmpty();
lineBox.enclose(a_segmentPointA);
lineBox.enclose(a_segmentPointB);
// test for intersection between the line segment and the root of the
// collision tree; the root will recursively call children down the tree
a_colSquareDistance = dir.lengthsq();
bool result = m_root->computeCollision(a_segmentPointA, dir, lineBox,
a_colTriangle, a_colPoint, a_colSquareDistance);
// if there was a collision, set m_lastCollision to the intersected triangle
// returned by the call to the root of the tree, and set the output
// parameter for the intersected mesh to the parent of this triangle
if (result)
{
if (a_proxyCall != -1) m_lastCollision = a_colTriangle;
a_colObject = a_colTriangle->getParent();
}
else
{
if (a_proxyCall != -1) m_lastCollision = NULL;
}
// return whether there was an intersection
return result;
}
//===========================================================================
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 BasicFluidParticle::GetVelocity(cVector3d& velocity) {
velocity.copyfrom(_velocity);
}
