标签:
参考: http://blog.sina.com.cn/s/blog_61feffe10100msbz.html, http://allenchou.net/2013/12/game-physics-constraints-sequential-impulse/
约束就是限制物体的运动方式, 可以用来处理碰撞, 模拟关节等的东西. 例如碰撞就是两个物体最小距离至少是0, 关节是两个物体被一颗钉子钉在一起只能绕这个钉子旋转, 弹簧是两个物体的距离要保持在某一范围内.
对于一个3D刚体有6个自由度, x, y, z轴上的线性运动各一个, 角度上则有3个. 2D刚体有3个自由度, x, y轴两个, 角度只有一个. 给一对物体添加约束会限制这些自由度.
举个例子, 假如2D物体A和B被一根杆子连了起来, 那么它们运动起来一定会保持一个和杆子长度L相等的距离, 于是可以得到一个等式:
(xA - xB)2 + (yA - yB)2 - L2 = 0
其实就是两点距离公式. 上面这个式子就能表达一种约束了.
为了表达出各种情况的约束, 设一条式子:
C(xA, yA, rA, xB, yB, rB) = 0
其中r是物体的角度, C是一个表达约束条件的函数. 3D的情况把z和另外两个角度加到参数里就是了.
不过实际用的时候都是对物体的速度作约束而不是直接改变物体坐标位置, 所以对C用时间t求导:
dC / dt = dC / dx * dx / dt= dC / dx * v
v是速度向量, v = [vxA vyA wA vxB vyB wB]T, VxA是物体A的x轴线速度, wA是物体A的角速度. x是个关于位置的向量函数.
然后根据偏导数链式法则, dC/dx变成一个雅可比矩阵J(可以简单翻一下百科, 不知道是个啥其实也没多大关系), 于是变成:
dC / dt = J * v
J = [jVxA jVyA jWA jVxB jVyB jWB]
然后上面的C = 0的式子就变成了:
J * V + b = 0
这个突然冒出来的b是个"bias"项, 虽然不知道确切来说是个啥但以后有的地方会用得到.
V还是那个关于两个物体速度的列向量, 为了满足约束条件物体速度改变前后都要满足上面的式子, 于是:
J(V + ΔV) + b = 0
令力f = JT * λ, 雅可比矩阵J是C的梯度的矩阵(导数可以表示函数沿某个量变化的快慢), λ向量表示约束力的大小. 差不多就是通过施加"约束力"来使得物体的运动满足约束条件.
f = Ma, a = f * M-1, v = a * t, v = f * M-1 * t
于是: ΔV = JT * λ * M-1 * Δt
M是关于两物体质量的矩阵:
M = [MA 0 0 0; 0 IA 0 0; 0 0 MB 0; 0 0 0 IB]
其中MA = [mA 0; 0 mA], IA = iA, m是质量, i是转动惯量. 3D的情况下MA = [mA 0 0; 0 mA 0; 0 0 mA], IA对齐3个角度也是个3×3矩阵
dC / dt = J * V + J * Δt * M-1 * JT * λ
J * M-1 / JT (Δt * λ) = dC / dt - J * V
冲量p = F * t, 把λ从力改成冲量隐藏那个Δt, λ = λ * Δt, 为了满足约束条件dC/dt = 0, 于是:
J(V + M-1 * JT * λ) + b = 0
然后求得:
λ = -(J * V + b) / (J * M-1 * JT)
ΔV = M-1 * JT * λ
所以怎么应用约束呢?
首先两个物体当前速度V是一致的, 雅可比矩阵J是自己根据需要的约束关系计算出来的, 然后用代码计算出ΔV, 然后V += ΔV, 就消去了物体不满足约束的运动.
把式子都丢进matlab里算, 得到:
λ = (-jVxA * vXA - jVyA * vYA - jWA * wA - jVxB * vXB - jVyB * vYB - jWB * wB - b) / (jVxA * jVxA / mA + jVyA * jVyA / mA + jWA * jWA / iA + jVxB * jVxB / mB + jVyB * jVyB / mB + jWB * jWB / iB)
ΔV = [jVxA / mA * λ,
jVyA / mA * λ,
jWA / iA * λ,
jVxB / mB * λ,
jVyB / mB * λ,
jWB / iB * λ]
下面的链接有一些别人推导好的J和b
碰撞约束: http://allenchou.net/2013/12/game-physics-resolution-contact-constraints/
点对点约束: http://blog.sina.com.cn/s/blog_61feffe10100mt08.html (http://www.codezealot.org/archives/225)
距离约束: http://blog.sina.com.cn/s/blog_61feffe10100mtgp.html (http://www.dyn4j.org/2010/09/distance-constraint/)
(function (P, undefined) { ‘use strict‘; P.require(‘class‘); var C; C = P.Class( null, function (oBodyA, oBodyB, nJVxA, nJVyA, nJWA, nJVxB, nJVyB, nJWB, nB) { this.bodyA = oBodyA; this.bodyB = oBodyB; this.jVxA = nJVxA; this.jVyA = nJVyA; this.jWA = nJWA; this.jVxB = nJVxB; this.jVyB = nJVyB; this.jWB = nJWB; this.b = nB; }, { bodyA: null, bodyB: null, jVxA: null, jVyA: null, jWA: null, jVxB: null, jVyB: null, jWB: null, b: null, solve: function () { var bodyA = this.bodyA, bodyB = this.bodyB, vXA = bodyA.velocity.x, vYA = bodyA.velocity.y, wA = bodyA.angularVelocity, vXB = bodyB.velocity.x, vYB = bodyB.velocity.y, wB = bodyB.angularVelocity, invMA = bodyA.inverseMass, invMB = bodyB.inverseMass, invIA = bodyA.inverseInertia, invIB = bodyB.inverseInertia, jVxA = this.jVxA, jVyA = this.jVyA, jWA = this.jWA, jVxB = this.jVxB, jVyB = this.jVyB, jWB = this.jWB, b = this.b, lambda = (-jVxA * vXA - jVyA * vYA - jWA * wA - jVxB * vXB - jVyB * vYB - jWB * wB - b) / (jVxA * jVxA * invMA + jVyA * jVyA * invMA + jWA * jWA * invIA + jVxB * jVxB * invMB + jVyB * jVyB * invMB + jWB * jWB * invIB); bodyA.velocity.x += jVxA * invMA * lambda; bodyA.velocity.y += jVyA * invMA * lambda; bodyA.angularVelocity += jWA * invIA * lambda; bodyB.velocity.x += jVxB * invMB * lambda; bodyB.velocity.y += jVyB * invMB * lambda; bodyB.angularVelocity += jWB * invIB * lambda; } } ); return P.Constraints = C; })(pngx);
一个不靠谱的2D物理引擎(5) - 约束(Constraints)
标签:
原文地址:http://www.cnblogs.com/pngx/p/4313764.html