[GAMES103]作业1 实现带碰撞和响应的刚体运动

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;

    }

}