11using System.Collections.Generic;
14 public static class PhysicsUtility {
29 public static Vector3 ToLinearVelocity(Vector3 deltaPosition,
float deltaTime) {
30 return deltaPosition / deltaTime;
33 public static Vector3 ToLinearVelocity(Vector3 startPosition, Vector3 destinationPosition,
float deltaTime) {
34 return ToLinearVelocity(destinationPosition - startPosition, deltaTime);
37 public static Vector3 ToAngularVelocity(Quaternion deltaRotation,
float deltaTime) {
40 deltaRotation.ToAngleAxis(out deltaAngle, out deltaAxis);
42 if (
float.IsInfinity(deltaAxis.x)) {
43 deltaAxis = Vector3.zero;
47 if (deltaAngle > 180) {
51 return deltaAxis * deltaAngle * Mathf.Deg2Rad / deltaTime;
54 public static Vector3 ToAngularVelocity(Quaternion startRotation, Quaternion destinationRotation,
float deltaTime) {
55 return ToAngularVelocity(destinationRotation * Quaternion.Inverse(startRotation), deltaTime);
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));
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);
67 return numberOfColliders > 0;
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);
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));
79 float radius = otherColliderPositionOnBox.magnitude;
80 otherColliderPositionOnBox = (box.attachedRigidbody.rotation * otherColliderPositionOnBox) + box.attachedRigidbody.position;
82 generateSphereContact(otherColliderPositionOnBox, 0f, box.attachedRigidbody.GetPointVelocity(otherColliderPositionOnBox), layerMask, ref softContacts, ref originalVelocities, otherCollider, interpretAsEllipsoid);
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;
91 Vector3 otherColliderPositionOnSphere = sphere.transform.TransformPoint(unrotatedOtherColliderPosition + sphere.center);
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);
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;
99 switch (capsule.direction) {
101 a =
Vector3.right * ((capsule.height * 0.5f) - capsule.radius);
104 a =
Vector3.up * ((capsule.height * 0.5f) - capsule.radius);
107 a =
Vector3.forward * ((capsule.height * 0.5f) - capsule.radius);
110 a =
Vector3.up * ((capsule.height * 0.5f) - capsule.radius);
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;
120 Vector3 otherColliderPositionOnCapsule = capsule.transform.TransformPoint(otherColliderPosOnSegment + displacement + capsule.center);
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);
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;
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;
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;
141 originalVelocities.Add(contact.body, originalBodyVelocities);
145 if (contact.body.useGravity) {
146 contact.body.AddForce(-
Physics.gravity, ForceMode.Acceleration);
147 contact.body.velocity +=
Physics.gravity * Time.fixedDeltaTime;
151 if (interpretAsEllipsoid || temporaryCollider is MeshCollider) {
153 contact.normal = (temporaryCollider.bounds.center - spherePosition).normalized;
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;
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);
164 contact.position = spherePosition + (contact.normal * sphereRadius);
165 contact.velocity = sphereVelocity;
166 contact.invWorldInertiaTensor = originalBodyVelocities.invWorldInertiaTensor;
168 softContacts.Add(contact);
172 static void applySoftContact(Rigidbody thisObject, Vector3 handContactPoint, Vector3 handVelocityAtContactPoint, Vector3 normal, Matrix4x4 invWorldInertiaTensor) {
174 Vector3 objectVelocityAtContactPoint = thisObject.GetPointVelocity(handContactPoint);
175 Vector3 velocityDifference = objectVelocityAtContactPoint - handVelocityAtContactPoint;
176 float relativeVelocity =
Vector3.Dot(normal, velocityDifference);
177 if (relativeVelocity >
float.Epsilon)
186 float invVelocityDifferenceMagnitude = 1f / velocityDifference.magnitude;
187 Vector3 vel_dir = velocityDifference * invVelocityDifferenceMagnitude;
188 float t = -relativeVelocity * invVelocityDifferenceMagnitude;
190 normal = t * vel_dir + (1.0f - t) * normal;
192 if (normal.sqrMagnitude >
float.Epsilon) { normal = normal.normalized; }
193 relativeVelocity =
Vector3.Dot(normal, velocityDifference);
199 float jacDiagABInv = 1.0f / computeImpulseDenominator(thisObject, handContactPoint, normal, invWorldInertiaTensor);
200 float velocityImpulse = -relativeVelocity * jacDiagABInv;
202 applyImpulseNow(thisObject, normal * velocityImpulse, handContactPoint, invWorldInertiaTensor);
205 public static void applySoftContacts(List<SoftContact> softContacts, Dictionary<Rigidbody, Velocities> originalVelocities) {
206 if (softContacts.Count > 0) {
208 bool m_iterateForwards =
true;
209 for (
int i = 0; i < 10; i++) {
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);
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);
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);
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);
227 m_iterateForwards = !m_iterateForwards;
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;
238 softContacts.Clear();
239 originalVelocities.Clear();
243 static float computeImpulseDenominator(Rigidbody thisObject, Vector3 pos, Vector3 normal, Matrix4x4 invWorldInertiaTensor) {
244 Vector3 r0 = pos - thisObject.worldCenterOfMass;
247 return 1f / thisObject.mass +
Vector3.Dot(normal, vec);
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;
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);
Matrix4x4 invWorldInertiaTensor