Penalty-Based Multibody Animation（5）

        前面我们了解了基于穿透的碰撞计算可以看作是一个简谐运动的一种特殊情况，并且分析了各个参数的确定，但是仍然还有一个问题摆在我们面前。就是，我们终归是要知道两个碰撞之后的刚体的运动学参数的啊。这里的运动学参数指定的是物体的速度、角速度、加速度、角加速度。

        也就是如下的参数

         首先我们确定我们求解的目标，就是已知目前物体关于穿透的一切参数，比如说碰撞的接触点、碰撞的深度、碰撞产生的穿透力、刚体的位置等。

        然后我们定义一些参数。我们假设空间中存在K对碰撞点、n个物体。在 k 对穿透点处 ik 与 jk 表示两个发生碰撞的物体，并且定义ik < jk。同样，每队穿透点都有一个法线nk和穿透点pki表示第k对穿透点在i上那一点相对于物体i坐标系的位置。

        同样可以得到公式：

         这时候我们定义某对物体上的所有穿透点的一个向量。

         注意：rjk其中jk表示第包含第k对碰撞点的物体的索引值，所谓的rjk实际上是某个j物体质心坐标系的原点。

        定义深度向量为：

         显然，为了让两物体迅速的分开，两物体的相对方向，必然要与物体间的深度向量同向，那么我们也可以认为，两物体分离的相对速度就是 d 的导数。

        对 d 的求导可以认为是对几个刚体上某个点的速度的加减法（因为无论pi与pj都是刚体上某一点的世界坐标位置）。这里为了方便表达，构造了一个看起来十分复杂的用来描述物体间接触的矩阵。

        然后定义一个关于物体速度与角速度的向量 

        最后就可以将d的导数写成下面的形式。 

         继续求导

         根据刚体运动中局部坐标系中点的导数，是可以求得C的导数的（pki  ri都是变化的，所以rki导数不为0）

         再根据牛顿-欧拉方程可以求得

         注意这里C与穿透力的乘积主要是为了添加上穿透力带来的力矩，而对力不产生影响。

        接着就可以推导出

         然后再根据critiacl damped的的条件联立方程，得到

         将式子展开并且在排列简化就可以得到

         注意，上面的式子A有时候并不可逆，就导致方程的借不唯一。这时候我们引入singular value decomposition（SVD）的方法来计算。

        定义向量

         定义矩阵

        定义四元数

         用四元数构造矩阵

         注意：上式子由下图导出。

         继续推导

        这时候如果我们要限制穿透力只能沿着发现方向，就可以定义：

         利用关键阻尼的条件

         这里面与上面不一样的是N的导数，但是N表示的仅仅是一个方向，所以他的导数就可以用角速度叉乘以自身方向来求得。

         最后求解出 f 就可以得到