刚体模拟的基本原理
在刚体模拟的过程中，这里只考虑最简单的情况，不考虑破碎等。需要改变的量为位置信息X和旋转信息Q。利用牛顿第二运动定律计算出下一秒的X，Q。
位置X和速度V
为了维持模拟系统的稳定，一般在模拟的过程中采用蛙跳算法，模拟过程如下
旋转X和角速度W
在碰撞的过程中，还可能会发生旋转，这就需要旋转X和角速度W，模拟的过程如下
碰撞过程
物理的碰撞过程较为复杂，因为这里使用三角形的网络，先使用最简单的点面模型。设平面的法向量为n,平面上有一点p,需要判断的点为x。由点到直线的距离设为：
冲量法
物理在经过碰撞之后，会得到碰撞点的速度和位置信息，但是这些信息不是刚体的位置和速度信息，需要映射到刚体的体系之中，这里采用冲量法。首先刚体有2个速度，分别是速度和角速度，先映射到一个速度之上：
补充信息
本文只是简述刚体模拟的最基本的流程，还有很多的东西，例如速度的分解，旋转矩阵，惯性矩阵都没有解释，还有惯性矩阵怎么通过旋转矩阵变化得到。还有刚体基本信息等。需要自己了解。
games103作业一代码
using UnityEngine;
using System.Collections;
using System;
public class Rigid_Bunny : MonoBehaviour
{
bool launched = false;
float dt = 0.015f;
Vector3 v = new Vector3(0, 0, 0); // 速度(控制物体移动)
Vector3 w = new Vector3(0, 0, 0); // 角速度(控制物质旋转)
float mass; //质量
Matrix4x4 I_ref; //惯性矩阵
float linear_decay = 0.999f; //速度衰减
float angular_decay = 0.98f; //角速度衰减
float restitution = 0.5f; //弹性系数
float friction = 0.2f; // 摩擦系数
Vector3 gravity = 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;//diag = mv^2
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)//得到向量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;
}
private Quaternion Add(Quaternion a, Quaternion b)
{
a.x += b.x;
a.y += b.y;
a.z += b.z;
a.w += b.w;
return a;
}
private Matrix4x4 Matrix_subtraction(Matrix4x4 a, Matrix4x4 b)
{
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
a[i, j] -= b[i, j];
}
}
return a;
}
private Matrix4x4 Matrix_miltiply_float(Matrix4x4 a, float b)
{
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
a[i, j] *= b;
}
}
return a;
}
// In this function, update v and w by the impulse due to the collision with
//a plane <P, N>
//P 为该平面上的一个点，N为该平面的法线
void Collision_Impulse(Vector3 P, Vector3 N)
{
//1.获取物体的每一个顶点(局部坐标)
Mesh mesh = GetComponent<MeshFilter>().mesh;
Vector3[] vertices = mesh.vertices;
//2.得到每一个顶点的全局坐标旋转矩阵R,和平移向量
Matrix4x4 R = Matrix4x4.Rotate(transform.rotation); //旋转矩阵
Vector3 T = transform.position; //平移向量
Vector3 sum = new Vector3(0, 0, 0); //碰撞点
int collisionNum = 0; //碰撞点数量
for (int i = 0; i < vertices.Length; i++)
{
//3.计算每个顶点到该表面的距离d
Vector3 r_i = vertices[i];
Vector3 Rri = R.MultiplyVector(r_i);
Vector3 x_i = T + Rri;
float d = Vector3.Dot(x_i - P, N);
if (d < 0.0f)//发生碰撞(只有当平面为无限大平面时才能这样判断，否则还要判断碰撞点是否在物体上)
{
//4.将该点移动到距离表面最近的点。?????
//x_i -= d * N
//5.判断物体是否仍向墙体内部运动
Vector3 v_i = v + Vector3.Cross(w, Rri);
float v_N_size = Vector3.Dot(v_i, N);
if (v_N_size < 0.0f)
{
sum += r_i;
collisionNum++;
}
}
}
//Debug.LogFormat("共有{0}个点", collisionNum);
if (collisionNum == 0) return;
Matrix4x4 I_rot = R * I_ref * Matrix4x4.Transpose(R);//惯性张量（全局）
Matrix4x4 I_inverse = Matrix4x4.Inverse(I_rot); //惯性张量的逆（全局）
Vector3 r_collision = sum / collisionNum; //虚拟碰撞点（局部坐标）
Vector3 Rr_collision = R.MultiplyVector(r_collision);
//Vector3 x_collision = T + Rr_collision; //虚拟碰撞点（全局坐标）
Vector3 v_collision = v + Vector3.Cross(w, Rr_collision);
//6.如果物体仍向墙体内部运动,修改为新速度
Vector3 v_N = Vector3.Dot(v_collision, N) * N;
Vector3 v_T = v_collision - v_N;
Vector3 v_N_new = -1.0f * restitution * v_N;
float a = Math.Max(1.0f - friction * (1.0f + restitution) * v_N.magnitude / v_T.magnitude, 0.0f);
Vector3 v_T_new = a * v_T;
Vector3 v_new = v_N_new + v_T_new;
//7.通过新速度量计算冲量J
Matrix4x4 Rri_star = Get_Cross_Matrix(Rr_collision);
Matrix4x4 K = Matrix_subtraction(Matrix_miltiply_float(Matrix4x4.identity, 1.0f / mass),
Rri_star * I_inverse * Rri_star);
Vector3 J = K.inverse.MultiplyVector(v_new - v_collision);
//8.计算dv,dw;
v += 1.0f / mass * J;
w += I_inverse.MultiplyVector(Vector3.Cross(Rr_collision, J));
//9.通过冲量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;
Debug.Log("返回原点");
}
if (Input.GetKey("l"))
{
v = new Vector3(5, 2, 0);
w = new Vector3(0, 1, 0);
Debug.Log("开始运动");
launched = true;
}
if (launched)
{
// Part I: Update velocities
v += dt * gravity;
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)); //碰撞操作更新速度和角速度V,W
// Part III: Update position & orientation
Vector3 x_0 = transform.position;
Quaternion q_0 = transform.rotation; //上一帧的旋转角度和位置
//Update linear status
Vector3 x = x_0 + dt * v; //蛙跳计算新的位置 x1 = x0+v1*t
//Update angular status
Vector3 dw = 0.5f * dt * w;
Quaternion qw = new Quaternion(dw.x, dw.y, dw.z, 0.0f);
Quaternion q = Add(q_0, qw * q_0); //q1 = q0+(w1/2*t)*q0
// Part IV: Assign to the object
transform.position = x;
transform.rotation = q;
}
}
}
