martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Storing MInv * jacT instead of each separately, fixed failing due to size of spheres being now the diameter instead of radius.

master
Martin Felis 2020-11-24 21:45:38 +01:00
parent 3fd715611d
commit b00e78cd9c
3 changed files with 19 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/rbdlsim.h
View File

@ -58,8 +58,8 @@ struct CollisionInfo {
Vector3d dir = Vector3d::Zero();
VectorNd jacA = VectorNd::Zero(1);
VectorNd jacB = VectorNd::Zero(1);
MatrixNd MInvA = MatrixNd::Zero(1, 1);
MatrixNd MInvB = MatrixNd::Zero(1, 1);
VectorNd MInvJacTA = VectorNd::Zero(1);
VectorNd MInvJacTB = VectorNd::Zero(1);
double GMInvGTA = 0.;
double GMInvGTB = 0.;
double tangentAccumImpulse0 = 0.;
@ -132,9 +132,11 @@ SimBody CreateBoxBody(
const Vector3d& vel);
void PrepareConstraintImpulse(
double dt,
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo);
void CalcCollisions(
SimBody& body_a,
SimBody& body_b,

20
src/rbdlsim.cc
View File

@ -512,9 +512,7 @@ void CalcImpulseVariables(
const Vector3d& pos,
const Vector3d& dir,
const double depth,
const Vector3d& tangent0,
const Vector3d& tangent1,
MatrixNd* MInv,
VectorNd* MInvJacT,
VectorNd* jac,
double* G_MInv_GT,
MatrixNd* tangentJac,
@ -543,13 +541,13 @@ void CalcImpulseVariables(
// Compute vectors and matrices of the contact system
MatrixNd M(MatrixNd::Zero(ndof, ndof));
CompositeRigidBodyAlgorithm(*model, q, M, false);
*MInv = M.inverse();
MatrixNd G_constr(MatrixNd::Zero(3, ndof));
CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
(*jac) = dir.transpose() * G_constr;
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
(*MInvJacT) = M.llt().solve(jac->transpose());
*G_MInv_GT = (*jac) * (*MInvJacT);
assert(!isnan(*G_MInv_GT));
double beta = 0.01;
@ -574,9 +572,7 @@ void PrepareConstraintImpulse(
cinfo.posA,
cinfo.dir,
cinfo.depth,
cinfo.tangent0,
cinfo.tangent1,
&cinfo.MInvA,
&cinfo.MInvJacTA,
&cinfo.jacA,
&cinfo.GMInvGTA,
&cinfo.tangentJacA,
@ -591,9 +587,7 @@ void PrepareConstraintImpulse(
cinfo.posB,
cinfo.dir,
-cinfo.depth,
cinfo.tangent0,
cinfo.tangent1,
&cinfo.MInvB,
&cinfo.MInvJacTB,
&cinfo.jacB,
&cinfo.GMInvGTB,
&cinfo.tangentJacB,
@ -645,12 +639,12 @@ void ApplyConstraintImpulse(
CollisionInfo& cinfo) {
if (body_a && !body_a->mIsStatic) {
body_a->qdot +=
cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
cinfo.MInvJacTA * (-cinfo.deltaImpulse);
assert(!isnan(body_a->qdot.squaredNorm()));
}
if (body_b && !body_b->mIsStatic) {
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
body_b->qdot += cinfo.MInvJacTB * (cinfo.deltaImpulse);
assert(!isnan(body_b->qdot.squaredNorm()));
}
}

16
tests/CollisionTests.cc
View File

@ -24,14 +24,14 @@ TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
CollisionInfo cinfo;
SECTION("Box and Sphere Touching") {
sphere.pos.set(0., 1.5, 0.);
sphere.pos.set(0., 1.0, 0.);
cresult = CheckPenetration(box, sphere, cinfo);
REQUIRE(cresult == true);
}
SECTION("Box and Sphere Intersecting") {
sphere.pos.set(0., 1.4, 0.);
sphere.pos.set(0., 0.9, 0.);
cresult = CheckPenetration(box, sphere, cinfo);
REQUIRE(cresult == true);
@ -272,7 +272,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
REQUIRE(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
@ -304,7 +304,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
VectorNd old_vel = sphere_a_body.qdot;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
@ -329,7 +329,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
REQUIRE(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
PrepareConstraintImpulse(0.001, &sphere_a_body, &ground_body, cinfo);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
@ -377,7 +377,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
@ -398,7 +398,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
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);
@ -431,7 +431,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
SECTION("CheckForceAndJacobianDirections") {
REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);