静止接触

多个刚体相互接触时，力相互影响，需要建立方程组，同时求解。

算法

刚体上一点的加速度

刚体移动$x(t)$，旋转$R(t)$，那么其上的一点$p_0$：

速度：

其中，$r(t)=p(t)-x(t)$

加速度：

刚体上一点的角加速度

刚体上一点的角速度即刚体的角速度$\omega(t)$，由角动量$L(t)=I(t)\omega(t)$，有：

角加速度：

角动量的变化率等于力矩$\dot{L}(t)=\tau(t)$，而转动惯量：

因为$\dot{R}(t) = \omega(t) \times R(t)$，有：

因此：

碰撞

在$t$时刻，第$i$个接触点处的两个刚体$a$和$b$的距离：

要满足静止的条件，有：

因为接触$d_i(t)=0$，即$p_a(t)-p_b(t)=0$，所以：

第一项$2\dot{\hat{n}}_i(t) \cdot (\dot{p}_a(t)- \dot{p}_b(t))$只与速度相关。

由前面刚体上一点的加速度，有：

第$j$个接触对刚体$a$的于$p_j$处施加力$f_j\hat{n}_j(t)$，则其有影响：

故：

第一项与$f_i$有关，第二项无关

非接触力合力对刚体$a$于第$i$个接触点处有影响：

把所有对刚体$a$有影响的力产生的加速度求和：

刚体b同理

所以整理与$f_i$有关的项，和无关的项：

整理成矩阵的形式：

整理成矩阵的形式：

其中：

程序

计算$b_i$：

VectorXf ForceBasedDynamics::ComputeBVector(std::vector<Contact> &C)
{
    VectorXf B(C.size());
    B.setZero();

    for (int i = 0; i < C.size(); ++i)
    {
        Contact &c = C[i];
        Rigidbody &a = c.a_;
        Rigidbody &b = c.b_;
        Vector3f &n = c.normal_;
        Vector3f ra = c.position_ - a.GetPosition();
        Vector3f rb = c.position_ - b.GetPosition();

        /*Get the external forces and torques*/
        Vector3f f_ext_a{0.F, g, 0.F};
        Vector3f f_ext_b{0.F, g, 0.F};
        Vector3f t_ext_a{0.F, 0.F, 0.F};
        Vector3f t_ext_b{0.F, 0.F, 0.F};

        Vector3f a_ext_part, a_vel_part, b_ext_part, b_vel_part;
        a_ext_part = f_ext_a * a.GetMassInv() + ((a.GetInertiaMomentInv() * t_ext_a).cross(ra));
        b_ext_part = f_ext_b * b.GetMassInv() + ((b.GetInertiaMomentInv() * t_ext_b).cross(rb));

        if (a.GetMassInv() == 0.F)
        {
            a_vel_part = Vector3f::Zero();
        }
        else
        {
            a_vel_part = (a.GetAngularVelocity().cross(a.GetAngularVelocity().cross(ra))) +
                         ((a.GetInertiaMomentInv() *
                           (a.GetInertiaMomentInv().inverse() * a.GetAngularVelocity().cross(a.GetAngularVelocity())))
                              .cross(ra));
        }
        if (b.GetMassInv() == 0.F)
        {
            b_vel_part = Vector3f::Zero();
        }
        else
        {
            b_vel_part = (b.GetAngularVelocity().cross(b.GetAngularVelocity().cross(rb))) +
                         ((b.GetInertiaMomentInv() *
                           (b.GetInertiaMomentInv().inverse() * b.GetAngularVelocity().cross(b.GetAngularVelocity())))
                              .cross(rb));
        }
        float k1 = n.dot((a_ext_part + a_vel_part) - (b_ext_part + b_vel_part));
        Vector3f ndot = computeNdot(c);

        Vector3f pta = a.GetVelocity() + a.GetAngularVelocity().cross(ra);
        Vector3f ptb = b.GetVelocity() + b.GetAngularVelocity().cross(rb);

        float k2 = 2.f * ndot.dot(pta - ptb);
        B(i) = k1 + k2;
    }

    return B;
}

计算$a_{ij}$：

float compute_aij(const Contact &ci, const Contact &cj)
{
    /* If the bodies involved in the ith and jth contact are distinct, then aij is zero.*/
    if ((&ci.a_ != &cj.a_) && (&ci.a_ != &cj.b_) && (&ci.b_ != &cj.a_) && (&ci.b_ != &cj.b_))
    {
        return 0;
    }

    Rigidbody &a = ci.a_;
    Rigidbody &b = ci.b_;
    const Vector3f &ni = ci.normal_;
    const Vector3f &nj = cj.normal_;
    const Vector3f &pi = ci.position_;
    const Vector3f &pj = cj.position_;
    Vector3f ra = pi - a.GetPosition();
    Vector3f rb = pi - b.GetPosition();

    /* what force and torque does contact j exert on body a?*/
    Vector3f force_on_a = {0.F, 0.F, 0.F};
    Vector3f torque_on_a = {0.F, 0.F, 0.F};
    if (&cj.a_ == &ci.a_)
    {
        /* force direction of jth contact force on a*/
        force_on_a = nj;

        /*torque direction*/
        torque_on_a = (pj - a.GetPosition()).cross(nj);
    }
    else if (&cj.b_ == &ci.a_)
    {
        force_on_a = -nj;
        torque_on_a = (pj - a.GetPosition()).cross(-nj);
    }

    /* what force and torque does contact j exert on body b?*/
    Vector3f force_on_b = {0.F, 0.F, 0.F};
    Vector3f torque_on_b = {0.F, 0.F, 0.F};
    if (&cj.a_ == &ci.b_)
    {
        /* force direction of jth contact force on a*/
        force_on_b = nj;

        /*torque direction*/
        torque_on_b = (pj - b.GetPosition()).cross(nj);
    }
    else if (&cj.b_ == &ci.b_)
    {
        force_on_b = -nj;
        torque_on_b = (pj - b.GetPosition()).cross(-nj);
    }

    /* Now compute how the jth contact force affects the linear and angular acceleration of the contact point on body A */
    Vector3f a_linear = force_on_a * a.GetMassInv(), a_angular = (a.GetInertiaMomentInv() * torque_on_a).cross(ra);
    /* Same for B */
    Vector3f b_linear = force_on_b * b.GetMassInv(), b_angular = (b.GetInertiaMomentInv() * torque_on_b).cross(rb);
    return ni.dot((a_linear + a_angular) - (b_linear + b_angular));
}

// derivative of the normal vector
Vector3f computeNdot(const Contact &c)
{
    return c.b_.GetAngularVelocity().cross(c.normal_);
}

建构A矩阵：

Eigen::MatrixXf ForceBasedDynamics::BuildMatrix(std::vector<Contact> &C)
{
    MatrixXf M(C.size(), C.size());
    for (int i = 0; i < C.size(); ++i)
    {
        for (int j = 0; j < C.size(); ++j)
        {
            M(i, j) = compute_aij(C[i], C[j]);
            assert(!isnan(M(i, j)));
        }
    }
    return M;
}

迭代求解：

const size_t kIterateCount = 50;

Eigen::VectorXf CraneMath::solveGS(Eigen::MatrixXf M, Eigen::VectorXf b) {
  Eigen::VectorXf x(b.size());
  x.setOnes();

  int n = 0;
  while (n < kIterateCount) {
    for (int i = 0; i < b.size(); i++) {
      float delta = 0.F;
      for (int j = 0; j < b.size(); j++) {
        if (i != j) {
          delta += M(i, j) * x(j);
        }
      }

      x(i) = (b(i) - delta) / M(i, i);
    }
    n++;
  }

  return x;
}

测试

参考

1. Baraff D. Physically based modeling: Rigid body simulation[J]. SIGGRAPH Course Notes, ACM SIGGRAPH, 2001, 2(1): 2-1.