From fedb4a5ea43cd5792d34e6b4ed7931bd90983172 Mon Sep 17 00:00:00 2001 From: Ruslan Date: Sun, 29 Dec 2013 19:57:03 +1100 Subject: [PATCH] Updated to Box2D 2.3.0 --- Box2D/Box2D.h | 3 +- Box2D/Collision/Shapes/b2ChainShape.cpp | 23 +- Box2D/Collision/Shapes/b2ChainShape.h | 4 +- Box2D/Collision/Shapes/b2CircleShape.cpp | 1 - Box2D/Collision/Shapes/b2EdgeShape.cpp | 5 +- Box2D/Collision/Shapes/b2PolygonShape.cpp | 180 +++- Box2D/Collision/Shapes/b2PolygonShape.h | 22 +- Box2D/Collision/b2BroadPhase.cpp | 3 - Box2D/Collision/b2BroadPhase.h | 11 +- Box2D/Collision/b2CollideCircle.cpp | 2 +- Box2D/Collision/b2CollideEdge.cpp | 12 +- Box2D/Collision/b2CollidePolygon.cpp | 142 +--- Box2D/Collision/b2Collision.cpp | 5 +- Box2D/Collision/b2Collision.h | 7 +- Box2D/Collision/b2Distance.cpp | 16 +- Box2D/Collision/b2DynamicTree.cpp | 15 +- Box2D/Collision/b2DynamicTree.h | 5 + Box2D/Collision/b2TimeOfImpact.cpp | 879 ++++++++++---------- Box2D/Common/b2BlockAllocator.cpp | 8 +- Box2D/Common/b2Draw.h | 5 + Box2D/Common/b2GrowableStack.h | 4 +- Box2D/Common/b2Math.h | 27 +- Box2D/Common/b2Settings.cpp | 10 +- Box2D/Common/b2Settings.h | 5 +- Box2D/Common/b2Timer.cpp | 5 +- Box2D/Common/b2Timer.h | 7 +- .../Dynamics/Contacts/b2ChainAndCircleContact.cpp | 1 - .../Dynamics/Contacts/b2ChainAndPolygonContact.cpp | 1 - Box2D/Dynamics/Contacts/b2CircleContact.cpp | 1 - Box2D/Dynamics/Contacts/b2Contact.cpp | 17 +- Box2D/Dynamics/Contacts/b2Contact.h | 20 +- Box2D/Dynamics/Contacts/b2ContactSolver.cpp | 7 +- Box2D/Dynamics/Contacts/b2ContactSolver.h | 1 + Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp | 1 - .../Dynamics/Contacts/b2EdgeAndPolygonContact.cpp | 1 - .../Contacts/b2PolygonAndCircleContact.cpp | 1 - Box2D/Dynamics/Contacts/b2PolygonContact.cpp | 1 - Box2D/Dynamics/Joints/b2GearJoint.cpp | 668 ++++++++------- Box2D/Dynamics/Joints/b2Joint.cpp | 32 +- Box2D/Dynamics/Joints/b2Joint.h | 6 +- Box2D/Dynamics/Joints/b2MotorJoint.cpp | 304 +++++++ Box2D/Dynamics/Joints/b2MotorJoint.h | 133 +++ Box2D/Dynamics/Joints/b2MouseJoint.cpp | 5 + Box2D/Dynamics/Joints/b2MouseJoint.h | 3 + Box2D/Dynamics/Joints/b2PrismaticJoint.cpp | 907 ++++++++++----------- Box2D/Dynamics/Joints/b2PulleyJoint.cpp | 28 +- Box2D/Dynamics/Joints/b2PulleyJoint.h | 9 + Box2D/Dynamics/Joints/b2RevoluteJoint.cpp | 760 +++++++++-------- Box2D/Dynamics/Joints/b2WeldJoint.cpp | 444 +++++----- Box2D/Dynamics/Joints/b2WheelJoint.cpp | 2 +- Box2D/Dynamics/Joints/b2WheelJoint.h | 2 - Box2D/Dynamics/b2Body.cpp | 45 +- Box2D/Dynamics/b2Body.h | 102 ++- Box2D/Dynamics/b2ContactManager.cpp | 7 +- Box2D/Dynamics/b2Fixture.cpp | 4 +- Box2D/Dynamics/b2Island.cpp | 8 +- Box2D/Dynamics/b2World.cpp | 25 +- Box2D/Dynamics/b2World.h | 7 +- Box2D/box2d.pri | 6 +- box2dbody.cpp | 4 +- 60 files changed, 2802 insertions(+), 2167 deletions(-) create mode 100644 Box2D/Dynamics/Joints/b2MotorJoint.cpp create mode 100644 Box2D/Dynamics/Joints/b2MotorJoint.h diff --git a/Box2D/Box2D.h b/Box2D/Box2D.h index f674d82..1870461 100644 --- a/Box2D/Box2D.h +++ b/Box2D/Box2D.h @@ -56,12 +56,13 @@ For discussion please visit http://box2d.org/forum #include #include #include -#include +#include #include #include #include #include #include #include +#include #endif diff --git a/Box2D/Collision/Shapes/b2ChainShape.cpp b/Box2D/Collision/Shapes/b2ChainShape.cpp index bd23346..069b4ca 100644 --- a/Box2D/Collision/Shapes/b2ChainShape.cpp +++ b/Box2D/Collision/Shapes/b2ChainShape.cpp @@ -19,8 +19,7 @@ #include #include #include -#include -using namespace std; +#include b2ChainShape::~b2ChainShape() { @@ -33,6 +32,14 @@ void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) { b2Assert(m_vertices == NULL && m_count == 0); b2Assert(count >= 3); + 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); + } + m_count = count + 1; m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); @@ -47,11 +54,23 @@ void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) { b2Assert(m_vertices == NULL && m_count == 0); 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); + } + m_count = count; m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); + m_hasPrevVertex = false; m_hasNextVertex = false; + + m_prevVertex.SetZero(); + m_nextVertex.SetZero(); } void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex) diff --git a/Box2D/Collision/Shapes/b2ChainShape.h b/Box2D/Collision/Shapes/b2ChainShape.h index d7c5b29..e836dfe 100644 --- a/Box2D/Collision/Shapes/b2ChainShape.h +++ b/Box2D/Collision/Shapes/b2ChainShape.h @@ -95,8 +95,8 @@ inline b2ChainShape::b2ChainShape() m_radius = b2_polygonRadius; m_vertices = NULL; m_count = 0; - m_hasPrevVertex = NULL; - m_hasNextVertex = NULL; + m_hasPrevVertex = false; + m_hasNextVertex = false; } #endif diff --git a/Box2D/Collision/Shapes/b2CircleShape.cpp b/Box2D/Collision/Shapes/b2CircleShape.cpp index d494b13..39d4ae9 100644 --- a/Box2D/Collision/Shapes/b2CircleShape.cpp +++ b/Box2D/Collision/Shapes/b2CircleShape.cpp @@ -18,7 +18,6 @@ #include #include -using namespace std; b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const { diff --git a/Box2D/Collision/Shapes/b2EdgeShape.cpp b/Box2D/Collision/Shapes/b2EdgeShape.cpp index cbac41c..8bcd12f 100644 --- a/Box2D/Collision/Shapes/b2EdgeShape.cpp +++ b/Box2D/Collision/Shapes/b2EdgeShape.cpp @@ -18,7 +18,6 @@ #include #include -using namespace std; void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) { @@ -105,11 +104,11 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, output->fraction = t; if (numerator > 0.0f) { - output->normal = -normal; + output->normal = -b2Mul(xf.q, normal); } else { - output->normal = normal; + output->normal = b2Mul(xf.q, normal); } return true; } diff --git a/Box2D/Collision/Shapes/b2PolygonShape.cpp b/Box2D/Collision/Shapes/b2PolygonShape.cpp index 499e168..2f32e9f 100644 --- a/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/Box2D/Collision/Shapes/b2PolygonShape.cpp @@ -29,7 +29,7 @@ b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const void b2PolygonShape::SetAsBox(float32 hx, float32 hy) { - m_vertexCount = 4; + m_count = 4; m_vertices[0].Set(-hx, -hy); m_vertices[1].Set( hx, -hy); m_vertices[2].Set( hx, hy); @@ -43,7 +43,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy) void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) { - m_vertexCount = 4; + m_count = 4; m_vertices[0].Set(-hx, -hy); m_vertices[1].Set( hx, -hy); m_vertices[2].Set( hx, hy); @@ -59,7 +59,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa xf.q.Set(angle); // Transform vertices and normals. - for (int32 i = 0; i < m_vertexCount; ++i) + for (int32 i = 0; i < m_count; ++i) { m_vertices[i] = b2Mul(xf, m_vertices[i]); m_normals[i] = b2Mul(xf.q, m_normals[i]); @@ -120,61 +120,131 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { b2Assert(3 <= count && count <= b2_maxPolygonVertices); - m_vertexCount = count; + if (count < 3) + { + SetAsBox(1.0f, 1.0f); + return; + } + + int32 n = b2Min(count, b2_maxPolygonVertices); - // Copy vertices. - for (int32 i = 0; i < m_vertexCount; ++i) + // Perform welding and copy vertices into local buffer. + b2Vec2 ps[b2_maxPolygonVertices]; + int32 tempCount = 0; + for (int32 i = 0; i < n; ++i) { - m_vertices[i] = vertices[i]; + b2Vec2 v = vertices[i]; + + bool unique = true; + for (int32 j = 0; j < tempCount; ++j) + { + if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) + { + unique = false; + break; + } + } + + if (unique) + { + ps[tempCount++] = v; + } } - // Compute normals. Ensure the edges have non-zero length. - for (int32 i = 0; i < m_vertexCount; ++i) + n = tempCount; + if (n < 3) { - int32 i1 = i; - int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; - b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; - b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); - m_normals[i] = b2Cross(edge, 1.0f); - m_normals[i].Normalize(); + // Polygon is degenerate. + b2Assert(false); + SetAsBox(1.0f, 1.0f); + return; } -#ifdef _DEBUG - // Ensure the polygon is convex and the interior - // is to the left of each edge. - for (int32 i = 0; i < m_vertexCount; ++i) + // Create the convex hull using the Gift wrapping algorithm + // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm + + // Find the right most point on the hull + int32 i0 = 0; + float32 x0 = ps[0].x; + for (int32 i = 1; i < n; ++i) { - int32 i1 = i; - int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; - b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + float32 x = ps[i].x; + if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) + { + i0 = i; + x0 = x; + } + } + + int32 hull[b2_maxPolygonVertices]; + int32 m = 0; + int32 ih = i0; + + for (;;) + { + hull[m] = ih; - for (int32 j = 0; j < m_vertexCount; ++j) + int32 ie = 0; + for (int32 j = 1; j < n; ++j) { - // Don't check vertices on the current edge. - if (j == i1 || j == i2) + if (ie == ih) { + ie = j; continue; } - - b2Vec2 r = m_vertices[j] - m_vertices[i1]; - // If this crashes, your polygon is non-convex, has colinear edges, - // or the winding order is wrong. - float32 s = b2Cross(edge, r); - b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order"); + b2Vec2 r = ps[ie] - ps[hull[m]]; + b2Vec2 v = ps[j] - ps[hull[m]]; + float32 c = b2Cross(r, v); + if (c < 0.0f) + { + ie = j; + } + + // Collinearity check + if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) + { + ie = j; + } + } + + ++m; + ih = ie; + + if (ie == i0) + { + break; } } -#endif + + m_count = m; + + // Copy vertices. + for (int32 i = 0; i < m; ++i) + { + m_vertices[i] = ps[hull[i]]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (int32 i = 0; i < m; ++i) + { + int32 i1 = i; + int32 i2 = i + 1 < m ? i + 1 : 0; + b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); + m_normals[i] = b2Cross(edge, 1.0f); + m_normals[i].Normalize(); + } // Compute the polygon centroid. - m_centroid = ComputeCentroid(m_vertices, m_vertexCount); + m_centroid = ComputeCentroid(m_vertices, m); } bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const { b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); - for (int32 i = 0; i < m_vertexCount; ++i) + for (int32 i = 0; i < m_count; ++i) { float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); if (dot > 0.0f) @@ -200,7 +270,7 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu int32 index = -1; - for (int32 i = 0; i < m_vertexCount; ++i) + for (int32 i = 0; i < m_count; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 @@ -265,7 +335,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil b2Vec2 lower = b2Mul(xf, m_vertices[0]); b2Vec2 upper = lower; - for (int32 i = 1; i < m_vertexCount; ++i) + for (int32 i = 1; i < m_count; ++i) { b2Vec2 v = b2Mul(xf, m_vertices[i]); lower = b2Min(lower, v); @@ -303,7 +373,7 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // // The rest of the derivation is handled by computer algebra. - b2Assert(m_vertexCount >= 3); + b2Assert(m_count >= 3); b2Vec2 center; center.Set(0.0f, 0.0f); float32 area = 0.0f; @@ -314,19 +384,19 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const b2Vec2 s(0.0f, 0.0f); // This code would put the reference point inside the polygon. - for (int32 i = 0; i < m_vertexCount; ++i) + for (int32 i = 0; i < m_count; ++i) { s += m_vertices[i]; } - s *= 1.0f / m_vertexCount; + s *= 1.0f / m_count; const float32 k_inv3 = 1.0f / 3.0f; - for (int32 i = 0; i < m_vertexCount; ++i) + for (int32 i = 0; i < m_count; ++i) { // Triangle vertices. b2Vec2 e1 = m_vertices[i] - s; - b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s; + b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; float32 D = b2Cross(e1, e2); @@ -359,3 +429,31 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // Shift to center of mass then to original body origin. massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); } + +bool b2PolygonShape::Validate() const +{ + for (int32 i = 0; i < m_count; ++i) + { + int32 i1 = i; + int32 i2 = i < m_count - 1 ? i1 + 1 : 0; + b2Vec2 p = m_vertices[i1]; + b2Vec2 e = m_vertices[i2] - p; + + for (int32 j = 0; j < m_count; ++j) + { + if (j == i1 || j == i2) + { + continue; + } + + b2Vec2 v = m_vertices[j] - p; + float32 c = b2Cross(e, v); + if (c < 0.0f) + { + return false; + } + } + } + + return true; +} diff --git a/Box2D/Collision/Shapes/b2PolygonShape.h b/Box2D/Collision/Shapes/b2PolygonShape.h index 5c53212..b192980 100644 --- a/Box2D/Collision/Shapes/b2PolygonShape.h +++ b/Box2D/Collision/Shapes/b2PolygonShape.h @@ -36,12 +36,14 @@ public: /// @see b2Shape::GetChildCount int32 GetChildCount() const; - /// Copy vertices. This assumes the vertices define a convex polygon. - /// It is assumed that the exterior is the the right of each edge. + /// Create a convex hull from the given array of local points. /// The count must be in the range [3, b2_maxPolygonVertices]. - void Set(const b2Vec2* vertices, int32 vertexCount); + /// @warning the points may be re-ordered, even if they form a convex polygon + /// @warning collinear points are handled but not removed. Collinear points + /// may lead to poor stacking behavior. + void Set(const b2Vec2* points, int32 count); - /// Build vertices to represent an axis-aligned box. + /// Build vertices to represent an axis-aligned box centered on the local origin. /// @param hx the half-width. /// @param hy the half-height. void SetAsBox(float32 hx, float32 hy); @@ -67,28 +69,32 @@ public: void ComputeMass(b2MassData* massData, float32 density) const; /// Get the vertex count. - int32 GetVertexCount() const { return m_vertexCount; } + int32 GetVertexCount() const { return m_count; } /// Get a vertex by index. const b2Vec2& GetVertex(int32 index) const; + /// Validate convexity. This is a very time consuming operation. + /// @returns true if valid + bool Validate() const; + b2Vec2 m_centroid; b2Vec2 m_vertices[b2_maxPolygonVertices]; b2Vec2 m_normals[b2_maxPolygonVertices]; - int32 m_vertexCount; + int32 m_count; }; inline b2PolygonShape::b2PolygonShape() { m_type = e_polygon; m_radius = b2_polygonRadius; - m_vertexCount = 0; + m_count = 0; m_centroid.SetZero(); } inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const { - b2Assert(0 <= index && index < m_vertexCount); + b2Assert(0 <= index && index < m_count); return m_vertices[index]; } diff --git a/Box2D/Collision/b2BroadPhase.cpp b/Box2D/Collision/b2BroadPhase.cpp index b34df87..04e955d 100644 --- a/Box2D/Collision/b2BroadPhase.cpp +++ b/Box2D/Collision/b2BroadPhase.cpp @@ -17,8 +17,6 @@ */ #include -#include -using namespace std; b2BroadPhase::b2BroadPhase() { @@ -90,7 +88,6 @@ void b2BroadPhase::UnBufferMove(int32 proxyId) if (m_moveBuffer[i] == proxyId) { m_moveBuffer[i] = e_nullProxy; - return; } } } diff --git a/Box2D/Collision/b2BroadPhase.h b/Box2D/Collision/b2BroadPhase.h index a289bbf..0c460ca 100644 --- a/Box2D/Collision/b2BroadPhase.h +++ b/Box2D/Collision/b2BroadPhase.h @@ -28,7 +28,6 @@ struct b2Pair { int32 proxyIdA; int32 proxyIdB; - int32 next; }; /// The broad-phase is used for computing pairs and performing volume queries and ray casts. @@ -100,6 +99,11 @@ public: /// Get the quality metric of the embedded tree. float32 GetTreeQuality() const; + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2& newOrigin); + private: friend class b2DynamicTree; @@ -245,4 +249,9 @@ inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) cons m_tree.RayCast(callback, input); } +inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) +{ + m_tree.ShiftOrigin(newOrigin); +} + #endif diff --git a/Box2D/Collision/b2CollideCircle.cpp b/Box2D/Collision/b2CollideCircle.cpp index ae91c73..4181fd4 100644 --- a/Box2D/Collision/b2CollideCircle.cpp +++ b/Box2D/Collision/b2CollideCircle.cpp @@ -63,7 +63,7 @@ void b2CollidePolygonAndCircle( int32 normalIndex = 0; float32 separation = -b2_maxFloat; float32 radius = polygonA->m_radius + circleB->m_radius; - int32 vertexCount = polygonA->m_vertexCount; + int32 vertexCount = polygonA->m_count; const b2Vec2* vertices = polygonA->m_vertices; const b2Vec2* normals = polygonA->m_normals; diff --git a/Box2D/Collision/b2CollideEdge.cpp b/Box2D/Collision/b2CollideEdge.cpp index 92a8e11..88e311c 100644 --- a/Box2D/Collision/b2CollideEdge.cpp +++ b/Box2D/Collision/b2CollideEdge.cpp @@ -425,8 +425,8 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const } // Get polygonB in frameA - m_polygonB.count = polygonB->m_vertexCount; - for (int32 i = 0; i < polygonB->m_vertexCount; ++i) + m_polygonB.count = polygonB->m_count; + for (int32 i = 0; i < polygonB->m_count; ++i) { m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); @@ -497,13 +497,13 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const ie[0].v = m_polygonB.vertices[i1]; ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = i1; + ie[0].id.cf.indexB = static_cast(i1); ie[0].id.cf.typeA = b2ContactFeature::e_face; ie[0].id.cf.typeB = b2ContactFeature::e_vertex; ie[1].v = m_polygonB.vertices[i2]; ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = i2; + ie[1].id.cf.indexB = static_cast(i2); ie[1].id.cf.typeA = b2ContactFeature::e_face; ie[1].id.cf.typeB = b2ContactFeature::e_vertex; @@ -530,13 +530,13 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const ie[0].v = m_v1; ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = primaryAxis.index; + ie[0].id.cf.indexB = static_cast(primaryAxis.index); ie[0].id.cf.typeA = b2ContactFeature::e_vertex; ie[0].id.cf.typeB = b2ContactFeature::e_face; ie[1].v = m_v2; ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = primaryAxis.index; + ie[1].id.cf.indexB = static_cast(primaryAxis.index); ie[1].id.cf.typeA = b2ContactFeature::e_vertex; ie[1].id.cf.typeB = b2ContactFeature::e_face; diff --git a/Box2D/Collision/b2CollidePolygon.cpp b/Box2D/Collision/b2CollidePolygon.cpp index 6869528..0aac38b 100644 --- a/Box2D/Collision/b2CollidePolygon.cpp +++ b/Box2D/Collision/b2CollidePolygon.cpp @@ -19,123 +19,46 @@ #include #include -// Find the separation between poly1 and poly2 for a give edge normal on poly1. -static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - const b2Vec2* vertices1 = poly1->m_vertices; - const b2Vec2* normals1 = poly1->m_normals; - - int32 count2 = poly2->m_vertexCount; - const b2Vec2* vertices2 = poly2->m_vertices; - - b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount); - - // Convert normal from poly1's frame into poly2's frame. - b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]); - b2Vec2 normal1 = b2MulT(xf2.q, normal1World); - - // Find support vertex on poly2 for -normal. - int32 index = 0; - float32 minDot = b2_maxFloat; - - for (int32 i = 0; i < count2; ++i) - { - float32 dot = b2Dot(vertices2[i], normal1); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]); - b2Vec2 v2 = b2Mul(xf2, vertices2[index]); - float32 separation = b2Dot(v2 - v1, normal1World); - return separation; -} - // Find the max separation between poly1 and poly2 using edge normals from poly1. static float32 b2FindMaxSeparation(int32* edgeIndex, const b2PolygonShape* poly1, const b2Transform& xf1, const b2PolygonShape* poly2, const b2Transform& xf2) { - int32 count1 = poly1->m_vertexCount; - const b2Vec2* normals1 = poly1->m_normals; - - // Vector pointing from the centroid of poly1 to the centroid of poly2. - b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid); - b2Vec2 dLocal1 = b2MulT(xf1.q, d); - - // Find edge normal on poly1 that has the largest projection onto d. - int32 edge = 0; - float32 maxDot = -b2_maxFloat; + int32 count1 = poly1->m_count; + int32 count2 = poly2->m_count; + const b2Vec2* n1s = poly1->m_normals; + const b2Vec2* v1s = poly1->m_vertices; + const b2Vec2* v2s = poly2->m_vertices; + b2Transform xf = b2MulT(xf2, xf1); + + int32 bestIndex = 0; + float32 maxSeparation = -b2_maxFloat; for (int32 i = 0; i < count1; ++i) { - float32 dot = b2Dot(normals1[i], dLocal1); - if (dot > maxDot) - { - maxDot = dot; - edge = i; - } - } - - // Get the separation for the edge normal. - float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); - - // Check the separation for the previous edge normal. - int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; - float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); - - // Check the separation for the next edge normal. - int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0; - float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); - - // Find the best edge and the search direction. - int32 bestEdge; - float32 bestSeparation; - int32 increment; - if (sPrev > s && sPrev > sNext) - { - increment = -1; - bestEdge = prevEdge; - bestSeparation = sPrev; - } - else if (sNext > s) - { - increment = 1; - bestEdge = nextEdge; - bestSeparation = sNext; - } - else - { - *edgeIndex = edge; - return s; - } + // Get poly1 normal in frame2. + b2Vec2 n = b2Mul(xf.q, n1s[i]); + b2Vec2 v1 = b2Mul(xf, v1s[i]); - // Perform a local search for the best edge normal. - for ( ; ; ) - { - if (increment == -1) - edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; - else - edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; - - s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); - - if (s > bestSeparation) + // Find deepest point for normal i. + float32 si = b2_maxFloat; + for (int32 j = 0; j < count2; ++j) { - bestEdge = edge; - bestSeparation = s; + float32 sij = b2Dot(n, v2s[j] - v1); + if (sij < si) + { + si = sij; + } } - else + + if (si > maxSeparation) { - break; + maxSeparation = si; + bestIndex = i; } } - *edgeIndex = bestEdge; - return bestSeparation; + *edgeIndex = bestIndex; + return maxSeparation; } static void b2FindIncidentEdge(b2ClipVertex c[2], @@ -144,11 +67,11 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], { const b2Vec2* normals1 = poly1->m_normals; - int32 count2 = poly2->m_vertexCount; + int32 count2 = poly2->m_count; const b2Vec2* vertices2 = poly2->m_vertices; const b2Vec2* normals2 = poly2->m_normals; - b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount); + b2Assert(0 <= edge1 && edge1 < poly1->m_count); // Get the normal of the reference edge in poly2's frame. b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); @@ -210,12 +133,11 @@ void b2CollidePolygons(b2Manifold* manifold, const b2PolygonShape* poly1; // reference polygon const b2PolygonShape* poly2; // incident polygon b2Transform xf1, xf2; - int32 edge1; // reference edge + int32 edge1; // reference edge uint8 flip; - const float32 k_relativeTol = 0.98f; - const float32 k_absoluteTol = 0.001f; + const float32 k_tol = 0.1f * b2_linearSlop; - if (separationB > k_relativeTol * separationA + k_absoluteTol) + if (separationB > separationA + k_tol) { poly1 = polyB; poly2 = polyA; @@ -239,7 +161,7 @@ void b2CollidePolygons(b2Manifold* manifold, b2ClipVertex incidentEdge[2]; b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - int32 count1 = poly1->m_vertexCount; + int32 count1 = poly1->m_count; const b2Vec2* vertices1 = poly1->m_vertices; int32 iv1 = edge1; diff --git a/Box2D/Collision/b2Collision.cpp b/Box2D/Collision/b2Collision.cpp index 8ecb337..4ebf355 100644 --- a/Box2D/Collision/b2Collision.cpp +++ b/Box2D/Collision/b2Collision.cpp @@ -44,6 +44,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold, b2Vec2 cA = pointA + radiusA * normal; b2Vec2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); + separations[0] = b2Dot(cB - cA, normal); } break; @@ -58,6 +59,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold, b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; b2Vec2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cB - cA, normal); } } break; @@ -73,6 +75,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold, b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; b2Vec2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cA - cB, normal); } // Ensure normal points from A to B. @@ -217,7 +220,7 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); // VertexA is hitting edgeB. - vOut[numOut].id.cf.indexA = vertexIndexA; + vOut[numOut].id.cf.indexA = static_cast(vertexIndexA); vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; diff --git a/Box2D/Collision/b2Collision.h b/Box2D/Collision/b2Collision.h index 8bb316c..ad023b3 100644 --- a/Box2D/Collision/b2Collision.h +++ b/Box2D/Collision/b2Collision.h @@ -20,7 +20,7 @@ #define B2_COLLISION_H #include -#include +#include /// @file /// Structures and functions used for computing contact points, distance @@ -117,8 +117,9 @@ struct b2WorldManifold const b2Transform& xfA, float32 radiusA, const b2Transform& xfB, float32 radiusB); - b2Vec2 normal; ///< world vector pointing from A to B - b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) + b2Vec2 normal; ///< world vector pointing from A to B + b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) + float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters }; /// This is used for determining the state of contact points. diff --git a/Box2D/Collision/b2Distance.cpp b/Box2D/Collision/b2Distance.cpp index 8309ea4..78daab3 100644 --- a/Box2D/Collision/b2Distance.cpp +++ b/Box2D/Collision/b2Distance.cpp @@ -31,7 +31,7 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index) { case b2Shape::e_circle: { - const b2CircleShape* circle = (b2CircleShape*)shape; + const b2CircleShape* circle = static_cast(shape); m_vertices = &circle->m_p; m_count = 1; m_radius = circle->m_radius; @@ -40,16 +40,16 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index) case b2Shape::e_polygon: { - const b2PolygonShape* polygon = (b2PolygonShape*)shape; + const b2PolygonShape* polygon = static_cast(shape); m_vertices = polygon->m_vertices; - m_count = polygon->m_vertexCount; + m_count = polygon->m_count; m_radius = polygon->m_radius; } break; case b2Shape::e_chain: { - const b2ChainShape* chain = (b2ChainShape*)shape; + const b2ChainShape* chain = static_cast(shape); b2Assert(0 <= index && index < chain->m_count); m_buffer[0] = chain->m_vertices[index]; @@ -70,7 +70,7 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index) case b2Shape::e_edge: { - const b2EdgeShape* edge = (b2EdgeShape*)shape; + const b2EdgeShape* edge = static_cast(shape); m_vertices = &edge->m_vertex1; m_count = 2; m_radius = edge->m_radius; @@ -141,6 +141,7 @@ struct b2Simplex v->wA = b2Mul(transformA, wALocal); v->wB = b2Mul(transformB, wBLocal); v->w = v->wB - v->wA; + v->a = 1.0f; m_count = 1; } } @@ -244,7 +245,7 @@ struct b2Simplex { case 0: b2Assert(false); - return 0.0; + return 0.0f; case 1: return 0.0f; @@ -465,8 +466,7 @@ void b2Distance(b2DistanceOutput* output, int32 saveA[3], saveB[3]; int32 saveCount = 0; - b2Vec2 closestPoint = simplex.GetClosestPoint(); - float32 distanceSqr1 = closestPoint.LengthSquared(); + float32 distanceSqr1 = b2_maxFloat; float32 distanceSqr2 = distanceSqr1; // Main iteration loop. diff --git a/Box2D/Collision/b2DynamicTree.cpp b/Box2D/Collision/b2DynamicTree.cpp index 2f81958..7035ab8 100644 --- a/Box2D/Collision/b2DynamicTree.cpp +++ b/Box2D/Collision/b2DynamicTree.cpp @@ -17,10 +17,7 @@ */ #include -#include -#include -using namespace std; - +#include b2DynamicTree::b2DynamicTree() { @@ -769,3 +766,13 @@ void b2DynamicTree::RebuildBottomUp() Validate(); } + +void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) +{ + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + m_nodes[i].aabb.lowerBound -= newOrigin; + m_nodes[i].aabb.upperBound -= newOrigin; + } +} diff --git a/Box2D/Collision/b2DynamicTree.h b/Box2D/Collision/b2DynamicTree.h index 0787785..bf80f73 100644 --- a/Box2D/Collision/b2DynamicTree.h +++ b/Box2D/Collision/b2DynamicTree.h @@ -118,6 +118,11 @@ public: /// Build an optimal tree. Very expensive. For testing. void RebuildBottomUp(); + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2& newOrigin); + private: int32 AllocateNode(); diff --git a/Box2D/Collision/b2TimeOfImpact.cpp b/Box2D/Collision/b2TimeOfImpact.cpp index 8ddedef..60da95c 100644 --- a/Box2D/Collision/b2TimeOfImpact.cpp +++ b/Box2D/Collision/b2TimeOfImpact.cpp @@ -21,463 +21,466 @@ #include #include #include +#include -#include -using namespace std; +#include +float32 b2_toiTime, b2_toiMaxTime; int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; int32 b2_toiRootIters, b2_toiMaxRootIters; +// struct b2SeparationFunction { - enum Type - { - e_points, - e_faceA, - e_faceB - }; - - // TODO_ERIN might not need to return the separation - - float32 Initialize(const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float32 t1) - { - m_proxyA = proxyA; - m_proxyB = proxyB; - int32 count = cache->count; - b2Assert(0 < count && count < 3); - - m_sweepA = sweepA; - m_sweepB = sweepB; - - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t1); - m_sweepB.GetTransform(&xfB, t1); - - if (count == 1) - { - m_type = e_points; - b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - m_axis = pointB - pointA; - float32 s = m_axis.Normalize(); - return s; - } - else if (cache->indexA[0] == cache->indexA[1]) - { - // Two points on B and one on A. - m_type = e_faceB; - b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); - b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); - - m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.q, m_axis); - - m_localPoint = 0.5f * (localPointB1 + localPointB2); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 s = b2Dot(pointA - pointB, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - else - { - // Two points on A and one or two points on B. - m_type = e_faceA; - b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); - - m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.q, m_axis); - - m_localPoint = 0.5f * (localPointA1 + localPointA2); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 s = b2Dot(pointB - pointA, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - } - - float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 axisA = b2MulT(xfA.q, m_axis); - b2Vec2 axisB = b2MulT(xfB.q, -m_axis); - - *indexA = m_proxyA->GetSupport(axisA); - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, m_axis); - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 axisB = b2MulT(xfB.q, -normal); - - *indexA = -1; - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 axisA = b2MulT(xfA.q, -normal); - - *indexB = -1; - *indexA = m_proxyA->GetSupport(axisA); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - *indexA = -1; - *indexB = -1; - return 0.0f; - } - } - - float32 Evaluate(int32 indexA, int32 indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { -// b2Vec2 axisA = b2MulT(xfA.q, m_axis); -// b2Vec2 axisB = b2MulT(xfB.q, -m_axis); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); - - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - -// b2Vec2 axisB = b2MulT(xfB.q, -normal); - - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - -// b2Vec2 axisA = b2MulT(xfA.q, -normal); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - return 0.0f; - } - } - - const b2DistanceProxy* m_proxyA; - const b2DistanceProxy* m_proxyB; - b2Sweep m_sweepA, m_sweepB; - Type m_type; - b2Vec2 m_localPoint; - b2Vec2 m_axis; + enum Type + { + e_points, + e_faceA, + e_faceB + }; + + // TODO_ERIN might not need to return the separation + + float32 Initialize(const b2SimplexCache* cache, + const b2DistanceProxy* proxyA, const b2Sweep& sweepA, + const b2DistanceProxy* proxyB, const b2Sweep& sweepB, + float32 t1) + { + m_proxyA = proxyA; + m_proxyB = proxyB; + int32 count = cache->count; + b2Assert(0 < count && count < 3); + + m_sweepA = sweepA; + m_sweepB = sweepB; + + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t1); + m_sweepB.GetTransform(&xfB, t1); + + if (count == 1) + { + m_type = e_points; + b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + m_axis = pointB - pointA; + float32 s = m_axis.Normalize(); + return s; + } + else if (cache->indexA[0] == cache->indexA[1]) + { + // Two points on B and one on A. + m_type = e_faceB; + b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); + b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); + + m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfB.q, m_axis); + + m_localPoint = 0.5f * (localPointB1 + localPointB2); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 s = b2Dot(pointA - pointB, normal); + if (s < 0.0f) + { + m_axis = -m_axis; + s = -s; + } + return s; + } + else + { + // Two points on A and one or two points on B. + m_type = e_faceA; + b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); + + m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfA.q, m_axis); + + m_localPoint = 0.5f * (localPointA1 + localPointA2); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 s = b2Dot(pointB - pointA, normal); + if (s < 0.0f) + { + m_axis = -m_axis; + s = -s; + } + return s; + } + } + + // + float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) + { + case e_points: + { + b2Vec2 axisA = b2MulT(xfA.q, m_axis); + b2Vec2 axisB = b2MulT(xfB.q, -m_axis); + + *indexA = m_proxyA->GetSupport(axisA); + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, m_axis); + return separation; + } + + case e_faceA: + { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 axisB = b2MulT(xfB.q, -normal); + + *indexA = -1; + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: + { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 axisA = b2MulT(xfA.q, -normal); + + *indexB = -1; + *indexA = m_proxyA->GetSupport(axisA); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 separation = b2Dot(pointA - pointB, normal); + return separation; + } + + default: + b2Assert(false); + *indexA = -1; + *indexB = -1; + return 0.0f; + } + } + + // + float32 Evaluate(int32 indexA, int32 indexB, float32 t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) + { + case e_points: + { + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + float32 separation = b2Dot(pointB - pointA, m_axis); + + return separation; + } + + case e_faceA: + { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: + { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 separation = b2Dot(pointA - pointB, normal); + return separation; + } + + default: + b2Assert(false); + return 0.0f; + } + } + + const b2DistanceProxy* m_proxyA; + const b2DistanceProxy* m_proxyB; + b2Sweep m_sweepA, m_sweepB; + Type m_type; + b2Vec2 m_localPoint; + b2Vec2 m_axis; }; // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) { - ++b2_toiCalls; - - output->state = b2TOIOutput::e_unknown; - output->t = input->tMax; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Sweep sweepA = input->sweepA; - b2Sweep sweepB = input->sweepB; - - // Large rotations can make the root finder fail, so we normalize the - // sweep angles. - sweepA.Normalize(); - sweepB.Normalize(); - - float32 tMax = input->tMax; - - float32 totalRadius = proxyA->m_radius + proxyB->m_radius; - float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); - float32 tolerance = 0.25f * b2_linearSlop; - b2Assert(target > tolerance); - - float32 t1 = 0.0f; - const int32 k_maxIterations = 20; // TODO_ERIN b2Settings - int32 iter = 0; - - // Prepare input for distance query. - b2SimplexCache cache; - cache.count = 0; - b2DistanceInput distanceInput; - distanceInput.proxyA = input->proxyA; - distanceInput.proxyB = input->proxyB; - distanceInput.useRadii = false; - - // The outer loop progressively attempts to compute new separating axes. - // This loop terminates when an axis is repeated (no progress is made). - for(;;) - { - b2Transform xfA, xfB; - sweepA.GetTransform(&xfA, t1); - sweepB.GetTransform(&xfB, t1); - - // Get the distance between shapes. We can also use the results - // to get a separating axis. - distanceInput.transformA = xfA; - distanceInput.transformB = xfB; - b2DistanceOutput distanceOutput; - b2Distance(&distanceOutput, &cache, &distanceInput); - - // If the shapes are overlapped, we give up on continuous collision. - if (distanceOutput.distance <= 0.0f) - { - // Failure! - output->state = b2TOIOutput::e_overlapped; - output->t = 0.0f; - break; - } - - if (distanceOutput.distance < target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_touching; - output->t = t1; - break; - } - - // Initialize the separating axis. - b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); + b2Timer timer; + + ++b2_toiCalls; + + output->state = b2TOIOutput::e_unknown; + output->t = input->tMax; + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + b2Sweep sweepA = input->sweepA; + b2Sweep sweepB = input->sweepB; + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.Normalize(); + sweepB.Normalize(); + + float32 tMax = input->tMax; + + float32 totalRadius = proxyA->m_radius + proxyB->m_radius; + float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); + float32 tolerance = 0.25f * b2_linearSlop; + b2Assert(target > tolerance); + + float32 t1 = 0.0f; + const int32 k_maxIterations = 20; // TODO_ERIN b2Settings + int32 iter = 0; + + // Prepare input for distance query. + b2SimplexCache cache; + cache.count = 0; + b2DistanceInput distanceInput; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for(;;) + { + b2Transform xfA, xfB; + sweepA.GetTransform(&xfA, t1); + sweepB.GetTransform(&xfB, t1); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA; + distanceInput.transformB = xfB; + b2DistanceOutput distanceOutput; + b2Distance(&distanceOutput, &cache, &distanceInput); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.distance <= 0.0f) + { + // Failure! + output->state = b2TOIOutput::e_overlapped; + output->t = 0.0f; + break; + } + + if (distanceOutput.distance < target + tolerance) + { + // Victory! + output->state = b2TOIOutput::e_touching; + output->t = t1; + break; + } + + // Initialize the separating axis. + b2SeparationFunction fcn; + fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); #if 0 - // Dump the curve seen by the root finder - { - const int32 N = 100; - float32 dx = 1.0f / N; - float32 xs[N+1]; - float32 fs[N+1]; + // Dump the curve seen by the root finder + { + const int32 N = 100; + float32 dx = 1.0f / N; + float32 xs[N+1]; + float32 fs[N+1]; - float32 x = 0.0f; + float32 x = 0.0f; - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float32 f = fcn.Evaluate(xfA, xfB) - target; + for (int32 i = 0; i <= N; ++i) + { + sweepA.GetTransform(&xfA, x); + sweepB.GetTransform(&xfB, x); + float32 f = fcn.Evaluate(xfA, xfB) - target; - printf("%g %g\n", x, f); + printf("%g %g\n", x, f); - xs[i] = x; - fs[i] = f; + xs[i] = x; + fs[i] = f; - x += dx; - } - } + x += dx; + } + } #endif - // Compute the TOI on the separating axis. We do this by successively - // resolving the deepest point. This loop is bounded by the number of vertices. - bool done = false; - float32 t2 = tMax; - int32 pushBackIter = 0; - for (;;) - { - // Find the deepest point at t2. Store the witness point indices. - int32 indexA, indexB; - float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); - - // Is the final configuration separated? - if (s2 > target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_separated; - output->t = tMax; - done = true; - break; - } - - // Has the separation reached tolerance? - if (s2 > target - tolerance) - { - // Advance the sweeps - t1 = t2; - break; - } - - // Compute the initial separation of the witness points. - float32 s1 = fcn.Evaluate(indexA, indexB, t1); - - // Check for initial overlap. This might happen if the root finder - // runs out of iterations. - if (s1 < target - tolerance) - { - output->state = b2TOIOutput::e_failed; - output->t = t1; - done = true; - break; - } - - // Check for touching - if (s1 <= target + tolerance) - { - // Victory! t1 should hold the TOI (could be 0.0). - output->state = b2TOIOutput::e_touching; - output->t = t1; - done = true; - break; - } - - // Compute 1D root of: f(x) - target = 0 - int32 rootIterCount = 0; - float32 a1 = t1, a2 = t2; - for (;;) - { - // Use a mix of the secant rule and bisection. - float32 t; - if (rootIterCount & 1) - { - // Secant rule to improve convergence. - t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); - } - else - { - // Bisection to guarantee progress. - t = 0.5f * (a1 + a2); - } - - float32 s = fcn.Evaluate(indexA, indexB, t); - - if (b2Abs(s - target) < tolerance) - { - // t2 holds a tentative value for t1 - t2 = t; - break; - } - - // Ensure we continue to bracket the root. - if (s > target) - { - a1 = t; - s1 = s; - } - else - { - a2 = t; - s2 = s; - } - - ++rootIterCount; - ++b2_toiRootIters; - - if (rootIterCount == 50) - { - break; - } - } - - b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); - - ++pushBackIter; - - if (pushBackIter == b2_maxPolygonVertices) - { - break; - } - } - - ++iter; - ++b2_toiIters; - - if (done) - { - break; - } - - if (iter == k_maxIterations) - { - // Root finder got stuck. Semi-victory. - output->state = b2TOIOutput::e_failed; - output->t = t1; - break; - } - } - - b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float32 t2 = tMax; + int32 pushBackIter = 0; + for (;;) + { + // Find the deepest point at t2. Store the witness point indices. + int32 indexA, indexB; + float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); + + // Is the final configuration separated? + if (s2 > target + tolerance) + { + // Victory! + output->state = b2TOIOutput::e_separated; + output->t = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float32 s1 = fcn.Evaluate(indexA, indexB, t1); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) + { + output->state = b2TOIOutput::e_failed; + output->t = t1; + done = true; + break; + } + + // Check for touching + if (s1 <= target + tolerance) + { + // Victory! t1 should hold the TOI (could be 0.0). + output->state = b2TOIOutput::e_touching; + output->t = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int32 rootIterCount = 0; + float32 a1 = t1, a2 = t2; + for (;;) + { + // Use a mix of the secant rule and bisection. + float32 t; + if (rootIterCount & 1) + { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2); + } + + ++rootIterCount; + ++b2_toiRootIters; + + float32 s = fcn.Evaluate(indexA, indexB, t); + + if (b2Abs(s - target) < tolerance) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if (s > target) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if (rootIterCount == 50) + { + break; + } + } + + b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == b2_maxPolygonVertices) + { + break; + } + } + + ++iter; + ++b2_toiIters; + + if (done) + { + break; + } + + if (iter == k_maxIterations) + { + // Root finder got stuck. Semi-victory. + output->state = b2TOIOutput::e_failed; + output->t = t1; + break; + } + } + + b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); + + float32 time = timer.GetMilliseconds(); + b2_toiMaxTime = b2Max(b2_toiMaxTime, time); + b2_toiTime += time; } diff --git a/Box2D/Common/b2BlockAllocator.cpp b/Box2D/Common/b2BlockAllocator.cpp index d5fa97d..f7a4688 100644 --- a/Box2D/Common/b2BlockAllocator.cpp +++ b/Box2D/Common/b2BlockAllocator.cpp @@ -17,11 +17,9 @@ */ #include -#include -#include -#include -#include -using namespace std; +#include +#include +#include int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = { diff --git a/Box2D/Common/b2Draw.h b/Box2D/Common/b2Draw.h index 7c4676b..20359c9 100644 --- a/Box2D/Common/b2Draw.h +++ b/Box2D/Common/b2Draw.h @@ -16,6 +16,9 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#ifndef B2_DRAW_H +#define B2_DRAW_H + #include /// Color for debug drawing. Each value has the range [0,1]. @@ -79,3 +82,5 @@ public: protected: uint32 m_drawFlags; }; + +#endif diff --git a/Box2D/Common/b2GrowableStack.h b/Box2D/Common/b2GrowableStack.h index 27a8eb2..4094644 100644 --- a/Box2D/Common/b2GrowableStack.h +++ b/Box2D/Common/b2GrowableStack.h @@ -19,7 +19,7 @@ #ifndef B2_GROWABLE_STACK_H #define B2_GROWABLE_STACK_H #include -#include +#include /// This is a growable LIFO stack with an initial capacity of N. /// If the stack size exceeds the initial capacity, the heap is used @@ -51,7 +51,7 @@ public: T* old = m_stack; m_capacity *= 2; m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - std::memcpy(m_stack, old, m_count * sizeof(T)); + memcpy(m_stack, old, m_count * sizeof(T)); if (old != m_array) { b2Free(old); diff --git a/Box2D/Common/b2Math.h b/Box2D/Common/b2Math.h index 11c7070..755c519 100644 --- a/Box2D/Common/b2Math.h +++ b/Box2D/Common/b2Math.h @@ -20,24 +20,13 @@ #define B2_MATH_H #include +#include -#include -#include -#include -#include - -/// This function is used to ensure that a floating point number is -/// not a NaN or infinity. +/// This function is used to ensure that a floating point number is not a NaN or infinity. inline bool b2IsValid(float32 x) { - if (x != x) - { - // NaN. - return false; - } - - float32 infinity = std::numeric_limits::infinity(); - return -infinity < x && x < infinity; + int32 ix = *reinterpret_cast(&x); + return (ix & 0x7f800000) != 0x7f800000; } /// This is a approximate yet fast inverse square-root. @@ -57,8 +46,8 @@ inline float32 b2InvSqrt(float32 x) return x; } -#define b2Sqrt(x) std::sqrt(x) -#define b2Atan2(y, x) std::atan2(y, x) +#define b2Sqrt(x) sqrtf(x) +#define b2Atan2(y, x) atan2f(y, x) /// A 2D column vector. struct b2Vec2 @@ -714,8 +703,8 @@ inline void b2Sweep::Advance(float32 alpha) { b2Assert(alpha0 < 1.0f); float32 beta = (alpha - alpha0) / (1.0f - alpha0); - c0 = (1.0f - beta) * c0 + beta * c; - a0 = (1.0f - beta) * a0 + beta * a; + c0 += beta * (c - c0); + a0 += beta * (a - a0); alpha0 = alpha; } diff --git a/Box2D/Common/b2Settings.cpp b/Box2D/Common/b2Settings.cpp index baebe91..c780260 100644 --- a/Box2D/Common/b2Settings.cpp +++ b/Box2D/Common/b2Settings.cpp @@ -17,11 +17,11 @@ */ #include -#include -#include -#include +#include +#include +#include -b2Version b2_version = {2, 2, 1}; +b2Version b2_version = {2, 3, 0}; // Memory allocators. Modify these to use your own allocator. void* b2Alloc(int32 size) @@ -41,4 +41,4 @@ void b2Log(const char* string, ...) va_start(args, string); vprintf(string, args); va_end(args); -} \ No newline at end of file +} diff --git a/Box2D/Common/b2Settings.h b/Box2D/Common/b2Settings.h index 44c0eb1..86f76bd 100644 --- a/Box2D/Common/b2Settings.h +++ b/Box2D/Common/b2Settings.h @@ -19,8 +19,9 @@ #ifndef B2_SETTINGS_H #define B2_SETTINGS_H -#include -#include +#include +#include +#include #define B2_NOT_USED(x) ((void)(x)) #define b2Assert(A) assert(A) diff --git a/Box2D/Common/b2Timer.cpp b/Box2D/Common/b2Timer.cpp index 17cd247..cbea530 100644 --- a/Box2D/Common/b2Timer.cpp +++ b/Box2D/Common/b2Timer.cpp @@ -22,6 +22,7 @@ float64 b2Timer::s_invFrequency = 0.0f; +#define WIN32_LEAN_AND_MEAN #include b2Timer::b2Timer() @@ -72,14 +73,14 @@ void b2Timer::Reset() timeval t; gettimeofday(&t, 0); m_start_sec = t.tv_sec; - m_start_msec = t.tv_usec * 0.001f; + m_start_usec = t.tv_usec; } float32 b2Timer::GetMilliseconds() const { timeval t; gettimeofday(&t, 0); - return (t.tv_sec - m_start_sec) * 1000 + t.tv_usec * 0.001f - m_start_msec; + return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec); } #else diff --git a/Box2D/Common/b2Timer.h b/Box2D/Common/b2Timer.h index b6db888..edf1ca4 100644 --- a/Box2D/Common/b2Timer.h +++ b/Box2D/Common/b2Timer.h @@ -16,6 +16,9 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#ifndef B2_TIMER_H +#define B2_TIMER_H + #include /// Timer for profiling. This has platform specific code and may @@ -40,6 +43,8 @@ private: static float64 s_invFrequency; #elif defined(__linux__) || defined (__APPLE__) unsigned long m_start_sec; - unsigned long m_start_msec; + unsigned long m_start_usec; #endif }; + +#endif diff --git a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp index 81bf682..01c921f 100644 --- a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp +++ b/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp @@ -23,7 +23,6 @@ #include #include -using namespace std; b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp index 0da2187..4642fa4 100644 --- a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp +++ b/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp @@ -23,7 +23,6 @@ #include #include -using namespace std; b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/Box2D/Dynamics/Contacts/b2CircleContact.cpp index ade0920..b47f35a 100644 --- a/Box2D/Dynamics/Contacts/b2CircleContact.cpp +++ b/Box2D/Dynamics/Contacts/b2CircleContact.cpp @@ -24,7 +24,6 @@ #include #include -using namespace std; b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2Contact.cpp b/Box2D/Dynamics/Contacts/b2Contact.cpp index e7e2a2e..d9f1021 100644 --- a/Box2D/Dynamics/Contacts/b2Contact.cpp +++ b/Box2D/Dynamics/Contacts/b2Contact.cpp @@ -102,14 +102,19 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) { b2Assert(s_initialized == true); - if (contact->m_manifold.pointCount > 0) + b2Fixture* fixtureA = contact->m_fixtureA; + b2Fixture* fixtureB = contact->m_fixtureB; + + if (contact->m_manifold.pointCount > 0 && + fixtureA->IsSensor() == false && + fixtureB->IsSensor() == false) { - contact->GetFixtureA()->GetBody()->SetAwake(true); - contact->GetFixtureB()->GetBody()->SetAwake(true); + fixtureA->GetBody()->SetAwake(true); + fixtureB->GetBody()->SetAwake(true); } - b2Shape::Type typeA = contact->GetFixtureA()->GetType(); - b2Shape::Type typeB = contact->GetFixtureB()->GetType(); + b2Shape::Type typeA = fixtureA->GetType(); + b2Shape::Type typeB = fixtureB->GetType(); b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); @@ -147,6 +152,8 @@ b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); + + m_tangentSpeed = 0.0f; } // Update the contact manifold and touching status. diff --git a/Box2D/Dynamics/Contacts/b2Contact.h b/Box2D/Dynamics/Contacts/b2Contact.h index f8e74e9..36e31e2 100644 --- a/Box2D/Dynamics/Contacts/b2Contact.h +++ b/Box2D/Dynamics/Contacts/b2Contact.h @@ -36,7 +36,7 @@ class b2ContactListener; /// For example, anything slides on ice. inline float32 b2MixFriction(float32 friction1, float32 friction2) { - return std::sqrt(friction1 * friction2); + return b2Sqrt(friction1 * friction2); } /// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. @@ -135,6 +135,12 @@ public: /// Reset the restitution to the default value. void ResetRestitution(); + /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. + void SetTangentSpeed(float32 speed); + + /// Get the desired tangent speed. In meters per second. + float32 GetTangentSpeed() const; + /// Evaluate this contact with your own manifold and transforms. virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; @@ -209,6 +215,8 @@ protected: float32 m_friction; float32 m_restitution; + + float32 m_tangentSpeed; }; inline b2Manifold* b2Contact::GetManifold() @@ -328,4 +336,14 @@ inline void b2Contact::ResetRestitution() m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); } +inline void b2Contact::SetTangentSpeed(float32 speed) +{ + m_tangentSpeed = speed; +} + +inline float32 b2Contact::GetTangentSpeed() const +{ + return m_tangentSpeed; +} + #endif diff --git a/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/Box2D/Dynamics/Contacts/b2ContactSolver.cpp index 4e510a6..8224fdc 100644 --- a/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/Box2D/Dynamics/Contacts/b2ContactSolver.cpp @@ -73,6 +73,7 @@ b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) b2ContactVelocityConstraint* vc = m_velocityConstraints + i; vc->friction = contact->m_friction; vc->restitution = contact->m_restitution; + vc->tangentSpeed = contact->m_tangentSpeed; vc->indexA = bodyA->m_islandIndex; vc->indexB = bodyB->m_islandIndex; vc->invMassA = bodyA->m_invMass; @@ -320,7 +321,7 @@ void b2ContactSolver::SolveVelocityConstraints() b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute tangent force - float32 vt = b2Dot(dv, tangent); + float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed; float32 lambda = vcp->tangentMass * (-vt); // b2Clamp the accumulated force @@ -763,8 +764,8 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde iA = pc->invIA; } - float32 mB = pc->invMassB; - float32 iB = pc->invIB; + float32 mB = 0.0f; + float32 iB = 0.; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc->invMassB; diff --git a/Box2D/Dynamics/Contacts/b2ContactSolver.h b/Box2D/Dynamics/Contacts/b2ContactSolver.h index ff3a49c..a622e30 100644 --- a/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ b/Box2D/Dynamics/Contacts/b2ContactSolver.h @@ -51,6 +51,7 @@ struct b2ContactVelocityConstraint float32 invIA, invIB; float32 friction; float32 restitution; + float32 tangentSpeed; int32 pointCount; int32 contactIndex; }; diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp index 742a101..d1fd329 100644 --- a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp +++ b/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp @@ -21,7 +21,6 @@ #include #include -using namespace std; b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp index 0eb09cf..ba536bc 100644 --- a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp +++ b/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp @@ -21,7 +21,6 @@ #include #include -using namespace std; b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp index b5712af..7c9cbcc 100644 --- a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ b/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp @@ -21,7 +21,6 @@ #include #include -using namespace std; b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/Box2D/Dynamics/Contacts/b2PolygonContact.cpp index 47dce21..b5706ca 100644 --- a/Box2D/Dynamics/Contacts/b2PolygonContact.cpp +++ b/Box2D/Dynamics/Contacts/b2PolygonContact.cpp @@ -24,7 +24,6 @@ #include #include -using namespace std; b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { diff --git a/Box2D/Dynamics/Joints/b2GearJoint.cpp b/Box2D/Dynamics/Joints/b2GearJoint.cpp index 03b07a5..155eb0c 100644 --- a/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/Box2D/Dynamics/Joints/b2GearJoint.cpp @@ -44,380 +44,376 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) : b2Joint(def) { - 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; - - // 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; - float32 aA = m_bodyA->m_sweep.a; - b2Transform xfC = m_bodyC->m_xf; - float32 aC = m_bodyC->m_sweep.a; - - if (m_typeA == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; - m_localAnchorC = revolute->m_localAnchorA; - m_localAnchorA = revolute->m_localAnchorB; - m_referenceAngleA = revolute->m_referenceAngle; - m_localAxisC.SetZero(); - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; - m_localAnchorC = prismatic->m_localAnchorA; - m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_referenceAngle; - m_localAxisC = prismatic->m_localXAxisA; - - b2Vec2 pC = m_localAnchorC; - b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - m_bodyD = m_joint2->GetBodyA(); - m_bodyB = m_joint2->GetBodyB(); - - // Get geometry of joint2 - b2Transform xfB = m_bodyB->m_xf; - float32 aB = m_bodyB->m_sweep.a; - b2Transform xfD = m_bodyD->m_xf; - float32 aD = m_bodyD->m_sweep.a; - - if (m_typeB == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; - m_localAnchorD = revolute->m_localAnchorA; - m_localAnchorB = revolute->m_localAnchorB; - m_referenceAngleB = revolute->m_referenceAngle; - m_localAxisD.SetZero(); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; - m_localAnchorD = prismatic->m_localAnchorA; - m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_referenceAngle; - m_localAxisD = prismatic->m_localXAxisA; - - b2Vec2 pD = m_localAnchorD; - b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - m_ratio = def->ratio; - - m_constant = coordinateA + m_ratio * coordinateB; - - m_impulse = 0.0f; + 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; + + // 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; + float32 aA = m_bodyA->m_sweep.a; + b2Transform xfC = m_bodyC->m_xf; + float32 aC = m_bodyC->m_sweep.a; + + if (m_typeA == e_revoluteJoint) + { + b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; + m_localAnchorC = revolute->m_localAnchorA; + m_localAnchorA = revolute->m_localAnchorB; + m_referenceAngleA = revolute->m_referenceAngle; + m_localAxisC.SetZero(); + + coordinateA = aA - aC - m_referenceAngleA; + } + else + { + b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; + m_localAnchorC = prismatic->m_localAnchorA; + m_localAnchorA = prismatic->m_localAnchorB; + m_referenceAngleA = prismatic->m_referenceAngle; + m_localAxisC = prismatic->m_localXAxisA; + + b2Vec2 pC = m_localAnchorC; + b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); + coordinateA = b2Dot(pA - pC, m_localAxisC); + } + + m_bodyD = m_joint2->GetBodyA(); + m_bodyB = m_joint2->GetBodyB(); + + // Get geometry of joint2 + b2Transform xfB = m_bodyB->m_xf; + float32 aB = m_bodyB->m_sweep.a; + b2Transform xfD = m_bodyD->m_xf; + float32 aD = m_bodyD->m_sweep.a; + + if (m_typeB == e_revoluteJoint) + { + b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; + m_localAnchorD = revolute->m_localAnchorA; + m_localAnchorB = revolute->m_localAnchorB; + m_referenceAngleB = revolute->m_referenceAngle; + m_localAxisD.SetZero(); + + coordinateB = aB - aD - m_referenceAngleB; + } + else + { + b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; + m_localAnchorD = prismatic->m_localAnchorA; + m_localAnchorB = prismatic->m_localAnchorB; + m_referenceAngleB = prismatic->m_referenceAngle; + m_localAxisD = prismatic->m_localXAxisA; + + b2Vec2 pD = m_localAnchorD; + b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); + coordinateB = b2Dot(pB - pD, m_localAxisD); + } + + m_ratio = def->ratio; + + m_constant = coordinateA + m_ratio * coordinateB; + + m_impulse = 0.0f; } void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_indexC = m_bodyC->m_islandIndex; - m_indexD = m_bodyD->m_islandIndex; - m_lcA = m_bodyA->m_sweep.localCenter; - m_lcB = m_bodyB->m_sweep.localCenter; - m_lcC = m_bodyC->m_sweep.localCenter; - m_lcD = m_bodyD->m_sweep.localCenter; - m_mA = m_bodyA->m_invMass; - m_mB = m_bodyB->m_invMass; - m_mC = m_bodyC->m_invMass; - m_mD = m_bodyD->m_invMass; - m_iA = m_bodyA->m_invI; - m_iB = m_bodyB->m_invI; - m_iC = m_bodyC->m_invI; - m_iD = m_bodyD->m_invI; - -// b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - -// b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - -// b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - -// b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - m_mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - m_JvAC.SetZero(); - m_JwA = 1.0f; - m_JwC = 1.0f; - m_mass += m_iA + m_iC; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - m_JvAC = u; - m_JwC = b2Cross(rC, u); - m_JwA = b2Cross(rA, u); - m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; - } - - if (m_typeB == e_revoluteJoint) - { - m_JvBD.SetZero(); - m_JwB = m_ratio; - m_JwD = m_ratio; - m_mass += m_ratio * m_ratio * (m_iB + m_iD); - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - m_JvBD = m_ratio * u; - m_JwD = m_ratio * b2Cross(rD, u); - m_JwB = m_ratio * b2Cross(rB, u); - m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; - } - - // Compute effective mass. - m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - - if (data.step.warmStarting) - { - vA += (m_mA * m_impulse) * m_JvAC; - wA += m_iA * m_impulse * m_JwA; - vB += (m_mB * m_impulse) * m_JvBD; - wB += m_iB * m_impulse * m_JwB; - vC -= (m_mC * m_impulse) * m_JvAC; - wC -= m_iC * m_impulse * m_JwC; - vD -= (m_mD * m_impulse) * m_JvBD; - wD -= m_iD * m_impulse * m_JwD; - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_indexC = m_bodyC->m_islandIndex; + m_indexD = m_bodyD->m_islandIndex; + m_lcA = m_bodyA->m_sweep.localCenter; + m_lcB = m_bodyB->m_sweep.localCenter; + m_lcC = m_bodyC->m_sweep.localCenter; + m_lcD = m_bodyD->m_sweep.localCenter; + m_mA = m_bodyA->m_invMass; + m_mB = m_bodyB->m_invMass; + m_mC = m_bodyC->m_invMass; + m_mD = m_bodyD->m_invMass; + m_iA = m_bodyA->m_invI; + m_iB = m_bodyB->m_invI; + m_iC = m_bodyC->m_invI; + m_iD = m_bodyD->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 aC = data.positions[m_indexC].a; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + + float32 aD = data.positions[m_indexD].a; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + m_mass = 0.0f; + + if (m_typeA == e_revoluteJoint) + { + m_JvAC.SetZero(); + m_JwA = 1.0f; + m_JwC = 1.0f; + m_mass += m_iA + m_iC; + } + else + { + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + m_JvAC = u; + m_JwC = b2Cross(rC, u); + m_JwA = b2Cross(rA, u); + m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; + } + + if (m_typeB == e_revoluteJoint) + { + m_JvBD.SetZero(); + m_JwB = m_ratio; + m_JwD = m_ratio; + m_mass += m_ratio * m_ratio * (m_iB + m_iD); + } + else + { + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + m_JvBD = m_ratio * u; + m_JwD = m_ratio * b2Cross(rD, u); + m_JwB = m_ratio * b2Cross(rB, u); + m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; + } + + // Compute effective mass. + m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; + + if (data.step.warmStarting) + { + vA += (m_mA * m_impulse) * m_JvAC; + wA += m_iA * m_impulse * m_JwA; + vB += (m_mB * m_impulse) * m_JvBD; + wB += m_iB * m_impulse * m_JwB; + vC -= (m_mC * m_impulse) * m_JvAC; + wC -= m_iC * m_impulse * m_JwC; + vD -= (m_mD * m_impulse) * m_JvBD; + wD -= m_iD * m_impulse * m_JwD; + } + else + { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); - Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - vA += (m_mA * impulse) * m_JvAC; - wA += m_iA * impulse * m_JwA; - vB += (m_mB * impulse) * m_JvBD; - wB += m_iB * impulse * m_JwB; - vC -= (m_mC * impulse) * m_JvAC; - wC -= m_iC * impulse * m_JwC; - vD -= (m_mD * impulse) * m_JvBD; - wD -= m_iD * impulse * m_JwD; - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; + + float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); + Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); + + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; + + vA += (m_mA * impulse) * m_JvAC; + wA += m_iA * impulse * m_JwA; + vB += (m_mB * impulse) * m_JvBD; + wB += m_iB * impulse * m_JwB; + vC -= (m_mC * impulse) * m_JvAC; + wC -= m_iC * impulse * m_JwC; + vD -= (m_mD * impulse) * m_JvBD; + wD -= m_iD * impulse * m_JwD; + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; - b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - float32 linearError = 0.0f; - - float32 coordinateA, coordinateB; - - b2Vec2 JvAC, JvBD; - float32 JwA, JwB, JwC, JwD; - float32 mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - JvAC.SetZero(); - JwA = 1.0f; - JwC = 1.0f; - mass += m_iA + m_iC; - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - JvAC = u; - JwC = b2Cross(rC, u); - JwA = b2Cross(rA, u); - mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; - - b2Vec2 pC = m_localAnchorC - m_lcC; - b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - if (m_typeB == e_revoluteJoint) - { - JvBD.SetZero(); - JwB = m_ratio; - JwD = m_ratio; - mass += m_ratio * m_ratio * (m_iB + m_iD); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - JvBD = m_ratio * u; - JwD = m_ratio * b2Cross(rD, u); - JwB = m_ratio * b2Cross(rB, u); - mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; - - b2Vec2 pD = m_localAnchorD - m_lcD; - b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; - - float32 impulse = 0.0f; - if (mass > 0.0f) - { - impulse = -C / mass; - } - - cA += m_mA * impulse * JvAC; - aA += m_iA * impulse * JwA; - cB += m_mB * impulse * JvBD; - aB += m_iB * impulse * JwB; - cC -= m_mC * impulse * JvAC; - aC -= m_iC * impulse * JwC; - cD -= m_mD * impulse * JvBD; - aD -= m_iD * impulse * JwD; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - data.positions[m_indexC].c = cC; - data.positions[m_indexC].a = aC; - data.positions[m_indexD].c = cD; - data.positions[m_indexD].a = aD; - - // TODO_ERIN not implemented - return linearError < b2_linearSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 cC = data.positions[m_indexC].c; + float32 aC = data.positions[m_indexC].a; + b2Vec2 cD = data.positions[m_indexD].c; + float32 aD = data.positions[m_indexD].a; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + float32 linearError = 0.0f; + + float32 coordinateA, coordinateB; + + b2Vec2 JvAC, JvBD; + float32 JwA, JwB, JwC, JwD; + float32 mass = 0.0f; + + if (m_typeA == e_revoluteJoint) + { + JvAC.SetZero(); + JwA = 1.0f; + JwC = 1.0f; + mass += m_iA + m_iC; + + coordinateA = aA - aC - m_referenceAngleA; + } + else + { + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + JvAC = u; + JwC = b2Cross(rC, u); + JwA = b2Cross(rA, u); + mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; + + b2Vec2 pC = m_localAnchorC - m_lcC; + b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); + coordinateA = b2Dot(pA - pC, m_localAxisC); + } + + if (m_typeB == e_revoluteJoint) + { + JvBD.SetZero(); + JwB = m_ratio; + JwD = m_ratio; + mass += m_ratio * m_ratio * (m_iB + m_iD); + + coordinateB = aB - aD - m_referenceAngleB; + } + else + { + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + JvBD = m_ratio * u; + JwD = m_ratio * b2Cross(rD, u); + JwB = m_ratio * b2Cross(rB, u); + mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; + + b2Vec2 pD = m_localAnchorD - m_lcD; + b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); + coordinateB = b2Dot(pB - pD, m_localAxisD); + } + + float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; + + float32 impulse = 0.0f; + if (mass > 0.0f) + { + impulse = -C / mass; + } + + cA += m_mA * impulse * JvAC; + aA += m_iA * impulse * JwA; + cB += m_mB * impulse * JvBD; + aB += m_iB * impulse * JwB; + cC -= m_mC * impulse * JvAC; + aC -= m_iC * impulse * JwC; + cD -= m_mD * impulse * JvBD; + aD -= m_iD * impulse * JwD; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + data.positions[m_indexC].c = cC; + data.positions[m_indexC].a = aC; + data.positions[m_indexD].c = cD; + data.positions[m_indexD].a = aD; + + // TODO_ERIN not implemented + return linearError < b2_linearSlop; } b2Vec2 b2GearJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchorA); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2GearJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchorB); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P = m_impulse * m_JvAC; - return inv_dt * P; + b2Vec2 P = m_impulse * m_JvAC; + return inv_dt * P; } float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const { - float32 L = m_impulse * m_JwA; - return inv_dt * L; + float32 L = m_impulse * m_JwA; + return inv_dt * L; } void b2GearJoint::SetRatio(float32 ratio) { - b2Assert(b2IsValid(ratio)); - m_ratio = ratio; + b2Assert(b2IsValid(ratio)); + m_ratio = ratio; } float32 b2GearJoint::GetRatio() const { - return m_ratio; + 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); + 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/Box2D/Dynamics/Joints/b2Joint.cpp b/Box2D/Dynamics/Joints/b2Joint.cpp index 84802bf..dd55be9 100644 --- a/Box2D/Dynamics/Joints/b2Joint.cpp +++ b/Box2D/Dynamics/Joints/b2Joint.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -42,70 +43,77 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) case e_distanceJoint: { void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); - joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def); + joint = new (mem) b2DistanceJoint(static_cast(def)); } break; case e_mouseJoint: { void* mem = allocator->Allocate(sizeof(b2MouseJoint)); - joint = new (mem) b2MouseJoint((b2MouseJointDef*)def); + joint = new (mem) b2MouseJoint(static_cast(def)); } break; case e_prismaticJoint: { void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); - joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def); + joint = new (mem) b2PrismaticJoint(static_cast(def)); } break; case e_revoluteJoint: { void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); - joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def); + joint = new (mem) b2RevoluteJoint(static_cast(def)); } break; case e_pulleyJoint: { void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); - joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def); + joint = new (mem) b2PulleyJoint(static_cast(def)); } break; case e_gearJoint: { void* mem = allocator->Allocate(sizeof(b2GearJoint)); - joint = new (mem) b2GearJoint((b2GearJointDef*)def); + joint = new (mem) b2GearJoint(static_cast(def)); } break; case e_wheelJoint: { void* mem = allocator->Allocate(sizeof(b2WheelJoint)); - joint = new (mem) b2WheelJoint((b2WheelJointDef*)def); + joint = new (mem) b2WheelJoint(static_cast(def)); } break; case e_weldJoint: { void* mem = allocator->Allocate(sizeof(b2WeldJoint)); - joint = new (mem) b2WeldJoint((b2WeldJointDef*)def); + joint = new (mem) b2WeldJoint(static_cast(def)); } break; case e_frictionJoint: { void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); - joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def); + joint = new (mem) b2FrictionJoint(static_cast(def)); } break; case e_ropeJoint: { void* mem = allocator->Allocate(sizeof(b2RopeJoint)); - joint = new (mem) b2RopeJoint((b2RopeJointDef*)def); + joint = new (mem) b2RopeJoint(static_cast(def)); + } + break; + + case e_motorJoint: + { + void* mem = allocator->Allocate(sizeof(b2MotorJoint)); + joint = new (mem) b2MotorJoint(static_cast(def)); } break; @@ -162,6 +170,10 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2RopeJoint)); break; + case e_motorJoint: + allocator->Free(joint, sizeof(b2MotorJoint)); + break; + default: b2Assert(false); break; diff --git a/Box2D/Dynamics/Joints/b2Joint.h b/Box2D/Dynamics/Joints/b2Joint.h index c0f9db8..dd076b6 100644 --- a/Box2D/Dynamics/Joints/b2Joint.h +++ b/Box2D/Dynamics/Joints/b2Joint.h @@ -38,7 +38,8 @@ enum b2JointType e_wheelJoint, e_weldJoint, e_frictionJoint, - e_ropeJoint + e_ropeJoint, + e_motorJoint }; enum b2LimitState @@ -145,6 +146,9 @@ public: /// Dump this joint to the log file. virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } + /// Shift the origin for any points stored in world coordinates. + virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); } + protected: friend class b2World; friend class b2Body; diff --git a/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/Box2D/Dynamics/Joints/b2MotorJoint.cpp new file mode 100644 index 0000000..e5bbcb1 --- /dev/null +++ b/Box2D/Dynamics/Joints/b2MotorJoint.cpp @@ -0,0 +1,304 @@ +/* +* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org +* +* 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. +*/ + +#include +#include +#include + +// Point-to-point constraint +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) +{ + bodyA = bA; + bodyB = bB; + b2Vec2 xB = bodyB->GetPosition(); + linearOffset = bodyA->GetLocalPoint(xB); + + float32 angleA = bodyA->GetAngle(); + float32 angleB = bodyB->GetAngle(); + angularOffset = angleB - angleA; +} + +b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def) +: b2Joint(def) +{ + m_linearOffset = def->linearOffset; + m_angularOffset = def->angularOffset; + + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; + + m_maxForce = def->maxForce; + m_maxTorque = def->maxTorque; + m_correctionFactor = def->correctionFactor; +} + +void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) +{ + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective mass matrix. + m_rA = b2Mul(qA, -m_localCenterA); + m_rB = b2Mul(qB, -m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat22 K; + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; + + m_linearMass = K.GetInverse(); + + m_angularMass = iA + iB; + if (m_angularMass > 0.0f) + { + m_angularMass = 1.0f / m_angularMass; + } + + m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); + m_angularError = aB - aA - m_angularOffset; + + if (data.step.warmStarting) + { + // Scale impulses to support a variable time step. + m_linearImpulse *= data.step.dtRatio; + m_angularImpulse *= data.step.dtRatio; + + b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); + } + else + { + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) +{ + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + float32 h = data.step.dt; + float32 inv_h = data.step.inv_dt; + + // Solve angular friction + { + float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; + float32 impulse = -m_angularMass * Cdot; + + float32 oldImpulse = m_angularImpulse; + float32 maxImpulse = h * m_maxTorque; + m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve linear friction + { + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; + + b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); + b2Vec2 oldImpulse = m_linearImpulse; + m_linearImpulse += impulse; + + float32 maxImpulse = h * m_maxForce; + + if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + m_linearImpulse.Normalize(); + m_linearImpulse *= maxImpulse; + } + + impulse = m_linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); + + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; +} + +bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) +{ + B2_NOT_USED(data); + + return true; +} + +b2Vec2 b2MotorJoint::GetAnchorA() const +{ + return m_bodyA->GetPosition(); +} + +b2Vec2 b2MotorJoint::GetAnchorB() const +{ + return m_bodyB->GetPosition(); +} + +b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const +{ + return inv_dt * m_linearImpulse; +} + +float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const +{ + return inv_dt * m_angularImpulse; +} + +void b2MotorJoint::SetMaxForce(float32 force) +{ + b2Assert(b2IsValid(force) && force >= 0.0f); + m_maxForce = force; +} + +float32 b2MotorJoint::GetMaxForce() const +{ + return m_maxForce; +} + +void b2MotorJoint::SetMaxTorque(float32 torque) +{ + b2Assert(b2IsValid(torque) && torque >= 0.0f); + m_maxTorque = torque; +} + +float32 b2MotorJoint::GetMaxTorque() const +{ + return m_maxTorque; +} + +void b2MotorJoint::SetCorrectionFactor(float32 factor) +{ + b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); + m_correctionFactor = factor; +} + +float32 b2MotorJoint::GetCorrectionFactor() const +{ + return m_correctionFactor; +} + +void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) +{ + if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_linearOffset = linearOffset; + } +} + +const b2Vec2& b2MotorJoint::GetLinearOffset() const +{ + return m_linearOffset; +} + +void b2MotorJoint::SetAngularOffset(float32 angularOffset) +{ + if (angularOffset != m_angularOffset) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_angularOffset = angularOffset; + } +} + +float32 b2MotorJoint::GetAngularOffset() const +{ + return m_angularOffset; +} + +void b2MotorJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2MotorJointDef 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.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y); + b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset); + b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); + b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); + b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/Box2D/Dynamics/Joints/b2MotorJoint.h b/Box2D/Dynamics/Joints/b2MotorJoint.h new file mode 100644 index 0000000..0b76d6f --- /dev/null +++ b/Box2D/Dynamics/Joints/b2MotorJoint.h @@ -0,0 +1,133 @@ +/* +* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org +* +* 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 B2_MOTOR_JOINT_H +#define B2_MOTOR_JOINT_H + +#include + +/// Motor joint definition. +struct b2MotorJointDef : public b2JointDef +{ + b2MotorJointDef() + { + type = e_motorJoint; + linearOffset.SetZero(); + angularOffset = 0.0f; + maxForce = 1.0f; + maxTorque = 1.0f; + correctionFactor = 0.3f; + } + + /// Initialize the bodies and offsets using the current transforms. + void Initialize(b2Body* bodyA, b2Body* bodyB); + + /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians. + float32 angularOffset; + + /// The maximum motor force in N. + float32 maxForce; + + /// The maximum motor torque in N-m. + float32 maxTorque; + + /// Position correction factor in the range [0,1]. + float32 correctionFactor; +}; + +/// A motor joint is used to control the relative motion +/// between two bodies. A typical usage is to control the movement +/// of a dynamic body with respect to the ground. +class b2MotorJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const; + b2Vec2 GetAnchorB() const; + + b2Vec2 GetReactionForce(float32 inv_dt) const; + float32 GetReactionTorque(float32 inv_dt) const; + + /// Set/get the target linear offset, in frame A, in meters. + void SetLinearOffset(const b2Vec2& linearOffset); + const b2Vec2& GetLinearOffset() const; + + /// Set/get the target angular offset, in radians. + void SetAngularOffset(float32 angularOffset); + float32 GetAngularOffset() const; + + /// Set the maximum friction force in N. + void SetMaxForce(float32 force); + + /// Get the maximum friction force in N. + float32 GetMaxForce() const; + + /// Set the maximum friction torque in N*m. + void SetMaxTorque(float32 torque); + + /// Get the maximum friction torque in N*m. + float32 GetMaxTorque() const; + + /// Set the position correction factor in the range [0,1]. + void SetCorrectionFactor(float32 factor); + + /// Get the position correction factor in the range [0,1]. + float32 GetCorrectionFactor() const; + + /// Dump to b2Log + void Dump(); + +protected: + + friend class b2Joint; + + b2MotorJoint(const b2MotorJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data); + void SolveVelocityConstraints(const b2SolverData& data); + bool SolvePositionConstraints(const b2SolverData& data); + + // Solver shared + b2Vec2 m_linearOffset; + float32 m_angularOffset; + b2Vec2 m_linearImpulse; + float32 m_angularImpulse; + float32 m_maxForce; + float32 m_maxTorque; + float32 m_correctionFactor; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + b2Vec2 m_linearError; + float32 m_angularError; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat22 m_linearMass; + float32 m_angularMass; +}; + +#endif diff --git a/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/Box2D/Dynamics/Joints/b2MouseJoint.cpp index 4d19bd4..ea96edd 100644 --- a/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ b/Box2D/Dynamics/Joints/b2MouseJoint.cpp @@ -215,3 +215,8 @@ float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * 0.0f; } + +void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) +{ + m_targetA -= newOrigin; +} diff --git a/Box2D/Dynamics/Joints/b2MouseJoint.h b/Box2D/Dynamics/Joints/b2MouseJoint.h index 682e783..8b50a0e 100644 --- a/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -92,6 +92,9 @@ public: /// The mouse joint does not support dumping. void Dump() { b2Log("Mouse joint dumping is not supported.\n"); } + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2& newOrigin); + protected: friend class b2Joint; diff --git a/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp index 196df02..26f5396 100644 --- a/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -89,549 +89,538 @@ void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + localAxisA = bodyA->GetLocalVector(axis); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localXAxisA.Normalize(); - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorMass = 0.0; - m_motorImpulse = 0.0f; - - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_maxMotorForce = def->maxMotorForce; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; - - m_axis.SetZero(); - m_perp.SetZero(); + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localXAxisA.Normalize(); + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); + m_referenceAngle = def->referenceAngle; + + m_impulse.SetZero(); + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + + m_lowerTranslation = def->lowerTranslation; + m_upperTranslation = def->upperTranslation; + m_maxMotorForce = def->maxMotorForce; + m_motorSpeed = def->motorSpeed; + m_enableLimit = def->enableLimit; + m_enableMotor = def->enableMotor; + m_limitState = e_inactiveLimit; + + m_axis.SetZero(); + m_perp.SetZero(); } void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(qA, m_localXAxisA); - m_a1 = b2Cross(d + rA, m_axis); - m_a2 = b2Cross(rB, m_axis); - - m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(qA, m_localYAxisA); - - m_s1 = b2Cross(d + rA, m_perp); - m_s2 = b2Cross(rB, 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; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For bodies with 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; - - m_K.ex.Set(k11, k12, k13); - m_K.ey.Set(k12, k22, k23); - m_K.ez.Set(k13, k23, k33); - } - - // Compute motor and limit terms. - if (m_enableLimit) - { - float32 jointTranslation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - m_limitState = e_equalLimits; - } - else if (jointTranslation <= m_lowerTranslation) - { - if (m_limitState != e_atLowerLimit) - { - m_limitState = e_atLowerLimit; - m_impulse.z = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; - float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; - float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective masses. + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = (cB - cA) + rB - rA; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Compute motor Jacobian and effective mass. + { + m_axis = b2Mul(qA, m_localXAxisA); + m_a1 = b2Cross(d + rA, m_axis); + m_a2 = b2Cross(rB, m_axis); + + m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + if (m_motorMass > 0.0f) + { + m_motorMass = 1.0f / m_motorMass; + } + } + + // Prismatic constraint. + { + m_perp = b2Mul(qA, m_localYAxisA); + + m_s1 = b2Cross(d + rA, m_perp); + m_s2 = b2Cross(rB, 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; + float32 k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with 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; + + m_K.ex.Set(k11, k12, k13); + m_K.ey.Set(k12, k22, k23); + m_K.ez.Set(k13, k23, k33); + } + + // Compute motor and limit terms. + if (m_enableLimit) + { + float32 jointTranslation = b2Dot(m_axis, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) + { + m_limitState = e_equalLimits; + } + else if (jointTranslation <= m_lowerTranslation) + { + if (m_limitState != e_atLowerLimit) + { + m_limitState = e_atLowerLimit; + m_impulse.z = 0.0f; + } + } + else if (jointTranslation >= m_upperTranslation) + { + if (m_limitState != e_atUpperLimit) + { + m_limitState = e_atUpperLimit; + m_impulse.z = 0.0f; + } + } + else + { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + } + else + { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + + if (m_enableMotor == false) + { + m_motorImpulse = 0.0f; + } + + if (data.step.warmStarting) + { + // Account for variable time step. + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; + float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; + float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 LA = impulse * m_a1; - float32 LB = impulse * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - b2Vec2 Cdot1; - Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; - Cdot1.y = wB - wA; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2; - Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 f1 = m_impulse; - b2Vec3 df = m_K.Solve33(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.z = b2Max(m_impulse.z, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.z = b2Min(m_impulse.z, 0.0f); - } - - // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); - b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); - m_impulse.x = f2r.x; - m_impulse.y = f2r.y; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.z * m_axis; - float32 LA = df.x * m_s1 + df.y + df.z * m_a1; - float32 LB = df.x * m_s2 + df.y + df.z * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - b2Vec2 df = m_K.Solve22(-Cdot1); - m_impulse.x += df.x; - m_impulse.y += df.y; - - b2Vec2 P = df.x * m_perp; - float32 LA = df.x * m_s1 + df.y; - float32 LB = df.x * m_s2 + df.y; - - vA -= mA * P; - wA -= iA * LA; - - 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; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Solve linear motor constraint. + if (m_enableMotor && m_limitState != e_equalLimits) + { + float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + float32 impulse = m_motorMass * (m_motorSpeed - Cdot); + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorForce; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + b2Vec2 P = impulse * m_axis; + float32 LA = impulse * m_a1; + float32 LB = impulse * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + b2Vec2 Cdot1; + Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; + Cdot1.y = wB - wA; + + if (m_enableLimit && m_limitState != e_inactiveLimit) + { + // Solve prismatic and limit constraint in block form. + float32 Cdot2; + Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + + b2Vec3 f1 = m_impulse; + b2Vec3 df = m_K.Solve33(-Cdot); + m_impulse += df; + + if (m_limitState == e_atLowerLimit) + { + m_impulse.z = b2Max(m_impulse.z, 0.0f); + } + else if (m_limitState == e_atUpperLimit) + { + m_impulse.z = b2Min(m_impulse.z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); + b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); + m_impulse.x = f2r.x; + m_impulse.y = f2r.y; + + df = m_impulse - f1; + + b2Vec2 P = df.x * m_perp + df.z * m_axis; + float32 LA = df.x * m_s1 + df.y + df.z * m_a1; + float32 LB = df.x * m_s2 + df.y + df.z * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + b2Vec2 df = m_K.Solve22(-Cdot1); + m_impulse.x += df.x; + m_impulse.y += df.y; + + b2Vec2 P = df.x * m_perp; + float32 LA = df.x * m_s1 + df.y; + float32 LB = df.x * m_s2 + df.y; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute fresh Jacobians - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - b2Vec2 axis = b2Mul(qA, m_localXAxisA); - float32 a1 = b2Cross(d + rA, axis); - float32 a2 = b2Cross(rB, axis); - b2Vec2 perp = b2Mul(qA, m_localYAxisA); - - float32 s1 = b2Cross(d + rA, perp); - float32 s2 = b2Cross(rB, perp); - - b2Vec3 impulse; - b2Vec2 C1; - C1.x = b2Dot(perp, d); - C1.y = aB - aA - m_referenceAngle; - - float32 linearError = b2Abs(C1.x); - float32 angularError = b2Abs(C1.y); - - bool active = false; - float32 C2 = 0.0f; - if (m_enableLimit) - { - float32 translation = b2Dot(axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Max(linearError, b2Abs(translation)); - active = true; - } - else if (translation <= m_lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = b2Max(linearError, m_lowerTranslation - translation); - active = true; - } - else if (translation >= m_upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = b2Max(linearError, translation - m_upperTranslation); - active = true; - } - } - - if (active) - { - 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 * a1 + iB * a2; - float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; - - 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 = K.Solve33(-C); - } - else - { - 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; - } - - b2Mat22 K; - K.ex.Set(k11, k12); - K.ey.Set(k12, k22); - - b2Vec2 impulse1 = K.Solve(-C1); - impulse.x = impulse1.x; - impulse.y = impulse1.y; - impulse.z = 0.0f; - } - - b2Vec2 P = impulse.x * perp + impulse.z * axis; - float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; - float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; - - cA -= mA * P; - aA -= iA * LA; - cB += mB * P; - aB += iB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Compute fresh Jacobians + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = cB + rB - cA - rA; + + b2Vec2 axis = b2Mul(qA, m_localXAxisA); + float32 a1 = b2Cross(d + rA, axis); + float32 a2 = b2Cross(rB, axis); + b2Vec2 perp = b2Mul(qA, m_localYAxisA); + + float32 s1 = b2Cross(d + rA, perp); + float32 s2 = b2Cross(rB, perp); + + b2Vec3 impulse; + b2Vec2 C1; + C1.x = b2Dot(perp, d); + C1.y = aB - aA - m_referenceAngle; + + float32 linearError = b2Abs(C1.x); + float32 angularError = b2Abs(C1.y); + + bool active = false; + float32 C2 = 0.0f; + if (m_enableLimit) + { + float32 translation = b2Dot(axis, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) + { + // Prevent large angular corrections + C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); + linearError = b2Max(linearError, b2Abs(translation)); + active = true; + } + else if (translation <= m_lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); + linearError = b2Max(linearError, m_lowerTranslation - translation); + active = true; + } + else if (translation >= m_upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); + linearError = b2Max(linearError, translation - m_upperTranslation); + active = true; + } + } + + if (active) + { + 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 * a1 + iB * a2; + float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; + + 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 = K.Solve33(-C); + } + else + { + 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; + } + + b2Mat22 K; + K.ex.Set(k11, k12); + K.ey.Set(k12, k22); + + b2Vec2 impulse1 = K.Solve(-C1); + impulse.x = impulse1.x; + impulse.y = impulse1.y; + impulse.z = 0.0f; + } + + b2Vec2 P = impulse.x * perp + impulse.z * axis; + float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; + float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; + + cA -= mA * P; + aA -= iA * LA; + cB += mB * P; + aB += iB * LB; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return linearError <= b2_linearSlop && angularError <= b2_angularSlop; } b2Vec2 b2PrismaticJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchorA); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2PrismaticJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchorB); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const { - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); + return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); } float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const { - return inv_dt * m_impulse.y; + return inv_dt * m_impulse.y; } float32 b2PrismaticJoint::GetJointTranslation() const { - b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); + b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); - float32 translation = b2Dot(d, axis); - return translation; + float32 translation = b2Dot(d, axis); + return translation; } float32 b2PrismaticJoint::GetJointSpeed() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - float32 wB = bB->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); + b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); + b2Vec2 p1 = bA->m_sweep.c + rA; + b2Vec2 p2 = bB->m_sweep.c + rB; + b2Vec2 d = p2 - p1; + b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); + + b2Vec2 vA = bA->m_linearVelocity; + b2Vec2 vB = bB->m_linearVelocity; + float32 wA = bA->m_angularVelocity; + float32 wB = bB->m_angularVelocity; + + float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); + return speed; } bool b2PrismaticJoint::IsLimitEnabled() const { - return m_enableLimit; + return m_enableLimit; } void b2PrismaticJoint::EnableLimit(bool flag) { - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2PrismaticJoint::GetLowerLimit() const { - return m_lowerTranslation; + return m_lowerTranslation; } float32 b2PrismaticJoint::GetUpperLimit() const { - return m_upperTranslation; + return m_upperTranslation; } void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) { - b2Assert(lower <= upper); - if (lower != m_lowerTranslation || upper != m_upperTranslation) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; - m_impulse.z = 0.0f; - } + b2Assert(lower <= upper); + if (lower != m_lowerTranslation || upper != m_upperTranslation) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_lowerTranslation = lower; + m_upperTranslation = upper; + m_impulse.z = 0.0f; + } } bool b2PrismaticJoint::IsMotorEnabled() const { - return m_enableMotor; + return m_enableMotor; } void b2PrismaticJoint::EnableMotor(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; } void b2PrismaticJoint::SetMotorSpeed(float32 speed) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; } void b2PrismaticJoint::SetMaxMotorForce(float32 force) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorForce = force; } float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const { - return inv_dt * m_motorImpulse; + 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); + 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/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/Box2D/Dynamics/Joints/b2PulleyJoint.cpp index b7bab69..9cd340f 100644 --- a/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/Box2D/Dynamics/Joints/b2PulleyJoint.cpp @@ -293,13 +293,28 @@ b2Vec2 b2PulleyJoint::GetGroundAnchorB() const float32 b2PulleyJoint::GetLengthA() const { + return m_lengthA; +} + +float32 b2PulleyJoint::GetLengthB() const +{ + return m_lengthB; +} + +float32 b2PulleyJoint::GetRatio() const +{ + return m_ratio; +} + +float32 b2PulleyJoint::GetCurrentLengthA() const +{ b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); b2Vec2 s = m_groundAnchorA; b2Vec2 d = p - s; return d.Length(); } -float32 b2PulleyJoint::GetLengthB() const +float32 b2PulleyJoint::GetCurrentLengthB() const { b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); b2Vec2 s = m_groundAnchorB; @@ -307,11 +322,6 @@ float32 b2PulleyJoint::GetLengthB() const return d.Length(); } -float32 b2PulleyJoint::GetRatio() const -{ - return m_ratio; -} - void b2PulleyJoint::Dump() { int32 indexA = m_bodyA->m_islandIndex; @@ -330,3 +340,9 @@ void b2PulleyJoint::Dump() b2Log(" jd.ratio = %.15lef;\n", m_ratio); b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } + +void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) +{ + m_groundAnchorA -= newOrigin; + m_groundAnchorB -= newOrigin; +} diff --git a/Box2D/Dynamics/Joints/b2PulleyJoint.h b/Box2D/Dynamics/Joints/b2PulleyJoint.h index 24480ba..44843e7 100644 --- a/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -100,9 +100,18 @@ public: /// Get the pulley ratio. float32 GetRatio() const; + /// Get the current length of the segment attached to bodyA. + float32 GetCurrentLengthA() const; + + /// Get the current length of the segment attached to bodyB. + float32 GetCurrentLengthB() const; + /// Dump joint to dmLog void Dump(); + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2& newOrigin); + protected: friend class b2Joint; diff --git a/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp index ac172da..64bd190 100644 --- a/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ b/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp @@ -35,470 +35,468 @@ void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - - m_lowerAngle = def->lowerAngle; - m_upperAngle = def->upperAngle; - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_referenceAngle = def->referenceAngle; + + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + + m_lowerAngle = def->lowerAngle; + m_upperAngle = def->upperAngle; + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableLimit = def->enableLimit; + m_enableMotor = def->enableMotor; + m_limitState = e_inactiveLimit; } void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - -// b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - -// b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - 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; - - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - - if (m_enableMotor == false || fixedRotation) - { - m_motorImpulse = 0.0f; - } - - if (m_enableLimit && fixedRotation == false) - { - float32 jointAngle = aB - aA - m_referenceAngle; - if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) - { - m_limitState = e_equalLimits; - } - else if (jointAngle <= m_lowerAngle) - { - if (m_limitState != e_atLowerLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atLowerLimit; - } - else if (jointAngle >= m_upperAngle) - { - if (m_limitState != e_atUpperLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atUpperLimit; - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + 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; + + m_motorMass = iA + iB; + if (m_motorMass > 0.0f) + { + m_motorMass = 1.0f / m_motorMass; + } + + if (m_enableMotor == false || fixedRotation) + { + m_motorImpulse = 0.0f; + } + + if (m_enableLimit && fixedRotation == false) + { + float32 jointAngle = aB - aA - m_referenceAngle; + if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) + { + m_limitState = e_equalLimits; + } + else if (jointAngle <= m_lowerAngle) + { + if (m_limitState != e_atLowerLimit) + { + m_impulse.z = 0.0f; + } + m_limitState = e_atLowerLimit; + } + else if (jointAngle >= m_upperAngle) + { + if (m_limitState != e_atUpperLimit) + { + m_impulse.z = 0.0f; + } + m_limitState = e_atUpperLimit; + } + else + { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + } + else + { + m_limitState = e_inactiveLimit; + } + + if (data.step.warmStarting) + { + // Scale impulses to support a variable time step. + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P(m_impulse.x, m_impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); + } + else + { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - // Solve motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - 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 = -m_mass.Solve33(Cdot); - - if (m_limitState == e_equalLimits) - { - m_impulse += impulse; - } - else if (m_limitState == e_atLowerLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse < 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - else if (m_limitState == e_atUpperLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse > 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - 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); - } - else - { - // Solve point-to-point constraint - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = m_mass.Solve22(-Cdot); - - m_impulse.x += impulse.x; - m_impulse.y += impulse.y; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + // Solve motor constraint. + if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) + { + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve limit constraint. + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) + { + 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 = -m_mass.Solve33(Cdot); + + if (m_limitState == e_equalLimits) + { + m_impulse += impulse; + } + else if (m_limitState == e_atLowerLimit) + { + float32 newImpulse = m_impulse.z + impulse.z; + if (newImpulse < 0.0f) + { + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); + impulse.x = reduced.x; + impulse.y = reduced.y; + impulse.z = -m_impulse.z; + m_impulse.x += reduced.x; + m_impulse.y += reduced.y; + m_impulse.z = 0.0f; + } + else + { + m_impulse += impulse; + } + } + else if (m_limitState == e_atUpperLimit) + { + float32 newImpulse = m_impulse.z + impulse.z; + if (newImpulse > 0.0f) + { + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); + impulse.x = reduced.x; + impulse.y = reduced.y; + impulse.z = -m_impulse.z; + m_impulse.x += reduced.x; + m_impulse.y += reduced.y; + m_impulse.z = 0.0f; + } + else + { + 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); + } + else + { + // Solve point-to-point constraint + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + b2Vec2 impulse = m_mass.Solve22(-Cdot); + + m_impulse.x += impulse.x; + m_impulse.y += impulse.y; + + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); + + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 angularError = 0.0f; - float32 positionError = 0.0f; - - bool fixedRotation = (m_invIA + m_invIB == 0.0f); - - // Solve angular limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - float32 angle = aB - aA - m_referenceAngle; - float32 limitImpulse = 0.0f; - - if (m_limitState == e_equalLimits) - { - // Prevent large angular corrections - float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - angularError = b2Abs(C); - } - else if (m_limitState == e_atLowerLimit) - { - float32 C = angle - m_lowerAngle; - angularError = -C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); - limitImpulse = -m_motorMass * C; - } - else if (m_limitState == e_atUpperLimit) - { - float32 C = angle - m_upperAngle; - angularError = C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - } - - aA -= m_invIA * limitImpulse; - aB += m_invIB * limitImpulse; - } - - // Solve point-to-point constraint. - { - qA.Set(aA); - qB.Set(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - b2Vec2 C = cB + rB - cA - rA; - positionError = C.Length(); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; - K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - - b2Vec2 impulse = -K.Solve(C); - - cA -= mA * impulse; - aA -= iA * b2Cross(rA, impulse); - - cB += mB * impulse; - aB += iB * b2Cross(rB, impulse); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float32 angularError = 0.0f; + float32 positionError = 0.0f; + + bool fixedRotation = (m_invIA + m_invIB == 0.0f); + + // Solve angular limit constraint. + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) + { + float32 angle = aB - aA - m_referenceAngle; + float32 limitImpulse = 0.0f; + + if (m_limitState == e_equalLimits) + { + // Prevent large angular corrections + float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); + limitImpulse = -m_motorMass * C; + angularError = b2Abs(C); + } + else if (m_limitState == e_atLowerLimit) + { + float32 C = angle - m_lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); + limitImpulse = -m_motorMass * C; + } + else if (m_limitState == e_atUpperLimit) + { + float32 C = angle - m_upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); + limitImpulse = -m_motorMass * C; + } + + aA -= m_invIA * limitImpulse; + aB += m_invIB * limitImpulse; + } + + // Solve point-to-point constraint. + { + qA.Set(aA); + qB.Set(aB); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + b2Vec2 C = cB + rB - cA - rA; + positionError = C.Length(); + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat22 K; + K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; + K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; + + b2Vec2 impulse = -K.Solve(C); + + cA -= mA * impulse; + aA -= iA * b2Cross(rA, impulse); + + cB += mB * impulse; + aB += iB * b2Cross(rB, impulse); + } + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } b2Vec2 b2RevoluteJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchorA); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2RevoluteJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchorB); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; } float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const { - return inv_dt * m_impulse.z; + return inv_dt * m_impulse.z; } float32 b2RevoluteJoint::GetJointAngle() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; } float32 b2RevoluteJoint::GetJointSpeed() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_angularVelocity - bA->m_angularVelocity; + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_angularVelocity - bA->m_angularVelocity; } bool b2RevoluteJoint::IsMotorEnabled() const { - return m_enableMotor; + return m_enableMotor; } void b2RevoluteJoint::EnableMotor(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; } float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const { - return inv_dt * m_motorImpulse; + return inv_dt * m_motorImpulse; } void b2RevoluteJoint::SetMotorSpeed(float32 speed) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; } void b2RevoluteJoint::SetMaxMotorTorque(float32 torque) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; } bool b2RevoluteJoint::IsLimitEnabled() const { - return m_enableLimit; + return m_enableLimit; } void b2RevoluteJoint::EnableLimit(bool flag) { - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2RevoluteJoint::GetLowerLimit() const { - return m_lowerAngle; + return m_lowerAngle; } float32 b2RevoluteJoint::GetUpperLimit() const { - return m_upperAngle; + return m_upperAngle; } void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) { - b2Assert(lower <= upper); - - if (lower != m_lowerAngle || upper != m_upperAngle) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_impulse.z = 0.0f; - m_lowerAngle = lower; - m_upperAngle = upper; - } + b2Assert(lower <= upper); + + if (lower != m_lowerAngle || upper != m_upperAngle) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_impulse.z = 0.0f; + m_lowerAngle = lower; + 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); + 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/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/Box2D/Dynamics/Joints/b2WeldJoint.cpp index f04bf88..253f721 100644 --- a/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -36,295 +36,293 @@ void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_referenceAngle = def->referenceAngle; + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; - m_impulse.SetZero(); + m_impulse.SetZero(); } void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - -// b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - -// b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - 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) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_impulse.z); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + 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) + { + // Scale impulses to support a variable time step. + m_impulse *= data.step.dtRatio; + + b2Vec2 P(m_impulse.x, m_impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_impulse.z); + } + else + { + m_impulse.SetZero(); + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - if (m_frequencyHz > 0.0f) - { - float32 Cdot2 = wB - wA; + if (m_frequencyHz > 0.0f) + { + float32 Cdot2 = wB - wA; - float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); - m_impulse.z += impulse2; + float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); + m_impulse.z += impulse2; - wA -= iA * impulse2; - wB += iB * impulse2; + wA -= iA * impulse2; + wB += iB * impulse2; - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); - m_impulse.x += impulse1.x; - m_impulse.y += impulse1.y; + b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); + m_impulse.x += impulse1.x; + m_impulse.y += impulse1.y; - b2Vec2 P = impulse1; + b2Vec2 P = impulse1; - vA -= mA * P; - wA -= iA * b2Cross(m_rA, P); + 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); + 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; + b2Vec3 impulse = -b2Mul(m_mass, Cdot); + m_impulse += impulse; - b2Vec2 P(impulse.x, impulse.y); + b2Vec2 P(impulse.x, impulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + impulse.z); - vB += mB * P; - wB += iB * (b2Cross(m_rB, 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; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Rot qA(aA), qB(aB); + b2Rot qA(aA), qB(aB); - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - float32 positionError, angularError; + float32 positionError, angularError; - 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; + 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; - if (m_frequencyHz > 0.0f) - { - b2Vec2 C1 = cB + rB - cA - rA; + if (m_frequencyHz > 0.0f) + { + b2Vec2 C1 = cB + rB - cA - rA; - positionError = C1.Length(); - angularError = 0.0f; + positionError = C1.Length(); + angularError = 0.0f; - b2Vec2 P = -K.Solve22(C1); + b2Vec2 P = -K.Solve22(C1); - cA -= mA * P; - aA -= iA * b2Cross(rA, P); + cA -= mA * P; + aA -= iA * b2Cross(rA, P); - 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); + } + else + { + b2Vec2 C1 = cB + rB - cA - rA; + float32 C2 = aB - aA - m_referenceAngle; - positionError = C1.Length(); - angularError = b2Abs(C2); + positionError = C1.Length(); + angularError = b2Abs(C2); - b2Vec3 C(C1.x, C1.y, C2); + b2Vec3 C(C1.x, C1.y, C2); + + b2Vec3 impulse = -K.Solve33(C); + b2Vec2 P(impulse.x, impulse.y); - b2Vec3 impulse = -K.Solve33(C); - b2Vec2 P(impulse.x, impulse.y); + cA -= mA * P; + aA -= iA * (b2Cross(rA, P) + impulse.z); - cA -= mA * P; - aA -= iA * (b2Cross(rA, P) + impulse.z); + cB += mB * P; + aB += iB * (b2Cross(rB, 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; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; + return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } b2Vec2 b2WeldJoint::GetAnchorA() const { - return m_bodyA->GetWorldPoint(m_localAnchorA); + return m_bodyA->GetWorldPoint(m_localAnchorA); } b2Vec2 b2WeldJoint::GetAnchorB() const { - return m_bodyB->GetWorldPoint(m_localAnchorB); + return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; } float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const { - return inv_dt * m_impulse.z; + 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); + 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/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/Box2D/Dynamics/Joints/b2WheelJoint.cpp index 5ecc892..c9f6de6 100644 --- a/Box2D/Dynamics/Joints/b2WheelJoint.cpp +++ b/Box2D/Dynamics/Joints/b2WheelJoint.cpp @@ -55,7 +55,7 @@ b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) m_mass = 0.0f; m_impulse = 0.0f; - m_motorMass = 0.0; + m_motorMass = 0.0f; m_motorImpulse = 0.0f; m_springMass = 0.0f; m_springImpulse = 0.0f; diff --git a/Box2D/Dynamics/Joints/b2WheelJoint.h b/Box2D/Dynamics/Joints/b2WheelJoint.h index 968eeaf..ca59d7a 100644 --- a/Box2D/Dynamics/Joints/b2WheelJoint.h +++ b/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -79,8 +79,6 @@ struct b2WheelJointDef : public b2JointDef class b2WheelJoint : public b2Joint { public: - void GetDefinition(b2WheelJointDef* def) const; - b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; diff --git a/Box2D/Dynamics/b2Body.cpp b/Box2D/Dynamics/b2Body.cpp index b19c7ff..b655053 100644 --- a/Box2D/Dynamics/b2Body.cpp +++ b/Box2D/Dynamics/b2Body.cpp @@ -141,10 +141,25 @@ void b2Body::SetType(b2BodyType type) m_force.SetZero(); m_torque = 0.0f; - // Since the body type changed, we need to flag contacts for filtering. + // Delete the attached contacts. + b2ContactEdge* ce = m_contactList; + while (ce) + { + b2ContactEdge* ce0 = ce; + ce = ce->next; + m_world->m_contactManager.Destroy(ce0->contact); + } + m_contactList = NULL; + + // Touch the proxies so that new contacts will be created (when appropriate) + b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - f->Refilter(); + int32 proxyCount = f->m_proxyCount; + for (int32 i = 0; i < proxyCount; ++i) + { + broadPhase->TouchProxy(f->m_proxies[i].proxyId); + } } } @@ -421,8 +436,6 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle) { f->Synchronize(broadPhase, m_xf, m_xf); } - - m_world->m_contactManager.FindNewContacts(); } void b2Body::SynchronizeFixtures() @@ -483,6 +496,28 @@ void b2Body::SetActive(bool flag) } } +void b2Body::SetFixedRotation(bool flag) +{ + bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; + if (status == flag) + { + return; + } + + if (flag) + { + m_flags |= e_fixedRotationFlag; + } + else + { + m_flags &= ~e_fixedRotationFlag; + } + + m_angularVelocity = 0.0f; + + ResetMassData(); +} + void b2Body::Dump() { int32 bodyIndex = m_islandIndex; @@ -511,4 +546,4 @@ void b2Body::Dump() b2Log(" }\n"); } b2Log("}\n"); -} \ No newline at end of file +} diff --git a/Box2D/Dynamics/b2Body.h b/Box2D/Dynamics/b2Body.h index c600951..24bae64 100644 --- a/Box2D/Dynamics/b2Body.h +++ b/Box2D/Dynamics/b2Body.h @@ -154,8 +154,8 @@ public: void DestroyFixture(b2Fixture* fixture); /// Set the position of the body's origin and rotation. - /// This breaks any contacts and wakes the other bodies. /// Manipulating a body's transform may cause non-physical behavior. + /// Note: contacts are updated on the next call to b2World::Step. /// @param position the world position of the body's local origin. /// @param angle the world rotation in radians. void SetTransform(const b2Vec2& position, float32 angle); @@ -184,7 +184,7 @@ public: /// Get the linear velocity of the center of mass. /// @return the linear velocity of the center of mass. - b2Vec2 GetLinearVelocity() const; + const b2Vec2& GetLinearVelocity() const; /// Set the angular velocity. /// @param omega the new angular velocity in radians/second. @@ -199,28 +199,33 @@ public: /// affect the angular velocity. This wakes up the body. /// @param force the world force vector, usually in Newtons (N). /// @param point the world position of the point of application. - void ApplyForce(const b2Vec2& force, const b2Vec2& point); + /// @param wake also wake up the body + void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake); /// Apply a force to the center of mass. This wakes up the body. /// @param force the world force vector, usually in Newtons (N). - void ApplyForceToCenter(const b2Vec2& force); + /// @param wake also wake up the body + void ApplyForceToCenter(const b2Vec2& force, bool wake); /// Apply a torque. This affects the angular velocity /// without affecting the linear velocity of the center of mass. /// This wakes up the body. /// @param torque about the z-axis (out of the screen), usually in N-m. - void ApplyTorque(float32 torque); + /// @param wake also wake up the body + void ApplyTorque(float32 torque, bool wake); /// Apply an impulse at a point. This immediately modifies the velocity. /// It also modifies the angular velocity if the point of application /// is not at the center of mass. This wakes up the body. /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. /// @param point the world position of the point of application. - void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point); + /// @param wake also wake up the body + void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); /// Apply an angular impulse. /// @param impulse the angular impulse in units of kg*m*m/s - void ApplyAngularImpulse(float32 impulse); + /// @param wake also wake up the body + void ApplyAngularImpulse(float32 impulse, bool wake); /// Get the total mass of the body. /// @return the mass, usually in kilograms (kg). @@ -315,11 +320,11 @@ public: /// Set the sleep state of the body. A sleeping body has very /// low CPU cost. - /// @param flag set to true to put body to sleep, false to wake it. + /// @param flag set to true to wake the body, false to put it to sleep. void SetAwake(bool flag); /// Get the sleeping state of this body. - /// @return true if the body is sleeping. + /// @return true if the body is awake. bool IsAwake() const; /// Set the active state of the body. An inactive body is not @@ -387,15 +392,16 @@ private: friend class b2Contact; friend class b2DistanceJoint; + friend class b2FrictionJoint; friend class b2GearJoint; - friend class b2WheelJoint; + friend class b2MotorJoint; friend class b2MouseJoint; friend class b2PrismaticJoint; friend class b2PulleyJoint; friend class b2RevoluteJoint; - friend class b2WeldJoint; - friend class b2FrictionJoint; friend class b2RopeJoint; + friend class b2WeldJoint; + friend class b2WheelJoint; // m_flags enum @@ -505,7 +511,7 @@ inline void b2Body::SetLinearVelocity(const b2Vec2& v) m_linearVelocity = v; } -inline b2Vec2 b2Body::GetLinearVelocity() const +inline const b2Vec2& b2Body::GetLinearVelocity() const { return m_linearVelocity; } @@ -655,20 +661,6 @@ inline bool b2Body::IsActive() const return (m_flags & e_activeFlag) == e_activeFlag; } -inline void b2Body::SetFixedRotation(bool flag) -{ - if (flag) - { - m_flags |= e_fixedRotationFlag; - } - else - { - m_flags &= ~e_fixedRotationFlag; - } - - ResetMassData(); -} - inline bool b2Body::IsFixedRotation() const { return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; @@ -742,79 +734,101 @@ inline void* b2Body::GetUserData() const return m_userData; } -inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point) +inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) { if (m_type != b2_dynamicBody) { return; } - if (IsAwake() == false) + if (wake && (m_flags & e_awakeFlag) == 0) { SetAwake(true); } - m_force += force; - m_torque += b2Cross(point - m_sweep.c, force); + // Don't accumulate a force if the body is sleeping. + if (m_flags & e_awakeFlag) + { + m_force += force; + m_torque += b2Cross(point - m_sweep.c, force); + } } -inline void b2Body::ApplyForceToCenter(const b2Vec2& force) +inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) { if (m_type != b2_dynamicBody) { return; } - if (IsAwake() == false) + if (wake && (m_flags & e_awakeFlag) == 0) { SetAwake(true); } - m_force += force; + // Don't accumulate a force if the body is sleeping + if (m_flags & e_awakeFlag) + { + m_force += force; + } } -inline void b2Body::ApplyTorque(float32 torque) +inline void b2Body::ApplyTorque(float32 torque, bool wake) { if (m_type != b2_dynamicBody) { return; } - if (IsAwake() == false) + if (wake && (m_flags & e_awakeFlag) == 0) { SetAwake(true); } - m_torque += torque; + // Don't accumulate a force if the body is sleeping + if (m_flags & e_awakeFlag) + { + m_torque += torque; + } } -inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point) +inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) { if (m_type != b2_dynamicBody) { return; } - if (IsAwake() == false) + if (wake && (m_flags & e_awakeFlag) == 0) { SetAwake(true); } - m_linearVelocity += m_invMass * impulse; - m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); + + // Don't accumulate velocity if the body is sleeping + if (m_flags & e_awakeFlag) + { + m_linearVelocity += m_invMass * impulse; + m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); + } } -inline void b2Body::ApplyAngularImpulse(float32 impulse) +inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake) { if (m_type != b2_dynamicBody) { return; } - if (IsAwake() == false) + if (wake && (m_flags & e_awakeFlag) == 0) { SetAwake(true); } - m_angularVelocity += m_invI * impulse; + + // Don't accumulate velocity if the body is sleeping + if (m_flags & e_awakeFlag) + { + m_angularVelocity += m_invI * impulse; + } } inline void b2Body::SynchronizeTransform() diff --git a/Box2D/Dynamics/b2ContactManager.cpp b/Box2D/Dynamics/b2ContactManager.cpp index 47331bd..d8a189b 100644 --- a/Box2D/Dynamics/b2ContactManager.cpp +++ b/Box2D/Dynamics/b2ContactManager.cpp @@ -286,8 +286,11 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) bodyB->m_contactList = &c->m_nodeB; // Wake up the bodies - bodyA->SetAwake(true); - bodyB->SetAwake(true); + if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) + { + bodyA->SetAwake(true); + bodyB->SetAwake(true); + } ++m_contactCount; } diff --git a/Box2D/Dynamics/b2Fixture.cpp b/Box2D/Dynamics/b2Fixture.cpp index 1d09c6d..fb17f0a 100644 --- a/Box2D/Dynamics/b2Fixture.cpp +++ b/Box2D/Dynamics/b2Fixture.cpp @@ -267,11 +267,11 @@ void b2Fixture::Dump(int32 bodyIndex) 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) + 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.Set(vs, %d);\n", s->m_vertexCount); + b2Log(" shape.Set(vs, %d);\n", s->m_count); } break; diff --git a/Box2D/Dynamics/b2Island.cpp b/Box2D/Dynamics/b2Island.cpp index 0ecb273..d46589d 100644 --- a/Box2D/Dynamics/b2Island.cpp +++ b/Box2D/Dynamics/b2Island.cpp @@ -211,10 +211,10 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g // Solution: v(t) = v0 * exp(-c * t) // Time 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 - // Taylor expansion: - // v2 = (1.0f - c * dt) * v1 - v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f); - w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f); + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + v *= 1.0f / (1.0f + h * b->m_linearDamping); + w *= 1.0f / (1.0f + h * b->m_angularDamping); } m_positions[i].c = c; diff --git a/Box2D/Dynamics/b2World.cpp b/Box2D/Dynamics/b2World.cpp index fae03ec..a3a6a1e 100644 --- a/Box2D/Dynamics/b2World.cpp +++ b/Box2D/Dynamics/b2World.cpp @@ -1073,7 +1073,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color case b2Shape::e_polygon: { b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - int32 vertexCount = poly->m_vertexCount; + int32 vertexCount = poly->m_count; b2Assert(vertexCount <= b2_maxPolygonVertices); b2Vec2 vertices[b2_maxPolygonVertices]; @@ -1256,6 +1256,29 @@ float32 b2World::GetTreeQuality() const return m_contactManager.m_broadPhase.GetTreeQuality(); } +void b2World::ShiftOrigin(const b2Vec2& newOrigin) +{ + b2Assert((m_flags & e_locked) == 0); + if ((m_flags & e_locked) == e_locked) + { + return; + } + + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_xf.p -= newOrigin; + b->m_sweep.c0 -= newOrigin; + b->m_sweep.c -= newOrigin; + } + + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + j->ShiftOrigin(newOrigin); + } + + m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); +} + void b2World::Dump() { if ((m_flags & e_locked) == e_locked) diff --git a/Box2D/Dynamics/b2World.h b/Box2D/Dynamics/b2World.h index eba7572..4f94f6e 100644 --- a/Box2D/Dynamics/b2World.h +++ b/Box2D/Dynamics/b2World.h @@ -104,7 +104,7 @@ public: /// @see SetAutoClearForces void ClearForces(); - /// Call this to draw shapes and other debug draw data. + /// Call this to draw shapes and other debug draw data. This is intentionally non-const. void DrawDebugData(); /// Query the world for all fixtures that potentially overlap the @@ -194,6 +194,11 @@ public: /// Get the flag that controls automatic clearing of forces after each time step. bool GetAutoClearForces() const; + /// Shift the world origin. Useful for large worlds. + /// The body shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2& newOrigin); + /// Get the contact manager for testing. const b2ContactManager& GetContactManager() const; diff --git a/Box2D/box2d.pri b/Box2D/box2d.pri index 038f43d..e8f4efd 100644 --- a/Box2D/box2d.pri +++ b/Box2D/box2d.pri @@ -45,7 +45,8 @@ SOURCES += \ $$PWD/Dynamics/b2Fixture.cpp \ $$PWD/Dynamics/b2ContactManager.cpp \ $$PWD/Dynamics/b2Body.cpp \ - $$PWD/Rope/b2Rope.cpp + $$PWD/Rope/b2Rope.cpp \ + $$PWD/Dynamics/Joints/b2MotorJoint.cpp HEADERS += \ $$PWD/Box2D.h \ @@ -93,4 +94,5 @@ HEADERS += \ $$PWD/Dynamics/b2Fixture.h \ $$PWD/Dynamics/b2ContactManager.h \ $$PWD/Dynamics/b2Body.h \ - $$PWD/Rope/b2Rope.h + $$PWD/Rope/b2Rope.h \ + $$PWD/Dynamics/Joints/b2MotorJoint.h diff --git a/box2dbody.cpp b/box2dbody.cpp index 46477dc..980b72f 100644 --- a/box2dbody.cpp +++ b/box2dbody.cpp @@ -271,14 +271,14 @@ void Box2DBody::applyLinearImpulse(const QPointF &impulse, mBody->ApplyLinearImpulse(b2Vec2(impulse.x() / scaleRatio, -impulse.y() / scaleRatio), b2Vec2(point.x() / scaleRatio, - -point.y() / scaleRatio)); + -point.y() / scaleRatio),true); } } void Box2DBody::applyTorque(qreal torque) { if (mBody) - mBody->ApplyTorque(torque); + mBody->ApplyTorque(torque,true); } QPointF Box2DBody::getWorldCenter() const -- 2.1.4