Tanoda
ColliderUtil.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
9
10using System;
11using System.Collections.Generic;
12using UnityEngine;
13
15 public static class ColliderUtil {
16
17 //*************
18 //GENRAL HELPER
19
23 public static bool WithinDistance(Vector3 point, Vector3 center, float radius) {
24 return (point - center).sqrMagnitude < radius * radius;
25 }
26
27 public static bool WithinDistance(Vector3 pointToOrigin, float radius) {
28 return pointToOrigin.sqrMagnitude < radius * radius;
29 }
30
35 public static Vector3 GetPointAtDistance(Vector3 point, Vector3 center, float distance) {
36 return (point - center).normalized * distance + center;
37 }
38
46 public static bool IsPointInside(this Collider collider, Vector3 localPosition, float extrude = 0.0f) {
47 if (collider is SphereCollider) return (collider as SphereCollider).IsPointInside(localPosition, extrude);
48 if (collider is BoxCollider) return (collider as BoxCollider).IsPointInside(localPosition, extrude);
49 if (collider is CapsuleCollider) return (collider as CapsuleCollider).IsPointInside(localPosition, extrude);
50 if (collider is MeshCollider) return collider.bounds.Contains(collider.transform.TransformPoint(localPosition));
51
52 throw nullOrInvalidException(collider);
53 }
54
62 public static Vector3 ClosestPointOnSurface(this Collider collider, Vector3 localPosition, float extrude = 0.0f) {
63 if (collider is SphereCollider) return (collider as SphereCollider).ClosestPointOnSurface(localPosition, extrude);
64 if (collider is BoxCollider) return (collider as BoxCollider).ClosestPointOnSurface(localPosition, extrude);
65 if (collider is CapsuleCollider) return (collider as CapsuleCollider).ClosestPointOnSurface(localPosition, extrude);
66 if (collider is MeshCollider) return collider.transform.InverseTransformPoint(collider.ClosestPointOnBounds(collider.transform.TransformPoint(localPosition)));
67
68 throw nullOrInvalidException(collider);
69 }
70
75 public static bool IsPointInside(List<Collider> colliders, Vector3 globalPosition, float extrude = 0.0f) {
76 for (int i = 0; i < colliders.Count; i++) {
77 Collider collider = colliders[i];
78 Vector3 localPosition = collider.transform.InverseTransformPoint(globalPosition);
79 if (collider.IsPointInside(localPosition, extrude)) {
80 return true;
81 }
82 }
83 return false;
84 }
85
91 public static Vector3 ClosestPointOnSurfaces(List<Collider> colliders, Vector3 globalPosition, float extrude = 0.0f) {
92 Transform chosenTransform = null;
93 Vector3 closestPoint = Vector3.zero;
94 float closestDistance = float.MaxValue;
95
96 for (int i = 0; i < colliders.Count; i++) {
97 Collider collider = colliders[i];
98 Vector3 localPoint = collider.transform.InverseTransformPoint(globalPosition);
99 Vector3 point = collider.ClosestPointOnSurface(localPoint, extrude);
100 float distance = (globalPosition - point).sqrMagnitude;
101 if (distance < closestDistance) {
102 chosenTransform = collider.transform;
103 closestDistance = distance;
104 closestPoint = point;
105 }
106 }
107
108 return chosenTransform.TransformPoint(closestPoint);
109 }
110
111 //***************
112 //Raycasting
113
114 public static bool SegmentCast(List<Collider> colliders, Vector3 worldStart, Vector3 worldEnd, out RaycastHit hitInfo) {
115 Ray ray = new Ray(worldStart, worldEnd - worldStart);
116 float dist = Vector3.Distance(worldStart, worldEnd);
117
118 hitInfo = new RaycastHit();
119
120 bool hitAny = false;
121 RaycastHit tempHit;
122 foreach (Collider collider in colliders) {
123 if (collider.Raycast(ray, out tempHit, dist)) {
124 if (!hitAny || tempHit.distance < hitInfo.distance) {
125 hitInfo = tempHit;
126 hitAny = true;
127 }
128 }
129 }
130
131 return hitAny;
132 }
133
134 //***************
135 //SPHERE COLLIDER
136
137 public static bool IsPointInside(this SphereCollider collider, Vector3 localPosition, float extrude = 0.0f) {
138 localPosition -= collider.center;
139 return WithinDistance(localPosition, collider.radius + extrude);
140 }
141
142 public static Vector3 ClosestPointOnSurface(this SphereCollider collider, Vector3 localPosition, float extrude = 0.0f) {
143 return GetPointAtDistance(localPosition, collider.center, collider.radius + extrude);
144 }
145
146 //************
147 //BOX COLLIDER
148
149 public static bool IsPointInside(this BoxCollider collider, Vector3 localPosition, float extrude = 0.0f) {
150 localPosition -= collider.center;
151 if (Mathf.Abs(localPosition.x) > (collider.size.x / 2.0f) + (extrude / collider.transform.lossyScale.x)) {
152 return false;
153 }
154 if (Mathf.Abs(localPosition.y) > (collider.size.y / 2.0f) + (extrude / collider.transform.lossyScale.y)) {
155 return false;
156 }
157 if (Mathf.Abs(localPosition.z) > (collider.size.z / 2.0f) + (extrude / collider.transform.lossyScale.z)) {
158 return false;
159 }
160 return true;
161 }
162
163 public static Vector3 ClosestPointOnSurface(this BoxCollider collider, Vector3 localPosition, float extrude = 0.0f) {
164 localPosition -= collider.center;
165
166 Vector3 radius = collider.size / 2.0f;
167 radius.x += extrude;
168 radius.y += extrude;
169 radius.z += extrude;
170
171 localPosition.x = Mathf.Clamp(localPosition.x, -radius.x, radius.x);
172 localPosition.y = Mathf.Clamp(localPosition.y, -radius.y, radius.y);
173 localPosition.z = Mathf.Clamp(localPosition.z, -radius.z, radius.z);
174
175 //If an internal point
176 if (Mathf.Abs(localPosition.x) < radius.x &&
177 Mathf.Abs(localPosition.y) < radius.y &&
178 Mathf.Abs(localPosition.z) < radius.z) {
179 //Snap closest axis to the bounds
180 //Farthest from the center
181 if (Mathf.Abs(localPosition.x) > Mathf.Abs(localPosition.y)) {
182 if (Mathf.Abs(localPosition.x) > Mathf.Abs(localPosition.z)) {
183 //x is farthest
184 localPosition.x = (localPosition.x > 0) ? radius.x : -radius.x;
185 } else {
186 //z is farthest
187 localPosition.z = (localPosition.z > 0) ? radius.z : -radius.z;
188 }
189 } else {
190 if (Mathf.Abs(localPosition.y) > Mathf.Abs(localPosition.z)) {
191 //y is farthest
192 localPosition.y = (localPosition.y > 0) ? radius.y : -radius.y;
193 } else {
194 //z is farthest
195 localPosition.z = (localPosition.z > 0) ? radius.z : -radius.z;
196 }
197 }
198 }
199
200 localPosition += collider.center;
201 return localPosition;
202 }
203
204 //****************
205 //CAPSULE COLLIDER
206
211 public static void GetSegmentInfo(this CapsuleCollider collider, out Vector3 localV0, out Vector3 localV1, out float length) {
212 Vector3 axis = Vector3.right;
213 if (collider.direction == 1) {
214 axis = Vector3.up;
215 } else {
216 axis = Vector3.forward;
217 }
218
219 length = Mathf.Max(0, collider.height - collider.radius * 2);
220 localV0 = axis * length / 2.0f + collider.center;
221 localV1 = -axis * length / 2.0f + collider.center;
222 }
223
224 public static bool IsPointInside(this CapsuleCollider collider, Vector3 localPosition, float tolerance = 0.0f) {
225 Vector3 v0, v1;
226 float length;
227 collider.GetSegmentInfo(out v0, out v1, out length);
228
229 if (length == 0.0f) {
230 return WithinDistance(localPosition, collider.radius + tolerance);
231 }
232
233 float t = Vector3.Dot(localPosition - v0, v1 - v0) / (length * length);
234 if (t <= 0.0f) {
235 return WithinDistance(localPosition, v0, collider.radius + tolerance);
236 }
237 if (t >= 1.0f) {
238 return WithinDistance(localPosition, v1, collider.radius + tolerance);
239 }
240
241 Vector3 projection = v0 + t * (v1 - v0);
242 return WithinDistance(localPosition, projection, collider.radius + tolerance);
243 }
244
245 public static Vector3 ClosestPointOnSurface(this CapsuleCollider collider, Vector3 localPosition, float extrude = 0.0f) {
246 Vector3 v0, v1;
247 float length;
248 collider.GetSegmentInfo(out v0, out v1, out length);
249
250 if (length == 0.0f) {
251 return GetPointAtDistance(localPosition, collider.center, collider.radius);
252 }
253
254 float t = Vector3.Dot(localPosition - v0, v1 - v0) / (length * length);
255 if (t <= 0.0f) {
256 return GetPointAtDistance(localPosition, v0, collider.radius + extrude);
257 }
258 if (t >= 1.0f) {
259 return GetPointAtDistance(localPosition, v1, collider.radius + extrude);
260 }
261
262 Vector3 projection = v0 + t * (v1 - v0);
263 return GetPointAtDistance(localPosition, projection, collider.radius + extrude);
264 }
265
266 public static float DistanceToSegment(Vector3 a, Vector3 b, Vector3 p) {
267 float t = Vector3.Dot(p - a, b - a) / Vector3.Dot(a - b, a - b);
268 if (t <= 0.0f) {
269 return Vector3.Distance(p, a);
270 }
271 if (t >= 1.0f) {
272 return Vector3.Distance(p, b);
273 }
274 Vector3 projection = a + t * (b - a);
275 return Vector3.Distance(p, projection);
276 }
277
278 private static Exception nullOrInvalidException(Collider collider) {
279 if (collider == null) {
280 return new ArgumentNullException();
281 } else {
282 return new ArgumentException("Collider type of " + collider.GetType() + " is not supported.");
283 }
284 }
285 }
286}