刚体模拟动画入门

欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。

本文模拟的是刚体碰撞

基于物理的运动模拟

刚体运动的分解

由于刚体本身不会形变。
从物理角度，我们可以把刚体运动分为平移速度v和旋转速度w。
随着时间推移，物体的中心坐标X会跟随速度V发生变化。

x = x+t*v.

旋转姿态也是同理，
会随着时间推移，物体的旋转姿态R会跟随速度w发生变化。

R = R+t*w.

物理模拟过程

上述过程是在没有外力的情况下，实际情况会受到重力，空气阻力，还有碰撞力的影响。

对于加速度以及速度的模拟如下。

先物体受到的力进行计算f是合力。
合力/M就是加速度，由加速度得到新速度，由速度得到位移。

对于旋转，需要考虑转动惯量。

$R是旋转矩阵，{\tau }_i是某一点受到的力矩，\tau 是总力矩$
$I是转动惯量，(I{)}^{-1}\tau 是角速度的加速度，\omega 是角速度$
$q是最终物体转动的结果$

利用冲量处理碰撞

碰撞时点速度计算

冲量与点速度的关系

证明如下：

处理碰撞过程

算法实现

关键步骤

正常过程：
根据速度更新位移。
根据角速度更新旋转。

碰撞过程：
根据碰撞计算出点速度。
根据点速度计算出受到的冲量J.
根据冲量计算出速度与角速度。

代码

代码库：https://github.com/LightningBilly/ACMAlgorithms/tree/master/图形学算法/模拟算法/刚体模拟/

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.98f;				// for velocity decay
	float angular_decay	= 0.5f;				
	float restitutionN 	= 0.5f;					// for collision
	public float restitutionT	= 0.5f;
	private float collision_offset = 0.03f;
	Matrix4x4 rotation_matrix; // 存储当前旋转矩阵
	private Vector3[] vertices;

	// Use this for initialization
	void Start () 
	{		
		Mesh mesh = GetComponent<MeshFilter>().mesh;
		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;
	}
	
	Quaternion Add_Quaternion(Quaternion q1, Quaternion q2)
	{
		return new Quaternion(
			q1.x + q2.x,
			q1.y + q2.y,
			q1.z + q2.z,
			q1.w + q2.w);
	}

	Matrix4x4 Minus_Martix(Matrix4x4 m1, Matrix4x4 m2)
	{
		return new Matrix4x4(
			m1.GetColumn(0) - m2.GetColumn(0),
			m1.GetColumn(1) - m2.GetColumn(1),
			m1.GetColumn(2) - m2.GetColumn(2),
			m1.GetColumn(3) - m2.GetColumn(3));
	}

	Vector3Int Devide_Vector(Vector3 l, Vector3 r)
	{
		return new Vector3Int((int)(l.x / r.x), (int)(l.y / r.y), (int)(l.z / r.z));
	}

	void Impulse_Response(Vector3 collisionPosition, Vector3 collisionNormal)
	{
		Vector3 ri = rotation_matrix.MultiplyVector(collisionPosition);
		Vector3 collisionVertexVelocity = v + Vector3.Cross(w, ri); // 计算点速度
		float collisionVertexVelocityNSize = Vector3.Dot(collisionVertexVelocity, collisionNormal); // 查看点速度与法向方向是否相反
		if (collisionVertexVelocityNSize >= 0) return; // 相同，不会发生碰撞返回
		
		Vector3 collisionVertexVelocityN = collisionNormal * collisionVertexVelocityNSize; // 计算垂直速度
		Vector3 collisionVertexVelocityT = collisionVertexVelocity - collisionVertexVelocityN; // 计算摩擦速度
		
		float a = Mathf.Max(1 - restitutionT * (1 + restitutionN) * collisionVertexVelocityN.magnitude / collisionVertexVelocityT.magnitude, 0);
		collisionVertexVelocityN *= -restitutionN;
		collisionVertexVelocityT *= a;
		
		float massInverse = 1.0f / mass;
		Matrix4x4 massInverseIdentity = new Matrix4x4(
			new Vector4(massInverse, 0, 0, 0),
			new Vector4(0, massInverse, 0, 0),
			new Vector4(0, 0, massInverse, 0),
			new Vector4(0, 0, 0, massInverse)
		);
		
		Matrix4x4 inertiaInverse = (rotation_matrix * I_ref * rotation_matrix.transpose).inverse;
		Matrix4x4 riRotateMartix = Get_Cross_Matrix(ri);
		Matrix4x4 K = Minus_Martix(massInverseIdentity, riRotateMartix * inertiaInverse * riRotateMartix);
		Vector3 J = K.inverse.MultiplyVector(collisionVertexVelocityN + collisionVertexVelocityT - collisionVertexVelocity);
		v += massInverse * J;
		w += inertiaInverse.MultiplyVector(Vector3.Cross(ri, J));
		// linear_decay *= 0.0001f;
		// angular_decay *= 0.001f;
	}
	
	// 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)
	{
		Vector3 positonWithOffset = P + collision_offset * N;
		Vector3 collisionVertexAveragePosition = new Vector3();
		uint collisionVertexNum = 0;
		// 将所有碰撞点取平均值作为碰撞点。
		foreach (var r in vertices)
		{
			Vector3 vertexPosition = transform.position + rotation_matrix.MultiplyVector(r);
			if(Vector3.Dot(vertexPosition - positonWithOffset, N) < 0)
			{
				collisionVertexAveragePosition += r;
				collisionVertexNum++;
			}
		}
		if (collisionVertexNum == 0) return;
		collisionVertexAveragePosition /= collisionVertexNum;
		print("coll");
		// 计算碰撞后的速度与角速度
		Impulse_Response(collisionVertexAveragePosition, N);
	}

	// Update is called once per frame
	void Update () 
	{
		//Game Control
		if(Input.GetKey("r"))
		{
			transform.position = new Vector3 (0, 0.6f, 0);
			restitutionN = 0.5f;
			launched=false;
			print("rrrr");
		}
		if(Input.GetKey("l"))
		{
			v = new Vector3 (5, 2, 0);
			// v = new Vector3 (0, 0, 0);
			launched=true;
			print("llll");
		}

		
		if (!launched) return;
		// Part I: Update velocities
		Vector3 gravity = new Vector3(0, -9.8f, 0);
		//Vector3 gravity = new Vector3(2, 0, 0);
		v += dt * gravity; // 计算重加速度与原来速度之和
		v *= linear_decay; // 阻力
		w *= angular_decay; // 阻力
		
		if (v.magnitude < 1f) restitutionN *= 0.5f;
		

		// Part II: Collision Impulse
		rotation_matrix = Matrix4x4.Rotate(transform.rotation);
		// Collision_Impulse(new Vector3(0, 0.01f, 0), new Vector3(0, 1, 0));
		Collision_Impulse(new Vector3(0, 0, 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 += dt * v; // 计算位移
		
		
		//Update angular status
		Quaternion q = transform.rotation;
		q = Add_Quaternion(new Quaternion(w.x, w.y, w.z, 0) * q, q); //计算转动状态
		

		// Part IV: Assign to the object
		transform.position = x;
		transform.rotation = q;
	}
}

算法效果

---

本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。