From 3fd715611d93409172da509db4cdc45788d81cf0 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Sat, 21 Nov 2020 21:18:57 +0100 Subject: [PATCH] Added applying slight separation impulse in case of penetration, also started working on friction along tangents. --- include/rbdlsim.h | 12 ++++++++ src/rbdlsim.cc | 75 +++++++++++++++++++++++++++++++++++++---------- src/simulator.cc | 2 +- 3 files changed, 73 insertions(+), 16 deletions(-) diff --git a/include/rbdlsim.h b/include/rbdlsim.h index 973152a..28f6153 100644 --- a/include/rbdlsim.h +++ b/include/rbdlsim.h @@ -62,6 +62,18 @@ struct CollisionInfo { MatrixNd MInvB = MatrixNd::Zero(1, 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 effectiveRestitution = 1.0; double depth = 0.; diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 7e3002a..bb0d038 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -102,29 +102,48 @@ static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) { cinfo.posB = temp_pos; } +static void sCalcTangentVectors(const Vector3d &normal, Vector3d* tangent0, Vector3d* tangent1) { + if (fabs(normal.dot(Vector3d(0., 0., 1.))) < 0.6) { + *tangent0 = normal.cross(Vector3d(0., 0., 1.)); + *tangent1 = tangent0->cross(normal); + } else { + *tangent0 = normal.cross(Vector3d(1., 0., 0.)); + *tangent1 = tangent0->cross(normal); + } +} + bool CheckPenetration( const SimShape& shape_a, const SimShape& shape_b, CollisionInfo& cinfo) { cinfo.mShapeA = &shape_a; cinfo.mShapeB = &shape_b; + bool result = false; if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) { - return CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo); - } - if (shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) { - bool result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo); - sSwapCollisionInfoShapeOrder(cinfo); + result = CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo); + sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); return result; - } - if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) { - return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo); - } - if (shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) { - return CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo); - } - if (shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) { + } 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); + 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); + 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); + 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); return result; } @@ -487,13 +506,19 @@ void CalcCollisions( } void CalcImpulseVariables( + double dt, SimBody* body, unsigned int body_index, const Vector3d& pos, const Vector3d& dir, + const double depth, + const Vector3d& tangent0, + const Vector3d& tangent1, MatrixNd* MInv, VectorNd* jac, double* G_MInv_GT, + MatrixNd* tangentJac, + MatrixNd* tangentGMInvGT, double* bias_vel, double restitution) { if (body == nullptr || body->mIsStatic) { @@ -527,32 +552,52 @@ void CalcImpulseVariables( *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose(); assert(!isnan(*G_MInv_GT)); - *bias_vel = (*jac) * qdot * restitution; + double beta = 0.01; + double delta_slop = cCollisionEps; + *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05); + + (*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(); } void PrepareConstraintImpulse( + double dt, SimBody* body_a, SimBody* body_b, CollisionInfo& cinfo) { CalcImpulseVariables( + dt, body_a, cinfo.mBodyAIndex, cinfo.posA, cinfo.dir, + cinfo.depth, + cinfo.tangent0, + cinfo.tangent1, &cinfo.MInvA, &cinfo.jacA, &cinfo.GMInvGTA, + &cinfo.tangentJacA, + &cinfo.tangentGMInvGTA, &cinfo.biasVelocityA, cinfo.effectiveRestitution); CalcImpulseVariables( + dt, body_b, cinfo.mBodyBIndex, cinfo.posB, cinfo.dir, + -cinfo.depth, + cinfo.tangent0, + cinfo.tangent1, &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB, + &cinfo.tangentJacB, + &cinfo.tangentGMInvGTB, &cinfo.biasVelocityB, cinfo.effectiveRestitution); } @@ -678,7 +723,7 @@ void World::detectCollisions() { void World::resolveCollisions(double dt) { for (CollisionInfo& cinfo : mContactPoints) { - PrepareConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); + PrepareConstraintImpulse(dt, cinfo.mBodyA, cinfo.mBodyB, cinfo); } int num_iter = 20; diff --git a/src/simulator.cc b/src/simulator.cc index d1bec58..5eb765f 100644 --- a/src/simulator.cc +++ b/src/simulator.cc @@ -102,7 +102,7 @@ void simulator_reset() { for (int i = 0; i < sWorld.mBodies.size(); i++) { sWorld.mBodies[i].q.block(0, 0, 3, 1) = - Vector3d::Random() * 3. + Vector3d(0., 5., 0.); + Vector3d::Random() * 0.3 + Vector3d(0., 5., 0.); sWorld.mBodies[i].q[2] = 0.; }