Implemented tangential friction. Yay!
Martin Felis
parent
ca4f4048df
commit
d01417e8c9
|
|
@ -55,26 +55,24 @@ struct CollisionInfo {
|
|||
double biasVelocityB = 0.;
|
||||
double accumImpulse = 0.;
|
||||
double deltaImpulse = 0.;
|
||||
Vector3d dir = Vector3d::Zero();
|
||||
Vector3d dir = Vector3d(0., 1., 0.);
|
||||
VectorNd jacA = VectorNd::Zero(1);
|
||||
VectorNd jacB = VectorNd::Zero(1);
|
||||
VectorNd MInvJacTA = VectorNd::Zero(1);
|
||||
VectorNd MInvJacTB = VectorNd::Zero(1);
|
||||
double GMInvGTA = 0.;
|
||||
double GMInvGTB = 0.;
|
||||
double tangentAccumImpulse0 = 0.;
|
||||
double tangentAccumImpulse1 = 0.;
|
||||
double tangentDeltaImpulse0 = 0.;
|
||||
double tangentDeltaImpulse1 = 0.;
|
||||
Vector3d tangent0 = Vector3d::Zero();
|
||||
Vector3d tangent1 = Vector3d::Zero();
|
||||
VectorNd tangentJacA = MatrixNd::Zero(2,2);
|
||||
VectorNd tangentJacB = MatrixNd::Zero(2,2);
|
||||
MatrixNd tangentMInvA = MatrixNd::Zero(2, 2);
|
||||
MatrixNd tangentMInvB = MatrixNd::Zero(2, 2);
|
||||
MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2);
|
||||
MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2);
|
||||
double accumFrictionImpulse[2] = {0.,0.};
|
||||
double deltaFrictionImpulse[2] = {0.,0.};
|
||||
Vector3d tangents[2] = {Vector3d::Zero(), Vector3d::Zero()};
|
||||
VectorNd tangentJacA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
|
||||
VectorNd tangentJacB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
|
||||
VectorNd tangentMInvJacTA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
|
||||
VectorNd tangentMInvJacTB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
|
||||
double tangentGMInvGTA[2] = {0., 0.};
|
||||
double tangentGMInvGTB[2] = {0., 0.};
|
||||
double effectiveRestitution = 1.0;
|
||||
double effectiveFriction = 0.2;
|
||||
|
||||
double depth = 0.;
|
||||
};
|
||||
|
|
@ -141,11 +139,21 @@ void CalcCollisions(
|
|||
SimBody& body_a,
|
||||
SimBody& body_b,
|
||||
std::vector<CollisionInfo>& collisions);
|
||||
|
||||
void CalcFrictionImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo);
|
||||
|
||||
void ApplyFrictionImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo);
|
||||
|
||||
void CalcConstraintImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo,
|
||||
const double dt);
|
||||
CollisionInfo& cinfo);
|
||||
void ApplyConstraintImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
|
|
|
|||
132
src/rbdlsim.cc
132
src/rbdlsim.cc
|
|
@ -110,6 +110,9 @@ static void sCalcTangentVectors(const Vector3d &normal, Vector3d* tangent0, Vect
|
|||
*tangent0 = normal.cross(Vector3d(1., 0., 0.));
|
||||
*tangent1 = tangent0->cross(normal);
|
||||
}
|
||||
|
||||
assert (tangent0->squaredNorm() > cCollisionEps);
|
||||
assert (tangent1->squaredNorm() > cCollisionEps);
|
||||
}
|
||||
|
||||
bool CheckPenetration(
|
||||
|
|
@ -121,29 +124,29 @@ bool CheckPenetration(
|
|||
bool result = false;
|
||||
if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) {
|
||||
result = CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
return result;
|
||||
} else if (
|
||||
shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
|
||||
result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
|
||||
sSwapCollisionInfoShapeOrder(cinfo);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
return result;
|
||||
} else if (
|
||||
shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
|
||||
result = CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
return result;
|
||||
} else if (
|
||||
shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
|
||||
result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
return result;
|
||||
} else if (
|
||||
shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
|
||||
bool result = CheckPenetrationBoxVsPlane(shape_b, shape_a, cinfo);
|
||||
sSwapCollisionInfoShapeOrder(cinfo);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -166,6 +169,8 @@ bool CheckPenetration(
|
|||
cinfo.depth = depth;
|
||||
}
|
||||
|
||||
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
|
||||
|
||||
return !intersect;
|
||||
}
|
||||
|
||||
|
|
@ -511,12 +516,14 @@ void CalcImpulseVariables(
|
|||
unsigned int body_index,
|
||||
const Vector3d& pos,
|
||||
const Vector3d& dir,
|
||||
const Vector3d* tangents,
|
||||
const double depth,
|
||||
VectorNd* MInvJacT,
|
||||
VectorNd* jac,
|
||||
double* G_MInv_GT,
|
||||
MatrixNd* tangentJac,
|
||||
MatrixNd* tangentGMInvGT,
|
||||
VectorNd* tangentJac,
|
||||
VectorNd* tangentMInvJacT,
|
||||
double* tangentGMInvGT,
|
||||
double* bias_vel,
|
||||
double restitution) {
|
||||
if (body == nullptr || body->mIsStatic) {
|
||||
|
|
@ -546,18 +553,24 @@ void CalcImpulseVariables(
|
|||
CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
|
||||
(*jac) = dir.transpose() * G_constr;
|
||||
|
||||
(*MInvJacT) = M.llt().solve(jac->transpose());
|
||||
SimpleMath::LLT<MatrixNd> M_llt = M.llt();
|
||||
(*MInvJacT) = M_llt.solve(jac->transpose());
|
||||
*G_MInv_GT = (*jac) * (*MInvJacT);
|
||||
assert(!isnan(*G_MInv_GT));
|
||||
|
||||
double beta = 0.01;
|
||||
double delta_slop = cCollisionEps;
|
||||
*bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05);
|
||||
double delta_slop = 0.05;
|
||||
*bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - delta_slop);
|
||||
|
||||
(*tangentJac).resize(2,ndof);
|
||||
(*tangentJac).block(0,0,1,ndof) = tangent0.transpose() * G_constr;
|
||||
(*tangentJac).block(1,0,1,ndof) = tangent0.transpose() * G_constr;
|
||||
(*tangentGMInvGT) = (*tangentJac) * (*MInv) * (*tangentJac).transpose();
|
||||
tangentJac[0] = tangents[0].transpose() * G_constr;
|
||||
tangentJac[1] = tangents[1].transpose() * G_constr;
|
||||
tangentMInvJacT[0] = M_llt.solve(tangentJac[0].transpose());
|
||||
tangentMInvJacT[1] = M_llt.solve(tangentJac[1].transpose());
|
||||
tangentGMInvGT[0] = tangentJac[0] * tangentMInvJacT[0];
|
||||
tangentGMInvGT[1] = tangentJac[1] * tangentMInvJacT[1];
|
||||
|
||||
assert (tangentGMInvGT[0] > 0.);
|
||||
assert (tangentGMInvGT[1] > 0.);
|
||||
}
|
||||
|
||||
void PrepareConstraintImpulse(
|
||||
|
|
@ -571,12 +584,14 @@ void PrepareConstraintImpulse(
|
|||
cinfo.mBodyAIndex,
|
||||
cinfo.posA,
|
||||
cinfo.dir,
|
||||
cinfo.tangents,
|
||||
cinfo.depth,
|
||||
&cinfo.MInvJacTA,
|
||||
&cinfo.jacA,
|
||||
&cinfo.GMInvGTA,
|
||||
&cinfo.tangentJacA,
|
||||
&cinfo.tangentGMInvGTA,
|
||||
cinfo.tangentJacA,
|
||||
cinfo.tangentMInvJacTA,
|
||||
cinfo.tangentGMInvGTA,
|
||||
&cinfo.biasVelocityA,
|
||||
cinfo.effectiveRestitution);
|
||||
|
||||
|
|
@ -586,43 +601,93 @@ void PrepareConstraintImpulse(
|
|||
cinfo.mBodyBIndex,
|
||||
cinfo.posB,
|
||||
cinfo.dir,
|
||||
cinfo.tangents,
|
||||
-cinfo.depth,
|
||||
&cinfo.MInvJacTB,
|
||||
&cinfo.jacB,
|
||||
&cinfo.GMInvGTB,
|
||||
&cinfo.tangentJacB,
|
||||
&cinfo.tangentGMInvGTB,
|
||||
cinfo.tangentJacB,
|
||||
cinfo.tangentMInvJacTB,
|
||||
cinfo.tangentGMInvGTB,
|
||||
&cinfo.biasVelocityB,
|
||||
cinfo.effectiveRestitution);
|
||||
}
|
||||
|
||||
/// Calculates the impulse that we apply on body_b to resolve the contact.
|
||||
void CalcFrictionImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo) {
|
||||
// Todo: add nonlinear effects * dt
|
||||
|
||||
double rhs_tangent[2] = {0., 0.};
|
||||
if (body_a && !body_a->mIsStatic) {
|
||||
rhs_tangent[0] += cinfo.tangentJacA[0] * body_a->qdot;
|
||||
rhs_tangent[1] += cinfo.tangentJacA[1] * body_a->qdot;
|
||||
}
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
rhs_tangent[0] -= cinfo.tangentJacB[0] * body_b->qdot;
|
||||
rhs_tangent[1] -= cinfo.tangentJacB[1] * body_b->qdot;
|
||||
}
|
||||
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
double denom = cinfo.tangentGMInvGTA[i] + cinfo.tangentGMInvGTB[i];
|
||||
assert (denom > cCollisionEps);
|
||||
double old_impulse = cinfo.accumFrictionImpulse[i];
|
||||
|
||||
cinfo.deltaFrictionImpulse[i] = rhs_tangent[i] / denom;
|
||||
cinfo.accumFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] + cinfo.deltaFrictionImpulse[i];
|
||||
if (cinfo.accumFrictionImpulse[i] >= cinfo.effectiveFriction * cinfo.accumImpulse) {
|
||||
cinfo.accumFrictionImpulse[i] = cinfo.effectiveFriction * cinfo.accumImpulse;
|
||||
}
|
||||
if (cinfo.accumFrictionImpulse[i] < -cinfo.effectiveFriction * cinfo.accumImpulse) {
|
||||
cinfo.accumFrictionImpulse[i] = -cinfo.effectiveFriction * cinfo.accumImpulse;
|
||||
}
|
||||
cinfo.deltaFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] - old_impulse;
|
||||
|
||||
assert (!isnan(cinfo.deltaFrictionImpulse[i]));
|
||||
}
|
||||
}
|
||||
|
||||
void ApplyFrictionImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo) {
|
||||
if (body_a && !body_a->mIsStatic) {
|
||||
body_a->qdot +=
|
||||
cinfo.tangentMInvJacTA[0] * (-cinfo.deltaFrictionImpulse[0]);
|
||||
body_a->qdot +=
|
||||
cinfo.tangentMInvJacTA[1] * (-cinfo.deltaFrictionImpulse[1]);
|
||||
|
||||
assert(!isnan(body_a->qdot.squaredNorm()));
|
||||
}
|
||||
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
body_b->qdot +=
|
||||
-cinfo.tangentMInvJacTB[0] * (-cinfo.deltaFrictionImpulse[0]);
|
||||
body_b->qdot +=
|
||||
-cinfo.tangentMInvJacTB[1] * (-cinfo.deltaFrictionImpulse[1]);
|
||||
|
||||
assert(!isnan(body_b->qdot.squaredNorm()));
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculates the impulse that we apply on body_b to resolve the contact.
|
||||
void CalcConstraintImpulse(
|
||||
SimBody* body_a,
|
||||
SimBody* body_b,
|
||||
CollisionInfo& cinfo,
|
||||
const double dt) {
|
||||
CollisionInfo& cinfo) {
|
||||
// Todo: add nonlinear effects * dt
|
||||
|
||||
double ref_a = 0.;
|
||||
double ref_b = 0.;
|
||||
double vel_a = 0.;
|
||||
double vel_b = 0.;
|
||||
double ref = 0.;
|
||||
double rhs = 0.;
|
||||
if (body_a && !body_a->mIsStatic) {
|
||||
vel_a = cinfo.jacA * body_a->qdot;
|
||||
ref_a += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
|
||||
rhs += cinfo.jacA * body_a->qdot + cinfo.biasVelocityA;
|
||||
}
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
vel_b = cinfo.jacB * body_b->qdot;
|
||||
ref_b += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
|
||||
rhs += -cinfo.jacB * body_b->qdot - cinfo.biasVelocityB;
|
||||
}
|
||||
|
||||
ref = ref_a + ref_b;
|
||||
|
||||
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
|
||||
assert(denom > cCollisionEps);
|
||||
|
||||
|
|
@ -723,7 +788,10 @@ void World::resolveCollisions(double dt) {
|
|||
int num_iter = 20;
|
||||
for (int i = 0; i < num_iter; i++) {
|
||||
for (CollisionInfo& cinfo : mContactPoints) {
|
||||
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
|
||||
CalcFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
|
||||
ApplyFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
|
||||
|
||||
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
|
||||
ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -275,7 +275,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
|
||||
|
||||
SECTION("EnsureImpulseDirection") {
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
|
||||
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
|
||||
}
|
||||
|
|
@ -289,7 +289,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
}
|
||||
|
||||
SECTION("CalculateImpulse") {
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
|
||||
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
|
||||
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
|
|
@ -298,7 +298,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
|
||||
SECTION("ImpulseMustNotPull") {
|
||||
sphere_a_body.qdot[1] = 1.23;
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12);
|
||||
}
|
||||
|
||||
|
|
@ -306,7 +306,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
cinfo.effectiveRestitution = 1.0;
|
||||
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
|
||||
VectorNd old_vel = sphere_a_body.qdot;
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12);
|
||||
}
|
||||
|
|
@ -332,7 +332,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
PrepareConstraintImpulse(0.001, &sphere_a_body, &ground_body, cinfo);
|
||||
|
||||
SECTION("EnsureImpulseDirection") {
|
||||
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
|
||||
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
|
||||
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
|
||||
}
|
||||
|
|
@ -346,7 +346,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
}
|
||||
|
||||
SECTION("CalculateImpulse") {
|
||||
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
|
||||
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
|
||||
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
|
||||
ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
|
||||
|
|
@ -390,7 +390,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
}
|
||||
|
||||
SECTION("CalculateImpulse") {
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
REQUIRE(sphere_a_body.qdot[1] > -0.1);
|
||||
REQUIRE(sphere_b_body.qdot[1] < 0.1);
|
||||
|
|
@ -401,7 +401,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
|
||||
VectorNd old_vel_a = sphere_a_body.qdot;
|
||||
VectorNd old_vel_b = sphere_b_body.qdot;
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12);
|
||||
REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12);
|
||||
|
|
@ -444,7 +444,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
}
|
||||
|
||||
SECTION("CalculateImpulse") {
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
REQUIRE(sphere_a_body.qdot[1] < 0.1);
|
||||
REQUIRE(sphere_b_body.qdot[1] > -0.1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue