Tanoda
Collision.cs
Go to the documentation of this file.
1/******************************************************************************
2 * Copyright (C) Ultraleap, Inc. 2011-2020. *
3 * *
4 * Use subject to the terms of the Apache License 2.0 available at *
5 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
6 * between Ultraleap and you, your company or other organization. *
7 ******************************************************************************/
8
9using Leap.Unity.Infix;
10using Leap.Unity.Swizzle;
11using System;
12using UnityEngine;
13
14namespace Leap.Unity.Geometry {
15
16 public static class Collision {
17
18 #region Short-hand
19
20 private static float Dot(Vector3 a, Vector3 b) {
21 return Vector3.Dot(a, b);
22 }
23
24 private static float Dot(Vector2 a, Vector2 b) {
25 return Vector2.Dot(a, b);
26 }
27
28 #endregion
29
30 #region Box-Sphere
31
32 public static bool DoesOverlap(Box box, Sphere sphere) {
33 return DoesOverlap(sphere, box);
34 }
35 public static bool DoesOverlap(Sphere sphere, Box box) {
36 var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
37 var sphereCenter_box = box.matrix.inverse
38 .MultiplyPoint3x4(sphereCenter_world).Abs();
39
40 float x = sphereCenter_box.x,
41 y = sphereCenter_box.y,
42 z = sphereCenter_box.z;
43
44 float bx = box.radii.x.Abs(),
45 by = box.radii.y.Abs(),
46 bz = box.radii.z.Abs();
47
48 float dx = x - Mathf.Min(bx, x),
49 dy = y - Mathf.Min(by, y),
50 dz = z - Mathf.Min(bz, z);
51
52 if (dx == 0 && dy == 0 && dz == 0) return true;
53
54 var boxSurfaceToSphereCenter_box = new Vector3(dx, dy, dz);
55 var boxSurfaceToSphereCenter_world
56 = box.matrix.MultiplyVector(boxSurfaceToSphereCenter_box);
57 var boxSurfaceToSphereCenter_sphere
58 = sphere.matrix.inverse.MultiplyVector(boxSurfaceToSphereCenter_world);
59
60 return boxSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
61 }
62
63 #endregion
64
65 #region Rect-Sphere
66
71 public static float DistanceTo(Rect rect, Sphere sphere) {
72 return DistanceBetween(sphere, rect);
73 }
78 public static float DistanceBetween(Sphere sphere, Rect rect) {
79 var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
80 var sphereCenter_rect = rect.matrix.inverse
81 .MultiplyPoint3x4(sphereCenter_world).Abs();
82
83 float x = sphereCenter_rect.x,
84 y = sphereCenter_rect.y,
85 z = sphereCenter_rect.z;
86
87 float rx = rect.radii.x.Abs(),
88 ry = rect.radii.y.Abs(),
89 rz = 0f;
90
91 float dx = x - Mathf.Min(rx, x),
92 dy = y - Mathf.Min(ry, y),
93 dz = z - Mathf.Min(rz, z);
94
95 if (dx == 0 && dy == 0 && dz == 0) return 0f;
96
97 var rectSurfaceToSphereCenter_rect = new Vector3(dx, dy, dz);
98 var rectSurfaceToSphereCenter_world
99 = rect.matrix.MultiplyVector(rectSurfaceToSphereCenter_rect);
100
101 return Mathf.Max(0f, rectSurfaceToSphereCenter_world.magnitude - sphere.radius);
102 }
103
104 public static bool DoesOverlap(Rect rect, Sphere sphere) {
105 return DoesOverlap(sphere, rect);
106 }
107 public static bool DoesOverlap(Sphere sphere, Rect rect) {
108 var sphereCenter_world = sphere.matrix.MultiplyPoint3x4(Vector3.zero);
109 var sphereCenter_rect = rect.matrix.inverse
110 .MultiplyPoint3x4(sphereCenter_world).Abs();
111
112 float x = sphereCenter_rect.x,
113 y = sphereCenter_rect.y,
114 z = sphereCenter_rect.z;
115
116 float rx = rect.radii.x.Abs(),
117 ry = rect.radii.y.Abs(),
118 rz = 0f;
119
120 float dx = x - Mathf.Min(rx, x),
121 dy = y - Mathf.Min(ry, y),
122 dz = z - Mathf.Min(rz, z);
123
124 if (dx == 0 && dy == 0 && dz == 0) return true;
125
126 var rectSurfaceToSphereCenter_rect = new Vector3(dx, dy, dz);
127 var rectSurfaceToSphereCenter_world
128 = rect.matrix.MultiplyVector(rectSurfaceToSphereCenter_rect);
129 var rectSurfaceToSphereCenter_sphere
130 = sphere.matrix.inverse.MultiplyVector(rectSurfaceToSphereCenter_world);
131
132 return rectSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
133 }
134
135 #endregion
136
137 #region Rect-Segment3
138
139 //[ThreadStatic]
140 //private static LocalSegment3[] _rectEdgesBuffer = new LocalSegment3[4];
141
142 public static float Intersect(LocalSegment3 segment, Rect rect) {
143 return Intersect(rect, segment);
144 }
145 public static float Intersect(Rect rect, LocalSegment3 segment) {
146 Vector3 closestPointOnSegment_unused, closestPointOnRect_unused;
147 return Intersect(rect, segment,
148 out closestPointOnRect_unused, out closestPointOnSegment_unused);
149 }
150 public static float Intersect(LocalSegment3 segment, Rect rect,
151 out Vector3 closestPointOnSegment,
152 out Vector3 closestPointOnRect) {
153 return Intersect(rect, segment, out closestPointOnRect, out closestPointOnSegment);
154 }
155 public static float Intersect(Rect rect, LocalSegment3 segment,
156 out Vector3 closestPointOnRect,
157 out Vector3 closestPointOnSegment) {
158 float closestSqrDist = float.PositiveInfinity;
159 closestPointOnRect = closestPointOnSegment = default(Vector3);
160
161 Vector3 segmentA_rect;
162 bool aProjectsInside = rect.ContainsProjectedPoint(segment.a, out segmentA_rect);
163 Vector3 segmentB_rect;
164 bool bProjectsInside = rect.ContainsProjectedPoint(segment.b, out segmentB_rect);
165
166 // Test if the segment intersects with the rect plane and is inside the rect.
167 var plane_rect = rect.ToLocalPlane();
168 var segment_rect = new LocalSegment3(segmentA_rect, segmentB_rect);
169 Vector3 pointOnPlane_rect; float amountAlongSegment; bool isLineCoplanar;
170 var intersectsPlane = Intersect(plane_rect, segment_rect,
171 out pointOnPlane_rect, out amountAlongSegment,
172 out isLineCoplanar);
173 if (intersectsPlane) {
174 var absPointOnPlane_rect = pointOnPlane_rect.Abs();
175 var absRadii = rect.radii.Abs();
176 if (absPointOnPlane_rect.x <= absRadii.x && absPointOnPlane_rect.y <= absRadii.y) {
177 closestPointOnRect = rect.matrix.MultiplyPoint3x4(pointOnPlane_rect);
178 closestPointOnSegment = segment.Evaluate(amountAlongSegment);
179 return 0f;
180 }
181 }
182
183 // Must test the rect segments and plane-projection distances to find the closest
184 // pair of points and return their squared-distance.
185
186 // Segment point A <> plane, when A projects inside rect.
187 if (aProjectsInside) {
188 var testPointRect = rect.matrix.MultiplyPoint3x4(segmentA_rect.WithZ(0f));
189 var testSqrDist = (testPointRect - segment.a).sqrMagnitude;
190 if (testSqrDist < closestSqrDist) {
191 closestSqrDist = testSqrDist;
192 closestPointOnRect = testPointRect;
193 closestPointOnSegment = segment.a;
194 }
195 }
196
197 // Segment point B <> plane, when B projects inside rect.
198 if (bProjectsInside) {
199 var testPointRect = rect.matrix.MultiplyPoint3x4(segmentB_rect.WithZ(0f));
200 var testSqrDist = (testPointRect - segment.b).sqrMagnitude;
201 if (testSqrDist < closestSqrDist) {
202 closestSqrDist = testSqrDist;
203 closestPointOnRect = testPointRect;
204 closestPointOnSegment = segment.b;
205 }
206 }
207
208 if (!aProjectsInside || !bProjectsInside) {
209 // Closest point segment <> {AB, BC, CD, DA}.
210 foreach (var rectSegment in rect.segments) {
211 Vector3 testClosestPointRectSegment;
212 Vector3 testClosestPointSegment;
213 float unusedT1, unusedT2;
214 var testSqrDist = Intersect(rectSegment, segment,
215 out unusedT1, out unusedT2,
216 out testClosestPointRectSegment,
217 out testClosestPointSegment);
218
219 // Debug
220 //RuntimeGizmos.RuntimeGizmoDrawer drawer;
221 //if (RuntimeGizmos.RuntimeGizmoManager.TryGetGizmoDrawer(out drawer)) {
222 // drawer.DrawLine(testClosestPointRectSegment, testClosestPointSegment);
223 //}
224
225 if (testSqrDist < closestSqrDist) {
226 closestSqrDist = testSqrDist;
227 closestPointOnRect = testClosestPointRectSegment;
228 closestPointOnSegment = testClosestPointSegment;
229 }
230 }
231 }
232
233 return closestSqrDist;
234 }
235
236 #endregion
237
238 #region Plane-Segment3
239
240 // https://stackoverflow.com/a/18543221 for line-plane intersection.
241
249 public static bool Intersect(LocalPlane plane, LocalSegment3 line) {
250 Vector3 pointOnPlane_unused;
251 float amountAlongSegment_unused;
252 bool isLineCoplanar_unused;
253 return Intersect(plane, line,
254 out pointOnPlane_unused,
255 out amountAlongSegment_unused,
256 out isLineCoplanar_unused);
257 }
258
259
285 public static bool Intersect(LocalPlane plane, LocalSegment3 line,
286 out Vector3 pointOnPlane,
287 out float amountAlongSegment,
288 out bool isLineCoplanar,
289 bool intersectInfiniteLine = false) {
290 var planePosition = plane.position;
291 var planeNormal = plane.normal;
292
293 var a = line.a;
294 var b = line.b;
295
296 var u = b - a;
297 var uDotN = planeNormal.Dot(u);
298 if (uDotN.Abs() > float.Epsilon) {
299 isLineCoplanar = false;
300
301 var w = a - planePosition;
302 amountAlongSegment = -(planeNormal.Dot(w)) / uDotN;
303 pointOnPlane = a + u * amountAlongSegment;
304
305 return intersectInfiniteLine
306 || (amountAlongSegment >= 0f && amountAlongSegment <= 1f);
307 }
308 else {
309 var pa = a - planePosition;
310 if (planeNormal.Dot(pa).Abs() < float.Epsilon) {
311 isLineCoplanar = true;
312 }
313 else {
314 isLineCoplanar = false;
315 }
316
317 if (isLineCoplanar) {
318 pointOnPlane = (a + b) / 2f;
319 amountAlongSegment = 0.5f;
320 return true;
321 }
322 else {
323 pointOnPlane = new Vector3(float.NaN, float.NaN, float.NaN);
324 amountAlongSegment = float.NaN;
325 return false;
326 }
327 }
328 }
329
330 #endregion
331
332 #region Segment-Segment
333
338 private static float Signed2DTriArea(Vector2 a, Vector2 b, Vector2 c) {
339 return (a.x - c.x) * (b.y - c.y)
340 - (a.y - c.y) * (b.x - c.x);
341 }
342
348 public static bool Intersect(LocalSegment2 seg1, LocalSegment2 seg2,
349 out float t, out Vector2 p) {
350 Vector2 a = seg1.a, b = seg1.b, c = seg2.a, d = seg2.b;
351
352 // Sign of areas correspond to which side of ab the points c and d are.
353 float a1 = Signed2DTriArea(a, b, d); // Compute winding of abd (+ or -).
354 float a2 = Signed2DTriArea(a, b, c); // To intersect, must have opposite sign.
355
356 // If c and d are on different sides of ab, areas have different signs.
357 // Also verify that a1 and a2 are non-zero (areas are zero for degenerate triangles).
358 if (a1 * a2 < 0f && a1 != 0f && a2 != 0f) {
359 // Compute signs for a and b with respect to cd.
360 float a3 = Signed2DTriArea(c, d, a); // Winding of cda (+ or -).
361 // Since area is constant, a1 - a2 = a3 - a4, or a4 = a3 + a2 - a1
362 // float a4 = Signed2DTriArea(c, d, b);
363 float a4 = a3 + a2 - a1;
364 // The points a and b are on different sides of cd if areas have different signs.
365 if (a3 * a4 < 0f) {
366 // The segments intersect. Find intersection point along L(t) = a + t * (b - a).
367 // Given the height h1 of a over cd and height h2 of b over cd,
368 // t = h1 / (h1 - h2) = (b*h1/2) / (b*h1/2 - b*h2/2) = a3 / (a3 - a4),
369 // where b (the base of the triangles cda and cdb, i.e., the length of cd)
370 // cancels out.
371 t = a3 / (a3 - a4);
372 p = a + t * (b - a);
373 return true;
374 }
375 }
376
377 // Segments don't intersect, or are collinear.
378 t = default(float); p = default(Vector2);
379 return false;
380 }
381
387 public static float Intersect(LocalSegment3 seg1, LocalSegment3 seg2,
388 out float t1, out float t2,
389 out Vector3 c1, out Vector3 c2) {
390 Vector3 d1 = seg1.b - seg1.a; // Direction vector of seg1.
391 Vector3 d2 = seg2.b - seg2.a; // Direction vector of seg2.
392 Vector3 r = seg1.a - seg2.a;
393 float a = Dot(d1, d1); // Squared length of seg1.
394 float e = Dot(d2, d2); // Squared length of seg2.
395 float f = Dot(d2, r);
396
397 // Check if either or both segments degenerate into points.
398 if (a <= float.Epsilon && e <= float.Epsilon) {
399 // Both segments degenerate into points.
400 t1 = t2 = 0f;
401 c1 = seg1.a; c2 = seg2.a;
402 return Dot(c1 - c2, c1 - c2);
403 }
404 if (a <= float.Epsilon) {
405 // First segment degenerates into a point.
406 t1 = 0f;
407 t2 = (f / e).Clamped01();
408 }
409 else {
410 float c = Dot(d1, r);
411 if (e <= float.Epsilon) {
412 // Second segment degenerates into a point.
413 t2 = 0f;
414 t1 = (-c / a).Clamped01();
415 }
416 else {
417 // General, non-degenerate case.
418 float b = Dot(d1, d2);
419 float denom = a*e - b*b; // Always non-negative.
420
421 // If the segments are not parallel, compute the closest point on seg1 to seg2
422 // and clamp to seg1. Otherwise, pick an arbitrary t1 (here 0).
423 if (denom != 0f) {
424 t1 = ((b*f - c*e) / denom).Clamped01();
425 }
426 else {
427 t1 = 0f;
428 }
429 // Compute the point on seg2 closest to t1.
430 float t2nom = b*t1 + f; // avoid dividing e until later.
431
432 // If t2 in [0, 1], we're done. Otherwise, recompute t1 for the new value of
433 // t2 and clamp to [0, 1].
434 if (t2nom < 0f) {
435 t2 = 0f;
436 t1 = (-c / a).Clamped01();
437 }
438 else if (t2nom > e) {
439 t2 = 1f;
440 t1 = ((b - c) / a).Clamped01();
441 }
442 else {
443 t2 = t2nom / e;
444 }
445 }
446 }
447
448 c1 = seg1.a + d1 * t1;
449 c2 = seg2.a + d2 * t2;
450 return (c1 - c2).sqrMagnitude;
451 }
452
453 #region ARCHIVE
454
455 // 2D intersection:
456 // https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
457 // https://ideone.com/PnPJgb
458
463 public static bool Intersect2D(LocalSegment3 segment0, LocalSegment3 segment1,
464 out Vector2 intersectionPoint,
465 out float amountAlongSegment0,
466 out float amountAlongSegment1) {
467
468 Vector2 p = segment0.a, q = segment0.b, r = segment1.a, s = segment1.b;
469
470 // t = (q − p) × s / (r × s)
471 // u = (q − p) × r / (r × s)
472
473 var denom = Fake2DCross(r, s);
474
475 if (denom == 0) {
476 // Lines are collinear or parallel.
477 amountAlongSegment0 = float.NaN;
478 amountAlongSegment1 = float.NaN;
479 intersectionPoint = new Vector2(float.NaN, float.NaN);
480 return false;
481 }
482
483 var tNumer = Fake2DCross(q - p, s);
484 var uNumer = Fake2DCross(q - p, r);
485
486 amountAlongSegment0 = tNumer / denom;
487 amountAlongSegment1 = uNumer / denom;
488
489 if ( amountAlongSegment0 < 0 || amountAlongSegment0 > 1
490 || amountAlongSegment1 < 0 || amountAlongSegment1 > 1) {
491 // Line segments do not intersect within their ranges.
492 intersectionPoint = default(Vector2);
493 return false;
494 }
495
496 intersectionPoint = p + r * amountAlongSegment0;
497 return true;
498 }
499
500 private static float Fake2DCross(Vector2 a, Vector2 b) {
501 return a.x * b.y - a.y * b.x;
502 }
503
504 #endregion
505
506 #endregion
507
508 #region Point-Segment
509
513 public static float SqrDistPointSegment(LocalSegment3 segment, Vector3 p) {
514 return SqrDistPointSegment(segment.a, segment.b, p);
515 }
519 public static float SqrDistPointSegment(Vector3 a, Vector3 b, Vector3 c) {
520 Vector3 ab = b - a, ac = c - a, bc = c - b;
521 float e = Dot(ac, ab);
522 // Handle cases where C projects outside AB.
523 if (e <= 0f) return Dot(ac, ac);
524 float f = Dot(ab, ab);
525 if (e >= f) return Dot(bc, bc);
526 // Handle cases where C projects onto AB.
527 return Dot(ac, ac) - e * e / f;
528 }
529
533 public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p) {
534 float unusedT;
535 return ClosestPtPointSegment(segment.a, segment.b, p, out unusedT);
536 }
542 public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p,
543 out float t) {
544 return ClosestPtPointSegment(segment.a, segment.b, p, out t);
545 }
551 public static Vector3 ClosestPtPointSegment(Vector3 a, Vector3 b, Vector3 c,
552 out float t) {
553 var ab = b - a;
554 // Project c onto ab, computing parameterized position of the point: a + t*(b - a)
555 t = Dot(c - a, ab) / Dot(ab, ab);
556 // If outside the segment, clamp the parameterization to the closest endpoint.
557 if (t < 0f) t = 0f;
558 if (t > 1f) t = 1f;
559 // Compute and output the projected position from the clamped parameterization.
560 return a + t * ab;
561 }
562
563 #endregion
564
565 }
566
567}