16 public static class Collision {
20 private static float Dot(Vector3 a, Vector3 b) {
24 private static float Dot(Vector2 a, Vector2 b) {
32 public static bool DoesOverlap(Box box, Sphere sphere) {
33 return DoesOverlap(sphere, box);
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();
40 float x = sphereCenter_box.x,
41 y = sphereCenter_box.y,
42 z = sphereCenter_box.z;
44 float bx = box.radii.x.Abs(),
45 by = box.radii.y.Abs(),
46 bz = box.radii.z.Abs();
48 float dx = x - Mathf.Min(bx, x),
49 dy = y - Mathf.Min(by, y),
50 dz = z - Mathf.Min(bz, z);
52 if (dx == 0 && dy == 0 && dz == 0)
return true;
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);
60 return boxSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
71 public static float DistanceTo(Rect rect, Sphere sphere) {
72 return DistanceBetween(sphere, rect);
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();
83 float x = sphereCenter_rect.x,
84 y = sphereCenter_rect.y,
85 z = sphereCenter_rect.z;
87 float rx = rect.radii.x.Abs(),
88 ry = rect.radii.y.Abs(),
91 float dx = x - Mathf.Min(rx, x),
92 dy = y - Mathf.Min(ry, y),
93 dz = z - Mathf.Min(rz, z);
95 if (dx == 0 && dy == 0 && dz == 0)
return 0f;
97 var rectSurfaceToSphereCenter_rect =
new Vector3(dx, dy, dz);
98 var rectSurfaceToSphereCenter_world
99 = rect.matrix.MultiplyVector(rectSurfaceToSphereCenter_rect);
101 return Mathf.Max(0f, rectSurfaceToSphereCenter_world.magnitude - sphere.radius);
104 public static bool DoesOverlap(Rect rect, Sphere sphere) {
105 return DoesOverlap(sphere, rect);
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();
112 float x = sphereCenter_rect.x,
113 y = sphereCenter_rect.y,
114 z = sphereCenter_rect.z;
116 float rx = rect.radii.x.Abs(),
117 ry = rect.radii.y.Abs(),
120 float dx = x - Mathf.Min(rx, x),
121 dy = y - Mathf.Min(ry, y),
122 dz = z - Mathf.Min(rz, z);
124 if (dx == 0 && dy == 0 && dz == 0)
return true;
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);
132 return rectSurfaceToSphereCenter_sphere.sqrMagnitude < sphere.radius * sphere.radius;
137 #region Rect-Segment3
142 public static float Intersect(LocalSegment3 segment, Rect rect) {
143 return Intersect(rect, segment);
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);
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);
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);
162 bool aProjectsInside = rect.ContainsProjectedPoint(segment.a, out segmentA_rect);
164 bool bProjectsInside = rect.ContainsProjectedPoint(segment.b, out segmentB_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,
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);
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;
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;
208 if (!aProjectsInside || !bProjectsInside) {
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);
225 if (testSqrDist < closestSqrDist) {
226 closestSqrDist = testSqrDist;
227 closestPointOnRect = testClosestPointRectSegment;
228 closestPointOnSegment = testClosestPointSegment;
233 return closestSqrDist;
238 #region Plane-Segment3
249 public static bool Intersect(LocalPlane plane, LocalSegment3 line) {
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);
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;
297 var uDotN = planeNormal.Dot(u);
298 if (uDotN.Abs() >
float.Epsilon) {
299 isLineCoplanar =
false;
301 var w = a - planePosition;
302 amountAlongSegment = -(planeNormal.Dot(w)) / uDotN;
303 pointOnPlane = a + u * amountAlongSegment;
305 return intersectInfiniteLine
306 || (amountAlongSegment >= 0f && amountAlongSegment <= 1f);
309 var pa = a - planePosition;
310 if (planeNormal.Dot(pa).Abs() <
float.Epsilon) {
311 isLineCoplanar =
true;
314 isLineCoplanar =
false;
317 if (isLineCoplanar) {
318 pointOnPlane = (a + b) / 2f;
319 amountAlongSegment = 0.5f;
323 pointOnPlane =
new Vector3(
float.NaN,
float.NaN,
float.NaN);
324 amountAlongSegment =
float.NaN;
332 #region Segment-Segment
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);
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;
353 float a1 = Signed2DTriArea(a, b, d);
354 float a2 = Signed2DTriArea(a, b, c);
358 if (a1 * a2 < 0f && a1 != 0f && a2 != 0f) {
360 float a3 = Signed2DTriArea(c, d, a);
363 float a4 = a3 + a2 - a1;
378 t =
default(
float); p =
default(Vector2);
387 public static float Intersect(LocalSegment3 seg1, LocalSegment3 seg2,
388 out
float t1, out
float t2,
389 out Vector3 c1, out Vector3 c2) {
393 float a = Dot(d1, d1);
394 float e = Dot(d2, d2);
395 float f = Dot(d2, r);
398 if (a <=
float.Epsilon && e <=
float.Epsilon) {
401 c1 = seg1.a; c2 = seg2.a;
402 return Dot(c1 - c2, c1 - c2);
404 if (a <=
float.Epsilon) {
407 t2 = (f / e).Clamped01();
410 float c = Dot(d1, r);
411 if (e <=
float.Epsilon) {
414 t1 = (-c / a).Clamped01();
418 float b = Dot(d1, d2);
419 float denom = a*e - b*b;
424 t1 = ((b*f - c*e) / denom).Clamped01();
430 float t2nom = b*t1 + f;
436 t1 = (-c / a).Clamped01();
438 else if (t2nom > e) {
440 t1 = ((b - c) / a).Clamped01();
448 c1 = seg1.a + d1 * t1;
449 c2 = seg2.a + d2 * t2;
450 return (c1 - c2).sqrMagnitude;
463 public static bool Intersect2D(LocalSegment3 segment0, LocalSegment3 segment1,
464 out Vector2 intersectionPoint,
465 out
float amountAlongSegment0,
466 out
float amountAlongSegment1) {
468 Vector2 p = segment0.a, q = segment0.b, r = segment1.a, s = segment1.b;
473 var denom = Fake2DCross(r, s);
477 amountAlongSegment0 =
float.NaN;
478 amountAlongSegment1 =
float.NaN;
479 intersectionPoint =
new Vector2(
float.NaN,
float.NaN);
483 var tNumer = Fake2DCross(q - p, s);
484 var uNumer = Fake2DCross(q - p, r);
486 amountAlongSegment0 = tNumer / denom;
487 amountAlongSegment1 = uNumer / denom;
489 if ( amountAlongSegment0 < 0 || amountAlongSegment0 > 1
490 || amountAlongSegment1 < 0 || amountAlongSegment1 > 1) {
492 intersectionPoint =
default(Vector2);
496 intersectionPoint = p + r * amountAlongSegment0;
500 private static float Fake2DCross(Vector2 a, Vector2 b) {
501 return a.x * b.y - a.y * b.x;
508 #region Point-Segment
513 public static float SqrDistPointSegment(LocalSegment3 segment, Vector3 p) {
514 return SqrDistPointSegment(segment.a, segment.b, p);
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);
523 if (e <= 0f)
return Dot(ac, ac);
524 float f = Dot(ab, ab);
525 if (e >= f)
return Dot(bc, bc);
527 return Dot(ac, ac) - e * e / f;
533 public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p) {
535 return ClosestPtPointSegment(segment.a, segment.b, p, out unusedT);
542 public static Vector3 ClosestPtPointSegment(LocalSegment3 segment, Vector3 p,
544 return ClosestPtPointSegment(segment.a, segment.b, p, out t);
551 public static Vector3 ClosestPtPointSegment(Vector3 a, Vector3 b, Vector3 c,
555 t = Dot(c - a, ab) / Dot(ab, ab);