CroakFang
277 lines
9.8 KiB
C#
277 lines
9.8 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
|
|
namespace RootMotion.Dynamics {
|
|
|
|
/// <summary>
|
|
/// Library of static helper methods for working with PhysX Rigidbodies.
|
|
/// </summary>
|
|
public static class PhysXTools {
|
|
|
|
/// <summary>
|
|
/// Predicts unconstrained Rigidbody (ignoring collisions and joints) position and rotation after specified PhysX steps.
|
|
/// </summary>
|
|
public static void Predict(Rigidbody r, int steps, out Vector3 position, out Quaternion rotation)
|
|
{
|
|
Predict(r, steps, out position, out rotation, Physics.gravity, r.drag, r.angularDrag);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Predicts unconstrained Rigidbody (ignoring collisions and joints) position and rotation after specified PhysX steps using custom gravity, drag and angularDrag settings.
|
|
/// </summary>
|
|
public static void Predict(Rigidbody r, int steps, out Vector3 position, out Quaternion rotation, Vector3 gravity, float drag, float angularDrag)
|
|
{
|
|
position = r.position;
|
|
rotation = r.rotation;
|
|
Vector3 velocity = r.velocity;
|
|
Vector3 angularVelocity = r.angularVelocity;
|
|
|
|
for (int i = 0; i < steps; i++)
|
|
{
|
|
Predict(ref position, ref rotation, ref velocity, ref angularVelocity, gravity, drag, angularDrag);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Predicts a single PhysX step of an unconstrained Rigidbody (ignoring collisions and joints) position and rotation using custom gravity, drag and angularDrag settings.
|
|
/// </summary>
|
|
public static void Predict(ref Vector3 position, ref Quaternion rotation, ref Vector3 velocity, ref Vector3 angularVelocity, Vector3 gravity, float drag, float angularDrag)
|
|
{
|
|
velocity += gravity * Time.fixedDeltaTime;
|
|
|
|
velocity -= velocity * drag * Time.fixedDeltaTime;
|
|
angularVelocity -= angularVelocity * angularDrag * Time.fixedDeltaTime;
|
|
|
|
Vector3 deltaPos = velocity * Time.fixedDeltaTime;
|
|
Vector3 deltaRot = angularVelocity * Time.fixedDeltaTime * Mathf.Rad2Deg;
|
|
|
|
position += deltaPos;
|
|
rotation *= Quaternion.Euler(deltaRot);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the center of mass of a puppet.
|
|
/// </summary>
|
|
public static Vector3 GetCenterOfMass(PuppetMaster puppet) {
|
|
Vector3 CoM = Vector3.zero;
|
|
float c = 0f;
|
|
|
|
for (int i = 0; i < puppet.muscles.Length; i++) {
|
|
if (puppet.muscles[i].joint.gameObject.activeInHierarchy) {
|
|
CoM += puppet.muscles[i].rigidbody.worldCenterOfMass * puppet.muscles[i].rigidbody.mass;
|
|
|
|
c += puppet.muscles[i].rigidbody.mass;
|
|
}
|
|
}
|
|
|
|
return CoM / c;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the center of mass of an array of Rigidbodies.
|
|
/// </summary>
|
|
public static Vector3 GetCenterOfMass(Rigidbody[] rigidbodies) {
|
|
Vector3 CoM = Vector3.zero;
|
|
float c = 0f;
|
|
|
|
for (int i = 0; i < rigidbodies.Length; i++) {
|
|
if (rigidbodies[i].gameObject.activeInHierarchy) {
|
|
CoM += rigidbodies[i].worldCenterOfMass * rigidbodies[i].mass;
|
|
|
|
c += rigidbodies[i].mass;
|
|
}
|
|
}
|
|
|
|
return CoM / c;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the velocity of the center of mass of an array of Rigidbodies.
|
|
/// </summary>
|
|
public static Vector3 GetCenterOfMassVelocity(Rigidbody[] rigidbodies) {
|
|
Vector3 CoM = Vector3.zero;
|
|
float c = 0f;
|
|
|
|
for (int i = 0; i < rigidbodies.Length; i++) {
|
|
if (rigidbodies[i].gameObject.activeInHierarchy) {
|
|
CoM += rigidbodies[i].velocity * rigidbodies[i].mass;
|
|
|
|
c += rigidbodies[i].mass;
|
|
}
|
|
}
|
|
|
|
return CoM / c;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Divides an angular acceleration by an inertia tensor.
|
|
/// </summary>
|
|
public static void DivByInertia(ref Vector3 v, Quaternion rotation, Vector3 inertiaTensor) {
|
|
v = rotation * Div(Quaternion.Inverse(rotation) * v, inertiaTensor);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scales an angular acceleration by an inertia tensor
|
|
/// </summary>
|
|
public static void ScaleByInertia(ref Vector3 v, Quaternion rotation, Vector3 inertiaTensor) {
|
|
v = rotation * Vector3.Scale(Quaternion.Inverse(rotation) * v, inertiaTensor);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the angular acceleration from one vector to another.
|
|
/// </summary>
|
|
public static Vector3 GetFromToAcceleration(Vector3 fromV, Vector3 toV) {
|
|
Quaternion fromTo = Quaternion.FromToRotation(fromV, toV);
|
|
float requiredAccelerationDeg = 0f;
|
|
Vector3 axis = Vector3.zero;
|
|
fromTo.ToAngleAxis(out requiredAccelerationDeg, out axis);
|
|
|
|
Vector3 requiredAcceleration = requiredAccelerationDeg * axis * Mathf.Deg2Rad;
|
|
|
|
return requiredAcceleration / Time.fixedDeltaTime;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the angular acceleration from "fromR" to "toR."
|
|
/// Does not guarantee full accuracy with rotations around multiple axes).
|
|
/// </summary>
|
|
public static Vector3 GetAngularAcceleration(Quaternion fromR, Quaternion toR) {
|
|
Vector3 axis = Vector3.Cross(fromR * Vector3.forward, toR * Vector3.forward);
|
|
Vector3 axis2 = Vector3.Cross(fromR * Vector3.up, toR * Vector3.up);
|
|
float angle = Quaternion.Angle(fromR, toR);
|
|
Vector3 acc = Vector3.Normalize(axis + axis2) * angle * Mathf.Deg2Rad;
|
|
|
|
return acc / Time.fixedDeltaTime;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds torque to the Ridigbody that accelerates it from it's current rotation to another using any force mode.
|
|
/// </summary>
|
|
public static void AddFromToTorque(Rigidbody r, Quaternion toR, ForceMode forceMode) {
|
|
Vector3 requiredAcceleration = GetAngularAcceleration(r.rotation, toR); // Acceleration required for a single solver step
|
|
requiredAcceleration -= r.angularVelocity; // Compensate for angular velocity
|
|
|
|
switch (forceMode) {
|
|
case ForceMode.Acceleration:
|
|
r.AddTorque(requiredAcceleration / Time.fixedDeltaTime, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Force:
|
|
Vector3 force = requiredAcceleration / Time.fixedDeltaTime;
|
|
ScaleByInertia(ref force, r.rotation, r.inertiaTensor);
|
|
r.AddTorque(force, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Impulse:
|
|
Vector3 impulse = requiredAcceleration;
|
|
ScaleByInertia(ref impulse, r.rotation, r.inertiaTensor);
|
|
r.AddTorque(impulse, forceMode);
|
|
break;
|
|
|
|
case ForceMode.VelocityChange:
|
|
r.AddTorque(requiredAcceleration, forceMode);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds torque to the Ridigbody that accelerates it from one direction to another using any force mode.
|
|
/// </summary>
|
|
public static void AddFromToTorque(Rigidbody r, Vector3 fromV, Vector3 toV, ForceMode forceMode) {
|
|
Vector3 requiredAcceleration = GetFromToAcceleration(fromV, toV); // Acceleration required for a single solver step
|
|
requiredAcceleration -= r.angularVelocity; // Compensate for angular velocity
|
|
|
|
switch(forceMode) {
|
|
case ForceMode.Acceleration:
|
|
r.AddTorque(requiredAcceleration / Time.fixedDeltaTime, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Force:
|
|
Vector3 force = requiredAcceleration / Time.fixedDeltaTime;
|
|
ScaleByInertia(ref force, r.rotation, r.inertiaTensor);
|
|
r.AddTorque(force, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Impulse:
|
|
Vector3 impulse = requiredAcceleration;
|
|
ScaleByInertia(ref impulse, r.rotation, r.inertiaTensor);
|
|
r.AddTorque(impulse, forceMode);
|
|
break;
|
|
|
|
case ForceMode.VelocityChange:
|
|
r.AddTorque(requiredAcceleration, forceMode);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a force to a Rigidbody that gets it from one place to another within a single simulation step using any force mode.
|
|
/// </summary>
|
|
public static void AddFromToForce(Rigidbody r, Vector3 fromV, Vector3 toV, ForceMode forceMode) {
|
|
Vector3 requiredAcceleration = GetLinearAcceleration(fromV, toV);
|
|
requiredAcceleration -= r.velocity;
|
|
|
|
switch(forceMode) {
|
|
case ForceMode.Acceleration:
|
|
r.AddForce(requiredAcceleration / Time.fixedDeltaTime, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Force:
|
|
Vector3 force = requiredAcceleration / Time.fixedDeltaTime;
|
|
force *= r.mass;
|
|
r.AddForce(force, forceMode);
|
|
break;
|
|
|
|
case ForceMode.Impulse:
|
|
Vector3 impulse = requiredAcceleration;
|
|
impulse *= r.mass;
|
|
r.AddForce(impulse, forceMode);
|
|
break;
|
|
|
|
case ForceMode.VelocityChange:
|
|
r.AddForce(requiredAcceleration, forceMode);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the linear acceleration from one point to another.
|
|
/// </summary>
|
|
public static Vector3 GetLinearAcceleration(Vector3 fromPoint, Vector3 toPoint) {
|
|
return (toPoint - fromPoint) / Time.fixedDeltaTime;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The rotation expressed by the joint's axis and secondary axis
|
|
/// </summary>
|
|
public static Quaternion ToJointSpace(ConfigurableJoint joint) {
|
|
Vector3 forward = Vector3.Cross (joint.axis, joint.secondaryAxis);
|
|
Vector3 up = Vector3.Cross (forward, joint.axis);
|
|
return Quaternion.LookRotation (forward, up);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the inertia tensor for a cuboid.
|
|
/// </summary>
|
|
public static Vector3 CalculateInertiaTensorCuboid(Vector3 size, float mass) {
|
|
float x2 = size.x * size.x;
|
|
float y2 = size.y * size.y;
|
|
float z2 = size.z * size.z;
|
|
|
|
float mlp = 1f/12f * mass;
|
|
|
|
return new Vector3(
|
|
mlp * (y2 + z2),
|
|
mlp * (x2 + z2),
|
|
mlp * (x2 + y2));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Divide all the values in v by their respective values in v2.
|
|
/// </summary>
|
|
public static Vector3 Div(Vector3 v, Vector3 v2) {
|
|
return new Vector3(v.x / v2.x, v.y / v2.y, v.z / v2.z);
|
|
}
|
|
}
|
|
}
|