From 0474f8167a45241640207d052cd1846fe3b97706 Mon Sep 17 00:00:00 2001 From: Alex Szpakowski Date: Mon, 30 Nov 2015 23:55:50 -0400 Subject: [PATCH] Updated Box2D to 2.3.2-git. --- .../Box2D/Collision/Shapes/b2ChainShape.cpp | 9 ++-- .../Box2D/Collision/Shapes/b2ChainShape.h | 3 ++ .../Box2D/Collision/Shapes/b2PolygonShape.cpp | 10 ++++- src/libraries/Box2D/Common/b2Draw.h | 6 +-- src/libraries/Box2D/Common/b2Math.h | 1 - src/libraries/Box2D/Common/b2Settings.cpp | 2 +- .../Dynamics/Contacts/b2ContactSolver.cpp | 41 +++++++++++-------- .../Dynamics/Joints/b2PrismaticJoint.cpp | 3 ++ .../Box2D/Dynamics/Joints/b2WeldJoint.cpp | 18 +++++++- .../Box2D/Dynamics/Joints/b2WheelJoint.h | 5 +-- src/libraries/Box2D/Dynamics/b2World.cpp | 38 ++++++++--------- src/libraries/Box2D/Dynamics/b2World.h | 2 +- 12 files changed, 87 insertions(+), 51 deletions(-) diff --git a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp b/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp index f1ed2cca3..0c7282cc1 100755 --- a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp +++ b/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp @@ -22,6 +22,11 @@ #include b2ChainShape::~b2ChainShape() +{ + Clear(); +} + +void b2ChainShape::Clear() { b2Free(m_vertices); m_vertices = NULL; @@ -56,10 +61,8 @@ void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) b2Assert(count >= 2); for (int32 i = 1; i < count; ++i) { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); + b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop); } m_count = count; diff --git a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h b/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h index e836dfef9..44493c43d 100755 --- a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h +++ b/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h @@ -37,6 +37,9 @@ public: /// The destructor frees the vertices using b2Free. ~b2ChainShape(); + /// Clear all data. + void Clear(); + /// Create a loop. This automatically adjusts connectivity. /// @param vertices an array of vertices, these are copied /// @param count the vertex count diff --git a/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp b/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp index 2f32e9fe2..26ba7b3a7 100755 --- a/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp @@ -138,7 +138,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) bool unique = true; for (int32 j = 0; j < tempCount; ++j) { - if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) + if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop))) { unique = false; break; @@ -217,6 +217,14 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) } } + if (m < 3) + { + // Polygon is degenerate. + b2Assert(false); + SetAsBox(1.0f, 1.0f); + return; + } + m_count = m; // Copy vertices. diff --git a/src/libraries/Box2D/Common/b2Draw.h b/src/libraries/Box2D/Common/b2Draw.h index 20359c913..be65fb0ff 100755 --- a/src/libraries/Box2D/Common/b2Draw.h +++ b/src/libraries/Box2D/Common/b2Draw.h @@ -25,9 +25,9 @@ struct b2Color { b2Color() {} - b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} - void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } - float32 r, g, b; + b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {} + void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; } + float32 r, g, b, a; }; /// Implement and register this class with a b2World to provide debug drawing of physics diff --git a/src/libraries/Box2D/Common/b2Math.h b/src/libraries/Box2D/Common/b2Math.h index 713100dfa..755c519b0 100755 --- a/src/libraries/Box2D/Common/b2Math.h +++ b/src/libraries/Box2D/Common/b2Math.h @@ -21,7 +21,6 @@ #include #include -#include /// This function is used to ensure that a floating point number is not a NaN or infinity. inline bool b2IsValid(float32 x) diff --git a/src/libraries/Box2D/Common/b2Settings.cpp b/src/libraries/Box2D/Common/b2Settings.cpp index e2ceefc1d..ed6d83e0e 100755 --- a/src/libraries/Box2D/Common/b2Settings.cpp +++ b/src/libraries/Box2D/Common/b2Settings.cpp @@ -23,7 +23,7 @@ #include "common/Exception.h" -b2Version b2_version = {2, 3, 0}; +b2Version b2_version = {2, 3, 2}; // Memory allocators. Modify these to use your own allocator. void* b2Alloc(int32 size) diff --git a/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp index 8224fdc2d..e31b6c236 100755 --- a/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp @@ -26,6 +26,8 @@ #define B2_DEBUG_SOLVER 0 +bool g_blockSolve = true; + struct b2ContactPositionConstraint { b2Vec2 localPoints[b2_maxManifoldPoints]; @@ -213,7 +215,7 @@ void b2ContactSolver::InitializeVelocityConstraints() } // If we have two points, then prepare the block solver. - if (vc->pointCount == 2) + if (vc->pointCount == 2 && g_blockSolve) { b2VelocityConstraintPoint* vcp1 = vc->points + 0; b2VelocityConstraintPoint* vcp2 = vc->points + 1; @@ -341,29 +343,32 @@ void b2ContactSolver::SolveVelocityConstraints() } // Solve normal constraints - if (vc->pointCount == 1) + if (pointCount == 1 || g_blockSolve == false) { - b2VelocityConstraintPoint* vcp = vc->points + 0; + for (int32 i = 0; i < pointCount; ++i) + { + b2VelocityConstraintPoint* vcp = vc->points + i; - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); + // Relative velocity at contact + b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - // Compute normal impulse - float32 vn = b2Dot(dv, normal); - float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); + // Compute normal impulse + float32 vn = b2Dot(dv, normal); + float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); - // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp->normalImpulse; - vcp->normalImpulse = newImpulse; + // b2Clamp the accumulated impulse + float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp->normalImpulse; + vcp->normalImpulse = newImpulse; - // Apply contact impulse - b2Vec2 P = lambda * normal; - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); + // Apply contact impulse + b2Vec2 P = lambda * normal; + vA -= mA * P; + wA -= iA * b2Cross(vcp->rA, P); - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); + vB += mB * P; + wB += iB * b2Cross(vcp->rB, P); + } } else { diff --git a/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp index 26f539681..217e14502 100755 --- a/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -174,6 +174,9 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) m_s1 = b2Cross(d + rA, m_perp); m_s2 = b2Cross(rB, m_perp); + float32 s1test; + s1test = b2Cross(rA, m_perp); + float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; float32 k12 = iA * m_s1 + iB * m_s2; float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; diff --git a/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp index 253f7216e..ffdd386b1 100755 --- a/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -129,6 +129,12 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) invM += m_gamma; m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; } + else if (K.ez.z == 0.0f) + { + K.GetInverse22(&m_mass); + m_gamma = 0.0f; + m_bias = 0.0f; + } else { K.GetSymInverse33(&m_mass); @@ -271,7 +277,17 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec3 C(C1.x, C1.y, C2); - b2Vec3 impulse = -K.Solve33(C); + b2Vec3 impulse; + if (K.ez.z > 0.0f) + { + impulse = -K.Solve33(C); + } + else + { + b2Vec2 impulse2 = -K.Solve22(C1); + impulse.Set(impulse2.x, impulse2.y, 0.0f); + } + b2Vec2 P(impulse.x, impulse.y); cA -= mA * P; diff --git a/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h index ca59d7a64..a33c77f4b 100755 --- a/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h +++ b/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -72,9 +72,8 @@ struct b2WheelJointDef : public b2JointDef }; /// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in bodyA and rotation in the plane. You can use a -/// joint limit to restrict the range of motion and a joint motor to drive -/// the rotation or to model rotational friction. +/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to +/// line constraint with a rotational motor and a linear spring/damper. /// This joint is designed for vehicle suspensions. class b2WheelJoint : public b2Joint { diff --git a/src/libraries/Box2D/Dynamics/b2World.cpp b/src/libraries/Box2D/Dynamics/b2World.cpp index a3a6a1e41..c6cf2957b 100755 --- a/src/libraries/Box2D/Dynamics/b2World.cpp +++ b/src/libraries/Box2D/Dynamics/b2World.cpp @@ -37,7 +37,7 @@ b2World::b2World(const b2Vec2& gravity) { m_destructionListener = NULL; - m_debugDraw = NULL; + g_debugDraw = NULL; m_bodyList = NULL; m_jointList = NULL; @@ -101,7 +101,7 @@ void b2World::SetContactListener(b2ContactListener* listener) void b2World::SetDebugDraw(b2Draw* debugDraw) { - m_debugDraw = debugDraw; + g_debugDraw = debugDraw; } b2Body* b2World::CreateBody(const b2BodyDef* def) @@ -1040,7 +1040,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color float32 radius = circle->m_radius; b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - m_debugDraw->DrawSolidCircle(center, radius, axis, color); + g_debugDraw->DrawSolidCircle(center, radius, axis, color); } break; @@ -1049,7 +1049,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); - m_debugDraw->DrawSegment(v1, v2, color); + g_debugDraw->DrawSegment(v1, v2, color); } break; @@ -1063,8 +1063,8 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color for (int32 i = 1; i < count; ++i) { b2Vec2 v2 = b2Mul(xf, vertices[i]); - m_debugDraw->DrawSegment(v1, v2, color); - m_debugDraw->DrawCircle(v1, 0.05f, color); + g_debugDraw->DrawSegment(v1, v2, color); + g_debugDraw->DrawCircle(v1, 0.05f, color); v1 = v2; } } @@ -1082,7 +1082,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color vertices[i] = b2Mul(xf, poly->m_vertices[i]); } - m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); + g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); } break; @@ -1107,7 +1107,7 @@ void b2World::DrawJoint(b2Joint* joint) switch (joint->GetType()) { case e_distanceJoint: - m_debugDraw->DrawSegment(p1, p2, color); + g_debugDraw->DrawSegment(p1, p2, color); break; case e_pulleyJoint: @@ -1115,9 +1115,9 @@ void b2World::DrawJoint(b2Joint* joint) b2PulleyJoint* pulley = (b2PulleyJoint*)joint; b2Vec2 s1 = pulley->GetGroundAnchorA(); b2Vec2 s2 = pulley->GetGroundAnchorB(); - m_debugDraw->DrawSegment(s1, p1, color); - m_debugDraw->DrawSegment(s2, p2, color); - m_debugDraw->DrawSegment(s1, s2, color); + g_debugDraw->DrawSegment(s1, p1, color); + g_debugDraw->DrawSegment(s2, p2, color); + g_debugDraw->DrawSegment(s1, s2, color); } break; @@ -1126,20 +1126,20 @@ void b2World::DrawJoint(b2Joint* joint) break; default: - m_debugDraw->DrawSegment(x1, p1, color); - m_debugDraw->DrawSegment(p1, p2, color); - m_debugDraw->DrawSegment(x2, p2, color); + g_debugDraw->DrawSegment(x1, p1, color); + g_debugDraw->DrawSegment(p1, p2, color); + g_debugDraw->DrawSegment(x2, p2, color); } } void b2World::DrawDebugData() { - if (m_debugDraw == NULL) + if (g_debugDraw == NULL) { return; } - uint32 flags = m_debugDraw->GetFlags(); + uint32 flags = g_debugDraw->GetFlags(); if (flags & b2Draw::e_shapeBit) { @@ -1191,7 +1191,7 @@ void b2World::DrawDebugData() //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - //m_debugDraw->DrawSegment(cA, cB, color); + //g_debugDraw->DrawSegment(cA, cB, color); } } @@ -1219,7 +1219,7 @@ void b2World::DrawDebugData() vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - m_debugDraw->DrawPolygon(vs, 4, color); + g_debugDraw->DrawPolygon(vs, 4, color); } } } @@ -1231,7 +1231,7 @@ void b2World::DrawDebugData() { b2Transform xf = b->GetTransform(); xf.p = b->GetWorldCenter(); - m_debugDraw->DrawTransform(xf); + g_debugDraw->DrawTransform(xf); } } } diff --git a/src/libraries/Box2D/Dynamics/b2World.h b/src/libraries/Box2D/Dynamics/b2World.h index 4f94f6efe..3968cc784 100755 --- a/src/libraries/Box2D/Dynamics/b2World.h +++ b/src/libraries/Box2D/Dynamics/b2World.h @@ -247,7 +247,7 @@ private: bool m_allowSleep; b2DestructionListener* m_destructionListener; - b2Draw* m_debugDraw; + b2Draw* g_debugDraw; // This is used to compute the time step ratio to // support a variable time step.