Game103的作业1：刚体运动

我先mark一下，这是我写的小兔子，但是这个小兔子有点小问题，比如会不停原地碰撞什么的，做这个花了我两天了…等我以后熟悉了各种debug技巧再来看，反正基本的算法流程已经基本过了一遍了，各位读者可以参考下啊嘿嘿，发现bug欢迎留言！

using UnityEngine;
using System.Collections;

public class Rigid_Bunny : MonoBehaviour
{
	bool launched = false;
	float dt = 0.015f;
	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.1f;                   // for collision
	float fricion = 0.1f;						// for friction

	// 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;
	}

	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;
	}
	Vector3 vector4to3(Vector4 v4)
    {
		return new Vector3(v4[0], v4[1], v4[2]);
    }
	Vector4 vector3to4(Vector3 v3)
	{
		return new Vector4(v3[0], v3[1], v3[2],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;
		Vector3 x = transform.position; //object positon
		Matrix4x4 R = Matrix4x4.Rotate(transform.rotation);

		Vector3 avg_r = new Vector3(0, 0, 0);//find average collision point
		int avg_r_num = 0;//record the number of collision points
		for(int i=0;i<vertices.Length;i++)
        {
			Vector3 r = x+vector4to3(R*vector3to4(vertices[i]));
			//r = x + R.MultiplyVector(vertices[i]);	//Also can use this method
			float sdf = Vector3.Dot(r - P, N);
			if(sdf<=0&&Vector3.Dot(v,N)<0)
            {
				avg_r+=(r-x);
				avg_r_num++;
            }
        }
		if (avg_r_num == 0)
			return;//not occurr collison
		
		avg_r /= avg_r_num;
		Vector3 vi = v + Vector3.Cross(w, avg_r);
		Vector3 vni = Vector3.Dot(vi, N) * N;
		Vector3 vti = vi - vni;
		float a = 1 - fricion * (1 + restitution) * Vector3.Magnitude(vni) / Vector3.Magnitude(vti);
		vni = -restitution * vi;
		vti = a * vti;
		vi = vni + vti;
		Matrix4x4 K = Get_Cross_Matrix(R.MultiplyVector(avg_r)) * Matrix4x4.Inverse(I_ref) * Get_Cross_Matrix(R.MultiplyVector(avg_r));
		for(int i=0;i<4;i++)
			for(int j1=0;j1<4;j1++)
            {
				if (i == j1)
					K[i, j1] = 1f / mass - K[i, j1];
				else
					K[i, j1] = 0 - K[i, j1];
            }
		Vector3 j = Matrix4x4.Inverse(K).MultiplyVector(vi - (v + Vector3.Cross(w, avg_r)));
		v = v + 1f / mass * j;
		w = w + Matrix4x4.Inverse(I_ref).MultiplyVector(Vector3.Cross(R.MultiplyVector(avg_r), vector4to3(j)));
	}

	// Update is called once per frame
	void Update()
	{
		//Game Control
		if (Input.GetKey("r"))
		{
			transform.position = new Vector3(0, 0.0f, 0);
			restitution = 0.5f;
			launched = false;
		}
		if (Input.GetKey("l"))
		{
			v = new Vector3(5, 2, 0);
			launched = true;
		}
		if (launched == false)
			return;
		// Part I: Update velocities
		Vector3 g = new Vector3(0, -9.78f, 0);//gravity
		v = v * linear_decay;
		w = w * angular_decay;
		v = v + dt * g;

		// 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 = x + v * dt;
		//Update angular status
		Quaternion q = transform.rotation;
		Quaternion dq = new Quaternion(dt / 2 * w[0], dt / 2 * w[1], dt / 2 * w[2], 0);
		q= new Quaternion(q[0] + dq[0], q[1] + dq[1], q[2] + dq[2], q[3] + dq[3]); 
		// Part IV: Assign to the object
		transform.position = x;
		transform.rotation = q;
	}
}