第一部分
检测碰撞
首先需要对所有点进行碰撞检测,判断是否与平面发生碰撞
这里碰撞检测根据读入给定的表面点和表面的法向量求点积判断是否小于0,如果是则证明两个的方向相反,发生碰撞。
同时判断是否速度方向和法线方向相反。
当同时满足以上两个条件的时候,则证明该点发生碰撞,此时需要记录碰撞的位置,按照平均的方式求碰撞点。
Mesh mesh = GetComponent<MeshFilter>().mesh;
Vector3[] vertices = mesh.vertices;//获得所有的顶点
Vector3 x = transform.position;
Matrix4x4 R = Matrix4x4.Rotate(transform.rotation);
Vector3 collosionPoint = new Vector3(0.0f, 0.0f, 0.0f);//根据位置求平均
UInt32 totalPoint = 0;
foreach (Vector3 vertex in vertices)
{Vector3 Rr_i = R.MultiplyVector(vertex);Vector3 x_i = x + Rr_i;float d = Vector3.Dot(x_i - P, N);//判断是否发生碰撞if (d < 0.0f)//发生碰撞{Vector3 v_i = v + Get_Cross_Matrix(w).MultiplyVector(Rr_i);float direction = Vector3.Dot(v_i, N);if (direction < 0.0f){collosionPoint += vertex;totalPoint += 1;}}}
求碰撞点的新的速度
根据算法流程,求出之前的速度,然后进行速度的分解,对不同方向的速度进行处理得到新的速度。
if (totalPoint <= 0) return;
collosionPoint /= totalPoint;
Vector3 Rr_i = R.MultiplyVector(collosionPoint);
Vector3 x_i = x + Rr_i;
Vector3 v_i = v + Get_Cross_Matrix(w).MultiplyVector(Rr_i);//得到点的速度
Vector3 v_N_i = Vector3.Dot(v_i, N) * N;
Vector3 v_T_i = v_i - v_N_i;//得到切线的速度
v_N_i = -restitution * v_N_i;
float a = Math.Max(1.0f - friction * (1.0f + restitution) * v_N_i.magnitude / v_T_i.magnitude, 0.0f);
v_T_i *= a;
Vector3 v_i_new = v_T_i + v_N_i;//新的速度
计算惯性张量和K矩阵
collosionPoint /= totalPoint;
Vector3 Rr_i = R.MultiplyVector(collosionPoint);
Matrix4x4 Rr_i_p = Get_Cross_Matrix(Rr_i);
Matrix4x4 I = R * I_ref * R.transpose;
Matrix4x4 I_inverse = I.inverse;
Matrix4x4 k = Matrix_sub_matrix(Matrix_multiply_float(Matrix4x4.identity, 1.0f / mass), Rr_i_p * I_inverse * Rr_i_p);
计算冲量,然后更新速度和角速度
Vector3 x_i = x + Rr_i;
Vector3 v_i = v + Get_Cross_Matrix(w).MultiplyVector(Rr_i);//得到点的速度
Vector3 v_N_i = Vector3.Dot(v_i, N) * N;
Vector3 v_T_i = v_i - v_N_i;//得到切线的速度
v_N_i = -restitution * v_N_i;
float a = Math.Max(1.0f - friction * (1.0f + restitution) * v_N_i.magnitude / v_T_i.magnitude, 0.0f);
v_T_i *= a;
Vector3 v_i_new = v_T_i + v_N_i;//新的速度Vector3 impulse = k.inverse.MultiplyVector(v_i_new-v_i);
v = v + Vector_multiply(impulse, 1.0f / mass);
w = w + I_inverse.MultiplyVector(Rr_i_p.MultiplyVector(impulse));
更新位置和quaternion
在Unity中Quaternion为(x,y,z,w)
if (launched)
{v += gravity * dt;v *= linear_decay;w *= angular_decay;// Part II: Collision ImpulseCollision_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 & orientationtransform.position += v * dt;//Update linear statusVector3 x = transform.position;//Update angular statusQuaternion q = transform.rotation;Vector3 qw = dt / 2.0f * w;Quaternion pq = new Quaternion(qw.x, qw.y, qw.z, 0.0f);q = Add(q, pq * q);// Part IV: Assign to the objecttransform.position = x;transform.rotation = q;
}
第一部分总代码
using UnityEngine;
using System.Collections;
using System.Numerics;
using Matrix4x4 = UnityEngine.Matrix4x4;
using Vector3 = UnityEngine.Vector3;
using Quaternion = UnityEngine.Quaternion;
using UnityEngine.UIElements;
using System;public class Rigid_Bunny : MonoBehaviour
{bool launched = false;float dt = 0.015f;Vector3 v = new Vector3(0, 0, 0); // velocityVector3 w = new Vector3(0, 0, 0); // angular velocityfloat mass; // massMatrix4x4 I_ref; // reference inertiafloat linear_decay = 0.999f; // for velocity decayfloat angular_decay = 0.98f; float restitution = 0.5f; // for collisionfloat friction = 0.2f;Vector3 gravity =new Vector3(0.0f, -9.8f, 0.0f );// Use this for initializationvoid 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 aMatrix4x4 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 Matrix4x4 Matrix_multiply_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;}private Matrix4x4 Matrix_sub_matrix(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 Vector3 Vector_multiply(Vector3 a,float b){for(int i = 0; i < 3; i++){a[i] *= b;}return a;}// 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;Matrix4x4 R = Matrix4x4.Rotate(transform.rotation);Vector3 collosionPoint = new Vector3(0.0f, 0.0f, 0.0f);//根据位置求平均int totalPoint = 0;foreach (Vector3 vertex in vertices){Vector3 p_Rr_i = R.MultiplyVector(vertex);Vector3 p_x_i = x + p_Rr_i;float d = Vector3.Dot(p_x_i - P, N);//判断是否发生碰撞if (d < 0.0f)//发生碰撞{Vector3 p_v_i = v + Get_Cross_Matrix(w).MultiplyVector(p_Rr_i);float direction = Vector3.Dot(p_v_i, N);if (direction < 0.0f){collosionPoint += vertex;totalPoint += 1;}}}if (totalPoint <= 0) return;collosionPoint /= (float) totalPoint;Vector3 Rr_i = R.MultiplyVector(collosionPoint);Matrix4x4 Rr_i_p = Get_Cross_Matrix(Rr_i);Matrix4x4 I = R * I_ref * R.transpose;Matrix4x4 I_inverse = I.inverse;Matrix4x4 k = Matrix_sub_matrix(Matrix_multiply_float(Matrix4x4.identity, 1.0f / mass), Rr_i_p * I_inverse * Rr_i_p);Vector3 x_i = x + Rr_i;Vector3 v_i = v + Get_Cross_Matrix(w).MultiplyVector(Rr_i);//得到点的速度Vector3 v_N_i = Vector3.Dot(v_i, N) * N;Vector3 v_T_i = v_i - v_N_i;//得到切线的速度v_N_i = -1.0f*restitution * v_N_i;float a = Math.Max(1.0f - friction * (1.0f + restitution) * v_N_i.magnitude / v_T_i.magnitude, 0.0f);v_T_i *= a;Vector3 v_i_new = v_T_i + v_N_i;//新的速度Vector3 impulse = k.inverse.MultiplyVector(v_i_new-v_i);v = v + Vector_multiply(impulse, 1.0f / mass);w = w + I_inverse.MultiplyVector(Rr_i_p.MultiplyVector(impulse));}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;}// Update is called once per framevoid Update () {//Game Controlif(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);launched=true;}// Part I: Update velocitiesif (launched){v += gravity * dt;v *= linear_decay;w *= angular_decay;// Part II: Collision ImpulseCollision_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 & orientationtransform.position += v * dt;//Update linear statusVector3 x = transform.position;//Update angular statusQuaternion q = transform.rotation;Vector3 qw = dt / 2.0f * w;Quaternion pq = new Quaternion(qw.x, qw.y, qw.z, 0.0f);q = Add(q, pq * q);// Part IV: Assign to the objecttransform.position = x;transform.rotation = q;}}
}
但是这个地方会不断的颤抖,并且会沿着切平面方向不断的移动
于是可以设定衰减
restitution = Math.Max(restitution - 0.05f, 0.0f);
friction = Math.Max(friction - 0.05f, 0.0f);
float a = 0.0f;
if (v_T_i.magnitude > 0.0f)
{a=Math.Max(1.0f - friction * (1.0f + restitution) * v_N_i.magnitude / v_T_i.magnitude, 0.0f);
}if (friction == 0.0f && restitution == 0.0f)
{a = 0.0f;
}
效果展示
提高部分
提高部分主要就是粒子的模拟,然后重新约束的过程
解读start的代码
这里是为了将质心设定为0,0,0。因为mesh.vertices是相对于局部坐标系的每个顶点的位置,但是不是相对于质心。
Mesh mesh = GetComponent<MeshFilter>().mesh;//拿到mesh的信息
V = new Vector3[mesh.vertices.Length];
X = mesh.vertices;
Q = mesh.vertices;
Debug.Log(mesh.vertices.Length);
//Centerizing Q.
Vector3 c=Vector3.zero;
for(int i=0; i<Q.Length; i++)c+=Q[i];
c/=Q.Length;
for(int i=0; i<Q.Length; i++)Q[i]-=c;
碰撞检测
这里的碰撞检测针对单个点,就没有力矩和惯性张量。
只需要检测一下点与平面是否发生碰撞,更改速度即可
void judgeCollision(int idx,Vector3 P,Vector3 N)
{Vector3 T = transform.position;float d = Vector3.Dot(P - X[idx],N);if (d < 0.0f){float pd = Vector3.Dot(V[idx],N);if (pd < 0.0f){Vector3 v_N = Vector3.Dot(V[idx], N) * N;Vector3 v_T = V[idx] - v_N;float a = Mathf.Max(1-fricion*(1+restitution)*v_N.magnitude/v_T.magnitude,0.0f);v_N = -1.0f * restitution * v_N;v_T = a * v_T;V[idx] = v_N + v_T;}}
}
更新每个点的位置
首先更新“梯形位置”同时计算c,A参数和对应的约束旋转矩阵
void Collision(float inv_dt,float dt)
{for(int i = 0; i < Q.Length; i++){judgeCollision(i, new Vector3(0, 0.01f, 0), new Vector3(0, 1, 0));judgeCollision(i, new Vector3(2, 0, 0), new Vector3(-1, 0, 0));Y[i] = X[i] + dt * V[i];}Vector3 c = new Vector3(0.0f, 0.0f, 0.0f);for(int i = 0; i < Q.Length; i++){c += Y[i];}c /= Q.Length;Matrix4x4 A = Matrix4x4.zero;for(int i = 0; i < Q.Length; i++){Vector3 y_c = Y[i] - c;A[0, 0] += y_c[0] * Q[i][0];A[0, 1] += y_c[0] * Q[i][1];A[0, 2] += y_c[0] * Q[i][2];A[1, 0] += y_c[1] * Q[i][0];A[1, 1] += y_c[1] * Q[i][1];A[1, 2] += y_c[1] * Q[i][2];A[2, 0] += y_c[2] * Q[i][0];A[2, 1] += y_c[2] * Q[i][1];A[2, 2] += y_c[2] * Q[i][2];}A[3, 3] = 1;A = A * QQt.inverse;Matrix4x4 R = Get_Rotation(A);
}
然后更新模型,最终的代码
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Matrix4x4 = UnityEngine.Matrix4x4;
using Vector3 = UnityEngine.Vector3;
using Quaternion = UnityEngine.Quaternion;public class Rigid_Bunny_by_Shape_Matching : MonoBehaviour
{public bool launched = false;Vector3[] X;Vector3[] Q;Vector3[] V;Vector3[] Y;Matrix4x4 QQt = Matrix4x4.zero;Vector3 gravity = new Vector3(0.0f, -9.8f, 0.0f);float linear_decay = 0.999f;float restitution = 5.0f;float fricion = 0.5f;// Start is called before the first frame updatevoid Start(){Mesh mesh = GetComponent<MeshFilter>().mesh;//拿到mesh的信息V = new Vector3[mesh.vertices.Length];X = mesh.vertices;Q = mesh.vertices;Y = mesh.vertices;Debug.Log(mesh.vertices.Length);//Centerizing Q.Vector3 c=Vector3.zero;for(int i=0; i<Q.Length; i++)c+=Q[i];c/=Q.Length;for(int i=0; i<Q.Length; i++)Q[i]-=c;//Get QQ^t ready.for(int i=0; i<Q.Length; i++){QQt[0, 0]+=Q[i][0]*Q[i][0];QQt[0, 1]+=Q[i][0]*Q[i][1];QQt[0, 2]+=Q[i][0]*Q[i][2];QQt[1, 0]+=Q[i][1]*Q[i][0];QQt[1, 1]+=Q[i][1]*Q[i][1];QQt[1, 2]+=Q[i][1]*Q[i][2];QQt[2, 0]+=Q[i][2]*Q[i][0];QQt[2, 1]+=Q[i][2]*Q[i][1];QQt[2, 2]+=Q[i][2]*Q[i][2];}QQt[3, 3]=1;for(int i=0; i<X.Length; i++)V[i][0]=4.0f;Update_Mesh(transform.position, Matrix4x4.Rotate(transform.rotation), 0);transform.position= new Vector3(0.0f, 0.6f, 0.0f);transform.rotation=Quaternion.identity;}// Polar Decomposition that returns the rotation from F.Matrix4x4 Get_Rotation(Matrix4x4 F){Matrix4x4 C = Matrix4x4.zero;for(int ii=0; ii<3; ii++)for(int jj=0; jj<3; jj++)for(int kk=0; kk<3; kk++)C[ii,jj]+=F[kk,ii]*F[kk,jj];Matrix4x4 C2 = Matrix4x4.zero;for(int ii=0; ii<3; ii++)for(int jj=0; jj<3; jj++)for(int kk=0; kk<3; kk++)C2[ii,jj]+=C[ii,kk]*C[jj,kk];float det = F[0,0]*F[1,1]*F[2,2]+F[0,1]*F[1,2]*F[2,0]+F[1,0]*F[2,1]*F[0,2]-F[0,2]*F[1,1]*F[2,0]-F[0,1]*F[1,0]*F[2,2]-F[0,0]*F[1,2]*F[2,1];float I_c = C[0,0]+C[1,1]+C[2,2];float I_c2 = I_c*I_c;float II_c = 0.5f*(I_c2-C2[0,0]-C2[1,1]-C2[2,2]);float III_c = det*det;float k = I_c2-3*II_c;Matrix4x4 inv_U = Matrix4x4.zero;if(k<1e-10f){float inv_lambda=1/Mathf.Sqrt(I_c/3);inv_U[0,0]=inv_lambda;inv_U[1,1]=inv_lambda;inv_U[2,2]=inv_lambda;}else{float l = I_c*(I_c*I_c-4.5f*II_c)+13.5f*III_c;float k_root = Mathf.Sqrt(k);float value=l/(k*k_root);if(value<-1.0f) value=-1.0f;if(value> 1.0f) value= 1.0f;float phi = Mathf.Acos(value);float lambda2=(I_c+2*k_root*Mathf.Cos(phi/3))/3.0f;float lambda=Mathf.Sqrt(lambda2);float III_u = Mathf.Sqrt(III_c);if(det<0) III_u=-III_u;float I_u = lambda + Mathf.Sqrt(-lambda2 + I_c + 2*III_u/lambda);float II_u=(I_u*I_u-I_c)*0.5f;float inv_rate, factor;inv_rate=1/(I_u*II_u-III_u);factor=I_u*III_u*inv_rate;Matrix4x4 U = Matrix4x4.zero;U[0,0]=factor;U[1,1]=factor;U[2,2]=factor;factor=(I_u*I_u-II_u)*inv_rate;for(int i=0; i<3; i++)for(int j=0; j<3; j++)U[i,j]+=factor*C[i,j]-inv_rate*C2[i,j];inv_rate=1/III_u;factor=II_u*inv_rate;inv_U[0,0]=factor;inv_U[1,1]=factor;inv_U[2,2]=factor;factor=-I_u*inv_rate;for(int i=0; i<3; i++)for(int j=0; j<3; j++)inv_U[i,j]+=factor*U[i,j]+inv_rate*C[i,j];}Matrix4x4 R=Matrix4x4.zero;for(int ii=0; ii<3; ii++)for(int jj=0; jj<3; jj++)for(int kk=0; kk<3; kk++)R[ii,jj]+=F[ii,kk]*inv_U[kk,jj];R[3,3]=1;return R;}// Update the mesh vertices according to translation c and rotation R.// It also updates the velocity.void Update_Mesh(Vector3 c, Matrix4x4 R, float inv_dt){for(int i=0; i<Q.Length; i++){Vector3 x=(Vector3)(R*Q[i])+c;V[i]=(x-X[i])*inv_dt;X[i]=x;} Mesh mesh = GetComponent<MeshFilter>().mesh;mesh.vertices=X;}void judgeCollision(int idx,Vector3 P,Vector3 N){float d = Vector3.Dot(X[idx] - P,N);if (d < 0.0f){float pd = Vector3.Dot(V[idx],N);if (pd < 0.0f){Vector3 v_N = Vector3.Dot(V[idx], N) * N;Vector3 v_T = V[idx] - v_N;float a = Mathf.Max(1.0f-fricion*(1.0f+restitution)*v_N.magnitude/v_T.magnitude,0.0f);v_N = -1.0f * restitution * v_N;v_T = a * v_T;V[idx] = v_N + v_T;}}}void Collision(float inv_dt,float dt){for(int i = 0; i < Q.Length; i++){judgeCollision(i, new Vector3(0, 0.01f, 0), new Vector3(0, 1, 0));judgeCollision(i, new Vector3(2.0f, 0, 0), new Vector3(-1.0f, 0, 0));Y[i] = X[i] + dt * V[i];}Vector3 c = new Vector3(0.0f, 0.0f, 0.0f);for(int i = 0; i < Q.Length; i++){c += Y[i];}c /= Q.Length;Matrix4x4 A = Matrix4x4.zero;for(int i = 0; i < Q.Length; i++){Vector3 y_c = Y[i] - c;A[0, 0] += y_c[0] * Q[i][0];A[0, 1] += y_c[0] * Q[i][1];A[0, 2] += y_c[0] * Q[i][2];A[1, 0] += y_c[1] * Q[i][0];A[1, 1] += y_c[1] * Q[i][1];A[1, 2] += y_c[1] * Q[i][2];A[2, 0] += y_c[2] * Q[i][0];A[2, 1] += y_c[2] * Q[i][1];A[2, 2] += y_c[2] * Q[i][2];}A[3, 3] = 1;A = A * QQt.inverse;Matrix4x4 R = Get_Rotation(A);Update_Mesh(c, R, inv_dt);}// Update is called once per framevoid Update(){float dt = 0.015f;if (Input.GetKey("r")){transform.position = new Vector3(0.0f, 0.6f, 0.0f);transform.rotation = Quaternion.identity;launched = false;for (int i = 0; i < X.Length; i++)V[i][0] = 4.0f;Update_Mesh(transform.position, Matrix4x4.Rotate(transform.rotation), 0);}if (Input.GetKey("l")){launched = true;//Step 1: run a simple particle system.for (int i = 0; i < V.Length; i++){V[i] = new Vector3(5.0f, 2.0f, 0.0f);}}if (launched){for(int i = 0; i < V.Length; i++){V[i] += gravity * dt;V[i] *= linear_decay;}//Step 2: Perform simple particle collision.Collision(1/dt,dt);Debug.Log(V[0]);// Step 3: Use shape matching to get new translation c and // new rotation R. Update the mesh by c and R.//Shape Matching (translation)//Shape Matching (rotation)//Update_Mesh(c, R, 1/dt);}}
}
效果展示
个人觉得刚体用这种方式不太好