TEST (RigidRepresentationContactTests, SetGet_BuildMlcp_Test)
{
Vector3d n(0.0, 1.0, 0.0);
double d = 0.0;
double radius = 0.01;
double violation = -radius;
Vector3d contactPosition = -n * (d - violation);
SurgSim::Math::RigidTransform3d poseRigid;
poseRigid.setIdentity();
std::shared_ptr<RigidRepresentation> rigid = std::make_shared<RigidRepresentation>("Rigid");
rigid->setLocalActive(true);
rigid->setIsGravityEnabled(false);
rigid->setLocalPose(poseRigid);
rigid->setDensity(1000.0);
rigid->setShape(std::make_shared<SphereShape>(radius));
auto loc = std::make_shared<RigidRepresentationLocalization>(rigid);
loc->setLocalPosition(contactPosition);
std::shared_ptr<RigidRepresentationContact> implementation = std::make_shared<RigidRepresentationContact>();
EXPECT_EQ(SurgSim::Math::MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, implementation->getMlcpConstraintType());
EXPECT_EQ(1u, implementation->getNumDof());
ContactConstraintData constraintData;
constraintData.setPlaneEquation(n, d);
MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(rigid->getNumDof(), 1, 1);
// Fill up the Mlcp
double dt = 1e-3;
implementation->build(dt, constraintData, loc,
&mlcpPhysicsProblem, 0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
// Violation b should be exactly violation = -radius (the sphere center is on the plane)
EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon);
// Constraint H should be
// H = dt.[nx ny nz nz.GPy-ny.GPz nx.GPz-nz.GPx ny.GPx-nx.GPy]
Vector3d n_GP = n.cross(Vector3d(0.0, 0.0, 0.0));
SurgSim::Math::Matrix h = mlcpPhysicsProblem.H;
EXPECT_NEAR(dt * n[0] , h(0, 0), epsilon);
EXPECT_NEAR(dt * n[1] , h(0, 1), epsilon);
EXPECT_NEAR(dt * n[2] , h(0, 2), epsilon);
EXPECT_NEAR(dt * n_GP[0], h(0, 3), epsilon);
EXPECT_NEAR(dt * n_GP[1], h(0, 4), epsilon);
EXPECT_NEAR(dt * n_GP[2], h(0, 5), epsilon);
// ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation
// This way, the constraint can verify that both ConstraintImplementation are the same type
ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
}
TEST (FixedConstraintFrictionlessContactTests, SetGet_BuildMlcp_Test)
{
SurgSim::Math::Vector3d n(0.0, 1.0, 0.0);
double d = 0.0;
double violation = -0.01;
SurgSim::Math::Vector3d contactPosition = -n * (d - violation);
SurgSim::Math::RigidTransform3d poseFixed;
poseFixed.setIdentity();
std::shared_ptr<FixedRepresentation> fixed = std::make_shared<FixedRepresentation>("Fixed");
fixed->setLocalActive(true);
fixed->setIsGravityEnabled(false);
fixed->setLocalPose(poseFixed);
auto loc = std::make_shared<FixedLocalization>(fixed);
loc->setLocalPosition(contactPosition);
std::shared_ptr<FixedConstraintFrictionlessContact> implementation =
std::make_shared<FixedConstraintFrictionlessContact>();
EXPECT_EQ(SurgSim::Physics::FRICTIONLESS_3DCONTACT, implementation->getConstraintType());
EXPECT_EQ(1u, implementation->getNumDof());
ContactConstraintData constraintData;
constraintData.setPlaneEquation(n, d);
MlcpPhysicsProblem mlcpPhysicsProblem = MlcpPhysicsProblem::Zero(fixed->getNumDof(), 1, 1);
// Fill up the Mlcp
double dt = 1e-3;
implementation->build(dt, constraintData, loc, &mlcpPhysicsProblem,
0, 0, SurgSim::Physics::CONSTRAINT_POSITIVE_SIDE);
// b should be exactly the violation
EXPECT_NEAR(violation, mlcpPhysicsProblem.b[0], epsilon);
// Constraint H should be [] (a fixed representation has no dof !)
// ConstraintTypes should contain 0 entry as it is setup by the constraint and not the ConstraintImplementation
// This way, the constraint can verify that both ConstraintImplementation are the same type
ASSERT_EQ(0u, mlcpPhysicsProblem.constraintTypes.size());
}