• API
• FAQ
• Tools
• Archive
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.

Top