// Add a mass to or remove a mass from the end of the chain
void TForm1::add_ball() {
// Put this ball to the right of the current last ball
CBall* b = new CBall();
m_active_balls.push_back(b);
int ball_index = m_active_balls.size() - 1;
CBall* neighbor = m_active_balls[ball_index - 1];
// Create a position for each ball, moving from left to right
// with increasing i
cVector3d pos = neighbor->getPos();
pos.add(INITIAL_BALL_SPACING,0,0);
b->setPos(pos);
world->addChild(b);
CSpring* s = new CSpring();
m_active_springs.push_back(s);
s->m_endpoint_1 = m_active_balls[ball_index];
s->m_endpoint_2 = m_active_balls[ball_index-1];
s->m_endpoint_1->m_springs.push_back(s);
s->m_endpoint_2->m_springs.push_back(s);
// Set the spring's rest length to be the initial distance between
// the balls
double d = cDistance(s->m_endpoint_1->getPos(),s->m_endpoint_2->getPos());
s->m_rest_length = d;
world->addChild(s);
}
//===========================================================================
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);
}
}
//-----------------------------------------------------------------------
double cSegment3d::distanceSquaredToPoint(const cVector3d& a_point,
double& a_t,
cVector3d* a_closestPoint)
{
double mag = cDistance(m_start,m_end);
// Project this point onto the line
a_t = (a_point - m_start) * (m_end - m_start) / (mag * mag);
// Clip to segment endpoints
if (a_t < 0.0)
a_t = 0.0;
else if (a_t > 1.0)
a_t = 1.0;
// Find the intersection point
cVector3d intersection = m_start + a_t * (m_end - m_start);
if (a_closestPoint)
{
*a_closestPoint = intersection;
}
// Compute distance
return cDistanceSq(a_point,intersection);
}
//==============================================================================
void cDrawArrow(const cVector3d& a_arrowStart,
const cVector3d& a_arrowTip,
const double a_width)
{
#ifdef C_USE_OPENGL
glPushMatrix();
// We don't really care about the up vector, but it can't
// be parallel to the arrow...
cVector3d up = cVector3d(0,1,0);
cVector3d arrow = a_arrowTip-a_arrowStart;
arrow.normalize();
double d = fabs(cDot(up,arrow));
if (d > .9)
{
up = cVector3d(1,0,0);
}
cLookAt(a_arrowStart, a_arrowTip, up);
double distance = cDistance(a_arrowTip,a_arrowStart);
// This flips the z axis around
glRotatef(180,1,0,0);
// create a new OpenGL quadratic object
GLUquadricObj *quadObj;
quadObj = gluNewQuadric();
#define ARROW_CYLINDER_PORTION 0.75
#define ARRROW_CONE_PORTION (1.0 - 0.75)
// set rendering style
gluQuadricDrawStyle(quadObj, GLU_FILL);
// set normal-rendering mode
gluQuadricNormals(quadObj, GLU_SMOOTH);
// render a cylinder and a cone
glRotatef(180,1,0,0);
gluDisk(quadObj,0,a_width,10,10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width, a_width, distance*ARROW_CYLINDER_PORTION, 10, 10);
glTranslated(0, 0, ARROW_CYLINDER_PORTION*distance);
glRotatef(180, 1, 0, 0);
gluDisk(quadObj, 0, a_width*2.0, 10, 10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width*2.0, 0.0, distance*ARRROW_CONE_PORTION, 10, 10);
// delete our quadric object
gluDeleteQuadric(quadObj);
glPopMatrix();
#endif
}
//==============================================================================
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);
}
}
//===========================================================================
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;
}
//===========================================================================
cGELLinearSpring::cGELLinearSpring(cGELMassParticle* a_node0, cGELMassParticle* a_node1)
{
// set nodes:
m_node0 = a_node0;
m_node1 = a_node1;
m_enabled = true;
// set default color
m_color = default_color;
// compute initial length
m_length0 = cDistance(m_node1->m_pos, m_node0->m_pos);
// set elongation spring constant [N/M]
m_kSpringElongation = default_kSpringElongation;
}
//===========================================================================
void cCamera::adjustClippingPlanes()
{
// check if world is valid
cWorld* world = getParentWorld();
if (world == NULL) { return; }
// compute size of the world
world->computeBoundaryBox(true);
// compute a distance slightly larger the world size
cVector3d max = world->getBoundaryMax();
cVector3d min = world->getBoundaryMin();
double distance = 2.0 * cDistance(min, max);
// update clipping plane:
setClippingPlanes(distance / 1000.0, distance);
}
void TForm1::compute_spring_forces()
{
double curtime = g_timer.GetTime();
if (g_last_iteration_time < 0)
{
g_last_iteration_time = curtime;
return;
}
double elapsed = curtime - g_last_iteration_time;
g_last_iteration_time = curtime;
unsigned int i;
// Clear the force that's applied to each ball
for(i=0; i<m_active_balls.size(); i++)
{
m_active_balls[i]->current_force.set(0,0,0);
}
if (simulationOn)
{
CBall* b = m_active_balls[0];
cVector3d old_p = b->getPos();
b->setPos(tool->m_deviceGlobalPos);
b->m_velocity = cDiv(elapsed,cSub(b->getPos(),old_p));
}
// Compute the current length of each spring and apply forces
// on each mass accordingly
for(i=0; i<m_active_springs.size(); i++)
{
CSpring* s = m_active_springs[i];
double d = cDistance(s->m_endpoint_1->getPos(),s->m_endpoint_2->getPos());
s->m_current_length = d;
// This spring's deviation from its rest length
//
// (positive deviation -> spring is too long)
double x = s->m_current_length - s->m_rest_length;
// Apply a force to ball 1 that pulls it toward ball 2
// when the spring is too long
cVector3d f1 = cMul(s->m_spring_constant*x*1.0,
cSub(s->m_endpoint_2->getPos(),s->m_endpoint_1->getPos()));
s->m_endpoint_1->current_force.add(f1);
// Add the opposite force to ball 2
s->m_endpoint_2->current_force.add(cMul(-1.0,f1));
}
// Update velocities and positions based on forces
for(i=0; i<m_active_balls.size(); i++)
{
CBall* b = m_active_balls[i];
// Certain forces don't get applied to the "haptic ball"
// when haptics are enabled...
if (simulationOn == 0 || i != 0) {
cVector3d f_damping = cMul(DAMPING_CONSTANT,b->m_velocity);
b->current_force.add(f_damping);
}
cVector3d f_gravity(0,GRAVITY_CONSTANT*b->m_mass,0);
b->current_force.add(f_gravity);
cVector3d p = b->getPos();
if (p.y - b->m_radius < FLOOR_Y_POSITION) {
double penetration = FLOOR_Y_POSITION - (p.y - b->m_radius);
b->current_force.add(0,m_floor_spring_constant*penetration,0);
}
cVector3d f_floor(0,0,0);
cVector3d a = cDiv(b->m_mass,b->current_force);
b->m_velocity.add(cMul(elapsed,a));
// We handle the 0th ball specially when haptics is enabled
if (simulationOn == 0 || i != 0) {
p.add(cMul(elapsed,b->m_velocity));
b->setPos(p);
}
}
// Set the haptic force appropriately to reflect the force
// applied to ball 0
m_haptic_force = cMul(HAPTIC_FORCE_CONSTANT,m_active_balls[0]->current_force);
}
//===========================================================================
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);
}
}
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);
}
// 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 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)
{
// state machine
const int STATE_IDLE = 1;
const int STATE_MODIFY_MAP = 2;
const int STATE_MOVE_CAMERA = 3;
int state = STATE_IDLE;
// current tool position
cVector3d toolGlobalPos; // global world coordinates
cVector3d toolLocalPos; // local coordinates
// previous tool position
cVector3d prevToolGlobalPos; // global world coordinates
cVector3d prevToolLocalPos; // local coordinates
// 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();
// read user switch
bool userSwitch = tool->getUserSwitch(0);
// update tool position
toolGlobalPos = tool->getDeviceGlobalPos();
toolLocalPos = tool->getDeviceLocalPos();
if ((state == STATE_MOVE_CAMERA) && (!userSwitch))
{
state = STATE_IDLE;
// enable haptic interaction with map
object->setHapticEnabled(true, true);
}
else if (((state == STATE_MODIFY_MAP) && (!userSwitch)) ||
((state == STATE_MODIFY_MAP) && (!tool->isInContact(magneticLine))))
{
state = STATE_IDLE;
// disable magnetic line
magneticLine->setHapticEnabled(false);
magneticLine->setShowEnabled(false);
// disable spheres
sphereA->setShowEnabled(false);
sphereB->setShowEnabled(false);
// enable haptic interaction with map
object->setHapticEnabled(true, true);
// disable forces
tool->setForcesOFF();
// update bounding box (can take a little time)
object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
// enable forces again
tool->setForcesON();
}
// user clicks with the mouse
else if ((state == STATE_IDLE) && (userSwitch))
{
// start deforming object
if (tool->isInContact(object))
{
state = STATE_MODIFY_MAP;
// update position of line
cVector3d posA = toolGlobalPos;
posA.z =-0.7;
cVector3d posB = toolGlobalPos;
posB.z = 0.7;
magneticLine->m_pointA = posA;
magneticLine->m_pointB = posB;
// update position of spheres
sphereA->setPos(posA);
sphereB->setPos(posB);
// enable spheres
sphereA->setShowEnabled(true);
sphereB->setShowEnabled(true);
// enable magnetic line
magneticLine->setHapticEnabled(true);
magneticLine->setShowEnabled(true);
// disable haptic interaction with map
object->setHapticEnabled(false, true);
}
// start moving camera
else
{
state = STATE_MOVE_CAMERA;
// disable haptic interaction with map
object->setHapticEnabled(false, true);
}
}
// modify map
else if (state == STATE_MODIFY_MAP)
{
// compute tool offset
cVector3d offset = toolGlobalPos - prevToolGlobalPos;
// map offset on z axis
double offsetHeight = offset.z;
// apply offset to all vertices through a weighted function
int numVertices = object->getNumVertices(true);
for (int i=0; i<numVertices; i++)
{
// get next vertex
cVertex* vertex = object->getVertex(i, true);
// compute distance between vertex and tool
cVector3d posTool = tool->getProxyGlobalPos();
cVector3d posVertex = vertex->getPos();
double distance = cDistance(posTool, posVertex);
// compute factor
double RADIUS = 0.4;
double relativeDistance = distance / RADIUS;
double clampedRelativeDistance = cClamp01(relativeDistance);
double w = 0.5 + 0.5 * cos(clampedRelativeDistance * CHAI_PI);
// apply offset
double offsetVertexHeight = w * offsetHeight;
posVertex.z = posVertex.z + offsetVertexHeight;
vertex->setPos(posVertex);
}
}
// move camera
else if (state == STATE_MOVE_CAMERA)
{
// compute tool offset
cVector3d offset = toolLocalPos - prevToolLocalPos;
// apply camera motion
cameraDistance = cameraDistance - 2 * offset.x;
cameraAngleH = cameraAngleH - 40 * offset.y;
cameraAngleV = cameraAngleV - 40 * offset.z;
updateCameraPosition();
}
// store tool position
prevToolLocalPos = toolLocalPos;
prevToolGlobalPos = toolGlobalPos;
// send forces to device
tool->applyForces();
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
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);
}
}
}
