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

Question

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

我的目标是实现:

1号。 b.transform = a.transform * rela 2号。质心(a + b)不变。
No.3。(第三牛顿法则)整个系统的速度(a + b)不变。
No.4。(第三牛顿法则)整个系统的角速度(a + b)不变。
，编号5。应该最小化两个对象的移动/旋转以解决该问题。

我希望对两个物体施加冲击/扭矩，以使它们逐渐满足要求。
该视频可以描述我想要的-(youtube link)。

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

例子

这是一个示例问题及其正确的解决方案(即最终的静止状态):-



代码(草稿)

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

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 。)

(几天后编辑添加)
我相信，如果我可以破解“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

顾名思义，约束是对两个物体运动的限制。因此，要成功地对约束进行建模，必须完全理解该约束施加在两个物体上的约束，并将这些约束表示为冲量或力方程。基于脉冲的求解器几乎总是在基于力的求解器上使用，因为一阶(速度)方程(以及所涉及的数字)比二阶(加速度)方程更容易计算和处理。因此，我们将着眼于为一般的一维约束(n-D约束可以表示为一个或多个一维约束)建模脉冲。

对一维约束的一阶(速度)约束建模

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

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

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

第一个方程对任何约束成立(这是牛顿第二定律)，但是第二个方程(此后称为约束方程)仅对给定约束成立。

对一维约束的零阶(位置)约束建模

我们的模型现在确保相对位置保持恒定(一阶限制)，但我们仍未强制执行零阶限制，即B相对于A的相对位置必须是由相对变换指定的位置 rela。如果 x1是约束系统位置中的“错误”，则可以使用Baumgarte术语 beta * x1/h将其“注入(inject)”到脉冲中，其中 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 |

求解器在约束的轴方向(通常称为法线)上应用此脉冲及其负数。

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

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

应用约束

一旦计算，脉冲 j必须以相反的方向应用于两个主体，以使约束系统上的总脉冲为零。当为正时，这种冲动意味着将两个物体推开，而当为负时，则意味着将它们拉在一起。因此，如果约束法线指向从主体A到主体B(按照约定)，则 -j * normal应用于主体A，而 +j * normal应用于主体B。

如果需要，可以将一维约束扩展到n维。一些约束(例如 Spring /距离约束)沿着一条线，因此本质上是 n维空间中的1-D约束，并且不需要针对不同轴进行多次求解迭代。

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

与Bullet 1.5约束求解器的区别

必须指出的是，您要查找的固定约束实际上是Bullet 1.5中的 Generic6DofConstraint (所有6个自由度都受限制)，而不是 Point2PointConstraint(这是一个球窝样的接缝)。您可以查看其实现以供引用。 Bullet 1.5约束求解器使用略有不同(且稍为波浪形)的阻尼脉冲方程式(在该方程中，他们将相对速度与阻尼因子相乘，但未将其与等效质量的倒数相加)，从而稍微削弱了约束更多的。这取决于您要选择的偏好，但是我认为这里使用的那个更合理，也更容易适应其他自然的软约束(例如，根据 Spring 频率和阻尼比对约束进行参数化，以模拟一个阻尼 Spring )。

您还可以在我的2D物理引擎中查看阻尼 Spring (柔和的距离约束) solver的自说明代码。