Tanoda
PhysicsUtility.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 UnityEngine;
11using System.Collections.Generic;
12
14 public static class PhysicsUtility {
15 public struct SoftContact {
16 public Rigidbody body;
17 public Vector3 position;
18 public Vector3 normal;
19 public Vector3 velocity;
20 public Matrix4x4 invWorldInertiaTensor;
21 }
22
23 public struct Velocities {
24 public Vector3 velocity;
25 public Vector3 angularVelocity;
26 public Matrix4x4 invWorldInertiaTensor;
27 }
28
29 public static Vector3 ToLinearVelocity(Vector3 deltaPosition, float deltaTime) {
30 return deltaPosition / deltaTime;
31 }
32
33 public static Vector3 ToLinearVelocity(Vector3 startPosition, Vector3 destinationPosition, float deltaTime) {
34 return ToLinearVelocity(destinationPosition - startPosition, deltaTime);
35 }
36
37 public static Vector3 ToAngularVelocity(Quaternion deltaRotation, float deltaTime) {
38 Vector3 deltaAxis;
39 float deltaAngle;
40 deltaRotation.ToAngleAxis(out deltaAngle, out deltaAxis);
41
42 if (float.IsInfinity(deltaAxis.x)) {
43 deltaAxis = Vector3.zero;
44 deltaAngle = 0;
45 }
46
47 if (deltaAngle > 180) {
48 deltaAngle -= 360.0f;
49 }
50
51 return deltaAxis * deltaAngle * Mathf.Deg2Rad / deltaTime;
52 }
53
54 public static Vector3 ToAngularVelocity(Quaternion startRotation, Quaternion destinationRotation, float deltaTime) {
55 return ToAngularVelocity(destinationRotation * Quaternion.Inverse(startRotation), deltaTime);
56 }
57
58 public static bool IsValid(this Vector3 v) {
59 return !(float.IsNaN(v.x) || float.IsNaN(v.y) || float.IsNaN(v.z)) && !(float.IsInfinity(v.x) || float.IsInfinity(v.y) || float.IsInfinity(v.z));
60 }
61
62 public static bool generateSphereContacts(Vector3 spherePosition, float sphereRadius, Vector3 sphereVelocity, int layerMask, ref List<SoftContact> softContacts, ref Dictionary<Rigidbody, Velocities> originalVelocities, ref Collider[] temporaryColliderSwapSpace, bool interpretAsEllipsoid = true) {
63 int numberOfColliders = Physics.OverlapSphereNonAlloc(spherePosition, sphereRadius, temporaryColliderSwapSpace, layerMask, QueryTriggerInteraction.Ignore);
64 for (int i = 0; i < numberOfColliders; i++) {
65 generateSphereContact(spherePosition, sphereRadius, sphereVelocity, layerMask, ref softContacts, ref originalVelocities, temporaryColliderSwapSpace[i], interpretAsEllipsoid);
66 }
67 return numberOfColliders > 0;
68 }
69
70 public static void generateBoxContact(BoxCollider box, int layerMask, Collider otherCollider, ref List<SoftContact> softContacts, ref Dictionary<Rigidbody, Velocities> originalVelocities, bool interpretAsEllipsoid = true) {
71 Vector3 boxExtent = Vector3.Scale(box.size * 0.5f, box.transform.lossyScale);
72 Vector3 boxCenter = Vector3.Scale(box.center, box.transform.lossyScale);
73 Vector3 unrotatedOtherColliderPosition = Quaternion.Inverse(box.attachedRigidbody.rotation) * (otherCollider.attachedRigidbody.position - box.attachedRigidbody.position);
74 Vector3 otherColliderPositionOnBox = new Vector3(
75 Mathf.Clamp(unrotatedOtherColliderPosition.x, -boxExtent.x + boxCenter.x, boxExtent.x + boxCenter.x),
76 Mathf.Clamp(unrotatedOtherColliderPosition.y, -boxExtent.y + boxCenter.y, boxExtent.y + boxCenter.y),
77 Mathf.Clamp(unrotatedOtherColliderPosition.z, -boxExtent.z + boxCenter.z, boxExtent.z + boxCenter.z));
78
79 float radius = otherColliderPositionOnBox.magnitude;
80 otherColliderPositionOnBox = (box.attachedRigidbody.rotation * otherColliderPositionOnBox) + box.attachedRigidbody.position;
81
82 generateSphereContact(otherColliderPositionOnBox, 0f, box.attachedRigidbody.GetPointVelocity(otherColliderPositionOnBox), layerMask, ref softContacts, ref originalVelocities, otherCollider, interpretAsEllipsoid);
83 }
84
85 public static void generateSphereContact(SphereCollider sphere, int layerMask, Collider otherCollider, ref List<SoftContact> softContacts, ref Dictionary<Rigidbody, Velocities> originalVelocities, bool interpretAsEllipsoid = true) {
86 Vector3 unrotatedOtherColliderPosition = sphere.transform.InverseTransformPoint(otherCollider.attachedRigidbody.position) - sphere.center;
87 float dist = unrotatedOtherColliderPosition.magnitude;
88 if (dist > sphere.radius) {
89 unrotatedOtherColliderPosition *= sphere.radius / dist;
90 }
91 Vector3 otherColliderPositionOnSphere = sphere.transform.TransformPoint(unrotatedOtherColliderPosition + sphere.center);
92
93 generateSphereContact(sphere.transform.TransformPoint(sphere.center), (sphere.transform.TransformPoint(sphere.center) - otherColliderPositionOnSphere).magnitude, sphere.attachedRigidbody.GetPointVelocity(otherColliderPositionOnSphere), layerMask, ref softContacts, ref originalVelocities, otherCollider, interpretAsEllipsoid);
94 }
95
96 public static void generateCapsuleContact(CapsuleCollider capsule, int layerMask, Collider otherCollider, ref List<SoftContact> softContacts, ref Dictionary<Rigidbody, Velocities> originalVelocities, bool interpretAsEllipsoid = true) {
97 Vector3 unrotatedOtherColliderPosition = capsule.transform.InverseTransformPoint(otherCollider.attachedRigidbody.position) - capsule.center;
98 Vector3 a, b;
99 switch (capsule.direction) {
100 case 0:
101 a = Vector3.right * ((capsule.height * 0.5f) - capsule.radius);
102 break;
103 case 1:
104 a = Vector3.up * ((capsule.height * 0.5f) - capsule.radius);
105 break;
106 case 2:
107 a = Vector3.forward * ((capsule.height * 0.5f) - capsule.radius);
108 break;
109 default:
110 a = Vector3.up * ((capsule.height * 0.5f) - capsule.radius);
111 break;
112 }
113 b = -a; Vector3 ba = b - a;
114 Vector3 otherColliderPosOnSegment = Vector3.Lerp(a, b, Vector3.Dot(unrotatedOtherColliderPosition - a, ba) / ba.sqrMagnitude);
115 Vector3 displacement = unrotatedOtherColliderPosition - otherColliderPosOnSegment;
116 float dist = displacement.magnitude;
117 if (dist > capsule.radius) {
118 displacement *= capsule.radius / dist;
119 }
120 Vector3 otherColliderPositionOnCapsule = capsule.transform.TransformPoint(otherColliderPosOnSegment + displacement + capsule.center);
121
122 generateSphereContact(capsule.transform.TransformPoint(capsule.center), (capsule.transform.TransformPoint(capsule.center) - otherColliderPositionOnCapsule).magnitude, capsule.attachedRigidbody.GetPointVelocity(otherColliderPositionOnCapsule), layerMask, ref softContacts, ref originalVelocities, otherCollider, interpretAsEllipsoid);
123 }
124
125 public static void generateSphereContact(Vector3 spherePosition, float sphereRadius, Vector3 sphereVelocity, int layerMask, ref List<SoftContact> softContacts, ref Dictionary<Rigidbody, Velocities> originalVelocities, Collider temporaryCollider, bool interpretAsEllipsoid = true) {
126 if (temporaryCollider.attachedRigidbody != null && !temporaryCollider.attachedRigidbody.isKinematic) {
127 SoftContact contact = new SoftContact();
128 contact.body = temporaryCollider.attachedRigidbody;
129
130 //Store this rigidbody's pre-contact velocities and inverse world inertia tensor
131 Velocities originalBodyVelocities;
132 if (!originalVelocities.TryGetValue(contact.body, out originalBodyVelocities)) {
133 originalBodyVelocities = new Velocities();
134 originalBodyVelocities.velocity = contact.body.velocity;
135 originalBodyVelocities.angularVelocity = contact.body.angularVelocity;
136
137 Matrix4x4 tensorRotation = Matrix4x4.TRS(Vector3.zero, contact.body.rotation * contact.body.inertiaTensorRotation, Vector3.one);
138 Matrix4x4 worldInertiaTensor = (tensorRotation * Matrix4x4.Scale(contact.body.inertiaTensor) * tensorRotation.inverse);
139 originalBodyVelocities.invWorldInertiaTensor = worldInertiaTensor.inverse;
140
141 originalVelocities.Add(contact.body, originalBodyVelocities);
142
143 //Premptively apply the force due to gravity BEFORE soft contact
144 //so soft contact can factor it in during the collision solve
145 if (contact.body.useGravity) {
146 contact.body.AddForce(-Physics.gravity, ForceMode.Acceleration);
147 contact.body.velocity += Physics.gravity * Time.fixedDeltaTime;
148 }
149 }
150
151 if (interpretAsEllipsoid || temporaryCollider is MeshCollider) {
152 //First get the world spherical normal
153 contact.normal = (temporaryCollider.bounds.center - spherePosition).normalized;
154 //Then divide by the world extends squared to get the world ellipsoidal normal
155 Vector3 colliderExtentsSquared = Vector3.Scale(temporaryCollider.bounds.extents, (temporaryCollider.bounds.extents));
156 contact.normal = new Vector3(contact.normal.x / colliderExtentsSquared.x, contact.normal.y / colliderExtentsSquared.y, contact.normal.z / colliderExtentsSquared.z).normalized;
157 } else {
158 //Else, use the analytic support functions that we have for boxes and capsules to generate the normal
159 Vector3 objectLocalBoneCenter = temporaryCollider.transform.InverseTransformPoint(spherePosition);
160 Vector3 objectPoint = temporaryCollider.transform.TransformPoint(temporaryCollider.ClosestPointOnSurface(objectLocalBoneCenter));
161 contact.normal = (objectPoint - spherePosition).normalized * (temporaryCollider.IsPointInside(objectLocalBoneCenter) ? -1f : 1f);
162 }
163
164 contact.position = spherePosition + (contact.normal * sphereRadius);
165 contact.velocity = sphereVelocity;
166 contact.invWorldInertiaTensor = originalBodyVelocities.invWorldInertiaTensor;
167
168 softContacts.Add(contact);
169 }
170 }
171
172 static void applySoftContact(Rigidbody thisObject, Vector3 handContactPoint, Vector3 handVelocityAtContactPoint, Vector3 normal, Matrix4x4 invWorldInertiaTensor) {
173 //Determine the relative velocity between the soft contactor and the object at the contact point
174 Vector3 objectVelocityAtContactPoint = thisObject.GetPointVelocity(handContactPoint);
175 Vector3 velocityDifference = objectVelocityAtContactPoint - handVelocityAtContactPoint;
176 float relativeVelocity = Vector3.Dot(normal, velocityDifference);
177 if (relativeVelocity > float.Epsilon)
178 return;
179
180 // Define a cone of friction where the direction of velocity relative to the surface
181 // controls the degree to which perpendicular movement (relative to the surface) is
182 // resisted. Smoothly blending in friction prevents vibration while the impulse
183 // solver is iterating.
184 // Interpolation parameter 0..1: -handContactNormal.dot(vel_dir).
185
186 float invVelocityDifferenceMagnitude = 1f / velocityDifference.magnitude;
187 Vector3 vel_dir = velocityDifference * invVelocityDifferenceMagnitude;
188 float t = -relativeVelocity * invVelocityDifferenceMagnitude;
189
190 normal = t * vel_dir + (1.0f - t) * normal;
191
192 if (normal.sqrMagnitude > float.Epsilon) { normal = normal.normalized; }
193 relativeVelocity = Vector3.Dot(normal, velocityDifference);
194
195 // Apply impulse along calculated normal. Note this is slightly unusual. Instead of
196 // handling perpendicular movement as a separate constraint this calculates a "bent normal"
197 // to blend it in.
198
199 float jacDiagABInv = 1.0f / computeImpulseDenominator(thisObject, handContactPoint, normal, invWorldInertiaTensor);
200 float velocityImpulse = -relativeVelocity * jacDiagABInv;
201
202 applyImpulseNow(thisObject, normal * velocityImpulse, handContactPoint, invWorldInertiaTensor);
203 }
204
205 public static void applySoftContacts(List<SoftContact> softContacts, Dictionary<Rigidbody, Velocities> originalVelocities) {
206 if (softContacts.Count > 0) {
207 //Switch between iterating forwards and reverse through the list of contacts
208 bool m_iterateForwards = true;
209 for (int i = 0; i < 10; i++) {
210 // Pick a random partition.
211 int partition = UnityEngine.Random.Range(0, softContacts.Count);
212 if (m_iterateForwards) {
213 for (int it = partition; it < softContacts.Count; it++) {
214 applySoftContact(softContacts[it].body, softContacts[it].position, softContacts[it].velocity, softContacts[it].normal, softContacts[it].invWorldInertiaTensor);
215 }
216 for (int it = 0; it < partition; it++) {
217 applySoftContact(softContacts[it].body, softContacts[it].position, softContacts[it].velocity, softContacts[it].normal, softContacts[it].invWorldInertiaTensor);
218 }
219 } else {
220 for (int it = partition; 0 <= it; it--) {
221 applySoftContact(softContacts[it].body, softContacts[it].position, softContacts[it].velocity, softContacts[it].normal, softContacts[it].invWorldInertiaTensor);
222 }
223 for (int it = softContacts.Count - 1; partition <= it; it--) {
224 applySoftContact(softContacts[it].body, softContacts[it].position, softContacts[it].velocity, softContacts[it].normal, softContacts[it].invWorldInertiaTensor);
225 }
226 }
227 m_iterateForwards = !m_iterateForwards;
228 }
229
230 //Apply these changes in velocity as forces, so the PhysX solver can resolve them (optional)
231 foreach (KeyValuePair<Rigidbody, Velocities> RigidbodyVelocity in originalVelocities) {
232 RigidbodyVelocity.Key.AddForce(RigidbodyVelocity.Key.velocity - RigidbodyVelocity.Value.velocity, ForceMode.VelocityChange);
233 RigidbodyVelocity.Key.AddTorque(RigidbodyVelocity.Key.angularVelocity - RigidbodyVelocity.Value.angularVelocity, ForceMode.VelocityChange);
234 RigidbodyVelocity.Key.velocity = RigidbodyVelocity.Value.velocity;
235 RigidbodyVelocity.Key.angularVelocity = RigidbodyVelocity.Value.angularVelocity;
236 }
237
238 softContacts.Clear();
239 originalVelocities.Clear();
240 }
241 }
242
243 static float computeImpulseDenominator(Rigidbody thisObject, Vector3 pos, Vector3 normal, Matrix4x4 invWorldInertiaTensor) {
244 Vector3 r0 = pos - thisObject.worldCenterOfMass;
245 Vector3 c0 = Vector3.Cross(r0, normal);
246 Vector3 vec = Vector3.Cross(invWorldInertiaTensor * c0, r0);
247 return 1f / thisObject.mass + Vector3.Dot(normal, vec);
248 }
249
250 public static void applyImpulseNow(Rigidbody thisObject, Vector3 impulse, Vector3 worldPos, Matrix4x4 invWorldInertiaTensor) {
251 if (thisObject.mass != 0f && 1f / thisObject.mass != 0f && impulse.IsValid() && impulse != Vector3.zero) {
252 impulse = impulse.normalized * Mathf.Clamp(impulse.magnitude, 0f, 6f);
253 thisObject.velocity += impulse / thisObject.mass;
254 Vector3 angularImpulse = Vector3.Cross(worldPos - thisObject.worldCenterOfMass, invWorldInertiaTensor * impulse);
255 if (angularImpulse.magnitude < 4f) {
256 thisObject.angularVelocity += angularImpulse;
257 }
258 }
259 }
260
261 static void setPointVelocityNow(Rigidbody thisObject, Vector3 impulse, Vector3 worldPos, float LinearAngularRatio) {
262 if (thisObject.mass != 0f && 1f / thisObject.mass != 0f) {
263 LinearAngularRatio = Mathf.Clamp01(LinearAngularRatio);
264 thisObject.velocity = impulse * LinearAngularRatio;
265 thisObject.angularVelocity = Vector3.Cross(worldPos - thisObject.worldCenterOfMass, impulse / (worldPos - thisObject.worldCenterOfMass).sqrMagnitude) * (1f - LinearAngularRatio);
266 }
267 }
268 }
269}