diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index b7e2102..7e3002a 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -38,7 +38,7 @@ void SimBody::calcNextPositions( Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1)); Quaternion qd = q0.omegaToQDot(omega); Quaternion q1 = (q0 + qd * dt).normalize(); - assert (!isnan(q1.squaredNorm())); + assert(!isnan(q1.squaredNorm())); mModel.SetQuaternion(i, q1, io_q); } } @@ -81,7 +81,7 @@ void SimShapeSupport( len = ccdVec3Len2(&dir); if (len - CCD_EPS > CCD_ZERO) { ccdVec3Copy(v, &dir); - ccdVec3Scale(v, shape->scale[0] / CCD_SQRT(len)); + ccdVec3Scale(v, shape->scale[0] * CCD_REAL(0.5) / CCD_SQRT(len)); } else { ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO); } @@ -95,7 +95,7 @@ void SimShapeSupport( ccdVec3Add(v, &pos); } -static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) { +static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) { cinfo.dir *= -1.; Vector3d temp_pos = cinfo.posA; cinfo.posA = cinfo.posB; @@ -186,11 +186,6 @@ bool CheckPenetrationSphereVsPlane( assert(shape_a.mType == SimShape::Sphere); assert(shape_b.mType == SimShape::Plane); - // For now only support aligned spheres - assert( - (shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm() - < cCollisionEps); - Vector3d plane_normal = shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.)); Vector3d plane_point = shape_b.pos; @@ -203,6 +198,8 @@ bool CheckPenetrationSphereVsPlane( cinfo.dir = -plane_normal; cinfo.depth = sphere_center_height; + cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = + sphere_point_to_plane - sphere_center_height * plane_normal; cinfo.posA = sphere_point_to_plane; cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal; @@ -249,17 +246,23 @@ bool CheckPenetrationBoxVsPlane( bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo); if (isnan(cinfo.posA.squaredNorm())) { - gLog ("NaN error!"); + gLog("NaN error!"); } - assert (!isnan(cinfo.dir.squaredNorm())); - assert (!isnan(cinfo.posA.squaredNorm())); - assert (!isnan(cinfo.posB.squaredNorm())); + 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); + // also transform all contact manifold points + for (int i = 0; i < cinfo.mNumManifoldPoints; i++) { + cinfo.mManifoldPoints[i] = + shape_a_rot.transpose() * cinfo.mManifoldPoints[i] + shape_a.pos; + } + return result; } @@ -322,7 +325,8 @@ bool CheckPenetrationAABBVsPlane( for (int i = 0; i < 8; i++) { distances[i] = (vertices[i] - plane_pos).dot(plane_normal); if (distances[i] >= 0. && distances[i] < cCollisionEps) { - cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i]; + cinfo.mManifoldPoints[cinfo.mNumManifoldPoints] = vertices[i]; + cinfo.mNumManifoldPoints++; } max_depth = distances[i] < max_depth ? distances[i] : max_depth; } @@ -358,9 +362,26 @@ bool CheckPenetrationAABBVsPlane( assert(s >= 0); assert(s <= 1.); - cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0); + s = s < cCollisionEps ? 0. : s; + s = s > 1.0 - cCollisionEps ? 1.0 : s; + + Vector3d vc = v0 + s * (v1 - v0); + bool found_duplicate_point = false; + for (int j = 0; j < cinfo.mNumManifoldPoints; j++) { + if ((cinfo.mManifoldPoints[j] - vc).squaredNorm() < cCollisionEps) { + gLog("Removing duplicate point"); + found_duplicate_point = true; + break; + } + } + + if (!found_duplicate_point) { + cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = + v0 + s * (v1 - v0); + } + if (cinfo.mNumManifoldPoints > 4) { - gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints); + gLog("Have %d manifold points?!", cinfo.mNumManifoldPoints); } } } @@ -487,7 +508,7 @@ void CalcImpulseVariables( const VectorNd& q = body->q; const VectorNd& qdot = body->qdot; - assert (!isnan(q.squaredNorm())); + assert(!isnan(q.squaredNorm())); // Calculate local coordinates of the contact point UpdateKinematicsCustom(*model, &q, nullptr, nullptr); @@ -504,7 +525,7 @@ void CalcImpulseVariables( (*jac) = dir.transpose() * G_constr; *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose(); - assert (!isnan(*G_MInv_GT)); + assert(!isnan(*G_MInv_GT)); *bias_vel = (*jac) * qdot * restitution; } @@ -564,7 +585,7 @@ void CalcConstraintImpulse( ref = ref_a + ref_b; double denom = cinfo.GMInvGTA + cinfo.GMInvGTB; - assert (denom > cCollisionEps); + assert(denom > cCollisionEps); double old_impulse = cinfo.accumImpulse; // TODO: is this really needed here?? @@ -580,18 +601,18 @@ void ApplyConstraintImpulse( if (body_a && !body_a->mIsStatic) { body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse); - assert (!isnan(body_a->qdot.squaredNorm())); + assert(!isnan(body_a->qdot.squaredNorm())); } if (body_b && !body_b->mIsStatic) { body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse); - assert (!isnan(body_b->qdot.squaredNorm())); + assert(!isnan(body_b->qdot.squaredNorm())); } } void World::calcUnconstrainedVelUpdate(double dt) { for (SimBody& body : mBodies) { - assert (!isnan(body.q.squaredNorm())); + assert(!isnan(body.q.squaredNorm())); ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot); @@ -633,15 +654,22 @@ void World::detectCollisions() { if (has_penetration) { if (isnan(cinfo.posA.squaredNorm())) { - gLog ("NaN error!"); + 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); + assert(!isnan(cinfo.posA.squaredNorm())); + assert(!isnan(cinfo.posB.squaredNorm())); + + for (int i = 0; i < cinfo.mNumManifoldPoints; i++) { + CollisionInfo cpinfo(cinfo); + cinfo.posA = cinfo.mManifoldPoints[i]; + cinfo.posB = cinfo.mManifoldPoints[i]; + mContactPoints.push_back(cinfo); + } } } } @@ -767,4 +795,4 @@ SimBody CreateBoxBody( return result; } -} // namespace RBDLSim \ No newline at end of file +} // namespace RBDLSim diff --git a/src/simulator.cc b/src/simulator.cc index 3190db2..d1bec58 100644 --- a/src/simulator.cc +++ b/src/simulator.cc @@ -43,23 +43,29 @@ void simulator_init() { sWorld.mStaticShapes.push_back(sGroundShape); - double restitution = 0.01; + double restitution = 0.3; int num_bodies = 5; for (int i = 0; i < num_bodies; i++) { - SimBody body = CreateBoxBody( - 1., - Vector3d(2., 1., 1.), - restitution, - Vector3d::Random() * 5., - Vector3d::Zero()); + SimBody body; -// SimBody body = CreateSphereBody( -// 1., -// 1., -// restitution, -// Vector3d::Random() * 5., -// Vector3d::Zero()); + bool create_sphere = false; + + if (!create_sphere) { + body = CreateBoxBody( + 1., + Vector3d(2., 1., 1.), + restitution, + Vector3d::Random() * 5., + Vector3d::Zero()); + } else { + body = CreateSphereBody( + 1., + 1., + restitution, + Vector3d::Random() * 5., + Vector3d::Zero()); + } sWorld.mBodies.push_back(body); } @@ -232,14 +238,14 @@ void simulator_draw(srcmdbuf* cmdbuf) { gLog("Error: cannot render shape of type %d", cinfo.second.mType); } - simd4x4f trans = simd4x4f_create ( + simd4x4f trans = simd4x4f_create( // clang-format off simd4f_create(1.f, 0.f, 0.f, 0.f), simd4f_create(0.f, 1.f, 0.f, 0.f), simd4f_create(0.f, 0.f, 1.f, 0.f), simd4f_create(cinfo.second.pos[0], cinfo.second.pos[1], cinfo.second.pos[2], 1.f) // clang-format on - ); + ); simd4x4f scale = simd4x4f_create( // clang-format off