69 lines
3.3 KiB
C#
69 lines
3.3 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
|
|
namespace Obi
|
|
{
|
|
public struct ColliderRigidbody
|
|
{
|
|
public Matrix4x4 inverseInertiaTensor;
|
|
public Vector4 velocity;
|
|
public Vector4 angularVelocity;
|
|
public Vector4 com;
|
|
public float inverseMass;
|
|
|
|
public int constraintCount;
|
|
private int pad1;
|
|
private int pad2;
|
|
|
|
public void FromRigidbody(ObiRigidbody rb)
|
|
{
|
|
bool kinematic = !Application.isPlaying || rb.unityRigidbody.isKinematic || rb.kinematicForParticles;
|
|
|
|
//rotation = source.rotation;
|
|
velocity = rb.kinematicForParticles ? Vector3.zero : rb.linearVelocity + (rb.unityRigidbody.useGravity ? Physics.gravity * Time.fixedDeltaTime : Vector3.zero);
|
|
angularVelocity = rb.kinematicForParticles ? Vector3.zero : rb.angularVelocity;
|
|
|
|
// center of mass in unity is affected by local rotation and position, but not scale. We need it expressed in world space:
|
|
com = rb.unityRigidbody.position + rb.unityRigidbody.rotation * rb.unityRigidbody.centerOfMass;
|
|
|
|
Vector3 invTensor = new Vector3((rb.unityRigidbody.constraints & RigidbodyConstraints.FreezeRotationX) != 0 ? 0 : 1 / rb.unityRigidbody.inertiaTensor.x,
|
|
(rb.unityRigidbody.constraints & RigidbodyConstraints.FreezeRotationY) != 0 ? 0 : 1 / rb.unityRigidbody.inertiaTensor.y,
|
|
(rb.unityRigidbody.constraints & RigidbodyConstraints.FreezeRotationZ) != 0 ? 0 : 1 / rb.unityRigidbody.inertiaTensor.z);
|
|
|
|
// the inertia tensor is a diagonal matrix (Vector3) because it is expressed in the space generated by the principal axes of rotation (inertiaTensorRotation).
|
|
Vector3 inertiaTensor = kinematic ? Vector3.zero : invTensor;
|
|
|
|
// calculate full world space inertia matrix:
|
|
Matrix4x4 rotation = Matrix4x4.Rotate(rb.unityRigidbody.rotation * rb.unityRigidbody.inertiaTensorRotation);
|
|
inverseInertiaTensor = rotation * Matrix4x4.Scale(inertiaTensor) * rotation.transpose;
|
|
|
|
inverseMass = kinematic ? 0 : 1 / rb.unityRigidbody.mass;
|
|
|
|
}
|
|
|
|
public void FromRigidbody(ObiRigidbody2D rb)
|
|
{
|
|
|
|
bool kinematic = !Application.isPlaying || rb.unityRigidbody.isKinematic || rb.kinematicForParticles;
|
|
velocity = rb.linearVelocity;
|
|
|
|
// For some weird reason, in 2D angular velocity is measured in *degrees* per second,
|
|
// instead of radians. Seriously Unity, WTF??
|
|
angularVelocity = new Vector4(0, 0, rb.angularVelocity * Mathf.Deg2Rad, 0);
|
|
|
|
// center of mass in unity is affected by local rotation and poistion, but not scale. We need it expressed in world space:
|
|
com = rb.transform.position + rb.transform.rotation * rb.unityRigidbody.centerOfMass;
|
|
|
|
Vector3 inertiaTensor = kinematic ? Vector3.zero : new Vector3(0, 0, (rb.unityRigidbody.constraints & RigidbodyConstraints2D.FreezeRotation) != 0 ? 0 : 1 / rb.unityRigidbody.inertia);
|
|
|
|
Matrix4x4 rotation = Matrix4x4.Rotate(Quaternion.AngleAxis(rb.rotation, Vector3.forward));
|
|
inverseInertiaTensor = rotation * Matrix4x4.Scale(inertiaTensor) * rotation.transpose;
|
|
|
|
inverseMass = kinematic ? 0 : 1 / rb.unityRigidbody.mass;
|
|
|
|
}
|
|
|
|
|
|
}
|
|
}
|