From 3c7310e76d45f9f0a181997b07d3c293b9c09573 Mon Sep 17 00:00:00 2001 From: Bill Meltsner Date: Fri, 23 Sep 2011 16:09:18 -0400 Subject: [PATCH] Update to Box2D 2.2.1 (stop updating, Erin!) --HG-- branch : box2d-update --- src/modules/physics/box2d/Box2D/Box2D.h | 2 - .../Box2D/Collision/Shapes/b2ChainShape.h | 15 -- .../Box2D/Collision/Shapes/b2EdgeShape.h | 4 + .../box2d/Box2D/Collision/b2Distance.cpp | 10 +- .../physics/box2d/Box2D/Common/b2Math.cpp | 41 ++++ .../physics/box2d/Box2D/Common/b2Math.h | 14 ++ .../physics/box2d/Box2D/Common/b2Settings.cpp | 13 +- .../physics/box2d/Box2D/Common/b2Settings.h | 3 + .../physics/box2d/Box2D/Common/b2Timer.cpp | 4 +- .../physics/box2d/Box2D/Common/b2Timer.h | 2 +- .../Box2D/Dynamics/Joints/b2DistanceJoint.cpp | 26 ++- .../Box2D/Dynamics/Joints/b2DistanceJoint.h | 20 +- .../Box2D/Dynamics/Joints/b2FrictionJoint.cpp | 16 ++ .../Box2D/Dynamics/Joints/b2FrictionJoint.h | 9 + .../Box2D/Dynamics/Joints/b2GearJoint.cpp | 45 +++-- .../box2d/Box2D/Dynamics/Joints/b2GearJoint.h | 12 ++ .../box2d/Box2D/Dynamics/Joints/b2Joint.cpp | 1 + .../box2d/Box2D/Dynamics/Joints/b2Joint.h | 10 +- .../Box2D/Dynamics/Joints/b2MouseJoint.h | 3 + .../Dynamics/Joints/b2PrismaticJoint.cpp | 68 +++++-- .../Box2D/Dynamics/Joints/b2PrismaticJoint.h | 26 ++- .../Box2D/Dynamics/Joints/b2PulleyJoint.cpp | 22 +++ .../Box2D/Dynamics/Joints/b2PulleyJoint.h | 5 + .../Box2D/Dynamics/Joints/b2RevoluteJoint.cpp | 21 ++ .../Box2D/Dynamics/Joints/b2RevoluteJoint.h | 19 +- .../Box2D/Dynamics/Joints/b2RopeJoint.cpp | 15 ++ .../box2d/Box2D/Dynamics/Joints/b2RopeJoint.h | 12 +- .../Box2D/Dynamics/Joints/b2WeldJoint.cpp | 179 ++++++++++++++---- .../box2d/Box2D/Dynamics/Joints/b2WeldJoint.h | 42 +++- .../Box2D/Dynamics/Joints/b2WheelJoint.cpp | 21 +- .../Box2D/Dynamics/Joints/b2WheelJoint.h | 22 ++- .../physics/box2d/Box2D/Dynamics/b2Body.cpp | 30 +++ .../physics/box2d/Box2D/Dynamics/b2Body.h | 5 +- .../box2d/Box2D/Dynamics/b2ContactManager.cpp | 20 +- .../box2d/Box2D/Dynamics/b2Fixture.cpp | 75 ++++++++ .../physics/box2d/Box2D/Dynamics/b2Fixture.h | 13 +- .../physics/box2d/Box2D/Dynamics/b2TimeStep.h | 24 +-- .../physics/box2d/Box2D/Dynamics/b2World.cpp | 85 ++++++++- .../physics/box2d/Box2D/Dynamics/b2World.h | 14 +- 39 files changed, 815 insertions(+), 153 deletions(-) diff --git a/src/modules/physics/box2d/Box2D/Box2D.h b/src/modules/physics/box2d/Box2D/Box2D.h index 817cd1ccf..66d221742 100755 --- a/src/modules/physics/box2d/Box2D/Box2D.h +++ b/src/modules/physics/box2d/Box2D/Box2D.h @@ -64,6 +64,4 @@ For discussion please visit http://box2d.org/forum #include #include -#include - #endif diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h index 34593e2b8..6aa41ea41 100755 --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h @@ -79,21 +79,6 @@ public: /// @see b2Shape::ComputeMass void ComputeMass(b2MassData* massData, float32 density) const; - /// Get the number of vertices. - int32 GetVertexCount() const { return m_count; } - - /// Get the vertices (read-only). - const b2Vec2& GetVertex(int32 index) const - { - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; - } - - /// Get the vertices (read-only). - const b2Vec2* GetVertices() const { return m_vertices; } - -protected: - /// The vertices. Owned by this class. b2Vec2* m_vertices; diff --git a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h index 780eb2c5c..99f822bec 100755 --- a/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h +++ b/src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h @@ -63,6 +63,10 @@ inline b2EdgeShape::b2EdgeShape() { m_type = e_edge; m_radius = b2_polygonRadius; + m_vertex0.x = 0.0f; + m_vertex0.y = 0.0f; + m_vertex3.x = 0.0f; + m_vertex3.y = 0.0f; m_hasVertex0 = false; m_hasVertex3 = false; } diff --git a/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp b/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp index 2bfb7598a..1010c8c0e 100755 --- a/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp +++ b/src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp @@ -50,16 +50,16 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index) case b2Shape::e_chain: { const b2ChainShape* chain = (b2ChainShape*)shape; - b2Assert(0 <= index && index < chain->GetVertexCount()); + b2Assert(0 <= index && index < chain->m_count); - m_buffer[0] = chain->GetVertex(index); - if (index + 1 < chain->GetVertexCount()) + m_buffer[0] = chain->m_vertices[index]; + if (index + 1 < chain->m_count) { - m_buffer[1] = chain->GetVertex(index + 1); + m_buffer[1] = chain->m_vertices[index + 1]; } else { - m_buffer[1] = chain->GetVertex(0); + m_buffer[1] = chain->m_vertices[0]; } m_vertices = m_buffer; diff --git a/src/modules/physics/box2d/Box2D/Common/b2Math.cpp b/src/modules/physics/box2d/Box2D/Common/b2Math.cpp index b06111dc4..4974fe187 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Math.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2Math.cpp @@ -51,3 +51,44 @@ b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const x.y = det * (a11 * b.y - a21 * b.x); return x; } + +/// +void b2Mat33::GetInverse22(b2Mat33* M) const +{ + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + float32 det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; + M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; + M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; +} + +/// Returns the zero matrix if singular. +void b2Mat33::GetSymInverse33(b2Mat33* M) const +{ + float32 det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; + float32 a22 = ey.y, a23 = ez.y; + float32 a33 = ez.z; + + M->ex.x = det * (a22 * a33 - a23 * a23); + M->ex.y = det * (a13 * a23 - a12 * a33); + M->ex.z = det * (a12 * a23 - a13 * a22); + + M->ey.x = M->ex.y; + M->ey.y = det * (a11 * a33 - a13 * a13); + M->ey.z = det * (a13 * a12 - a11 * a23); + + M->ez.x = M->ex.z; + M->ez.y = M->ey.z; + M->ez.z = det * (a11 * a22 - a12 * a12); +} diff --git a/src/modules/physics/box2d/Box2D/Common/b2Math.h b/src/modules/physics/box2d/Box2D/Common/b2Math.h index fdcd939de..9808258bf 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Math.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Math.h @@ -295,6 +295,14 @@ struct b2Mat33 /// 2-by-2 matrix equation. b2Vec2 Solve22(const b2Vec2& b) const; + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + void GetInverse22(b2Mat33* M) const; + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + void GetSymInverse33(b2Mat33* M) const; + b2Vec3 ex, ey, ez; }; @@ -533,6 +541,12 @@ inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) return v.x * A.ex + v.y * A.ey + v.z * A.ez; } +/// Multiply a matrix times a vector. +inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) +{ + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); +} + /// Multiply two rotations: q * r inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) { diff --git a/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp b/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp index 97f5283f4..05a322305 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2Settings.cpp @@ -18,8 +18,10 @@ #include #include +#include +#include -b2Version b2_version = {2, 2, 0}; +b2Version b2_version = {2, 2, 1}; // Memory allocators. Modify these to use your own allocator. void* b2Alloc(int32 size) @@ -31,3 +33,12 @@ void b2Free(void* mem) { free(mem); } + +// You can modify this to use your logging facility. +void b2Log(const char* string, ...) +{ + va_list args; + va_start(args, string); + vprintf(string, args); + va_end(args); +} \ No newline at end of file diff --git a/src/modules/physics/box2d/Box2D/Common/b2Settings.h b/src/modules/physics/box2d/Box2D/Common/b2Settings.h index eabc526d0..7e0a165b5 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Settings.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Settings.h @@ -132,6 +132,9 @@ void* b2Alloc(int32 size); /// If you implement b2Alloc, you should also implement this function. void b2Free(void* mem); +/// Logging function. +void b2Log(const char* string, ...); + /// Version numbering scheme. /// See http://en.wikipedia.org/wiki/Software_versioning struct b2Version diff --git a/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp b/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp index b1863c296..ea3479f11 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp +++ b/src/modules/physics/box2d/Box2D/Common/b2Timer.cpp @@ -18,11 +18,11 @@ #include -#if defined(WIN32) +#if defined(_WIN32) float64 b2Timer::s_invFrequency = 0.0f; -#include +#include b2Timer::b2Timer() { diff --git a/src/modules/physics/box2d/Box2D/Common/b2Timer.h b/src/modules/physics/box2d/Box2D/Common/b2Timer.h index 678f6ffe4..19bde287f 100755 --- a/src/modules/physics/box2d/Box2D/Common/b2Timer.h +++ b/src/modules/physics/box2d/Box2D/Common/b2Timer.h @@ -35,7 +35,7 @@ public: private: -#if defined(WIN32) +#if defined(_WIN32) float64 m_start; static float64 s_invFrequency; #elif defined(__linux__) || defined (__APPLE__) diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp index 7b0e2a507..1ced7d9de 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp @@ -123,8 +123,13 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; m_bias = C * h * k * m_gamma; - m_mass = invMass + m_gamma; - m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f; + invMass += m_gamma; + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + } + else + { + m_gamma = 0.0f; + m_bias = 0.0f; } if (data.step.warmStarting) @@ -236,3 +241,20 @@ float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const B2_NOT_USED(inv_dt); return 0.0f; } + +void b2DistanceJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2DistanceJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.length = %.15lef;\n", m_length); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h index a8d35a5e6..9d0528b95 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h @@ -44,16 +44,17 @@ struct b2DistanceJointDef : public b2JointDef void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchorA, const b2Vec2& anchorB); - /// The local anchor point relative to body1's origin. + /// The local anchor point relative to bodyA's origin. b2Vec2 localAnchorA; - /// The local anchor point relative to body2's origin. + /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; /// The natural length between the anchor points. float32 length; - /// The mass-spring-damper frequency in Hertz. + /// The mass-spring-damper frequency in Hertz. A value of 0 + /// disables softness. float32 frequencyHz; /// The damping ratio. 0 = no damping, 1 = critical damping. @@ -78,19 +79,28 @@ public: /// Unit is N*m. This is always zero for a distance joint. float32 GetReactionTorque(float32 inv_dt) const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// Set/get the natural length. /// Manipulating the length can lead to non-physical behavior when the frequency is zero. void SetLength(float32 length); float32 GetLength() const; - // Set/get frequency in Hz. + /// Set/get frequency in Hz. void SetFrequency(float32 hz); float32 GetFrequency() const; - // Set/get damping ratio. + /// Set/get damping ratio. void SetDampingRatio(float32 ratio); float32 GetDampingRatio() const; + /// Dump joint to dmLog + void Dump(); + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp index 8c363e6a1..075e72bb4 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp @@ -233,3 +233,19 @@ float32 b2FrictionJoint::GetMaxTorque() const { return m_maxTorque; } + +void b2FrictionJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2FrictionJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); + b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h index 3e72be2fe..785d9ac15 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h @@ -61,6 +61,12 @@ public: b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// Set the maximum friction force in N. void SetMaxForce(float32 force); @@ -73,6 +79,9 @@ public: /// Get the maximum friction torque in N*m. float32 GetMaxTorque() const; + /// Dump joint to dmLog + void Dump(); + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp index cff95f2bc..201f01233 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp @@ -44,16 +44,21 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) : b2Joint(def) { - m_typeA = def->joint1->GetType(); - m_typeB = def->joint2->GetType(); + m_joint1 = def->joint1; + m_joint2 = def->joint2; + + m_typeA = m_joint1->GetType(); + m_typeB = m_joint2->GetType(); b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); float32 coordinateA, coordinateB; - m_bodyC = def->joint1->GetBodyA(); - m_bodyA = def->joint1->GetBodyB(); + // TODO_ERIN there might be some problem with the joint edges in b2Joint. + + m_bodyC = m_joint1->GetBodyA(); + m_bodyA = m_joint1->GetBodyB(); // Get geometry of joint1 b2Transform xfA = m_bodyA->m_xf; @@ -76,7 +81,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; m_localAnchorC = prismatic->m_localAnchorA; m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_refAngle; + m_referenceAngleA = prismatic->m_referenceAngle; m_localAxisC = prismatic->m_localXAxisA; b2Vec2 pC = m_localAnchorC; @@ -84,8 +89,8 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) coordinateA = b2Dot(pA - pC, m_localAxisC); } - m_bodyD = def->joint2->GetBodyA(); - m_bodyB = def->joint2->GetBodyB(); + m_bodyD = m_joint2->GetBodyA(); + m_bodyB = m_joint2->GetBodyB(); // Get geometry of joint2 b2Transform xfB = m_bodyB->m_xf; @@ -108,7 +113,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; m_localAnchorD = prismatic->m_localAnchorA; m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_refAngle; + m_referenceAngleB = prismatic->m_referenceAngle; m_localAxisD = prismatic->m_localXAxisA; b2Vec2 pD = m_localAnchorD; @@ -315,9 +320,9 @@ bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) if (m_typeB == e_revoluteJoint) { JvBD.SetZero(); - JwB = 1.0f; - JwD = 1.0f; - mass += m_iB + m_iD; + JwB = m_ratio; + JwD = m_ratio; + mass += m_ratio * m_ratio * (m_iB + m_iD); coordinateB = aB - aD - m_referenceAngleB; } @@ -398,3 +403,21 @@ float32 b2GearJoint::GetRatio() const { return m_ratio; } + +void b2GearJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + int32 index1 = m_joint1->m_index; + int32 index2 = m_joint2->m_index; + + b2Log(" b2GearJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.joint1 = joints[%d];\n", index1); + b2Log(" jd.joint2 = joints[%d];\n", index2); + b2Log(" jd.ratio = %.15lef;\n", m_ratio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h index 5651d5418..48cdfd886 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h @@ -62,10 +62,19 @@ public: b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; + /// Get the first joint. + b2Joint* GetJoint1() { return m_joint1; } + + /// Get the second joint. + b2Joint* GetJoint2() { return m_joint2; } + /// Set/Get the gear ratio. void SetRatio(float32 ratio); float32 GetRatio() const; + /// Dump joint to dmLog + void Dump(); + protected: friend class b2Joint; @@ -75,6 +84,9 @@ protected: void SolveVelocityConstraints(const b2SolverData& data); bool SolvePositionConstraints(const b2SolverData& data); + b2Joint* m_joint1; + b2Joint* m_joint2; + b2JointType m_typeA; b2JointType m_typeB; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp index eee195a9d..85322f77c 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp @@ -177,6 +177,7 @@ b2Joint::b2Joint(const b2JointDef* def) m_next = NULL; m_bodyA = def->bodyA; m_bodyB = def->bodyB; + m_index = 0; m_collideConnected = def->collideConnected; m_islandFlag = false; m_userData = def->userData; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h index ed6ad4103..90154ccb8 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h @@ -118,10 +118,10 @@ public: /// Get the anchor point on bodyB in world coordinates. virtual b2Vec2 GetAnchorB() const = 0; - /// Get the reaction force on body2 at the joint anchor in Newtons. + /// Get the reaction force on bodyB at the joint anchor in Newtons. virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0; - /// Get the reaction torque on body2 in N*m. + /// Get the reaction torque on bodyB in N*m. virtual float32 GetReactionTorque(float32 inv_dt) const = 0; /// Get the next joint the world joint list. @@ -142,10 +142,14 @@ public: /// the flag is only checked when fixture AABBs begin to overlap. bool GetCollideConnected() const; + /// Dump this joint to the log file. + virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } + protected: friend class b2World; friend class b2Body; friend class b2Island; + friend class b2GearJoint; static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator); static void Destroy(b2Joint* joint, b2BlockAllocator* allocator); @@ -167,6 +171,8 @@ protected: b2Body* m_bodyA; b2Body* m_bodyB; + int32 m_index; + bool m_islandFlag; bool m_collideConnected; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h index 3c90d0161..bb2e28418 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -89,6 +89,9 @@ public: void SetDampingRatio(float32 ratio); float32 GetDampingRatio() const; + /// The mouse joint does not support dumping. + void Dump() { b2Log("Mouse joint dumping is not supported.\n"); } + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp index 99f0c7d6f..88a388072 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -103,8 +103,9 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; m_localXAxisA = def->localAxisA; + m_localXAxisA.Normalize(); m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - m_refAngle = def->referenceAngle; + m_referenceAngle = def->referenceAngle; m_impulse.SetZero(); m_motorMass = 0.0; @@ -348,6 +349,17 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) vB += mB * P; wB += iB * LB; + + b2Vec2 Cdot10 = Cdot1; + + Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; + Cdot1.y = wB - wA; + + if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f) + { + b2Vec2 test = b2Mul22(m_K, df); + Cdot1.x += 0.0f; + } } data.velocities[m_indexA].v = vA; @@ -384,7 +396,7 @@ bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec3 impulse; b2Vec2 C1; C1.x = b2Dot(perp, d); - C1.y = aB - aA - m_refAngle; + C1.y = aB - aA - m_referenceAngle; float32 linearError = b2Abs(C1.x); float32 angularError = b2Abs(C1.y); @@ -419,43 +431,45 @@ bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) if (active) { - 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; + float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float32 k12 = iA * s1 + iB * s2; + float32 k13 = iA * s1 * a1 + iB * s2 * a2; float32 k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } - float32 k23 = iA * m_a1 + iB * m_a2; - float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + float32 k23 = iA * a1 + iB * a2; + float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; - m_K.ex.Set(k11, k12, k13); - m_K.ey.Set(k12, k22, k23); - m_K.ez.Set(k13, k23, k33); + b2Mat33 K; + K.ex.Set(k11, k12, k13); + K.ey.Set(k12, k22, k23); + K.ez.Set(k13, k23, k33); b2Vec3 C; C.x = C1.x; C.y = C1.y; C.z = C2; - impulse = m_K.Solve33(-C); + impulse = K.Solve33(-C); } else { - float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; - float32 k12 = iA * m_s1 + iB * m_s2; + float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float32 k12 = iA * s1 + iB * s2; float32 k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } - m_K.ex.Set(k11, k12, 0.0f); - m_K.ey.Set(k12, k22, 0.0f); + b2Mat22 K; + K.ex.Set(k11, k12); + K.ey.Set(k12, k22); - b2Vec2 impulse1 = m_K.Solve22(-C1); + b2Vec2 impulse1 = K.Solve(-C1); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; @@ -599,3 +613,25 @@ float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const { return inv_dt * m_motorImpulse; } + +void b2PrismaticJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2PrismaticJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation); + b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h index f271e7579..8d0f34229 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h @@ -45,16 +45,16 @@ struct b2PrismaticJointDef : public b2JointDef } /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. + /// anchor and unit world axis. void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - /// The local anchor point relative to body1's origin. + /// The local anchor point relative to bodyA's origin. b2Vec2 localAnchorA; - /// The local anchor point relative to body2's origin. + /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; - /// The local translation axis in body1. + /// The local translation unit axis in bodyA. b2Vec2 localAxisA; /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. @@ -92,6 +92,18 @@ public: b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + + /// The local joint axis relative to bodyA. + const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } + + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } + /// Get the current joint translation, usually in meters. float32 GetJointTranslation() const; @@ -127,10 +139,14 @@ public: /// Set the maximum motor force, usually in N. void SetMaxMotorForce(float32 force); + float32 GetMaxMotorForce() const { return m_maxMotorForce; } /// Get the current motor force given the inverse time step, usually in N. float32 GetMotorForce(float32 inv_dt) const; + /// Dump to b2Log + void Dump(); + protected: friend class b2Joint; friend class b2GearJoint; @@ -145,7 +161,7 @@ protected: b2Vec2 m_localAnchorB; b2Vec2 m_localXAxisA; b2Vec2 m_localYAxisA; - float32 m_refAngle; + float32 m_referenceAngle; b2Vec3 m_impulse; float32 m_motorImpulse; float32 m_lowerTranslation; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp index 1a09d907e..e0bb7da51 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp @@ -59,6 +59,9 @@ b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; + m_lengthA = def->lengthA; + m_lengthB = def->lengthB; + b2Assert(def->ratio != 0.0f); m_ratio = def->ratio; @@ -308,3 +311,22 @@ float32 b2PulleyJoint::GetRatio() const { return m_ratio; } + +void b2PulleyJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2PulleyJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y); + b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.lengthA = %.15lef;\n", m_lengthA); + b2Log(" jd.lengthB = %.15lef;\n", m_lengthB); + b2Log(" jd.ratio = %.15lef;\n", m_ratio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h index a5269c618..639789f52 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -100,6 +100,9 @@ public: /// Get the pulley ratio. float32 GetRatio() const; + /// Dump joint to dmLog + void Dump(); + protected: friend class b2Joint; @@ -111,6 +114,8 @@ protected: b2Vec2 m_groundAnchorA; b2Vec2 m_groundAnchorB; + float32 m_lengthA; + float32 m_lengthB; // Solver shared b2Vec2 m_localAnchorA; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp index 7718f03fc..ffb17252c 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp @@ -481,3 +481,24 @@ void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) m_upperAngle = upper; } } + +void b2RevoluteJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2RevoluteJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle); + b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h index 03eae0b3a..e62cdbccd 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h @@ -52,13 +52,13 @@ struct b2RevoluteJointDef : public b2JointDef /// anchor point. void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - /// The local anchor point relative to body1's origin. + /// The local anchor point relative to bodyA's origin. b2Vec2 localAnchorA; - /// The local anchor point relative to body2's origin. + /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; - /// The body2 angle minus body1 angle in the reference state (radians). + /// The bodyB angle minus bodyA angle in the reference state (radians). float32 referenceAngle; /// A flag to enable joint limits. @@ -93,6 +93,15 @@ public: b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } + /// Get the current joint angle in radians. float32 GetJointAngle() const; @@ -128,6 +137,7 @@ public: /// Set the maximum motor torque, usually in N-m. void SetMaxMotorTorque(float32 torque); + float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } /// Get the reaction force given the inverse time step. /// Unit is N. @@ -141,6 +151,9 @@ public: /// Unit is N*m. float32 GetMotorTorque(float32 inv_dt) const; + /// Dump to b2Log. + void Dump(); + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp index 4b1e666c1..107ce1fa5 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp @@ -224,3 +224,18 @@ b2LimitState b2RopeJoint::GetLimitState() const { return m_state; } + +void b2RopeJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2RopeJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.maxLength = %.15lef;\n", m_maxLength); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h index 660b4ed50..30252460c 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h @@ -64,11 +64,21 @@ public: b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; - /// Get the maximum length of the rope. + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + + /// Set/Get the maximum length of the rope. + void SetMaxLength(float32 length) { m_maxLength = length; } float32 GetMaxLength() const; b2LimitState GetLimitState() const; + /// Dump joint to dmLog + void Dump(); + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp index 11eb5ec65..9a86686e2 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -49,6 +49,8 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; m_referenceAngle = def->referenceAngle; + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; m_impulse.SetZero(); } @@ -91,15 +93,50 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; - m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; - m_mass.ex.y = m_mass.ey.x; - m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; - m_mass.ex.z = m_mass.ez.x; - m_mass.ey.z = m_mass.ez.y; - m_mass.ez.z = iA + iB; + b2Mat33 K; + K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + K.ez.x = -m_rA.y * iA - m_rB.y * iB; + K.ex.y = K.ey.x; + K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + K.ez.y = m_rA.x * iA + m_rB.x * iB; + K.ex.z = K.ez.x; + K.ey.z = K.ez.y; + K.ez.z = iA + iB; + + if (m_frequencyHz > 0.0f) + { + K.GetInverse22(&m_mass); + + float32 invM = iA + iB; + float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; + + float32 C = aB - aA - m_referenceAngle; + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 d = 2.0f * m * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m * omega * omega; + + // magic formulas + float32 h = data.step.dt; + m_gamma = h * (d + h * k); + m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; + m_bias = C * h * k * m_gamma; + + invM += m_gamma; + m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; + } + else + { + K.GetSymInverse33(&m_mass); + m_gamma = 0.0f; + m_bias = 0.0f; + } if (data.step.warmStarting) { @@ -135,20 +172,47 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + if (m_frequencyHz > 0.0f) + { + float32 Cdot2 = wB - wA; - b2Vec3 impulse = -m_mass.Solve33(Cdot); - m_impulse += impulse; + float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); + m_impulse.z += impulse2; - b2Vec2 P(impulse.x, impulse.y); + wA -= iA * impulse2; + wB += iB * impulse2; - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); + b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); + m_impulse.x += impulse1.x; + m_impulse.y += impulse1.y; + + b2Vec2 P = impulse1; + + vA -= mA * P; + wA -= iA * b2Cross(m_rA, P); + + vB += mB * P; + wB += iB * b2Cross(m_rB, P); + } + else + { + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + float32 Cdot2 = wB - wA; + b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + + b2Vec3 impulse = -b2Mul(m_mass, Cdot); + m_impulse += impulse; + + b2Vec2 P(impulse.x, impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + impulse.z); + } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; @@ -171,33 +235,53 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 C1 = cB + rB - cA - rA; - float32 C2 = aB - aA - m_referenceAngle; + float32 positionError, angularError; - float32 positionError = C1.Length(); - float32 angularError = b2Abs(C2); + b2Mat33 K; + K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.ez.x = -rA.y * iA - rB.y * iB; + K.ex.y = K.ey.x; + K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + K.ez.y = rA.x * iA + rB.x * iB; + K.ex.z = K.ez.x; + K.ey.z = K.ez.y; + K.ez.z = iA + iB; - m_mass.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - m_mass.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - m_mass.ez.x = -rA.y * iA - rB.y * iB; - m_mass.ex.y = m_mass.ey.x; - m_mass.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - m_mass.ez.y = rA.x * iA + rB.x * iB; - m_mass.ex.z = m_mass.ez.x; - m_mass.ey.z = m_mass.ez.y; - m_mass.ez.z = iA + iB; + if (m_frequencyHz > 0.0f) + { + b2Vec2 C1 = cB + rB - cA - rA; - b2Vec3 C(C1.x, C1.y, C2); + positionError = C1.Length(); + angularError = 0.0f; - b2Vec3 impulse = -m_mass.Solve33(C); + b2Vec2 P = -K.Solve22(C1); - b2Vec2 P(impulse.x, impulse.y); + cA -= mA * P; + aA -= iA * b2Cross(rA, P); - cA -= mA * P; - aA -= iA * (b2Cross(rA, P) + impulse.z); + cB += mB * P; + aB += iB * b2Cross(rB, P); + } + else + { + b2Vec2 C1 = cB + rB - cA - rA; + float32 C2 = aB - aA - m_referenceAngle; - cB += mB * P; - aB += iB * (b2Cross(rB, P) + impulse.z); + positionError = C1.Length(); + angularError = b2Abs(C2); + + b2Vec3 C(C1.x, C1.y, C2); + + b2Vec3 impulse = -K.Solve33(C); + b2Vec2 P(impulse.x, impulse.y); + + cA -= mA * P; + aA -= iA * (b2Cross(rA, P) + impulse.z); + + cB += mB * P; + aB += iB * (b2Cross(rB, P) + impulse.z); + } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; @@ -227,3 +311,20 @@ float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_impulse.z; } + +void b2WeldJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2WeldJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h index 51c4b402b..2adfc26ab 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h @@ -32,20 +32,29 @@ struct b2WeldJointDef : public b2JointDef localAnchorA.Set(0.0f, 0.0f); localAnchorB.Set(0.0f, 0.0f); referenceAngle = 0.0f; + frequencyHz = 0.0f; + dampingRatio = 0.0f; } /// Initialize the bodies, anchors, and reference angle using a world /// anchor point. - void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor); + void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - /// The local anchor point relative to body1's origin. + /// The local anchor point relative to bodyA's origin. b2Vec2 localAnchorA; - /// The local anchor point relative to body2's origin. + /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; - /// The body2 angle minus body1 angle in the reference state (radians). + /// The bodyB angle minus bodyA angle in the reference state (radians). float32 referenceAngle; + + /// The mass-spring-damper frequency in Hertz. Rotation only. + /// Disable softness with a value of 0. + float32 frequencyHz; + + /// The damping ratio. 0 = no damping, 1 = critical damping. + float32 dampingRatio; }; /// A weld joint essentially glues two bodies together. A weld joint may @@ -59,6 +68,26 @@ public: b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } + + /// Set/get frequency in Hz. + void SetFrequency(float32 hz) { m_frequencyHz = hz; } + float32 GetFrequency() const { return m_frequencyHz; } + + /// Set/get damping ratio. + void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } + float32 GetDampingRatio() const { return m_dampingRatio; } + + /// Dump to b2Log + void Dump(); + protected: friend class b2Joint; @@ -69,10 +98,15 @@ protected: void SolveVelocityConstraints(const b2SolverData& data); bool SolvePositionConstraints(const b2SolverData& data); + float32 m_frequencyHz; + float32 m_dampingRatio; + float32 m_bias; + // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; float32 m_referenceAngle; + float32 m_gamma; b2Vec3 m_impulse; // Solver temp diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp index 826192347..998c627db 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp @@ -398,7 +398,22 @@ float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const return inv_dt * m_motorImpulse; } +void b2WheelJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; - - - + b2Log(" b2WheelJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h index 7398d218c..9a8b13639 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -46,13 +46,13 @@ struct b2WheelJointDef : public b2JointDef /// anchor and world axis. void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - /// The local anchor point relative to body1's origin. + /// The local anchor point relative to bodyA's origin. b2Vec2 localAnchorA; - /// The local anchor point relative to body2's origin. + /// The local anchor point relative to bodyB's origin. b2Vec2 localAnchorB; - /// The local translation axis in body1. + /// The local translation axis in bodyA. b2Vec2 localAxisA; /// Enable/disable the joint motor. @@ -72,19 +72,30 @@ struct b2WheelJointDef : public b2JointDef }; /// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in body1 and rotation in the plane. You can use a +/// 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. /// This joint is designed for vehicle suspensions. class b2WheelJoint : public b2Joint { public: + void GetDefinition(b2WheelJointDef* def) const; + b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; b2Vec2 GetReactionForce(float32 inv_dt) const; float32 GetReactionTorque(float32 inv_dt) const; + /// The local anchor point relative to bodyA's origin. + const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + + /// The local anchor point relative to bodyB's origin. + const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + + /// The local joint axis relative to bodyA. + const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } + /// Get the current joint translation, usually in meters. float32 GetJointTranslation() const; @@ -118,6 +129,9 @@ public: void SetSpringDampingRatio(float32 ratio); float32 GetSpringDampingRatio() const; + /// Dump to b2Log + void Dump(); + protected: friend class b2Joint; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp index 858977c81..b50bf45ad 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp @@ -481,4 +481,34 @@ void b2Body::SetActive(bool flag) } m_contactList = NULL; } +} + +void b2Body::Dump() +{ + int32 bodyIndex = m_islandIndex; + + b2Log("{\n"); + b2Log(" b2BodyDef bd;\n"); + b2Log(" bd.type = b2BodyType(%d);\n", m_type); + b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); + b2Log(" bd.angle = %.15lef;\n", m_sweep.a); + b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); + b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); + b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); + b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); + b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); + b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); + b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); + b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); + b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); + b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); + b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); + b2Log("\n"); + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) + { + b2Log(" {\n"); + f->Dump(bodyIndex); + b2Log(" }\n"); + } + b2Log("}\n"); } \ No newline at end of file diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h index 5ebc5ddd1..73f2cb415 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Body.h @@ -375,6 +375,9 @@ public: b2World* GetWorld(); const b2World* GetWorld() const; + /// Dump this body to a log file + void Dump(); + private: friend class b2World; @@ -474,7 +477,7 @@ inline const b2Vec2& b2Body::GetPosition() const inline float32 b2Body::GetAngle() const { - return m_xf.q.GetAngle(); + return m_sweep.a; } inline const b2Vec2& b2Body::GetWorldCenter() const diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp index ee12b325e..5ac12b350 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp @@ -115,16 +115,6 @@ void b2ContactManager::Collide() b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); - bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; - bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - c = c->GetNext(); - continue; - } - // Is this contact flagged for filtering? if (c->m_flags & b2Contact::e_filterFlag) { @@ -150,6 +140,16 @@ void b2ContactManager::Collide() c->m_flags &= ~b2Contact::e_filterFlag; } + bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; + bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; + + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) + { + c = c->GetNext(); + continue; + } + int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp index 478088a44..77537b276 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp @@ -226,3 +226,78 @@ void b2Fixture::SetSensor(bool sensor) } } +void b2Fixture::Dump(int32 bodyIndex) +{ + b2Log(" b2FixtureDef fd;\n"); + b2Log(" fd.friction = %.15lef;\n", m_friction); + b2Log(" fd.restitution = %.15lef;\n", m_restitution); + b2Log(" fd.density = %.15lef;\n", m_density); + b2Log(" fd.isSensor = bool(%d);\n", m_isSensor); + b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); + b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); + b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); + + switch (m_shape->m_type) + { + case b2Shape::e_circle: + { + b2CircleShape* s = (b2CircleShape*)m_shape; + b2Log(" b2CircleShape shape;\n"); + b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); + b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y); + } + break; + + case b2Shape::e_edge: + { + b2EdgeShape* s = (b2EdgeShape*)m_shape; + b2Log(" b2EdgeShape shape;\n"); + b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); + b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y); + b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y); + b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y); + b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y); + b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0); + b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3); + } + break; + + case b2Shape::e_polygon: + { + b2PolygonShape* s = (b2PolygonShape*)m_shape; + b2Log(" b2PolygonShape shape;\n"); + b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); + for (int32 i = 0; i < s->m_vertexCount; ++i) + { + b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Log(" shape.Set(vs, %d);\n", s->m_vertexCount); + } + break; + + case b2Shape::e_chain: + { + b2ChainShape* s = (b2ChainShape*)m_shape; + b2Log(" b2ChainShape shape;\n"); + b2Log(" b2Vec2 vs[%d];\n", s->m_count); + for (int32 i = 0; i < s->m_count; ++i) + { + b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Log(" shape.CreateChain(vs, %d);\n", s->m_count); + b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y); + b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y); + b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex); + b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex); + } + break; + + default: + return; + } + + b2Log("\n"); + b2Log(" fd.shape = &shape;\n"); + b2Log("\n"); + b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h index a7b5c3428..5ebd0c3fd 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h @@ -31,6 +31,13 @@ class b2Fixture; /// This holds contact filtering data. struct b2Filter { + b2Filter() + { + categoryBits = 0x0001; + maskBits = 0xFFFF; + groupIndex = 0; + } + /// The collision category bits. Normally you would just set one bit. uint16 categoryBits; @@ -56,9 +63,6 @@ struct b2FixtureDef friction = 0.2f; restitution = 0.0f; density = 0.0f; - filter.categoryBits = 0x0001; - filter.maskBits = 0xFFFF; - filter.groupIndex = 0; isSensor = false; } @@ -188,6 +192,9 @@ public: /// the body transform. const b2AABB& GetAABB(int32 childIndex) const; + /// Dump this fixture to the log file. + void Dump(int32 bodyIndex); + protected: friend class b2Body; diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h b/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h index 80916cb65..abe6fb6e0 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h @@ -21,18 +21,18 @@ #include -/// Profiling data. Times are in milliseconds. -struct b2Profile -{ - float32 step; - float32 collide; - float32 solve; - float32 solveInit; - float32 solveVelocity; - float32 solvePosition; - float32 broadphase; - float32 solveTOI; -}; +/// Profiling data. Times are in milliseconds. +struct b2Profile +{ + float32 step; + float32 collide; + float32 solve; + float32 solveInit; + float32 solveVelocity; + float32 solvePosition; + float32 broadphase; + float32 solveTOI; +}; /// This is an internal structure. struct b2TimeStep diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp b/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp index 7dcddd03c..baacded4f 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp @@ -34,7 +34,7 @@ #include #include -b2World::b2World(const b2Vec2& gravity, bool doSleep) +b2World::b2World(const b2Vec2& gravity) { m_destructionListener = NULL; m_debugDraw = NULL; @@ -51,7 +51,7 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep) m_stepComplete = true; - m_allowSleep = doSleep; + m_allowSleep = true; m_gravity = gravity; m_flags = e_clearForces; @@ -364,6 +364,24 @@ void b2World::DestroyJoint(b2Joint* j) } } +// +void b2World::SetAllowSleeping(bool flag) +{ + if (flag == m_allowSleep) + { + return; + } + + m_allowSleep = flag; + if (m_allowSleep == false) + { + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->SetAwake(true); + } + } +} + // Find islands, integrate and solve constraints, solve position constraints void b2World::Solve(const b2TimeStep& step) { @@ -1038,8 +1056,8 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color case b2Shape::e_chain: { b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); - int32 count = chain->GetVertexCount(); - const b2Vec2* vertices = chain->GetVertices(); + int32 count = chain->m_count; + const b2Vec2* vertices = chain->m_vertices; b2Vec2 v1 = b2Mul(xf, vertices[0]); for (int32 i = 1; i < count; ++i) @@ -1237,3 +1255,62 @@ float32 b2World::GetTreeQuality() const { return m_contactManager.m_broadPhase.GetTreeQuality(); } + +void b2World::Dump() +{ + if ((m_flags & e_locked) == e_locked) + { + return; + } + + b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); + b2Log("m_world->SetGravity(g);\n"); + + b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); + b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); + int32 i = 0; + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_islandIndex = i; + b->Dump(); + ++i; + } + + i = 0; + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + j->m_index = i; + ++i; + } + + // First pass on joints, skip gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type == e_gearJoint) + { + continue; + } + + b2Log("{\n"); + j->Dump(); + b2Log("}\n"); + } + + // Second pass on joints, only gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type != e_gearJoint) + { + continue; + } + + b2Log("{\n"); + j->Dump(); + b2Log("}\n"); + } + + b2Log("b2Free(joints);\n"); + b2Log("b2Free(bodies);\n"); + b2Log("joints = NULL;\n"); + b2Log("bodies = NULL;\n"); +} diff --git a/src/modules/physics/box2d/Box2D/Dynamics/b2World.h b/src/modules/physics/box2d/Box2D/Dynamics/b2World.h index 381e1f1ac..eba757276 100755 --- a/src/modules/physics/box2d/Box2D/Dynamics/b2World.h +++ b/src/modules/physics/box2d/Box2D/Dynamics/b2World.h @@ -43,8 +43,7 @@ class b2World public: /// Construct a world object. /// @param gravity the world gravity vector. - /// @param doSleep improve performance by not simulating inactive bodies. - b2World(const b2Vec2& gravity, bool doSleep); + b2World(const b2Vec2& gravity); /// Destruct the world. All physics entities are destroyed and all heap memory is released. ~b2World(); @@ -142,14 +141,21 @@ public: b2Contact* GetContactList(); const b2Contact* GetContactList() const; + /// Enable/disable sleep. + void SetAllowSleeping(bool flag); + bool GetAllowSleeping() const { return m_allowSleep; } + /// Enable/disable warm starting. For testing. void SetWarmStarting(bool flag) { m_warmStarting = flag; } + bool GetWarmStarting() const { return m_warmStarting; } /// Enable/disable continuous physics. For testing. void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } + bool GetContinuousPhysics() const { return m_continuousPhysics; } /// Enable/disable single stepped continuous physics. For testing. void SetSubStepping(bool flag) { m_subStepping = flag; } + bool GetSubStepping() const { return m_subStepping; } /// Get the number of broad-phase proxies. int32 GetProxyCount() const; @@ -194,6 +200,10 @@ public: /// Get the current profile. const b2Profile& GetProfile() const; + /// Dump the world into the log file. + /// @warning this should be called outside of a time step. + void Dump(); + private: // m_flags