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.less.ly: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;