//-----------------------------------------------------------------------
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);
}
bool createTetGenMesh(cGELMesh *a_object, char *a_filename, char *a_filenameHighRes)
{
a_object->m_useSkeletonModel = false;
a_object->m_useMassParticleModel = true;
cGELMesh* model = new cGELMesh(world);
model->loadFromFile(a_filenameHighRes);
tetgenio input;
if (input.load_off(a_filename))
{
// use TetGen to tetrahedralize our mesh
tetgenio output;
tetrahedralize(TETGEN_SWITCHES0, &input, &output);
// create a vertex in the object for each point of the result
for (int p = 0, pi = 0; p < output.numberofpoints; ++p, pi += 3)
{
cVector3d point;
point.x = output.pointlist[pi+0];
point.y = output.pointlist[pi+1];
point.z = output.pointlist[pi+2];
a_object->newVertex(point);
}
// create a triangle for each face on the surface
set<int> outside;
for (int t = 0, ti = 0; t < output.numberoftrifaces; ++t, ti += 3)
{
cVector3d p[3];
int vi[3];
for (int i = 0; i < 3; ++i) {
int tc = output.trifacelist[ti+i];
outside.insert(tc);
vi[i] = tc;
int pi = tc*3;
p[i].x = output.pointlist[pi+0];
p[i].y = output.pointlist[pi+1];
p[i].z = output.pointlist[pi+2];
}
//unsigned int index = a_object->newTriangle(p[1], p[0], p[2]);
a_object->newTriangle(vi[1], vi[0], vi[2]);
}
a_object->computeAllNormals();
// compute a boundary box
a_object->computeBoundaryBox(true);
// get dimensions of object
double size = cSub(a_object->getBoundaryMax(), a_object->getBoundaryMin()).length();
// resize object to screen
if (size > 0)
{
model->scale(1.5 / size);
a_object->scale(1.5 / size);
}
// build dynamic vertices
cGELMassParticle::default_mass = 0.002;
cGELMassParticle::default_kDampingPos = 4.0;
cGELMassParticle::default_gravity.set(0,0,-0.1);
a_object->buildVertices();
// get all the edges of our tetrahedra
set< pair<int,int> > springs;
for (int t = 0, ti = 0; t < output.numberoftetrahedra; ++t, ti += 4)
{
// store each edge of the tetrahedron as a pair of indices
for (int i = 0; i < 4; ++i) {
int v0 = output.tetrahedronlist[ti+i];
for (int j = i+1; j < 4; ++j) {
int v1 = output.tetrahedronlist[ti+j];
springs.insert(pair<int,int>(min(v0,v1), max(v0,v1)));
}
}
}
// create a spring on each tetrahedral edge we found in the output
cGELLinearSpring::default_kSpringElongation = 40.0; // 0.55;
for (set< pair<int,int> >::iterator it = springs.begin(); it != springs.end(); ++it)
{
cVertex* v0 = a_object->getVertex(it->first);
cVertex* v1 = a_object->getVertex(it->second);
cGELMassParticle* m0 = a_object->m_gelVertices[v0->m_tag].m_massParticle;
cGELMassParticle* m1 = a_object->m_gelVertices[v1->m_tag].m_massParticle;
cGELLinearSpring* spring = new cGELLinearSpring(m0, m1);
a_object->m_linearSprings.push_back(spring);
}
// extract texture
int numModelV = model->getNumVertices(true);
for (set<int>::iterator it = outside.begin(); it != outside.end(); ++it)
{
cVertex *v = a_object->getVertex(*it);
cVertex *t = 0;
double closest = 1e10;
for (int j = 0; j < numModelV; ++j) {
cVertex *test = model->getVertex(j, true);
double d = cDistanceSq(v->getPos(), test->getPos());
if (d < closest) {
closest = d;
t = test;
}
}
v->setTexCoord(t->getTexCoord());
}
cMesh* mesh = (cMesh*)model->getChild(0);
mesh->m_texture->setWrapMode(GL_CLAMP, GL_CLAMP);
a_object->setTexture(mesh->m_texture, true);
a_object->setUseTexture(true, true);
cMaterial mat;
mat.m_ambient.set(0.7, 0.7, 0.7);
mat.m_diffuse.set(0.8, 0.8, 0.8);
mat.m_specular.set(0.0, 0.0, 0.0);
a_object->setMaterial(mat, true);
return (true);
}
return (false);
}
//==============================================================================
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);
}
}
