AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix hull mass data calculation and make it more robust, bugfixes

Browse Source
This commit is contained in:
Irlan
2017-02-27 02:06:33 -03:00
parent 56ac6d1ad5
commit 091c99b5cf
15 changed files with 359 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/bounce/dynamics/body.cpp
View File

@ -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);

60
src/bounce/dynamics/contacts/collide/collide_capsules.cpp
View File

@ -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);
}
}

11
src/bounce/dynamics/island.cpp
View File

@ -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;

2
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@ -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

79
src/bounce/dynamics/shapes/hull_shape.cpp
View File

@ -18,6 +18,7 @@
#include <bounce/dynamics/shapes/hull_shape.h>
#include <bounce/collision/shapes/hull.h>
#include <bounce/dynamics/time_step.h>
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

10
src/bounce/dynamics/shapes/sphere_shape.cpp
View File

@ -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

12
src/testbed/framework/test_entries.cpp
View File

@ -27,12 +27,13 @@
#include <testbed/tests/hull_collision.h>
#include <testbed/tests/linear_motion.h>
#include <testbed/tests/angular_motion.h>
#include <testbed/tests/multiple_shapes.h>
#include <testbed/tests/quadric_shapes.h>
#include <testbed/tests/spring.h>
#include <testbed/tests/newton_cradle.h>
#include <testbed/tests/hinge_motor.h>
#include <testbed/tests/hinge_chain.h>
#include <testbed/tests/ragdoll.h>
#include <testbed/tests/quadrics.h>
#include <testbed/tests/mesh_contact_test.h>
#include <testbed/tests/sphere_stack.h>
#include <testbed/tests/capsule_stack.h>
@ -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 },