void CSceneCollisionManager::testTriangleIntersection(SCollisionData* colData, const core::triangle3df& triangle) { const core::plane3d<f32> trianglePlane = triangle.getPlane(); // only check front facing polygons if ( !trianglePlane.isFrontFacing(colData->normalizedVelocity) ) return; // get interval of plane intersection f32 t1, t0; bool embeddedInPlane = false; // calculate signed distance from sphere position to triangle plane f32 signedDistToTrianglePlane = trianglePlane.getDistanceTo( colData->basePoint); f32 normalDotVelocity = trianglePlane.Normal.dotProduct(colData->velocity); if ( core::iszero ( normalDotVelocity ) ) { // sphere is traveling parallel to plane if (fabs(signedDistToTrianglePlane) >= 1.0f) return; // no collision possible else { // sphere is embedded in plane embeddedInPlane = true; t0 = 0.0; t1 = 1.0; } } else { normalDotVelocity = core::reciprocal ( normalDotVelocity ); // N.D is not 0. Calculate intersection interval t0 = (-1.f - signedDistToTrianglePlane) * normalDotVelocity; t1 = (1.f - signedDistToTrianglePlane) * normalDotVelocity; // Swap so t0 < t1 if (t0 > t1) { f32 tmp = t1; t1 = t0; t0 = tmp; } // check if at least one value is within the range if (t0 > 1.0f || t1 < 0.0f) return; // both t values are outside 1 and 0, no collision possible // clamp to 0 and 1 t0 = core::clamp ( t0, 0.f, 1.f ); t1 = core::clamp ( t1, 0.f, 1.f ); } // at this point we have t0 and t1, if there is any intersection, it // is between this interval core::vector3df collisionPoint; bool foundCollision = false; f32 t = 1.0f; // first check the easy case: Collision within the triangle; // if this happens, it must be at t0 and this is when the sphere // rests on the front side of the triangle plane. This can only happen // if the sphere is not embedded in the triangle plane. if (!embeddedInPlane) { core::vector3df planeIntersectionPoint = (colData->basePoint - trianglePlane.Normal) + (colData->velocity * t0); if (triangle.isPointInsideFast(planeIntersectionPoint)) { foundCollision = true; t = t0; collisionPoint = planeIntersectionPoint; } } // if we havent found a collision already we will have to sweep // the sphere against points and edges of the triangle. Note: A // collision inside the triangle will always happen before a // vertex or edge collision. if (!foundCollision) { core::vector3df velocity = colData->velocity; core::vector3df base = colData->basePoint; f32 velocitySqaredLength = velocity.getLengthSQ(); f32 a,b,c; f32 newT; // for each edge or vertex a quadratic equation has to be solved: // a*t^2 + b*t + c = 0. We calculate a,b, and c for each test. // check against points a = velocitySqaredLength; // p1 b = 2.0f * (velocity.dotProduct(base - triangle.pointA)); c = (triangle.pointA-base).getLengthSQ() - 1.f; if (getLowestRoot(a,b,c,t, &newT)) { t = newT; foundCollision = true; collisionPoint = triangle.pointA; } // p2 if (!foundCollision) { b = 2.0f * (velocity.dotProduct(base - triangle.pointB)); c = (triangle.pointB-base).getLengthSQ() - 1.f; if (getLowestRoot(a,b,c,t, &newT)) { t = newT; foundCollision = true; collisionPoint = triangle.pointB; } } // p3 if (!foundCollision) { b = 2.0f * (velocity.dotProduct(base - triangle.pointC)); c = (triangle.pointC-base).getLengthSQ() - 1.f; if (getLowestRoot(a,b,c,t, &newT)) { t = newT; foundCollision = true; collisionPoint = triangle.pointC; } } // check against edges: // p1 --- p2 core::vector3df edge = triangle.pointB - triangle.pointA; core::vector3df baseToVertex = triangle.pointA - base; f32 edgeSqaredLength = edge.getLengthSQ(); f32 edgeDotVelocity = edge.dotProduct(velocity); f32 edgeDotBaseToVertex = edge.dotProduct(baseToVertex); // calculate parameters for equation a = edgeSqaredLength* -velocitySqaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSqaredLength* (2.f *velocity.dotProduct(baseToVertex)) - 2.0f*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSqaredLength* (1.f -baseToVertex.getLengthSQ()) + edgeDotBaseToVertex*edgeDotBaseToVertex; // does the swept sphere collide against infinite edge? if (getLowestRoot(a,b,c,t,&newT)) { f32 f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength; if (f >=0.0f && f <= 1.0f) { // intersection took place within segment t = newT; foundCollision = true; collisionPoint = triangle.pointA + (edge*f); } } // p2 --- p3 edge = triangle.pointC-triangle.pointB; baseToVertex = triangle.pointB - base; edgeSqaredLength = edge.getLengthSQ(); edgeDotVelocity = edge.dotProduct(velocity); edgeDotBaseToVertex = edge.dotProduct(baseToVertex); // calculate parameters for equation a = edgeSqaredLength* -velocitySqaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) - 2.0f*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) + edgeDotBaseToVertex*edgeDotBaseToVertex; // does the swept sphere collide against infinite edge? if (getLowestRoot(a,b,c,t,&newT)) { f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSqaredLength; if (f >=0.0f && f <= 1.0f) { // intersection took place within segment t = newT; foundCollision = true; collisionPoint = triangle.pointB + (edge*f); } } // p3 --- p1 edge = triangle.pointA-triangle.pointC; baseToVertex = triangle.pointC - base; edgeSqaredLength = edge.getLengthSQ(); edgeDotVelocity = edge.dotProduct(velocity); edgeDotBaseToVertex = edge.dotProduct(baseToVertex); // calculate parameters for equation a = edgeSqaredLength* -velocitySqaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) - 2.0f*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) + edgeDotBaseToVertex*edgeDotBaseToVertex; // does the swept sphere collide against infinite edge? if (getLowestRoot(a,b,c,t,&newT)) { f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSqaredLength; if (f >=0.0f && f <= 1.0f) { // intersection took place within segment t = newT; foundCollision = true; collisionPoint = triangle.pointC + (edge*f); } } }// end no collision found // set result: if (foundCollision) { // distance to collision is t f32 distToCollision = t*colData->velocity.getLength(); // does this triangle qualify for closest hit? if (!colData->foundCollision || distToCollision < colData->nearestDistance) { colData->nearestDistance = distToCollision; colData->intersectionPoint = collisionPoint; colData->foundCollision = true; colData->intersectionTriangle = triangle; ++colData->triangleHits; } }// end found collision }
// Assumes: p1,p2 and p3 are given in ellisoid space: void checkTriangle(CollisionPacket* colPackage,VECTOR p1,VECTOR p2,VECTOR p3) { #ifndef __GNUC__ // Make the plane containing this triangle. PLANE trianglePlane(p1,p2,p3); // Is triangle front-facing to the velocity vector? // We only check front-facing triangles if (trianglePlane.isFrontFacingTo(colPackage->normalizedVelocity) || (! isBackCulling)) { // Get interval of plane intersection: double t0, t1; bool embeddedInPlane = false; // Calculate the signed distance from sphere // position to triangle plane double signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage->basePoint); // cache this as we're going to use it a few times below: float normalDotVelocity = trianglePlane.normal.dot(colPackage->velocity); // if sphere is travelling parrallel to the plane: if (normalDotVelocity == 0.0f) { if (fabs(signedDistToTrianglePlane) >= 1.0f) { // Sphere is not embedded in plane. // No collision possible: return; } else { // sphere is embedded in plane. // It intersects in the whole range [0..1] embeddedInPlane = true; t0 = 0.0; t1 = 1.0; } } else { // N dot D is not 0. Calculate intersection interval: t0=(-1.0-signedDistToTrianglePlane)/normalDotVelocity; t1=( 1.0-signedDistToTrianglePlane)/normalDotVelocity; // Swap so t0 < t1 if (t0 > t1) { double temp = t1; t1 = t0; t0 = temp; } // Check that at least one result is within range: if (t0 > 1.0f || t1 < 0.0f) { // Both t values are outside values [0,1] // No collision possible: return; } // Clamp to [0,1] if (t0 < 0.0) t0 = 0.0; if (t1 < 0.0) t1 = 0.0; if (t0 > 1.0) t0 = 1.0; if (t1 > 1.0) t1 = 1.0; } // OK, at this point we have two time values t0 and t1 // between which the swept sphere intersects with the // triangle plane. If any collision is to occur it must // happen within this interval. VECTOR collisionPoint; bool foundCollison = false; float t = 1.0; // First we check for the easy case - collision inside // the triangle. If this happens it must be at time t0 // as this is when the sphere rests on the front side // of the triangle plane. Note, this can only happen if // the sphere is not embedded in the triangle plane. if (!embeddedInPlane) { VECTOR planeIntersectionPoint = (colPackage->basePoint-trianglePlane.normal) + colPackage->velocity*t0; if (checkPointInTriangle(planeIntersectionPoint,p1,p2,p3)) { foundCollison = true; t = t0; collisionPoint = planeIntersectionPoint; } } // if we haven't found a collision already we'll have to // sweep sphere against points and edges of the triangle. // Note: A collision inside the triangle (the check above) // will always happen before a vertex or edge collision! // This is why we can skip the swept test if the above // gives a collision! if (foundCollison == false) { // some commonly used terms: VECTOR velocity = colPackage->velocity; VECTOR base = colPackage->basePoint; float velocitySquaredLength = velocity.squaredLength(); float a,b,c; // Params for equation float newT; // For each vertex or edge a quadratic equation have to // be solved. We parameterize this equation as // a*t^2 + b*t + c = 0 and below we calculate the // parameters a,b and c for each test. // Check against points: a = velocitySquaredLength; // P1 b = 2.0*(velocity.dot(base-p1)); c = (p1-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p1; } // P2 b = 2.0*(velocity.dot(base-p2)); c = (p2-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p2; } // P3 b = 2.0*(velocity.dot(base-p3)); c = (p3-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p3; } // Check agains edges: // p1 -> p2: VECTOR edge = p2-p1; VECTOR baseToVertex = p1 - base; float edgeSquaredLength = edge.squaredLength(); float edgeDotVelocity = edge.dot(velocity); float edgeDotBaseToVertex = edge.dot(baseToVertex); // Calculate parameters for equation a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; // Does the swept sphere collide against infinite edge? if (getLowestRoot(a,b,c, t, &newT)) { // Check if intersection is within line segment: float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { // intersection took place within segment. t = newT; foundCollison = true; collisionPoint = p1 + edge*f; } } // p2 -> p3: edge = p3-p2; baseToVertex = p2 - base; edgeSquaredLength = edge.squaredLength(); edgeDotVelocity = edge.dot(velocity); edgeDotBaseToVertex = edge.dot(baseToVertex); a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; if (getLowestRoot(a,b,c, t, &newT)) { float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { t = newT; foundCollison = true; collisionPoint = p2 + edge*f; } } // p3 -> p1: edge = p1-p3; baseToVertex = p3 - base; edgeSquaredLength = edge.squaredLength(); edgeDotVelocity = edge.dot(velocity); edgeDotBaseToVertex = edge.dot(baseToVertex); a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; if (getLowestRoot(a,b,c, t, &newT)) { float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { t = newT; foundCollison = true; collisionPoint = p3 + edge*f; } } } // Set result: if (foundCollison == true) { // distance to collision: 't' is time of collision float distToCollision = t*colPackage->velocity.length(); // Does this triangle qualify for the closest hit? if (distToCollision < colPackage->nearestDistance) { // Collision information nessesary for sliding colPackage->nearestDistance = distToCollision; colPackage->intersectionPoint=collisionPoint; colPackage->foundCollision = true; // trigger collection of triangle index when leave colPackage->triangleindex = -2; } } } // if not backface #endif }
int checkCollide(glm::vec3 N,glm::vec3 p,glm::vec3 vel, glm::vec3 ta, glm::vec3 tb, glm::vec3 tc, glm::vec3 &result, float &distance){ // Figure out two times, t0 and t1. // By finding the signed distance to the plane at two places. // We want to know if a) it intersects the plane of the triangle // b) if it is within the bounds of the triangle (ta/tb/tc) // c) or if it's on the edges / points of the triangles. // This function returns 1 if it does intersect, or 0 otherwise. // get interval of intersection float t0, t1; bool embedded = false; // calc distance float distToPlane = SignedDistance(N,p,ta); float nDvel = glm::dot(N,vel); if (nDvel < 0.0f){ if (fabs(distToPlane) >= 1.0f){ return 0; } else { embedded = true; t0 = 0.0; t1 = 1.0; } } else{ t0 = (-1.0-distToPlane)/nDvel; t1 = (1.0 - distToPlane)/nDvel; if (t0 > t1){ float temp = t1; t1 = t0; t0 = temp; } if (t0>1.0f || t1< 0.0f){ return 0; } if (t0<0.0) t0 = 0.0; if (t1<0.0) t1 = 0.0; if (t0>1.0) t0 = 1.0; if (t1>1.0) t1 = 1.0; } glm::vec3 colPoint; bool foundCol = false; float t = 1.0; glm::vec3 planeIntersect = (p-N + t0*vel); if (!embedded){ if(checkPointInTriangle(planeIntersect, ta, tb, tc)){ foundCol = true; t = t0; colPoint = planeIntersect; distance = t*glm::length(vel); result = colPoint; return 1; } return 0; } /// VERY IMPORTANT. This is where it checks for intersection in the area of the triangle. else { if(checkPointInTriangle(planeIntersect, ta, tb, tc)){ foundCol = true; t = t0; colPoint = planeIntersect; distance = t*glm::length(vel); result = colPoint; return 1; } } if (foundCol ==false){ glm::vec3 base = p; float velSquared = SquaredLength(vel); float a,b,c; float newT; a = velSquared; // for ta b = 2.0f*glm::dot(vel,base-ta); c = SquaredLength(ta - base) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = ta; distance = t*glm::length(vel); result = colPoint; return 1; } // for tb b = 2.0f*glm::dot(vel,base-tb); c = SquaredLength(tb - base) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = tb; distance = t*glm::length(vel); result = colPoint; return 1; } // for tc b = 2.0f*glm::dot(vel,base-tc); c = SquaredLength((tc - base)) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = tc; distance = t*glm::length(vel); result = colPoint; return 1; } // now edges // ta -> tb glm::vec3 edge = tb-ta; glm::vec3 baseToVertex = ta - base; float edgeSquared = SquaredLength(edge); float edgeDotVel = glm::dot(edge, vel); float edgeDotBaseToVert = glm::dot(edge, baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint = ta + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } // tb -> tc edge = tc-tb; baseToVertex = tb - base; edgeSquared = SquaredLength(edge); edgeDotVel = glm::dot(edge, vel); edgeDotBaseToVert = glm::dot(edge,baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint =tb + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } // tc -> ta edge = ta-tc; baseToVertex = tc - base; edgeSquared = SquaredLength(edge); edgeDotVel = glm::dot(edge, vel); edgeDotBaseToVert = glm::dot(edge,baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint =tc + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } } distance = t*glm::length(vel); return 0; }