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

Fixed simulation crash by increasing the manifold array as I'm getting more manifolds than expected.

master
Martin Felis 2020-11-20 23:16:37 +01:00
parent 8dd349752d
commit 4b19fd9c0b
3 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/rbdlsim.h
View File

@ -47,7 +47,7 @@ struct CollisionInfo {
const SimShape* mShapeB = nullptr;
int mBodyAIndex;
int mBodyBIndex;
Vector3d mManifoldPoints[4];
Vector3d mManifoldPoints[8];
int mNumManifoldPoints = 0;
Vector3d posA = Vector3d::Zero();
Vector3d posB = Vector3d::Zero();

34
src/rbdlsim.cc
View File

@ -248,6 +248,14 @@ bool CheckPenetrationBoxVsPlane(
bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
if (isnan(cinfo.posA.squaredNorm())) {
gLog ("NaN error!");
}
assert (!isnan(cinfo.dir.squaredNorm()));
assert (!isnan(cinfo.posA.squaredNorm()));
assert (!isnan(cinfo.posB.squaredNorm()));
cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
@ -297,14 +305,14 @@ bool CheckPenetrationAABBVsPlane(
const Vector3d& scale = shape_a.scale;
// clang-format off
Vector3d vertices[8] = {
Vector3d( scale[0] * 0.5, -scale[0] * 0.5, scale[0] * 0.5),
Vector3d( scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
Vector3d( scale[0] * 0.5, scale[0] * 0.5, scale[0] * 0.5),
Vector3d( scale[0] * 0.5, scale[0] * 0.5, -scale[0] * 0.5),
Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, scale[0] * 0.5),
Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
Vector3d(-scale[0] * 0.5, scale[0] * 0.5, scale[0] * 0.5),
Vector3d(-scale[0] * 0.5, scale[0] * 0.5, -scale[0] * 0.5)
Vector3d( scale[0] * 0.5, -scale[1] * 0.5, scale[2] * 0.5),
Vector3d( scale[0] * 0.5, -scale[1] * 0.5, -scale[2] * 0.5),
Vector3d( scale[0] * 0.5, scale[1] * 0.5, scale[2] * 0.5),
Vector3d( scale[0] * 0.5, scale[1] * 0.5, -scale[2] * 0.5),
Vector3d(-scale[0] * 0.5, -scale[1] * 0.5, scale[2] * 0.5),
Vector3d(-scale[0] * 0.5, -scale[1] * 0.5, -scale[2] * 0.5),
Vector3d(-scale[0] * 0.5, scale[1] * 0.5, scale[2] * 0.5),
Vector3d(-scale[0] * 0.5, scale[1] * 0.5, -scale[2] * 0.5)
};
// clang-format on
@ -351,6 +359,9 @@ bool CheckPenetrationAABBVsPlane(
assert(s <= 1.);
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
if (cinfo.mNumManifoldPoints > 4) {
gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints);
}
}
}
}
@ -476,6 +487,8 @@ void CalcImpulseVariables(
const VectorNd& q = body->q;
const VectorNd& qdot = body->qdot;
assert (!isnan(q.squaredNorm()));
// Calculate local coordinates of the contact point
UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
Vector3d point_local_b =
@ -619,10 +632,15 @@ void World::detectCollisions() {
cinfo.effectiveRestitution = ref_body_shape.restitution;
if (has_penetration) {
if (isnan(cinfo.posA.squaredNorm())) {
gLog ("NaN error!");
}
cinfo.mBodyA = nullptr;
cinfo.mBodyAIndex = -1;
cinfo.mBodyB = &ref_body;
cinfo.mBodyBIndex = body_col_info.first;
assert (!isnan(cinfo.posA.squaredNorm()));
assert (!isnan(cinfo.posB.squaredNorm()));
mContactPoints.push_back(cinfo);
}
}

4
src/simulator.cc
View File

@ -43,9 +43,9 @@ void simulator_init() {
sWorld.mStaticShapes.push_back(sGroundShape);
double restitution = 0.2;
double restitution = 0.01;
int num_bodies = 1;
int num_bodies = 5;
for (int i = 0; i < num_bodies; i++) {
SimBody body = CreateBoxBody(
1.,