1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487 |
|
public class CollisionWorld
{
public List<BoundingSphere> Sphere = new List<BoundingSphere>();
public Vector3 pos;
public bool found;
Vector3 gravity = Vector3.Up;
public void UpdateCollisionWorld()
{
foreach (BoundingSphere e in Sphere)
{
if (Sphere.Count != 0)
{
CollisionPacket collisionPacket = new CollisionPacket();
collisionPacket.eRadius = e.Radius;
collisionPacket.R3Position = e.Center;
collisionPacket.R3Velocity = new Vector3(.115f,0f,.115f);
CollideAndSlide(ref collisionPacket, gravity);
pos = collisionPacket.R3Position;
if (collisionPacket.foundCollision == true)
{
found = true;
}
}
}
}
public struct CollisionPacket
{
//public:
public float eRadius; // ellipsoid radius
// Information about the move being requested: (in R3)
public Vector3 R3Velocity;
public Vector3 R3Position;
// Information about the move being requested: (in eSpace)
public Vector3 velocity;
public Vector3 normalizedVelocity;
public Vector3 basePoint;
// Hit information
public bool foundCollision;
public float nearestDistance;
public Vector3 intersectionPoint;
}
public void CollideAndSlide(ref CollisionPacket collisionPacket, Vector3 gravity)
{
Vector3 eSpacePosition = collisionPacket.R3Position / collisionPacket.eRadius;
Vector3 eSpaceVelocity = collisionPacket.R3Velocity / collisionPacket.eRadius;
collisionRecursionDepth = 0;
Vector3 finalPosition = CollideWithWorld(eSpacePosition, eSpaceVelocity, ref collisionPacket);
collisionPacket.R3Position = finalPosition * collisionPacket.eRadius;
collisionPacket.R3Velocity = gravity;
eSpaceVelocity = gravity / collisionPacket.eRadius;
collisionRecursionDepth = 0;
finalPosition = CollideWithWorld(finalPosition, eSpaceVelocity, ref collisionPacket);
finalPosition = finalPosition * collisionPacket.eRadius;
}
// Set this to match application scale..
const float unitsPerMeter = 100.0f;
short collisionRecursionDepth;
public Vector3 CollideWithWorld(Vector3 pos, Vector3 vel, ref CollisionPacket collisionPackage)
{
float unitScale = unitsPerMeter / 100.0f;
float veryCloseDistance = 0.005f * unitScale;
// do we need to worry?
if (collisionRecursionDepth > 5)
return pos;
// Ok, we need to worry:
collisionPackage.velocity = vel;
collisionPackage.normalizedVelocity = vel;
collisionPackage.normalizedVelocity.Normalize();
collisionPackage.basePoint = pos;
collisionPackage.foundCollision = false;
// Check for collision (calls the collision routines)
// Application specific!!
for (int i = 0; i < Triangles.Count; i++)
{
Vector3 p1, p2, p3;
p1 = Triangles[i].Vert1 / collisionPackage.eRadius;
p2 = Triangles[i].Vert2 / collisionPackage.eRadius;
p3 = Triangles[i].Vert3 / collisionPackage.eRadius;
CheckTriangle(ref collisionPackage, p1, p2, p3);
}
// If no collision we just move along the velocity
if (collisionPackage.foundCollision == false)
{
return pos + vel;
}
// *** Collision occured ***
// The original destination point
Vector3 destinationPoint = pos + vel;
Vector3 newBasePoint = pos;
// only update if we are not already very close
// and if so we only move very close to intersection..not
// to the exact spot.
if (collisionPackage.nearestDistance >= veryCloseDistance)
{
Vector3 V = SetLength(vel, collisionPackage.nearestDistance - veryCloseDistance);
newBasePoint = collisionPackage.basePoint + V;
// Adjust polygon intersection point (so sliding
// plane will be unaffected by the fact that we
// move slightly less than collision tells us)
V.Normalize();
collisionPackage.intersectionPoint -= veryCloseDistance * V;
}
// Determine the sliding plane
Vector3 slidePlaneOrigin = collisionPackage.intersectionPoint;
Vector3 slidePlaneNormal = newBasePoint - collisionPackage.intersectionPoint;
slidePlaneNormal.Normalize();
Plane slidingPlane = BuildPlane(slidePlaneOrigin, slidePlaneNormal);
// Again, sorry about formatting.. but look carefully ;)
Vector3 newDestinationPoint = destinationPoint - SignedDistanceTo(slidingPlane, destinationPoint) * slidePlaneNormal;
// Generate the slide vector, which will become our new
// velocity vector for the next iteration
Vector3 newVelocityVector = newDestinationPoint - collisionPackage.intersectionPoint;
// Recurse:
// dont recurse if the new velocity is very small
if (newVelocityVector.Length() < veryCloseDistance)
{
return newBasePoint;
}
collisionRecursionDepth++;
return CollideWithWorld(newBasePoint, newVelocityVector, ref collisionPackage);
}
// Assumes: p1,p2 and p3 are given in ellisoid space:
public void CheckTriangle(ref CollisionPacket colPackage, Vector3 p1, Vector3 p2, Vector3 p3)
{
// Make the plane containing this triangle.
Plane trianglePlane = new Plane(p1, p2, p3);
// Is triangle front-facing to the velocity vector?
// We only check front-facing triangles
// (your choice of course)
// Get interval of plane intersection:
float t0, t1;
bool embeddedInPlane = false;
// Calculate the signed distance from sphere
// position to triangle plane
float signedDistToTrianglePlane = SignedDistanceTo(trianglePlane, colPackage.basePoint);
// cache this as we?re going to use it a few times below:
float normalDotVelocity = Vector3.Dot(trianglePlane.Normal, colPackage.velocity);
// if sphere is travelling parrallel to the plane:
if (normalDotVelocity == 0.0f)
{
if (Math.Abs(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.0f;
t1 = 1.0f;
}
}
else
{
// N dot D is not 0. Calculate intersection interval:
t0 = (-1.0f - signedDistToTrianglePlane) / normalDotVelocity;
t1 = (1.0f - signedDistToTrianglePlane) / normalDotVelocity;
// Swap so t0 < t1
if (t0 > t1)
{
float 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.0f;
if (t1 < 0.0) t1 = 0.0f;
if (t0 > 1.0) t0 = 1.0f;
if (t1 > 1.0) t1 = 1.0f;
}
// 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.
Vector3 collisionPoint = Vector3.Zero;
bool foundCollison = false;
float t = 1.0f;
// 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)
{
Vector3 planeIntersectionPoint = (colPackage.basePoint - trianglePlane.Normal) + t0 * colPackage.velocity;
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:
Vector3 velocity = colPackage.velocity;
Vector3 basePoint = colPackage.basePoint;
float velocitySquaredLength = velocity.LengthSquared();
float a, b, c; // Params for equation
float newT = 0;
// 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.0f * Vector3.Dot(velocity, basePoint - p1);
c = Vector3.Subtract(p1, basePoint).LengthSquared() - 1.0f;
if (GetLowestRoot(a, b, c, t, ref newT))
{
t = newT;
foundCollison = true;
collisionPoint = p1;
}
// P2
b = 2.0f * Vector3.Dot(velocity, basePoint - p2);
c = Vector3.Subtract(p2, basePoint).LengthSquared() - 1.0f;
if (GetLowestRoot(a, b, c, t, ref newT))
{
t = newT;
foundCollison = true;
collisionPoint = p2;
}
// P3
b = 2.0f * Vector3.Dot(velocity, basePoint - p3);
c = Vector3.Subtract(p3, basePoint).LengthSquared() - 1.0f;
if (GetLowestRoot(a, b, c, t, ref newT))
{
t = newT;
foundCollison = true;
collisionPoint = p3;
}
// Check agains edges:
// p1 . p2:
Vector3 edge = p2 - p1;
Vector3 baseToVertex = p1 - basePoint;
float edgeSquaredLength = edge.LengthSquared();
float edgeDotVelocity = Vector3.Dot(edge, velocity);
float edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
// Calculate parameters for equation
a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
// Does the swept sphere collide against infinite edge?
if (GetLowestRoot(a, b, c, t, ref 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 + f * edge;
}
}
// p2 . p3:
edge = p3 - p2;
baseToVertex = p2 - basePoint;
edgeSquaredLength = edge.LengthSquared();
edgeDotVelocity = Vector3.Dot(edge, velocity);
edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
if (GetLowestRoot(a, b, c, t, ref newT))
{
float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
if (f >= 0.0 && f <= 1.0)
{
t = newT;
foundCollison = true;
collisionPoint = p2 + f * edge;
}
}
// p3 . p1:
edge = p1 - p3;
baseToVertex = p3 - basePoint;
edgeSquaredLength = edge.LengthSquared();
edgeDotVelocity = Vector3.Dot(edge, velocity);
edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
a = edgeSquaredLength * -velocitySquaredLength +
edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
if (GetLowestRoot(a, b, c, t, ref newT))
{
float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
if (f >= 0.0 && f <= 1.0)
{
t = newT;
foundCollison = true;
collisionPoint = p3 + f * edge;
}
}
}
// 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?
// it does if it?s the first hit or the closest
if (colPackage.foundCollision == false ||
distToCollision < colPackage.nearestDistance)
{
// Collision information nessesary for sliding
colPackage.nearestDistance = distToCollision;
colPackage.intersectionPoint = collisionPoint;
colPackage.foundCollision = true;
}
}
}
#region Utility Functions...
private Vector3 SetLength(Vector3 v, float l)
{
float len = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
v.X *= l / len;
v.Y *= l / len;
v.Z *= l / len;
return v;
}
private Plane BuildPlane(Vector3 position, Vector3 normal)
{
float d = -Vector3.Dot(position, normal);
return new Plane(normal, d);
}
/// <summary>
/// Checks if the current point is inside the triangle.
/// </summary>
/// <param name="point">Point to check</param>
/// <returns>True if the point is inside the triangle</returns>
private bool CheckPointInTriangle(Vector3 point, Vector3 pa, Vector3 pb, Vector3 pc)
{
Vector3 e10 = pb - pa;
Vector3 e20 = pc - pa;
float a = Vector3.Dot(e10, e10);
float b = Vector3.Dot(e10, e20);
float c = Vector3.Dot(e20, e20);
float ac_bb = (a * c) - (b * b);
Vector3 vp = new Vector3(point.X - pa.X, point.Y - pa.Y, point.Z - pa.Z);
float d = Vector3.Dot(vp, e10);
float e = Vector3.Dot(vp, e20);
float x = (d * c) - (e * b);
float y = (e * a) - (d * b);
float z = x + y - ac_bb;
uint inTriangle = (((uint)z & ~((uint)x | (uint)y)) & 0x80000000);
return (inTriangle != 0);
}
private bool IsFrontFacingTo(Plane plane, Vector3 direction)
{
double dot = Vector3.Dot(plane.Normal, direction);
return (dot <= 0);
}
private float SignedDistanceTo(Plane plane, Vector3 point)
{
return Vector3.Dot(point, plane.Normal) + plane.D;
}
private bool GetLowestRoot(float a, float b, float c, float maxR, ref float root)
{
// Check if a solution exists
float determinant = b * b - 4.0f * a * c;
// If determinant is negative it means no solutions.
if (determinant < 0.0f) return false;
// calculate the two roots: (if determinant == 0 then
// x1==x2 but let?s disregard that slight optimization)
float sqrtD = (float)Math.Sqrt(determinant);
float r1 = (-b - sqrtD) / (2 * a);
float r2 = (-b + sqrtD) / (2 * a);
// Sort so x1 <= x2
if (r1 > r2)
{
float temp = r2;
r2 = r1;
r1 = temp;
}
// Get lowest root:
if (r1 > 0 && r1 < maxR)
{
root = r1;
return true;
}
// It is possible that we want x2 - this can happen
// if x1 < 0
if (r2 > 0 && r2 < maxR)
{
root = r2;
return true;
}
// No (valid) solutions
return false;
}
#endregion
public List<Triangle> Triangles = new List<Triangle>();
}
public struct Triangle
{
public Vector3 Vert1;
public Vector3 Vert2;
public Vector3 Vert3;
public Triangle(Vector3 v1, Vector3 v2, Vector3 v3)
{
Vert1 = v1;
Vert2 = v2;
Vert3 = v3;
}
}
|