实验一主要实现简单的刚体动画模拟(一只兔子)，包括 impulse 的碰撞检测与响应，以及 Shape Matching方法。
完整项目已上传至github。
文章目录
简单刚体模拟(不考虑碰撞)
物体在场景中的位置由其平移矩阵，以及旋转矩阵决定（刚体不能形变，所以不需要缩放矩阵）。
平移运动
对于平移运动，状态变量(state variable) 包括位置 x 和 速度 v，其正常更新如下
但这个积分没法在计算机内直接求，可以使用数值积分的方法进行运算，比如显式积分和隐式积分
- 显式积分
- 隐式积分
这里我们采用 Leapfrog Integration 方法，其在速度的更新上采用显式积分，而在位置的更新上采用隐式积分。
那么在仿真过程，平移运动的更新过程如下，这里的 △ t \triangle t △t 为用户指定的变量。
在 lab1 中的对应代码如下，这里只考虑了重力，linear_decay表示速度的衰减系数，比如空气阻力等。
// Part I: Update velocities
v += dt * gravity;
v *= linear_decay;
//Update linear status
Vector3 x = x_0 + dt * v;
旋转运动
旋转运动在仿真中一般使用矩阵、欧拉角以及四元数表示，对于矩阵与欧拉角都有一定的缺点，这里主要使用四元数(当然它们能互相转换)。
我们选择四元数 q 表示朝向，即从局部坐标转化为全局坐标。再使用一个3D向量 ω ⃗ \vec{ \omega } ω 表示角速度(angular velocity)。
- ω ⃗ \vec{ \omega } ω 的方向为旋转轴的方向
- ω ⃗ \vec{ \omega } ω 的大小为角速度的大小
这里引入力矩，其表示如下。与力的作用会改变平移速度相同，力矩会改变角速度。
我们知道动量表示为
P
⃗
=
m
v
⃗
\vec {P}=m \vec{v}
P=mv，而角动量表示为
L
⃗
=
I
×
ω
⃗
\vec{L} = I \times \vec{ \omega }
L=I×ω 这里的
I
I
I 为一个3x3的矩阵，称为惯性张量，其描述了物体中的相对于质心的的质量分布，其可以如下表示。我们把刚体看作很多粒子(质点)组成的质点系，这里的
r
i
r_i
ri 表示该粒子(质点)在局部系中的位置，
m
i
m_i
mi 表示该质点的质量，
R
R
R 表示旋转矩阵，即将局部系转为全局系的矩阵，其可以由四元数q转换得到。这里先求出一个
I
r
e
f
I_{ref}
Iref 的目的是其可以在仿真开始前就计算出来，仿真过程中只需要计算
I
=
R
I
r
e
f
R
T
I = RI_{ref}R^T
I=RIrefRT 即可，减小计算开销。
与
P
⃗
\vec {P}
P 的微分表示力类似，
L
⃗
\vec{L}
L 的微分表示力矩
τ
\tau
τ。且
L
⃗
\vec{L}
L 的微分也可表示为
I
×
β
⃗
I \times \vec{\beta}
I×β 的形式，这里的
β
⃗
\vec{\beta}
β 表示角加速度。那么仿真过程中角速度的更新如下
由于lab1中只考虑了重力，所以其力矩和角速度是不需要更新的，当然碰撞之后对于的角速度要更新。
2024/3/13更新，这里为了方便理解，可以把角速度=>速度、力矩=>力、惯性张量=>质量、角动量=>动量，那么再看上面的公式就很好理解了，这里为什么 I r e f I_{ref} Iref 要左右乘上一个 R T R^T RT和 R R R，首先旋转矩阵是单位正交矩阵，那么 R T = R − 1 R^T=R^{-1} RT=R−1，那么要在更新角速度的公式中，力矩是在世界坐标系下的，那么如果要将参考系下的惯性张量 I r e f I_{ref} Iref应用，首先得变换到参考系，所以坐成一个 R − 1 R^{-1} R−1，即 R T R^T RT。这样得到的是在参考系下的角加速度，那么还要转换回世界坐标系下，就要再左乘矩阵 R R R。
详细推导角速度更新四元数的公式可以看课后材料的2.3部分和Appendix B
粒子碰撞检测与响应
碰撞检测
我们可以使用
ϕ
(
x
⃗
)
\phi(\vec{x})
ϕ(x) 定义位于
x
⃗
\vec{x}
x位置的粒子到一个曲面的有向距离，其符号由粒子位于曲面的内侧还是外侧确定。
常见的
ϕ
(
x
⃗
)
\phi(\vec{x})
ϕ(x) 的实例
碰撞响应
Penalty Methods
Quadratic Penalty Method
一个 penalty 方法会在下一次更新中应用一次穿透力，即这次更新穿透还是发生了，我们在下一次更新时进行纠正。
Quadratic Penalty Method with a Buffer
上一个方法的问题在于穿透已经实际产生了，我们可以加一个缓冲区，在穿透还未发生时阻止其产生。需要注意的 k 的大小设置很重要，如果 k 太小穿透仍会发生，而 k 过大会产生 overshooting 问题，即受到的穿透力过大导致物体飞出去。
Log-Barrier Penalty Method
使用log-barrier penalty 能确保力足够大，但是仍存在overshooting问题，且需要保证
ϕ
(
x
⃗
)
<
0
\phi(\vec{x})<0
ϕ(x)<0 不会发生，否则会导致发生穿透越陷越深。
Impulse Method
Impulse 方法假设碰撞会立即改变位置与速度
我们可以把当前速度看成与碰撞平面法向量相同的法向速度
v
N
⃗
\vec{v_N}
vN，以及相切的切向速度
v
T
⃗
\vec{v_T}
vT。对于法向速度，我们要将其反向且乘上一个摩擦系数，因为会有损耗，切向速度也要乘上摩擦系数。
而摩擦系数与新速度和旧速度之间的关系要满足 Coulomb’s law，因此 a 可以如上图求出。
刚体碰撞检测与响应
碰撞检测
与粒子的碰撞检测相同，刚体可以看作很多粒子组成的系统，那么我们就对所有粒子进行粒子的碰撞检测。
Impulse方法的碰撞响应
我们可以按照粒子的 Impulse 方法的碰撞响应来进行，但是我们不能直接对速度进行更新(按照老师的话其只是一个中间变量)，因为我们对于速度的更新是分为平移速度和角速度的，我们要想办法转为对这两个量的更新。
向量叉乘可以转化为对应的矩阵乘向量
其整体流程如下
需要注意的是，我们要对所有碰撞的点求平均位置与平均速度来进行后续计算。这里不直接更新位置的原因是，这个问题是非线性的，之后会约束的章节讨论。
作业代码与提高项
Unity 2021.3.21f1c1
作业代码
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); // 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 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>
void Collision_Impulse(Vector3 P, Vector3 N)
{
Mesh mesh = GetComponent<MeshFilter>().mesh;
Vector3[] vertices = mesh.vertices;
Matrix4x4 R = Matrix4x4.Rotate(transform.rotation); // rotation matrix
Vector3 T = transform.position; // translation vector
Vector3 sum = new Vector3(0, 0, 0);
int collisionNum = 0; // number of collision
for (int i = 0; i < vertices.Length; i++)
{
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) // collision occur
{
Vector3 v_i = v + Vector3.Cross(w, Rri);
float v_N_size = Vector3.Dot(v_i, N);
// check velocity
if (v_N_size < 0.0f)
{
sum += r_i;
collisionNum++;
}
}
}
if (collisionNum == 0) return;
Matrix4x4 I_rot = R * I_ref * R.transpose;
Matrix4x4 I_inverse = I_rot.inverse;
Vector3 r_collision = sum / (float)collisionNum; // virtual collision point（local coordination）
Vector3 Rr_collision = R.MultiplyVector(r_collision);
//Vector3 x_collision = T + Rr_collision; // virtual collision point（global coordination）
Vector3 v_collision = v + Vector3.Cross(w, Rr_collision);
// Compute the wanted v_N
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;
// Compute the impulse 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);
// Update v and w with impulse J
v = v + 1.0f / mass * J;
w = w + I_inverse.MultiplyVector(Vector3.Cross(Rr_collision, J));
}
// Update is called once per frame
void Update()
{
//Game Control
if (Input.GetKey("r"))
{
// return initial state
transform.position = new Vector3(0, 0.6f, 0);
transform.eulerAngles = new Vector3(80, 0, 0);
restitution = 0.5f;
launched = false;
Debug.Log("return to origin");
}
if (Input.GetKey("l"))
{
v = new Vector3(5, 2, 0);
w = new Vector3(0, 1, 0);
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));
// Part III: Update position & orientation
Vector3 x_0 = transform.position;
Quaternion q_0 = transform.rotation;
//Update linear status
Vector3 x = x_0 + dt * v;
//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);
// Part IV: Assign to the object
transform.position = x;
transform.rotation = q;
}
}
}
运行结果
提高项
Shape Matching的思想是将刚体看作粒子系统，针对每个粒子有其自己的位置与速度，分别对每个粒子进行更新。之后对更新后的粒子，使用刚体的约束进行修复。这里的刚体约束就是每个粒子的位置，由质心坐标与其相对于质心的矢量以及旋转矩阵R有关，即
x
i
⃗
=
c
⃗
+
R
r
i
⃗
\vec{x_i}=\vec{c}+R\vec{r_i}
xi=c+Rri。要让约束后的位置和初始位置
y
i
y_i
yi 尽可能接近。
其更新过程如下
对于粒子的碰撞响应，采用之前impulse的方法，但是直接更新其法向速度和切向速度。
代码如下
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
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, -9.8f, 0);
float linear_decay = 0.999f; // for velocity decay
float restitution = 5.0f; // for collision
float friction = 0.5f;
// Start is called before the first frame update
void Start()
{
Mesh mesh = GetComponent<MeshFilter>().mesh;
V = new Vector3[mesh.vertices.Length];
Y = mesh.vertices;
X = mesh.vertices;
Q = mesh.vertices;
//Centerizing Q.
Vector3 c = Vector3.zero;
for (int i = 0; i < Q.Length; i++)
c += Q[i];
c /= Q.Length;
//Debug.Log(c);
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;
//Debug.Log(transform.position);
// transform X from local coordination to global coordination
Update_Mesh(transform.position, Matrix4x4.Rotate(transform.rotation), 0);
transform.position = Vector3.zero;
transform.rotation = Quaternion.identity;
//Debug.Log(transform.position);
}
// 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 Collision(float inv_dt)
{
Vector3 P = new Vector3(0, 0.01f, 0);
Vector3 N = new Vector3(0, 1, 0);
// check collision with ground
for (int i = 0; i < X.Length; i++)
{
float d = Vector3.Dot(X[i] - P, N);
if (d < 0.0f) // collision occur
{
float v_N_size = Vector3.Dot(V[i], N);
// check velocity
if (v_N_size < 0.0f)
{
Vector3 v_N = v_N_size * N;
Vector3 v_T = V[i] - 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;
V[i] = v_N_new + v_T_new;
}
}
}
P = new Vector3(2.01f, 0, 0);
N = new Vector3(-1, 0, 0);
// check collsion with wall
for (int i = 0; i < X.Length; i++)
{
float d = Vector3.Dot(X[i] - P, N);
if (d < 0.0f) // collision occur
{
float v_N_size = Vector3.Dot(V[i], N);
// check velocity
if (v_N_size < 0.0f)
{
Vector3 v_N = v_N_size * N;
Vector3 v_T = V[i] - 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;
V[i] = v_N_new + v_T_new;
}
}
}
}
Matrix4x4 Vector3_mul_Vector3T(Vector3 v1, Vector3 v2)
{
Matrix4x4 res = Matrix4x4.zero;
res[3, 3] = 1.0f;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
{
res[i, j] = v1[i] * v2[j];
}
return res;
}
Matrix4x4 Matrix4x4_add_Matrix4x4(Matrix4x4 m1, Matrix4x4 m2)
{
Matrix4x4 res = Matrix4x4.zero;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
{
res[i, j] = m1[i, j] + m2[i, j];
}
return res;
}
// Update is called once per frame
void Update()
{
//Game Control
if (Input.GetKey("r"))
{
// return initial state
launched = false;
for (int i = 0; i < V.Length; i++)
{
V[i] = new Vector3(4.0f, 0.0f, 0.0f);
}
Update_Mesh(new Vector3(0, 0.6f, 0),
Matrix4x4.Rotate(Quaternion.Euler(new Vector3(80, 0, 0))), 0);
Debug.Log("return to origin");
}
if (Input.GetKey("l"))
{
launched = true;
for (int i = 0; i < V.Length; i++)
{
V[i] = new Vector3(5.0f, 2.0f, 0.0f);
}
}
if (!launched)
return;
float dt = 0.015f;
//Step 1: run a simple particle system.
for (int i = 0; i < V.Length; i++)
{
V[i] = V[i] + dt * gravity;
V[i] = V[i] * linear_decay;
}
//Step 2: Perform simple particle collision.
Collision(1 / dt);
// Step 3: Use shape matching to get new translation c and
// new rotation R. Update the mesh by c and R.
//Shape Matching (translation)
for (int i = 0; i < V.Length; i++)
{
Y[i] = X[i] + V[i] * dt;
}
Vector3 c = Vector3.zero;
for (int i = 0; i < Y.Length; i++)
{
c = c + Y[i];
}
c = c / Y.Length;
//Shape Matching (rotation)
Matrix4x4 A = Matrix4x4.zero;
for (int i = 0; i < Y.Length; i++)
{
A = Matrix4x4_add_Matrix4x4(A, Vector3_mul_Vector3T(Y[i] - c, Q[i]));
}
A[3, 3] = 1.0f;
A = A * QQt.inverse;
Matrix4x4 R = Get_Rotation(A);
//Update_Mesh(c, R, 1/dt);
Update_Mesh(c, R, 1 / dt);
}
}
运行结果
1904
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
