计算3D固定关节约束中两个物体的脉冲/扭矩

Question

我有2个刚体（a＆b）和1个固定关节约束（带有相对转换rela）。

我的目标是实现：

1 b.transform = a.transform * rela
号。2号。质心（a+ b）不变。
No.3。 （第3牛顿法则）整个系统的速度（a+ b）不变。
No.4。 （第3牛顿法则）整个系统的角速度（a+ b）不变。
否。5 .最小化两个物体的移动/旋转以解决问题。

我希望对两个物体施加冲击/扭矩，以使它们逐渐满足要求。
这部影片可以描绘出我想要的-（youtube链接）。

如何解决适用于每个身体的冲动/扭矩值？
我想要一个大概的主意/算法。
它可以是没有任何代码的描述文本。

例

这是一个示例问题及其正确的解决方案（即最终的静止状态）：

代码（草稿）

这是我当前的代码段，以防万一：

class Transform {
    Vec3 pos;
    Matrix33 basis;
};

每个刚体具有以下领域：

class RigidBody {
    float mass;
    Matrix33 inertiaTensor;
    Transform transform;
    Vec3 velocity;
    Vec3 angularVelocity;
};

该修复关节约束是： -

class FixConstraint {
    Transform rela;
    RigidBody* a;
    RigidBody* b;
};

我可怜的解决方案草稿

简而言之，我必须修改12个变量。

• 的位置a和b（XYZ - 6个变量）
• a和b的方向（角度xyz-6个变量）

然后，我可以使用“我的目标” 1号和2号创建一些方程式。
然后，充其量，我必须用12个未知变量求解12个线性方程。
我怀疑是否必须那么难。

我以前的互联网搜索

我研究了各种来源，但主要是：

• 只需进入迭代求解器即可。
• 尝试对角矩阵+ jacobian：只有专家才能理解。
• “为什么不查看（在此处插入Physic Engine的名称）源代码？” 没有对初学者友好的解释。
• 显示如何使用（物理引擎的名称）创建固定关节约束。

这是一些最有用的：

• 子弹物理求解器：https : //github.com/svn2github/bullet/blob/master/tags/bullet-1.5f/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
• “稳定，强大且多功能的多体动力学动画”研究论文http://image.diku.dk/kenny/download/erleben.05.thesis.pdf （请参阅第6章）
• （已编辑添加） Featherstone的布娃娃算法：https ://en.wikipedia.org/wiki/Featherstone%27s_algorithm （但它着重于许多约束而非一个约束）

（编辑了一些措辞和规则，感谢fafl和Nico Schertler。）

---

（几天后编辑添加）
我相信，如果我能破解（Bullet Physics的）“ Point2PointConstraint.cpp” ，我将完全理解该算法和原理。

为了防万一，我也将其复制粘贴到此处。
这是第一部分：

SimdVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{
    normal[i] = 1;
    new (&m_jac[i]) JacobianEntry(
        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
        m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
        m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
        normal,
        m_rbA.getInvInertiaDiagLocal(),
        m_rbA.getInvMass(),
        m_rbB.getInvInertiaDiagLocal(),
        m_rbB.getInvMass());
    normal[i] = 0;
}

这是第二部分：

SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
SimdVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{       
    normal[i] = 1;
    SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();

    SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
    SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    //this jacobian entry could be re-used for all iterations

    SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
    SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
    SimdVector3 vel = vel1 - vel2;

    SimdScalar rel_vel;
    rel_vel = normal.dot(vel);

    //positional error (zeroth order error)
    SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal

    SimdScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;

    SimdVector3 impulse_vector = normal * impulse;
    m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
    m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());

    normal[i] = 0;
}

Answer 1

顾名思义，约束是对两个物体的运动施加的限制。因此，为了成功地对约束进行建模，必须充分理解该约束对两个物体施加了哪些限制，并将这些约束表示为脉冲或力方程。基于脉冲的求解器几乎总是比基于力的求解器使用，因为一阶（速度）方程（以及涉及的数字）比二阶（加速度）方程更容易计算和处理。因此，我们将研究一般一维约束的建模脉冲（nD 约束可以表示为一个或多个一维约束）。

对一维约束的一阶（速度）限制进行建模

使用脉冲对约束进行建模的第一步是将约束表示为一阶（速度/动量）方程。所讨论的固定约束（下文简称为“约束”）具有简单的位置（零阶）限制：约束空间中的线性位置和角度位置必须保持相同（由相对变换 指定rela）。我们可以通过以下事实将其转换为一阶限制：为了使两个物体的相对位置恒定，它们的相对速度必须为零。因此，该约束可以解释为确保约束空间中两个物体的相对线速度和角速度为零。

现在可以将约束建模为必须应用于系统的脉冲，以便相对速度变为零。如果v1和v2是系统的初始和最终相对速度，m是系统的质量，j是为满足约束而施加的冲量，则

v2 = v1 + j/m    (1)
v2 = 0           (2)

第一个方程对于任何约束都成立（这是牛顿第二定律），但第二个方程（以下称为约束方程）仅对于给定的约束成立。

对一维约束的零阶（位置）限制进行建模

我们的模型现在确保相对位置保持不变（一阶限制），但我们仍然没有强制执行零阶限制，即 B 相对于 A 的相对位置必须是由相对变换指定的位置rela。如果x1是约束系统位置中的“误差”，则可以使用 Baumgarte 项将其“注入”到脉冲中，beta * x1/h其中h是时间步长，并且beta是通常介于 0 和 1 之间的参数。您可以将beta其视为您希望解算器解决每一帧的位置误差。

我们的约束方程现在变成

v2 + beta*x1/h = 0

注意：如果求解器使用半隐式积分，则x1是初始位置误差，但如果求解器使用隐式积分，则误差实际上是x2 = x1 + v2*h。

阻尼/软化约束

正如您所指出的，您正在寻找的约束是软（或阻尼）约束，因为您希望运动逐渐发生。为了以指数方式阻尼约束，我们模拟指数衰减微分方程并添加一个与 成正比的值（当时间步长变得无穷小时，j它与速度的导数成正比）。dv = v2 - v1

我们的约束方程现在变成

v2 + beta * x1/h + gamma * j = 0

其中gamma >= 0是另一个参数（也称为阻尼系数）。如果gamma = 0，约束将是无阻尼的或刚性的。

消除v2这两个方程并求解所得方程可j得出

j = -(v1 + beta * x1/h) * (1/(gamma + 1/m))
                         | equivalent mass |

求解器在约束的轴（通常称为法线）方向上应用该脉冲及其负值。

适应和使用一般一维软约束

这种通用的一维软约束脉冲可以重新利用来创建许多不同的关节，如固定关节、弹簧关节等。对于固定关节，我们希望将 B 在 A 空间中的位置和旋转限制为相对变换的位置和旋转。因此，在 A 的空间中，x1 = B.position - rela.origin和v1 = relative velocity of A and B along x1。然而，由于我们想要限制 3 维位置（即x13d 向量，而不是标量），因此我们可以分别求解三个轴中每一个轴上的一维约束。然后可以使用相同的约束对角位置和速度重复该过程。可以遵循相同的过程来导出任何一维约束的冲量。

应用约束

一旦计算出来，冲量j必须以相反的方向施加到两个物体上，以便约束系统上的总冲量为零。当为正值时，这种冲动意味着将两个物体推开，当为负值时，意味着将它们拉到一起。因此，如果约束法线从主体 A 指向主体 B（按照惯例），-j * normal则应用于主体 A 并+j * normal应用于主体 B。

如果需要，一维约束可以扩展到 nD。一些约束（例如弹簧/距离约束）沿着一条线，因此本质上是n维度空间中的一维约束，并且不需要针对不同轴进行多次求解迭代。

如果您的求解器使用热启动，您还可以修改约束方程以在阻尼稳定性时考虑累积脉冲。阻尼项变为gamma * (j + j0)，其中j0是热启动期间施加的累积脉冲。

与 Bullet 1.5 约束求解器的差异

必须指出的是，您正在寻找的固定约束实际上是Generic6DofConstraint（所有 6 个自由度都受到限制），而不是Point2PointConstraintBullet 1.5 中的（这是一个球和插座状的关节）。您可以查看其实现以供参考。Bullet 1.5 约束求解器使用稍微不同（并且稍微更手动）的阻尼脉冲方程（其中它们将相对速度乘以阻尼因子，但不将其添加到等效质量的倒数），这会稍微阻尼约束更多的。这取决于您想要选择哪一种，但我认为这里使用的一种更合理，也更容易适应其他自然软约束（例如，根据弹簧频率和阻尼比参数化约束来模拟阻尼弹簧）。

您还可以查看我的 2D 物理引擎中阻尼弹簧（软距离约束）解算器的希望不言自明的代码。