cs代码如下:
using UnityEngine;
using System.Collections;
public class Rigid_Bunny : MonoBehaviour
{
bool launched = false;
float dt = 0.0015f;
Vector3 v = new Vector3(0, 0, 0); // velocity
Vector3 w = new Vector3(0, 0, 0); // angular velocity
float mass; // mass
Matrix4x4 I_ref; // reference inertia
float linear_decay = 0.999f; // for velocity decay
float angular_decay = 0.98f;
float restitution = 0.5f; // for collision
float mu_T = 0.5f;// may be coefficient of air resistance
Vector3 G = new Vector3(0.0f, -9.8f, 0.0f);
// Use this for initialization
void Start()
{
Mesh mesh = GetComponent<MeshFilter>().mesh;
Vector3[] vertices = mesh.vertices;
float m = 1;
mass = 0;
for (int i = 0; i < vertices.Length; i++)
{
mass += m;
float diag = m * vertices[i].sqrMagnitude;
I_ref[0, 0] += diag;
I_ref[1, 1] += diag;
I_ref[2, 2] += diag;
I_ref[0, 0] -= m * vertices[i][0] * vertices[i][0];
I_ref[0, 1] -= m * vertices[i][0] * vertices[i][1];
I_ref[0, 2] -= m * vertices[i][0] * vertices[i][2];
I_ref[1, 0] -= m * vertices[i][1] * vertices[i][0];
I_ref[1, 1] -= m * vertices[i][1] * vertices[i][1];
I_ref[1, 2] -= m * vertices[i][1] * vertices[i][2];
I_ref[2, 0] -= m * vertices[i][2] * vertices[i][0];
I_ref[2, 1] -= m * vertices[i][2] * vertices[i][1];
I_ref[2, 2] -= m * vertices[i][2] * vertices[i][2];
}
I_ref[3, 3] = 1;
}
Quaternion addTwoQuaternion(Quaternion q0, Quaternion q1)
{
Quaternion result = new Quaternion(q0.x + q1.x, q0.y + q1.y,
q0.z + q1.z, q0.w + q1.w);
return result;
}
Matrix4x4 Get_Cross_Matrix(Vector3 a)
{
//Get the cross product matrix of vector a
Matrix4x4 A = Matrix4x4.zero;
A[0, 0] = 0;
A[0, 1] = -a[2];
A[0, 2] = a[1];
A[1, 0] = a[2];
A[1, 1] = 0;
A[1, 2] = -a[0];
A[2, 0] = -a[1];
A[2, 1] = a[0];
A[2, 2] = 0;
A[3, 3] = 1;
return A;
}
Matrix4x4 multiplyScalar(Matrix4x4 a, float b)
{
Matrix4x4 rlt = Matrix4x4.zero;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
rlt[i, j] = a[i, j] * b;
}
}
return rlt;
}
Matrix4x4 addTwoMatrix(Matrix4x4 a, Matrix4x4 b)
{
Matrix4x4 rlt = Matrix4x4.zero;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
rlt[i, j] = a[i, j] + b[i, j];
}
}
return rlt;
}
Matrix4x4 minusTwoMatrix(Matrix4x4 a, Matrix4x4 b)
{
return addTwoMatrix(a, multiplyScalar(b, -1));
}
// In this function, update v and w by the impulse due to the collision with
//a plane <P, N>
void Collision_Impulse(Vector3 P, Vector3 N)
{
Mesh mesh = GetComponent<MeshFilter>().mesh;
Vector3[] vertices = mesh.vertices; // may be this is local position
int[] vid_collision = new int[vertices.Length];
int num_collision = 0;
Matrix4x4 R = Matrix4x4.Rotate(transform.rotation);
for (int i = 0; i < vertices.Length; i++)
{
Vector3 xi = transform.position + R.MultiplyVector(vertices[i]);
if (Vector3.Dot(xi - P, N) < 0)
{
vid_collision[num_collision] = i;
num_collision++;
}
}
if (num_collision == 0)
{
return;
}
Vector3 ri = new Vector3(0, 0, 0);
for (int i = 0; i < num_collision; i++)
{
ri += vertices[vid_collision[i]];
}
ri = ri / (float)(num_collision);
Vector3 Rri = R.MultiplyVector(ri);
Vector3 Vi = v + Vector3.Cross(w, Rri);
if (Vector3.Dot(Vi, N) > 0) return; // it may be in the state of Rebound ??
// calc Compute the wanted 𝐯_𝑖^new
Vector3 VN = Vector3.Dot(Vi, N) * N;
Vector3 VT = Vi - VN;
restitution = Mathf.Max(restitution - 0.0005f, 0); // We can decrease the restitution 𝜇_𝐍 to reduce oscillation
float a = Mathf.Max(0, 1.0f - mu_T * (1.0f + restitution)) * Vector3.Magnitude(VN) / Vector3.Magnitude(VT);
Vector3 ViNew = -1.0f * restitution * VN + a * VT;
// Compute the impulse j
Matrix4x4 I_rot = R * I_ref * Matrix4x4.Transpose(R);
Matrix4x4 I_inverse = Matrix4x4.Inverse(I_rot);
Matrix4x4 Rri_star = Get_Cross_Matrix(Rri);
Matrix4x4 K = minusTwoMatrix(multiplyScalar(Matrix4x4.identity, (1.0f / mass)),
Rri_star * I_inverse * Rri_star);
Vector3 J = K.inverse.MultiplyVector(ViNew - Vi);
// update v and w
v = v + 1 / mass * J;
w = w + I_inverse.MultiplyVector(Vector3.Cross(Rri, J));
}
// Update is called once per frame
void Update()
{
//Game Control
if (Input.GetKey("r"))
{
transform.position = new Vector3(0, 0.6f, 0);
restitution = 0.5f;
launched = false;
}
if (Input.GetKey("l"))
{
v = new Vector3(5, 2, 0);
w = new Vector3(2, 5, 0);
launched = true;
}
// Part I: Update velocities
v[1] -= 9.8f * dt;
if (launched == false) return;
v = v + dt * G;
v = v * linear_decay;
w *= angular_decay;
// Part II: Collision Impulse
Collision_Impulse(new Vector3(0, 0.01f, 0), new Vector3(0, 1, 0));
Collision_Impulse(new Vector3(2, 0, 0), new Vector3(-1, 0, 0));
// Part III: Update position & orientation
//Update linear status
Vector3 x = transform.position;
x += v * dt;
//TODO change x if launched = true
//Update angular status
Quaternion q = transform.rotation;
Quaternion wq = new Quaternion(w.x, w.y, w.z, 0);
Quaternion delt_q = wq * q;
q.x += delt_q.x * 0.5f * dt;
q.y += delt_q.y * 0.5f * dt;
q.z += delt_q.z * 0.5f * dt;
q.w += delt_q.w * 0.5f * dt;
//TODO #real part is the last element
// Part IV: Assign to the object
transform.position = x;
transform.rotation = q;
}
}