SHARE
TWEET

Untitled

a guest Jul 18th, 2015 169 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. void Physics::_sphereTriangleCollision(CollisionPacket* colPackage, const Vec3 &p1,const Vec3 &p2,const Vec3 &p3)
  3. {
  4.         // Make the plane containing this triangle.
  5.         Plane trianglePlane(p1,p2,p3);
  6.  
  7.         // Is triangle front-facing to the velocity vector?
  8.         // We only check front-facing triangles
  9.         // (your choice of course)
  10.         if (!trianglePlane.isFrontFacingTo(colPackage->velocity)) {
  11.                 std::cout << "not front facing. returning...\n";
  12.                 return;
  13.         }
  14.  
  15.         // Get interval of plane intersection:
  16.         Real t0, t1;
  17.         bool embeddedInPlane = false;
  18.        
  19.         // Calculate the signed distance from sphere
  20.         // position to triangle plane
  21.         Real signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage->basePoint);
  22.  
  23.         // cache this as we’re going to use it a few times below:
  24.         Real normalDotVelocity = glm::dot(trianglePlane.normal(),colPackage->velocity);
  25.  
  26.         // if sphere is travelling parrallel to the plane:
  27.         if (normalDotVelocity == (Real)0.0) {
  28.                 if (std::fabs(signedDistToTrianglePlane) >= (Real)1.0) {
  29.                         // Sphere is not embedded in plane.
  30.                         // No collision possible:
  31.                         return;
  32.                 }
  33.                 else {
  34.                         // sphere is embedded in plane.
  35.                         // It intersects in the whole range [0..1]
  36.                         embeddedInPlane = true;
  37.                         t0 = (Real)0.0;
  38.                         t1 = (Real)1.0;
  39.                 }
  40.         }
  41.         else {
  42.                 // N dot D is not 0. Calculate intersection interval:
  43.                 t0 = ((Real)-1.0 - signedDistToTrianglePlane) / normalDotVelocity;
  44.                 t1 = ((Real)1.0 - signedDistToTrianglePlane) / normalDotVelocity;
  45.  
  46.                 // Swap so t0 < t1
  47.                 if (t0 > t1) {
  48.                         Real temp = t1;
  49.                         t1 = t0;
  50.                         t0 = temp;
  51.                 }
  52.  
  53.                 // Check that at least one result is within range:
  54.                 if (t0 > (Real)1.0 || t1 < (Real)0.0) {
  55.                         // Both t values are outside values [0,1]
  56.                         // No collision possible:
  57.                         return;
  58.                 }
  59.  
  60.                 // Clamp to [0,1]
  61.                 if (t0 < (Real)0.0) t0 = (Real)0.0;
  62.                 if (t1 < (Real)0.0) t1 = (Real)0.0;
  63.                 if (t0 > (Real)1.0) t0 = (Real)1.0;
  64.                 if (t1 > (Real)1.0) t1 = (Real)1.0;
  65.         }
  66.  
  67.         // OK, at this point we have two time values t0 and t1
  68.         // between which the swept sphere intersects with the
  69.         // triangle plane. If any collision is to occur it must
  70.         // happen within this interval.
  71.         Vec3 collisionPoint;
  72.         bool foundCollison = false;
  73.         Real t = (Real)1.0;
  74.  
  75.         // First we check for the easy case - collision inside
  76.         // the triangle. If this happens it must be at time t0
  77.         // as this is when the sphere rests on the front side
  78.         // of the triangle plane. Note, this can only happen if
  79.         // the sphere is not embedded in the triangle plane.
  80.        
  81.         if (!embeddedInPlane) {
  82.                 Vec3 planeIntersectionPoint = (colPackage->basePoint - trianglePlane.normal()) + t0 * colPackage->velocity;
  83.                 if( checkPointInTriangle2((glm::vec3)planeIntersectionPoint, (glm::vec3)p1, (glm::vec3)p2, (glm::vec3)p3) ) //can use checkPointInTriangle2 as well.
  84.                 {
  85.                         foundCollison = true;
  86.                         t = t0;
  87.                         collisionPoint = planeIntersectionPoint;
  88.                         //std::cout << "collision inside the triangle!\n";
  89.                 }
  90.         }
  91.  
  92.         // if we haven’t found a collision already we’ll have to
  93.         // sweep sphere against points and edges of the triangle.
  94.         // Note: A collision inside the triangle (the check above)
  95.         // will always happen before a vertex or edge collision!
  96.         // This is why we can skip the swept test if the above
  97.         // gives a collision!
  98.         if (!foundCollison) {
  99.                 // some commonly used terms:
  100.                 Vec3 velocity = colPackage->velocity;
  101.                 Vec3 base = colPackage->basePoint;
  102.                 Real velocitySquaredLength = glm::squaredLength(velocity);
  103.                 Real a,b,c; // Params for equation
  104.                 Real newT;
  105.  
  106.                 // For each vertex or edge a quadratic equation have to
  107.                 // be solved. We parameterize this equation as
  108.                 // a*t^2 + b*t + c = 0 and below we calculate the
  109.                 // parameters a,b and c for each test.
  110.                 // Check against points:
  111.                 a = velocitySquaredLength;
  112.                
  113.                 // P1
  114.                 b = (Real)2.0*(glm::dot(velocity,base-p1));
  115.                 c = glm::squaredLength(p1-base) - (Real)1.0;
  116.                 if (getLowestRoot(a,b,c, t, &newT)) {
  117.                         t = newT;
  118.                         foundCollison = true;
  119.                         collisionPoint = p1;
  120.                 }
  121.  
  122.                 // P2
  123.                 b = (Real)2.0*(glm::dot(velocity,base-p2));
  124.                 c = glm::squaredLength(p2-base) - (Real)1.0;
  125.                 if (getLowestRoot(a,b,c, t, &newT)) {
  126.                         t = newT;
  127.                         foundCollison = true;
  128.                         collisionPoint = p2;
  129.                 }
  130.  
  131.                 // P3
  132.                 b = (Real)2.0*(glm::dot(velocity,base-p3));
  133.                 c = glm::squaredLength(p3-base) - (Real)1.0;
  134.                 if (getLowestRoot(a,b,c, t, &newT)) {
  135.                         t = newT;
  136.                         foundCollison = true;
  137.                         collisionPoint = p3;
  138.                 }
  139.  
  140.                 // Check agains edges:
  141.                 // p1 -> p2:
  142.                 Vec3 edge = p2-p1;
  143.                 Vec3 baseToVertex = p1 - base;
  144.                 Real edgeSquaredLength = glm::squaredLength(edge);
  145.                 Real edgeDotVelocity = glm::dot(edge,velocity);
  146.                 Real edgeDotBaseToVertex = glm::dot(edge,baseToVertex);
  147.  
  148.                 // Calculate parameters for equation
  149.                 a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
  150.                 b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0*edgeDotVelocity*edgeDotBaseToVertex;
  151.                 c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex*edgeDotBaseToVertex;
  152.  
  153.                 // Does the swept sphere collide against infinite edge?
  154.                 if (getLowestRoot(a,b,c, t, &newT)) {
  155.                         // Check if intersection is within line segment:
  156.                         Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
  157.                         if (f >= (Real)0.0 && f <= (Real)1.0) {
  158.                                 // intersection took place within segment.
  159.                                 t = newT;
  160.                                 foundCollison = true;
  161.                                 collisionPoint = p1 + f*edge;
  162.                         }
  163.                 }
  164.  
  165.                 // p2 -> p3:
  166.                 edge = p3-p2;
  167.                 baseToVertex = p2 - base;
  168.                 edgeSquaredLength = glm::squaredLength(edge);
  169.                 edgeDotVelocity = glm::dot(edge,velocity);
  170.                 edgeDotBaseToVertex = glm::dot(edge,baseToVertex);
  171.                 a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
  172.                 b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0 * edgeDotVelocity * edgeDotBaseToVertex;
  173.                 c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex * edgeDotBaseToVertex;
  174.                 if (getLowestRoot(a,b,c, t, &newT)) {
  175.                         Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
  176.                         if (f >= (Real)0.0 && f <= (Real)1.0) {
  177.                                 t = newT;
  178.                                 foundCollison = true;
  179.                                 collisionPoint = p2 + f*edge;
  180.                         }
  181.                 }
  182.  
  183.                 // p3 -> p1:
  184.                 edge = p1-p3;
  185.                 baseToVertex = p3 - base;
  186.                 edgeSquaredLength = glm::squaredLength(edge);
  187.                 edgeDotVelocity = glm::dot(edge,velocity);
  188.                
  189.                 edgeDotBaseToVertex = glm::dot(edge,baseToVertex);
  190.                 a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
  191.                 b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0 * edgeDotVelocity * edgeDotBaseToVertex;
  192.                 c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex * edgeDotBaseToVertex;
  193.                 if (getLowestRoot(a,b,c, t, &newT)) {
  194.                         Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
  195.                         if (f >= (Real)0.0 && f <= (Real)1.0) {
  196.                                 t = newT;
  197.                                 foundCollison = true;
  198.                                 collisionPoint = p3 + f*edge;
  199.                         }
  200.                 }
  201.         }
  202.  
  203.         // Set result:
  204.         if (foundCollison == true) {
  205.                 // distance to collision: ’t’ is time of collision
  206.                 Real distToCollision = t * glm::length(colPackage->velocity);
  207.                
  208.                 // Does this triangle qualify for the closest hit?
  209.                 // it does if it’s the first hit or the closest
  210.                 if (colPackage->foundCollision == false || distToCollision < colPackage->nearestDistance) {
  211.                         // Collision information nessesary for sliding
  212.                         colPackage->nearestDistance = distToCollision;
  213.                         colPackage->intersectionPoint = collisionPoint;
  214.                         colPackage->foundCollision = true;
  215.                 }
  216.         }
  217. }
  218.  
  219. void solveCollision(CollisionPacket *colData)
  220. {
  221.         //slide plane from point and normal
  222.         Plane slidePlane(colData->intersectionPoint, colData->basePoint - colData->intersectionPoint);
  223.  
  224.         //destination point as if there was no collision response
  225.         Vec3 destinationPoint = colData->basePoint + colData->velocity;
  226.        
  227.         //new destination point
  228.         Vec3 newDestinationPoint = destinationPoint - slidePlane.signedDistanceTo(destinationPoint) * slidePlane.normal();
  229.        
  230.         Vec3 intersectionToNewDestination = newDestinationPoint - colData->intersectionPoint;
  231.         colData->velocity = colData->velocity * colData->nearestDistance + intersectionToNewDestination;
  232. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top