//===========================================================================
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);
}
//===========================================================================
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);
}
// 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;
}
//==============================================================================
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);
}
}
