碰撞反应中的物理公式

一个刚体的碰撞系统要有碰撞检测和碰撞反应两部分，看到有好多讲碰撞检测的讲碰撞反应的到不多，于是就自己写个吧～
里面有些是理论力学的东西，大家要是不了解给我留言我会解释的。

假设有A,B两个物体碰撞。
首先根据牛顿碰撞定律： 碰撞后两物体的分离速度与碰撞前两物体的接近速度成正比，比值称为恢复系数。
即  
 Vab' * n = -e * Vab * n;    (*为向量点乘，x为向量叉乘)
e即为恢复系数。当e为1时为弹性碰撞，为0时为非弹性碰撞。n为碰撞时法线。
公式变形为 （Va' - Vb'）* n = -e * (Va - Vb) * n;     (1)

碰撞时动量的改变为 p = m * v;    
角动量的改变为 r x p = I * w;         
I是物体的惯量系数，它是一个固定的量，只与物体密度形态有关，如何得出以后再说。
r 为碰撞接触点到物体质心的向量。
由这两个公式可以得到速度改变为
v = p / m;     (2)
w = r x p / I;    (3)

接触点速度为  V = v + w x r;    公式2，3代入的

V = p / m + (r x p / I) x r;
   = p / m + r' * (p / I) * r';    (4)
   = p * ( 1/m + r' *(1/I) * r' );

其中1/m + r' *(1/I) * r'成为K矩阵。
一个碰撞接触点的数据结构包括 碰撞点坐标position, 碰撞法线normal, 碰撞点到物体的最小距离distance。
(position到物体最近点的向量为Vector，根据normal和Vector点乘的正负判断是否碰撞)
I, m为已知 可以很简单地求出K矩阵。

所以把(4)带到(1)为
[ (Va + P / Ma  + Ra' *(P/Ia)*Ra') - (Vb - P / Mb - Rb' *(P/Ib)*Rb') ] = -e *(Va -Vb) *n;
一顿乱推之后......
P = -(1+e) * (Va -Vb) *n / (Ka +Kb)
然后根据公式(2)，(3)更新A，B的线速度，角速度就完了。