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 */; };
buildActionMask = 2147483647;
files = (
491331AF1372653600DFB46D /* Icon.png in Resources */,
- 491331B01372653600DFB46D /* Info.plist in Resources */,
- 491331B21372653600DFB46D /* MainWindow.xib in Resources */,
);
runOnlyForDeploymentPostprocessing = 0;
};
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
// These include files constitute the main Box2D API\r
\r
#include <Box2D/Common/b2Settings.h>\r
+#include <Box2D/Common/b2Draw.h>\r
+#include <Box2D/Common/b2Timer.h>\r
\r
#include <Box2D/Collision/Shapes/b2CircleShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+#include <Box2D/Collision/Shapes/b2LoopShape.h>\r
#include <Box2D/Collision/Shapes/b2PolygonShape.h>\r
\r
#include <Box2D/Collision/b2BroadPhase.h>\r
#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>\r
#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>\r
#include <Box2D/Dynamics/Joints/b2GearJoint.h>\r
-#include <Box2D/Dynamics/Joints/b2LineJoint.h>\r
+#include <Box2D/Dynamics/Joints/b2WheelJoint.h>\r
#include <Box2D/Dynamics/Joints/b2MouseJoint.h>\r
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>\r
#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>\r
#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>\r
+#include <Box2D/Dynamics/Joints/b2RopeJoint.h>\r
#include <Box2D/Dynamics/Joints/b2WeldJoint.h>\r
\r
+#include <Box2D/Rope/b2Rope.h>\r
+\r
#endif\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
#include <Box2D/Collision/Shapes/b2CircleShape.h>\r
#include <new>\r
+using namespace std;\r
\r
b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const\r
{\r
return clone;\r
}\r
\r
+int32 b2CircleShape::GetChildCount() const\r
+{\r
+ return 1;\r
+}\r
+\r
bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const\r
{\r
b2Vec2 center = transform.position + b2Mul(transform.R, m_p);\r
// From Section 3.1.2\r
// x = s + a * r\r
// norm(x) = radius\r
-bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const\r
+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const\r
{\r
+ B2_NOT_USED(childIndex);\r
+\r
b2Vec2 position = transform.position + b2Mul(transform.R, m_p);\r
b2Vec2 s = input.p1 - position;\r
float32 b = b2Dot(s, s) - m_radius * m_radius;\r
return false;\r
}\r
\r
-void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const\r
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const\r
{\r
+ B2_NOT_USED(childIndex);\r
+\r
b2Vec2 p = transform.position + b2Mul(transform.R, m_p);\r
aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);\r
aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
/// Implement b2Shape.\r
b2Shape* Clone(b2BlockAllocator* allocator) const;\r
\r
+ /// @see b2Shape::GetChildCount\r
+ int32 GetChildCount() const;\r
+\r
/// Implement b2Shape.\r
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;\r
\r
/// Implement b2Shape.\r
- bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;\r
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const;\r
\r
/// @see b2Shape::ComputeAABB\r
- void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;\r
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;\r
\r
/// @see b2Shape::ComputeMass\r
void ComputeMass(b2MassData* massData, float32 density) const;\r
--- /dev/null
+/*\r
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+#include <new>\r
+using namespace std;\r
+\r
+void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)\r
+{\r
+ m_vertex1 = v1;\r
+ m_vertex2 = v2;\r
+ m_hasVertex0 = false;\r
+ m_hasVertex3 = false;\r
+}\r
+\r
+b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2EdgeShape));\r
+ b2EdgeShape* clone = new (mem) b2EdgeShape;\r
+ *clone = *this;\r
+ return clone;\r
+}\r
+\r
+int32 b2EdgeShape::GetChildCount() const\r
+{\r
+ return 1;\r
+}\r
+\r
+bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const\r
+{\r
+ B2_NOT_USED(xf);\r
+ B2_NOT_USED(p);\r
+ return false;\r
+}\r
+\r
+// p = p1 + t * d\r
+// v = v1 + s * e\r
+// p1 + t * d = v1 + s * e\r
+// s * e - t * d = p1 - v1\r
+bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& xf, int32 childIndex) const\r
+{\r
+ B2_NOT_USED(childIndex);\r
+\r
+ // Put the ray into the edge's frame of reference.\r
+ b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);\r
+ b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);\r
+ b2Vec2 d = p2 - p1;\r
+\r
+ b2Vec2 v1 = m_vertex1;\r
+ b2Vec2 v2 = m_vertex2;\r
+ b2Vec2 e = v2 - v1;\r
+ b2Vec2 normal(e.y, -e.x);\r
+ normal.Normalize();\r
+\r
+ // q = p1 + t * d\r
+ // dot(normal, q - v1) = 0\r
+ // dot(normal, p1 - v1) + t * dot(normal, d) = 0\r
+ float32 numerator = b2Dot(normal, v1 - p1);\r
+ float32 denominator = b2Dot(normal, d);\r
+\r
+ if (denominator == 0.0f)\r
+ {\r
+ return false;\r
+ }\r
+\r
+ float32 t = numerator / denominator;\r
+ if (t < 0.0f || 1.0f < t)\r
+ {\r
+ return false;\r
+ }\r
+\r
+ b2Vec2 q = p1 + t * d;\r
+\r
+ // q = v1 + s * r\r
+ // s = dot(q - v1, r) / dot(r, r)\r
+ b2Vec2 r = v2 - v1;\r
+ float32 rr = b2Dot(r, r);\r
+ if (rr == 0.0f)\r
+ {\r
+ return false;\r
+ }\r
+\r
+ float32 s = b2Dot(q - v1, r) / rr;\r
+ if (s < 0.0f || 1.0f < s)\r
+ {\r
+ return false;\r
+ }\r
+\r
+ output->fraction = t;\r
+ if (numerator > 0.0f)\r
+ {\r
+ output->normal = -normal;\r
+ }\r
+ else\r
+ {\r
+ output->normal = normal;\r
+ }\r
+ return true;\r
+}\r
+\r
+void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const\r
+{\r
+ B2_NOT_USED(childIndex);\r
+\r
+ b2Vec2 v1 = b2Mul(xf, m_vertex1);\r
+ b2Vec2 v2 = b2Mul(xf, m_vertex2);\r
+\r
+ b2Vec2 lower = b2Min(v1, v2);\r
+ b2Vec2 upper = b2Max(v1, v2);\r
+\r
+ b2Vec2 r(m_radius, m_radius);\r
+ aabb->lowerBound = lower - r;\r
+ aabb->upperBound = upper + r;\r
+}\r
+\r
+void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const\r
+{\r
+ B2_NOT_USED(density);\r
+\r
+ massData->mass = 0.0f;\r
+ massData->center = 0.5f * (m_vertex1 + m_vertex2);\r
+ massData->I = 0.0f;\r
+}\r
--- /dev/null
+/*\r
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#ifndef B2_EDGE_SHAPE_H\r
+#define B2_EDGE_SHAPE_H\r
+\r
+#include <Box2D/Collision/Shapes/b2Shape.h>\r
+\r
+/// A line segment (edge) shape. These can be connected in chains or loops\r
+/// to other edge shapes. The connectivity information is used to ensure\r
+/// correct contact normals.\r
+class b2EdgeShape : public b2Shape\r
+{\r
+public:\r
+ b2EdgeShape();\r
+\r
+ /// Set this as an isolated edge.\r
+ void Set(const b2Vec2& v1, const b2Vec2& v2);\r
+\r
+ /// Implement b2Shape.\r
+ b2Shape* Clone(b2BlockAllocator* allocator) const;\r
+\r
+ /// @see b2Shape::GetChildCount\r
+ int32 GetChildCount() const;\r
+\r
+ /// @see b2Shape::TestPoint\r
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;\r
+\r
+ /// Implement b2Shape.\r
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const;\r
+\r
+ /// @see b2Shape::ComputeAABB\r
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;\r
+\r
+ /// @see b2Shape::ComputeMass\r
+ void ComputeMass(b2MassData* massData, float32 density) const;\r
+ \r
+ /// These are the edge vertices\r
+ b2Vec2 m_vertex1, m_vertex2;\r
+\r
+ /// Optional adjacent vertices. These are used for smooth collision.\r
+ b2Vec2 m_vertex0, m_vertex3;\r
+ bool m_hasVertex0, m_hasVertex3;\r
+};\r
+\r
+inline b2EdgeShape::b2EdgeShape()\r
+{\r
+ m_type = e_edge;\r
+ m_radius = b2_polygonRadius;\r
+ m_hasVertex0 = false;\r
+ m_hasVertex3 = false;\r
+}\r
+\r
+#endif\r
--- /dev/null
+/*\r
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#include <Box2D/Collision/Shapes/b2LoopShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+#include <new>\r
+#include <cstring>\r
+using namespace std;\r
+\r
+b2LoopShape::~b2LoopShape()\r
+{\r
+ b2Free(m_vertices);\r
+ m_vertices = NULL;\r
+ m_count = 0;\r
+}\r
+\r
+void b2LoopShape::Create(const b2Vec2* vertices, int32 count)\r
+{\r
+ b2Assert(m_vertices == NULL && m_count == 0);\r
+ b2Assert(count >= 2);\r
+ m_count = count;\r
+ m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));\r
+ memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));\r
+}\r
+\r
+b2Shape* b2LoopShape::Clone(b2BlockAllocator* allocator) const\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2LoopShape));\r
+ b2LoopShape* clone = new (mem) b2LoopShape;\r
+ clone->Create(m_vertices, m_count);\r
+ return clone;\r
+}\r
+\r
+int32 b2LoopShape::GetChildCount() const\r
+{\r
+ return m_count;\r
+}\r
+\r
+void b2LoopShape::GetChildEdge(b2EdgeShape* edge, int32 index) const\r
+{\r
+ b2Assert(2 <= m_count);\r
+ b2Assert(0 <= index && index < m_count);\r
+ edge->m_type = b2Shape::e_edge;\r
+ edge->m_radius = m_radius;\r
+ edge->m_hasVertex0 = true;\r
+ edge->m_hasVertex3 = true;\r
+\r
+ int32 i0 = index - 1 >= 0 ? index - 1 : m_count - 1;\r
+ int32 i1 = index;\r
+ int32 i2 = index + 1 < m_count ? index + 1 : 0;\r
+ int32 i3 = index + 2;\r
+ while (i3 >= m_count)\r
+ {\r
+ i3 -= m_count;\r
+ }\r
+\r
+ edge->m_vertex0 = m_vertices[i0];\r
+ edge->m_vertex1 = m_vertices[i1];\r
+ edge->m_vertex2 = m_vertices[i2];\r
+ edge->m_vertex3 = m_vertices[i3];\r
+}\r
+\r
+bool b2LoopShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const\r
+{\r
+ B2_NOT_USED(xf);\r
+ B2_NOT_USED(p);\r
+ return false;\r
+}\r
+\r
+bool b2LoopShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& xf, int32 childIndex) const\r
+{\r
+ b2Assert(childIndex < m_count);\r
+\r
+ b2EdgeShape edgeShape;\r
+\r
+ int32 i1 = childIndex;\r
+ int32 i2 = childIndex + 1;\r
+ if (i2 == m_count)\r
+ {\r
+ i2 = 0;\r
+ }\r
+\r
+ edgeShape.m_vertex1 = m_vertices[i1];\r
+ edgeShape.m_vertex2 = m_vertices[i2];\r
+\r
+ return edgeShape.RayCast(output, input, xf, 0);\r
+}\r
+\r
+void b2LoopShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const\r
+{\r
+ b2Assert(childIndex < m_count);\r
+\r
+ int32 i1 = childIndex;\r
+ int32 i2 = childIndex + 1;\r
+ if (i2 == m_count)\r
+ {\r
+ i2 = 0;\r
+ }\r
+\r
+ b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);\r
+ b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);\r
+\r
+ aabb->lowerBound = b2Min(v1, v2);\r
+ aabb->upperBound = b2Max(v1, v2);\r
+}\r
+\r
+void b2LoopShape::ComputeMass(b2MassData* massData, float32 density) const\r
+{\r
+ B2_NOT_USED(density);\r
+\r
+ massData->mass = 0.0f;\r
+ massData->center.SetZero();\r
+ massData->I = 0.0f;\r
+}\r
--- /dev/null
+/*\r
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#ifndef B2_LOOP_SHAPE_H\r
+#define B2_LOOP_SHAPE_H\r
+\r
+#include <Box2D/Collision/Shapes/b2Shape.h>\r
+\r
+class b2EdgeShape;\r
+\r
+/// A loop shape is a free form sequence of line segments that form a circular list.\r
+/// The loop may cross upon itself, but this is not recommended for smooth collision.\r
+/// The loop has double sided collision, so you can use inside and outside collision.\r
+/// Therefore, you may use any winding order.\r
+/// Since there may be many vertices, they are allocated using b2Alloc.\r
+class b2LoopShape : public b2Shape\r
+{\r
+public:\r
+ b2LoopShape();\r
+\r
+ /// The destructor frees the vertices using b2Free.\r
+ ~b2LoopShape();\r
+\r
+ /// Create the loop shape, copy all vertices.\r
+ void Create(const b2Vec2* vertices, int32 count);\r
+\r
+ /// Implement b2Shape. Vertices are cloned using b2Alloc.\r
+ b2Shape* Clone(b2BlockAllocator* allocator) const;\r
+\r
+ /// @see b2Shape::GetChildCount\r
+ int32 GetChildCount() const;\r
+\r
+ /// Get a child edge.\r
+ void GetChildEdge(b2EdgeShape* edge, int32 index) const;\r
+\r
+ /// This always return false.\r
+ /// @see b2Shape::TestPoint\r
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;\r
+\r
+ /// Implement b2Shape.\r
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const;\r
+\r
+ /// @see b2Shape::ComputeAABB\r
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;\r
+\r
+ /// Chains have zero mass.\r
+ /// @see b2Shape::ComputeMass\r
+ void ComputeMass(b2MassData* massData, float32 density) const;\r
+\r
+ /// Get the number of vertices.\r
+ int32 GetCount() const { return m_count; }\r
+\r
+ /// Get the vertices (read-only).\r
+ const b2Vec2& GetVertex(int32 index) const\r
+ {\r
+ b2Assert(0 <= index && index < m_count);\r
+ return m_vertices[index];\r
+ }\r
+\r
+ /// Get the vertices (read-only).\r
+ const b2Vec2* GetVertices() const { return m_vertices; }\r
+\r
+protected:\r
+\r
+ /// The vertices. Owned by this class.\r
+ b2Vec2* m_vertices;\r
+\r
+ /// The vertex count.\r
+ int32 m_count;\r
+};\r
+\r
+inline b2LoopShape::b2LoopShape()\r
+{\r
+ m_type = e_loop;\r
+ m_radius = b2_polygonRadius;\r
+ m_vertices = NULL;\r
+ m_count = 0;\r
+}\r
+\r
+#endif\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
}\r
}\r
\r
-void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)\r
+int32 b2PolygonShape::GetChildCount() const\r
{\r
- m_vertexCount = 2;\r
- m_vertices[0] = v1;\r
- m_vertices[1] = v2;\r
- m_centroid = 0.5f * (v1 + v2);\r
- m_normals[0] = b2Cross(v2 - v1, 1.0f);\r
- m_normals[0].Normalize();\r
- m_normals[1] = -m_normals[0];\r
+ return 1;\r
}\r
\r
static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)\r
{\r
- b2Assert(count >= 2);\r
+ b2Assert(count >= 3);\r
\r
b2Vec2 c; c.Set(0.0f, 0.0f);\r
float32 area = 0.0f;\r
\r
- if (count == 2)\r
- {\r
- c = 0.5f * (vs[0] + vs[1]);\r
- return c;\r
- }\r
-\r
// pRef is the reference point for forming triangles.\r
// It's location doesn't change the result (except for rounding error).\r
b2Vec2 pRef(0.0f, 0.0f);\r
\r
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)\r
{\r
- b2Assert(2 <= count && count <= b2_maxPolygonVertices);\r
+ b2Assert(3 <= count && count <= b2_maxPolygonVertices);\r
m_vertexCount = count;\r
\r
// Copy vertices.\r
return true;\r
}\r
\r
-bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const\r
+bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& xf, int32 childIndex) const\r
{\r
+ B2_NOT_USED(childIndex);\r
+\r
// Put the ray into the polygon's frame of reference.\r
b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);\r
b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);\r
b2Vec2 d = p2 - p1;\r
\r
- if (m_vertexCount == 2)\r
- {\r
- b2Vec2 v1 = m_vertices[0];\r
- b2Vec2 v2 = m_vertices[1];\r
- b2Vec2 normal = m_normals[0];\r
-\r
- // q = p1 + t * d\r
- // dot(normal, q - v1) = 0\r
- // dot(normal, p1 - v1) + t * dot(normal, d) = 0\r
- float32 numerator = b2Dot(normal, v1 - p1);\r
- float32 denominator = b2Dot(normal, d);\r
-\r
- if (denominator == 0.0f)\r
- {\r
- return false;\r
- }\r
- \r
- float32 t = numerator / denominator;\r
- if (t < 0.0f || 1.0f < t)\r
- {\r
- return false;\r
- }\r
-\r
- b2Vec2 q = p1 + t * d;\r
+ float32 lower = 0.0f, upper = input.maxFraction;\r
\r
- // q = v1 + s * r\r
- // s = dot(q - v1, r) / dot(r, r)\r
- b2Vec2 r = v2 - v1;\r
- float32 rr = b2Dot(r, r);\r
- if (rr == 0.0f)\r
- {\r
- return false;\r
- }\r
+ int32 index = -1;\r
\r
- float32 s = b2Dot(q - v1, r) / rr;\r
- if (s < 0.0f || 1.0f < s)\r
- {\r
- return false;\r
- }\r
+ for (int32 i = 0; i < m_vertexCount; ++i)\r
+ {\r
+ // p = p1 + a * d\r
+ // dot(normal, p - v) = 0\r
+ // dot(normal, p1 - v) + a * dot(normal, d) = 0\r
+ float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);\r
+ float32 denominator = b2Dot(m_normals[i], d);\r
\r
- output->fraction = t;\r
- if (numerator > 0.0f)\r
- {\r
- output->normal = -normal;\r
+ if (denominator == 0.0f)\r
+ { \r
+ if (numerator < 0.0f)\r
+ {\r
+ return false;\r
+ }\r
}\r
else\r
{\r
- output->normal = normal;\r
- }\r
- return true;\r
- }\r
- else\r
- {\r
- float32 lower = 0.0f, upper = input.maxFraction;\r
-\r
- int32 index = -1;\r
-\r
- for (int32 i = 0; i < m_vertexCount; ++i)\r
- {\r
- // p = p1 + a * d\r
- // dot(normal, p - v) = 0\r
- // dot(normal, p1 - v) + a * dot(normal, d) = 0\r
- float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);\r
- float32 denominator = b2Dot(m_normals[i], d);\r
-\r
- if (denominator == 0.0f)\r
- { \r
- if (numerator < 0.0f)\r
- {\r
- return false;\r
- }\r
- }\r
- else\r
+ // Note: we want this predicate without division:\r
+ // lower < numerator / denominator, where denominator < 0\r
+ // Since denominator < 0, we have to flip the inequality:\r
+ // lower < numerator / denominator <==> denominator * lower > numerator.\r
+ if (denominator < 0.0f && numerator < lower * denominator)\r
{\r
- // Note: we want this predicate without division:\r
- // lower < numerator / denominator, where denominator < 0\r
- // Since denominator < 0, we have to flip the inequality:\r
- // lower < numerator / denominator <==> denominator * lower > numerator.\r
- if (denominator < 0.0f && numerator < lower * denominator)\r
- {\r
- // Increase lower.\r
- // The segment enters this half-space.\r
- lower = numerator / denominator;\r
- index = i;\r
- }\r
- else if (denominator > 0.0f && numerator < upper * denominator)\r
- {\r
- // Decrease upper.\r
- // The segment exits this half-space.\r
- upper = numerator / denominator;\r
- }\r
+ // Increase lower.\r
+ // The segment enters this half-space.\r
+ lower = numerator / denominator;\r
+ index = i;\r
}\r
-\r
- // The use of epsilon here causes the assert on lower to trip\r
- // in some cases. Apparently the use of epsilon was to make edge\r
- // shapes work, but now those are handled separately.\r
- //if (upper < lower - b2_epsilon)\r
- if (upper < lower)\r
+ else if (denominator > 0.0f && numerator < upper * denominator)\r
{\r
- return false;\r
+ // Decrease upper.\r
+ // The segment exits this half-space.\r
+ upper = numerator / denominator;\r
}\r
}\r
\r
- b2Assert(0.0f <= lower && lower <= input.maxFraction);\r
-\r
- if (index >= 0)\r
+ // The use of epsilon here causes the assert on lower to trip\r
+ // in some cases. Apparently the use of epsilon was to make edge\r
+ // shapes work, but now those are handled separately.\r
+ //if (upper < lower - b2_epsilon)\r
+ if (upper < lower)\r
{\r
- output->fraction = lower;\r
- output->normal = b2Mul(xf.R, m_normals[index]);\r
- return true;\r
+ return false;\r
}\r
}\r
\r
+ b2Assert(0.0f <= lower && lower <= input.maxFraction);\r
+\r
+ if (index >= 0)\r
+ {\r
+ output->fraction = lower;\r
+ output->normal = b2Mul(xf.R, m_normals[index]);\r
+ return true;\r
+ }\r
+\r
return false;\r
}\r
\r
-void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const\r
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const\r
{\r
+ B2_NOT_USED(childIndex);\r
+\r
b2Vec2 lower = b2Mul(xf, m_vertices[0]);\r
b2Vec2 upper = lower;\r
\r
//\r
// The rest of the derivation is handled by computer algebra.\r
\r
- b2Assert(m_vertexCount >= 2);\r
-\r
- // A line segment has zero mass.\r
- if (m_vertexCount == 2)\r
- {\r
- massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);\r
- massData->mass = 0.0f;\r
- massData->I = 0.0f;\r
- return;\r
- }\r
+ b2Assert(m_vertexCount >= 3);\r
\r
b2Vec2 center; center.Set(0.0f, 0.0f);\r
float32 area = 0.0f;\r
float32 I = 0.0f;\r
\r
- // pRef is the reference point for forming triangles.\r
+ // s is the reference point for forming triangles.\r
// It's location doesn't change the result (except for rounding error).\r
- b2Vec2 pRef(0.0f, 0.0f);\r
-#if 0\r
+ b2Vec2 s(0.0f, 0.0f);\r
+\r
// This code would put the reference point inside the polygon.\r
for (int32 i = 0; i < m_vertexCount; ++i)\r
{\r
- pRef += m_vertices[i];\r
+ s += m_vertices[i];\r
}\r
- pRef *= 1.0f / count;\r
-#endif\r
+ s *= 1.0f / m_vertexCount;\r
\r
const float32 k_inv3 = 1.0f / 3.0f;\r
\r
for (int32 i = 0; i < m_vertexCount; ++i)\r
{\r
// Triangle vertices.\r
- b2Vec2 p1 = pRef;\r
- b2Vec2 p2 = m_vertices[i];\r
- b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];\r
-\r
- b2Vec2 e1 = p2 - p1;\r
- b2Vec2 e2 = p3 - p1;\r
+ b2Vec2 e1 = m_vertices[i] - s;\r
+ b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s;\r
\r
float32 D = b2Cross(e1, e2);\r
\r
area += triangleArea;\r
\r
// Area weighted centroid\r
- center += triangleArea * k_inv3 * (p1 + p2 + p3);\r
+ center += triangleArea * k_inv3 * (e1 + e2);\r
\r
- float32 px = p1.x, py = p1.y;\r
float32 ex1 = e1.x, ey1 = e1.y;\r
float32 ex2 = e2.x, ey2 = e2.y;\r
\r
- float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;\r
- float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;\r
+ float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;\r
+ float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;\r
\r
- I += D * (intx2 + inty2);\r
+ I += (0.25f * k_inv3 * D) * (intx2 + inty2);\r
}\r
\r
// Total mass\r
// Center of mass\r
b2Assert(area > b2_epsilon);\r
center *= 1.0f / area;\r
- massData->center = center;\r
+ massData->center = center + s;\r
\r
// Inertia tensor relative to the local origin.\r
- massData->I = density * I;\r
+ massData->I = density * I + massData->mass * b2Dot(s, s);\r
}\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
/// A convex polygon. It is assumed that the interior of the polygon is to\r
/// the left of each edge.\r
+/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.\r
+/// In most cases you should not need many vertices for a convex polygon.\r
class b2PolygonShape : public b2Shape\r
{\r
public:\r
/// Implement b2Shape.\r
b2Shape* Clone(b2BlockAllocator* allocator) const;\r
\r
+ /// @see b2Shape::GetChildCount\r
+ int32 GetChildCount() const;\r
+\r
/// Copy vertices. This assumes the vertices define a convex polygon.\r
/// It is assumed that the exterior is the the right of each edge.\r
+ /// The count must be in the range [3, b2_maxPolygonVertices].\r
void Set(const b2Vec2* vertices, int32 vertexCount);\r
\r
/// Build vertices to represent an axis-aligned box.\r
/// @param angle the rotation of the box in local coordinates.\r
void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);\r
\r
- /// Set this as a single edge.\r
- void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2);\r
-\r
/// @see b2Shape::TestPoint\r
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;\r
\r
/// Implement b2Shape.\r
- bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;\r
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const;\r
\r
/// @see b2Shape::ComputeAABB\r
- void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;\r
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;\r
\r
/// @see b2Shape::ComputeMass\r
void ComputeMass(b2MassData* massData, float32 density) const;\r
\r
- /// Get the supporting vertex index in the given direction.\r
- int32 GetSupport(const b2Vec2& d) const;\r
-\r
- /// Get the supporting vertex in the given direction.\r
- const b2Vec2& GetSupportVertex(const b2Vec2& d) const;\r
-\r
/// Get the vertex count.\r
int32 GetVertexCount() const { return m_vertexCount; }\r
\r
m_centroid.SetZero();\r
}\r
\r
-inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const\r
-{\r
- int32 bestIndex = 0;\r
- float32 bestValue = b2Dot(m_vertices[0], d);\r
- for (int32 i = 1; i < m_vertexCount; ++i)\r
- {\r
- float32 value = b2Dot(m_vertices[i], d);\r
- if (value > bestValue)\r
- {\r
- bestIndex = i;\r
- bestValue = value;\r
- }\r
- }\r
-\r
- return bestIndex;\r
-}\r
-\r
-inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const\r
-{\r
- int32 bestIndex = 0;\r
- float32 bestValue = b2Dot(m_vertices[0], d);\r
- for (int32 i = 1; i < m_vertexCount; ++i)\r
- {\r
- float32 value = b2Dot(m_vertices[i], d);\r
- if (value > bestValue)\r
- {\r
- bestIndex = i;\r
- bestValue = value;\r
- }\r
- }\r
-\r
- return m_vertices[bestIndex];\r
-}\r
-\r
inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const\r
{\r
b2Assert(0 <= index && index < m_vertexCount);\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
/// A shape is used for collision detection. You can create a shape however you like.\r
/// Shapes used for simulation in b2World are created automatically when a b2Fixture\r
-/// is created.\r
+/// is created. Shapes may encapsulate a one or more child shapes.\r
class b2Shape\r
{\r
public:\r
{\r
e_unknown= -1,\r
e_circle = 0,\r
- e_polygon = 1,\r
- e_typeCount = 2,\r
+ e_edge = 1,\r
+ e_polygon = 2,\r
+ e_loop = 3,\r
+ e_typeCount = 4\r
};\r
\r
b2Shape() { m_type = e_unknown; }\r
/// @return the shape type.\r
Type GetType() const;\r
\r
+ /// Get the number of child primitives.\r
+ virtual int32 GetChildCount() const = 0;\r
+\r
/// Test a point for containment in this shape. This only works for convex shapes.\r
/// @param xf the shape world transform.\r
/// @param p a point in world coordinates.\r
virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;\r
\r
- /// Cast a ray against this shape.\r
+ /// Cast a ray against a child shape.\r
/// @param output the ray-cast results.\r
/// @param input the ray-cast input parameters.\r
/// @param transform the transform to be applied to the shape.\r
- virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0;\r
+ /// @param childIndex the child shape index\r
+ virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,\r
+ const b2Transform& transform, int32 childIndex) const = 0;\r
\r
- /// Given a transform, compute the associated axis aligned bounding box for this shape.\r
+ /// Given a transform, compute the associated axis aligned bounding box for a child shape.\r
/// @param aabb returns the axis aligned box.\r
/// @param xf the world transform of the shape.\r
- virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) const = 0;\r
+ /// @param childIndex the child shape\r
+ virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;\r
\r
/// Compute the mass properties of this shape using its dimensions and density.\r
/// The inertia tensor is computed about the local origin.\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
#include <Box2D/Collision/b2BroadPhase.h>\r
#include <cstring>\r
+using namespace std;\r
\r
b2BroadPhase::b2BroadPhase()\r
{\r
}\r
}\r
\r
+void b2BroadPhase::TouchProxy(int32 proxyId)\r
+{\r
+ BufferMove(proxyId);\r
+}\r
+\r
void b2BroadPhase::BufferMove(int32 proxyId)\r
{\r
if (m_moveCount == m_moveCapacity)\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
enum\r
{\r
- e_nullProxy = -1,\r
+ e_nullProxy = -1\r
};\r
\r
b2BroadPhase();\r
/// call UpdatePairs to finalized the proxy pairs (for your time step).\r
void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);\r
\r
+ /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.\r
+ void TouchProxy(int32 proxyId);\r
+\r
/// Get the fat AABB for a proxy.\r
const b2AABB& GetFatAABB(int32 proxyId) const;\r
\r
template <typename T>\r
void RayCast(T* callback, const b2RayCastInput& input) const;\r
\r
- /// Compute the height of the embedded tree.\r
- int32 ComputeHeight() const;\r
+ /// Get the height of the embedded tree.\r
+ int32 GetTreeHeight() const;\r
+\r
+ /// Get the balance of the embedded tree.\r
+ int32 GetTreeBalance() const;\r
+\r
+ /// Get the quality metric of the embedded tree.\r
+ float32 GetTreeQuality() const;\r
\r
private:\r
\r
return m_proxyCount;\r
}\r
\r
-inline int32 b2BroadPhase::ComputeHeight() const\r
+inline int32 b2BroadPhase::GetTreeHeight() const\r
+{\r
+ return m_tree.GetHeight();\r
+}\r
+\r
+inline int32 b2BroadPhase::GetTreeBalance() const\r
+{\r
+ return m_tree.GetMaxBalance();\r
+}\r
+\r
+inline float32 b2BroadPhase::GetTreeQuality() const\r
{\r
- return m_tree.ComputeHeight();\r
+ return m_tree.GetAreaRatio();\r
}\r
\r
template <typename T>\r
}\r
\r
// Try to keep the tree balanced.\r
- m_tree.Rebalance(4);\r
+ //m_tree.Rebalance(4);\r
}\r
\r
template <typename T>\r
/*\r
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
--- /dev/null
+/*\r
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#include <Box2D/Collision/b2Collision.h>\r
+#include <Box2D/Collision/Shapes/b2CircleShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+#include <Box2D/Collision/Shapes/b2PolygonShape.h>\r
+\r
+enum b2EdgeType\r
+{\r
+ b2_isolated,\r
+ b2_concave,\r
+ b2_flat,\r
+ b2_convex\r
+};\r
+\r
+// Compute contact points for edge versus circle.\r
+// This accounts for edge connectivity.\r
+void b2CollideEdgeAndCircle(b2Manifold* manifold,\r
+ const b2EdgeShape* edgeA, const b2Transform& xfA,\r
+ const b2CircleShape* circleB, const b2Transform& xfB)\r
+{\r
+ manifold->pointCount = 0;\r
+\r
+ // Compute circle in frame of edge\r
+ b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));\r
+\r
+ b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;\r
+ b2Vec2 e = B - A;\r
+\r
+ // Barycentric coordinates\r
+ float32 u = b2Dot(e, B - Q);\r
+ float32 v = b2Dot(e, Q - A);\r
+\r
+ float32 radius = edgeA->m_radius + circleB->m_radius;\r
+\r
+ b2ContactFeature cf;\r
+ cf.indexB = 0;\r
+ cf.typeB = b2ContactFeature::e_vertex;\r
+\r
+ // Region A\r
+ if (v <= 0.0f)\r
+ {\r
+ b2Vec2 P = A;\r
+ b2Vec2 d = Q - P;\r
+ float32 dd = b2Dot(d, d);\r
+ if (dd > radius * radius)\r
+ {\r
+ return;\r
+ }\r
+\r
+ // Is there an edge connected to A?\r
+ if (edgeA->m_hasVertex0)\r
+ {\r
+ b2Vec2 A1 = edgeA->m_vertex0;\r
+ b2Vec2 B1 = A;\r
+ b2Vec2 e1 = B1 - A1;\r
+ float32 u1 = b2Dot(e1, B1 - Q);\r
+\r
+ // Is the circle in Region AB of the previous edge?\r
+ if (u1 > 0.0f)\r
+ {\r
+ return;\r
+ }\r
+ }\r
+\r
+ cf.indexA = 0;\r
+ cf.typeA = b2ContactFeature::e_vertex;\r
+ manifold->pointCount = 1;\r
+ manifold->type = b2Manifold::e_circles;\r
+ manifold->localNormal.SetZero();\r
+ manifold->localPoint = P;\r
+ manifold->points[0].id.key = 0;\r
+ manifold->points[0].id.cf = cf;\r
+ manifold->points[0].localPoint = circleB->m_p;\r
+ return;\r
+ }\r
+ \r
+ // Region B\r
+ if (u <= 0.0f)\r
+ {\r
+ b2Vec2 P = B;\r
+ b2Vec2 d = Q - P;\r
+ float32 dd = b2Dot(d, d);\r
+ if (dd > radius * radius)\r
+ {\r
+ return;\r
+ }\r
+\r
+ // Is there an edge connected to B?\r
+ if (edgeA->m_hasVertex3)\r
+ {\r
+ b2Vec2 B2 = edgeA->m_vertex3;\r
+ b2Vec2 A2 = B;\r
+ b2Vec2 e2 = B2 - A2;\r
+ float32 v2 = b2Dot(e2, Q - A2);\r
+\r
+ // Is the circle in Region AB of the next edge?\r
+ if (v2 > 0.0f)\r
+ {\r
+ return;\r
+ }\r
+ }\r
+\r
+ cf.indexA = 1;\r
+ cf.typeA = b2ContactFeature::e_vertex;\r
+ manifold->pointCount = 1;\r
+ manifold->type = b2Manifold::e_circles;\r
+ manifold->localNormal.SetZero();\r
+ manifold->localPoint = P;\r
+ manifold->points[0].id.key = 0;\r
+ manifold->points[0].id.cf = cf;\r
+ manifold->points[0].localPoint = circleB->m_p;\r
+ return;\r
+ }\r
+\r
+ // Region AB\r
+ float32 den = b2Dot(e, e);\r
+ b2Assert(den > 0.0f);\r
+ b2Vec2 P = (1.0f / den) * (u * A + v * B);\r
+ b2Vec2 d = Q - P;\r
+ float32 dd = b2Dot(d, d);\r
+ if (dd > radius * radius)\r
+ {\r
+ return;\r
+ }\r
+\r
+ b2Vec2 n(-e.y, e.x);\r
+ if (b2Dot(n, Q - A) < 0.0f)\r
+ {\r
+ n.Set(-n.x, -n.y);\r
+ }\r
+ n.Normalize();\r
+\r
+ cf.indexA = 0;\r
+ cf.typeA = b2ContactFeature::e_face;\r
+ manifold->pointCount = 1;\r
+ manifold->type = b2Manifold::e_faceA;\r
+ manifold->localNormal = n;\r
+ manifold->localPoint = A;\r
+ manifold->points[0].id.key = 0;\r
+ manifold->points[0].id.cf = cf;\r
+ manifold->points[0].localPoint = circleB->m_p;\r
+}\r
+\r
+struct b2EPAxis\r
+{\r
+ enum Type\r
+ {\r
+ e_unknown,\r
+ e_edgeA,\r
+ e_edgeB\r
+ };\r
+\r
+ Type type;\r
+ int32 index;\r
+ float32 separation;\r
+};\r
+\r
+// Edge shape plus more stuff.\r
+struct b2FatEdge\r
+{\r
+ b2Vec2 v0, v1, v2, v3;\r
+ b2Vec2 normal;\r
+ bool hasVertex0, hasVertex3;\r
+};\r
+\r
+// This lets us treate and edge shape and a polygon in the same\r
+// way in the SAT collider.\r
+struct b2EPProxy\r
+{\r
+ b2Vec2 vertices[b2_maxPolygonVertices];\r
+ b2Vec2 normals[b2_maxPolygonVertices];\r
+ b2Vec2 centroid;\r
+ int32 count;\r
+};\r
+\r
+// This class collides and edge and a polygon, taking into account edge adjacency.\r
+struct b2EPCollider\r
+{\r
+ b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA,\r
+ const b2PolygonShape* polygonB_in, const b2Transform& xfB);\r
+\r
+ void Collide(b2Manifold* manifold);\r
+\r
+ void ComputeAdjacency();\r
+ b2EPAxis ComputeEdgeSeparation();\r
+ b2EPAxis ComputePolygonSeparation();\r
+ void FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2);\r
+\r
+ b2FatEdge m_edgeA;\r
+\r
+ b2EPProxy m_proxyA, m_proxyB;\r
+\r
+ b2Transform m_xf;\r
+ b2Vec2 m_normal0, m_normal2;\r
+ b2Vec2 m_limit11, m_limit12;\r
+ b2Vec2 m_limit21, m_limit22;\r
+ float32 m_radius;\r
+};\r
+\r
+b2EPCollider::b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA,\r
+ const b2PolygonShape* polygonB, const b2Transform& xfB)\r
+{\r
+ m_xf = b2MulT(xfA, xfB);\r
+\r
+ // Edge geometry\r
+ m_edgeA.v0 = edgeA->m_vertex0;\r
+ m_edgeA.v1 = edgeA->m_vertex1;\r
+ m_edgeA.v2 = edgeA->m_vertex2;\r
+ m_edgeA.v3 = edgeA->m_vertex3;\r
+ b2Vec2 e = m_edgeA.v2 - m_edgeA.v1;\r
+\r
+ // Normal points outwards in CCW order.\r
+ m_edgeA.normal.Set(e.y, -e.x);\r
+ m_edgeA.normal.Normalize();\r
+ m_edgeA.hasVertex0 = edgeA->m_hasVertex0;\r
+ m_edgeA.hasVertex3 = edgeA->m_hasVertex3;\r
+\r
+ // Proxy for edge\r
+ m_proxyA.vertices[0] = m_edgeA.v1;\r
+ m_proxyA.vertices[1] = m_edgeA.v2;\r
+ m_proxyA.normals[0] = m_edgeA.normal;\r
+ m_proxyA.normals[1] = -m_edgeA.normal;\r
+ m_proxyA.centroid = 0.5f * (m_edgeA.v1 + m_edgeA.v2);\r
+ m_proxyA.count = 2;\r
+\r
+ // Proxy for polygon\r
+ m_proxyB.count = polygonB->m_vertexCount;\r
+ m_proxyB.centroid = b2Mul(m_xf, polygonB->m_centroid);\r
+ for (int32 i = 0; i < polygonB->m_vertexCount; ++i)\r
+ {\r
+ m_proxyB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);\r
+ m_proxyB.normals[i] = b2Mul(m_xf.R, polygonB->m_normals[i]);\r
+ }\r
+\r
+ m_radius = 2.0f * b2_polygonRadius;\r
+\r
+ m_limit11.SetZero();\r
+ m_limit12.SetZero();\r
+ m_limit21.SetZero();\r
+ m_limit22.SetZero();\r
+}\r
+\r
+// Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.\r
+// Edge adjacency is handle to produce locally valid contact points and normals. This is intended\r
+// to allow the polygon to slide smoothly over an edge chain.\r
+//\r
+// Algorithm\r
+// 1. Classify front-side or back-side collision with edge.\r
+// 2. Compute separation\r
+// 3. Process adjacent edges\r
+// 4. Classify adjacent edge as convex, flat, null, or concave\r
+// 5. Skip null or concave edges. Concave edges get a separate manifold.\r
+// 6. If the edge is flat, compute contact points as normal. Discard boundary points.\r
+// 7. If the edge is convex, compute it's separation.\r
+// 8. Use the minimum separation of up to three edges. If the minimum separation\r
+// is not the primary edge, return.\r
+// 9. If the minimum separation is the primary edge, compute the contact points and return.\r
+void b2EPCollider::Collide(b2Manifold* manifold)\r
+{\r
+ manifold->pointCount = 0;\r
+\r
+ ComputeAdjacency();\r
+\r
+ b2EPAxis edgeAxis = ComputeEdgeSeparation();\r
+\r
+ // If no valid normal can be found than this edge should not collide.\r
+ // This can happen on the middle edge of a 3-edge zig-zag chain.\r
+ if (edgeAxis.type == b2EPAxis::e_unknown)\r
+ {\r
+ return;\r
+ }\r
+\r
+ if (edgeAxis.separation > m_radius)\r
+ {\r
+ return;\r
+ }\r
+\r
+ b2EPAxis polygonAxis = ComputePolygonSeparation();\r
+ if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)\r
+ {\r
+ return;\r
+ }\r
+\r
+ // Use hysteresis for jitter reduction.\r
+ const float32 k_relativeTol = 0.98f;\r
+ const float32 k_absoluteTol = 0.001f;\r
+\r
+ b2EPAxis primaryAxis;\r
+ if (polygonAxis.type == b2EPAxis::e_unknown)\r
+ {\r
+ primaryAxis = edgeAxis;\r
+ }\r
+ else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)\r
+ {\r
+ primaryAxis = polygonAxis;\r
+ }\r
+ else\r
+ {\r
+ primaryAxis = edgeAxis;\r
+ }\r
+\r
+ b2EPProxy* proxy1;\r
+ b2EPProxy* proxy2;\r
+ b2ClipVertex incidentEdge[2];\r
+ if (primaryAxis.type == b2EPAxis::e_edgeA)\r
+ {\r
+ proxy1 = &m_proxyA;\r
+ proxy2 = &m_proxyB;\r
+ manifold->type = b2Manifold::e_faceA;\r
+ }\r
+ else\r
+ {\r
+ proxy1 = &m_proxyB;\r
+ proxy2 = &m_proxyA;\r
+ manifold->type = b2Manifold::e_faceB;\r
+ }\r
+\r
+ int32 edge1 = primaryAxis.index;\r
+\r
+ FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);\r
+ int32 count1 = proxy1->count;\r
+ const b2Vec2* vertices1 = proxy1->vertices;\r
+\r
+ int32 iv1 = edge1;\r
+ int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;\r
+\r
+ b2Vec2 v11 = vertices1[iv1];\r
+ b2Vec2 v12 = vertices1[iv2];\r
+\r
+ b2Vec2 tangent = v12 - v11;\r
+ tangent.Normalize();\r
+ \r
+ b2Vec2 normal = b2Cross(tangent, 1.0f);\r
+ b2Vec2 planePoint = 0.5f * (v11 + v12);\r
+\r
+ // Face offset.\r
+ float32 frontOffset = b2Dot(normal, v11);\r
+\r
+ // Side offsets, extended by polytope skin thickness.\r
+ float32 sideOffset1 = -b2Dot(tangent, v11) + m_radius;\r
+ float32 sideOffset2 = b2Dot(tangent, v12) + m_radius;\r
+\r
+ // Clip incident edge against extruded edge1 side edges.\r
+ b2ClipVertex clipPoints1[2];\r
+ b2ClipVertex clipPoints2[2];\r
+ int np;\r
+\r
+ // Clip to box side 1\r
+ np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);\r
+\r
+ if (np < b2_maxManifoldPoints)\r
+ {\r
+ return;\r
+ }\r
+\r
+ // Clip to negative box side 1\r
+ np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);\r
+\r
+ if (np < b2_maxManifoldPoints)\r
+ {\r
+ return;\r
+ }\r
+\r
+ // Now clipPoints2 contains the clipped points.\r
+ if (primaryAxis.type == b2EPAxis::e_edgeA)\r
+ {\r
+ manifold->localNormal = normal;\r
+ manifold->localPoint = planePoint;\r
+ }\r
+ else\r
+ {\r
+ manifold->localNormal = b2MulT(m_xf.R, normal);\r
+ manifold->localPoint = b2MulT(m_xf, planePoint);\r
+ }\r
+\r
+ int32 pointCount = 0;\r
+ for (int32 i = 0; i < b2_maxManifoldPoints; ++i)\r
+ {\r
+ float32 separation;\r
+ \r
+ separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;\r
+\r
+ if (separation <= m_radius)\r
+ {\r
+ b2ManifoldPoint* cp = manifold->points + pointCount;\r
+\r
+ if (primaryAxis.type == b2EPAxis::e_edgeA)\r
+ {\r
+ cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);\r
+ cp->id = clipPoints2[i].id;\r
+ }\r
+ else\r
+ {\r
+ cp->localPoint = clipPoints2[i].v;\r
+ cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;\r
+ cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;\r
+ cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;\r
+ cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;\r
+ }\r
+\r
+ ++pointCount;\r
+ }\r
+ }\r
+\r
+ manifold->pointCount = pointCount;\r
+}\r
+\r
+// Compute allowable normal ranges based on adjacency.\r
+// A normal n is allowable iff:\r
+// cross(n, n1) >= 0.0f && cross(n2, n) >= 0.0f\r
+// n points from A to B (edge to polygon)\r
+void b2EPCollider::ComputeAdjacency()\r
+{\r
+ b2Vec2 v0 = m_edgeA.v0;\r
+ b2Vec2 v1 = m_edgeA.v1;\r
+ b2Vec2 v2 = m_edgeA.v2;\r
+ b2Vec2 v3 = m_edgeA.v3;\r
+\r
+ // Determine allowable the normal regions based on adjacency.\r
+ // Note: it may be possible that no normal is admissable.\r
+ b2Vec2 centerB = m_proxyB.centroid;\r
+ if (m_edgeA.hasVertex0)\r
+ {\r
+ b2Vec2 e0 = v1 - v0;\r
+ b2Vec2 e1 = v2 - v1;\r
+ b2Vec2 n0(e0.y, -e0.x);\r
+ b2Vec2 n1(e1.y, -e1.x);\r
+ n0.Normalize();\r
+ n1.Normalize();\r
+\r
+ bool convex = b2Cross(n0, n1) >= 0.0f;\r
+ bool front0 = b2Dot(n0, centerB - v0) >= 0.0f;\r
+ bool front1 = b2Dot(n1, centerB - v1) >= 0.0f;\r
+\r
+ if (convex)\r
+ {\r
+ if (front0 || front1)\r
+ {\r
+ m_limit11 = n1;\r
+ m_limit12 = n0;\r
+ }\r
+ else\r
+ {\r
+ m_limit11 = -n1;\r
+ m_limit12 = -n0;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (front0 && front1)\r
+ {\r
+ m_limit11 = n0;\r
+ m_limit12 = n1;\r
+ }\r
+ else\r
+ {\r
+ m_limit11 = -n0;\r
+ m_limit12 = -n1;\r
+ }\r
+ }\r
+ }\r
+ else\r
+ {\r
+ m_limit11.SetZero();\r
+ m_limit12.SetZero();\r
+ }\r
+\r
+ if (m_edgeA.hasVertex3)\r
+ {\r
+ b2Vec2 e1 = v2 - v1;\r
+ b2Vec2 e2 = v3 - v2;\r
+ b2Vec2 n1(e1.y, -e1.x);\r
+ b2Vec2 n2(e2.y, -e2.x);\r
+ n1.Normalize();\r
+ n2.Normalize();\r
+\r
+ bool convex = b2Cross(n1, n2) >= 0.0f;\r
+ bool front1 = b2Dot(n1, centerB - v1) >= 0.0f;\r
+ bool front2 = b2Dot(n2, centerB - v2) >= 0.0f;\r
+\r
+ if (convex)\r
+ {\r
+ if (front1 || front2)\r
+ {\r
+ m_limit21 = n2;\r
+ m_limit22 = n1;\r
+ }\r
+ else\r
+ {\r
+ m_limit21 = -n2;\r
+ m_limit22 = -n1;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (front1 && front2)\r
+ {\r
+ m_limit21 = n1;\r
+ m_limit22 = n2;\r
+ }\r
+ else\r
+ {\r
+ m_limit21 = -n1;\r
+ m_limit22 = -n2;\r
+ }\r
+ }\r
+ }\r
+ else\r
+ {\r
+ m_limit21.SetZero();\r
+ m_limit22.SetZero();\r
+ }\r
+}\r
+\r
+b2EPAxis b2EPCollider::ComputeEdgeSeparation()\r
+{\r
+ // EdgeA separation\r
+ b2EPAxis bestAxis;\r
+ bestAxis.type = b2EPAxis::e_unknown;\r
+ bestAxis.index = -1;\r
+ bestAxis.separation = -FLT_MAX;\r
+ b2Vec2 normals[2] = {m_edgeA.normal, -m_edgeA.normal};\r
+ \r
+ for (int32 i = 0; i < 2; ++i)\r
+ {\r
+ b2Vec2 n = normals[i];\r
+\r
+ // Adjacency\r
+ bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop;\r
+ bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop;\r
+\r
+ if (valid1 == false || valid2 == false)\r
+ {\r
+ continue;\r
+ }\r
+ \r
+ b2EPAxis axis;\r
+ axis.type = b2EPAxis::e_edgeA;\r
+ axis.index = i;\r
+ axis.separation = FLT_MAX;\r
+\r
+ for (int32 j = 0; j < m_proxyB.count; ++j)\r
+ {\r
+ float32 s = b2Dot(n, m_proxyB.vertices[j] - m_edgeA.v1);\r
+ if (s < axis.separation)\r
+ {\r
+ axis.separation = s;\r
+ }\r
+ }\r
+\r
+ if (axis.separation > m_radius)\r
+ {\r
+ return axis;\r
+ }\r
+\r
+ if (axis.separation > bestAxis.separation)\r
+ {\r
+ bestAxis = axis;\r
+ }\r
+ }\r
+\r
+ return bestAxis;\r
+}\r
+\r
+b2EPAxis b2EPCollider::ComputePolygonSeparation()\r
+{\r
+ b2EPAxis axis;\r
+ axis.type = b2EPAxis::e_unknown;\r
+ axis.index = -1;\r
+ axis.separation = -FLT_MAX;\r
+ for (int32 i = 0; i < m_proxyB.count; ++i)\r
+ {\r
+ b2Vec2 n = -m_proxyB.normals[i];\r
+\r
+ // Adjacency\r
+ bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop;\r
+ bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop;\r
+\r
+ if (valid1 == false && valid2 == false)\r
+ {\r
+ continue;\r
+ }\r
+\r
+ float32 s1 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v1);\r
+ float32 s2 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v2);\r
+ float32 s = b2Min(s1, s2);\r
+\r
+ if (s > m_radius)\r
+ {\r
+ axis.type = b2EPAxis::e_edgeB;\r
+ axis.index = i;\r
+ axis.separation = s;\r
+ }\r
+\r
+ if (s > axis.separation)\r
+ {\r
+ axis.type = b2EPAxis::e_edgeB;\r
+ axis.index = i;\r
+ axis.separation = s;\r
+ }\r
+ }\r
+\r
+ return axis;\r
+}\r
+\r
+void b2EPCollider::FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2)\r
+{\r
+ int32 count1 = proxy1->count;\r
+ const b2Vec2* normals1 = proxy1->normals;\r
+\r
+ int32 count2 = proxy2->count;\r
+ const b2Vec2* vertices2 = proxy2->vertices;\r
+ const b2Vec2* normals2 = proxy2->normals;\r
+\r
+ b2Assert(0 <= edge1 && edge1 < count1);\r
+\r
+ // Get the normal of the reference edge in proxy2's frame.\r
+ b2Vec2 normal1 = normals1[edge1];\r
+\r
+ // Find the incident edge on proxy2.\r
+ int32 index = 0;\r
+ float32 minDot = b2_maxFloat;\r
+ for (int32 i = 0; i < count2; ++i)\r
+ {\r
+ float32 dot = b2Dot(normal1, normals2[i]);\r
+ if (dot < minDot)\r
+ {\r
+ minDot = dot;\r
+ index = i;\r
+ }\r
+ }\r
+\r
+ // Build the clip vertices for the incident edge.\r
+ int32 i1 = index;\r
+ int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;\r
+\r
+ c[0].v = vertices2[i1];\r
+ c[0].id.cf.indexA = (uint8)edge1;\r
+ c[0].id.cf.indexB = (uint8)i1;\r
+ c[0].id.cf.typeA = b2ContactFeature::e_face;\r
+ c[0].id.cf.typeB = b2ContactFeature::e_vertex;\r
+\r
+ c[1].v = vertices2[i2];\r
+ c[1].id.cf.indexA = (uint8)edge1;\r
+ c[1].id.cf.indexB = (uint8)i2;\r
+ c[1].id.cf.typeA = b2ContactFeature::e_face;\r
+ c[1].id.cf.typeB = b2ContactFeature::e_vertex;\r
+}\r
+\r
+void b2CollideEdgeAndPolygon( b2Manifold* manifold,\r
+ const b2EdgeShape* edgeA, const b2Transform& xfA,\r
+ const b2PolygonShape* polygonB, const b2Transform& xfB)\r
+{\r
+ b2EPCollider collider(edgeA, xfA, polygonB, xfB);\r
+ collider.Collide(manifold);\r
+}\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;\r
\r
c[0].v = b2Mul(xf2, vertices2[i1]);\r
- c[0].id.features.referenceEdge = (uint8)edge1;\r
- c[0].id.features.incidentEdge = (uint8)i1;\r
- c[0].id.features.incidentVertex = 0;\r
+ c[0].id.cf.indexA = (uint8)edge1;\r
+ c[0].id.cf.indexB = (uint8)i1;\r
+ c[0].id.cf.typeA = b2ContactFeature::e_face;\r
+ c[0].id.cf.typeB = b2ContactFeature::e_vertex;\r
\r
c[1].v = b2Mul(xf2, vertices2[i2]);\r
- c[1].id.features.referenceEdge = (uint8)edge1;\r
- c[1].id.features.incidentEdge = (uint8)i2;\r
- c[1].id.features.incidentVertex = 1;\r
+ c[1].id.cf.indexA = (uint8)edge1;\r
+ c[1].id.cf.indexB = (uint8)i2;\r
+ c[1].id.cf.typeA = b2ContactFeature::e_face;\r
+ c[1].id.cf.typeB = b2ContactFeature::e_vertex;\r
}\r
\r
// Find edge normal of max separation on A - return if separating axis is found\r
int32 count1 = poly1->m_vertexCount;\r
const b2Vec2* vertices1 = poly1->m_vertices;\r
\r
- b2Vec2 v11 = vertices1[edge1];\r
- b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];\r
+ int32 iv1 = edge1;\r
+ int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;\r
+\r
+ b2Vec2 v11 = vertices1[iv1];\r
+ b2Vec2 v12 = vertices1[iv2];\r
\r
b2Vec2 localTangent = v12 - v11;\r
localTangent.Normalize();\r
int np;\r
\r
// Clip to box side 1\r
- np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);\r
+ np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);\r
\r
if (np < 2)\r
return;\r
\r
// Clip to negative box side 1\r
- np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);\r
+ np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);\r
\r
if (np < 2)\r
{\r
b2ManifoldPoint* cp = manifold->points + pointCount;\r
cp->localPoint = b2MulT(xf2, clipPoints2[i].v);\r
cp->id = clipPoints2[i].id;\r
- cp->id.features.flip = flip;\r
+ if (flip)\r
+ {\r
+ // Swap features\r
+ b2ContactFeature cf = cp->id.cf;\r
+ cp->id.cf.indexA = cf.indexB;\r
+ cp->id.cf.indexB = cf.indexA;\r
+ cp->id.cf.typeA = cf.typeB;\r
+ cp->id.cf.typeB = cf.typeA;\r
+ }\r
++pointCount;\r
}\r
}\r
/*\r
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
// Sutherland-Hodgman clipping.\r
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],\r
- const b2Vec2& normal, float32 offset)\r
+ const b2Vec2& normal, float32 offset, int32 vertexIndexA)\r
{\r
// Start with no output points\r
int32 numOut = 0;\r
// Find intersection point of edge and plane\r
float32 interp = distance0 / (distance0 - distance1);\r
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);\r
- if (distance0 > 0.0f)\r
- {\r
- vOut[numOut].id = vIn[0].id;\r
- }\r
- else\r
- {\r
- vOut[numOut].id = vIn[1].id;\r
- }\r
+\r
+ // VertexA is hitting edgeB.\r
+ vOut[numOut].id.cf.indexA = vertexIndexA;\r
+ vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;\r
+ vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;\r
+ vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;\r
++numOut;\r
}\r
\r
return numOut;\r
}\r
\r
-bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,\r
- const b2Transform& xfA, const b2Transform& xfB)\r
+bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,\r
+ const b2Shape* shapeB, int32 indexB,\r
+ const b2Transform& xfA, const b2Transform& xfB)\r
{\r
b2DistanceInput input;\r
- input.proxyA.Set(shapeA);\r
- input.proxyB.Set(shapeB);\r
+ input.proxyA.Set(shapeA, indexA);\r
+ input.proxyB.Set(shapeB, indexB);\r
input.transformA = xfA;\r
input.transformB = xfB;\r
input.useRadii = true;\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
class b2Shape;\r
class b2CircleShape;\r
+class b2EdgeShape;\r
class b2PolygonShape;\r
\r
const uint8 b2_nullFeature = UCHAR_MAX;\r
\r
+/// The features that intersect to form the contact point\r
+/// This must be 4 bytes or less.\r
+struct b2ContactFeature\r
+{\r
+ enum Type\r
+ {\r
+ e_vertex = 0,\r
+ e_face = 1\r
+ };\r
+\r
+ uint8 indexA; ///< Feature index on shapeA\r
+ uint8 indexB; ///< Feature index on shapeB\r
+ uint8 typeA; ///< The feature type on shapeA\r
+ uint8 typeB; ///< The feature type on shapeB\r
+};\r
+\r
/// Contact ids to facilitate warm starting.\r
union b2ContactID\r
{\r
- /// The features that intersect to form the contact point\r
- struct Features\r
- {\r
- uint8 referenceEdge; ///< The edge that defines the outward contact normal.\r
- uint8 incidentEdge; ///< The edge most anti-parallel to the reference edge.\r
- uint8 incidentVertex; ///< The vertex (0 or 1) on the incident edge that was clipped.\r
- uint8 flip; ///< A value of 1 indicates that the reference edge is on shape2.\r
- } features;\r
+ b2ContactFeature cf;\r
uint32 key; ///< Used to quickly compare contact ids.\r
};\r
\r
return 0.5f * (upperBound - lowerBound);\r
}\r
\r
+ /// Get the perimeter length\r
+ float32 GetPerimeter() const\r
+ {\r
+ float32 wx = upperBound.x - lowerBound.x;\r
+ float32 wy = upperBound.y - lowerBound.y;\r
+ return 2.0f * (wx + wy);\r
+ }\r
+\r
+ /// Combine an AABB into this one.\r
+ void Combine(const b2AABB& aabb)\r
+ {\r
+ lowerBound = b2Min(lowerBound, aabb.lowerBound);\r
+ upperBound = b2Max(upperBound, aabb.upperBound);\r
+ }\r
+\r
/// Combine two AABBs into this one.\r
void Combine(const b2AABB& aabb1, const b2AABB& aabb2)\r
{\r
\r
/// Compute the collision manifold between two circles.\r
void b2CollideCircles(b2Manifold* manifold,\r
- const b2CircleShape* circle1, const b2Transform& xf1,\r
- const b2CircleShape* circle2, const b2Transform& xf2);\r
+ const b2CircleShape* circleA, const b2Transform& xfA,\r
+ const b2CircleShape* circleB, const b2Transform& xfB);\r
\r
/// Compute the collision manifold between a polygon and a circle.\r
void b2CollidePolygonAndCircle(b2Manifold* manifold,\r
- const b2PolygonShape* polygon, const b2Transform& xf1,\r
- const b2CircleShape* circle, const b2Transform& xf2);\r
+ const b2PolygonShape* polygonA, const b2Transform& xfA,\r
+ const b2CircleShape* circleB, const b2Transform& xfB);\r
\r
/// Compute the collision manifold between two polygons.\r
void b2CollidePolygons(b2Manifold* manifold,\r
- const b2PolygonShape* polygon1, const b2Transform& xf1,\r
- const b2PolygonShape* polygon2, const b2Transform& xf2);\r
+ const b2PolygonShape* polygonA, const b2Transform& xfA,\r
+ const b2PolygonShape* polygonB, const b2Transform& xfB);\r
+\r
+/// Compute the collision manifold between an edge and a circle.\r
+void b2CollideEdgeAndCircle(b2Manifold* manifold,\r
+ const b2EdgeShape* polygonA, const b2Transform& xfA,\r
+ const b2CircleShape* circleB, const b2Transform& xfB);\r
+\r
+/// Compute the collision manifold between an edge and a circle.\r
+void b2CollideEdgeAndPolygon(b2Manifold* manifold,\r
+ const b2EdgeShape* edgeA, const b2Transform& xfA,\r
+ const b2PolygonShape* circleB, const b2Transform& xfB);\r
\r
/// Clipping for contact manifolds.\r
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],\r
- const b2Vec2& normal, float32 offset);\r
+ const b2Vec2& normal, float32 offset, int32 vertexIndexA);\r
\r
/// Determine if two generic shapes overlap.\r
-bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,\r
- const b2Transform& xfA, const b2Transform& xfB);\r
+bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,\r
+ const b2Shape* shapeB, int32 indexB,\r
+ const b2Transform& xfA, const b2Transform& xfB);\r
\r
// ---------------- Inline Functions ------------------------------------------\r
\r
/*\r
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
#include <Box2D/Collision/b2Distance.h>\r
#include <Box2D/Collision/Shapes/b2CircleShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+#include <Box2D/Collision/Shapes/b2LoopShape.h>\r
#include <Box2D/Collision/Shapes/b2PolygonShape.h>\r
\r
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.\r
int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;\r
\r
-void b2DistanceProxy::Set(const b2Shape* shape)\r
+void b2DistanceProxy::Set(const b2Shape* shape, int32 index)\r
{\r
switch (shape->GetType())\r
{\r
}\r
break;\r
\r
+ case b2Shape::e_loop:\r
+ {\r
+ const b2LoopShape* loop = (b2LoopShape*)shape;\r
+ b2Assert(0 <= index && index < loop->GetCount());\r
+\r
+ m_buffer[0] = loop->GetVertex(index);\r
+ if (index + 1 < loop->GetCount())\r
+ {\r
+ m_buffer[1] = loop->GetVertex(index + 1);\r
+ }\r
+ else\r
+ {\r
+ m_buffer[1] = loop->GetVertex(0);\r
+ }\r
+\r
+ m_vertices = m_buffer;\r
+ m_count = 2;\r
+ m_radius = loop->m_radius;\r
+ }\r
+ break;\r
+\r
+ case b2Shape::e_edge:\r
+ {\r
+ const b2EdgeShape* edge = (b2EdgeShape*)shape;\r
+ m_vertices = &edge->m_vertex1;\r
+ m_count = 2;\r
+ m_radius = edge->m_radius;\r
+ }\r
+ break;\r
+\r
default:\r
b2Assert(false);\r
}\r
\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
#define B2_DISTANCE_H\r
\r
#include <Box2D/Common/b2Math.h>\r
-#include <climits>\r
\r
class b2Shape;\r
\r
\r
/// Initialize the proxy using the given shape. The shape\r
/// must remain in scope while the proxy is in use.\r
- void Set(const b2Shape* shape);\r
+ void Set(const b2Shape* shape, int32 index);\r
\r
/// Get the supporting vertex index in the given direction.\r
int32 GetSupport(const b2Vec2& d) const;\r
/// Get a vertex by index. Used by b2Distance.\r
const b2Vec2& GetVertex(int32 index) const;\r
\r
+ b2Vec2 m_buffer[2];\r
const b2Vec2* m_vertices;\r
int32 m_count;\r
float32 m_radius;\r
/*\r
-* Copyright (c) 2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
#include <Box2D/Collision/b2DynamicTree.h>\r
#include <cstring>\r
#include <cfloat>\r
+using namespace std;\r
+\r
+\r
+#if B2_USE_DYNAMIC_TREE\r
\r
b2DynamicTree::b2DynamicTree()\r
{\r
\r
m_nodeCapacity = 16;\r
m_nodeCount = 0;\r
- m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));\r
- memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));\r
+ m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));\r
+ memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));\r
\r
// Build a linked list for the free list.\r
for (int32 i = 0; i < m_nodeCapacity - 1; ++i)\r
{\r
m_nodes[i].next = i + 1;\r
+ m_nodes[i].height = -1;\r
}\r
m_nodes[m_nodeCapacity-1].next = b2_nullNode;\r
+ m_nodes[m_nodeCapacity-1].height = -1;\r
m_freeList = 0;\r
\r
m_path = 0;\r
b2Assert(m_nodeCount == m_nodeCapacity);\r
\r
// The free list is empty. Rebuild a bigger pool.\r
- b2DynamicTreeNode* oldNodes = m_nodes;\r
+ b2TreeNode* oldNodes = m_nodes;\r
m_nodeCapacity *= 2;\r
- m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));\r
- memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));\r
+ m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));\r
+ memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));\r
b2Free(oldNodes);\r
\r
// Build a linked list for the free list. The parent\r
for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)\r
{\r
m_nodes[i].next = i + 1;\r
+ m_nodes[i].height = -1;\r
}\r
m_nodes[m_nodeCapacity-1].next = b2_nullNode;\r
+ m_nodes[m_nodeCapacity-1].height = -1;\r
m_freeList = m_nodeCount;\r
}\r
\r
m_nodes[nodeId].parent = b2_nullNode;\r
m_nodes[nodeId].child1 = b2_nullNode;\r
m_nodes[nodeId].child2 = b2_nullNode;\r
+ m_nodes[nodeId].height = 0;\r
+ m_nodes[nodeId].userData = NULL;\r
++m_nodeCount;\r
return nodeId;\r
}\r
b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);\r
b2Assert(0 < m_nodeCount);\r
m_nodes[nodeId].next = m_freeList;\r
+ m_nodes[nodeId].height = -1;\r
m_freeList = nodeId;\r
--m_nodeCount;\r
}\r
m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;\r
m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;\r
m_nodes[proxyId].userData = userData;\r
+ m_nodes[proxyId].height = 0;\r
\r
InsertLeaf(proxyId);\r
\r
- // Rebalance if necessary.\r
- int32 iterationCount = m_nodeCount >> 4;\r
- int32 tryCount = 0;\r
- int32 height = ComputeHeight();\r
- while (height > 64 && tryCount < 10)\r
- {\r
- Rebalance(iterationCount);\r
- height = ComputeHeight();\r
- ++tryCount;\r
- }\r
-\r
return proxyId;\r
}\r
\r
return;\r
}\r
\r
- // Find the best sibling for this node.\r
- b2Vec2 center = m_nodes[leaf].aabb.GetCenter();\r
- int32 sibling = m_root;\r
- if (m_nodes[sibling].IsLeaf() == false)\r
+ // Find the best sibling for this node\r
+ b2AABB leafAABB = m_nodes[leaf].aabb;\r
+ int32 index = m_root;\r
+ while (m_nodes[index].IsLeaf() == false)\r
{\r
- do \r
- {\r
- int32 child1 = m_nodes[sibling].child1;\r
- int32 child2 = m_nodes[sibling].child2;\r
+ int32 child1 = m_nodes[index].child1;\r
+ int32 child2 = m_nodes[index].child2;\r
\r
- b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);\r
- b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);\r
+ float32 area = m_nodes[index].aabb.GetPerimeter();\r
\r
- float32 norm1 = delta1.x + delta1.y;\r
- float32 norm2 = delta2.x + delta2.y;\r
+ b2AABB combinedAABB;\r
+ combinedAABB.Combine(m_nodes[index].aabb, leafAABB);\r
+ float32 combinedArea = combinedAABB.GetPerimeter();\r
\r
- if (norm1 < norm2)\r
- {\r
- sibling = child1;\r
- }\r
- else\r
- {\r
- sibling = child2;\r
- }\r
+ // Cost of creating a new parent for this node and the new leaf\r
+ float32 cost = 2.0f * combinedArea;\r
\r
- }\r
- while(m_nodes[sibling].IsLeaf() == false);\r
- }\r
+ // Minimum cost of pushing the leaf further down the tree\r
+ float32 inheritanceCost = 2.0f * (combinedArea - area);\r
\r
- // Create a parent for the siblings.\r
- int32 node1 = m_nodes[sibling].parent;\r
- int32 node2 = AllocateNode();\r
- m_nodes[node2].parent = node1;\r
- m_nodes[node2].userData = NULL;\r
- m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb);\r
+ // Cost of descending into child1\r
+ float32 cost1;\r
+ if (m_nodes[child1].IsLeaf())\r
+ {\r
+ b2AABB aabb;\r
+ aabb.Combine(leafAABB, m_nodes[child1].aabb);\r
+ cost1 = aabb.GetPerimeter() + inheritanceCost;\r
+ }\r
+ else\r
+ {\r
+ b2AABB aabb;\r
+ aabb.Combine(leafAABB, m_nodes[child1].aabb);\r
+ float32 oldArea = m_nodes[child1].aabb.GetPerimeter();\r
+ float32 newArea = aabb.GetPerimeter();\r
+ cost1 = (newArea - oldArea) + inheritanceCost;\r
+ }\r
\r
- if (node1 != b2_nullNode)\r
- {\r
- if (m_nodes[m_nodes[sibling].parent].child1 == sibling)\r
+ // Cost of descending into child2\r
+ float32 cost2;\r
+ if (m_nodes[child2].IsLeaf())\r
{\r
- m_nodes[node1].child1 = node2;\r
+ b2AABB aabb;\r
+ aabb.Combine(leafAABB, m_nodes[child2].aabb);\r
+ cost2 = aabb.GetPerimeter() + inheritanceCost;\r
}\r
else\r
{\r
- m_nodes[node1].child2 = node2;\r
+ b2AABB aabb;\r
+ aabb.Combine(leafAABB, m_nodes[child2].aabb);\r
+ float32 oldArea = m_nodes[child2].aabb.GetPerimeter();\r
+ float32 newArea = aabb.GetPerimeter();\r
+ cost2 = newArea - oldArea + inheritanceCost;\r
}\r
\r
- m_nodes[node2].child1 = sibling;\r
- m_nodes[node2].child2 = leaf;\r
- m_nodes[sibling].parent = node2;\r
- m_nodes[leaf].parent = node2;\r
+ // Descend according to the minimum cost.\r
+ if (cost < cost1 && cost < cost2)\r
+ {\r
+ break;\r
+ }\r
\r
- do \r
+ // Descend\r
+ if (cost1 < cost2)\r
{\r
- if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb))\r
- {\r
- break;\r
- }\r
+ index = child1;\r
+ }\r
+ else\r
+ {\r
+ index = child2;\r
+ }\r
+ }\r
+\r
+ int32 sibling = index;\r
\r
- m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);\r
- node2 = node1;\r
- node1 = m_nodes[node1].parent;\r
+ // Create a new parent.\r
+ int32 oldParent = m_nodes[sibling].parent;\r
+ int32 newParent = AllocateNode();\r
+ m_nodes[newParent].parent = oldParent;\r
+ m_nodes[newParent].userData = NULL;\r
+ m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);\r
+ m_nodes[newParent].height = m_nodes[sibling].height + 1;\r
+\r
+ if (oldParent != b2_nullNode)\r
+ {\r
+ // The sibling was not the root.\r
+ if (m_nodes[oldParent].child1 == sibling)\r
+ {\r
+ m_nodes[oldParent].child1 = newParent;\r
+ }\r
+ else\r
+ {\r
+ m_nodes[oldParent].child2 = newParent;\r
}\r
- while(node1 != b2_nullNode);\r
+\r
+ m_nodes[newParent].child1 = sibling;\r
+ m_nodes[newParent].child2 = leaf;\r
+ m_nodes[sibling].parent = newParent;\r
+ m_nodes[leaf].parent = newParent;\r
}\r
else\r
{\r
- m_nodes[node2].child1 = sibling;\r
- m_nodes[node2].child2 = leaf;\r
- m_nodes[sibling].parent = node2;\r
- m_nodes[leaf].parent = node2;\r
- m_root = node2;\r
+ // The sibling was the root.\r
+ m_nodes[newParent].child1 = sibling;\r
+ m_nodes[newParent].child2 = leaf;\r
+ m_nodes[sibling].parent = newParent;\r
+ m_nodes[leaf].parent = newParent;\r
+ m_root = newParent;\r
}\r
+\r
+ // Walk back up the tree fixing heights and AABBs\r
+ index = m_nodes[leaf].parent;\r
+ while (index != b2_nullNode)\r
+ {\r
+ index = Balance(index);\r
+\r
+ int32 child1 = m_nodes[index].child1;\r
+ int32 child2 = m_nodes[index].child2;\r
+\r
+ b2Assert(child1 != b2_nullNode);\r
+ b2Assert(child2 != b2_nullNode);\r
+\r
+ m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);\r
+ m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);\r
+\r
+ index = m_nodes[index].parent;\r
+ }\r
+\r
+ //Validate();\r
}\r
\r
void b2DynamicTree::RemoveLeaf(int32 leaf)\r
return;\r
}\r
\r
- int32 node2 = m_nodes[leaf].parent;\r
- int32 node1 = m_nodes[node2].parent;\r
+ int32 parent = m_nodes[leaf].parent;\r
+ int32 grandParent = m_nodes[parent].parent;\r
int32 sibling;\r
- if (m_nodes[node2].child1 == leaf)\r
+ if (m_nodes[parent].child1 == leaf)\r
{\r
- sibling = m_nodes[node2].child2;\r
+ sibling = m_nodes[parent].child2;\r
}\r
else\r
{\r
- sibling = m_nodes[node2].child1;\r
+ sibling = m_nodes[parent].child1;\r
}\r
\r
- if (node1 != b2_nullNode)\r
+ if (grandParent != b2_nullNode)\r
{\r
- // Destroy node2 and connect node1 to sibling.\r
- if (m_nodes[node1].child1 == node2)\r
+ // Destroy parent and connect sibling to grandParent.\r
+ if (m_nodes[grandParent].child1 == parent)\r
{\r
- m_nodes[node1].child1 = sibling;\r
+ m_nodes[grandParent].child1 = sibling;\r
}\r
else\r
{\r
- m_nodes[node1].child2 = sibling;\r
+ m_nodes[grandParent].child2 = sibling;\r
}\r
- m_nodes[sibling].parent = node1;\r
- FreeNode(node2);\r
+ m_nodes[sibling].parent = grandParent;\r
+ FreeNode(parent);\r
\r
// Adjust ancestor bounds.\r
- while (node1 != b2_nullNode)\r
+ int32 index = grandParent;\r
+ while (index != b2_nullNode)\r
{\r
- b2AABB oldAABB = m_nodes[node1].aabb;\r
- m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);\r
+ index = Balance(index);\r
\r
- if (oldAABB.Contains(m_nodes[node1].aabb))\r
- {\r
- break;\r
- }\r
+ int32 child1 = m_nodes[index].child1;\r
+ int32 child2 = m_nodes[index].child2;\r
\r
- node1 = m_nodes[node1].parent;\r
+ m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);\r
+ m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);\r
+\r
+ index = m_nodes[index].parent;\r
}\r
}\r
else\r
{\r
m_root = sibling;\r
m_nodes[sibling].parent = b2_nullNode;\r
- FreeNode(node2);\r
+ FreeNode(parent);\r
+ }\r
+\r
+ //Validate();\r
+}\r
+\r
+// Perform a left or right rotation if node A is imbalanced.\r
+// Returns the new root index.\r
+int32 b2DynamicTree::Balance(int32 iA)\r
+{\r
+ b2Assert(iA != b2_nullNode);\r
+\r
+ b2TreeNode* A = m_nodes + iA;\r
+ if (A->IsLeaf() || A->height < 2)\r
+ {\r
+ return iA;\r
+ }\r
+\r
+ int32 iB = A->child1;\r
+ int32 iC = A->child2;\r
+ b2Assert(0 <= iB && iB < m_nodeCapacity);\r
+ b2Assert(0 <= iC && iC < m_nodeCapacity);\r
+\r
+ b2TreeNode* B = m_nodes + iB;\r
+ b2TreeNode* C = m_nodes + iC;\r
+\r
+ int32 balance = C->height - B->height;\r
+\r
+ // Rotate C up\r
+ if (balance > 1)\r
+ {\r
+ int32 iF = C->child1;\r
+ int32 iG = C->child2;\r
+ b2TreeNode* F = m_nodes + iF;\r
+ b2TreeNode* G = m_nodes + iG;\r
+ b2Assert(0 <= iF && iF < m_nodeCapacity);\r
+ b2Assert(0 <= iG && iG < m_nodeCapacity);\r
+\r
+ // Swap A and C\r
+ C->child1 = iA;\r
+ C->parent = A->parent;\r
+ A->parent = iC;\r
+\r
+ // A's old parent should point to C\r
+ if (C->parent != b2_nullNode)\r
+ {\r
+ if (m_nodes[C->parent].child1 == iA)\r
+ {\r
+ m_nodes[C->parent].child1 = iC;\r
+ }\r
+ else\r
+ {\r
+ b2Assert(m_nodes[C->parent].child2 == iA);\r
+ m_nodes[C->parent].child2 = iC;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ m_root = iC;\r
+ }\r
+\r
+ // Rotate\r
+ if (F->height > G->height)\r
+ {\r
+ C->child2 = iF;\r
+ A->child2 = iG;\r
+ G->parent = iA;\r
+ A->aabb.Combine(B->aabb, G->aabb);\r
+ C->aabb.Combine(A->aabb, F->aabb);\r
+\r
+ A->height = 1 + b2Max(B->height, G->height);\r
+ C->height = 1 + b2Max(A->height, F->height);\r
+ }\r
+ else\r
+ {\r
+ C->child2 = iG;\r
+ A->child2 = iF;\r
+ F->parent = iA;\r
+ A->aabb.Combine(B->aabb, F->aabb);\r
+ C->aabb.Combine(A->aabb, G->aabb);\r
+\r
+ A->height = 1 + b2Max(B->height, F->height);\r
+ C->height = 1 + b2Max(A->height, G->height);\r
+ }\r
+\r
+ return iC;\r
}\r
+ \r
+ // Rotate B up\r
+ if (balance < -1)\r
+ {\r
+ int32 iD = B->child1;\r
+ int32 iE = B->child2;\r
+ b2TreeNode* D = m_nodes + iD;\r
+ b2TreeNode* E = m_nodes + iE;\r
+ b2Assert(0 <= iD && iD < m_nodeCapacity);\r
+ b2Assert(0 <= iE && iE < m_nodeCapacity);\r
+\r
+ // Swap A and B\r
+ B->child1 = iA;\r
+ B->parent = A->parent;\r
+ A->parent = iB;\r
+\r
+ // A's old parent should point to B\r
+ if (B->parent != b2_nullNode)\r
+ {\r
+ if (m_nodes[B->parent].child1 == iA)\r
+ {\r
+ m_nodes[B->parent].child1 = iB;\r
+ }\r
+ else\r
+ {\r
+ b2Assert(m_nodes[B->parent].child2 == iA);\r
+ m_nodes[B->parent].child2 = iB;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ m_root = iB;\r
+ }\r
+\r
+ // Rotate\r
+ if (D->height > E->height)\r
+ {\r
+ B->child2 = iD;\r
+ A->child1 = iE;\r
+ E->parent = iA;\r
+ A->aabb.Combine(C->aabb, E->aabb);\r
+ B->aabb.Combine(A->aabb, D->aabb);\r
+\r
+ A->height = 1 + b2Max(C->height, E->height);\r
+ B->height = 1 + b2Max(A->height, D->height);\r
+ }\r
+ else\r
+ {\r
+ B->child2 = iE;\r
+ A->child1 = iD;\r
+ D->parent = iA;\r
+ A->aabb.Combine(C->aabb, D->aabb);\r
+ B->aabb.Combine(A->aabb, E->aabb);\r
+\r
+ A->height = 1 + b2Max(C->height, D->height);\r
+ B->height = 1 + b2Max(A->height, E->height);\r
+ }\r
+\r
+ return iB;\r
+ }\r
+\r
+ return iA;\r
}\r
\r
-void b2DynamicTree::Rebalance(int32 iterations)\r
+int32 b2DynamicTree::GetHeight() const\r
{\r
if (m_root == b2_nullNode)\r
{\r
- return;\r
+ return 0;\r
}\r
\r
- for (int32 i = 0; i < iterations; ++i)\r
+ return m_nodes[m_root].height;\r
+}\r
+\r
+//\r
+float32 b2DynamicTree::GetAreaRatio() const\r
+{\r
+ if (m_root == b2_nullNode)\r
{\r
- int32 node = m_root;\r
+ return 0.0f;\r
+ }\r
\r
- uint32 bit = 0;\r
- while (m_nodes[node].IsLeaf() == false)\r
+ const b2TreeNode* root = m_nodes + m_root;\r
+ float32 rootArea = root->aabb.GetPerimeter();\r
+\r
+ float32 totalArea = 0.0f;\r
+ for (int32 i = 0; i < m_nodeCapacity; ++i)\r
+ {\r
+ const b2TreeNode* node = m_nodes + i;\r
+ if (node->height < 0)\r
{\r
- int32* children = &m_nodes[node].child1;\r
- node = children[(m_path >> bit) & 1];\r
- bit = (bit + 1) & (8* sizeof(uint32) - 1);\r
+ // Free node in pool\r
+ continue;\r
}\r
- ++m_path;\r
\r
- RemoveLeaf(node);\r
- InsertLeaf(node);\r
+ totalArea += node->aabb.GetPerimeter();\r
}\r
+\r
+ return totalArea / rootArea;\r
}\r
\r
// Compute the height of a sub-tree.\r
int32 b2DynamicTree::ComputeHeight(int32 nodeId) const\r
{\r
- if (nodeId == b2_nullNode)\r
+ b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);\r
+ b2TreeNode* node = m_nodes + nodeId;\r
+\r
+ if (node->IsLeaf())\r
{\r
return 0;\r
}\r
\r
- b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);\r
- b2DynamicTreeNode* node = m_nodes + nodeId;\r
int32 height1 = ComputeHeight(node->child1);\r
int32 height2 = ComputeHeight(node->child2);\r
return 1 + b2Max(height1, height2);\r
\r
int32 b2DynamicTree::ComputeHeight() const\r
{\r
- return ComputeHeight(m_root);\r
+ int32 height = ComputeHeight(m_root);\r
+ return height;\r
+}\r
+\r
+void b2DynamicTree::ValidateStructure(int32 index) const\r
+{\r
+ if (index == b2_nullNode)\r
+ {\r
+ return;\r
+ }\r
+\r
+ if (index == m_root)\r
+ {\r
+ b2Assert(m_nodes[index].parent == b2_nullNode);\r
+ }\r
+\r
+ const b2TreeNode* node = m_nodes + index;\r
+\r
+ int32 child1 = node->child1;\r
+ int32 child2 = node->child2;\r
+\r
+ if (node->IsLeaf())\r
+ {\r
+ b2Assert(child1 == b2_nullNode);\r
+ b2Assert(child2 == b2_nullNode);\r
+ b2Assert(node->height == 0);\r
+ return;\r
+ }\r
+\r
+ b2Assert(0 <= child1 && child1 < m_nodeCapacity);\r
+ b2Assert(0 <= child2 && child2 < m_nodeCapacity);\r
+\r
+ b2Assert(m_nodes[child1].parent == index);\r
+ b2Assert(m_nodes[child2].parent == index);\r
+\r
+ ValidateStructure(child1);\r
+ ValidateStructure(child2);\r
+}\r
+\r
+void b2DynamicTree::ValidateMetrics(int32 index) const\r
+{\r
+ if (index == b2_nullNode)\r
+ {\r
+ return;\r
+ }\r
+\r
+ const b2TreeNode* node = m_nodes + index;\r
+\r
+ int32 child1 = node->child1;\r
+ int32 child2 = node->child2;\r
+\r
+ if (node->IsLeaf())\r
+ {\r
+ b2Assert(child1 == b2_nullNode);\r
+ b2Assert(child2 == b2_nullNode);\r
+ b2Assert(node->height == 0);\r
+ return;\r
+ }\r
+\r
+ b2Assert(0 <= child1 && child1 < m_nodeCapacity);\r
+ b2Assert(0 <= child2 && child2 < m_nodeCapacity);\r
+\r
+ int32 height1 = m_nodes[child1].height;\r
+ int32 height2 = m_nodes[child2].height;\r
+ int32 height = 1 + b2Max(height1, height2);\r
+ b2Assert(node->height == height);\r
+\r
+ b2AABB aabb;\r
+ aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);\r
+\r
+ b2Assert(aabb.lowerBound == node->aabb.lowerBound);\r
+ b2Assert(aabb.upperBound == node->aabb.upperBound);\r
+\r
+ ValidateMetrics(child1);\r
+ ValidateMetrics(child2);\r
+}\r
+\r
+void b2DynamicTree::Validate() const\r
+{\r
+ ValidateStructure(m_root);\r
+ ValidateMetrics(m_root);\r
+\r
+ int32 freeCount = 0;\r
+ int32 freeIndex = m_freeList;\r
+ while (freeIndex != b2_nullNode)\r
+ {\r
+ b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);\r
+ freeIndex = m_nodes[freeIndex].next;\r
+ ++freeCount;\r
+ }\r
+\r
+ b2Assert(GetHeight() == ComputeHeight());\r
+\r
+ b2Assert(m_nodeCount + freeCount == m_nodeCapacity);\r
+}\r
+\r
+int32 b2DynamicTree::GetMaxBalance() const\r
+{\r
+ int32 maxBalance = 0;\r
+ for (int32 i = 0; i < m_nodeCapacity; ++i)\r
+ {\r
+ const b2TreeNode* node = m_nodes + i;\r
+ if (node->height <= 1)\r
+ {\r
+ continue;\r
+ }\r
+\r
+ b2Assert(node->IsLeaf() == false);\r
+\r
+ int32 child1 = node->child1;\r
+ int32 child2 = node->child2;\r
+ int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);\r
+ maxBalance = b2Max(maxBalance, balance);\r
+ }\r
+\r
+ return maxBalance;\r
+}\r
+\r
+void b2DynamicTree::RebuildBottomUp()\r
+{\r
+ int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));\r
+ int32 count = 0;\r
+\r
+ // Build array of leaves. Free the rest.\r
+ for (int32 i = 0; i < m_nodeCapacity; ++i)\r
+ {\r
+ if (m_nodes[i].height < 0)\r
+ {\r
+ // free node in pool\r
+ continue;\r
+ }\r
+\r
+ if (m_nodes[i].IsLeaf())\r
+ {\r
+ m_nodes[i].parent = b2_nullNode;\r
+ nodes[count] = i;\r
+ ++count;\r
+ }\r
+ else\r
+ {\r
+ FreeNode(i);\r
+ }\r
+ }\r
+\r
+ while (count > 1)\r
+ {\r
+ float32 minCost = b2_maxFloat;\r
+ int32 iMin = -1, jMin = -1;\r
+ for (int32 i = 0; i < count; ++i)\r
+ {\r
+ b2AABB aabbi = m_nodes[nodes[i]].aabb;\r
+\r
+ for (int32 j = i + 1; j < count; ++j)\r
+ {\r
+ b2AABB aabbj = m_nodes[nodes[j]].aabb;\r
+ b2AABB b;\r
+ b.Combine(aabbi, aabbj);\r
+ float32 cost = b.GetPerimeter();\r
+ if (cost < minCost)\r
+ {\r
+ iMin = i;\r
+ jMin = j;\r
+ minCost = cost;\r
+ }\r
+ }\r
+ }\r
+\r
+ int32 index1 = nodes[iMin];\r
+ int32 index2 = nodes[jMin];\r
+ b2TreeNode* child1 = m_nodes + index1;\r
+ b2TreeNode* child2 = m_nodes + index2;\r
+\r
+ int32 parentIndex = AllocateNode();\r
+ b2TreeNode* parent = m_nodes + parentIndex;\r
+ parent->child1 = index1;\r
+ parent->child2 = index2;\r
+ parent->height = 1 + b2Max(child1->height, child2->height);\r
+ parent->aabb.Combine(child1->aabb, child2->aabb);\r
+ parent->parent = b2_nullNode;\r
+\r
+ child1->parent = parentIndex;\r
+ child2->parent = parentIndex;\r
+\r
+ nodes[jMin] = nodes[count-1];\r
+ nodes[iMin] = parentIndex;\r
+ --count;\r
+ }\r
+\r
+ m_root = nodes[0];\r
+ b2Free(nodes);\r
+\r
+ Validate();\r
+}\r
+\r
+#elif B2_USE_BRUTE_FORCE\r
+\r
+b2DynamicTree::b2DynamicTree()\r
+{\r
+ m_proxyCapacity = 128;\r
+ m_proxyCount = 0;\r
+\r
+ m_proxyMap = (int32*)b2Alloc(m_proxyCapacity * sizeof(int32));\r
+ m_proxies = (b2Proxy*)b2Alloc(m_proxyCapacity * sizeof(b2Proxy));\r
+\r
+ // Build the free list\r
+ m_freeId = 0;\r
+ int32 last = m_proxyCapacity - 1;\r
+ for (int32 i = m_freeId; i < last; ++i)\r
+ {\r
+ m_proxyMap[i] = i + 1;\r
+ }\r
+\r
+ m_proxyMap[last] = b2_nullNode;\r
+}\r
+\r
+b2DynamicTree::~b2DynamicTree()\r
+{\r
+ b2Free(m_proxyMap);\r
+ b2Free(m_proxies);\r
+}\r
+\r
+int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)\r
+{\r
+ if (m_proxyCount == m_proxyCapacity)\r
+ {\r
+ m_proxyCapacity *= 2;\r
+ int32* proxyMap = (int32*)b2Alloc(m_proxyCapacity * sizeof(int32));\r
+ b2Proxy* proxies = (b2Proxy*)b2Alloc(m_proxyCapacity * sizeof(b2Proxy));\r
+\r
+ memcpy(proxyMap, m_proxyMap, m_proxyCount * sizeof(int32));\r
+ memcpy(proxies, m_proxies, m_proxyCount * sizeof(b2Proxy));\r
+\r
+ b2Free(m_proxyMap);\r
+ b2Free(m_proxies);\r
+ m_proxyMap = proxyMap;\r
+ m_proxies = proxies;\r
+ proxyMap = NULL;\r
+ proxies = NULL;\r
+\r
+ m_freeId = m_proxyCount;\r
+ int32 last = m_proxyCapacity - 1;\r
+ for (int32 i = m_freeId; i < last; ++i)\r
+ {\r
+ m_proxyMap[i] = i + 1;\r
+ }\r
+\r
+ m_proxyMap[last] = b2_nullNode;\r
+ }\r
+\r
+ b2Assert(0 <= m_freeId && m_freeId < m_proxyCapacity);\r
+ int32 id = m_freeId;\r
+ m_freeId = m_proxyMap[id];\r
+ int32 index = m_proxyCount;\r
+\r
+ m_proxies[index].aabb = aabb;\r
+ m_proxies[index].userData = userData;\r
+ m_proxies[index].id = id;\r
+ m_proxyMap[id] = index;\r
+ ++m_proxyCount;\r
+\r
+ return id;\r
+}\r
+\r
+void b2DynamicTree::DestroyProxy(int32 proxyId)\r
+{\r
+ b2Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity);\r
+ int32 index = m_proxyMap[proxyId];\r
+\r
+ // Add to free list\r
+ m_proxyMap[proxyId] = m_freeId;\r
+ m_freeId = proxyId;\r
+\r
+ // Keep proxy array contiguous\r
+ if (index < m_proxyCount - 1)\r
+ {\r
+ m_proxies[index] = m_proxies[m_proxyCount-1];\r
+ int32 id = m_proxies[index].id;\r
+ m_proxyMap[id] = index;\r
+ }\r
+\r
+ --m_proxyCount;\r
+\r
+ Validate();\r
}\r
+\r
+bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)\r
+{\r
+ b2Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity);\r
+ B2_NOT_USED(displacement);\r
+\r
+ int32 index = m_proxyMap[proxyId];\r
+\r
+ if (m_proxies[index].aabb.Contains(aabb))\r
+ {\r
+ return false;\r
+ }\r
+\r
+ // Extend AABB.\r
+ b2AABB b = aabb;\r
+ b2Vec2 r(b2_aabbExtension, b2_aabbExtension);\r
+ b.lowerBound = b.lowerBound - r;\r
+ b.upperBound = b.upperBound + r;\r
+\r
+ // Predict AABB displacement.\r
+ b2Vec2 d = b2_aabbMultiplier * displacement;\r
+\r
+ if (d.x < 0.0f)\r
+ {\r
+ b.lowerBound.x += d.x;\r
+ }\r
+ else\r
+ {\r
+ b.upperBound.x += d.x;\r
+ }\r
+\r
+ if (d.y < 0.0f)\r
+ {\r
+ b.lowerBound.y += d.y;\r
+ }\r
+ else\r
+ {\r
+ b.upperBound.y += d.y;\r
+ }\r
+\r
+ m_proxies[index].aabb = b;\r
+\r
+ return true;\r
+}\r
+\r
+void b2DynamicTree::Validate() const\r
+{\r
+ b2Assert(m_proxyCount > 0 || m_freeId == b2_nullNode);\r
+ b2Assert(m_freeId == b2_nullNode || m_freeId < m_proxyCapacity);\r
+\r
+ int32 id = m_freeId;\r
+ int32 freeCount = 0;\r
+ while (id != b2_nullNode)\r
+ {\r
+ ++freeCount;\r
+ b2Assert(freeCount <= m_proxyCapacity);\r
+ id = m_proxyMap[id];\r
+ }\r
+\r
+ b2Assert(freeCount + m_proxyCount == m_proxyCapacity);\r
+\r
+ b2Assert(m_proxyCount <= m_proxyCapacity);\r
+\r
+ for (int32 i = 0; i < m_proxyCount; ++i)\r
+ {\r
+ int32 id = m_proxies[i].id;\r
+\r
+ b2Assert(m_proxyMap[id] == i);\r
+ }\r
+}\r
+\r
+#endif\r
/*\r
-* Copyright (c) 2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
#define B2_DYNAMIC_TREE_H\r
\r
#include <Box2D/Collision/b2Collision.h>\r
-\r
-/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.\r
+#include <Box2D/Common/b2GrowableStack.h>\r
\r
#define b2_nullNode (-1)\r
\r
+#define B2_USE_DYNAMIC_TREE 1\r
+#define B2_USE_BRUTE_FORCE 0\r
+\r
+#if B2_USE_DYNAMIC_TREE\r
+\r
/// A node in the dynamic tree. The client does not interact with this directly.\r
-struct b2DynamicTreeNode\r
+struct b2TreeNode\r
{\r
bool IsLeaf() const\r
{\r
return child1 == b2_nullNode;\r
}\r
\r
- /// This is the fattened AABB.\r
+ /// Enlarged AABB\r
b2AABB aabb;\r
\r
- //int32 userData;\r
void* userData;\r
\r
union\r
\r
int32 child1;\r
int32 child2;\r
+\r
+ // leaf = 0, free node = -1\r
+ int32 height;\r
};\r
\r
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.\r
/// A dynamic tree arranges data in a binary tree to accelerate\r
/// queries such as volume queries and ray casts. Leafs are proxies\r
/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor\r
class b2DynamicTree\r
{\r
public:\r
-\r
/// Constructing the tree initializes the node pool.\r
b2DynamicTree();\r
\r
/// @return true if the proxy was re-inserted.\r
bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);\r
\r
- /// Perform some iterations to re-balance the tree.\r
- void Rebalance(int32 iterations);\r
-\r
/// Get proxy user data.\r
/// @return the proxy user data or 0 if the id is invalid.\r
void* GetUserData(int32 proxyId) const;\r
/// Get the fat AABB for a proxy.\r
const b2AABB& GetFatAABB(int32 proxyId) const;\r
\r
- /// Compute the height of the tree.\r
- int32 ComputeHeight() const;\r
-\r
/// Query an AABB for overlapping proxies. The callback class\r
/// is called for each proxy that overlaps the supplied AABB.\r
template <typename T>\r
template <typename T>\r
void RayCast(T* callback, const b2RayCastInput& input) const;\r
\r
+ /// Validate this tree. For testing.\r
+ void Validate() const;\r
+\r
+ /// Compute the height of the binary tree in O(N) time. Should not be\r
+ /// called often.\r
+ int32 GetHeight() const;\r
+\r
+ /// Get the maximum balance of an node in the tree. The balance is the difference\r
+ /// in height of the two children of a node.\r
+ int32 GetMaxBalance() const;\r
+\r
+ /// Get the ratio of the sum of the node areas to the root area.\r
+ float32 GetAreaRatio() const;\r
+\r
+ /// Build an optimal tree. Very expensive. For testing.\r
+ void RebuildBottomUp();\r
+\r
private:\r
\r
int32 AllocateNode();\r
void InsertLeaf(int32 node);\r
void RemoveLeaf(int32 node);\r
\r
+ int32 Balance(int32 index);\r
+\r
+ int32 ComputeHeight() const;\r
int32 ComputeHeight(int32 nodeId) const;\r
\r
+ void ValidateStructure(int32 index) const;\r
+ void ValidateMetrics(int32 index) const;\r
+\r
int32 m_root;\r
\r
- b2DynamicTreeNode* m_nodes;\r
+ b2TreeNode* m_nodes;\r
int32 m_nodeCount;\r
int32 m_nodeCapacity;\r
\r
int32 m_freeList;\r
\r
- /// This is used incrementally traverse the tree for re-balancing.\r
+ /// This is used to incrementally traverse the tree for re-balancing.\r
uint32 m_path;\r
\r
int32 m_insertionCount;\r
template <typename T>\r
inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const\r
{\r
- const int32 k_stackSize = 128;\r
- int32 stack[k_stackSize];\r
-\r
- int32 count = 0;\r
- stack[count++] = m_root;\r
+ b2GrowableStack<int32, 256> stack;\r
+ stack.Push(m_root);\r
\r
- while (count > 0)\r
+ while (stack.GetCount() > 0)\r
{\r
- int32 nodeId = stack[--count];\r
+ int32 nodeId = stack.Pop();\r
if (nodeId == b2_nullNode)\r
{\r
continue;\r
}\r
\r
- const b2DynamicTreeNode* node = m_nodes + nodeId;\r
+ const b2TreeNode* node = m_nodes + nodeId;\r
\r
if (b2TestOverlap(node->aabb, aabb))\r
{\r
}\r
else\r
{\r
- if (count < k_stackSize)\r
- {\r
- stack[count++] = node->child1;\r
- }\r
-\r
- if (count < k_stackSize)\r
- {\r
- stack[count++] = node->child2;\r
- }\r
+ stack.Push(node->child1);\r
+ stack.Push(node->child2);\r
}\r
}\r
}\r
segmentAABB.upperBound = b2Max(p1, t);\r
}\r
\r
- const int32 k_stackSize = 128;\r
- int32 stack[k_stackSize];\r
+ b2GrowableStack<int32, 256> stack;\r
+ stack.Push(m_root);\r
\r
- int32 count = 0;\r
- stack[count++] = m_root;\r
-\r
- while (count > 0)\r
+ while (stack.GetCount() > 0)\r
{\r
- int32 nodeId = stack[--count];\r
+ int32 nodeId = stack.Pop();\r
if (nodeId == b2_nullNode)\r
{\r
continue;\r
}\r
\r
- const b2DynamicTreeNode* node = m_nodes + nodeId;\r
+ const b2TreeNode* node = m_nodes + nodeId;\r
\r
if (b2TestOverlap(node->aabb, segmentAABB) == false)\r
{\r
}\r
else\r
{\r
- if (count < k_stackSize)\r
- {\r
- stack[count++] = node->child1;\r
- }\r
+ stack.Push(node->child1);\r
+ stack.Push(node->child2);\r
+ }\r
+ }\r
+}\r
+\r
+#elif B2_USE_BRUTE_FORCE 0\r
+\r
+struct b2Proxy\r
+{\r
+ /// This is the fattened AABB.\r
+ b2AABB aabb;\r
+ void* userData;\r
+ int32 id;\r
+};\r
+\r
+/// This implementation is not a tree at all. It is just a cache friendly array of AABBs.\r
+class b2DynamicTree\r
+{\r
+public:\r
+\r
+ /// Constructing the tree initializes the node pool.\r
+ b2DynamicTree();\r
+\r
+ /// Destroy the tree, freeing the node pool.\r
+ ~b2DynamicTree();\r
+\r
+ /// Create a proxy. Provide a tight fitting AABB and a userData pointer.\r
+ int32 CreateProxy(const b2AABB& aabb, void* userData);\r
+\r
+ /// Destroy a proxy. This asserts if the id is invalid.\r
+ void DestroyProxy(int32 proxyId);\r
+\r
+ /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,\r
+ /// then the proxy is removed from the tree and re-inserted. Otherwise\r
+ /// the function returns immediately.\r
+ /// @return true if the proxy was re-inserted.\r
+ bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);\r
+\r
+ /// Perform some iterations to re-balance the tree.\r
+ void Rebalance(int32 iterations)\r
+ {\r
+ B2_NOT_USED(iterations);\r
+ }\r
+\r
+ /// Get proxy user data.\r
+ /// @return the proxy user data or 0 if the id is invalid.\r
+ void* GetUserData(int32 proxyId) const;\r
+\r
+ /// Get the fat AABB for a proxy.\r
+ const b2AABB& GetFatAABB(int32 proxyId) const;\r
+\r
+ /// Compute the height of the binary tree in O(N) time. Should not be\r
+ /// called often.\r
+ int32 ComputeHeight() const\r
+ {\r
+ return 0;\r
+ }\r
+\r
+ /// Query an AABB for overlapping proxies. The callback class\r
+ /// is called for each proxy that overlaps the supplied AABB.\r
+ template <typename T>\r
+ void Query(T* callback, const b2AABB& aabb) const;\r
+\r
+ /// Ray-cast against the proxies in the tree. This relies on the callback\r
+ /// to perform a exact ray-cast in the case were the proxy contains a shape.\r
+ /// The callback also performs the any collision filtering. This has performance\r
+ /// roughly equal to k * log(n), where k is the number of collisions and n is the\r
+ /// number of proxies in the tree.\r
+ /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).\r
+ /// @param callback a callback class that is called for each proxy that is hit by the ray.\r
+ template <typename T>\r
+ void RayCast(T* callback, const b2RayCastInput& input) const;\r
+\r
+ void Validate() const;\r
+\r
+private:\r
\r
- if (count < k_stackSize)\r
+ // Map of ids to proxies indices. This may have holes (which contain a free list).\r
+ int32* m_proxyMap;\r
+\r
+ // Contiguous array of proxies\r
+ b2Proxy* m_proxies;\r
+\r
+ int32 m_proxyCount;\r
+ int32 m_proxyCapacity;\r
+\r
+ int32 m_freeId;\r
+};\r
+\r
+inline void* b2DynamicTree::GetUserData(int32 proxyId) const\r
+{\r
+ b2Assert(0 <= proxyId && proxyId < m_proxyCapacity);\r
+ int32 index = m_proxyMap[proxyId];\r
+ return m_proxies[index].userData;\r
+}\r
+\r
+inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const\r
+{\r
+ b2Assert(0 <= proxyId && proxyId < m_proxyCapacity);\r
+ int32 index = m_proxyMap[proxyId];\r
+ return m_proxies[index].aabb;\r
+}\r
+\r
+template <typename T>\r
+inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const\r
+{\r
+ for (int32 i = 0; i < m_proxyCount; ++i)\r
+ {\r
+ if (b2TestOverlap(m_proxies[i].aabb, aabb))\r
+ {\r
+ bool proceed = callback->QueryCallback(m_proxies[i].id);\r
+ if (proceed == false)\r
{\r
- stack[count++] = node->child2;\r
+ return;\r
}\r
}\r
}\r
}\r
\r
+template <typename T>\r
+inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const\r
+{\r
+ b2Vec2 p1 = input.p1;\r
+ b2Vec2 p2 = input.p2;\r
+ b2Vec2 r = p2 - p1;\r
+ b2Assert(r.LengthSquared() > 0.0f);\r
+ r.Normalize();\r
+\r
+ // v is perpendicular to the segment.\r
+ b2Vec2 v = b2Cross(1.0f, r);\r
+ b2Vec2 abs_v = b2Abs(v);\r
+\r
+ // Separating axis for segment (Gino, p80).\r
+ // |dot(v, p1 - c)| > dot(|v|, h)\r
+\r
+ float32 maxFraction = input.maxFraction;\r
+\r
+ // Build a bounding box for the segment.\r
+ b2AABB segmentAABB;\r
+ {\r
+ b2Vec2 t = p1 + maxFraction * (p2 - p1);\r
+ segmentAABB.lowerBound = b2Min(p1, t);\r
+ segmentAABB.upperBound = b2Max(p1, t);\r
+ }\r
+\r
+ for (int32 i = 0; i < m_proxyCount; ++i)\r
+ {\r
+ const b2Proxy* proxy = m_proxies + i;\r
+ b2AABB proxyAABB = proxy->aabb;\r
+\r
+ if (b2TestOverlap(proxyAABB, segmentAABB) == false)\r
+ {\r
+ continue;\r
+ }\r
+\r
+ // Separating axis for segment (Gino, p80).\r
+ // |dot(v, p1 - c)| > dot(|v|, h)\r
+ b2Vec2 c = proxyAABB.GetCenter();\r
+ b2Vec2 h = proxyAABB.GetExtents();\r
+ float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);\r
+ if (separation > 0.0f)\r
+ {\r
+ continue;\r
+ }\r
+\r
+ b2RayCastInput subInput;\r
+ subInput.p1 = input.p1;\r
+ subInput.p2 = input.p2;\r
+ subInput.maxFraction = maxFraction;\r
+\r
+ float32 value = callback->RayCastCallback(subInput, proxy->id);\r
+\r
+ if (value == 0.0f)\r
+ {\r
+ // The client has terminated the ray cast.\r
+ return;\r
+ }\r
+\r
+ if (value > 0.0f)\r
+ {\r
+ // Update segment bounding box.\r
+ maxFraction = value;\r
+ b2Vec2 t = p1 + maxFraction * (p2 - p1);\r
+ segmentAABB.lowerBound = b2Min(p1, t);\r
+ segmentAABB.upperBound = b2Max(p1, t);\r
+ }\r
+ }\r
+}\r
+\r
+#endif\r
+\r
#endif\r
/*\r
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
#include <Box2D/Collision/Shapes/b2PolygonShape.h>\r
\r
#include <cstdio>\r
+using namespace std;\r
\r
int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;\r
int32 b2_toiRootIters, b2_toiMaxRootIters;\r
\r
-int32 b2_toiMaxOptIters;\r
-\r
struct b2SeparationFunction\r
{\r
enum Type\r
\r
float32 Initialize(const b2SimplexCache* cache,\r
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,\r
- const b2DistanceProxy* proxyB, const b2Sweep& sweepB)\r
+ const b2DistanceProxy* proxyB, const b2Sweep& sweepB,\r
+ float32 t1)\r
{\r
m_proxyA = proxyA;\r
m_proxyB = proxyB;\r
m_sweepB = sweepB;\r
\r
b2Transform xfA, xfB;\r
- m_sweepA.GetTransform(&xfA, 0.0f);\r
- m_sweepB.GetTransform(&xfB, 0.0f);\r
+ m_sweepA.GetTransform(&xfA, t1);\r
+ m_sweepB.GetTransform(&xfB, t1);\r
\r
if (count == 1)\r
{\r
\r
// Initialize the separating axis.\r
b2SeparationFunction fcn;\r
- fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB);\r
+ fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);\r
#if 0\r
// Dump the curve seen by the root finder\r
{\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
#include <Box2D/Common/b2Math.h>\r
#include <Box2D/Collision/b2Distance.h>\r
-#include <climits>\r
\r
/// Input parameters for b2TimeOfImpact\r
struct b2TOIInput\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
#include <climits>\r
#include <cstring>\r
#include <memory>\r
+using namespace std;\r
\r
int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = \r
{\r
if (size == 0)\r
return NULL;\r
\r
- b2Assert(0 < size && size <= b2_maxBlockSize);\r
+ b2Assert(0 < size);\r
+\r
+ if (size > b2_maxBlockSize)\r
+ {\r
+ return b2Alloc(size);\r
+ }\r
\r
int32 index = s_blockSizeLookup[size];\r
b2Assert(0 <= index && index < b2_blockSizes);\r
return;\r
}\r
\r
- b2Assert(0 < size && size <= b2_maxBlockSize);\r
+ b2Assert(0 < size);\r
+\r
+ if (size > b2_maxBlockSize)\r
+ {\r
+ b2Free(p);\r
+ return;\r
+ }\r
\r
int32 index = s_blockSizeLookup[size];\r
b2Assert(0 <= index && index < b2_blockSizes);\r
/*\r
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
\r
#include <Box2D/Common/b2Settings.h>\r
\r
-const int32 b2_chunkSize = 4096;\r
+const int32 b2_chunkSize = 16 * 1024;\r
const int32 b2_maxBlockSize = 640;\r
const int32 b2_blockSizes = 14;\r
const int32 b2_chunkArrayIncrement = 128;\r
struct b2Block;\r
struct b2Chunk;\r
\r
-// This is a small object allocator used for allocating small\r
-// objects that persist for more than one time step.\r
-// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp\r
+/// This is a small object allocator used for allocating small\r
+/// objects that persist for more than one time step.\r
+/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp\r
class b2BlockAllocator\r
{\r
public:\r
b2BlockAllocator();\r
~b2BlockAllocator();\r
\r
+ /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.\r
void* Allocate(int32 size);\r
+\r
+ /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.\r
void Free(void* p, int32 size);\r
\r
void Clear();\r
/*\r
-* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2011 Erin Catto http://box2d.org\r
*\r
* This software is provided 'as-is', without any express or implied\r
* warranty. In no event will the authors be held liable for any damages\r
* 3. This notice may not be removed or altered from any source distribution.\r
*/\r
\r
-#ifndef B2_TOI_SOLVER_H\r
-#define B2_TOI_SOLVER_H\r
+#include <Box2D/Common/b2Draw.h>\r
\r
-#include <Box2D/Common/b2Math.h>\r
-\r
-class b2Contact;\r
-class b2Body;\r
-struct b2TOIConstraint;\r
-class b2StackAllocator;\r
-\r
-/// This is a pure position solver for a single movable body in contact with\r
-/// multiple non-moving bodies.\r
-class b2TOISolver\r
+b2Draw::b2Draw()\r
{\r
-public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
+ m_drawFlags = 0;\r
+}\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
-\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
+void b2Draw::SetFlags(uint32 flags)\r
+{\r
+ m_drawFlags = flags;\r
+}\r
\r
-private:\r
+uint32 b2Draw::GetFlags() const\r
+{\r
+ return m_drawFlags;\r
+}\r
\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
-};\r
+void b2Draw::AppendFlags(uint32 flags)\r
+{\r
+ m_drawFlags |= flags;\r
+}\r
\r
-#endif\r
+void b2Draw::ClearFlags(uint32 flags)\r
+{\r
+ m_drawFlags &= ~flags;\r
+}\r
--- /dev/null
+/*\r
+* Copyright (c) 2011 Erin Catto http://box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#include <Box2D/Common/b2Math.h>\r
+\r
+/// Color for debug drawing. Each value has the range [0,1].\r
+struct b2Color\r
+{\r
+ b2Color() {}\r
+ b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}\r
+ void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }\r
+ float32 r, g, b;\r
+};\r
+\r
+/// Implement and register this class with a b2World to provide debug drawing of physics\r
+/// entities in your game.\r
+class b2Draw\r
+{\r
+public:\r
+ b2Draw();\r
+\r
+ virtual ~b2Draw() {}\r
+\r
+ enum\r
+ {\r
+ e_shapeBit = 0x0001, ///< draw shapes\r
+ e_jointBit = 0x0002, ///< draw joint connections\r
+ e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes\r
+ e_pairBit = 0x0008, ///< draw broad-phase pairs\r
+ e_centerOfMassBit = 0x0010 ///< draw center of mass frame\r
+ };\r
+\r
+ /// Set the drawing flags.\r
+ void SetFlags(uint32 flags);\r
+\r
+ /// Get the drawing flags.\r
+ uint32 GetFlags() const;\r
+ \r
+ /// Append flags to the current flags.\r
+ void AppendFlags(uint32 flags);\r
+\r
+ /// Clear flags from the current flags.\r
+ void ClearFlags(uint32 flags);\r
+\r
+ /// Draw a closed polygon provided in CCW order.\r
+ virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;\r
+\r
+ /// Draw a solid closed polygon provided in CCW order.\r
+ virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;\r
+\r
+ /// Draw a circle.\r
+ virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;\r
+ \r
+ /// Draw a solid circle.\r
+ virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;\r
+ \r
+ /// Draw a line segment.\r
+ virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;\r
+\r
+ /// Draw a transform. Choose your own length scale.\r
+ /// @param xf a transform.\r
+ virtual void DrawTransform(const b2Transform& xf) = 0;\r
+\r
+protected:\r
+ uint32 m_drawFlags;\r
+};\r
--- /dev/null
+/*\r
+* Copyright (c) 2010 Erin Catto http://www.box2d.org\r
+*\r
+* This software is provided 'as-is', without any express or implied\r
+* warranty. In no event will the authors be held liable for any damages\r
+* arising from the use of this software.\r
+* Permission is granted to anyone to use this software for any purpose,\r
+* including commercial applications, and to alter it and redistribute it\r
+* freely, subject to the following restrictions:\r
+* 1. The origin of this software must not be misrepresented; you must not\r
+* claim that you wrote the original software. If you use this software\r
+* in a product, an acknowledgment in the product documentation would be\r
+* appreciated but is not required.\r
+* 2. Altered source versions must be plainly marked as such, and must not be\r
+* misrepresented as being the original software.\r
+* 3. This notice may not be removed or altered from any source distribution.\r
+*/\r
+\r
+#ifndef B2_GROWABLE_STACK_H\r
+#define B2_GROWABLE_STACK_H\r
+#include <Box2D/Common/b2Settings.h>\r
+#include <cstring>\r
+\r
+/// This is a growable LIFO stack with an initial capacity of N.\r
+/// If the stack size exceeds the initial capacity, the heap is used\r
+/// to increase the size of the stack.\r
+template <typename T, int32 N>\r
+class b2GrowableStack\r
+{\r
+public:\r
+ b2GrowableStack()\r
+ {\r
+ m_stack = m_array;\r
+ m_count = 0;\r
+ m_capacity = N;\r
+ }\r
+\r
+ ~b2GrowableStack()\r
+ {\r
+ if (m_stack != m_array)\r
+ {\r
+ b2Free(m_stack);\r
+ m_stack = NULL;\r
+ }\r
+ }\r
+\r
+ void Push(const T& element)\r
+ {\r
+ if (m_count == m_capacity)\r
+ {\r
+ T* old = m_stack;\r
+ m_capacity *= 2;\r
+ m_stack = (T*)b2Alloc(m_capacity * sizeof(T));\r
+ std::memcpy(m_stack, old, m_count * sizeof(T));\r
+ if (old != m_array)\r
+ &
projects / tanks-ios.git / commitdiff