Return to Question

This is the full source code on GitHub

According to the constraint equation, the velocities of the three constraint points should be equal.

This is the full source code on GitHub

this is result:

This is the relative velocity of the center of mass of the three constraint points:

this is test data:

    float fMass1 = 100;
    float fMass2 = 100;
    const VECTOR3<float> vCenter[] = { VECTOR3<float>(0, 9, 0), VECTOR3<float>(0, 0, 0) };
    const VECTOR3<float> ContactPoint[] = { VECTOR3<float>(-3, 4.5, -3), VECTOR3<float>(-3, 4.5, 3), VECTOR3<float>(3, 4.5, -3), VECTOR3<float>(3, 4.5, 3) };
    const VECTOR3<float> vSize = VECTOR3<float>(6, 9, 6);

    VECTOR3<float> vVelocity1           = VECTOR3<float>(0, 20, 0);
    VECTOR3<float> vAngularVelocity1    = VECTOR3<float>(0, 0, -3);

    VECTOR3<float> vVelocity2           = VECTOR3<float>(0, 80, 0);
    VECTOR3<float> vAngularVelocity2    = VECTOR3<float>(10, 0, 0);

this is core code:

template<typename T>
void VelocityMultipleConstraint(const VelocityConstraintParameter<T>* param)
{
    MATRIX<3, 12, double> Jacobian;
    MATRIX<12, 12, double> InverseMass;
    MATRIX<12, 1, double> Velocity;

    for (int i = 0; i < 3; i++) {
        Jacobian(i, 0) = param[i].vDirection.x;
        Jacobian(i, 1) = param[i].vDirection.y;
        Jacobian(i, 2) = param[i].vDirection.z;

        const VECTOR3<T> vRadiusObject1 = param[i].vObj1ContactPoint - param[i].vBarycenter1;
        const VECTOR3<T> vRadiusCrossNormal1 = cross(vRadiusObject1, param[i].vDirection);

        Jacobian(i, 3) = vRadiusCrossNormal1.x;
        Jacobian(i, 4) = vRadiusCrossNormal1.y;
        Jacobian(i, 5) = vRadiusCrossNormal1.z;

        Jacobian(i, 6) = -param[i].vDirection.x;
        Jacobian(i, 7) = -param[i].vDirection.y;
        Jacobian(i, 8) = -param[i].vDirection.z;

        const VECTOR3<T> vRadiusObject2 = param[i].vObj2ContactPoint - param[i].vBarycenter2;
        const VECTOR3<T> vRadiusCrossNormal2 = cross(-vRadiusObject2, param[i].vDirection);

        Jacobian(i, 9) = vRadiusCrossNormal2.x;
        Jacobian(i, 10) = vRadiusCrossNormal2.y;
        Jacobian(i, 11) = vRadiusCrossNormal2.z;
    }
    InverseMass(0, 0) = param[0].fInverseMass1;             InverseMass(1, 1) = param[0].fInverseMass1;                 InverseMass(2, 2) = param[0].fInverseMass1;
    InverseMass(3, 3) = param[0].mtInverseInertia1(0, 0);   InverseMass(3, 4) = param[0].mtInverseInertia1(0, 1);       InverseMass(3, 5) = param[0].mtInverseInertia1(0, 2);
    InverseMass(4, 3) = param[0].mtInverseInertia1(1, 0);   InverseMass(4, 4) = param[0].mtInverseInertia1(1, 1);       InverseMass(4, 5) = param[0].mtInverseInertia1(1, 2);
    InverseMass(5, 3) = param[0].mtInverseInertia1(2, 0);   InverseMass(5, 4) = param[0].mtInverseInertia1(2, 1);       InverseMass(5, 5) = param[0].mtInverseInertia1(2, 2);


    InverseMass(6, 6) = param[0].fInverseMass2;               InverseMass(7, 7) = param[0].fInverseMass2;               InverseMass(8, 8) = param[0].fInverseMass2;
    InverseMass(9, 9) = param[0].mtInverseInertia2(0, 0);     InverseMass(9, 10) = param[0].mtInverseInertia2(0, 1);     InverseMass(9, 11) = param[0].mtInverseInertia2(0, 2);
    InverseMass(10, 9) = param[0].mtInverseInertia2(1, 0);     InverseMass(10, 10) = param[0].mtInverseInertia2(1, 1);     InverseMass(10, 11) = param[0].mtInverseInertia2(1, 2);
    InverseMass(11, 9) = param[0].mtInverseInertia2(2, 0);     InverseMass(11, 10) = param[0].mtInverseInertia2(2, 1);     InverseMass(11, 11) = param[0].mtInverseInertia2(2, 2);

    Velocity(0, 0) = param[0].vVelocity1.x;         Velocity(1, 0) = param[0].vVelocity1.y;            Velocity(2, 0) = param[0].vVelocity1.z;
    Velocity(3, 0) = param[0].vAngularVelocity1.x;  Velocity(4, 0) = param[0].vAngularVelocity1.y;     Velocity(5, 0) = param[0].vAngularVelocity1.z;
    Velocity(6, 0) = param[0].vVelocity2.x;         Velocity(7, 0) = param[0].vVelocity2.y;            Velocity(8, 0) = param[0].vVelocity2.z;
    Velocity(9, 0) = param[0].vAngularVelocity2.x;  Velocity(10, 0) = param[0].vAngularVelocity2.y;     Velocity(11, 0) = param[0].vAngularVelocity2.z;

    MATRIX<3, 1, double> Lambda = inverse( Jacobian * InverseMass * Jacobian.transpose()) * (-Jacobian * Velocity);

    MATRIX<12, 1, double> DeltaV = InverseMass * Jacobian.transpose() * Lambda;

    *param[0].pOutVelocity1          = VECTOR3<float>(param[0].vVelocity1.x + DeltaV(0, 0), param[0].vVelocity1.y + DeltaV(1, 0), param[0].vVelocity1.z + DeltaV(2, 0));
    *param[0].pOutAngularVelocity1   = VECTOR3<float>(param[0].vAngularVelocity1.x + DeltaV(3, 0), param[0].vAngularVelocity1.y + DeltaV(4, 0), param[0].vAngularVelocity1.z + DeltaV(5, 0));
    *param[0].pOutVelocity2          = VECTOR3<float>(param[0].vVelocity2.x + DeltaV(6, 0), param[0].vVelocity2.y + DeltaV(7, 0), param[0].vVelocity2.z + DeltaV(8, 0));
    *param[0].pOutAngularVelocity2   = VECTOR3<float>(param[0].vAngularVelocity2.x + DeltaV(9, 0), param[0].vAngularVelocity2.y + DeltaV(10, 0), param[0].vAngularVelocity2.z + DeltaV(11, 0));
   
}

this is result:

constraint point : 0
before : 1 :{x=-13.5000000 y=29.0000000 z=0.00000000 ...} 2 : {x=0.00000000 y=110.000000 z=45.0000000 ...}
constraint point : 1
before : 1 :{x=-13.5000000 y=29.0000000 z=0.00000000 ...} 2 : {x=0.00000000 y=50.0000000 z=45.0000000 ...}
constraint point : 2
before : 1 :{x=-13.5000000 y=11.0000000 z=0.00000000 ...} 2 : {x=0.00000000 y=110.000000 z=45.0000000 ...}
after : 1 : {x=5.06883144 y=45.9733505 z=0.528086662 ...} , 2 : {x=-9.93715286 y=54.8153305 z=-10.8799744 ...}
after : 1 : {x=-3.84213710 y=38.4656563 z=0.528086662 ...} , 2 : {x=-1.02618432 y=61.1101532 z=-10.8799744 ...}
after : 1 : {x=5.06883144 y=42.5455589 z=9.43905544 ...} , 2 : {x=-9.93715286 y=57.8786316 z=-19.7909431 ...}

This is the full source code on GitHub