From: dsc Date: Thu, 5 May 2011 06:30:05 +0000 (-0700) Subject: Updates to Box2D source from SVN trunk. X-Git-Tag: box2d-testbed~9 X-Git-Url: http://git.lttlst.com:3516/?a=commitdiff_plain;h=045551514a16496883d983655668c4381b85b696;p=tanks-ios.git Updates to Box2D source from SVN trunk. --- diff --git a/libs/box2d/box2d-iphone.xcodeproj/project.pbxproj b/libs/box2d/box2d-iphone.xcodeproj/project.pbxproj index 3eda5e3..6e1af9d 100644 --- a/libs/box2d/box2d-iphone.xcodeproj/project.pbxproj +++ b/libs/box2d/box2d-iphone.xcodeproj/project.pbxproj @@ -16,9 +16,7 @@ 491331AD1372653600DFB46D /* iPhoneTestEntries.mm in Sources */ = {isa = PBXBuildFile; fileRef = 491331A21372653600DFB46D /* iPhoneTestEntries.mm */; }; 491331AE1372653600DFB46D /* TestEntriesViewController.mm in Sources */ = {isa = PBXBuildFile; fileRef = 491331A41372653600DFB46D /* TestEntriesViewController.mm */; }; 491331AF1372653600DFB46D /* Icon.png in Resources */ = {isa = PBXBuildFile; fileRef = 491331A51372653600DFB46D /* Icon.png */; }; - 491331B01372653600DFB46D /* Info.plist in Resources */ = {isa = PBXBuildFile; fileRef = 491331A61372653600DFB46D /* Info.plist */; }; 491331B11372653600DFB46D /* main.m in Sources */ = {isa = PBXBuildFile; fileRef = 491331A71372653600DFB46D /* main.m */; }; - 491331B21372653600DFB46D /* MainWindow.xib in Resources */ = {isa = PBXBuildFile; fileRef = 491331A81372653600DFB46D /* MainWindow.xib */; }; 4913323813726B1F00DFB46D /* ApplyForce.h in Headers */ = {isa = PBXBuildFile; fileRef = 4913320D13726B1F00DFB46D /* ApplyForce.h */; }; 4913323913726B1F00DFB46D /* BodyTypes.h in Headers */ = {isa = PBXBuildFile; fileRef = 4913320E13726B1F00DFB46D /* BodyTypes.h */; }; 4913323A13726B1F00DFB46D /* Breakable.h in Headers */ = {isa = PBXBuildFile; fileRef = 4913320F13726B1F00DFB46D /* Breakable.h */; }; @@ -430,8 +428,6 @@ buildActionMask = 2147483647; files = ( 491331AF1372653600DFB46D /* Icon.png in Resources */, - 491331B01372653600DFB46D /* Info.plist in Resources */, - 491331B21372653600DFB46D /* MainWindow.xib in Resources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/libs/box2d/src/Box2D/Box2D.h b/libs/box2d/src/Box2D/Box2D.h index dc5701f..0414477 100644 --- a/libs/box2d/src/Box2D/Box2D.h +++ b/libs/box2d/src/Box2D/Box2D.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -32,8 +32,12 @@ For discussion please visit http://box2d.org/forum // These include files constitute the main Box2D API #include +#include +#include #include +#include +#include #include #include @@ -52,11 +56,14 @@ For discussion please visit http://box2d.org/forum #include #include #include -#include +#include #include #include #include #include +#include #include +#include + #endif diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.cpp b/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.cpp index a950b0b..80dac85 100644 --- a/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.cpp +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -18,6 +18,7 @@ #include #include +using namespace std; b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const { @@ -27,6 +28,11 @@ b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const return clone; } +int32 b2CircleShape::GetChildCount() const +{ + return 1; +} + bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const { b2Vec2 center = transform.position + b2Mul(transform.R, m_p); @@ -38,8 +44,11 @@ bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) con // From Section 3.1.2 // x = s + a * r // norm(x) = radius -bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const +bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const { + B2_NOT_USED(childIndex); + b2Vec2 position = transform.position + b2Mul(transform.R, m_p); b2Vec2 s = input.p1 - position; float32 b = b2Dot(s, s) - m_radius * m_radius; @@ -72,8 +81,10 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input return false; } -void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const +void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const { + B2_NOT_USED(childIndex); + b2Vec2 p = transform.position + b2Mul(transform.R, m_p); aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.h b/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.h index bb31da8..6c1fd54 100644 --- a/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.h +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2CircleShape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -30,14 +30,18 @@ public: /// Implement b2Shape. b2Shape* Clone(b2BlockAllocator* allocator) const; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + /// Implement b2Shape. bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const; + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeMass void ComputeMass(b2MassData* massData, float32 density) const; diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.cpp b/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.cpp new file mode 100644 index 0000000..19ca893 --- /dev/null +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.cpp @@ -0,0 +1,139 @@ +/* +* Copyright (c) 2006-2010 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 +using namespace std; + +void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) +{ + m_vertex1 = v1; + m_vertex2 = v2; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2EdgeShape)); + b2EdgeShape* clone = new (mem) b2EdgeShape; + *clone = *this; + return clone; +} + +int32 b2EdgeShape::GetChildCount() const +{ + return 1; +} + +bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +// p = p1 + t * d +// v = v1 + s * e +// p1 + t * d = v1 + s * e +// s * e - t * d = p1 - v1 +bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position); + b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position); + b2Vec2 d = p2 - p1; + + b2Vec2 v1 = m_vertex1; + b2Vec2 v2 = m_vertex2; + b2Vec2 e = v2 - v1; + b2Vec2 normal(e.y, -e.x); + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float32 numerator = b2Dot(normal, v1 - p1); + float32 denominator = b2Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float32 t = numerator / denominator; + if (t < 0.0f || 1.0f < t) + { + return false; + } + + b2Vec2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + b2Vec2 r = v2 - v1; + float32 rr = b2Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float32 s = b2Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output->fraction = t; + if (numerator > 0.0f) + { + output->normal = -normal; + } + else + { + output->normal = normal; + } + return true; +} + +void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 v1 = b2Mul(xf, m_vertex1); + b2Vec2 v2 = b2Mul(xf, m_vertex2); + + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center = 0.5f * (m_vertex1 + m_vertex2); + massData->I = 0.0f; +} diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.h b/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.h new file mode 100644 index 0000000..780eb2c --- /dev/null +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2EdgeShape.h @@ -0,0 +1,70 @@ +/* +* Copyright (c) 2006-2010 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_EDGE_SHAPE_H +#define B2_EDGE_SHAPE_H + +#include + +/// A line segment (edge) shape. These can be connected in chains or loops +/// to other edge shapes. The connectivity information is used to ensure +/// correct contact normals. +class b2EdgeShape : public b2Shape +{ +public: + b2EdgeShape(); + + /// Set this as an isolated edge. + void Set(const b2Vec2& v1, const b2Vec2& v2); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// These are the edge vertices + b2Vec2 m_vertex1, m_vertex2; + + /// Optional adjacent vertices. These are used for smooth collision. + b2Vec2 m_vertex0, m_vertex3; + bool m_hasVertex0, m_hasVertex3; +}; + +inline b2EdgeShape::b2EdgeShape() +{ + m_type = e_edge; + m_radius = b2_polygonRadius; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +#endif diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.cpp b/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.cpp new file mode 100644 index 0000000..7f7a216 --- /dev/null +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.cpp @@ -0,0 +1,130 @@ +/* +* Copyright (c) 2006-2010 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 +#include +using namespace std; + +b2LoopShape::~b2LoopShape() +{ + b2Free(m_vertices); + m_vertices = NULL; + m_count = 0; +} + +void b2LoopShape::Create(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(count >= 2); + m_count = count; + m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); +} + +b2Shape* b2LoopShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2LoopShape)); + b2LoopShape* clone = new (mem) b2LoopShape; + clone->Create(m_vertices, m_count); + return clone; +} + +int32 b2LoopShape::GetChildCount() const +{ + return m_count; +} + +void b2LoopShape::GetChildEdge(b2EdgeShape* edge, int32 index) const +{ + b2Assert(2 <= m_count); + b2Assert(0 <= index && index < m_count); + edge->m_type = b2Shape::e_edge; + edge->m_radius = m_radius; + edge->m_hasVertex0 = true; + edge->m_hasVertex3 = true; + + int32 i0 = index - 1 >= 0 ? index - 1 : m_count - 1; + int32 i1 = index; + int32 i2 = index + 1 < m_count ? index + 1 : 0; + int32 i3 = index + 2; + while (i3 >= m_count) + { + i3 -= m_count; + } + + edge->m_vertex0 = m_vertices[i0]; + edge->m_vertex1 = m_vertices[i1]; + edge->m_vertex2 = m_vertices[i2]; + edge->m_vertex3 = m_vertices[i3]; +} + +bool b2LoopShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +bool b2LoopShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + b2EdgeShape edgeShape; + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + edgeShape.m_vertex1 = m_vertices[i1]; + edgeShape.m_vertex2 = m_vertices[i2]; + + return edgeShape.RayCast(output, input, xf, 0); +} + +void b2LoopShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); + b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); + + aabb->lowerBound = b2Min(v1, v2); + aabb->upperBound = b2Max(v1, v2); +} + +void b2LoopShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center.SetZero(); + massData->I = 0.0f; +} diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.h b/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.h new file mode 100644 index 0000000..0dd7f1c --- /dev/null +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2LoopShape.h @@ -0,0 +1,96 @@ +/* +* Copyright (c) 2006-2010 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_LOOP_SHAPE_H +#define B2_LOOP_SHAPE_H + +#include + +class b2EdgeShape; + +/// A loop shape is a free form sequence of line segments that form a circular list. +/// The loop may cross upon itself, but this is not recommended for smooth collision. +/// The loop has double sided collision, so you can use inside and outside collision. +/// Therefore, you may use any winding order. +/// Since there may be many vertices, they are allocated using b2Alloc. +class b2LoopShape : public b2Shape +{ +public: + b2LoopShape(); + + /// The destructor frees the vertices using b2Free. + ~b2LoopShape(); + + /// Create the loop shape, copy all vertices. + void Create(const b2Vec2* vertices, int32 count); + + /// Implement b2Shape. Vertices are cloned using b2Alloc. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// Get a child edge. + void GetChildEdge(b2EdgeShape* edge, int32 index) const; + + /// This always return false. + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// Chains have zero mass. + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// Get the number of vertices. + int32 GetCount() const { return m_count; } + + /// Get the vertices (read-only). + const b2Vec2& GetVertex(int32 index) const + { + b2Assert(0 <= index && index < m_count); + return m_vertices[index]; + } + + /// Get the vertices (read-only). + const b2Vec2* GetVertices() const { return m_vertices; } + +protected: + + /// The vertices. Owned by this class. + b2Vec2* m_vertices; + + /// The vertex count. + int32 m_count; +}; + +inline b2LoopShape::b2LoopShape() +{ + m_type = e_loop; + m_radius = b2_polygonRadius; + m_vertices = NULL; + m_count = 0; +} + +#endif diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.cpp b/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.cpp index 429e647..a625aff 100644 --- a/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -66,30 +66,18 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa } } -void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2) +int32 b2PolygonShape::GetChildCount() const { - m_vertexCount = 2; - m_vertices[0] = v1; - m_vertices[1] = v2; - m_centroid = 0.5f * (v1 + v2); - m_normals[0] = b2Cross(v2 - v1, 1.0f); - m_normals[0].Normalize(); - m_normals[1] = -m_normals[0]; + return 1; } static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) { - b2Assert(count >= 2); + b2Assert(count >= 3); b2Vec2 c; c.Set(0.0f, 0.0f); float32 area = 0.0f; - if (count == 2) - { - c = 0.5f * (vs[0] + vs[1]); - return c; - } - // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). b2Vec2 pRef(0.0f, 0.0f); @@ -131,7 +119,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { - b2Assert(2 <= count && count <= b2_maxPolygonVertices); + b2Assert(3 <= count && count <= b2_maxPolygonVertices); m_vertexCount = count; // Copy vertices. @@ -198,131 +186,82 @@ bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const return true; } -bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const +bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const { + B2_NOT_USED(childIndex); + // Put the ray into the polygon's frame of reference. b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position); b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position); b2Vec2 d = p2 - p1; - if (m_vertexCount == 2) - { - b2Vec2 v1 = m_vertices[0]; - b2Vec2 v2 = m_vertices[1]; - b2Vec2 normal = m_normals[0]; - - // q = p1 + t * d - // dot(normal, q - v1) = 0 - // dot(normal, p1 - v1) + t * dot(normal, d) = 0 - float32 numerator = b2Dot(normal, v1 - p1); - float32 denominator = b2Dot(normal, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 t = numerator / denominator; - if (t < 0.0f || 1.0f < t) - { - return false; - } - - b2Vec2 q = p1 + t * d; + float32 lower = 0.0f, upper = input.maxFraction; - // q = v1 + s * r - // s = dot(q - v1, r) / dot(r, r) - b2Vec2 r = v2 - v1; - float32 rr = b2Dot(r, r); - if (rr == 0.0f) - { - return false; - } + int32 index = -1; - float32 s = b2Dot(q - v1, r) / rr; - if (s < 0.0f || 1.0f < s) - { - return false; - } + for (int32 i = 0; i < m_vertexCount; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float32 denominator = b2Dot(m_normals[i], d); - output->fraction = t; - if (numerator > 0.0f) - { - output->normal = -normal; + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return false; + } } else { - output->normal = normal; - } - return true; - } - else - { - float32 lower = 0.0f, upper = input.maxFraction; - - int32 index = -1; - - for (int32 i = 0; i < m_vertexCount; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); - float32 denominator = b2Dot(m_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return false; - } - } - else + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; } - - // The use of epsilon here causes the assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - //if (upper < lower - b2_epsilon) - if (upper < lower) + else if (denominator > 0.0f && numerator < upper * denominator) { - return false; + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; } } - b2Assert(0.0f <= lower && lower <= input.maxFraction); - - if (index >= 0) + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) { - output->fraction = lower; - output->normal = b2Mul(xf.R, m_normals[index]); - return true; + return false; } } + b2Assert(0.0f <= lower && lower <= input.maxFraction); + + if (index >= 0) + { + output->fraction = lower; + output->normal = b2Mul(xf.R, m_normals[index]); + return true; + } + return false; } -void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const +void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const { + B2_NOT_USED(childIndex); + b2Vec2 lower = b2Mul(xf, m_vertices[0]); b2Vec2 upper = lower; @@ -364,44 +303,30 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // // The rest of the derivation is handled by computer algebra. - b2Assert(m_vertexCount >= 2); - - // A line segment has zero mass. - if (m_vertexCount == 2) - { - massData->center = 0.5f * (m_vertices[0] + m_vertices[1]); - massData->mass = 0.0f; - massData->I = 0.0f; - return; - } + b2Assert(m_vertexCount >= 3); b2Vec2 center; center.Set(0.0f, 0.0f); float32 area = 0.0f; float32 I = 0.0f; - // pRef is the reference point for forming triangles. + // s is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). - b2Vec2 pRef(0.0f, 0.0f); -#if 0 + b2Vec2 s(0.0f, 0.0f); + // This code would put the reference point inside the polygon. for (int32 i = 0; i < m_vertexCount; ++i) { - pRef += m_vertices[i]; + s += m_vertices[i]; } - pRef *= 1.0f / count; -#endif + s *= 1.0f / m_vertexCount; const float32 k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_vertexCount; ++i) { // Triangle vertices. - b2Vec2 p1 = pRef; - b2Vec2 p2 = m_vertices[i]; - b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0]; - - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p1; + b2Vec2 e1 = m_vertices[i] - s; + b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s; float32 D = b2Cross(e1, e2); @@ -409,16 +334,15 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const area += triangleArea; // Area weighted centroid - center += triangleArea * k_inv3 * (p1 + p2 + p3); + center += triangleArea * k_inv3 * (e1 + e2); - float32 px = p1.x, py = p1.y; float32 ex1 = e1.x, ey1 = e1.y; float32 ex2 = e2.x, ey2 = e2.y; - float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px; - float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py; + float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; - I += D * (intx2 + inty2); + I += (0.25f * k_inv3 * D) * (intx2 + inty2); } // Total mass @@ -427,8 +351,8 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const // Center of mass b2Assert(area > b2_epsilon); center *= 1.0f / area; - massData->center = center; + massData->center = center + s; // Inertia tensor relative to the local origin. - massData->I = density * I; + massData->I = density * I + massData->mass * b2Dot(s, s); } diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.h b/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.h index 564d4b0..fd11bd1 100644 --- a/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.h +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2PolygonShape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -23,6 +23,8 @@ /// A convex polygon. It is assumed that the interior of the polygon is to /// the left of each edge. +/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. +/// In most cases you should not need many vertices for a convex polygon. class b2PolygonShape : public b2Shape { public: @@ -31,8 +33,12 @@ public: /// Implement b2Shape. b2Shape* Clone(b2BlockAllocator* allocator) const; + /// @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. + /// The count must be in the range [3, b2_maxPolygonVertices]. void Set(const b2Vec2* vertices, int32 vertexCount); /// Build vertices to represent an axis-aligned box. @@ -47,27 +53,19 @@ public: /// @param angle the rotation of the box in local coordinates. void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle); - /// Set this as a single edge. - void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2); - /// @see b2Shape::TestPoint bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const; + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; /// @see b2Shape::ComputeMass void ComputeMass(b2MassData* massData, float32 density) const; - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; - - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; - /// Get the vertex count. int32 GetVertexCount() const { return m_vertexCount; } @@ -88,40 +86,6 @@ inline b2PolygonShape::b2PolygonShape() m_centroid.SetZero(); } -inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_vertexCount; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; -} - -inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_vertexCount; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; -} - inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const { b2Assert(0 <= index && index < m_vertexCount); diff --git a/libs/box2d/src/Box2D/Collision/Shapes/b2Shape.h b/libs/box2d/src/Box2D/Collision/Shapes/b2Shape.h index 9082c0e..34656dd 100644 --- a/libs/box2d/src/Box2D/Collision/Shapes/b2Shape.h +++ b/libs/box2d/src/Box2D/Collision/Shapes/b2Shape.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -38,7 +38,7 @@ struct b2MassData /// A shape is used for collision detection. You can create a shape however you like. /// Shapes used for simulation in b2World are created automatically when a b2Fixture -/// is created. +/// is created. Shapes may encapsulate a one or more child shapes. class b2Shape { public: @@ -47,8 +47,10 @@ public: { e_unknown= -1, e_circle = 0, - e_polygon = 1, - e_typeCount = 2, + e_edge = 1, + e_polygon = 2, + e_loop = 3, + e_typeCount = 4 }; b2Shape() { m_type = e_unknown; } @@ -61,21 +63,27 @@ public: /// @return the shape type. Type GetType() const; + /// Get the number of child primitives. + virtual int32 GetChildCount() const = 0; + /// Test a point for containment in this shape. This only works for convex shapes. /// @param xf the shape world transform. /// @param p a point in world coordinates. virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; - /// Cast a ray against this shape. + /// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. - virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0; + /// @param childIndex the child shape index + virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const = 0; - /// Given a transform, compute the associated axis aligned bounding box for this shape. + /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. - virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) const = 0; + /// @param childIndex the child shape + virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; /// Compute the mass properties of this shape using its dimensions and density. /// The inertia tensor is computed about the local origin. diff --git a/libs/box2d/src/Box2D/Collision/b2BroadPhase.cpp b/libs/box2d/src/Box2D/Collision/b2BroadPhase.cpp index 12c7967..2aa62f9 100644 --- a/libs/box2d/src/Box2D/Collision/b2BroadPhase.cpp +++ b/libs/box2d/src/Box2D/Collision/b2BroadPhase.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -18,6 +18,7 @@ #include #include +using namespace std; b2BroadPhase::b2BroadPhase() { @@ -62,6 +63,11 @@ void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& di } } +void b2BroadPhase::TouchProxy(int32 proxyId) +{ + BufferMove(proxyId); +} + void b2BroadPhase::BufferMove(int32 proxyId) { if (m_moveCount == m_moveCapacity) diff --git a/libs/box2d/src/Box2D/Collision/b2BroadPhase.h b/libs/box2d/src/Box2D/Collision/b2BroadPhase.h index bff188e..c7398c9 100644 --- a/libs/box2d/src/Box2D/Collision/b2BroadPhase.h +++ b/libs/box2d/src/Box2D/Collision/b2BroadPhase.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -40,7 +40,7 @@ public: enum { - e_nullProxy = -1, + e_nullProxy = -1 }; b2BroadPhase(); @@ -57,6 +57,9 @@ public: /// call UpdatePairs to finalized the proxy pairs (for your time step). void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); + /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. + void TouchProxy(int32 proxyId); + /// Get the fat AABB for a proxy. const b2AABB& GetFatAABB(int32 proxyId) const; @@ -88,8 +91,14 @@ public: template void RayCast(T* callback, const b2RayCastInput& input) const; - /// Compute the height of the embedded tree. - int32 ComputeHeight() const; + /// Get the height of the embedded tree. + int32 GetTreeHeight() const; + + /// Get the balance of the embedded tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the embedded tree. + float32 GetTreeQuality() const; private: @@ -153,9 +162,19 @@ inline int32 b2BroadPhase::GetProxyCount() const return m_proxyCount; } -inline int32 b2BroadPhase::ComputeHeight() const +inline int32 b2BroadPhase::GetTreeHeight() const +{ + return m_tree.GetHeight(); +} + +inline int32 b2BroadPhase::GetTreeBalance() const +{ + return m_tree.GetMaxBalance(); +} + +inline float32 b2BroadPhase::GetTreeQuality() const { - return m_tree.ComputeHeight(); + return m_tree.GetAreaRatio(); } template @@ -211,7 +230,7 @@ void b2BroadPhase::UpdatePairs(T* callback) } // Try to keep the tree balanced. - m_tree.Rebalance(4); + //m_tree.Rebalance(4); } template diff --git a/libs/box2d/src/Box2D/Collision/b2CollideCircle.cpp b/libs/box2d/src/Box2D/Collision/b2CollideCircle.cpp index 6edf89d..0ad58f0 100644 --- a/libs/box2d/src/Box2D/Collision/b2CollideCircle.cpp +++ b/libs/box2d/src/Box2D/Collision/b2CollideCircle.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 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 diff --git a/libs/box2d/src/Box2D/Collision/b2CollideEdge.cpp b/libs/box2d/src/Box2D/Collision/b2CollideEdge.cpp new file mode 100644 index 0000000..e2ded85 --- /dev/null +++ b/libs/box2d/src/Box2D/Collision/b2CollideEdge.cpp @@ -0,0 +1,673 @@ +/* +* Copyright (c) 2007-2009 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 +#include + +enum b2EdgeType +{ + b2_isolated, + b2_concave, + b2_flat, + b2_convex +}; + +// Compute contact points for edge versus circle. +// This accounts for edge connectivity. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle in frame of edge + b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); + + b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; + b2Vec2 e = B - A; + + // Barycentric coordinates + float32 u = b2Dot(e, B - Q); + float32 v = b2Dot(e, Q - A); + + float32 radius = edgeA->m_radius + circleB->m_radius; + + b2ContactFeature cf; + cf.indexB = 0; + cf.typeB = b2ContactFeature::e_vertex; + + // Region A + if (v <= 0.0f) + { + b2Vec2 P = A; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA->m_hasVertex0) + { + b2Vec2 A1 = edgeA->m_vertex0; + b2Vec2 B1 = A; + b2Vec2 e1 = B1 - A1; + float32 u1 = b2Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region B + if (u <= 0.0f) + { + b2Vec2 P = B; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA->m_hasVertex3) + { + b2Vec2 B2 = edgeA->m_vertex3; + b2Vec2 A2 = B; + b2Vec2 e2 = B2 - A2; + float32 v2 = b2Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.indexA = 1; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region AB + float32 den = b2Dot(e, e); + b2Assert(den > 0.0f); + b2Vec2 P = (1.0f / den) * (u * A + v * B); + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + b2Vec2 n(-e.y, e.x); + if (b2Dot(n, Q - A) < 0.0f) + { + n.Set(-n.x, -n.y); + } + n.Normalize(); + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_face; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = n; + manifold->localPoint = A; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; +} + +struct b2EPAxis +{ + enum Type + { + e_unknown, + e_edgeA, + e_edgeB + }; + + Type type; + int32 index; + float32 separation; +}; + +// Edge shape plus more stuff. +struct b2FatEdge +{ + b2Vec2 v0, v1, v2, v3; + b2Vec2 normal; + bool hasVertex0, hasVertex3; +}; + +// This lets us treate and edge shape and a polygon in the same +// way in the SAT collider. +struct b2EPProxy +{ + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + b2Vec2 centroid; + int32 count; +}; + +// This class collides and edge and a polygon, taking into account edge adjacency. +struct b2EPCollider +{ + b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB_in, const b2Transform& xfB); + + void Collide(b2Manifold* manifold); + + void ComputeAdjacency(); + b2EPAxis ComputeEdgeSeparation(); + b2EPAxis ComputePolygonSeparation(); + void FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2); + + b2FatEdge m_edgeA; + + b2EPProxy m_proxyA, m_proxyB; + + b2Transform m_xf; + b2Vec2 m_normal0, m_normal2; + b2Vec2 m_limit11, m_limit12; + b2Vec2 m_limit21, m_limit22; + float32 m_radius; +}; + +b2EPCollider::b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + m_xf = b2MulT(xfA, xfB); + + // Edge geometry + m_edgeA.v0 = edgeA->m_vertex0; + m_edgeA.v1 = edgeA->m_vertex1; + m_edgeA.v2 = edgeA->m_vertex2; + m_edgeA.v3 = edgeA->m_vertex3; + b2Vec2 e = m_edgeA.v2 - m_edgeA.v1; + + // Normal points outwards in CCW order. + m_edgeA.normal.Set(e.y, -e.x); + m_edgeA.normal.Normalize(); + m_edgeA.hasVertex0 = edgeA->m_hasVertex0; + m_edgeA.hasVertex3 = edgeA->m_hasVertex3; + + // Proxy for edge + m_proxyA.vertices[0] = m_edgeA.v1; + m_proxyA.vertices[1] = m_edgeA.v2; + m_proxyA.normals[0] = m_edgeA.normal; + m_proxyA.normals[1] = -m_edgeA.normal; + m_proxyA.centroid = 0.5f * (m_edgeA.v1 + m_edgeA.v2); + m_proxyA.count = 2; + + // Proxy for polygon + m_proxyB.count = polygonB->m_vertexCount; + m_proxyB.centroid = b2Mul(m_xf, polygonB->m_centroid); + for (int32 i = 0; i < polygonB->m_vertexCount; ++i) + { + m_proxyB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); + m_proxyB.normals[i] = b2Mul(m_xf.R, polygonB->m_normals[i]); + } + + m_radius = 2.0f * b2_polygonRadius; + + m_limit11.SetZero(); + m_limit12.SetZero(); + m_limit21.SetZero(); + m_limit22.SetZero(); +} + +// Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points. +// Edge adjacency is handle to produce locally valid contact points and normals. This is intended +// to allow the polygon to slide smoothly over an edge chain. +// +// Algorithm +// 1. Classify front-side or back-side collision with edge. +// 2. Compute separation +// 3. Process adjacent edges +// 4. Classify adjacent edge as convex, flat, null, or concave +// 5. Skip null or concave edges. Concave edges get a separate manifold. +// 6. If the edge is flat, compute contact points as normal. Discard boundary points. +// 7. If the edge is convex, compute it's separation. +// 8. Use the minimum separation of up to three edges. If the minimum separation +// is not the primary edge, return. +// 9. If the minimum separation is the primary edge, compute the contact points and return. +void b2EPCollider::Collide(b2Manifold* manifold) +{ + manifold->pointCount = 0; + + ComputeAdjacency(); + + b2EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + // This can happen on the middle edge of a 3-edge zig-zag chain. + if (edgeAxis.type == b2EPAxis::e_unknown) + { + return; + } + + if (edgeAxis.separation > m_radius) + { + return; + } + + b2EPAxis polygonAxis = ComputePolygonSeparation(); + if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float32 k_relativeTol = 0.98f; + const float32 k_absoluteTol = 0.001f; + + b2EPAxis primaryAxis; + if (polygonAxis.type == b2EPAxis::e_unknown) + { + primaryAxis = edgeAxis; + } + else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + b2EPProxy* proxy1; + b2EPProxy* proxy2; + b2ClipVertex incidentEdge[2]; + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + proxy1 = &m_proxyA; + proxy2 = &m_proxyB; + manifold->type = b2Manifold::e_faceA; + } + else + { + proxy1 = &m_proxyB; + proxy2 = &m_proxyA; + manifold->type = b2Manifold::e_faceB; + } + + int32 edge1 = primaryAxis.index; + + FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2); + int32 count1 = proxy1->count; + const b2Vec2* vertices1 = proxy1->vertices; + + int32 iv1 = edge1; + int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + b2Vec2 v11 = vertices1[iv1]; + b2Vec2 v12 = vertices1[iv2]; + + b2Vec2 tangent = v12 - v11; + tangent.Normalize(); + + b2Vec2 normal = b2Cross(tangent, 1.0f); + b2Vec2 planePoint = 0.5f * (v11 + v12); + + // Face offset. + float32 frontOffset = b2Dot(normal, v11); + + // Side offsets, extended by polytope skin thickness. + float32 sideOffset1 = -b2Dot(tangent, v11) + m_radius; + float32 sideOffset2 = b2Dot(tangent, v12) + m_radius; + + // Clip incident edge against extruded edge1 side edges. + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int np; + + // Clip to box side 1 + np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Clip to negative box side 1 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->localNormal = normal; + manifold->localPoint = planePoint; + } + else + { + manifold->localNormal = b2MulT(m_xf.R, normal); + manifold->localPoint = b2MulT(m_xf, planePoint); + } + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + float32 separation; + + separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; + + if (separation <= m_radius) + { + b2ManifoldPoint* cp = manifold->points + pointCount; + + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + } + else + { + cp->localPoint = clipPoints2[i].v; + cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; + cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; + cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; + cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; + } + + ++pointCount; + } + } + + manifold->pointCount = pointCount; +} + +// Compute allowable normal ranges based on adjacency. +// A normal n is allowable iff: +// cross(n, n1) >= 0.0f && cross(n2, n) >= 0.0f +// n points from A to B (edge to polygon) +void b2EPCollider::ComputeAdjacency() +{ + b2Vec2 v0 = m_edgeA.v0; + b2Vec2 v1 = m_edgeA.v1; + b2Vec2 v2 = m_edgeA.v2; + b2Vec2 v3 = m_edgeA.v3; + + // Determine allowable the normal regions based on adjacency. + // Note: it may be possible that no normal is admissable. + b2Vec2 centerB = m_proxyB.centroid; + if (m_edgeA.hasVertex0) + { + b2Vec2 e0 = v1 - v0; + b2Vec2 e1 = v2 - v1; + b2Vec2 n0(e0.y, -e0.x); + b2Vec2 n1(e1.y, -e1.x); + n0.Normalize(); + n1.Normalize(); + + bool convex = b2Cross(n0, n1) >= 0.0f; + bool front0 = b2Dot(n0, centerB - v0) >= 0.0f; + bool front1 = b2Dot(n1, centerB - v1) >= 0.0f; + + if (convex) + { + if (front0 || front1) + { + m_limit11 = n1; + m_limit12 = n0; + } + else + { + m_limit11 = -n1; + m_limit12 = -n0; + } + } + else + { + if (front0 && front1) + { + m_limit11 = n0; + m_limit12 = n1; + } + else + { + m_limit11 = -n0; + m_limit12 = -n1; + } + } + } + else + { + m_limit11.SetZero(); + m_limit12.SetZero(); + } + + if (m_edgeA.hasVertex3) + { + b2Vec2 e1 = v2 - v1; + b2Vec2 e2 = v3 - v2; + b2Vec2 n1(e1.y, -e1.x); + b2Vec2 n2(e2.y, -e2.x); + n1.Normalize(); + n2.Normalize(); + + bool convex = b2Cross(n1, n2) >= 0.0f; + bool front1 = b2Dot(n1, centerB - v1) >= 0.0f; + bool front2 = b2Dot(n2, centerB - v2) >= 0.0f; + + if (convex) + { + if (front1 || front2) + { + m_limit21 = n2; + m_limit22 = n1; + } + else + { + m_limit21 = -n2; + m_limit22 = -n1; + } + } + else + { + if (front1 && front2) + { + m_limit21 = n1; + m_limit22 = n2; + } + else + { + m_limit21 = -n1; + m_limit22 = -n2; + } + } + } + else + { + m_limit21.SetZero(); + m_limit22.SetZero(); + } +} + +b2EPAxis b2EPCollider::ComputeEdgeSeparation() +{ + // EdgeA separation + b2EPAxis bestAxis; + bestAxis.type = b2EPAxis::e_unknown; + bestAxis.index = -1; + bestAxis.separation = -FLT_MAX; + b2Vec2 normals[2] = {m_edgeA.normal, -m_edgeA.normal}; + + for (int32 i = 0; i < 2; ++i) + { + b2Vec2 n = normals[i]; + + // Adjacency + bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop; + bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop; + + if (valid1 == false || valid2 == false) + { + continue; + } + + b2EPAxis axis; + axis.type = b2EPAxis::e_edgeA; + axis.index = i; + axis.separation = FLT_MAX; + + for (int32 j = 0; j < m_proxyB.count; ++j) + { + float32 s = b2Dot(n, m_proxyB.vertices[j] - m_edgeA.v1); + if (s < axis.separation) + { + axis.separation = s; + } + } + + if (axis.separation > m_radius) + { + return axis; + } + + if (axis.separation > bestAxis.separation) + { + bestAxis = axis; + } + } + + return bestAxis; +} + +b2EPAxis b2EPCollider::ComputePolygonSeparation() +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_unknown; + axis.index = -1; + axis.separation = -FLT_MAX; + for (int32 i = 0; i < m_proxyB.count; ++i) + { + b2Vec2 n = -m_proxyB.normals[i]; + + // Adjacency + bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop; + bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop; + + if (valid1 == false && valid2 == false) + { + continue; + } + + float32 s1 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v1); + float32 s2 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v2); + float32 s = b2Min(s1, s2); + + if (s > m_radius) + { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + } + + if (s > axis.separation) + { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + } + } + + return axis; +} + +void b2EPCollider::FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2) +{ + int32 count1 = proxy1->count; + const b2Vec2* normals1 = proxy1->normals; + + int32 count2 = proxy2->count; + const b2Vec2* vertices2 = proxy2->vertices; + const b2Vec2* normals2 = proxy2->normals; + + b2Assert(0 <= edge1 && edge1 < count1); + + // Get the normal of the reference edge in proxy2's frame. + b2Vec2 normal1 = normals1[edge1]; + + // Find the incident edge on proxy2. + int32 index = 0; + float32 minDot = b2_maxFloat; + for (int32 i = 0; i < count2; ++i) + { + float32 dot = b2Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int32 i1 = index; + int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + c[0].v = vertices2[i1]; + c[0].id.cf.indexA = (uint8)edge1; + c[0].id.cf.indexB = (uint8)i1; + c[0].id.cf.typeA = b2ContactFeature::e_face; + c[0].id.cf.typeB = b2ContactFeature::e_vertex; + + c[1].v = vertices2[i2]; + c[1].id.cf.indexA = (uint8)edge1; + c[1].id.cf.indexB = (uint8)i2; + c[1].id.cf.typeA = b2ContactFeature::e_face; + c[1].id.cf.typeB = b2ContactFeature::e_vertex; +} + +void b2CollideEdgeAndPolygon( b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + b2EPCollider collider(edgeA, xfA, polygonB, xfB); + collider.Collide(manifold); +} diff --git a/libs/box2d/src/Box2D/Collision/b2CollidePolygon.cpp b/libs/box2d/src/Box2D/Collision/b2CollidePolygon.cpp index b37b7ba..bc53465 100644 --- a/libs/box2d/src/Box2D/Collision/b2CollidePolygon.cpp +++ b/libs/box2d/src/Box2D/Collision/b2CollidePolygon.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -173,14 +173,16 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.features.referenceEdge = (uint8)edge1; - c[0].id.features.incidentEdge = (uint8)i1; - c[0].id.features.incidentVertex = 0; + c[0].id.cf.indexA = (uint8)edge1; + c[0].id.cf.indexB = (uint8)i1; + c[0].id.cf.typeA = b2ContactFeature::e_face; + c[0].id.cf.typeB = b2ContactFeature::e_vertex; c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.features.referenceEdge = (uint8)edge1; - c[1].id.features.incidentEdge = (uint8)i2; - c[1].id.features.incidentVertex = 1; + c[1].id.cf.indexA = (uint8)edge1; + c[1].id.cf.indexB = (uint8)i2; + c[1].id.cf.typeA = b2ContactFeature::e_face; + c[1].id.cf.typeB = b2ContactFeature::e_vertex; } // Find edge normal of max separation on A - return if separating axis is found @@ -242,8 +244,11 @@ void b2CollidePolygons(b2Manifold* manifold, int32 count1 = poly1->m_vertexCount; const b2Vec2* vertices1 = poly1->m_vertices; - b2Vec2 v11 = vertices1[edge1]; - b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + int32 iv1 = edge1; + int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + b2Vec2 v11 = vertices1[iv1]; + b2Vec2 v12 = vertices1[iv2]; b2Vec2 localTangent = v12 - v11; localTangent.Normalize(); @@ -270,13 +275,13 @@ void b2CollidePolygons(b2Manifold* manifold, int np; // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1); + np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2); + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { @@ -297,7 +302,15 @@ void b2CollidePolygons(b2Manifold* manifold, b2ManifoldPoint* cp = manifold->points + pointCount; cp->localPoint = b2MulT(xf2, clipPoints2[i].v); cp->id = clipPoints2[i].id; - cp->id.features.flip = flip; + if (flip) + { + // Swap features + b2ContactFeature cf = cp->id.cf; + cp->id.cf.indexA = cf.indexB; + cp->id.cf.indexB = cf.indexA; + cp->id.cf.typeA = cf.typeB; + cp->id.cf.typeB = cf.typeA; + } ++pointCount; } } diff --git a/libs/box2d/src/Box2D/Collision/b2Collision.cpp b/libs/box2d/src/Box2D/Collision/b2Collision.cpp index a86c7c5..f317cb9 100644 --- a/libs/box2d/src/Box2D/Collision/b2Collision.cpp +++ b/libs/box2d/src/Box2D/Collision/b2Collision.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 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 @@ -196,7 +196,7 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const // Sutherland-Hodgman clipping. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset) + const b2Vec2& normal, float32 offset, int32 vertexIndexA) { // Start with no output points int32 numOut = 0; @@ -215,26 +215,25 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], // Find intersection point of edge and plane float32 interp = distance0 / (distance0 - distance1); vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - if (distance0 > 0.0f) - { - vOut[numOut].id = vIn[0].id; - } - else - { - vOut[numOut].id = vIn[1].id; - } + + // VertexA is hitting edgeB. + vOut[numOut].id.cf.indexA = 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; ++numOut; } return numOut; } -bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB, - const b2Transform& xfA, const b2Transform& xfB) +bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB) { b2DistanceInput input; - input.proxyA.Set(shapeA); - input.proxyB.Set(shapeB); + input.proxyA.Set(shapeA, indexA); + input.proxyB.Set(shapeB, indexB); input.transformA = xfA; input.transformB = xfB; input.useRadii = true; diff --git a/libs/box2d/src/Box2D/Collision/b2Collision.h b/libs/box2d/src/Box2D/Collision/b2Collision.h index baffdbd..2bc110e 100644 --- a/libs/box2d/src/Box2D/Collision/b2Collision.h +++ b/libs/box2d/src/Box2D/Collision/b2Collision.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -28,21 +28,31 @@ class b2Shape; class b2CircleShape; +class b2EdgeShape; class b2PolygonShape; const uint8 b2_nullFeature = UCHAR_MAX; +/// The features that intersect to form the contact point +/// This must be 4 bytes or less. +struct b2ContactFeature +{ + enum Type + { + e_vertex = 0, + e_face = 1 + }; + + uint8 indexA; ///< Feature index on shapeA + uint8 indexB; ///< Feature index on shapeB + uint8 typeA; ///< The feature type on shapeA + uint8 typeB; ///< The feature type on shapeB +}; + /// Contact ids to facilitate warm starting. union b2ContactID { - /// The features that intersect to form the contact point - struct Features - { - uint8 referenceEdge; ///< The edge that defines the outward contact normal. - uint8 incidentEdge; ///< The edge most anti-parallel to the reference edge. - uint8 incidentVertex; ///< The vertex (0 or 1) on the incident edge that was clipped. - uint8 flip; ///< A value of 1 indicates that the reference edge is on shape2. - } features; + b2ContactFeature cf; uint32 key; ///< Used to quickly compare contact ids. }; @@ -165,6 +175,21 @@ struct b2AABB return 0.5f * (upperBound - lowerBound); } + /// Get the perimeter length + float32 GetPerimeter() const + { + float32 wx = upperBound.x - lowerBound.x; + float32 wy = upperBound.y - lowerBound.y; + return 2.0f * (wx + wy); + } + + /// Combine an AABB into this one. + void Combine(const b2AABB& aabb) + { + lowerBound = b2Min(lowerBound, aabb.lowerBound); + upperBound = b2Max(upperBound, aabb.upperBound); + } + /// Combine two AABBs into this one. void Combine(const b2AABB& aabb1, const b2AABB& aabb2) { @@ -191,26 +216,37 @@ struct b2AABB /// Compute the collision manifold between two circles. void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circle1, const b2Transform& xf1, - const b2CircleShape* circle2, const b2Transform& xf2); + const b2CircleShape* circleA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); /// Compute the collision manifold between a polygon and a circle. void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygon, const b2Transform& xf1, - const b2CircleShape* circle, const b2Transform& xf2); + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); /// Compute the collision manifold between two polygons. void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygon1, const b2Transform& xf1, - const b2PolygonShape* polygon2, const b2Transform& xf2); + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a circle. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a circle. +void b2CollideEdgeAndPolygon(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* circleB, const b2Transform& xfB); /// Clipping for contact manifolds. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset); + const b2Vec2& normal, float32 offset, int32 vertexIndexA); /// Determine if two generic shapes overlap. -bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB, - const b2Transform& xfA, const b2Transform& xfB); +bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB); // ---------------- Inline Functions ------------------------------------------ diff --git a/libs/box2d/src/Box2D/Collision/b2Distance.cpp b/libs/box2d/src/Box2D/Collision/b2Distance.cpp index f95c82f..39567dd 100644 --- a/libs/box2d/src/Box2D/Collision/b2Distance.cpp +++ b/libs/box2d/src/Box2D/Collision/b2Distance.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 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 @@ -18,12 +18,14 @@ #include #include +#include +#include #include // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; -void b2DistanceProxy::Set(const b2Shape* shape) +void b2DistanceProxy::Set(const b2Shape* shape, int32 index) { switch (shape->GetType()) { @@ -45,6 +47,36 @@ void b2DistanceProxy::Set(const b2Shape* shape) } break; + case b2Shape::e_loop: + { + const b2LoopShape* loop = (b2LoopShape*)shape; + b2Assert(0 <= index && index < loop->GetCount()); + + m_buffer[0] = loop->GetVertex(index); + if (index + 1 < loop->GetCount()) + { + m_buffer[1] = loop->GetVertex(index + 1); + } + else + { + m_buffer[1] = loop->GetVertex(0); + } + + m_vertices = m_buffer; + m_count = 2; + m_radius = loop->m_radius; + } + break; + + case b2Shape::e_edge: + { + const b2EdgeShape* edge = (b2EdgeShape*)shape; + m_vertices = &edge->m_vertex1; + m_count = 2; + m_radius = edge->m_radius; + } + break; + default: b2Assert(false); } diff --git a/libs/box2d/src/Box2D/Collision/b2Distance.h b/libs/box2d/src/Box2D/Collision/b2Distance.h index e56ea0a..54ed1e1 100644 --- a/libs/box2d/src/Box2D/Collision/b2Distance.h +++ b/libs/box2d/src/Box2D/Collision/b2Distance.h @@ -1,6 +1,6 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -21,7 +21,6 @@ #define B2_DISTANCE_H #include -#include class b2Shape; @@ -33,7 +32,7 @@ struct b2DistanceProxy /// Initialize the proxy using the given shape. The shape /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape); + void Set(const b2Shape* shape, int32 index); /// Get the supporting vertex index in the given direction. int32 GetSupport(const b2Vec2& d) const; @@ -47,6 +46,7 @@ struct b2DistanceProxy /// Get a vertex by index. Used by b2Distance. const b2Vec2& GetVertex(int32 index) const; + b2Vec2 m_buffer[2]; const b2Vec2* m_vertices; int32 m_count; float32 m_radius; diff --git a/libs/box2d/src/Box2D/Collision/b2DynamicTree.cpp b/libs/box2d/src/Box2D/Collision/b2DynamicTree.cpp index d8a05eb..f43ea1e 100644 --- a/libs/box2d/src/Box2D/Collision/b2DynamicTree.cpp +++ b/libs/box2d/src/Box2D/Collision/b2DynamicTree.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2009 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 @@ -19,6 +19,10 @@ #include #include #include +using namespace std; + + +#if B2_USE_DYNAMIC_TREE b2DynamicTree::b2DynamicTree() { @@ -26,15 +30,17 @@ b2DynamicTree::b2DynamicTree() m_nodeCapacity = 16; m_nodeCount = 0; - m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode)); + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); // Build a linked list for the free list. for (int32 i = 0; i < m_nodeCapacity - 1; ++i) { m_nodes[i].next = i + 1; + m_nodes[i].height = -1; } m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; m_freeList = 0; m_path = 0; @@ -57,10 +63,10 @@ int32 b2DynamicTree::AllocateNode() b2Assert(m_nodeCount == m_nodeCapacity); // The free list is empty. Rebuild a bigger pool. - b2DynamicTreeNode* oldNodes = m_nodes; + b2TreeNode* oldNodes = m_nodes; m_nodeCapacity *= 2; - m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode)); + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); b2Free(oldNodes); // Build a linked list for the free list. The parent @@ -68,8 +74,10 @@ int32 b2DynamicTree::AllocateNode() for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) { m_nodes[i].next = i + 1; + m_nodes[i].height = -1; } m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; m_freeList = m_nodeCount; } @@ -79,6 +87,8 @@ int32 b2DynamicTree::AllocateNode() m_nodes[nodeId].parent = b2_nullNode; m_nodes[nodeId].child1 = b2_nullNode; m_nodes[nodeId].child2 = b2_nullNode; + m_nodes[nodeId].height = 0; + m_nodes[nodeId].userData = NULL; ++m_nodeCount; return nodeId; } @@ -89,6 +99,7 @@ void b2DynamicTree::FreeNode(int32 nodeId) b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); b2Assert(0 < m_nodeCount); m_nodes[nodeId].next = m_freeList; + m_nodes[nodeId].height = -1; m_freeList = nodeId; --m_nodeCount; } @@ -105,20 +116,10 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; m_nodes[proxyId].userData = userData; + m_nodes[proxyId].height = 0; InsertLeaf(proxyId); - // Rebalance if necessary. - int32 iterationCount = m_nodeCount >> 4; - int32 tryCount = 0; - int32 height = ComputeHeight(); - while (height > 64 && tryCount < 10) - { - Rebalance(iterationCount); - height = ComputeHeight(); - ++tryCount; - } - return proxyId; } @@ -188,79 +189,133 @@ void b2DynamicTree::InsertLeaf(int32 leaf) return; } - // Find the best sibling for this node. - b2Vec2 center = m_nodes[leaf].aabb.GetCenter(); - int32 sibling = m_root; - if (m_nodes[sibling].IsLeaf() == false) + // Find the best sibling for this node + b2AABB leafAABB = m_nodes[leaf].aabb; + int32 index = m_root; + while (m_nodes[index].IsLeaf() == false) { - do - { - int32 child1 = m_nodes[sibling].child1; - int32 child2 = m_nodes[sibling].child2; + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; - b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center); - b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center); + float32 area = m_nodes[index].aabb.GetPerimeter(); - float32 norm1 = delta1.x + delta1.y; - float32 norm2 = delta2.x + delta2.y; + b2AABB combinedAABB; + combinedAABB.Combine(m_nodes[index].aabb, leafAABB); + float32 combinedArea = combinedAABB.GetPerimeter(); - if (norm1 < norm2) - { - sibling = child1; - } - else - { - sibling = child2; - } + // Cost of creating a new parent for this node and the new leaf + float32 cost = 2.0f * combinedArea; - } - while(m_nodes[sibling].IsLeaf() == false); - } + // Minimum cost of pushing the leaf further down the tree + float32 inheritanceCost = 2.0f * (combinedArea - area); - // Create a parent for the siblings. - int32 node1 = m_nodes[sibling].parent; - int32 node2 = AllocateNode(); - m_nodes[node2].parent = node1; - m_nodes[node2].userData = NULL; - m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb); + // Cost of descending into child1 + float32 cost1; + if (m_nodes[child1].IsLeaf()) + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + cost1 = aabb.GetPerimeter() + inheritanceCost; + } + else + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost1 = (newArea - oldArea) + inheritanceCost; + } - if (node1 != b2_nullNode) - { - if (m_nodes[m_nodes[sibling].parent].child1 == sibling) + // Cost of descending into child2 + float32 cost2; + if (m_nodes[child2].IsLeaf()) { - m_nodes[node1].child1 = node2; + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + cost2 = aabb.GetPerimeter() + inheritanceCost; } else { - m_nodes[node1].child2 = node2; + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost2 = newArea - oldArea + inheritanceCost; } - m_nodes[node2].child1 = sibling; - m_nodes[node2].child2 = leaf; - m_nodes[sibling].parent = node2; - m_nodes[leaf].parent = node2; + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) + { + break; + } - do + // Descend + if (cost1 < cost2) { - if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb)) - { - break; - } + index = child1; + } + else + { + index = child2; + } + } + + int32 sibling = index; - m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb); - node2 = node1; - node1 = m_nodes[node1].parent; + // Create a new parent. + int32 oldParent = m_nodes[sibling].parent; + int32 newParent = AllocateNode(); + m_nodes[newParent].parent = oldParent; + m_nodes[newParent].userData = NULL; + m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); + m_nodes[newParent].height = m_nodes[sibling].height + 1; + + if (oldParent != b2_nullNode) + { + // The sibling was not the root. + if (m_nodes[oldParent].child1 == sibling) + { + m_nodes[oldParent].child1 = newParent; + } + else + { + m_nodes[oldParent].child2 = newParent; } - while(node1 != b2_nullNode); + + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; } else { - m_nodes[node2].child1 = sibling; - m_nodes[node2].child2 = leaf; - m_nodes[sibling].parent = node2; - m_nodes[leaf].parent = node2; - m_root = node2; + // The sibling was the root. + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + m_root = newParent; } + + // Walk back up the tree fixing heights and AABBs + index = m_nodes[leaf].parent; + while (index != b2_nullNode) + { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + b2Assert(child1 != b2_nullNode); + b2Assert(child2 != b2_nullNode); + + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + index = m_nodes[index].parent; + } + + //Validate(); } void b2DynamicTree::RemoveLeaf(int32 leaf) @@ -271,89 +326,250 @@ void b2DynamicTree::RemoveLeaf(int32 leaf) return; } - int32 node2 = m_nodes[leaf].parent; - int32 node1 = m_nodes[node2].parent; + int32 parent = m_nodes[leaf].parent; + int32 grandParent = m_nodes[parent].parent; int32 sibling; - if (m_nodes[node2].child1 == leaf) + if (m_nodes[parent].child1 == leaf) { - sibling = m_nodes[node2].child2; + sibling = m_nodes[parent].child2; } else { - sibling = m_nodes[node2].child1; + sibling = m_nodes[parent].child1; } - if (node1 != b2_nullNode) + if (grandParent != b2_nullNode) { - // Destroy node2 and connect node1 to sibling. - if (m_nodes[node1].child1 == node2) + // Destroy parent and connect sibling to grandParent. + if (m_nodes[grandParent].child1 == parent) { - m_nodes[node1].child1 = sibling; + m_nodes[grandParent].child1 = sibling; } else { - m_nodes[node1].child2 = sibling; + m_nodes[grandParent].child2 = sibling; } - m_nodes[sibling].parent = node1; - FreeNode(node2); + m_nodes[sibling].parent = grandParent; + FreeNode(parent); // Adjust ancestor bounds. - while (node1 != b2_nullNode) + int32 index = grandParent; + while (index != b2_nullNode) { - b2AABB oldAABB = m_nodes[node1].aabb; - m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb); + index = Balance(index); - if (oldAABB.Contains(m_nodes[node1].aabb)) - { - break; - } + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; - node1 = m_nodes[node1].parent; + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + + index = m_nodes[index].parent; } } else { m_root = sibling; m_nodes[sibling].parent = b2_nullNode; - FreeNode(node2); + FreeNode(parent); + } + + //Validate(); +} + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +int32 b2DynamicTree::Balance(int32 iA) +{ + b2Assert(iA != b2_nullNode); + + b2TreeNode* A = m_nodes + iA; + if (A->IsLeaf() || A->height < 2) + { + return iA; + } + + int32 iB = A->child1; + int32 iC = A->child2; + b2Assert(0 <= iB && iB < m_nodeCapacity); + b2Assert(0 <= iC && iC < m_nodeCapacity); + + b2TreeNode* B = m_nodes + iB; + b2TreeNode* C = m_nodes + iC; + + int32 balance = C->height - B->height; + + // Rotate C up + if (balance > 1) + { + int32 iF = C->child1; + int32 iG = C->child2; + b2TreeNode* F = m_nodes + iF; + b2TreeNode* G = m_nodes + iG; + b2Assert(0 <= iF && iF < m_nodeCapacity); + b2Assert(0 <= iG && iG < m_nodeCapacity); + + // Swap A and C + C->child1 = iA; + C->parent = A->parent; + A->parent = iC; + + // A's old parent should point to C + if (C->parent != b2_nullNode) + { + if (m_nodes[C->parent].child1 == iA) + { + m_nodes[C->parent].child1 = iC; + } + else + { + b2Assert(m_nodes[C->parent].child2 == iA); + m_nodes[C->parent].child2 = iC; + } + } + else + { + m_root = iC; + } + + // Rotate + if (F->height > G->height) + { + C->child2 = iF; + A->child2 = iG; + G->parent = iA; + A->aabb.Combine(B->aabb, G->aabb); + C->aabb.Combine(A->aabb, F->aabb); + + A->height = 1 + b2Max(B->height, G->height); + C->height = 1 + b2Max(A->height, F->height); + } + else + { + C->child2 = iG; + A->child2 = iF; + F->parent = iA; + A->aabb.Combine(B->aabb, F->aabb); + C->aabb.Combine(A->aabb, G->aabb); + + A->height = 1 + b2Max(B->height, F->height); + C->height = 1 + b2Max(A->height, G->height); + } + + return iC; } + + // Rotate B up + if (balance < -1) + { + int32 iD = B->child1; + int32 iE = B->child2; + b2TreeNode* D = m_nodes + iD; + b2TreeNode* E = m_nodes + iE; + b2Assert(0 <= iD && iD < m_nodeCapacity); + b2Assert(0 <= iE && iE < m_nodeCapacity); + + // Swap A and B + B->child1 = iA; + B->parent = A->parent; + A->parent = iB; + + // A's old parent should point to B + if (B->parent != b2_nullNode) + { + if (m_nodes[B->parent].child1 == iA) + { + m_nodes[B->parent].child1 = iB; + } + else + { + b2Assert(m_nodes[B->parent].child2 == iA); + m_nodes[B->parent].child2 = iB; + } + } + else + { + m_root = iB; + } + + // Rotate + if (D->height > E->height) + { + B->child2 = iD; + A->child1 = iE; + E->parent = iA; + A->aabb.Combine(C->aabb, E->aabb); + B->aabb.Combine(A->aabb, D->aabb); + + A->height = 1 + b2Max(C->height, E->height); + B->height = 1 + b2Max(A->height, D->height); + } + else + { + B->child2 = iE; + A->child1 = iD; + D->parent = iA; + A->aabb.Combine(C->aabb, D->aabb); + B->aabb.Combine(A->aabb, E->aabb); + + A->height = 1 + b2Max(C->height, D->height); + B->height = 1 + b2Max(A->height, E->height); + } + + return iB; + } + + return iA; } -void b2DynamicTree::Rebalance(int32 iterations) +int32 b2DynamicTree::GetHeight() const { if (m_root == b2_nullNode) { - return; + return 0; } - for (int32 i = 0; i < iterations; ++i) + return m_nodes[m_root].height; +} + +// +float32 b2DynamicTree::GetAreaRatio() const +{ + if (m_root == b2_nullNode) { - int32 node = m_root; + return 0.0f; + } - uint32 bit = 0; - while (m_nodes[node].IsLeaf() == false) + const b2TreeNode* root = m_nodes + m_root; + float32 rootArea = root->aabb.GetPerimeter(); + + float32 totalArea = 0.0f; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height < 0) { - int32* children = &m_nodes[node].child1; - node = children[(m_path >> bit) & 1]; - bit = (bit + 1) & (8* sizeof(uint32) - 1); + // Free node in pool + continue; } - ++m_path; - RemoveLeaf(node); - InsertLeaf(node); + totalArea += node->aabb.GetPerimeter(); } + + return totalArea / rootArea; } // Compute the height of a sub-tree. int32 b2DynamicTree::ComputeHeight(int32 nodeId) const { - if (nodeId == b2_nullNode) + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2TreeNode* node = m_nodes + nodeId; + + if (node->IsLeaf()) { return 0; } - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2DynamicTreeNode* node = m_nodes + nodeId; int32 height1 = ComputeHeight(node->child1); int32 height2 = ComputeHeight(node->child2); return 1 + b2Max(height1, height2); @@ -361,5 +577,359 @@ int32 b2DynamicTree::ComputeHeight(int32 nodeId) const int32 b2DynamicTree::ComputeHeight() const { - return ComputeHeight(m_root); + int32 height = ComputeHeight(m_root); + return height; +} + +void b2DynamicTree::ValidateStructure(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + if (index == m_root) + { + b2Assert(m_nodes[index].parent == b2_nullNode); + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + b2Assert(m_nodes[child1].parent == index); + b2Assert(m_nodes[child2].parent == index); + + ValidateStructure(child1); + ValidateStructure(child2); +} + +void b2DynamicTree::ValidateMetrics(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + int32 height1 = m_nodes[child1].height; + int32 height2 = m_nodes[child2].height; + int32 height = 1 + b2Max(height1, height2); + b2Assert(node->height == height); + + b2AABB aabb; + aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + b2Assert(aabb.lowerBound == node->aabb.lowerBound); + b2Assert(aabb.upperBound == node->aabb.upperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); +} + +void b2DynamicTree::Validate() const +{ + ValidateStructure(m_root); + ValidateMetrics(m_root); + + int32 freeCount = 0; + int32 freeIndex = m_freeList; + while (freeIndex != b2_nullNode) + { + b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); + freeIndex = m_nodes[freeIndex].next; + ++freeCount; + } + + b2Assert(GetHeight() == ComputeHeight()); + + b2Assert(m_nodeCount + freeCount == m_nodeCapacity); +} + +int32 b2DynamicTree::GetMaxBalance() const +{ + int32 maxBalance = 0; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height <= 1) + { + continue; + } + + b2Assert(node->IsLeaf() == false); + + int32 child1 = node->child1; + int32 child2 = node->child2; + int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); + maxBalance = b2Max(maxBalance, balance); + } + + return maxBalance; +} + +void b2DynamicTree::RebuildBottomUp() +{ + int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); + int32 count = 0; + + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + if (m_nodes[i].height < 0) + { + // free node in pool + continue; + } + + if (m_nodes[i].IsLeaf()) + { + m_nodes[i].parent = b2_nullNode; + nodes[count] = i; + ++count; + } + else + { + FreeNode(i); + } + } + + while (count > 1) + { + float32 minCost = b2_maxFloat; + int32 iMin = -1, jMin = -1; + for (int32 i = 0; i < count; ++i) + { + b2AABB aabbi = m_nodes[nodes[i]].aabb; + + for (int32 j = i + 1; j < count; ++j) + { + b2AABB aabbj = m_nodes[nodes[j]].aabb; + b2AABB b; + b.Combine(aabbi, aabbj); + float32 cost = b.GetPerimeter(); + if (cost < minCost) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int32 index1 = nodes[iMin]; + int32 index2 = nodes[jMin]; + b2TreeNode* child1 = m_nodes + index1; + b2TreeNode* child2 = m_nodes + index2; + + int32 parentIndex = AllocateNode(); + b2TreeNode* parent = m_nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->height = 1 + b2Max(child1->height, child2->height); + parent->aabb.Combine(child1->aabb, child2->aabb); + parent->parent = b2_nullNode; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count-1]; + nodes[iMin] = parentIndex; + --count; + } + + m_root = nodes[0]; + b2Free(nodes); + + Validate(); +} + +#elif B2_USE_BRUTE_FORCE + +b2DynamicTree::b2DynamicTree() +{ + m_proxyCapacity = 128; + m_proxyCount = 0; + + m_proxyMap = (int32*)b2Alloc(m_proxyCapacity * sizeof(int32)); + m_proxies = (b2Proxy*)b2Alloc(m_proxyCapacity * sizeof(b2Proxy)); + + // Build the free list + m_freeId = 0; + int32 last = m_proxyCapacity - 1; + for (int32 i = m_freeId; i < last; ++i) + { + m_proxyMap[i] = i + 1; + } + + m_proxyMap[last] = b2_nullNode; +} + +b2DynamicTree::~b2DynamicTree() +{ + b2Free(m_proxyMap); + b2Free(m_proxies); +} + +int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) +{ + if (m_proxyCount == m_proxyCapacity) + { + m_proxyCapacity *= 2; + int32* proxyMap = (int32*)b2Alloc(m_proxyCapacity * sizeof(int32)); + b2Proxy* proxies = (b2Proxy*)b2Alloc(m_proxyCapacity * sizeof(b2Proxy)); + + memcpy(proxyMap, m_proxyMap, m_proxyCount * sizeof(int32)); + memcpy(proxies, m_proxies, m_proxyCount * sizeof(b2Proxy)); + + b2Free(m_proxyMap); + b2Free(m_proxies); + m_proxyMap = proxyMap; + m_proxies = proxies; + proxyMap = NULL; + proxies = NULL; + + m_freeId = m_proxyCount; + int32 last = m_proxyCapacity - 1; + for (int32 i = m_freeId; i < last; ++i) + { + m_proxyMap[i] = i + 1; + } + + m_proxyMap[last] = b2_nullNode; + } + + b2Assert(0 <= m_freeId && m_freeId < m_proxyCapacity); + int32 id = m_freeId; + m_freeId = m_proxyMap[id]; + int32 index = m_proxyCount; + + m_proxies[index].aabb = aabb; + m_proxies[index].userData = userData; + m_proxies[index].id = id; + m_proxyMap[id] = index; + ++m_proxyCount; + + return id; +} + +void b2DynamicTree::DestroyProxy(int32 proxyId) +{ + b2Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity); + int32 index = m_proxyMap[proxyId]; + + // Add to free list + m_proxyMap[proxyId] = m_freeId; + m_freeId = proxyId; + + // Keep proxy array contiguous + if (index < m_proxyCount - 1) + { + m_proxies[index] = m_proxies[m_proxyCount-1]; + int32 id = m_proxies[index].id; + m_proxyMap[id] = index; + } + + --m_proxyCount; + + Validate(); } + +bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) +{ + b2Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity); + B2_NOT_USED(displacement); + + int32 index = m_proxyMap[proxyId]; + + if (m_proxies[index].aabb.Contains(aabb)) + { + return false; + } + + // Extend AABB. + b2AABB b = aabb; + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + b.lowerBound = b.lowerBound - r; + b.upperBound = b.upperBound + r; + + // Predict AABB displacement. + b2Vec2 d = b2_aabbMultiplier * displacement; + + if (d.x < 0.0f) + { + b.lowerBound.x += d.x; + } + else + { + b.upperBound.x += d.x; + } + + if (d.y < 0.0f) + { + b.lowerBound.y += d.y; + } + else + { + b.upperBound.y += d.y; + } + + m_proxies[index].aabb = b; + + return true; +} + +void b2DynamicTree::Validate() const +{ + b2Assert(m_proxyCount > 0 || m_freeId == b2_nullNode); + b2Assert(m_freeId == b2_nullNode || m_freeId < m_proxyCapacity); + + int32 id = m_freeId; + int32 freeCount = 0; + while (id != b2_nullNode) + { + ++freeCount; + b2Assert(freeCount <= m_proxyCapacity); + id = m_proxyMap[id]; + } + + b2Assert(freeCount + m_proxyCount == m_proxyCapacity); + + b2Assert(m_proxyCount <= m_proxyCapacity); + + for (int32 i = 0; i < m_proxyCount; ++i) + { + int32 id = m_proxies[i].id; + + b2Assert(m_proxyMap[id] == i); + } +} + +#endif diff --git a/libs/box2d/src/Box2D/Collision/b2DynamicTree.h b/libs/box2d/src/Box2D/Collision/b2DynamicTree.h index b67686b..8afaa6d 100644 --- a/libs/box2d/src/Box2D/Collision/b2DynamicTree.h +++ b/libs/box2d/src/Box2D/Collision/b2DynamicTree.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2009 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 @@ -20,23 +20,26 @@ #define B2_DYNAMIC_TREE_H #include - -/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. +#include #define b2_nullNode (-1) +#define B2_USE_DYNAMIC_TREE 1 +#define B2_USE_BRUTE_FORCE 0 + +#if B2_USE_DYNAMIC_TREE + /// A node in the dynamic tree. The client does not interact with this directly. -struct b2DynamicTreeNode +struct b2TreeNode { bool IsLeaf() const { return child1 == b2_nullNode; } - /// This is the fattened AABB. + /// Enlarged AABB b2AABB aabb; - //int32 userData; void* userData; union @@ -47,8 +50,12 @@ struct b2DynamicTreeNode int32 child1; int32 child2; + + // leaf = 0, free node = -1 + int32 height; }; +/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. /// A dynamic tree arranges data in a binary tree to accelerate /// queries such as volume queries and ray casts. Leafs are proxies /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor @@ -59,7 +66,6 @@ struct b2DynamicTreeNode class b2DynamicTree { public: - /// Constructing the tree initializes the node pool. b2DynamicTree(); @@ -78,9 +84,6 @@ public: /// @return true if the proxy was re-inserted. bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - /// Perform some iterations to re-balance the tree. - void Rebalance(int32 iterations); - /// Get proxy user data. /// @return the proxy user data or 0 if the id is invalid. void* GetUserData(int32 proxyId) const; @@ -88,9 +91,6 @@ public: /// Get the fat AABB for a proxy. const b2AABB& GetFatAABB(int32 proxyId) const; - /// Compute the height of the tree. - int32 ComputeHeight() const; - /// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. template @@ -106,6 +106,23 @@ public: template void RayCast(T* callback, const b2RayCastInput& input) const; + /// Validate this tree. For testing. + void Validate() const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 GetHeight() const; + + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + int32 GetMaxBalance() const; + + /// Get the ratio of the sum of the node areas to the root area. + float32 GetAreaRatio() const; + + /// Build an optimal tree. Very expensive. For testing. + void RebuildBottomUp(); + private: int32 AllocateNode(); @@ -114,17 +131,23 @@ private: void InsertLeaf(int32 node); void RemoveLeaf(int32 node); + int32 Balance(int32 index); + + int32 ComputeHeight() const; int32 ComputeHeight(int32 nodeId) const; + void ValidateStructure(int32 index) const; + void ValidateMetrics(int32 index) const; + int32 m_root; - b2DynamicTreeNode* m_nodes; + b2TreeNode* m_nodes; int32 m_nodeCount; int32 m_nodeCapacity; int32 m_freeList; - /// This is used incrementally traverse the tree for re-balancing. + /// This is used to incrementally traverse the tree for re-balancing. uint32 m_path; int32 m_insertionCount; @@ -145,21 +168,18 @@ inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const template inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const { - const int32 k_stackSize = 128; - int32 stack[k_stackSize]; - - int32 count = 0; - stack[count++] = m_root; + b2GrowableStack stack; + stack.Push(m_root); - while (count > 0) + while (stack.GetCount() > 0) { - int32 nodeId = stack[--count]; + int32 nodeId = stack.Pop(); if (nodeId == b2_nullNode) { continue; } - const b2DynamicTreeNode* node = m_nodes + nodeId; + const b2TreeNode* node = m_nodes + nodeId; if (b2TestOverlap(node->aabb, aabb)) { @@ -173,15 +193,8 @@ inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const } else { - if (count < k_stackSize) - { - stack[count++] = node->child1; - } - - if (count < k_stackSize) - { - stack[count++] = node->child2; - } + stack.Push(node->child1); + stack.Push(node->child2); } } } @@ -213,21 +226,18 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con segmentAABB.upperBound = b2Max(p1, t); } - const int32 k_stackSize = 128; - int32 stack[k_stackSize]; + b2GrowableStack stack; + stack.Push(m_root); - int32 count = 0; - stack[count++] = m_root; - - while (count > 0) + while (stack.GetCount() > 0) { - int32 nodeId = stack[--count]; + int32 nodeId = stack.Pop(); if (nodeId == b2_nullNode) { continue; } - const b2DynamicTreeNode* node = m_nodes + nodeId; + const b2TreeNode* node = m_nodes + nodeId; if (b2TestOverlap(node->aabb, segmentAABB) == false) { @@ -270,17 +280,196 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con } else { - if (count < k_stackSize) - { - stack[count++] = node->child1; - } + stack.Push(node->child1); + stack.Push(node->child2); + } + } +} + +#elif B2_USE_BRUTE_FORCE 0 + +struct b2Proxy +{ + /// This is the fattened AABB. + b2AABB aabb; + void* userData; + int32 id; +}; + +/// This implementation is not a tree at all. It is just a cache friendly array of AABBs. +class b2DynamicTree +{ +public: + + /// Constructing the tree initializes the node pool. + b2DynamicTree(); + + /// Destroy the tree, freeing the node pool. + ~b2DynamicTree(); + + /// Create a proxy. Provide a tight fitting AABB and a userData pointer. + int32 CreateProxy(const b2AABB& aabb, void* userData); + + /// Destroy a proxy. This asserts if the id is invalid. + void DestroyProxy(int32 proxyId); + + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, + /// then the proxy is removed from the tree and re-inserted. Otherwise + /// the function returns immediately. + /// @return true if the proxy was re-inserted. + bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); + + /// Perform some iterations to re-balance the tree. + void Rebalance(int32 iterations) + { + B2_NOT_USED(iterations); + } + + /// Get proxy user data. + /// @return the proxy user data or 0 if the id is invalid. + void* GetUserData(int32 proxyId) const; + + /// Get the fat AABB for a proxy. + const b2AABB& GetFatAABB(int32 proxyId) const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 ComputeHeight() const + { + return 0; + } + + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T* callback, const b2AABB& aabb) const; + + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit by the ray. + template + void RayCast(T* callback, const b2RayCastInput& input) const; + + void Validate() const; + +private: - if (count < k_stackSize) + // Map of ids to proxies indices. This may have holes (which contain a free list). + int32* m_proxyMap; + + // Contiguous array of proxies + b2Proxy* m_proxies; + + int32 m_proxyCount; + int32 m_proxyCapacity; + + int32 m_freeId; +}; + +inline void* b2DynamicTree::GetUserData(int32 proxyId) const +{ + b2Assert(0 <= proxyId && proxyId < m_proxyCapacity); + int32 index = m_proxyMap[proxyId]; + return m_proxies[index].userData; +} + +inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const +{ + b2Assert(0 <= proxyId && proxyId < m_proxyCapacity); + int32 index = m_proxyMap[proxyId]; + return m_proxies[index].aabb; +} + +template +inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const +{ + for (int32 i = 0; i < m_proxyCount; ++i) + { + if (b2TestOverlap(m_proxies[i].aabb, aabb)) + { + bool proceed = callback->QueryCallback(m_proxies[i].id); + if (proceed == false) { - stack[count++] = node->child2; + return; } } } } +template +inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const +{ + b2Vec2 p1 = input.p1; + b2Vec2 p2 = input.p2; + b2Vec2 r = p2 - p1; + b2Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + b2Vec2 v = b2Cross(1.0f, r); + b2Vec2 abs_v = b2Abs(v); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float32 maxFraction = input.maxFraction; + + // Build a bounding box for the segment. + b2AABB segmentAABB; + { + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + + for (int32 i = 0; i < m_proxyCount; ++i) + { + const b2Proxy* proxy = m_proxies + i; + b2AABB proxyAABB = proxy->aabb; + + if (b2TestOverlap(proxyAABB, segmentAABB) == false) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + b2Vec2 c = proxyAABB.GetCenter(); + b2Vec2 h = proxyAABB.GetExtents(); + float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); + if (separation > 0.0f) + { + continue; + } + + b2RayCastInput subInput; + subInput.p1 = input.p1; + subInput.p2 = input.p2; + subInput.maxFraction = maxFraction; + + float32 value = callback->RayCastCallback(subInput, proxy->id); + + if (value == 0.0f) + { + // The client has terminated the ray cast. + return; + } + + if (value > 0.0f) + { + // Update segment bounding box. + maxFraction = value; + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + } +} + +#endif + #endif diff --git a/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.cpp b/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.cpp index b1f2f4e..2fef964 100644 --- a/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.cpp +++ b/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 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 @@ -23,12 +23,11 @@ #include #include +using namespace std; int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; int32 b2_toiRootIters, b2_toiMaxRootIters; -int32 b2_toiMaxOptIters; - struct b2SeparationFunction { enum Type @@ -42,7 +41,8 @@ struct b2SeparationFunction float32 Initialize(const b2SimplexCache* cache, const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB) + const b2DistanceProxy* proxyB, const b2Sweep& sweepB, + float32 t1) { m_proxyA = proxyA; m_proxyB = proxyB; @@ -53,8 +53,8 @@ struct b2SeparationFunction m_sweepB = sweepB; b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, 0.0f); - m_sweepB.GetTransform(&xfB, 0.0f); + m_sweepA.GetTransform(&xfA, t1); + m_sweepB.GetTransform(&xfB, t1); if (count == 1) { @@ -325,7 +325,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) // Initialize the separating axis. b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB); + fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); #if 0 // Dump the curve seen by the root finder { diff --git a/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.h b/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.h index b59fb83..179a170 100644 --- a/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.h +++ b/libs/box2d/src/Box2D/Collision/b2TimeOfImpact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -21,7 +21,6 @@ #include #include -#include /// Input parameters for b2TimeOfImpact struct b2TOIInput diff --git a/libs/box2d/src/Box2D/Common/b2BlockAllocator.cpp b/libs/box2d/src/Box2D/Common/b2BlockAllocator.cpp index ba06f04..f5060da 100644 --- a/libs/box2d/src/Box2D/Common/b2BlockAllocator.cpp +++ b/libs/box2d/src/Box2D/Common/b2BlockAllocator.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -21,6 +21,7 @@ #include #include #include +using namespace std; int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = { @@ -100,7 +101,12 @@ void* b2BlockAllocator::Allocate(int32 size) if (size == 0) return NULL; - b2Assert(0 < size && size <= b2_maxBlockSize); + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + return b2Alloc(size); + } int32 index = s_blockSizeLookup[size]; b2Assert(0 <= index && index < b2_blockSizes); @@ -155,7 +161,13 @@ void b2BlockAllocator::Free(void* p, int32 size) return; } - b2Assert(0 < size && size <= b2_maxBlockSize); + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + b2Free(p); + return; + } int32 index = s_blockSizeLookup[size]; b2Assert(0 <= index && index < b2_blockSizes); diff --git a/libs/box2d/src/Box2D/Common/b2BlockAllocator.h b/libs/box2d/src/Box2D/Common/b2BlockAllocator.h index 93eb2e3..8ba29a5 100644 --- a/libs/box2d/src/Box2D/Common/b2BlockAllocator.h +++ b/libs/box2d/src/Box2D/Common/b2BlockAllocator.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -21,7 +21,7 @@ #include -const int32 b2_chunkSize = 4096; +const int32 b2_chunkSize = 16 * 1024; const int32 b2_maxBlockSize = 640; const int32 b2_blockSizes = 14; const int32 b2_chunkArrayIncrement = 128; @@ -29,16 +29,19 @@ const int32 b2_chunkArrayIncrement = 128; struct b2Block; struct b2Chunk; -// This is a small object allocator used for allocating small -// objects that persist for more than one time step. -// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp +/// This is a small object allocator used for allocating small +/// objects that persist for more than one time step. +/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp class b2BlockAllocator { public: b2BlockAllocator(); ~b2BlockAllocator(); + /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. void* Allocate(int32 size); + + /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. void Free(void* p, int32 size); void Clear(); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Common/b2Draw.cpp similarity index 52% copy from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h copy to libs/box2d/src/Box2D/Common/b2Draw.cpp index c092e2a..327b580 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Common/b2Draw.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2011 Erin Catto http://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 @@ -16,36 +16,29 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#include -#include - -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; - -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +b2Draw::b2Draw() { -public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); + m_drawFlags = 0; +} - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); - - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); +void b2Draw::SetFlags(uint32 flags) +{ + m_drawFlags = flags; +} -private: +uint32 b2Draw::GetFlags() const +{ + return m_drawFlags; +} - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; -}; +void b2Draw::AppendFlags(uint32 flags) +{ + m_drawFlags |= flags; +} -#endif +void b2Draw::ClearFlags(uint32 flags) +{ + m_drawFlags &= ~flags; +} diff --git a/libs/box2d/src/Box2D/Common/b2Draw.h b/libs/box2d/src/Box2D/Common/b2Draw.h new file mode 100644 index 0000000..de62dd6 --- /dev/null +++ b/libs/box2d/src/Box2D/Common/b2Draw.h @@ -0,0 +1,81 @@ +/* +* Copyright (c) 2011 Erin Catto http://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 + +/// Color for debug drawing. Each value has the range [0,1]. +struct b2Color +{ + b2Color() {} + b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} + void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } + float32 r, g, b; +}; + +/// Implement and register this class with a b2World to provide debug drawing of physics +/// entities in your game. +class b2Draw +{ +public: + b2Draw(); + + virtual ~b2Draw() {} + + enum + { + e_shapeBit = 0x0001, ///< draw shapes + e_jointBit = 0x0002, ///< draw joint connections + e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes + e_pairBit = 0x0008, ///< draw broad-phase pairs + e_centerOfMassBit = 0x0010 ///< draw center of mass frame + }; + + /// Set the drawing flags. + void SetFlags(uint32 flags); + + /// Get the drawing flags. + uint32 GetFlags() const; + + /// Append flags to the current flags. + void AppendFlags(uint32 flags); + + /// Clear flags from the current flags. + void ClearFlags(uint32 flags); + + /// Draw a closed polygon provided in CCW order. + virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a solid closed polygon provided in CCW order. + virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a circle. + virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; + + /// Draw a solid circle. + virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; + + /// Draw a line segment. + virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; + + /// Draw a transform. Choose your own length scale. + /// @param xf a transform. + virtual void DrawTransform(const b2Transform& xf) = 0; + +protected: + uint32 m_drawFlags; +}; diff --git a/libs/box2d/src/Box2D/Common/b2GrowableStack.h b/libs/box2d/src/Box2D/Common/b2GrowableStack.h new file mode 100644 index 0000000..d2e0712 --- /dev/null +++ b/libs/box2d/src/Box2D/Common/b2GrowableStack.h @@ -0,0 +1,85 @@ +/* +* Copyright (c) 2010 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_GROWABLE_STACK_H +#define B2_GROWABLE_STACK_H +#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 +/// to increase the size of the stack. +template +class b2GrowableStack +{ +public: + b2GrowableStack() + { + m_stack = m_array; + m_count = 0; + m_capacity = N; + } + + ~b2GrowableStack() + { + if (m_stack != m_array) + { + b2Free(m_stack); + m_stack = NULL; + } + } + + void Push(const T& element) + { + if (m_count == m_capacity) + { + T* old = m_stack; + m_capacity *= 2; + m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); + std::memcpy(m_stack, old, m_count * sizeof(T)); + if (old != m_array) + { + b2Free(old); + } + } + + m_stack[m_count] = element; + ++m_count; + } + + T Pop() + { + b2Assert(m_count > 0); + --m_count; + return m_stack[m_count]; + } + + int32 GetCount() + { + return m_count; + } + +private: + T* m_stack; + T m_array[N]; + int32 m_count; + int32 m_capacity; +}; + + +#endif diff --git a/libs/box2d/src/Box2D/Common/b2Math.cpp b/libs/box2d/src/Box2D/Common/b2Math.cpp index f15a43b..c44f474 100644 --- a/libs/box2d/src/Box2D/Common/b2Math.cpp +++ b/libs/box2d/src/Box2D/Common/b2Math.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2007-2009 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 diff --git a/libs/box2d/src/Box2D/Common/b2Math.h b/libs/box2d/src/Box2D/Common/b2Math.h index c29f594..8eade76 100644 --- a/libs/box2d/src/Box2D/Common/b2Math.h +++ b/libs/box2d/src/Box2D/Common/b2Math.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -57,13 +57,8 @@ inline float32 b2InvSqrt(float32 x) return x; } -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) - -inline float32 b2Abs(float32 a) -{ - return a > 0.0f ? a : -a; -} +#define b2Sqrt(x) std::sqrt(x) +#define b2Atan2(y, x) std::atan2(y, x) /// A 2D column vector. struct b2Vec2 @@ -147,6 +142,12 @@ struct b2Vec2 return b2IsValid(x) && b2IsValid(y); } + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + b2Vec2 Skew() const + { + return b2Vec2(-y, x); + } + float32 x, y; }; @@ -364,12 +365,12 @@ struct b2Transform struct b2Sweep { /// Get the interpolated transform at a specific time. - /// @param alpha is a factor in [0,1], where 0 indicates t0. - void GetTransform(b2Transform* xf, float32 alpha) const; + /// @param beta is a factor in [0,1], where 0 indicates alpha0. + void GetTransform(b2Transform* xf, float32 beta) const; /// Advance the sweep forward, yielding a new initial state. - /// @param t the new initial time. - void Advance(float32 t); + /// @param alpha the new initial time. + void Advance(float32 alpha); /// Normalize the angles. void Normalize(); @@ -377,6 +378,10 @@ struct b2Sweep b2Vec2 localCenter; ///< local center of mass position b2Vec2 c0, c; ///< center world positions float32 a0, a; ///< world angles + + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + float32 alpha0; }; @@ -525,6 +530,21 @@ inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) return b2MulT(T.R, v - T.position); } +// v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p) +inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) +{ + b2Transform C; + C.R = b2MulT(A.R, B.R); + C.position = B.position - A.position; + return C; +} + +template +inline T b2Abs(T a) +{ + return a > T(0) ? a : -a; +} + inline b2Vec2 b2Abs(const b2Vec2& a) { return b2Vec2(b2Abs(a.x), b2Abs(a.y)); @@ -596,20 +616,23 @@ inline bool b2IsPowerOfTwo(uint32 x) return result; } -inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const +inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const { - xf->position = (1.0f - alpha) * c0 + alpha * c; - float32 angle = (1.0f - alpha) * a0 + alpha * a; + xf->position = (1.0f - beta) * c0 + beta * c; + float32 angle = (1.0f - beta) * a0 + beta * a; xf->R.Set(angle); // Shift to origin xf->position -= b2Mul(xf->R, localCenter); } -inline void b2Sweep::Advance(float32 t) +inline void b2Sweep::Advance(float32 alpha) { - c0 = (1.0f - t) * c0 + t * c; - a0 = (1.0f - t) * a0 + t * a; + 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; + alpha0 = alpha; } /// Normalize an angle in radians to be between -pi and pi diff --git a/libs/box2d/src/Box2D/Common/b2Settings.cpp b/libs/box2d/src/Box2D/Common/b2Settings.cpp index da30814..97f5283 100644 --- a/libs/box2d/src/Box2D/Common/b2Settings.cpp +++ b/libs/box2d/src/Box2D/Common/b2Settings.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -19,7 +19,7 @@ #include #include -b2Version b2_version = {2, 1, 2}; +b2Version b2_version = {2, 2, 0}; // Memory allocators. Modify these to use your own allocator. void* b2Alloc(int32 size) diff --git a/libs/box2d/src/Box2D/Common/b2Settings.h b/libs/box2d/src/Box2D/Common/b2Settings.h index 1aa8c26..43b5f2a 100644 --- a/libs/box2d/src/Box2D/Common/b2Settings.h +++ b/libs/box2d/src/Box2D/Common/b2Settings.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -32,6 +32,7 @@ typedef unsigned char uint8; typedef unsigned short uint16; typedef unsigned int uint32; typedef float float32; +typedef double float64; #define b2_maxFloat FLT_MAX #define b2_epsilon FLT_EPSILON @@ -46,7 +47,8 @@ typedef float float32; /// The maximum number of contact points between two convex shapes. #define b2_maxManifoldPoints 2 -/// The maximum number of vertices on a convex polygon. +/// The maximum number of vertices on a convex polygon. You cannot increase +/// this too much because b2BlockAllocator has a maximum object size. #define b2_maxPolygonVertices 8 /// This is used to fatten AABBs in the dynamic tree. This allows proxies @@ -72,6 +74,9 @@ typedef float float32; /// Making it larger may create artifacts for vertex collision. #define b2_polygonRadius (2.0f * b2_linearSlop) +/// Maximum number of sub-steps per contact in continuous physics simulation. +#define b2_maxSubSteps 8 + // Dynamics @@ -105,6 +110,7 @@ typedef float float32; /// to overshoot. #define b2_contactBaumgarte 0.2f + // Sleep /// The time that a body must be still before it will go to sleep. @@ -136,16 +142,4 @@ struct b2Version /// Current version. extern b2Version b2_version; -/// Friction mixing law. Feel free to customize this. -inline float32 b2MixFriction(float32 friction1, float32 friction2) -{ - return sqrtf(friction1 * friction2); -} - -/// Restitution mixing law. Feel free to customize this. -inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) -{ - return restitution1 > restitution2 ? restitution1 : restitution2; -} - #endif diff --git a/libs/box2d/src/Box2D/Common/b2StackAllocator.cpp b/libs/box2d/src/Box2D/Common/b2StackAllocator.cpp index cb2c42e..4a86283 100644 --- a/libs/box2d/src/Box2D/Common/b2StackAllocator.cpp +++ b/libs/box2d/src/Box2D/Common/b2StackAllocator.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Common/b2StackAllocator.h b/libs/box2d/src/Box2D/Common/b2StackAllocator.h index 315cc02..796c51d 100644 --- a/libs/box2d/src/Box2D/Common/b2StackAllocator.h +++ b/libs/box2d/src/Box2D/Common/b2StackAllocator.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Common/b2Timer.cpp b/libs/box2d/src/Box2D/Common/b2Timer.cpp new file mode 100644 index 0000000..ec7f2c3 --- /dev/null +++ b/libs/box2d/src/Box2D/Common/b2Timer.cpp @@ -0,0 +1,76 @@ +/* +* Copyright (c) 2011 Erin Catto http://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 + +float64 b2Timer::s_invFrequency = 0.0f; + +#if defined(WIN32) + +#include + +b2Timer::b2Timer() +{ + LARGE_INTEGER largeInteger; + + if (s_invFrequency == 0.0f) + { + QueryPerformanceFrequency(&largeInteger); + s_invFrequency = float64(largeInteger.QuadPart); + if (s_invFrequency > 0.0f) + { + s_invFrequency = 1000.0f / s_invFrequency; + } + } + + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); +} + +void b2Timer::Reset() +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); +} + +float32 b2Timer::GetMilliseconds() const +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + float64 count = float64(largeInteger.QuadPart); + float32 ms = float32(s_invFrequency * (count - m_start)); + return ms; +} + +#else + +b2Timer::b2Timer() +{ +} + +void b2Timer::Reset() +{ +} + +float32 b2Timer::GetMilliseconds() const +{ + return 0.0f; +} + +#endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Common/b2Timer.h similarity index 53% copy from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h copy to libs/box2d/src/Box2D/Common/b2Timer.h index c092e2a..14e1437 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Common/b2Timer.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2011 Erin Catto http://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 @@ -16,36 +16,25 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#include -#include - -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; - -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +/// Timer for profiling. This has platform specific code and may +/// not work on every platform. +class b2Timer { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + /// Constructor + b2Timer(); - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); + /// Reset the timer. + void Reset(); + + /// Get the time since construction or the last reset. + float32 GetMilliseconds() const; private: - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; + float64 m_start; + static float64 s_invFrequency; }; - -#endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.cpp index 6df0091..584ef2f 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -24,8 +24,9 @@ #include #include +using namespace std; -b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2CircleContact)); return new (mem) b2CircleContact(fixtureA, fixtureB); @@ -38,7 +39,7 @@ void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) } b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.h index afc277a..aac1f0b 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2CircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -26,7 +26,8 @@ class b2BlockAllocator; class b2CircleContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.cpp index e7ad1ca..fd19801 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -20,6 +20,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -38,6 +42,10 @@ void b2Contact::InitializeRegisters() AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); + AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); + AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); + AddType(b2LoopAndCircleContact::Create, b2LoopAndCircleContact::Destroy, b2Shape::e_loop, b2Shape::e_circle); + AddType(b2LoopAndPolygonContact::Create, b2LoopAndPolygonContact::Destroy, b2Shape::e_loop, b2Shape::e_polygon); } void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, @@ -58,7 +66,7 @@ void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* dest } } -b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) { if (s_initialized == false) { @@ -77,11 +85,11 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAl { if (s_registers[type1][type2].primary) { - return createFcn(fixtureA, fixtureB, allocator); + return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); } else { - return createFcn(fixtureB, fixtureA, allocator); + return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); } } else @@ -110,13 +118,16 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) destroyFcn(contact, allocator); } -b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB) +b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) { m_flags = e_enabledFlag; m_fixtureA = fA; m_fixtureB = fB; + m_indexA = indexA; + m_indexB = indexB; + m_manifold.pointCount = 0; m_prev = NULL; @@ -133,6 +144,9 @@ b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB) m_nodeB.other = NULL; m_toiCount = 0; + + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); } // Update the contact manifold and touching status. @@ -161,7 +175,7 @@ void b2Contact::Update(b2ContactListener* listener) { const b2Shape* shapeA = m_fixtureA->GetShape(); const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, shapeB, xfA, xfB); + touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); // Sensors don't generate manifolds. m_manifold.pointCount = 0; diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.h index a6add3d..7b5a6ab 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2Contact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -22,7 +22,6 @@ #include #include #include -#include #include class b2Body; @@ -33,7 +32,23 @@ class b2BlockAllocator; class b2StackAllocator; class b2ContactListener; -typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); +/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. +/// For example, anything slides on ice. +inline float32 b2MixFriction(float32 friction1, float32 friction2) +{ + return std::sqrt(friction1 * friction2); +} + +/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. +/// For example, a superball bounces on anything. +inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) +{ + return restitution1 > restitution2 ? restitution1 : restitution2; +} + +typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, + b2BlockAllocator* allocator); typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); struct b2ContactRegister @@ -86,14 +101,40 @@ public: b2Contact* GetNext(); const b2Contact* GetNext() const; - /// Get the first fixture in this contact. + /// Get fixture A in this contact. b2Fixture* GetFixtureA(); const b2Fixture* GetFixtureA() const; - /// Get the second fixture in this contact. + /// Get the child primitive index for fixture A. + int32 GetChildIndexA() const; + + /// Get fixture B in this contact. b2Fixture* GetFixtureB(); const b2Fixture* GetFixtureB() const; + /// Get the child primitive index for fixture B. + int32 GetChildIndexB() const; + + /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. + /// This value persists until set or reset. + void SetFriction(float32 friction); + + /// Get the friction. + float32 GetFriction() const; + + /// Reset the friction mixture to the default value. + void ResetFriction(); + + /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. + /// The value persists until you set or reset. + void SetRestitution(float32 restitution); + + /// Get the restitution. + float32 GetRestitution() const; + + /// Reset the restitution to the default value. + void ResetRestitution(); + /// Evaluate this contact with your own manifold and transforms. virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; @@ -122,6 +163,8 @@ protected: // This bullet contact had a TOI event e_bulletHitFlag = 0x0010, + // This contact has a valid TOI in m_toi + e_toiFlag = 0x0020 }; /// Flag this contact for filtering. Filtering will occur the next time step. @@ -130,12 +173,12 @@ protected: static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, b2Shape::Type typeA, b2Shape::Type typeB); static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {} - b2Contact(b2Fixture* fixtureA, b2Fixture* fixtureB); + b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); virtual ~b2Contact() {} void Update(b2ContactListener* listener); @@ -156,10 +199,16 @@ protected: b2Fixture* m_fixtureA; b2Fixture* m_fixtureB; + int32 m_indexA; + int32 m_indexB; + b2Manifold m_manifold; int32 m_toiCount; -// float32 m_toi; + float32 m_toi; + + float32 m_friction; + float32 m_restitution; }; inline b2Manifold* b2Contact::GetManifold() @@ -229,14 +278,54 @@ inline b2Fixture* b2Contact::GetFixtureB() return m_fixtureB; } +inline int32 b2Contact::GetChildIndexA() const +{ + return m_indexA; +} + inline const b2Fixture* b2Contact::GetFixtureB() const { return m_fixtureB; } +inline int32 b2Contact::GetChildIndexB() const +{ + return m_indexB; +} + inline void b2Contact::FlagForFiltering() { m_flags |= e_filterFlag; } +inline void b2Contact::SetFriction(float32 friction) +{ + m_friction = friction; +} + +inline float32 b2Contact::GetFriction() const +{ + return m_friction; +} + +inline void b2Contact::ResetFriction() +{ + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); +} + +inline void b2Contact::SetRestitution(float32 restitution) +{ + m_restitution = restitution; +} + +inline float32 b2Contact::GetRestitution() const +{ + return m_restitution; +} + +inline void b2Contact::ResetRestitution() +{ + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); +} + #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.cpp index f65fb60..cd99b0b 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -17,6 +17,8 @@ */ #include + + #include #include #include @@ -25,17 +27,17 @@ #define B2_DEBUG_SOLVER 0 -b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, - b2StackAllocator* allocator, float32 impulseRatio) +b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) { - m_allocator = allocator; + m_allocator = def->allocator; - m_constraintCount = contactCount; - m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint)); + m_count = def->count; + m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactConstraint)); - for (int32 i = 0; i < m_constraintCount; ++i) + // Initialize position independent portions of the constraints. + for (int32 i = 0; i < m_count; ++i) { - b2Contact* contact = contacts[i]; + b2Contact* contact = def->contacts[i]; b2Fixture* fixtureA = contact->m_fixtureA; b2Fixture* fixtureB = contact->m_fixtureB; @@ -47,30 +49,21 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, b2Body* bodyB = fixtureB->GetBody(); b2Manifold* manifold = contact->GetManifold(); - float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction()); - float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution()); - - b2Vec2 vA = bodyA->m_linearVelocity; - b2Vec2 vB = bodyB->m_linearVelocity; - float32 wA = bodyA->m_angularVelocity; - float32 wB = bodyB->m_angularVelocity; - b2Assert(manifold->pointCount > 0); - b2WorldManifold worldManifold; - worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB); - b2ContactConstraint* cc = m_constraints + i; + cc->friction = contact->m_friction; + cc->restitution = contact->m_restitution; cc->bodyA = bodyA; cc->bodyB = bodyB; cc->manifold = manifold; - cc->normal = worldManifold.normal; + cc->normal.SetZero(); cc->pointCount = manifold->pointCount; - cc->friction = friction; cc->localNormal = manifold->localNormal; cc->localPoint = manifold->localPoint; - cc->radius = radiusA + radiusB; + cc->radiusA = radiusA; + cc->radiusB = radiusB; cc->type = manifold->type; for (int32 j = 0; j < cc->pointCount; ++j) @@ -78,10 +71,63 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, b2ManifoldPoint* cp = manifold->points + j; b2ContactConstraintPoint* ccp = cc->points + j; - ccp->normalImpulse = impulseRatio * cp->normalImpulse; - ccp->tangentImpulse = impulseRatio * cp->tangentImpulse; + if (def->warmStarting) + { + ccp->normalImpulse = def->impulseRatio * cp->normalImpulse; + ccp->tangentImpulse = def->impulseRatio * cp->tangentImpulse; + } + else + { + ccp->normalImpulse = 0.0f; + ccp->tangentImpulse = 0.0f; + } ccp->localPoint = cp->localPoint; + ccp->rA.SetZero(); + ccp->rB.SetZero(); + ccp->normalMass = 0.0f; + ccp->tangentMass = 0.0f; + ccp->velocityBias = 0.0f; + } + + cc->K.SetZero(); + cc->normalMass.SetZero(); + } +} + +b2ContactSolver::~b2ContactSolver() +{ + m_allocator->Free(m_constraints); +} + +// Initialize position dependent portions of the velocity constraints. +void b2ContactSolver::InitializeVelocityConstraints() +{ + for (int32 i = 0; i < m_count; ++i) + { + b2ContactConstraint* cc = m_constraints + i; + + float32 radiusA = cc->radiusA; + float32 radiusB = cc->radiusB; + b2Body* bodyA = cc->bodyA; + b2Body* bodyB = cc->bodyB; + b2Manifold* manifold = cc->manifold; + + b2Vec2 vA = bodyA->m_linearVelocity; + b2Vec2 vB = bodyB->m_linearVelocity; + float32 wA = bodyA->m_angularVelocity; + float32 wB = bodyB->m_angularVelocity; + + b2Assert(manifold->pointCount > 0); + + b2WorldManifold worldManifold; + worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB); + + cc->normal = worldManifold.normal; + + for (int32 j = 0; j < cc->pointCount; ++j) + { + b2ContactConstraintPoint* ccp = cc->points + j; ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c; ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c; @@ -113,7 +159,7 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA)); if (vRel < -b2_velocityThreshold) { - ccp->velocityBias = -restitution * vRel; + ccp->velocityBias = -cc->restitution * vRel; } } @@ -122,7 +168,7 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, { b2ContactConstraintPoint* ccp1 = cc->points + 0; b2ContactConstraintPoint* ccp2 = cc->points + 1; - + float32 invMassA = bodyA->m_invMass; float32 invIA = bodyA->m_invI; float32 invMassB = bodyB->m_invMass; @@ -138,7 +184,7 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. - const float32 k_maxConditionNumber = 100.0f; + const float32 k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. @@ -156,15 +202,10 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount, } } -b2ContactSolver::~b2ContactSolver() -{ - m_allocator->Free(m_constraints); -} - void b2ContactSolver::WarmStart() { // Warm start. - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; @@ -191,7 +232,7 @@ void b2ContactSolver::WarmStart() void b2ContactSolver::SolveVelocityConstraints() { - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* bodyA = c->bodyA; @@ -490,7 +531,7 @@ void b2ContactSolver::SolveVelocityConstraints() void b2ContactSolver::StoreImpulses() { - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; b2Manifold* m = c->manifold; @@ -526,7 +567,7 @@ struct b2PositionSolverManifold } point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - cc->radius; + separation = b2Dot(pointB - pointA, normal) - cc->radiusA - cc->radiusB; } break; @@ -536,7 +577,7 @@ struct b2PositionSolverManifold b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint); b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; + separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB; point = clipPoint; } break; @@ -547,7 +588,7 @@ struct b2PositionSolverManifold b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint); b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; + separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB; point = clipPoint; // Ensure normal points from A to B @@ -567,7 +608,7 @@ bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte) { float32 minSeparation = 0.0f; - for (int32 i = 0; i < m_constraintCount; ++i) + for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* bodyA = c->bodyA; @@ -621,3 +662,75 @@ bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte) // push the separation above -b2_linearSlop. return minSeparation >= -1.5f * b2_linearSlop; } + +// Sequential position solver for position constraints. +bool b2ContactSolver::SolveTOIPositionConstraints(float32 baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB) +{ + float32 minSeparation = 0.0f; + + for (int32 i = 0; i < m_count; ++i) + { + b2ContactConstraint* c = m_constraints + i; + b2Body* bodyA = c->bodyA; + b2Body* bodyB = c->bodyB; + + float32 massA = 0.0f; + if (bodyA == toiBodyA || bodyA == toiBodyB) + { + massA = bodyA->m_mass; + } + + float32 massB = 0.0f; + if (bodyB == toiBodyA || bodyB == toiBodyB) + { + massB = bodyB->m_mass; + } + + float32 invMassA = bodyA->m_mass * bodyA->m_invMass; + float32 invIA = bodyA->m_mass * bodyA->m_invI; + float32 invMassB = bodyB->m_mass * bodyB->m_invMass; + float32 invIB = bodyB->m_mass * bodyB->m_invI; + + // Solve normal constraints + for (int32 j = 0; j < c->pointCount; ++j) + { + b2PositionSolverManifold psm; + psm.Initialize(c, j); + b2Vec2 normal = psm.normal; + + b2Vec2 point = psm.point; + float32 separation = psm.separation; + + b2Vec2 rA = point - bodyA->m_sweep.c; + b2Vec2 rB = point - bodyB->m_sweep.c; + + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float32 rnA = b2Cross(rA, normal); + float32 rnB = b2Cross(rB, normal); + float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; + + // Compute normal impulse + float32 impulse = K > 0.0f ? - C / K : 0.0f; + + b2Vec2 P = impulse * normal; + + bodyA->m_sweep.c -= invMassA * P; + bodyA->m_sweep.a -= invIA * b2Cross(rA, P); + bodyA->SynchronizeTransform(); + + bodyB->m_sweep.c += invMassB * P; + bodyB->m_sweep.a += invIB * b2Cross(rB, P); + bodyB->SynchronizeTransform(); + } + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -1.5f * b2_linearSlop; +} diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.h index b8555bb..93698d0 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2ContactSolver.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -50,29 +50,41 @@ struct b2ContactConstraint b2Body* bodyA; b2Body* bodyB; b2Manifold::Type type; - float32 radius; + float32 radiusA, radiusB; float32 friction; + float32 restitution; int32 pointCount; b2Manifold* manifold; }; +struct b2ContactSolverDef +{ + b2Contact** contacts; + int32 count; + b2StackAllocator* allocator; + float32 impulseRatio; + bool warmStarting; +}; + class b2ContactSolver { public: - b2ContactSolver(b2Contact** contacts, int32 contactCount, - b2StackAllocator* allocator, float32 impulseRatio); - + b2ContactSolver(b2ContactSolverDef* def); ~b2ContactSolver(); + void InitializeVelocityConstraints(); + void WarmStart(); void SolveVelocityConstraints(); void StoreImpulses(); bool SolvePositionConstraints(float32 baumgarte); + bool SolveTOIPositionConstraints(float32 baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB); b2StackAllocator* m_allocator; b2ContactConstraint* m_constraints; - int m_constraintCount; + int m_count; }; #endif + diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp new file mode 100644 index 0000000..0433650 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp @@ -0,0 +1,50 @@ +/* +* Copyright (c) 2006-2010 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 + +#include +using namespace std; + +b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); + return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); +} + +void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); + allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); +} + +b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndCircle( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h similarity index 53% copy from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h copy to libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h index c092e2a..11b61b8 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -16,36 +16,24 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H +#define B2_EDGE_AND_CIRCLE_CONTACT_H -#include +#include -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; +class b2BlockAllocator; -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +class b2EdgeAndCircleContact : public b2Contact { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2EdgeAndCircleContact() {} - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); - -private: - - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp new file mode 100644 index 0000000..8bac536 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp @@ -0,0 +1,50 @@ +/* +* Copyright (c) 2006-2010 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 + +#include +using namespace std; + +b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); + return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); +} + +void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); + allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); +} + +b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndPolygon( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h similarity index 53% copy from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h copy to libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h index c092e2a..74b27ee 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -16,36 +16,24 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#ifndef B2_EDGE_AND_POLYGON_CONTACT_H +#define B2_EDGE_AND_POLYGON_CONTACT_H -#include +#include -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; +class b2BlockAllocator; -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +class b2EdgeAndPolygonContact : public b2Contact { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2EdgeAndPolygonContact() {} - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); - -private: - - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp new file mode 100644 index 0000000..c1e4ab2 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp @@ -0,0 +1,54 @@ +/* +* Copyright (c) 2006-2010 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 +#include +#include + +#include +using namespace std; + +b2Contact* b2LoopAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2LoopAndCircleContact)); + return new (mem) b2LoopAndCircleContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2LoopAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2LoopAndCircleContact*)contact)->~b2LoopAndCircleContact(); + allocator->Free(contact, sizeof(b2LoopAndCircleContact)); +} + +b2LoopAndCircleContact::b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_loop); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2LoopAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + loop->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndCircle( manifold, &edge, xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h similarity index 53% copy from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h copy to libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h index c092e2a..33d6a41 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -16,36 +16,24 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#ifndef B2_LOOP_AND_CIRCLE_CONTACT_H +#define B2_LOOP_AND_CIRCLE_CONTACT_H -#include +#include -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; +class b2BlockAllocator; -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +class b2LoopAndCircleContact : public b2Contact { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); + ~b2LoopAndCircleContact() {} - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); - -private: - - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp new file mode 100644 index 0000000..8bd0795 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp @@ -0,0 +1,54 @@ +/* +* Copyright (c) 2006-2010 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 +#include +#include + +#include +using namespace std; + +b2Contact* b2LoopAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2LoopAndPolygonContact)); + return new (mem) b2LoopAndPolygonContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2LoopAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2LoopAndPolygonContact*)contact)->~b2LoopAndPolygonContact(); + allocator->Free(contact, sizeof(b2LoopAndPolygonContact)); +} + +b2LoopAndPolygonContact::b2LoopAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_loop); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2LoopAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + loop->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndPolygon( manifold, &edge, xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h similarity index 53% rename from libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h rename to libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h index c092e2a..7e96abc 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -16,36 +16,24 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_TOI_SOLVER_H -#define B2_TOI_SOLVER_H +#ifndef B2_LOOP_AND_POLYGON_CONTACT_H +#define B2_LOOP_AND_POLYGON_CONTACT_H -#include +#include -class b2Contact; -class b2Body; -struct b2TOIConstraint; -class b2StackAllocator; +class b2BlockAllocator; -/// This is a pure position solver for a single movable body in contact with -/// multiple non-moving bodies. -class b2TOISolver +class b2LoopAndPolygonContact : public b2Contact { public: - b2TOISolver(b2StackAllocator* allocator); - ~b2TOISolver(); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody); - void Clear(); + b2LoopAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); + ~b2LoopAndPolygonContact() {} - // Perform one solver iteration. Returns true if converged. - bool Solve(float32 baumgarte); - -private: - - b2TOIConstraint* m_constraints; - int32 m_count; - b2Body* m_toiBody; - b2StackAllocator* m_allocator; + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp index 837722d..880f83e 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -18,14 +18,12 @@ #include #include -#include -#include #include -#include #include +using namespace std; -b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); @@ -38,7 +36,7 @@ void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* al } b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h index 684b2ae..6beca16 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -26,7 +26,7 @@ class b2BlockAllocator; class b2PolygonAndCircleContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.cpp index eab2af5..52e1be8 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -24,8 +24,9 @@ #include #include +using namespace std; -b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) { void* mem = allocator->Allocate(sizeof(b2PolygonContact)); return new (mem) b2PolygonContact(fixtureA, fixtureB); @@ -38,7 +39,7 @@ void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) } b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) { b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.h b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.h index af544c2..4593214 100644 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.h +++ b/libs/box2d/src/Box2D/Dynamics/Contacts/b2PolygonContact.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -26,7 +26,8 @@ class b2BlockAllocator; class b2PolygonContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); diff --git a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.cpp b/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.cpp deleted file mode 100644 index 567cd71..0000000 --- a/libs/box2d/src/Box2D/Dynamics/Contacts/b2TOISolver.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com -* -* 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 -#include -#include - -struct b2TOIConstraint -{ - b2Vec2 localPoints[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; - b2Manifold::Type type; - float32 radius; - int32 pointCount; - b2Body* bodyA; - b2Body* bodyB; -}; - -b2TOISolver::b2TOISolver(b2StackAllocator* allocator) -{ - m_allocator = allocator; - m_constraints = NULL; - m_count = NULL; - m_toiBody = NULL; -} - -b2TOISolver::~b2TOISolver() -{ - Clear(); -} - -void b2TOISolver::Clear() -{ - if (m_allocator && m_constraints) - { - m_allocator->Free(m_constraints); - m_constraints = NULL; - } -} - -void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody) -{ - Clear(); - - m_count = count; - m_toiBody = toiBody; - - m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint)); - - for (int32 i = 0; i < m_count; ++i) - { - b2Contact* contact = contacts[i]; - - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - b2Shape* shapeA = fixtureA->GetShape(); - b2Shape* shapeB = fixtureB->GetShape(); - float32 radiusA = shapeA->m_radius; - float32 radiusB = shapeB->m_radius; - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - b2Manifold* manifold = contact->GetManifold(); - - b2Assert(manifold->pointCount > 0); - - b2TOIConstraint* constraint = m_constraints + i; - constraint->bodyA = bodyA; - constraint->bodyB = bodyB; - constraint->localNormal = manifold->localNormal; - constraint->localPoint = manifold->localPoint; - constraint->type = manifold->type; - constraint->pointCount = manifold->pointCount; - constraint->radius = radiusA + radiusB; - - for (int32 j = 0; j < constraint->pointCount; ++j) - { - b2ManifoldPoint* cp = manifold->points + j; - constraint->localPoints[j] = cp->localPoint; - } - } -} - -struct b2TOISolverManifold -{ - void Initialize(b2TOIConstraint* cc, int32 index) - { - b2Assert(cc->pointCount > 0); - - switch (cc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint); - b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - else - { - normal.Set(1.0f, 0.0f); - } - - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - cc->radius; - } - break; - - case b2Manifold::e_faceA: - { - normal = cc->bodyA->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint); - - b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = cc->bodyB->GetWorldVector(cc->localNormal); - b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint); - - b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - cc->radius; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float32 separation; -}; - -// Push out the toi body to provide clearance for further simulation. -bool b2TOISolver::Solve(float32 baumgarte) -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2TOIConstraint* c = m_constraints + i; - b2Body* bodyA = c->bodyA; - b2Body* bodyB = c->bodyB; - - float32 massA = bodyA->m_mass; - float32 massB = bodyB->m_mass; - - // Only the TOI body should move. - if (bodyA == m_toiBody) - { - massB = 0.0f; - } - else - { - massA = 0.0f; - } - - float32 invMassA = massA * bodyA->m_invMass; - float32 invIA = massA * bodyA->m_invI; - float32 invMassB = massB * bodyB->m_invMass; - float32 invIB = massB * bodyB->m_invI; - - // Solve normal constraints - for (int32 j = 0; j < c->pointCount; ++j) - { - b2TOISolverManifold psm; - psm.Initialize(c, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - bodyA->m_sweep.c; - b2Vec2 rB = point - bodyB->m_sweep.c; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - bodyA->m_sweep.c -= invMassA * P; - bodyA->m_sweep.a -= invIA * b2Cross(rA, P); - bodyA->SynchronizeTransform(); - - bodyB->m_sweep.c += invMassB * P; - bodyB->m_sweep.a += invIB * b2Cross(rB, P); - bodyB->SynchronizeTransform(); - } - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; -} diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.cpp index 3469bd9..3112279 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -46,7 +46,6 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, length = d.Length(); } - b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) : b2Joint(def) { diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.h index 448faa6..f2a9732 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2DistanceJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -70,7 +70,12 @@ public: b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; + /// Get the reaction force given the inverse time step. + /// Unit is N. b2Vec2 GetReactionForce(float32 inv_dt) const; + + /// Get the reaction torque given the inverse time step. + /// Unit is N*m. This is always zero for a distance joint. float32 GetReactionTorque(float32 inv_dt) const; /// Set/get the natural length. diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.cpp index 9097dee..83e9e7e 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.h index b4c4af2..bba5133 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2FrictionJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.cpp index 89b17ee..83433d2 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2007 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.h index eccca0d..cea3a10 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2GearJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.cpp index a7e19d3..eee195a 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -80,10 +81,10 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) } break; - case e_lineJoint: + case e_wheelJoint: { - void* mem = allocator->Allocate(sizeof(b2LineJoint)); - joint = new (mem) b2LineJoint((b2LineJointDef*)def); + void* mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint((b2WheelJointDef*)def); } break; @@ -101,6 +102,13 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) } break; + case e_ropeJoint: + { + void* mem = allocator->Allocate(sizeof(b2RopeJoint)); + joint = new (mem) b2RopeJoint((b2RopeJointDef*)def); + } + break; + default: b2Assert(false); break; @@ -138,8 +146,8 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2GearJoint)); break; - case e_lineJoint: - allocator->Free(joint, sizeof(b2LineJoint)); + case e_wheelJoint: + allocator->Free(joint, sizeof(b2WheelJoint)); break; case e_weldJoint: @@ -150,6 +158,10 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) allocator->Free(joint, sizeof(b2FrictionJoint)); break; + case e_ropeJoint: + allocator->Free(joint, sizeof(b2RopeJoint)); + break; + default: b2Assert(false); break; diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.h index 213ad7d..baffab3 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2Joint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -35,9 +35,10 @@ enum b2JointType e_pulleyJoint, e_mouseJoint, e_gearJoint, - e_lineJoint, + e_wheelJoint, e_weldJoint, e_frictionJoint, + e_ropeJoint }; enum b2LimitState @@ -130,6 +131,7 @@ public: /// Get the next joint the world joint list. b2Joint* GetNext(); + const b2Joint* GetNext() const; /// Get the user data pointer. void* GetUserData() const; @@ -140,6 +142,11 @@ public: /// Short-cut function to determine if either body is inactive. bool IsActive() const; + /// Get collide connected. + /// Note: modifying the collide connect flag won't work correctly because + /// the flag is only checked when fixture AABBs begin to overlap. + bool GetCollideConnected() const; + protected: friend class b2World; friend class b2Body; @@ -171,6 +178,7 @@ protected: void* m_userData; // Cache here per time step to reduce cache misses. + // TODO_ERIN nuke b2Vec2 m_localCenterA, m_localCenterB; float32 m_invMassA, m_invIA; float32 m_invMassB, m_invIB; @@ -213,6 +221,11 @@ inline b2Joint* b2Joint::GetNext() return m_next; } +inline const b2Joint* b2Joint::GetNext() const +{ + return m_next; +} + inline void* b2Joint::GetUserData() const { return m_userData; @@ -223,4 +236,9 @@ inline void b2Joint::SetUserData(void* data) m_userData = data; } +inline bool b2Joint::GetCollideConnected() const +{ + return m_collideConnected; +} + #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.cpp deleted file mode 100644 index b6b0a1c..0000000 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.cpp +++ /dev/null @@ -1,591 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com -* -* 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 - -// Linear constraint (point-to-line) -// d = p2 - p1 = x2 + r2 - x1 - r1 -// C = dot(perp, d) -// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) -// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// a = perp -// s1 = cross(d + r1, a) = cross(p2 - x1, a) -// s2 = cross(r2, a) = cross(p2 - x2, a) - - -// Motor/Limit linear constraint -// C = dot(ax1, d) -// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) -// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] - -// Block Solver -// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even -// when the mass has poor distribution (leading to large torques about the joint anchor points). -// -// The Jacobian has 3 rows: -// J = [-uT -s1 uT s2] // linear -// [-vT -a1 vT a2] // limit -// -// u = perp -// v = axis -// s1 = cross(d + r1, u), s2 = cross(r2, u) -// a1 = cross(d + r1, v), a2 = cross(r2, v) - -// M * (v2 - v1) = JT * df -// J * v2 = bias -// -// v2 = v1 + invM * JT * df -// J * (v1 + invM * JT * df) = bias -// K * df = bias - J * v1 = -Cdot -// K = J * invM * JT -// Cdot = J * v1 - bias -// -// Now solve for f2. -// df = f2 - f1 -// K * (f2 - f1) = -Cdot -// f2 = invK * (-Cdot) + f1 -// -// Clamp accumulated limit impulse. -// lower: f2(2) = max(f2(2), 0) -// upper: f2(2) = min(f2(2), 0) -// -// Solve for correct f2(1) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1 -// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1) -// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) -// -// Now compute impulse to be applied: -// df = f2 - f1 - -void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); -} - -b2LineJoint::b2LineJoint(const b2LineJointDef* def) -: b2Joint(def) -{ - m_localAnchor1 = def->localAnchorA; - m_localAnchor2 = def->localAnchorB; - m_localXAxis1 = def->localAxisA; - m_localYAxis1 = b2Cross(1.0f, m_localXAxis1); - - 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(); -} - -void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step) -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - m_localCenterA = b1->GetLocalCenter(); - m_localCenterB = b2->GetLocalCenter(); - - b2Transform xf1 = b1->GetTransform(); - b2Transform xf2 = b2->GetTransform(); - - // Compute the effective masses. - b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB); - b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; - - m_invMassA = b1->m_invMass; - m_invIA = b1->m_invI; - m_invMassB = b2->m_invMass; - m_invIB = b2->m_invI; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(xf1.R, m_localXAxis1); - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); - - m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2; - if (m_motorMass > b2_epsilon) - { - m_motorMass = 1.0f / m_motorMass; - } - else - { - m_motorMass = 0.0f; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(xf1.R, m_localYAxis1); - - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); - - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12); - m_K.col2.Set(k12, k22); - } - - // 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.y = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.y = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.y = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (step.warmStarting) - { - // Account for variable time step. - m_impulse *= step.dtRatio; - m_motorImpulse *= step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis; - float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1; - float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2; - - b1->m_linearVelocity -= m_invMassA * P; - b1->m_angularVelocity -= m_invIA * L1; - - b2->m_linearVelocity += m_invMassB * P; - b2->m_angularVelocity += m_invIB * L2; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } -} - -void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step) -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 v1 = b1->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w2 = b2->m_angularVelocity; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 L1 = impulse * m_a1; - float32 L2 = impulse * m_a2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - - float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; - b2Vec2 Cdot(Cdot1, Cdot2); - - b2Vec2 f1 = m_impulse; - b2Vec2 df = m_K.Solve(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.y = b2Max(m_impulse.y, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.y = b2Min(m_impulse.y, 0.0f); - } - - // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) - float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x; - float32 f2r; - if (m_K.col1.x != 0.0f) - { - f2r = b / m_K.col1.x + f1.x; - } - else - { - f2r = f1.x; - } - - m_impulse.x = f2r; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.y * m_axis; - float32 L1 = df.x * m_s1 + df.y * m_a1; - float32 L2 = df.x * m_s2 + df.y * m_a2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - float32 df; - if (m_K.col1.x != 0.0f) - { - df = - Cdot1 / m_K.col1.x; - } - else - { - df = 0.0f; - } - m_impulse.x += df; - - b2Vec2 P = df * m_perp; - float32 L1 = df * m_s1; - float32 L2 = df * m_s2; - - v1 -= m_invMassA * P; - w1 -= m_invIA * L1; - - v2 += m_invMassB * P; - w2 += m_invIB * L2; - } - - b1->m_linearVelocity = v1; - b1->m_angularVelocity = w1; - b2->m_linearVelocity = v2; - b2->m_angularVelocity = w2; -} - -bool b2LineJoint::SolvePositionConstraints(float32 baumgarte) -{ - B2_NOT_USED(baumgarte); - - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 c1 = b1->m_sweep.c; - float32 a1 = b1->m_sweep.a; - - b2Vec2 c2 = b2->m_sweep.c; - float32 a2 = b2->m_sweep.a; - - // Solve linear limit constraint. - float32 linearError = 0.0f, angularError = 0.0f; - bool active = false; - float32 C2 = 0.0f; - - b2Mat22 R1(a1), R2(a2); - - b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA); - b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB); - b2Vec2 d = c2 + r2 - c1 - r1; - - if (m_enableLimit) - { - m_axis = b2Mul(R1, m_localXAxis1); - - m_a1 = b2Cross(d + r1, m_axis); - m_a2 = b2Cross(r2, m_axis); - - float32 translation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - 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 = 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 = translation - m_upperTranslation; - active = true; - } - } - - m_perp = b2Mul(R1, m_localYAxis1); - - m_s1 = b2Cross(d + r1, m_perp); - m_s2 = b2Cross(r2, m_perp); - - b2Vec2 impulse; - float32 C1; - C1 = b2Dot(m_perp, d); - - linearError = b2Max(linearError, b2Abs(C1)); - angularError = 0.0f; - - if (active) - { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; - float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; - - m_K.col1.Set(k11, k12); - m_K.col2.Set(k12, k22); - - b2Vec2 C; - C.x = C1; - C.y = C2; - - impulse = m_K.Solve(-C); - } - else - { - float32 m1 = m_invMassA, m2 = m_invMassB; - float32 i1 = m_invIA, i2 = m_invIB; - - float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; - - float32 impulse1; - if (k11 != 0.0f) - { - impulse1 = - C1 / k11; - } - else - { - impulse1 = 0.0f; - } - - impulse.x = impulse1; - impulse.y = 0.0f; - } - - b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis; - float32 L1 = impulse.x * m_s1 + impulse.y * m_a1; - float32 L2 = impulse.x * m_s2 + impulse.y * m_a2; - - c1 -= m_invMassA * P; - a1 -= m_invIA * L1; - c2 += m_invMassB * P; - a2 += m_invIB * L2; - - // TODO_ERIN remove need for this. - b1->m_sweep.c = c1; - b1->m_sweep.a = a1; - b2->m_sweep.c = c2; - b2->m_sweep.a = a2; - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2LineJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchor1); -} - -b2Vec2 b2LineJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchor2); -} - -b2Vec2 b2LineJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis); -} - -float32 b2LineJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float32 b2LineJoint::GetJointTranslation() const -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1); - b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2); - b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2LineJoint::GetJointSpeed() const -{ - b2Body* b1 = m_bodyA; - b2Body* b2 = m_bodyB; - - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b1->GetWorldVector(m_localXAxis1); - - b2Vec2 v1 = b1->m_linearVelocity; - b2Vec2 v2 = b2->m_linearVelocity; - float32 w1 = b1->m_angularVelocity; - float32 w2 = b2->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1)); - return speed; -} - -bool b2LineJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2LineJoint::EnableLimit(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; -} - -float32 b2LineJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} - -float32 b2LineJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} - -void b2LineJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; -} - -bool b2LineJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2LineJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2LineJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2LineJoint::SetMaxMotorForce(float32 force) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; -} - -float32 b2LineJoint::GetMotorForce() const -{ - return m_motorImpulse; -} - - - - - diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.cpp index b72ac5f..2b01ef3 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.h index cd1959a..1f000e6 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp index a019888..94e9aab 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -150,7 +150,7 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) m_a2 = b2Cross(r2, m_axis); m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2; - if (m_motorMass > b2_epsilon) + if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } @@ -170,6 +170,10 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; float32 k22 = i1 + i2; + if (k22 == 0.0f) + { + k22 = 1.0f; + } float32 k23 = i1 * m_a1 + i2 * m_a2; float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; @@ -416,6 +420,10 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; float32 k22 = i1 + i2; + if (k22 == 0.0f) + { + k22 = 1.0f; + } float32 k23 = i1 * m_a1 + i2 * m_a2; float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; @@ -438,6 +446,10 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k22 = i1 + i2; + if (k22 == 0.0f) + { + k22 = 1.0f; + } m_K.col1.Set(k11, k12, 0.0f); m_K.col2.Set(k12, k22, 0.0f); @@ -530,9 +542,13 @@ bool b2PrismaticJoint::IsLimitEnabled() const void b2PrismaticJoint::EnableLimit(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2PrismaticJoint::GetLowerLimit() const @@ -548,10 +564,14 @@ float32 b2PrismaticJoint::GetUpperLimit() const void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) { b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = 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 @@ -580,7 +600,7 @@ void b2PrismaticJoint::SetMaxMotorForce(float32 force) m_maxMotorForce = force; } -float32 b2PrismaticJoint::GetMotorForce() const +float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const { - return m_motorImpulse; + return inv_dt * m_motorImpulse; } diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.h index 7a12c5d..7ccdd3e 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2PrismaticJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -129,8 +129,8 @@ public: /// Set the maximum motor force, usually in N. void SetMaxMotorForce(float32 force); - /// Get the current motor force, usually in N. - float32 GetMotorForce() const; + /// Get the current motor force given the inverse time step, usually in N. + float32 GetMotorForce(float32 inv_dt) const; protected: friend class b2Joint; diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.cpp index beb7db8..141c797 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2007 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 @@ -24,20 +24,13 @@ // length1 = norm(p1 - s1) // length2 = norm(p2 - s2) // C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) >= 0 +// C = C0 - (length1 + ratio * length2) // u1 = (p1 - s1) / norm(p1 - s1) // u2 = (p2 - s2) / norm(p2 - s2) // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) -// -// Limit: -// C = maxLength - length -// u = (p - s) / norm(p - s) -// Cdot = -dot(u, v + cross(w, r)) -// K = invMass + invI * cross(r, u)^2 -// 0 <= impulse void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& ga1, const b2Vec2& ga2, @@ -57,8 +50,6 @@ void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2, ratio = r; b2Assert(ratio > b2_epsilon); float32 C = lengthA + ratio * lengthB; - maxLengthA = C - ratio * b2_minPulleyLength; - maxLengthB = (C - b2_minPulleyLength) / ratio; } b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) @@ -74,12 +65,7 @@ b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) m_constant = def->lengthA + m_ratio * def->lengthB; - m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength); - m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio); - m_impulse = 0.0f; - m_limitImpulse1 = 0.0f; - m_limitImpulse2 = 0.0f; } void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) @@ -103,7 +89,7 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) float32 length1 = m_u1.Length(); float32 length2 = m_u2.Length(); - if (length1 > b2_linearSlop) + if (length1 > 10.0f * b2_linearSlop) { m_u1 *= 1.0f / length1; } @@ -112,7 +98,7 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) m_u1.SetZero(); } - if (length2 > b2_linearSlop) + if (length2 > 10.0f * b2_linearSlop) { m_u2 *= 1.0f / length2; } @@ -121,61 +107,28 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) m_u2.SetZero(); } - float32 C = m_constant - length1 - m_ratio * length2; - if (C > 0.0f) - { - m_state = e_inactiveLimit; - m_impulse = 0.0f; - } - else - { - m_state = e_atUpperLimit; - } - - if (length1 < m_maxLength1) - { - m_limitState1 = e_inactiveLimit; - m_limitImpulse1 = 0.0f; - } - else - { - m_limitState1 = e_atUpperLimit; - } - - if (length2 < m_maxLength2) - { - m_limitState2 = e_inactiveLimit; - m_limitImpulse2 = 0.0f; - } - else - { - m_limitState2 = e_atUpperLimit; - } - // Compute effective mass. float32 cr1u1 = b2Cross(r1, m_u1); float32 cr2u2 = b2Cross(r2, m_u2); - m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1; - m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2; - m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2; - b2Assert(m_limitMass1 > b2_epsilon); - b2Assert(m_limitMass2 > b2_epsilon); - b2Assert(m_pulleyMass > b2_epsilon); - m_limitMass1 = 1.0f / m_limitMass1; - m_limitMass2 = 1.0f / m_limitMass2; - m_pulleyMass = 1.0f / m_pulleyMass; + float32 m1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1; + float32 m2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2; + + m_pulleyMass = m1 + m_ratio * m_ratio * m2; + + if (m_pulleyMass > 0.0f) + { + m_pulleyMass = 1.0f / m_pulleyMass; + } if (step.warmStarting) { // Scale impulses to support variable time steps. m_impulse *= step.dtRatio; - m_limitImpulse1 *= step.dtRatio; - m_limitImpulse2 *= step.dtRatio; // Warm starting. - b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1; - b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2; + b2Vec2 P1 = -(m_impulse) * m_u1; + b2Vec2 P2 = (-m_ratio * m_impulse) * m_u2; b1->m_linearVelocity += b1->m_invMass * P1; b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); b2->m_linearVelocity += b2->m_invMass * P2; @@ -184,8 +137,6 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) else { m_impulse = 0.0f; - m_limitImpulse1 = 0.0f; - m_limitImpulse2 = 0.0f; } } @@ -199,16 +150,13 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step) b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - if (m_state == e_atUpperLimit) { b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1); b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2); float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2); float32 impulse = m_pulleyMass * (-Cdot); - float32 oldImpulse = m_impulse; - m_impulse = b2Max(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; + m_impulse += impulse; b2Vec2 P1 = -impulse * m_u1; b2Vec2 P2 = -m_ratio * impulse * m_u2; @@ -217,36 +165,6 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step) b2->m_linearVelocity += b2->m_invMass * P2; b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); } - - if (m_limitState1 == e_atUpperLimit) - { - b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1); - - float32 Cdot = -b2Dot(m_u1, v1); - float32 impulse = -m_limitMass1 * Cdot; - float32 oldImpulse = m_limitImpulse1; - m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse); - impulse = m_limitImpulse1 - oldImpulse; - - b2Vec2 P1 = -impulse * m_u1; - b1->m_linearVelocity += b1->m_invMass * P1; - b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); - } - - if (m_limitState2 == e_atUpperLimit) - { - b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2); - - float32 Cdot = -b2Dot(m_u2, v2); - float32 impulse = -m_limitMass2 * Cdot; - float32 oldImpulse = m_limitImpulse2; - m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse); - impulse = m_limitImpulse2 - oldImpulse; - - b2Vec2 P2 = -impulse * m_u2; - b2->m_linearVelocity += b2->m_invMass * P2; - b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); - } } bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte) @@ -259,117 +177,67 @@ bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte) b2Vec2 s1 = m_groundAnchor1; b2Vec2 s2 = m_groundAnchor2; - float32 linearError = 0.0f; + b2Vec2 r1 = b2Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter()); + b2Vec2 r2 = b2Mul(b2->m_xf.R, m_localAnchor2 - b2->GetLocalCenter()); - if (m_state == e_atUpperLimit) - { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - - b2Vec2 p1 = b1->m_sweep.c + r1; - b2Vec2 p2 = b2->m_sweep.c + r2; - - // Get the pulley axes. - m_u1 = p1 - s1; - m_u2 = p2 - s2; - - float32 length1 = m_u1.Length(); - float32 length2 = m_u2.Length(); - - if (length1 > b2_linearSlop) - { - m_u1 *= 1.0f / length1; - } - else - { - m_u1.SetZero(); - } - - if (length2 > b2_linearSlop) - { - m_u2 *= 1.0f / length2; - } - else - { - m_u2.SetZero(); - } - - float32 C = m_constant - length1 - m_ratio * length2; - linearError = b2Max(linearError, -C); - - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_pulleyMass * C; + b2Vec2 p1 = b1->m_sweep.c + r1; + b2Vec2 p2 = b2->m_sweep.c + r2; - b2Vec2 P1 = -impulse * m_u1; - b2Vec2 P2 = -m_ratio * impulse * m_u2; + // Get the pulley axes. + b2Vec2 u1 = p1 - s1; + b2Vec2 u2 = p2 - s2; - b1->m_sweep.c += b1->m_invMass * P1; - b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); - b2->m_sweep.c += b2->m_invMass * P2; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); + float32 length1 = u1.Length(); + float32 length2 = u2.Length(); - b1->SynchronizeTransform(); - b2->SynchronizeTransform(); + if (length1 > 10.0f * b2_linearSlop) + { + u1 *= 1.0f / length1; + } + else + { + u1.SetZero(); } - if (m_limitState1 == e_atUpperLimit) + if (length2 > 10.0f * b2_linearSlop) + { + u2 *= 1.0f / length2; + } + else { - b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); - b2Vec2 p1 = b1->m_sweep.c + r1; - - m_u1 = p1 - s1; - float32 length1 = m_u1.Length(); - - if (length1 > b2_linearSlop) - { - m_u1 *= 1.0f / length1; - } - else - { - m_u1.SetZero(); - } - - float32 C = m_maxLength1 - length1; - linearError = b2Max(linearError, -C); - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_limitMass1 * C; + u2.SetZero(); + } - b2Vec2 P1 = -impulse * m_u1; - b1->m_sweep.c += b1->m_invMass * P1; - b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); + // Compute effective mass. + float32 cr1u1 = b2Cross(r1, u1); + float32 cr2u2 = b2Cross(r2, u2); - b1->SynchronizeTransform(); - } + float32 m1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1; + float32 m2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2; + + float32 mass = m1 + m_ratio * m_ratio * m2; - if (m_limitState2 == e_atUpperLimit) + if (mass > 0.0f) { - b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); - b2Vec2 p2 = b2->m_sweep.c + r2; - - m_u2 = p2 - s2; - float32 length2 = m_u2.Length(); - - if (length2 > b2_linearSlop) - { - m_u2 *= 1.0f / length2; - } - else - { - m_u2.SetZero(); - } - - float32 C = m_maxLength2 - length2; - linearError = b2Max(linearError, -C); - C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - float32 impulse = -m_limitMass2 * C; - - b2Vec2 P2 = -impulse * m_u2; - b2->m_sweep.c += b2->m_invMass * P2; - b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); - - b2->SynchronizeTransform(); + mass = 1.0f / mass; } + float32 C = m_constant - length1 - m_ratio * length2; + float32 linearError = b2Abs(C); + + float32 impulse = -mass * C; + + b2Vec2 P1 = -impulse * u1; + b2Vec2 P2 = -m_ratio * impulse * u2; + + b1->m_sweep.c += b1->m_invMass * P1; + b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); + b2->m_sweep.c += b2->m_invMass * P2; + b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); + + b1->SynchronizeTransform(); + b2->SynchronizeTransform(); + return linearError < b2_linearSlop; } diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.h index 189decb..2e0af79 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -24,8 +24,7 @@ const float32 b2_minPulleyLength = 2.0f; /// Pulley joint definition. This requires two ground anchors, -/// two dynamic body anchor points, max lengths for each side, -/// and a pulley ratio. +/// two dynamic body anchor points, and a pulley ratio. struct b2PulleyJointDef : public b2JointDef { b2PulleyJointDef() @@ -36,9 +35,7 @@ struct b2PulleyJointDef : public b2JointDef localAnchorA.Set(-1.0f, 0.0f); localAnchorB.Set(1.0f, 0.0f); lengthA = 0.0f; - maxLengthA = 0.0f; lengthB = 0.0f; - maxLengthB = 0.0f; ratio = 1.0f; collideConnected = true; } @@ -64,15 +61,9 @@ struct b2PulleyJointDef : public b2JointDef /// The a reference length for the segment attached to bodyA. float32 lengthA; - /// The maximum length of the segment attached to bodyA. - float32 maxLengthA; - /// The a reference length for the segment attached to bodyB. float32 lengthB; - /// The maximum length of the segment attached to bodyB. - float32 maxLengthB; - /// The pulley ratio, used to simulate a block-and-tackle. float32 ratio; }; @@ -81,8 +72,10 @@ struct b2PulleyJointDef : public b2JointDef /// The pulley supports a ratio such that: /// length1 + ratio * length2 <= constant /// Yes, the force transmitted is scaled by the ratio. -/// The pulley also enforces a maximum length limit on both sides. This is -/// useful to prevent one side of the pulley hitting the top. +/// Warning: the pulley joint can get a bit squirrelly by itself. They often +/// work better when combined with prismatic joints. You should also cover the +/// the anchor points with static shapes to prevent one side from going to +/// zero length. class b2PulleyJoint : public b2Joint { public: @@ -127,22 +120,11 @@ protected: float32 m_constant; float32 m_ratio; - float32 m_maxLength1; - float32 m_maxLength2; - // Effective masses float32 m_pulleyMass; - float32 m_limitMass1; - float32 m_limitMass2; // Impulses for accumulation/warm starting. float32 m_impulse; - float32 m_limitImpulse1; - float32 m_limitImpulse2; - - b2LimitState m_state; - b2LimitState m_limitState1; - b2LimitState m_limitState2; }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp index f984526..5807cb5 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -214,7 +214,8 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0f) { - b2Vec2 reduced = m_mass.Solve22(-Cdot1); + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.col3.x, m_mass.col3.y); + b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; @@ -222,13 +223,18 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) 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 reduced = m_mass.Solve22(-Cdot1); + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.col3.x, m_mass.col3.y); + b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; @@ -236,6 +242,10 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) m_impulse.y += reduced.y; m_impulse.z = 0.0f; } + else + { + m_impulse += impulse; + } } b2Vec2 P(impulse.x, impulse.y); @@ -427,9 +437,9 @@ void b2RevoluteJoint::EnableMotor(bool flag) m_enableMotor = flag; } -float32 b2RevoluteJoint::GetMotorTorque() const +float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const { - return m_motorImpulse; + return inv_dt * m_motorImpulse; } void b2RevoluteJoint::SetMotorSpeed(float32 speed) @@ -453,9 +463,13 @@ bool b2RevoluteJoint::IsLimitEnabled() const void b2RevoluteJoint::EnableLimit(bool flag) { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } float32 b2RevoluteJoint::GetLowerLimit() const @@ -471,8 +485,13 @@ float32 b2RevoluteJoint::GetUpperLimit() const void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) { b2Assert(lower <= upper); - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerAngle = lower; - m_upperAngle = 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; + } } diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.h index c0180be..5214c83 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2RevoluteJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -93,9 +93,6 @@ public: b2Vec2 GetAnchorA() const; b2Vec2 GetAnchorB() const; - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - /// Get the current joint angle in radians. float32 GetJointAngle() const; @@ -132,8 +129,17 @@ public: /// Set the maximum motor torque, usually in N-m. void SetMaxMotorTorque(float32 torque); - /// Get the current motor torque, usually in N-m. - float32 GetMotorTorque() const; + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float32 inv_dt) const; + + /// Get the reaction torque due to the joint limit given the inverse time step. + /// Unit is N*m. + float32 GetReactionTorque(float32 inv_dt) const; + + /// Get the current motor torque given the inverse time step. + /// Unit is N*m. + float32 GetMotorTorque(float32 inv_dt) const; protected: diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.cpp new file mode 100644 index 0000000..813b356 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.cpp @@ -0,0 +1,197 @@ +/* +* Copyright (c) 2007-2010 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 + + +// Limit: +// C = norm(pB - pA) - L +// u = (pB - pA) / norm(pB - pA) +// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) +// J = [-u -cross(rA, u) u cross(rB, u)] +// K = J * invM * JT +// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 + +b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) +: b2Joint(def) +{ + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + + m_maxLength = def->maxLength; + + m_mass = 0.0f; + m_impulse = 0.0f; + m_state = e_inactiveLimit; + m_length = 0.0f; +} + +void b2RopeJoint::InitVelocityConstraints(const b2TimeStep& step) +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + m_rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); + m_rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + + // Rope axis + m_u = bB->m_sweep.c + m_rB - bA->m_sweep.c - m_rA; + + m_length = m_u.Length(); + + float32 C = m_length - m_maxLength; + if (C > 0.0f) + { + m_state = e_atUpperLimit; + } + else + { + m_state = e_inactiveLimit; + } + + if (m_length > b2_linearSlop) + { + m_u *= 1.0f / m_length; + } + else + { + m_u.SetZero(); + m_mass = 0.0f; + m_impulse = 0.0f; + return; + } + + // Compute effective mass. + float32 crA = b2Cross(m_rA, m_u); + float32 crB = b2Cross(m_rB, m_u); + float32 invMass = bA->m_invMass + bA->m_invI * crA * crA + bB->m_invMass + bB->m_invI * crB * crB; + + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (step.warmStarting) + { + // Scale the impulse to support a variable time step. + m_impulse *= step.dtRatio; + + b2Vec2 P = m_impulse * m_u; + bA->m_linearVelocity -= bA->m_invMass * P; + bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P); + bB->m_linearVelocity += bB->m_invMass * P; + bB->m_angularVelocity += bB->m_invI * b2Cross(m_rB, P); + } + else + { + m_impulse = 0.0f; + } +} + +void b2RopeJoint::SolveVelocityConstraints(const b2TimeStep& step) +{ + B2_NOT_USED(step); + + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vA = bA->m_linearVelocity + b2Cross(bA->m_angularVelocity, m_rA); + b2Vec2 vB = bB->m_linearVelocity + b2Cross(bB->m_angularVelocity, m_rB); + float32 C = m_length - m_maxLength; + float32 Cdot = b2Dot(m_u, vB - vA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += step.inv_dt * C; + } + + float32 impulse = -m_mass * Cdot; + float32 oldImpulse = m_impulse; + m_impulse = b2Min(0.0f, m_impulse + impulse); + impulse = m_impulse - oldImpulse; + + b2Vec2 P = impulse * m_u; + bA->m_linearVelocity -= bA->m_invMass * P; + bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P); + bB->m_linearVelocity += bB->m_invMass * P; + bB->m_angularVelocity += bB->m_invI * b2Cross(m_rB, P); +} + +bool b2RopeJoint::SolvePositionConstraints(float32 baumgarte) +{ + B2_NOT_USED(baumgarte); + + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); + b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); + + b2Vec2 u = bB->m_sweep.c + rB - bA->m_sweep.c - rA; + + float32 length = u.Normalize(); + float32 C = length - m_maxLength; + + C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); + + float32 impulse = -m_mass * C; + b2Vec2 P = impulse * u; + + bA->m_sweep.c -= bA->m_invMass * P; + bA->m_sweep.a -= bA->m_invI * b2Cross(rA, P); + bB->m_sweep.c += bB->m_invMass * P; + bB->m_sweep.a += bB->m_invI * b2Cross(rB, P); + + bA->SynchronizeTransform(); + bB->SynchronizeTransform(); + + return length - m_maxLength < b2_linearSlop; +} + +b2Vec2 b2RopeJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2RopeJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const +{ + b2Vec2 F = (inv_dt * m_impulse) * m_u; + return F; +} + +float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const +{ + B2_NOT_USED(inv_dt); + return 0.0f; +} + +float32 b2RopeJoint::GetMaxLength() const +{ + return m_maxLength; +} + +b2LimitState b2RopeJoint::GetLimitState() const +{ + return m_state; +} diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.h new file mode 100644 index 0000000..b297ab3 --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2RopeJoint.h @@ -0,0 +1,99 @@ +/* +* Copyright (c) 2006-2010 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_ROPE_JOINT_H +#define B2_ROPE_JOINT_H + +#include + +/// Rope joint definition. This requires two body anchor points and +/// a maximum lengths. +/// Note: by default the connected objects will not collide. +/// see collideConnected in b2JointDef. +struct b2RopeJointDef : public b2JointDef +{ + b2RopeJointDef() + { + type = e_ropeJoint; + localAnchorA.Set(-1.0f, 0.0f); + localAnchorB.Set(1.0f, 0.0f); + maxLength = 0.0f; + } + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum length of the rope. + /// Warning: this must be larger than b2_linearSlop or + /// the joint will have no effect. + float32 maxLength; +}; + +/// A rope joint enforces a maximum distance between two points +/// on two bodies. It has no other effect. +/// Warning: if you attempt to change the maximum length during +/// the simulation you will get some non-physical behavior. +/// A model that would allow you to dynamically modify the length +/// would have some sponginess, so I chose not to implement it +/// that way. See b2DistanceJoint if you want to dynamically +/// control length. +class b2RopeJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const; + b2Vec2 GetAnchorB() const; + + b2Vec2 GetReactionForce(float32 inv_dt) const; + float32 GetReactionTorque(float32 inv_dt) const; + + /// Get the maximum length of the rope. + float32 GetMaxLength() const; + + b2LimitState GetLimitState() const; + +protected: + + friend class b2Joint; + b2RopeJoint(const b2RopeJointDef* data); + + void InitVelocityConstraints(const b2TimeStep& step); + void SolveVelocityConstraints(const b2TimeStep& step); + bool SolvePositionConstraints(float32 baumgarte); + + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + + float32 m_maxLength; + float32 m_length; + + // Jacobian info + b2Vec2 m_u, m_rA, m_rB; + + // Effective mass + float32 m_mass; + + // Impulses for accumulation/warm starting. + float32 m_impulse; + + b2LimitState m_state; +}; + +#endif diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.cpp index 49b5513..f899d13 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.h index 4e63b6a..216b171 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2WeldJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.cpp new file mode 100644 index 0000000..292a9dc --- /dev/null +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.cpp @@ -0,0 +1,396 @@ +/* +* Copyright (c) 2006-2007 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 + +// Linear constraint (point-to-line) +// d = pB - pA = xB + rB - xA - rA +// C = dot(ay, d) +// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + +// Spring linear constraint +// C = dot(ax, d) +// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) +// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + +// Motor rotational constraint +// Cdot = wB - wA +// J = [0 0 -1 0 0 1] + +void b2WheelJointDef::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); +} + +b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) +: b2Joint(def) +{ + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); + + m_mass = 0.0f; + m_impulse = 0.0f; + m_motorMass = 0.0; + m_motorImpulse = 0.0f; + m_springMass = 0.0f; + m_springImpulse = 0.0f; + + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableMotor = def->enableMotor; + + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; + + m_bias = 0.0f; + m_gamma = 0.0f; + + m_ax.SetZero(); + m_ay.SetZero(); +} + +void b2WheelJoint::InitVelocityConstraints(const b2TimeStep& step) +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + m_localCenterA = bA->GetLocalCenter(); + m_localCenterB = bB->GetLocalCenter(); + + b2Transform xfA = bA->GetTransform(); + b2Transform xfB = bB->GetTransform(); + + // Compute the effective masses. + b2Vec2 rA = b2Mul(xfA.R, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(xfB.R, m_localAnchorB - m_localCenterB); + b2Vec2 d = bB->m_sweep.c + rB - bA->m_sweep.c - rA; + + m_invMassA = bA->m_invMass; + m_invIA = bA->m_invI; + m_invMassB = bB->m_invMass; + m_invIB = bB->m_invI; + + // Point to line constraint + { + m_ay = b2Mul(xfA.R, m_localYAxisA); + m_sAy = b2Cross(d + rA, m_ay); + m_sBy = b2Cross(rB, m_ay); + + m_mass = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; + + if (m_mass > 0.0f) + { + m_mass = 1.0f / m_mass; + } + } + + // Spring constraint + m_springMass = 0.0f; + if (m_frequencyHz > 0.0f) + { + m_ax = b2Mul(xfA.R, m_localXAxisA); + m_sAx = b2Cross(d + rA, m_ax); + m_sBx = b2Cross(rB, m_ax); + + float32 invMass = m_invMassA + m_invMassB + m_invIA * m_sAx * m_sAx + m_invIB * m_sBx * m_sBx; + + if (invMass > 0.0f) + { + m_springMass = 1.0f / invMass; + + float32 C = b2Dot(d, m_ax); + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 d = 2.0f * m_springMass * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m_springMass * omega * omega; + + // magic formulas + m_gamma = step.dt * (d + step.dt * k); + if (m_gamma > 0.0f) + { + m_gamma = 1.0f / m_gamma; + } + + m_bias = C * step.dt * k * m_gamma; + + m_springMass = invMass + m_gamma; + if (m_springMass > 0.0f) + { + m_springMass = 1.0f / m_springMass; + } + } + } + else + { + m_springImpulse = 0.0f; + m_springMass = 0.0f; + } + + // Rotational motor + if (m_enableMotor) + { + m_motorMass = m_invIA + m_invIB; + if (m_motorMass > 0.0f) + { + m_motorMass = 1.0f / m_motorMass; + } + } + else + { + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + } + + if (step.warmStarting) + { + // Account for variable time step. + m_impulse *= step.dtRatio; + m_springImpulse *= step.dtRatio; + m_motorImpulse *= step.dtRatio; + + b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; + float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; + float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; + + bA->m_linearVelocity -= m_invMassA * P; + bA->m_angularVelocity -= m_invIA * LA; + + bB->m_linearVelocity += m_invMassB * P; + bB->m_angularVelocity += m_invIB * LB; + } + else + { + m_impulse = 0.0f; + m_springImpulse = 0.0f; + m_motorImpulse = 0.0f; + } +} + +void b2WheelJoint::SolveVelocityConstraints(const b2TimeStep& step) +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 vA = bA->m_linearVelocity; + float32 wA = bA->m_angularVelocity; + b2Vec2 vB = bB->m_linearVelocity; + float32 wB = bB->m_angularVelocity; + + // Solve spring constraint + { + float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; + float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); + m_springImpulse += impulse; + + b2Vec2 P = impulse * m_ax; + float32 LA = impulse * m_sAx; + float32 LB = impulse * m_sBx; + + vA -= m_invMassA * P; + wA -= m_invIA * LA; + + vB += m_invMassB * P; + wB += m_invIB * LB; + } + + // Solve rotational motor constraint + { + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; + + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= m_invIA * impulse; + wB += m_invIB * impulse; + } + + // Solve point to line constraint + { + float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; + float32 impulse = m_mass * (-Cdot); + m_impulse += impulse; + + b2Vec2 P = impulse * m_ay; + float32 LA = impulse * m_sAy; + float32 LB = impulse * m_sBy; + + vA -= m_invMassA * P; + wA -= m_invIA * LA; + + vB += m_invMassB * P; + wB += m_invIB * LB; + } + + bA->m_linearVelocity = vA; + bA->m_angularVelocity = wA; + bB->m_linearVelocity = vB; + bB->m_angularVelocity = wB; +} + +bool b2WheelJoint::SolvePositionConstraints(float32 baumgarte) +{ + B2_NOT_USED(baumgarte); + + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 xA = bA->m_sweep.c; + float32 angleA = bA->m_sweep.a; + + b2Vec2 xB = bB->m_sweep.c; + float32 angleB = bB->m_sweep.a; + + b2Mat22 RA(angleA), RB(angleB); + + b2Vec2 rA = b2Mul(RA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(RB, m_localAnchorB - m_localCenterB); + b2Vec2 d = xB + rB - xA - rA; + + b2Vec2 ay = b2Mul(RA, m_localYAxisA); + + float32 sAy = b2Cross(d + rA, ay); + float32 sBy = b2Cross(rB, ay); + + float32 C = b2Dot(d, ay); + + float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; + + float32 impulse; + if (k != 0.0f) + { + impulse = - C / k; + } + else + { + impulse = 0.0f; + } + + b2Vec2 P = impulse * ay; + float32 LA = impulse * sAy; + float32 LB = impulse * sBy; + + xA -= m_invMassA * P; + angleA -= m_invIA * LA; + xB += m_invMassB * P; + angleB += m_invIB * LB; + + // TODO_ERIN remove need for this. + bA->m_sweep.c = xA; + bA->m_sweep.a = angleA; + bB->m_sweep.c = xB; + bB->m_sweep.a = angleB; + bA->SynchronizeTransform(); + bB->SynchronizeTransform(); + + return b2Abs(C) <= b2_linearSlop; +} + +b2Vec2 b2WheelJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2WheelJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const +{ + return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); +} + +float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +float32 b2WheelJoint::GetJointTranslation() const +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + + b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); + + float32 translation = b2Dot(d, axis); + return translation; +} + +float32 b2WheelJoint::GetJointSpeed() const +{ + float32 wA = m_bodyA->m_angularVelocity; + float32 wB = m_bodyB->m_angularVelocity; + return wB - wA; +} + +bool b2WheelJoint::IsMotorEnabled() const +{ + return m_enableMotor; +} + +void b2WheelJoint::EnableMotor(bool flag) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; +} + +void b2WheelJoint::SetMotorSpeed(float32 speed) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; +} + +void b2WheelJoint::SetMaxMotorTorque(float32 torque) +{ + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; +} + +float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + + + + + diff --git a/libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.h b/libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.h similarity index 54% rename from libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.h rename to libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.h index 803e4c1..5c9a90e 100644 --- a/libs/box2d/src/Box2D/Dynamics/Joints/b2LineJoint.h +++ b/libs/box2d/src/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2011 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 @@ -16,31 +16,30 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B2_LINE_JOINT_H -#define B2_LINE_JOINT_H +#ifndef B2_WHEEL_JOINT_H +#define B2_WHEEL_JOINT_H #include -/// Line joint definition. This requires defining a line of +/// Wheel joint definition. This requires defining a line of /// motion using an axis and an anchor point. The definition uses local /// anchor points and a local axis so that the initial configuration /// can violate the constraint slightly. The joint translation is zero /// when the local anchor points coincide in world space. Using local /// anchors and a local axis helps when saving and loading a game. -struct b2LineJointDef : public b2JointDef +struct b2WheelJointDef : public b2JointDef { - b2LineJointDef() + b2WheelJointDef() { - type = e_lineJoint; + type = e_wheelJoint; localAnchorA.SetZero(); localAnchorB.SetZero(); localAxisA.Set(1.0f, 0.0f); - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; enableMotor = false; - maxMotorForce = 0.0f; + maxMotorTorque = 0.0f; motorSpeed = 0.0f; + frequencyHz = 2.0f; + dampingRatio = 0.7f; } /// Initialize the bodies, anchors, axis, and reference angle using the world @@ -56,30 +55,28 @@ struct b2LineJointDef : public b2JointDef /// The local translation axis in body1. b2Vec2 localAxisA; - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float32 lowerTranslation; - - /// The upper translation limit, usually in meters. - float32 upperTranslation; - /// Enable/disable the joint motor. bool enableMotor; /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; + float32 maxMotorTorque; /// The desired motor speed in radians per second. float32 motorSpeed; + + /// Suspension frequency, zero indicates no suspension + float32 frequencyHz; + + /// Suspension damping ratio, one indicates critical damping + float32 dampingRatio; }; -/// A line joint. This joint provides two degrees of freedom: translation +/// A wheel joint. This joint provides two degrees of freedom: translation /// along an axis fixed in body1 and rotation in the plane. You can use a /// joint limit to restrict the range of motion and a joint motor to drive -/// the motion or to model joint friction. -class b2LineJoint : public b2Joint +/// the rotation or to model rotational friction. +/// This joint is designed for vehicle suspensions. +class b2WheelJoint : public b2Joint { public: b2Vec2 GetAnchorA() const; @@ -94,77 +91,96 @@ public: /// Get the current joint translation speed, usually in meters per second. float32 GetJointSpeed() const; - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; - - /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; - - /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); - /// Is the joint motor enabled? bool IsMotorEnabled() const; /// Enable/disable the joint motor. void EnableMotor(bool flag); - /// Set the motor speed, usually in meters per second. + /// Set the motor speed, usually in radians per second. void SetMotorSpeed(float32 speed); - /// Get the motor speed, usually in meters per second. + /// Get the motor speed, usually in radians per second. float32 GetMotorSpeed() const; - /// Set/Get the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const; + /// Set/Get the maximum motor force, usually in N-m. + void SetMaxMotorTorque(float32 torque); + float32 GetMaxMotorTorque() const; + + /// Get the current motor torque given the inverse time step, usually in N-m. + float32 GetMotorTorque(float32 inv_dt) const; - /// Get the current motor force, usually in N. - float32 GetMotorForce() const; + /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring. + void SetSpringFrequencyHz(float32 hz); + float32 GetSpringFrequencyHz() const; + + /// Set/Get the spring damping ratio + void SetSpringDampingRatio(float32 ratio); + float32 GetSpringDampingRatio() const; protected: friend class b2Joint; - b2LineJoint(const b2LineJointDef* def); + b2WheelJoint(const b2WheelJointDef* def); void InitVelocityConstraints(const b2TimeStep& step); void SolveVelocityConstraints(const b2TimeStep& step); bool SolvePositionConstraints(float32 baumgarte); - b2Vec2 m_localAnchor1; - b2Vec2 m_localAnchor2; - b2Vec2 m_localXAxis1; - b2Vec2 m_localYAxis1; - - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; - b2Mat22 m_K; - b2Vec2 m_impulse; + b2Vec2 m_ax, m_ay; + float32 m_sAx, m_sBx; + float32 m_sAy, m_sBy; - float32 m_motorMass; // effective mass for motor/limit translational constraint. + float32 m_mass; + float32 m_impulse; + float32 m_motorMass; float32 m_motorImpulse; + float32 m_springMass; + float32 m_springImpulse; - float32 m_lowerTranslation; - float32 m_upperTranslation; - float32 m_maxMotorForce; + float32 m_maxMotorTorque; float32 m_motorSpeed; + float32 m_frequencyHz; + float32 m_dampingRatio; + float32 m_bias; + float32 m_gamma; - bool m_enableLimit; bool m_enableMotor; - b2LimitState m_limitState; }; -inline float32 b2LineJoint::GetMotorSpeed() const +inline float32 b2WheelJoint::GetMotorSpeed() const { return m_motorSpeed; } +inline float32 b2WheelJoint::GetMaxMotorTorque() const +{ + return m_maxMotorTorque; +} + +inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) +{ + m_frequencyHz = hz; +} + +inline float32 b2WheelJoint::GetSpringFrequencyHz() const +{ + return m_frequencyHz; +} + +inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) +{ + m_dampingRatio = ratio; +} + +inline float32 b2WheelJoint::GetSpringDampingRatio() const +{ + return m_dampingRatio; +} + #endif diff --git a/libs/box2d/src/Box2D/Dynamics/b2Body.cpp b/libs/box2d/src/Box2D/Dynamics/b2Body.cpp index 4b88651..da44e3b 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Body.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2Body.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2007 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 @@ -28,7 +28,6 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) b2Assert(bd->linearVelocity.IsValid()); b2Assert(b2IsValid(bd->angle)); b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f); b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); @@ -74,6 +73,7 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) m_linearDamping = bd->linearDamping; m_angularDamping = bd->angularDamping; + m_gravityScale = bd->gravityScale; m_force.SetZero(); m_torque = 0.0f; @@ -130,9 +130,9 @@ void b2Body::SetType(b2BodyType type) m_torque = 0.0f; // Since the body type changed, we need to flag contacts for filtering. - for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next) + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - ce->contact->FlagForFiltering(); + f->Refilter(); } } @@ -153,7 +153,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) if (m_flags & e_activeFlag) { b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxy(broadPhase, m_xf); + fixture->CreateProxies(broadPhase, m_xf); } fixture->m_next = m_fixtureList; @@ -235,13 +235,8 @@ void b2Body::DestroyFixture(b2Fixture* fixture) if (m_flags & e_activeFlag) { - b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy); b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxy(broadPhase); - } - else - { - b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy); + fixture->DestroyProxies(broadPhase); } fixture->Destroy(allocator); @@ -428,6 +423,8 @@ void b2Body::SynchronizeFixtures() void b2Body::SetActive(bool flag) { + b2Assert(m_world->IsLocked() == false); + if (flag == IsActive()) { return; @@ -441,7 +438,7 @@ void b2Body::SetActive(bool flag) b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - f->CreateProxy(broadPhase, m_xf); + f->CreateProxies(broadPhase, m_xf); } // Contacts are created the next time step. @@ -454,7 +451,7 @@ void b2Body::SetActive(bool flag) b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { - f->DestroyProxy(broadPhase); + f->DestroyProxies(broadPhase); } // Destroy the attached contacts. diff --git a/libs/box2d/src/Box2D/Dynamics/b2Body.h b/libs/box2d/src/Box2D/Dynamics/b2Body.h index f2f915f..48a458d 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Body.h +++ b/libs/box2d/src/Box2D/Dynamics/b2Body.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -40,7 +40,10 @@ enum b2BodyType { b2_staticBody = 0, b2_kinematicBody, - b2_dynamicBody, + b2_dynamicBody + + // TODO_ERIN + //b2_bulletBody, }; /// A body definition holds all the data needed to construct a rigid body. @@ -63,7 +66,7 @@ struct b2BodyDef bullet = false; type = b2_staticBody; active = true; - inertiaScale = 1.0f; + gravityScale = 1.0f; } /// The body type: static, kinematic, or dynamic. @@ -115,8 +118,8 @@ struct b2BodyDef /// Use this to store application specific body data. void* userData; - /// Experimental: scales the inertia tensor. - float32 inertiaScale; + /// Scale the gravity applied to this body. + float32 gravityScale; }; /// A rigid body. These are created via b2World::CreateBody. @@ -281,6 +284,12 @@ public: /// Set the angular damping of the body. void SetAngularDamping(float32 angularDamping); + /// Get the gravity scale of the body. + float32 GetGravityScale() const; + + /// Set the angular damping of the body. + void SetGravityScale(float32 scale); + /// Set the type of this body. This may alter the mass and velocity. void SetType(b2BodyType type); @@ -368,17 +377,17 @@ private: friend class b2Island; friend class b2ContactManager; friend class b2ContactSolver; - friend class b2TOISolver; friend class b2DistanceJoint; friend class b2GearJoint; - friend class b2LineJoint; + friend class b2WheelJoint; friend class b2MouseJoint; friend class b2PrismaticJoint; friend class b2PulleyJoint; friend class b2RevoluteJoint; friend class b2WeldJoint; friend class b2FrictionJoint; + friend class b2RopeJoint; // m_flags enum @@ -389,7 +398,7 @@ private: e_bulletFlag = 0x0008, e_fixedRotationFlag = 0x0010, e_activeFlag = 0x0020, - e_toiFlag = 0x0040, + e_toiFlag = 0x0040 }; b2Body(const b2BodyDef* bd, b2World* world); @@ -436,6 +445,7 @@ private: float32 m_linearDamping; float32 m_angularDamping; + float32 m_gravityScale; float32 m_sleepTime; @@ -579,6 +589,16 @@ inline void b2Body::SetAngularDamping(float32 angularDamping) m_angularDamping = angularDamping; } +inline float32 b2Body::GetGravityScale() const +{ + return m_gravityScale; +} + +inline void b2Body::SetGravityScale(float32 scale) +{ + m_gravityScale = scale; +} + inline void b2Body::SetBullet(bool flag) { if (flag) @@ -780,10 +800,10 @@ inline void b2Body::SynchronizeTransform() m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter); } -inline void b2Body::Advance(float32 t) +inline void b2Body::Advance(float32 alpha) { // Advance to the new safe time. - m_sweep.Advance(t); + m_sweep.Advance(alpha); m_sweep.c = m_sweep.c0; m_sweep.a = m_sweep.a0; SynchronizeTransform(); diff --git a/libs/box2d/src/Box2D/Dynamics/b2ContactManager.cpp b/libs/box2d/src/Box2D/Dynamics/b2ContactManager.cpp index d8d96dd..cde07a3 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2ContactManager.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2ContactManager.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -110,6 +110,8 @@ void b2ContactManager::Collide() { b2Fixture* fixtureA = c->GetFixtureA(); b2Fixture* fixtureB = c->GetFixtureB(); + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); @@ -144,8 +146,8 @@ void b2ContactManager::Collide() c->m_flags &= ~b2Contact::e_filterFlag; } - int32 proxyIdA = fixtureA->m_proxyId; - int32 proxyIdB = fixtureB->m_proxyId; + int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; + int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. @@ -170,8 +172,14 @@ void b2ContactManager::FindNewContacts() void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) { - b2Fixture* fixtureA = (b2Fixture*)proxyUserDataA; - b2Fixture* fixtureB = (b2Fixture*)proxyUserDataB; + b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; + b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; + + b2Fixture* fixtureA = proxyA->fixture; + b2Fixture* fixtureB = proxyB->fixture; + + int32 indexA = proxyA->childIndex; + int32 indexB = proxyB->childIndex; b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); @@ -190,13 +198,16 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) { b2Fixture* fA = edge->contact->GetFixtureA(); b2Fixture* fB = edge->contact->GetFixtureB(); - if (fA == fixtureA && fB == fixtureB) + int32 iA = edge->contact->GetChildIndexA(); + int32 iB = edge->contact->GetChildIndexB(); + + if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } - if (fA == fixtureB && fB == fixtureA) + if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; @@ -219,11 +230,13 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) } // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, fixtureB, m_allocator); + b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); // Contact creation may swap fixtures. fixtureA = c->GetFixtureA(); fixtureB = c->GetFixtureB(); + indexA = c->GetChildIndexA(); + indexB = c->GetChildIndexB(); bodyA = fixtureA->GetBody(); bodyB = fixtureB->GetBody(); diff --git a/libs/box2d/src/Box2D/Dynamics/b2ContactManager.h b/libs/box2d/src/Box2D/Dynamics/b2ContactManager.h index fcba54a..dc1f77f 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2ContactManager.h +++ b/libs/box2d/src/Box2D/Dynamics/b2ContactManager.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 diff --git a/libs/box2d/src/Box2D/Dynamics/b2Fixture.cpp b/libs/box2d/src/Box2D/Dynamics/b2Fixture.cpp index 31355b5..e70606d 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Fixture.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2Fixture.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -18,29 +18,26 @@ #include #include +#include #include +#include #include +#include #include #include #include - b2Fixture::b2Fixture() { m_userData = NULL; m_body = NULL; m_next = NULL; - m_proxyId = b2BroadPhase::e_nullProxy; + m_proxies = NULL; + m_proxyCount = 0; m_shape = NULL; m_density = 0.0f; } -b2Fixture::~b2Fixture() -{ - b2Assert(m_shape == NULL); - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); -} - void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) { m_userData = def->userData; @@ -56,13 +53,28 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur m_shape = def->shape->Clone(allocator); + // Reserve proxy space + int32 childCount = m_shape->GetChildCount(); + m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); + for (int32 i = 0; i < childCount; ++i) + { + m_proxies[i].fixture = NULL; + m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; + } + m_proxyCount = 0; + m_density = def->density; } void b2Fixture::Destroy(b2BlockAllocator* allocator) { - // The proxy must be destroyed before calling this. - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); + // The proxies must be destroyed before calling this. + b2Assert(m_proxyCount == 0); + + // Free the proxy array. + int32 childCount = m_shape->GetChildCount(); + allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); + m_proxies = NULL; // Free the child shape. switch (m_shape->m_type) @@ -75,6 +87,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) } break; + case b2Shape::e_edge: + { + b2EdgeShape* s = (b2EdgeShape*)m_shape; + s->~b2EdgeShape(); + allocator->Free(s, sizeof(b2EdgeShape)); + } + break; + case b2Shape::e_polygon: { b2PolygonShape* s = (b2PolygonShape*)m_shape; @@ -83,6 +103,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) } break; + case b2Shape::e_loop: + { + b2LoopShape* s = (b2LoopShape*)m_shape; + s->~b2LoopShape(); + allocator->Free(s, sizeof(b2LoopShape)); + } + break; + default: b2Assert(false); break; @@ -91,50 +119,69 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) m_shape = NULL; } -void b2Fixture::CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf) +void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) { - b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); + b2Assert(m_proxyCount == 0); + + // Create proxies in the broad-phase. + m_proxyCount = m_shape->GetChildCount(); - // Create proxy in the broad-phase. - m_shape->ComputeAABB(&m_aabb, xf); - m_proxyId = broadPhase->CreateProxy(m_aabb, this); + for (int32 i = 0; i < m_proxyCount; ++i) + { + b2FixtureProxy* proxy = m_proxies + i; + m_shape->ComputeAABB(&proxy->aabb, xf, i); + proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); + proxy->fixture = this; + proxy->childIndex = i; + } } -void b2Fixture::DestroyProxy(b2BroadPhase* broadPhase) +void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) { - if (m_proxyId == b2BroadPhase::e_nullProxy) + // Destroy proxies in the broad-phase. + for (int32 i = 0; i < m_proxyCount; ++i) { - return; + b2FixtureProxy* proxy = m_proxies + i; + broadPhase->DestroyProxy(proxy->proxyId); + proxy->proxyId = b2BroadPhase::e_nullProxy; } - // Destroy proxy in the broad-phase. - broadPhase->DestroyProxy(m_proxyId); - m_proxyId = b2BroadPhase::e_nullProxy; + m_proxyCount = 0; } void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) { - if (m_proxyId == b2BroadPhase::e_nullProxy) + if (m_proxyCount == 0) { return; } - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1); - m_shape->ComputeAABB(&aabb2, transform2); + for (int32 i = 0; i < m_proxyCount; ++i) + { + b2FixtureProxy* proxy = m_proxies + i; + + // Compute an AABB that covers the swept shape (may miss some rotation effect). + b2AABB aabb1, aabb2; + m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); + m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - m_aabb.Combine(aabb1, aabb2); + proxy->aabb.Combine(aabb1, aabb2); - b2Vec2 displacement = transform2.position - transform1.position; + b2Vec2 displacement = transform2.position - transform1.position; - broadPhase->MoveProxy(m_proxyId, m_aabb, displacement); + broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); + } } void b2Fixture::SetFilterData(const b2Filter& filter) { m_filter = filter; + Refilter(); +} + +void b2Fixture::Refilter() +{ if (m_body == NULL) { return; @@ -154,6 +201,20 @@ void b2Fixture::SetFilterData(const b2Filter& filter) edge = edge->next; } + + b2World* world = m_body->GetWorld(); + + if (world == NULL) + { + return; + } + + // Touch each proxy so that new pairs may be created + b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; + for (int32 i = 0; i < m_proxyCount; ++i) + { + broadPhase->TouchProxy(m_proxies[i].proxyId); + } } void b2Fixture::SetSensor(bool sensor) diff --git a/libs/box2d/src/Box2D/Dynamics/b2Fixture.h b/libs/box2d/src/Box2D/Dynamics/b2Fixture.h index d9d3255..a7b5c34 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Fixture.h +++ b/libs/box2d/src/Box2D/Dynamics/b2Fixture.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -26,6 +26,7 @@ class b2BlockAllocator; class b2Body; class b2BroadPhase; +class b2Fixture; /// This holds contact filtering data. struct b2Filter @@ -61,8 +62,6 @@ struct b2FixtureDef isSensor = false; } - virtual ~b2FixtureDef() {} - /// The shape, this must be set. The shape will be cloned, so you /// can create the shape on the stack. const b2Shape* shape; @@ -87,6 +86,14 @@ struct b2FixtureDef b2Filter filter; }; +/// This proxy is used internally to connect fixtures to the broad-phase. +struct b2FixtureProxy +{ + b2AABB aabb; + b2Fixture* fixture; + int32 childIndex; + int32 proxyId; +}; /// A fixture is used to attach a shape to a body for collision detection. A fixture /// inherits its transform from its parent. Fixtures hold additional non-geometric data @@ -115,11 +122,15 @@ public: /// Set the contact filtering data. This will not update contacts until the next time /// step when either parent body is active and awake. + /// This automatically calls Refilter. void SetFilterData(const b2Filter& filter); /// Get the contact filtering data. const b2Filter& GetFilterData() const; + /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. + void Refilter(); + /// Get the parent body of this fixture. This is NULL if the fixture is not attached. /// @return the parent body. b2Body* GetBody(); @@ -138,14 +149,13 @@ public: void SetUserData(void* data); /// Test a point for containment in this fixture. - /// @param xf the shape world transform. /// @param p a point in world coordinates. bool TestPoint(const b2Vec2& p) const; /// Cast a ray against this shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; /// Get the mass data for this fixture. The mass data is based on the density and /// the shape. The rotational inertia is about the shape's origin. This operation @@ -162,19 +172,21 @@ public: /// Get the coefficient of friction. float32 GetFriction() const; - /// Set the coefficient of friction. + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. void SetFriction(float32 friction); /// Get the coefficient of restitution. float32 GetRestitution() const; - /// Set the coefficient of restitution. + /// Set the coefficient of restitution. This will _not_ change the restitution of + /// existing contacts. void SetRestitution(float32 restitution); /// Get the fixture's AABB. This AABB may be enlarge and/or stale. /// If you need a more accurate AABB, compute it using the shape and /// the body transform. - const b2AABB& GetAABB() const; + const b2AABB& GetAABB(int32 childIndex) const; protected: @@ -184,7 +196,6 @@ protected: friend class b2ContactManager; b2Fixture(); - ~b2Fixture(); // We need separation create/destroy functions from the constructor/destructor because // the destructor cannot access the allocator (no destructor arguments allowed by C++). @@ -192,13 +203,11 @@ protected: void Destroy(b2BlockAllocator* allocator); // These support body activation/deactivation. - void CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxy(b2BroadPhase* broadPhase); + void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); + void DestroyProxies(b2BroadPhase* broadPhase); void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - b2AABB m_aabb; - float32 m_density; b2Fixture* m_next; @@ -209,7 +218,9 @@ protected: float32 m_friction; float32 m_restitution; - int32 m_proxyId; + b2FixtureProxy* m_proxies; + int32 m_proxyCount; + b2Filter m_filter; bool m_isSensor; @@ -308,9 +319,9 @@ inline bool b2Fixture::TestPoint(const b2Vec2& p) const return m_shape->TestPoint(m_body->GetTransform(), p); } -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const +inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const { - return m_shape->RayCast(output, input, m_body->GetTransform()); + return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); } inline void b2Fixture::GetMassData(b2MassData* massData) const @@ -318,9 +329,10 @@ inline void b2Fixture::GetMassData(b2MassData* massData) const m_shape->ComputeMass(massData, m_density); } -inline const b2AABB& b2Fixture::GetAABB() const +inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const { - return m_aabb; + b2Assert(0 <= childIndex && childIndex < m_proxyCount); + return m_proxies[childIndex].aabb; } #endif diff --git a/libs/box2d/src/Box2D/Dynamics/b2Island.cpp b/libs/box2d/src/Box2D/Dynamics/b2Island.cpp index 0092499..671b78f 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Island.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2Island.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -16,6 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ +#include #include #include #include @@ -191,7 +192,7 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl } // Integrate velocities. - b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force); + b->m_linearVelocity += step.dt * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); b->m_angularVelocity += step.dt * b->m_invI * b->m_torque; // Apply damping. @@ -222,8 +223,22 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl } // Initialize velocity constraints. - b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio); - contactSolver.WarmStart(); + b2ContactSolverDef solverDef; + solverDef.contacts = m_contacts; + solverDef.count = m_contactCount; + solverDef.allocator = m_allocator; + solverDef.impulseRatio = step.dtRatio; + solverDef.warmStarting = step.warmStarting; + + b2ContactSolver contactSolver(&solverDef); + + contactSolver.InitializeVelocityConstraints(); + + if (step.warmStarting) + { + contactSolver.WarmStart(); + } + for (int32 i = 0; i < m_jointCount; ++i) { m_joints[i]->InitVelocityConstraints(step); @@ -349,6 +364,129 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl } } +void b2Island::SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB) +{ + b2ContactSolverDef solverDef; + solverDef.contacts = m_contacts; + solverDef.count = m_contactCount; + solverDef.allocator = m_allocator; + solverDef.impulseRatio = subStep.dtRatio; + solverDef.warmStarting = subStep.warmStarting; + b2ContactSolver contactSolver(&solverDef); + + // Solve position constraints. + const float32 k_toiBaumgarte = 0.75f; + for (int32 i = 0; i < subStep.positionIterations; ++i) + { + bool contactsOkay = contactSolver.SolveTOIPositionConstraints(k_toiBaumgarte, bodyA, bodyB); + if (contactsOkay) + { + break; + } + + if (i == subStep.positionIterations - 1) + { + i += 0; + } + } + +#if 0 + // Is the new position really safe? + for (int32 i = 0; i < m_contactCount; ++i) + { + b2Contact* c = m_contacts[i]; + b2Fixture* fA = c->GetFixtureA(); + b2Fixture* fB = c->GetFixtureB(); + + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); + + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + + b2DistanceInput input; + input.proxyA.Set(fA->GetShape(), indexA); + input.proxyB.Set(fB->GetShape(), indexB); + input.transformA = bA->GetTransform(); + input.transformB = bB->GetTransform(); + input.useRadii = false; + + b2DistanceOutput output; + b2SimplexCache cache; + cache.count = 0; + b2Distance(&output, &cache, &input); + + if (output.distance == 0 || cache.count == 3) + { + cache.count += 0; + } + } +#endif + + // Leap of faith to new safe state. + for (int32 i = 0; i < m_bodyCount; ++i) + { + m_bodies[i]->m_sweep.a0 = m_bodies[i]->m_sweep.a; + m_bodies[i]->m_sweep.c0 = m_bodies[i]->m_sweep.c; + } + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + contactSolver.InitializeVelocityConstraints(); + + // Solve velocity constraints. + for (int32 i = 0; i < subStep.velocityIterations; ++i) + { + contactSolver.SolveVelocityConstraints(); + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + // Integrate positions. + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Body* b = m_bodies[i]; + + if (b->GetType() == b2_staticBody) + { + continue; + } + + // Check for large velocities. + b2Vec2 translation = subStep.dt * b->m_linearVelocity; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) + { + translation.Normalize(); + b->m_linearVelocity = (b2_maxTranslation * subStep.inv_dt) * translation; + } + + float32 rotation = subStep.dt * b->m_angularVelocity; + if (rotation * rotation > b2_maxRotationSquared) + { + if (rotation < 0.0) + { + b->m_angularVelocity = -subStep.inv_dt * b2_maxRotation; + } + else + { + b->m_angularVelocity = subStep.inv_dt * b2_maxRotation; + } + } + + // Integrate + b->m_sweep.c += subStep.dt * b->m_linearVelocity; + b->m_sweep.a += subStep.dt * b->m_angularVelocity; + + // Compute new transform + b->SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + Report(contactSolver.m_constraints); +} + void b2Island::Report(const b2ContactConstraint* constraints) { if (m_listener == NULL) diff --git a/libs/box2d/src/Box2D/Dynamics/b2Island.h b/libs/box2d/src/Box2D/Dynamics/b2Island.h index 3a5fd11..8572081 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2Island.h +++ b/libs/box2d/src/Box2D/Dynamics/b2Island.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -60,6 +60,8 @@ public: void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); + void SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB); + void Add(b2Body* body) { b2Assert(m_bodyCount < m_bodyCapacity); @@ -98,8 +100,6 @@ public: int32 m_bodyCapacity; int32 m_contactCapacity; int32 m_jointCapacity; - - int32 m_positionIterationCount; }; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/b2TimeStep.h b/libs/box2d/src/Box2D/Dynamics/b2TimeStep.h index f59245d..74afb9c 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2TimeStep.h +++ b/libs/box2d/src/Box2D/Dynamics/b2TimeStep.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -21,15 +21,15 @@ #include -/// This is an internal structure. -struct b2TimeStep -{ - float32 dt; // time step - float32 inv_dt; // inverse time step (0 if dt == 0). - float32 dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; -}; +/// This is an internal structure. +struct b2TimeStep +{ + float32 dt; // time step + float32 inv_dt; // inverse time step (0 if dt == 0). + float32 dtRatio; // dt * inv_dt0 + int32 velocityIterations; + int32 positionIterations; + bool warmStarting; +}; #endif diff --git a/libs/box2d/src/Box2D/Dynamics/b2World.cpp b/libs/box2d/src/Box2D/Dynamics/b2World.cpp index d3b9e00..dffda12 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2World.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2World.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -23,12 +23,15 @@ #include #include #include -#include #include #include #include +#include +#include #include #include +#include +#include #include b2World::b2World(const b2Vec2& gravity, bool doSleep) @@ -44,6 +47,9 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep) m_warmStarting = true; m_continuousPhysics = true; + m_subStepping = false; + + m_stepComplete = true; m_allowSleep = doSleep; m_gravity = gravity; @@ -53,10 +59,29 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep) m_inv_dt0 = 0.0f; m_contactManager.m_allocator = &m_blockAllocator; + + memset(&m_profile, 0, sizeof(b2Profile)); } b2World::~b2World() { + // Some shapes allocate using b2Alloc. + b2Body* b = m_bodyList; + while (b) + { + b2Body* bNext = b->m_next; + + b2Fixture* f = b->m_fixtureList; + while (f) + { + b2Fixture* fNext = f->m_next; + f->m_proxyCount = 0; + f->Destroy(&m_blockAllocator); + f = fNext; + } + + b = bNext; + } } void b2World::SetDestructionListener(b2DestructionListener* listener) @@ -74,7 +99,7 @@ void b2World::SetContactListener(b2ContactListener* listener) m_contactManager.m_contactListener = listener; } -void b2World::SetDebugDraw(b2DebugDraw* debugDraw) +void b2World::SetDebugDraw(b2Draw* debugDraw) { m_debugDraw = debugDraw; } @@ -125,6 +150,8 @@ void b2World::DestroyBody(b2Body* b) } DestroyJoint(je0->joint); + + b->m_jointList = je; } b->m_jointList = NULL; @@ -150,10 +177,13 @@ void b2World::DestroyBody(b2Body* b) m_destructionListener->SayGoodbye(f0); } - f0->DestroyProxy(&m_contactManager.m_broadPhase); + f0->DestroyProxies(&m_contactManager.m_broadPhase); f0->Destroy(&m_blockAllocator); f0->~b2Fixture(); m_blockAllocator.Free(f0, sizeof(b2Fixture)); + + b->m_fixtureList = f; + b->m_fixtureCount -= 1; } b->m_fixtureList = NULL; b->m_fixtureCount = 0; @@ -513,270 +543,331 @@ void b2World::Solve(const b2TimeStep& step) m_contactManager.FindNewContacts(); } -// Advance a dynamic body to its first time of contact -// and adjust the position to ensure clearance. -void b2World::SolveTOI(b2Body* body) +// Find TOI contacts and solve them. +void b2World::SolveTOI(const b2TimeStep& step) { - // Find the minimum contact. - b2Contact* toiContact = NULL; - float32 toi = 1.0f; - b2Body* toiOther = NULL; - bool found; - int32 count; - int32 iter = 0; - - bool bullet = body->IsBullet(); - - // Iterate until all contacts agree on the minimum TOI. We have - // to iterate because the TOI algorithm may skip some intermediate - // collisions when objects rotate through each other. - do + b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); + + if (m_stepComplete) { - count = 0; - found = false; - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) + for (b2Body* b = m_bodyList; b; b = b->m_next) { - if (ce->contact == toiContact) - { - continue; - } + b->m_flags &= ~b2Body::e_islandFlag; + b->m_sweep.alpha0 = 0.0f; + } - b2Body* other = ce->other; - b2BodyType type = other->GetType(); + for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) + { + // Invalidate TOI + c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + c->m_toiCount = 0; + c->m_toi = 1.0f; + } + } - // Only bullets perform TOI with dynamic bodies. - if (bullet == true) - { - // Bullets only perform TOI with bodies that have their TOI resolved. - if ((other->m_flags & b2Body::e_toiFlag) == 0) - { - continue; - } + // Find TOI events and solve them. + for (;;) + { + // Find the first TOI. + b2Contact* minContact = NULL; + float32 minAlpha = 1.0f; - // No repeated hits on non-static bodies - if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0) - { - continue; - } - } - else if (type == b2_dynamicBody) + for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) + { + // Is this contact disabled? + if (c->IsEnabled() == false) { continue; } - // Check for a disabled contact. - b2Contact* contact = ce->contact; - if (contact->IsEnabled() == false) + // Prevent excessive sub-stepping. + if (c->m_toiCount > b2_maxSubSteps) { continue; } - // Prevent infinite looping. - if (contact->m_toiCount > 10) + float32 alpha = 1.0f; + if (c->m_flags & b2Contact::e_toiFlag) { - continue; + // This contact has a valid cached TOI. + alpha = c->m_toi; } + else + { + b2Fixture* fA = c->GetFixtureA(); + b2Fixture* fB = c->GetFixtureB(); - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; + // Is there a sensor? + if (fA->IsSensor() || fB->IsSensor()) + { + continue; + } - // Cull sensors. - if (fixtureA->IsSensor() || fixtureB->IsSensor()) - { - continue; - } + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); - b2Body* bodyA = fixtureA->m_body; - b2Body* bodyB = fixtureB->m_body; + b2BodyType typeA = bA->GetType(); + b2BodyType typeB = bB->GetType(); + b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fixtureA->GetShape()); - input.proxyB.Set(fixtureB->GetShape()); - input.sweepA = bodyA->m_sweep; - input.sweepB = bodyB->m_sweep; - input.tMax = toi; + bool awakeA = bA->IsAwake() && typeA != b2_staticBody; + bool awakeB = bB->IsAwake() && typeB != b2_staticBody; - b2TOIOutput output; - b2TimeOfImpact(&output, &input); + // Is at least one body awake? + if (awakeA == false && awakeB == false) + { + continue; + } - if (output.state == b2TOIOutput::e_touching && output.t < toi) - { - toiContact = contact; - toi = output.t; - toiOther = other; - found = true; - } + bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; + bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; - ++count; - } + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } - ++iter; - } while (found && count > 1 && iter < 50); + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + float32 alpha0 = bA->m_sweep.alpha0; - if (toiContact == NULL) - { - body->Advance(1.0f); - return; - } + if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) + { + alpha0 = bB->m_sweep.alpha0; + bA->m_sweep.Advance(alpha0); + } + else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) + { + alpha0 = bA->m_sweep.alpha0; + bB->m_sweep.Advance(alpha0); + } - b2Sweep backup = body->m_sweep; - body->Advance(toi); - toiContact->Update(m_contactManager.m_contactListener); - if (toiContact->IsEnabled() == false) - { - // Contact disabled. Backup and recurse. - body->m_sweep = backup; - SolveTOI(body); - } + b2Assert(alpha0 < 1.0f); - ++toiContact->m_toiCount; + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); - // Update all the valid contacts on this body and build a contact island. - b2Contact* contacts[b2_maxTOIContacts]; - count = 0; - for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next) - { - b2Body* other = ce->other; - b2BodyType type = other->GetType(); + // Compute the time of impact in interval [0, minTOI] + b2TOIInput input; + input.proxyA.Set(fA->GetShape(), indexA); + input.proxyB.Set(fB->GetShape(), indexB); + input.sweepA = bA->m_sweep; + input.sweepB = bB->m_sweep; + input.tMax = 1.0f; - // Only perform correction with static bodies, so the - // body won't get pushed out of the world. - if (type == b2_dynamicBody) - { - continue; - } + b2TOIOutput output; + b2TimeOfImpact(&output, &input); - // Check for a disabled contact. - b2Contact* contact = ce->contact; - if (contact->IsEnabled() == false) - { - continue; - } + // Beta is the fraction of the remaining portion of the . + float32 beta = output.t; + if (output.state == b2TOIOutput::e_touching) + { + alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); + } + else + { + alpha = 1.0f; + } - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; + c->m_toi = alpha; + c->m_flags |= b2Contact::e_toiFlag; + } - // Cull sensors. - if (fixtureA->IsSensor() || fixtureB->IsSensor()) - { - continue; + if (alpha < minAlpha) + { + // This is the minimum TOI found so far. + minContact = c; + minAlpha = alpha; + } } - // The contact likely has some new contact points. The listener - // gives the user a chance to disable the contact. - if (contact != toiContact) + if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) { - contact->Update(m_contactManager.m_contactListener); + // No more TOI events. Done! + m_stepComplete = true; + break; } - // Did the user disable the contact? - if (contact->IsEnabled() == false) - { - // Skip this contact. - continue; - } + // Advance the bodies to the TOI. + b2Fixture* fA = minContact->GetFixtureA(); + b2Fixture* fB = minContact->GetFixtureB(); + b2Body* bA = fA->GetBody(); + b2Body* bB = fB->GetBody(); + + b2Sweep backup1 = bA->m_sweep; + b2Sweep backup2 = bB->m_sweep; + + bA->Advance(minAlpha); + bB->Advance(minAlpha); - if (contact->IsTouching() == false) + // The TOI contact likely has some new contact points. + minContact->Update(m_contactManager.m_contactListener); + minContact->m_flags &= ~b2Contact::e_toiFlag; + ++minContact->m_toiCount; + + // Is the contact solid? + if (minContact->IsEnabled() == false || minContact->IsTouching() == false) { + // Restore the sweeps. + minContact->SetEnabled(false); + bA->m_sweep = backup1; + bB->m_sweep = backup2; + bA->SynchronizeTransform(); + bB->SynchronizeTransform(); continue; } - contacts[count] = contact; - ++count; - } + bA->SetAwake(true); + bB->SetAwake(true); - // Reduce the TOI body's overlap with the contact island. - b2TOISolver solver(&m_stackAllocator); - solver.Initialize(contacts, count, body); + // Build the island + island.Clear(); + island.Add(bA); + island.Add(bB); + island.Add(minContact); - const float32 k_toiBaumgarte = 0.75f; - bool solved = false; - for (int32 i = 0; i < 20; ++i) - { - bool contactsOkay = solver.Solve(k_toiBaumgarte); - if (contactsOkay) + bA->m_flags |= b2Body::e_islandFlag; + bB->m_flags |= b2Body::e_islandFlag; + minContact->m_flags |= b2Contact::e_islandFlag; + + // Get contacts on bodyA and bodyB. + b2Body* bodies[2] = {bA, bB}; + for (int32 i = 0; i < 2; ++i) { - solved = true; - break; - } - } + b2Body* body = bodies[i]; + if (body->m_type == b2_dynamicBody) + { + for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) + { + if (island.m_bodyCount == island.m_bodyCapacity) + { + break; + } - if (toiOther->GetType() != b2_staticBody) - { - toiContact->m_flags |= b2Contact::e_bulletHitFlag; - } -} + if (island.m_contactCount == island.m_contactCapacity) + { + break; + } -// Sequentially solve TOIs for each body. We bring each -// body to the time of contact and perform some position correction. -// Time is not conserved. -void b2World::SolveTOI() -{ - // Prepare all contacts. - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Enable the contact - c->m_flags |= b2Contact::e_enabledFlag; + b2Contact* contact = ce->contact; - // Set the number of TOI events for this contact to zero. - c->m_toiCount = 0; - } + // Has this contact already been added to the island? + if (contact->m_flags & b2Contact::e_islandFlag) + { + continue; + } - // Initialize the TOI flag. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - // Kinematic, and static bodies will not be affected by the TOI event. - // If a body was not in an island then it did not move. - if ((body->m_flags & b2Body::e_islandFlag) == 0 || body->GetType() == b2_kinematicBody || body->GetType() == b2_staticBody) - { - body->m_flags |= b2Body::e_toiFlag; - } - else - { - body->m_flags &= ~b2Body::e_toiFlag; - } - } + // Only add static, kinematic, or bullet bodies. + b2Body* other = ce->other; + if (other->m_type == b2_dynamicBody && + body->IsBullet() == false && other->IsBullet() == false) + { + continue; + } - // Collide non-bullets. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - if (body->m_flags & b2Body::e_toiFlag) - { - continue; - } + // Skip sensors. + bool sensorA = contact->m_fixtureA->m_isSensor; + bool sensorB = contact->m_fixtureB->m_isSensor; + if (sensorA || sensorB) + { + continue; + } + + // Tentatively advance the body to the TOI. + b2Sweep backup = other->m_sweep; + if ((other->m_flags & b2Body::e_islandFlag) == 0) + { + other->Advance(minAlpha); + } + + // Update the contact points + contact->Update(m_contactManager.m_contactListener); + + // Was the contact disabled by the user? + if (contact->IsEnabled() == false) + { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } - if (body->IsBullet() == true) - { - continue; - } + // Are there contact points? + if (contact->IsTouching() == false) + { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } - SolveTOI(body); + // Add the contact to the island + contact->m_flags |= b2Contact::e_islandFlag; + island.Add(contact); - body->m_flags |= b2Body::e_toiFlag; - } + // Has the other body already been added to the island? + if (other->m_flags & b2Body::e_islandFlag) + { + continue; + } + + // Add the other body to the island. + other->m_flags |= b2Body::e_islandFlag; - // Collide bullets. - for (b2Body* body = m_bodyList; body; body = body->m_next) - { - if (body->m_flags & b2Body::e_toiFlag) - { - continue; + if (other->m_type != b2_staticBody) + { + other->SetAwake(true); + } + + island.Add(other); + } + } } - if (body->IsBullet() == false) + b2TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + subStep.positionIterations = 20; + subStep.velocityIterations = step.velocityIterations; + subStep.warmStarting = false; + island.SolveTOI(subStep, bA, bB); + + // Reset island flags and synchronize broad-phase proxies. + for (int32 i = 0; i < island.m_bodyCount; ++i) { - continue; + b2Body* body = island.m_bodies[i]; + body->m_flags &= ~b2Body::e_islandFlag; + + if (body->m_type != b2_dynamicBody) + { + continue; + } + + body->SynchronizeFixtures(); + + // Invalidate all contact TOIs on this displaced body. + for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) + { + ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + } } - SolveTOI(body); + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + m_contactManager.FindNewContacts(); - body->m_flags |= b2Body::e_toiFlag; + if (m_subStepping) + { + m_stepComplete = false; + break; + } } } void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) { + b2Timer stepTimer; + // If new fixtures were added, we need to find the new contacts. if (m_flags & e_newFixture) { @@ -802,20 +893,28 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration step.dtRatio = m_inv_dt0 * dt; step.warmStarting = m_warmStarting; - + // Update contacts. This is where some contacts are destroyed. - m_contactManager.Collide(); + { + b2Timer timer; + m_contactManager.Collide(); + m_profile.collide = timer.GetMilliseconds(); + } // Integrate velocities, solve velocity constraints, and integrate positions. - if (step.dt > 0.0f) + if (m_stepComplete && step.dt > 0.0f) { + b2Timer timer; Solve(step); + m_profile.solve = timer.GetMilliseconds(); } // Handle TOI events. if (m_continuousPhysics && step.dt > 0.0f) { - SolveTOI(); + b2Timer timer; + SolveTOI(step); + m_profile.solveTOI = timer.GetMilliseconds(); } if (step.dt > 0.0f) @@ -829,6 +928,8 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration } m_flags &= ~e_locked; + + m_profile.step = stepTimer.GetMilliseconds(); } void b2World::ClearForces() @@ -844,8 +945,8 @@ struct b2WorldQueryWrapper { bool QueryCallback(int32 proxyId) { - b2Fixture* fixture = (b2Fixture*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(fixture); + b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); + return callback->ReportFixture(proxy->fixture); } const b2BroadPhase* broadPhase; @@ -865,9 +966,11 @@ struct b2WorldRayCastWrapper float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) { void* userData = broadPhase->GetUserData(proxyId); - b2Fixture* fixture = (b2Fixture*)userData; + b2FixtureProxy* proxy = (b2FixtureProxy*)userData; + b2Fixture* fixture = proxy->fixture; + int32 index = proxy->childIndex; b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input); + bool hit = fixture->RayCast(&output, input, index); if (hit) { @@ -911,6 +1014,32 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color } break; + case b2Shape::e_edge: + { + b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); + b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); + b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); + m_debugDraw->DrawSegment(v1, v2, color); + } + break; + + case b2Shape::e_loop: + { + b2LoopShape* loop = (b2LoopShape*)fixture->GetShape(); + int32 count = loop->GetCount(); + const b2Vec2* vertices = loop->GetVertices(); + + b2Vec2 v1 = b2Mul(xf, vertices[count - 1]); + for (int32 i = 0; i < count; ++i) + { + b2Vec2 v2 = b2Mul(xf, vertices[i]); + m_debugDraw->DrawSegment(v1, v2, color); + m_debugDraw->DrawCircle(v1, 0.05f, color); + v1 = v2; + } + } + break; + case b2Shape::e_polygon: { b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); @@ -979,7 +1108,7 @@ void b2World::DrawDebugData() uint32 flags = m_debugDraw->GetFlags(); - if (flags & b2DebugDraw::e_shapeBit) + if (flags & b2Draw::e_shapeBit) { for (b2Body* b = m_bodyList; b; b = b->GetNext()) { @@ -1010,7 +1139,7 @@ void b2World::DrawDebugData() } } - if (flags & b2DebugDraw::e_jointBit) + if (flags & b2Draw::e_jointBit) { for (b2Joint* j = m_jointList; j; j = j->GetNext()) { @@ -1018,22 +1147,22 @@ void b2World::DrawDebugData() } } - if (flags & b2DebugDraw::e_pairBit) + if (flags & b2Draw::e_pairBit) { b2Color color(0.3f, 0.9f, 0.9f); for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); + //b2Fixture* fixtureA = c->GetFixtureA(); + //b2Fixture* fixtureB = c->GetFixtureB(); - b2Vec2 cA = fixtureA->GetAABB().GetCenter(); - b2Vec2 cB = fixtureB->GetAABB().GetCenter(); + //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); + //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - m_debugDraw->DrawSegment(cA, cB, color); + //m_debugDraw->DrawSegment(cA, cB, color); } } - if (flags & b2DebugDraw::e_aabbBit) + if (flags & b2Draw::e_aabbBit) { b2Color color(0.9f, 0.3f, 0.9f); b2BroadPhase* bp = &m_contactManager.m_broadPhase; @@ -1047,19 +1176,23 @@ void b2World::DrawDebugData() for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) { - b2AABB aabb = bp->GetFatAABB(f->m_proxyId); - b2Vec2 vs[4]; - vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); - vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); - vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); - vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - - m_debugDraw->DrawPolygon(vs, 4, color); + for (int32 i = 0; i < f->m_proxyCount; ++i) + { + b2FixtureProxy* proxy = f->m_proxies + i; + b2AABB aabb = bp->GetFatAABB(proxy->proxyId); + b2Vec2 vs[4]; + vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); + vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); + vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); + vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); + + m_debugDraw->DrawPolygon(vs, 4, color); + } } } } - if (flags & b2DebugDraw::e_centerOfMassBit) + if (flags & b2Draw::e_centerOfMassBit) { for (b2Body* b = m_bodyList; b; b = b->GetNext()) { @@ -1074,3 +1207,18 @@ int32 b2World::GetProxyCount() const { return m_contactManager.m_broadPhase.GetProxyCount(); } + +int32 b2World::GetTreeHeight() const +{ + return m_contactManager.m_broadPhase.GetTreeHeight(); +} + +int32 b2World::GetTreeBalance() const +{ + return m_contactManager.m_broadPhase.GetTreeBalance(); +} + +float32 b2World::GetTreeQuality() const +{ + return m_contactManager.m_broadPhase.GetTreeQuality(); +} diff --git a/libs/box2d/src/Box2D/Dynamics/b2World.h b/libs/box2d/src/Box2D/Dynamics/b2World.h index bff1427..169d106 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2World.h +++ b/libs/box2d/src/Box2D/Dynamics/b2World.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -27,12 +27,23 @@ struct b2AABB; struct b2BodyDef; +struct b2Color; struct b2JointDef; struct b2TimeStep; class b2Body; +class b2Draw; class b2Fixture; class b2Joint; +/// Profiling data. Times are in milliseconds. +struct b2Profile +{ + float32 step; + float32 collide; + float32 solve; + float32 solveTOI; +}; + /// The world class manages all physics entities, dynamic simulation, /// and asynchronous queries. The world also contains efficient memory /// management facilities. @@ -63,7 +74,7 @@ public: /// Register a routine for debug drawing. The debug draw functions are called /// inside with b2World::DrawDebugData method. The debug draw object is owned /// by you and must remain in scope. - void SetDebugDraw(b2DebugDraw* debugDraw); + void SetDebugDraw(b2Draw* debugDraw); /// Create a rigid body given a definition. No reference to the definition /// is retained. @@ -94,9 +105,12 @@ public: int32 velocityIterations, int32 positionIterations); - /// Call this after you are done with time steps to clear the forces. You normally - /// call this after each call to Step, unless you are performing sub-steps. By default, - /// forces will be automatically cleared, so you don't need to call this function. + /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically + /// after each call to Step. The default behavior is modified by calling SetAutoClearForces. + /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain + /// a fixed sized time step under a variable frame-rate. + /// When you perform sub-stepping you will disable auto clearing of forces and instead call + /// ClearForces after all sub-steps are complete in one pass of your game loop. /// @see SetAutoClearForces void ClearForces(); @@ -121,17 +135,20 @@ public: /// the next body in the world list. A NULL body indicates the end of the list. /// @return the head of the world body list. b2Body* GetBodyList(); + const b2Body* GetBodyList() const; /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get /// the next joint in the world list. A NULL joint indicates the end of the list. /// @return the head of the world joint list. b2Joint* GetJointList(); + const b2Joint* GetJointList() const; /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get /// the next contact in the world list. A NULL contact indicates the end of the list. /// @return the head of the world contact list. /// @warning contacts are b2Contact* GetContactList(); + const b2Contact* GetContactList() const; /// Enable/disable warm starting. For testing. void SetWarmStarting(bool flag) { m_warmStarting = flag; } @@ -139,6 +156,9 @@ public: /// Enable/disable continuous physics. For testing. void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } + /// Enable/disable single stepped continuous physics. For testing. + void SetSubStepping(bool flag) { m_subStepping = flag; } + /// Get the number of broad-phase proxies. int32 GetProxyCount() const; @@ -151,6 +171,16 @@ public: /// Get the number of contacts (each may have 0 or more contact points). int32 GetContactCount() const; + /// Get the height of the dynamic tree. + int32 GetTreeHeight() const; + + /// Get the balance of the dynamic tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the dynamic tree. The smaller the better. + /// The minimum is 1. + float32 GetTreeQuality() const; + /// Change the global gravity vector. void SetGravity(const b2Vec2& gravity); @@ -166,6 +196,12 @@ public: /// Get the flag that controls automatic clearing of forces after each time step. bool GetAutoClearForces() const; + /// Get the contact manager for testing. + const b2ContactManager& GetContactManager() const; + + /// Get the current profile. + const b2Profile& GetProfile() const; + private: // m_flags @@ -173,16 +209,16 @@ private: { e_newFixture = 0x0001, e_locked = 0x0002, - e_clearForces = 0x0004, + e_clearForces = 0x0004 }; friend class b2Body; + friend class b2Fixture; friend class b2ContactManager; friend class b2Controller; void Solve(const b2TimeStep& step); - void SolveTOI(); - void SolveTOI(b2Body* body); + void SolveTOI(const b2TimeStep& step); void DrawJoint(b2Joint* joint); void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color); @@ -203,20 +239,21 @@ private: b2Vec2 m_gravity; bool m_allowSleep; - b2Body* m_groundBody; - b2DestructionListener* m_destructionListener; - b2DebugDraw* m_debugDraw; + b2Draw* m_debugDraw; // This is used to compute the time step ratio to // support a variable time step. float32 m_inv_dt0; - // This is for debugging the solver. + // These are for debugging the solver. bool m_warmStarting; - - // This is for debugging the solver. bool m_continuousPhysics; + bool m_subStepping; + + bool m_stepComplete; + + b2Profile m_profile; }; inline b2Body* b2World::GetBodyList() @@ -224,16 +261,31 @@ inline b2Body* b2World::GetBodyList() return m_bodyList; } +inline const b2Body* b2World::GetBodyList() const +{ + return m_bodyList; +} + inline b2Joint* b2World::GetJointList() { return m_jointList; } +inline const b2Joint* b2World::GetJointList() const +{ + return m_jointList; +} + inline b2Contact* b2World::GetContactList() { return m_contactManager.m_contactList; } +inline const b2Contact* b2World::GetContactList() const +{ + return m_contactManager.m_contactList; +} + inline int32 b2World::GetBodyCount() const { return m_bodyCount; @@ -282,4 +334,14 @@ inline bool b2World::GetAutoClearForces() const return (m_flags & e_clearForces) == e_clearForces; } +inline const b2ContactManager& b2World::GetContactManager() const +{ + return m_contactManager; +} + +inline const b2Profile& b2World::GetProfile() const +{ + return m_profile; +} + #endif diff --git a/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.cpp b/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.cpp index bc5fefb..82b28cc 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.cpp +++ b/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.cpp @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -34,28 +34,3 @@ bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; return collide; } - -b2DebugDraw::b2DebugDraw() -{ - m_drawFlags = 0; -} - -void b2DebugDraw::SetFlags(uint32 flags) -{ - m_drawFlags = flags; -} - -uint32 b2DebugDraw::GetFlags() const -{ - return m_drawFlags; -} - -void b2DebugDraw::AppendFlags(uint32 flags) -{ - m_drawFlags |= flags; -} - -void b2DebugDraw::ClearFlags(uint32 flags) -{ - m_drawFlags &= ~flags; -} diff --git a/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.h b/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.h index 82ee67e..33393f8 100644 --- a/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.h +++ b/libs/box2d/src/Box2D/Dynamics/b2WorldCallbacks.h @@ -1,5 +1,5 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* Copyright (c) 2006-2009 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 @@ -27,7 +27,6 @@ class b2Fixture; class b2Body; class b2Joint; class b2Contact; -struct b2ContactPoint; struct b2ContactResult; struct b2Manifold; @@ -152,66 +151,4 @@ public: const b2Vec2& normal, float32 fraction) = 0; }; -/// Color for debug drawing. Each value has the range [0,1]. -struct b2Color -{ - b2Color() {} - b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} - void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } - float32 r, g, b; -}; - -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. -class b2DebugDraw -{ -public: - b2DebugDraw(); - - virtual ~b2DebugDraw() {} - - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010, ///< draw center of mass frame - }; - - /// Set the drawing flags. - void SetFlags(uint32 flags); - - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); - - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); - - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; - - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; - -protected: - uint32 m_drawFlags; -}; - #endif diff --git a/libs/box2d/src/Box2D/Rope/b2Rope.cpp b/libs/box2d/src/Box2D/Rope/b2Rope.cpp new file mode 100644 index 0000000..97578b2 --- /dev/null +++ b/libs/box2d/src/Box2D/Rope/b2Rope.cpp @@ -0,0 +1,259 @@ +/* +* Copyright (c) 2011 Erin Catto http://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 + +b2Rope::b2Rope() +{ + m_count = 0; + m_ps = NULL; + m_p0s = NULL; + m_vs = NULL; + m_ims = NULL; + m_Ls = NULL; + m_as = NULL; + m_gravity.SetZero(); + m_k2 = 1.0f; + m_k3 = 0.1f; +} + +b2Rope::~b2Rope() +{ + b2Free(m_ps); + b2Free(m_p0s); + b2Free(m_vs); + b2Free(m_ims); + b2Free(m_Ls); + b2Free(m_as); +} + +void b2Rope::Initialize(const b2RopeDef* def) +{ + b2Assert(def->count >= 3); + m_count = def->count; + m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); + + for (int32 i = 0; i < m_count; ++i) + { + m_ps[i] = def->vertices[i]; + m_p0s[i] = def->vertices[i]; + m_vs[i].SetZero(); + + float32 m = def->masses[i]; + if (m > 0.0f) + { + m_ims[i] = 1.0f / m; + } + else + { + m_ims[i] = 0.0f; + } + } + + int32 count2 = m_count - 1; + int32 count3 = m_count - 2; + m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); + m_as = (float32*)b2Alloc(count3 * sizeof(float32)); + + for (int32 i = 0; i < count2; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i+1]; + m_Ls[i] = b2Distance(p1, p2); + } + + for (int32 i = 0; i < count3; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); + + m_as[i] = b2Atan2(a, b); + } + + m_gravity = def->gravity; + m_damping = def->damping; + m_k2 = def->k2; + m_k3 = def->k3; +} + +void b2Rope::Step(float32 h, int32 iterations) +{ + if (h == 0.0) + { + return; + } + + float32 d = expf(- h * m_damping); + + for (int32 i = 0; i < m_count; ++i) + { + m_p0s[i] = m_ps[i]; + if (m_ims[i] > 0.0f) + { + m_vs[i] += h * m_gravity; + } + m_vs[i] *= d; + m_ps[i] += h * m_vs[i]; + + } + + for (int32 i = 0; i < iterations; ++i) + { + SolveC2(); + SolveC3(); + SolveC2(); + } + + float32 inv_h = 1.0f / h; + for (int32 i = 0; i < m_count; ++i) + { + m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); + } +} + +void b2Rope::SolveC2() +{ + int32 count2 = m_count - 1; + + for (int32 i = 0; i < count2; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + + b2Vec2 d = p2 - p1; + float32 L = d.Normalize(); + + float32 im1 = m_ims[i]; + float32 im2 = m_ims[i + 1]; + + if (im1 + im2 == 0.0f) + { + continue; + } + + float32 s1 = im1 / (im1 + im2); + float32 s2 = im2 / (im1 + im2); + + p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; + p2 += m_k2 * s2 * (m_Ls[i] - L) * d; + + m_ps[i] = p1; + m_ps[i + 1] = p2; + } +} + +void b2Rope::SetAngle(float32 angle) +{ + int32 count3 = m_count - 2; + for (int32 i = 0; i < count3; ++i) + { + m_as[i] = angle; + } +} + +void b2Rope::SolveC3() +{ + int32 count3 = m_count - 2; + + for (int32 i = 0; i < count3; ++i) + { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + float32 m1 = m_ims[i]; + float32 m2 = m_ims[i + 1]; + float32 m3 = m_ims[i + 2]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float32 L1sqr = d1.LengthSquared(); + float32 L2sqr = d2.LengthSquared(); + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); + + float32 angle = b2Atan2(a, b); + + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); + if (mass == 0.0f) + { + continue; + } + + mass = 1.0f / mass; + + float32 C = angle - m_as[i]; + + while (C > b2_pi) + { + angle -= 2 * b2_pi; + C = angle - m_as[i]; + } + + while (C < -b2_pi) + { + angle += 2.0f * b2_pi; + C = angle - m_as[i]; + } + + float32 impulse = - m_k3 * mass * C; + + p1 += (m1 * impulse) * J1; + p2 += (m2 * impulse) * J2; + p3 += (m3 * impulse) * J3; + + m_ps[i] = p1; + m_ps[i + 1] = p2; + m_ps[i + 2] = p3; + } +} + +void b2Rope::Draw(b2Draw* draw) const +{ + b2Color c(0.4f, 0.5f, 0.7f); + + for (int32 i = 0; i < m_count - 1; ++i) + { + draw->DrawSegment(m_ps[i], m_ps[i+1], c); + } +} diff --git a/libs/box2d/src/Box2D/Rope/b2Rope.h b/libs/box2d/src/Box2D/Rope/b2Rope.h new file mode 100644 index 0000000..bc5375d --- /dev/null +++ b/libs/box2d/src/Box2D/Rope/b2Rope.h @@ -0,0 +1,115 @@ +/* +* Copyright (c) 2011 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_ROPE_H +#define B2_ROPE_H + +#include + +class b2Draw; + +/// +struct b2RopeDef +{ + b2RopeDef() + { + vertices = NULL; + count = 0; + masses = NULL; + gravity.SetZero(); + damping = 0.1f; + k2 = 0.9f; + k3 = 0.1f; + } + + /// + b2Vec2* vertices; + + /// + int32 count; + + /// + float32* masses; + + /// + b2Vec2 gravity; + + /// + float32 damping; + + /// Stretching stiffness + float32 k2; + + /// Bending stiffness. Values above 0.5 can make the simulation blow up. + float32 k3; +}; + +/// +class b2Rope +{ +public: + b2Rope(); + ~b2Rope(); + + /// + void Initialize(const b2RopeDef* def); + + /// + void Step(float32 timeStep, int32 iterations); + + /// + int32 GetVertexCount() const + { + return m_count; + } + + /// + const b2Vec2* GetVertices() const + { + return m_ps; + } + + /// + void Draw(b2Draw* draw) const; + + /// + void SetAngle(float32 angle); + +private: + + void SolveC2(); + void SolveC3(); + + int32 m_count; + b2Vec2* m_ps; + b2Vec2* m_p0s; + b2Vec2* m_vs; + + float32* m_ims; + + float32* m_Ls; + float32* m_as; + + b2Vec2 m_gravity; + float32 m_damping; + + float32 m_k2; + float32 m_k3; +}; + +#endif