From 091c99b5cfe1b5288e6fa7373beda95b194ebd5b Mon Sep 17 00:00:00 2001 From: Irlan Date: Mon, 27 Feb 2017 02:06:33 -0300 Subject: [PATCH] fix hull mass data calculation and make it more robust, bugfixes --- include/bounce/common/math/quat.h | 13 +- include/bounce/dynamics/body.h | 23 ++-- include/bounce/dynamics/shapes/hull_shape.h | 4 +- include/bounce/dynamics/time_step.h | 50 +++++--- include/testbed/tests/jenga.h | 2 +- include/testbed/tests/multiple_shapes.h | 121 ++++++++++++++++++ .../tests/{quadrics.h => quadric_shapes.h} | 32 ++--- include/testbed/tests/quickhull_test.h | 90 +++++++++---- src/bounce/dynamics/body.cpp | 17 ++- .../contacts/collide/collide_capsules.cpp | 60 +++++---- src/bounce/dynamics/island.cpp | 11 +- src/bounce/dynamics/shapes/capsule_shape.cpp | 2 - src/bounce/dynamics/shapes/hull_shape.cpp | 79 +++++++----- src/bounce/dynamics/shapes/sphere_shape.cpp | 10 +- src/testbed/framework/test_entries.cpp | 12 +- 15 files changed, 359 insertions(+), 167 deletions(-) create mode 100644 include/testbed/tests/multiple_shapes.h rename include/testbed/tests/{quadrics.h => quadric_shapes.h} (87%) diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h index 1ae95e3..40f1774 100644 --- a/include/bounce/common/math/quat.h +++ b/include/bounce/common/math/quat.h @@ -118,15 +118,12 @@ struct b3Quat *axis = s * v; } - *angle = 0.0f; // cosine check - if (w >= -1.0f && w <= 1.0f) - { - // half angle - float32 theta = acos(w); - // full angle - *angle = 2.0f * theta; - } + float32 cosine = b3Clamp(w, -1.0f, 1.0f); + // half angle + float32 theta = acos(cosine); + // full angle + *angle = 2.0f * theta; } float32 x, y, z, w; diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index 7136a0d..7358953 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -112,6 +112,7 @@ public: // Set the body world transform from a position, axis of rotation and an angle // of rotation about the axis. + // The transform defines a reference frame for this body world center of mass. // However, manipulating a body transform during the simulation may cause non-physical behaviour. void SetTransform(const b3Vec3& position, const b3Vec3& axis, float32 angle); @@ -185,13 +186,16 @@ public: // Set this body mass data. void SetMassData(const b3MassData* data); - // Get the linear kinetic energy of the body in Joules (kilogram-meters squared per second squared). + // Recalculate this body mass data based on all of its shapes. + void ResetMass(); + + // Get the linear kinetic energy of the body in Joules (kg m^2/s^2). float32 GetLinearEnergy() const; - // Get the angular kinetic energy of the body in Joules (kilogram-meters squared per second squared). + // Get the angular kinetic energy of the body in Joules (kg m^2/s^2). float32 GetAngularEnergy() const; - // Get the total kinetic energy of the body in Joules (kilogram-meters squared per second squared). + // Get the total kinetic energy of the body in Joules (kg m^2/s^2). float32 GetEnergy() const; // Transform a vector to the local space of this body. @@ -257,16 +261,8 @@ private: // Destroy all joints connected to the body. void DestroyJoints(); - // Recalculate the mass of the body based on the shapes associated - // with it. - void ResetMass(); - - // Synchronize this body transform with its world - // center of mass and orientation. - void SynchronizeTransform(); - - // Synchronize this body shape AABBs with the synchronized transform. void SynchronizeShapes(); + void SynchronizeTransform(); // Check if this body should collide with another. bool ShouldCollide(const b3Body* other) const; @@ -306,7 +302,10 @@ private: b3Vec3 m_linearVelocity; b3Vec3 m_angularVelocity; + // Motion proxy for CCD. b3Sweep m_sweep; + + // The body origin transform. b3Transform m_xf; // The parent world of this body. diff --git a/include/bounce/dynamics/shapes/hull_shape.h b/include/bounce/dynamics/shapes/hull_shape.h index 4324cd0..9424541 100644 --- a/include/bounce/dynamics/shapes/hull_shape.h +++ b/include/bounce/dynamics/shapes/hull_shape.h @@ -30,8 +30,8 @@ public : ~b3HullShape(); void Swap(const b3HullShape& other); - - void ComputeMass(b3MassData* data, float32 density) const; + + void ComputeMass(b3MassData* data, float32 density) const; void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const; diff --git a/include/bounce/dynamics/time_step.h b/include/bounce/dynamics/time_step.h index c151776..4fb1309 100644 --- a/include/bounce/dynamics/time_step.h +++ b/include/bounce/dynamics/time_step.h @@ -51,22 +51,42 @@ enum b3LimitState e_equalLimits }; -// Move an inertia tensor from the its current center -// to another. -inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center) +// Return the Steiner's matrix given the displacement vector from the old +// center of rotation to the new center of rotation. +// The result equals to transpose( skew(v) ) * skew(v) or diagonal(v^2) - outer(v) +inline b3Mat33 b3Steiner(const b3Vec3& v) { - // Paralell Axis Theorem - // J = I + m * dot(r, r) * E - outer(r, r) - // where - // I - inertia about the center of mass - // m - mass - // E - identity 3x3 - // r - displacement vector from the current com to the new com - // J - inertia tensor at the new center of rotation - float32 dd = b3Dot(center, center); - b3Mat33 A = b3Diagonal(mass * dd); - b3Mat33 B = b3Outer(center, center); - return inertia + A - B; + float32 xx = v.x * v.x; + float32 yy = v.y * v.y; + float32 zz = v.z * v.z; + + b3Mat33 S; + + S.x.x = yy + zz; + S.x.y = -v.x * v.y; + S.x.z = -v.x * v.z; + + S.y.x = S.x.y; + S.y.y = xx + zz; + S.y.z = -v.y * v.z; + + S.z.x = S.x.z; + S.z.y = S.y.z; + S.z.z = xx + yy; + + return S; +} + +// Move an inertia tensor given the displacement vector from the center of mass to the translated origin. +inline b3Mat33 b3MoveToOrigin(const b3Mat33& I, const b3Vec3& v) +{ + return I + b3Steiner(v); +} + +// Move an inertia tensor given the displacement vector from the origin to the translated center of mass. +inline b3Mat33 b3MoveToCOM(const b3Mat33& I, const b3Vec3& v) +{ + return I - b3Steiner(v); } // Compute the inertia matrix of a body measured in diff --git a/include/testbed/tests/jenga.h b/include/testbed/tests/jenga.h index 87ea01c..288ba8a 100644 --- a/include/testbed/tests/jenga.h +++ b/include/testbed/tests/jenga.h @@ -96,7 +96,7 @@ public: b3ShapeDef sd; sd.shape = &hs; sd.density = 0.1f; - sd.friction = 0.1f; + sd.friction = 0.3f; body->CreateShape(sd); } diff --git a/include/testbed/tests/multiple_shapes.h b/include/testbed/tests/multiple_shapes.h new file mode 100644 index 0000000..544d407 --- /dev/null +++ b/include/testbed/tests/multiple_shapes.h @@ -0,0 +1,121 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef MULTIPLE_SHAPES_H +#define MULTIPLE_SHAPES_H + +class MultipleShapes : public Test +{ +public: + MultipleShapes() + { + g_camera.m_center.Set(2.0f, -2.0f, 0.0f); + g_camera.m_zoom = 50.0f; + g_settings.drawCenterOfMasses = true; + + { + b3BodyDef bd; + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + hs.m_hull = &m_groundHull; + + b3ShapeDef sd; + sd.shape = &hs; + + body->CreateShape(sd); + } + + { + b3Transform xf; + xf.SetIdentity(); + xf.position.Set(-5.0f, 10.0f, 0.0f); + m_box1.SetTransform(xf); + } + + { + b3Transform xf; + xf.SetIdentity(); + xf.position.Set(5.0f, 10.0f, 0.0f); + m_box2.SetTransform(xf); + } + + { + b3Transform xf; + xf.SetIdentity(); + xf.position.Set(0.0f, 2.0f, 0.0f); + m_box3.SetTransform(xf); + } + + { + b3Transform xf; + xf.SetIdentity(); + xf.position.Set(0.0f, 6.0f, 0.0f); + m_box4.SetTransform(xf); + } + + { + b3Transform xf; + xf.SetIdentity(); + xf.position.Set(0.0f, 10.0f, 0.0f); + m_box5.SetTransform(xf); + } + + { + b3BodyDef bd; + bd.type = e_dynamicBody; + bd.angularVelocity.Set(0.0f, B3_PI, 0.0f); + + b3Body* body = m_world.CreateBody(bd); + + b3HullShape hs; + + b3ShapeDef sd; + sd.shape = &hs; + sd.density = 0.1f; + + hs.m_hull = &m_box1; + body->CreateShape(sd); + + hs.m_hull = &m_box2; + body->CreateShape(sd); + + hs.m_hull = &m_box3; + body->CreateShape(sd); + + hs.m_hull = &m_box4; + body->CreateShape(sd); + + hs.m_hull = &m_box5; + body->CreateShape(sd); + } + } + + static Test* Create() + { + return new MultipleShapes(); + } + + b3BoxHull m_box1; + b3BoxHull m_box2; + b3BoxHull m_box3; + b3BoxHull m_box4; + b3BoxHull m_box5; +}; + +#endif \ No newline at end of file diff --git a/include/testbed/tests/quadrics.h b/include/testbed/tests/quadric_shapes.h similarity index 87% rename from include/testbed/tests/quadrics.h rename to include/testbed/tests/quadric_shapes.h index f4559c8..f1d49f8 100644 --- a/include/testbed/tests/quadrics.h +++ b/include/testbed/tests/quadric_shapes.h @@ -16,8 +16,8 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef QUADRIC_H -#define QUADRIC_H +#ifndef QUADRIC_SHAPES_H +#define QUADRIC_SHAPES_H #include @@ -25,39 +25,41 @@ extern DebugDraw* g_debugDraw; extern Camera g_camera; extern Settings g_settings; -class Quadric : public Test +class QuadricShapes : public Test { public: - Quadric() + QuadricShapes() { + g_camera.m_center.Set(2.0f, -2.0f, 0.0f); g_camera.m_zoom = 20.0f; - g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI); - g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI); - g_camera.m_center.SetZero(); + g_settings.drawCenterOfMasses = true; { - qhHull hull; - b3StackArray points; ConstructCone(points); u32 size = qhGetMemorySize(points.Count()); void* p = b3Alloc(size); + + qhHull hull; hull.Construct(p, points); m_coneHull = ConvertHull(hull); + b3Free(p); } { - qhHull hull; b3StackArray points; ConstructCylinder(points); - u32 size = qhGetMemorySize(points.Count()); + const u32 size = qhGetMemorySize(points.Count()); void* p = b3Alloc(size); + + qhHull hull; hull.Construct(p, points); m_cylinderHull = ConvertHull(hull); + b3Free(p); } @@ -85,7 +87,7 @@ public: hull.m_hull = &m_coneHull; b3ShapeDef sdef; - sdef.density = 0.2f; + sdef.density = 0.1f; sdef.friction = 0.3f; sdef.shape = &hull; @@ -103,7 +105,7 @@ public: hull.m_hull = &m_cylinderHull; b3ShapeDef sdef; - sdef.density = 1.0f; + sdef.density = 0.1f; sdef.friction = 0.3f; sdef.shape = &hull; @@ -111,7 +113,7 @@ public: } } - ~Quadric() + ~QuadricShapes() { { b3Free(m_coneHull.vertices); @@ -130,7 +132,7 @@ public: static Test* Create() { - return new Quadric(); + return new QuadricShapes(); } b3Hull m_coneHull; diff --git a/include/testbed/tests/quickhull_test.h b/include/testbed/tests/quickhull_test.h index 8c94e2b..6fcdadf 100644 --- a/include/testbed/tests/quickhull_test.h +++ b/include/testbed/tests/quickhull_test.h @@ -25,6 +25,70 @@ extern DebugDraw* g_debugDraw; extern Camera g_camera; extern Settings g_settings; +inline b3Vec3 ComputeCentroid(const b3Hull& h) +{ + b3Vec3 c(0.0f, 0.0f, 0.0f); + float32 volume = 0.0f; + + // Pick reference point not too away from the origin + // to minimize floating point rounding errors. + b3Vec3 p1(0.0f, 0.0f, 0.0f); + // Put it inside the hull. + for (u32 i = 0; i < h.vertexCount; ++i) + { + p1 += h.vertices[i]; + } + p1 *= 1.0f / float32(h.vertexCount); + + const float32 inv4 = 0.25f; + const float32 inv6 = 1.0f / 6.0f; + const float32 inv60 = 1.0f / 60.0f; + const float32 inv120 = 1.0f / 120.0f; + + b3Vec3 diag(0.0f, 0.0f, 0.0f); + b3Vec3 offDiag(0.0f, 0.0f, 0.0f); + + // Triangulate convex polygons + for (u32 i = 0; i < h.faceCount; ++i) + { + const b3Face* face = h.GetFace(i); + const b3HalfEdge* begin = h.GetEdge(face->edge); + + const b3HalfEdge* edge = h.GetEdge(begin->next); + do + { + u32 i1 = begin->origin; + u32 i2 = edge->origin; + const b3HalfEdge* next = h.GetEdge(edge->next); + u32 i3 = next->origin; + + b3Vec3 p2 = h.vertices[i1]; + b3Vec3 p3 = h.vertices[i2]; + b3Vec3 p4 = h.vertices[i3]; + + b3Vec3 e1 = p2 - p1; + b3Vec3 e2 = p3 - p1; + b3Vec3 e3 = p4 - p1; + + float32 D = b3Det(e1, e2, e3); + + float32 tetraVolume = inv6 * D; + volume += tetraVolume; + + // Volume weighted centroid + c += tetraVolume * inv4 * (e1 + e2 + e3); + + edge = next; + } while (h.GetEdge(edge->next) != begin); + } + + // Centroid + B3_ASSERT(volume > B3_EPSILON); + c *= 1.0f / volume; + c += p1; + return c; +} + struct Pair { void* key; @@ -195,14 +259,7 @@ inline b3Hull ConvertHull(const qhHull& hull) out.faceCount = faceCount; out.faces = faces; out.planes = planes; - out.centroid.SetZero(); - - for (u32 i = 0; i < vertexCount; ++i) - { - out.centroid += vertices[i]; - } - out.centroid /= float32(vertexCount); - + out.centroid = ComputeCentroid(out); out.Validate(); return out; } @@ -351,23 +408,6 @@ public: m_qhull.Draw(g_debugDraw); } - void KeyDown(int button) - { - if (button == GLFW_KEY_LEFT) - { - //m_index = b3Max(m_index - 1, 0); - } - - if (button == GLFW_KEY_RIGHT) - { - //m_index = b3Min(m_index + 1, i32(horizon.Count()) - 1); - } - - if (button == GLFW_KEY_I) - { - } - } - static Test* Create() { return new QuickhullTest(); diff --git a/src/bounce/dynamics/body.cpp b/src/bounce/dynamics/body.cpp index 9b07a23..e068474 100644 --- a/src/bounce/dynamics/body.cpp +++ b/src/bounce/dynamics/body.cpp @@ -220,8 +220,7 @@ void b3Body::ResetMass() return; } - // Compute mass data for each shape. Each shape contributes to - // this body mass data. + // Accumulate the mass about the body origin of all shapes. b3Vec3 localCenter; localCenter.SetZero(); for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next) @@ -233,6 +232,7 @@ void b3Body::ResetMass() b3MassData massData; s->ComputeMass(&massData, s->m_density); + localCenter += massData.mass * massData.center; m_mass += massData.mass; m_I += massData.I; @@ -240,11 +240,17 @@ void b3Body::ResetMass() if (m_mass > 0.0f) { + // Compute local center of mass. m_invMass = 1.0f / m_mass; localCenter *= m_invMass; - // Center inertia about the center of mass. - m_I = b3MoveToCOM(m_I, m_mass, localCenter); + + // Shift inertia about the body origin into the body local center of mass. + m_I = m_I - m_mass * b3Steiner(localCenter); + + // Compute inverse inertia about the body local center of mass. m_invI = b3Inverse(m_I); + + // Align the inverse inertia with the world frame of the body. m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation); // Fix rotation. @@ -353,8 +359,7 @@ void b3Body::SetMassData(const b3MassData* massData) if (m_mass > 0.0f) { m_invMass = 1.0f / m_mass; - - m_I = b3MoveToCOM(massData->I, m_mass, massData->center); + m_I = massData->I - m_mass * b3Steiner(massData->center); m_invI = b3Inverse(m_I); m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation); diff --git a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp index 271b384..87d2720 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsules.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsules.cpp @@ -60,34 +60,6 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold, float32 totalRadius = hullA.radius + hullB.radius; - // todo return small distance output struct? - b3Vec3 pointA, pointB; - b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]); - float32 distance = b3Distance(pointA, pointB); - if (distance > totalRadius) - { - return; - } - - if (distance > B3_EPSILON) - { - b3Vec3 normal = (pointB - pointA) / distance; - b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal); - - manifold.pointCount = 1; - manifold.points[0].triangleKey = B3_NULL_TRIANGLE; - manifold.points[0].key = 0; - manifold.points[0].localNormal = b3MulT(xfA.rotation, normal); - manifold.points[0].localPoint = b3MulT(xfA, pointA); - manifold.points[0].localPoint2 = b3MulT(xfB, pointB); - - manifold.center = center; - manifold.normal = normal; - manifold.tangent1 = b3Perp(normal); - manifold.tangent2 = b3Cross(manifold.tangent1, normal); - return; - } - if (b3AreParalell(hullA, hullB)) { // Clip edge A against the side planes of edge B. @@ -97,8 +69,6 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold, b3ClipVertex clipEdgeA[2]; u32 clipCount = b3ClipEdgeToFace(clipEdgeA, edgeA, &hullB); - float32 totalRadius = hullA.radius + hullB.radius; - if (clipCount == 2) { b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdgeA[0].position, hullB.vertices[0], hullB.vertices[1]); @@ -136,7 +106,35 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold, manifold.normal = normal; manifold.tangent1 = b3Perp(normal); manifold.tangent2 = b3Cross(manifold.tangent1, normal); + + return; } } } -} + + b3Vec3 pointA, pointB; + b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]); + float32 distance = b3Distance(pointA, pointB); + if (distance > totalRadius) + { + return; + } + + if (distance > B3_EPSILON) + { + b3Vec3 normal = (pointB - pointA) / distance; + b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal); + + manifold.pointCount = 1; + manifold.points[0].triangleKey = B3_NULL_TRIANGLE; + manifold.points[0].key = 0; + manifold.points[0].localNormal = b3MulT(xfA.rotation, normal); + manifold.points[0].localPoint = b3MulT(xfA, pointA); + manifold.points[0].localPoint2 = b3MulT(xfB, pointB); + + manifold.center = center; + manifold.normal = normal; + manifold.tangent1 = b3Perp(normal); + manifold.tangent2 = b3Cross(manifold.tangent1, normal); + } +} \ No newline at end of file diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index d70a48c..9433696 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -87,14 +87,13 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, { float32 h = dt; - // Measure coefficient of damping. - // Box2D. + // Apply some damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 const float32 k_d = 0.1f; - float32 d = exp(-h * k_d); + float32 d = exp(-k_d * h); // 1. Integrate velocities for (u32 i = 0; i < m_bodyCount; ++i) @@ -121,7 +120,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, // Integrate torques // @todo add gyroscopic term - w += h * b3Mul(b->m_worldInvI, b->m_torque); + w += h * b->m_worldInvI * b->m_torque; // Clear torques b->m_torque.SetZero(); @@ -211,9 +210,9 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, w *= ratio; } - // Integrate position + // Integrate linear velocity x += h * v; - // Integrate orientation + // Integrate angular velocity q = b3Integrate(q, w, h); m_positions[i].x = x; diff --git a/src/bounce/dynamics/shapes/capsule_shape.cpp b/src/bounce/dynamics/shapes/capsule_shape.cpp index a77ad9a..a28aa6d 100644 --- a/src/bounce/dynamics/shapes/capsule_shape.cpp +++ b/src/bounce/dynamics/shapes/capsule_shape.cpp @@ -126,8 +126,6 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const massData->center = Ic_Capsule.center; massData->mass = Ic_Capsule.mass; massData->I = Ic; - - } void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const diff --git a/src/bounce/dynamics/shapes/hull_shape.cpp b/src/bounce/dynamics/shapes/hull_shape.cpp index 0fb1c77..4757b7b 100644 --- a/src/bounce/dynamics/shapes/hull_shape.cpp +++ b/src/bounce/dynamics/shapes/hull_shape.cpp @@ -18,6 +18,7 @@ #include #include +#include b3HullShape::b3HullShape() { @@ -46,7 +47,14 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const // Pick reference point not too away from the origin // to minimize floating point rounding errors. - b3Vec3 v1(0.0f, 0.0f, 0.0f); + b3Vec3 p1(0.0f, 0.0f, 0.0f); + + // Put it inside the hull. + for (u32 i = 0; i < m_hull->vertexCount; ++i) + { + p1 += m_hull->vertices[i]; + } + p1 *= 1.0f / float32(m_hull->vertexCount); const float32 inv4 = 0.25f; const float32 inv6 = 1.0f / 6.0f; @@ -54,7 +62,7 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const const float32 inv120 = 1.0f / 120.0f; b3Vec3 diag(0.0f, 0.0f, 0.0f); - b3Vec3 offDiag(0.0f, 0.0f, 0.0f); + b3Vec3 off_diag(0.0f, 0.0f, 0.0f); // Triangulate convex polygons for (u32 i = 0; i < m_hull->faceCount; ++i) @@ -70,22 +78,21 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const const b3HalfEdge* next = m_hull->GetEdge(edge->next); u32 i3 = next->origin; - b3Vec3 v2 = m_hull->vertices[i1]; - b3Vec3 v3 = m_hull->vertices[i2]; - b3Vec3 v4 = m_hull->vertices[i3]; + b3Vec3 p2 = m_hull->vertices[i1]; + b3Vec3 p3 = m_hull->vertices[i2]; + b3Vec3 p4 = m_hull->vertices[i3]; - // - b3Vec3 tetraCenter = inv4 * (v1 + v2 + v3 + v4); + b3Vec3 e1 = p2 - p1; + b3Vec3 e2 = p3 - p1; + b3Vec3 e3 = p4 - p1; + + float32 D = b3Det(e1, e2, e3); - b3Vec3 e1 = v2 - v1; - b3Vec3 e2 = v3 - v1; - b3Vec3 e3 = v4 - v1; - float32 det = b3Det(e1, e2, e3); - float32 tetraVolume = inv6 * det; - - // Volume weighted center of mass - center += tetraVolume * tetraCenter; + float32 tetraVolume = inv6 * D; volume += tetraVolume; + + // Volume weighted centroid + center += tetraVolume * inv4 * (e1 + e2 + e3); // Volume weighted inertia tensor // https://github.com/melax/sandbox @@ -94,37 +101,39 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const u32 j1 = (j + 1) % 3; u32 j2 = (j + 2) % 3; - diag[j] += inv60 * det * + diag[j] += inv60 * D * (e1[j] * e2[j] + e2[j] * e3[j] + e3[j] * e1[j] + - e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]); + e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]); - offDiag[j] += inv120 * det * + off_diag[j] += inv120 * D * (e1[j1] * e2[j2] + e2[j1] * e3[j2] + e3[j1] * e1[j2] + - e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] + - e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f); + e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] + + e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f); } edge = next; } while (m_hull->GetEdge(edge->next) != begin); } - B3_ASSERT(volume > 0.0f); - float32 invVolume = 0.0f; - if (volume != 0.0f) - { - invVolume = 1.0f / volume; - } - - diag = invVolume * diag; - offDiag = invVolume * offDiag; - - I.x.Set(diag.y + diag.z, -offDiag.z, -offDiag.y); - I.y.Set(-offDiag.z, diag.x + diag.z, -offDiag.x); - I.z.Set(-offDiag.y, -offDiag.x, diag.x + diag.y); - - massData->center = invVolume * center; + // Total mass massData->mass = density * volume; + + // Center of mass + B3_ASSERT(volume > B3_EPSILON); + float32 inv_volume = 1.0f / volume; + center *= inv_volume; + massData->center = center + p1; + + // Inertia tensor relative to the local origin (p1) + diag = inv_volume * diag; + off_diag = inv_volume * off_diag; + + I.x.Set(diag.y + diag.z, -off_diag.z, -off_diag.y); + I.y.Set(-off_diag.z, diag.x + diag.z, -off_diag.x); + I.z.Set(-off_diag.y, -off_diag.x, diag.x + diag.y); + massData->I = massData->mass * I; + massData->I = massData->I + massData->mass * b3Steiner(p1); } void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const diff --git a/src/bounce/dynamics/shapes/sphere_shape.cpp b/src/bounce/dynamics/shapes/sphere_shape.cpp index 7b29390..f36aa50 100644 --- a/src/bounce/dynamics/shapes/sphere_shape.cpp +++ b/src/bounce/dynamics/shapes/sphere_shape.cpp @@ -38,14 +38,16 @@ void b3SphereShape::Swap(const b3SphereShape& other) void b3SphereShape::ComputeMass(b3MassData* massData, float32 density) const { - // Compute inertia about the origin float32 volume = (4.0f / 3.0f) * B3_PI * m_radius * m_radius * m_radius; float32 mass = density * volume; - b3Mat33 Io = b3Diagonal(mass * (2.0f / 5.0f) * m_radius * m_radius); - // Move inertia to the sphere center + + // Inertia about the local shape center of mass + // Then shift to the local body origin + float32 I = mass * (0.4f * m_radius * m_radius + b3Dot(m_center, m_center)); + massData->center = m_center; massData->mass = mass; - massData->I = b3MoveToCOM(Io, mass, m_center); + massData->I = b3Diagonal(I); } void b3SphereShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const diff --git a/src/testbed/framework/test_entries.cpp b/src/testbed/framework/test_entries.cpp index 63d418a..ec470e0 100644 --- a/src/testbed/framework/test_entries.cpp +++ b/src/testbed/framework/test_entries.cpp @@ -27,12 +27,13 @@ #include #include #include +#include +#include #include #include #include #include #include -#include #include #include #include @@ -61,21 +62,22 @@ TestEntry g_tests[] = { "Hull Collision", &HullAndHull::Create }, { "Linear Motion", &LinearMotion::Create }, { "Angular Motion", &AngularMotion::Create }, + { "Multiple Shapes", &MultipleShapes::Create }, + { "Quadric Shapes", &QuadricShapes::Create }, { "Springs", &Spring::Create }, { "Newton's Cradle", &NewtonCradle::Create }, { "Hinge Motor", &HingeMotor::Create }, { "Hinge Chain", &HingeChain::Create }, { "Ragdoll", &Ragdoll::Create }, - { "Quadrics", &Quadric::Create }, + { "Thin Boxes", &Thin::Create }, { "Mesh Contact Test", &MeshContactTest::Create }, { "Sphere Stack", &SphereStack::Create }, { "Capsule Stack", &CapsuleStack::Create }, { "Box Stack", &BoxStack::Create }, { "Shape Stack", &ShapeStack::Create }, { "Jenga", &Jenga::Create }, - { "Thin Plates", &Thin::Create }, - { "Pyramid", &Pyramid::Create }, - { "Pyramid Rows", &Pyramids::Create }, + { "Box Pyramid", &Pyramid::Create }, + { "Box Pyramid Rows", &Pyramids::Create }, { "Ray Cast", &RayCast::Create }, { "Sensor Test", &SensorTest::Create }, { "Character Test", &Character::Create },