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
+ {\r
+ b2Free(old);\r
+ }\r
+ }\r
+\r
+ m_stack[m_count] = element;\r
+ ++m_count;\r
+ }\r
+\r
+ T Pop()\r
+ {\r
+ b2Assert(m_count > 0);\r
+ --m_count;\r
+ return m_stack[m_count];\r
+ }\r
+\r
+ int32 GetCount()\r
+ {\r
+ return m_count;\r
+ }\r
+\r
+private:\r
+ T* m_stack;\r
+ T m_array[N];\r
+ int32 m_count;\r
+ int32 m_capacity;\r
+};\r
+\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
/*\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
return x;\r
}\r
\r
-#define b2Sqrt(x) sqrtf(x)\r
-#define b2Atan2(y, x) atan2f(y, x)\r
-\r
-inline float32 b2Abs(float32 a)\r
-{\r
- return a > 0.0f ? a : -a;\r
-}\r
+#define b2Sqrt(x) std::sqrt(x)\r
+#define b2Atan2(y, x) std::atan2(y, x)\r
\r
/// A 2D column vector.\r
struct b2Vec2\r
return b2IsValid(x) && b2IsValid(y);\r
}\r
\r
+ /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)\r
+ b2Vec2 Skew() const\r
+ {\r
+ return b2Vec2(-y, x);\r
+ }\r
+\r
float32 x, y;\r
};\r
\r
struct b2Sweep\r
{\r
/// Get the interpolated transform at a specific time.\r
- /// @param alpha is a factor in [0,1], where 0 indicates t0.\r
- void GetTransform(b2Transform* xf, float32 alpha) const;\r
+ /// @param beta is a factor in [0,1], where 0 indicates alpha0.\r
+ void GetTransform(b2Transform* xf, float32 beta) const;\r
\r
/// Advance the sweep forward, yielding a new initial state.\r
- /// @param t the new initial time.\r
- void Advance(float32 t);\r
+ /// @param alpha the new initial time.\r
+ void Advance(float32 alpha);\r
\r
/// Normalize the angles.\r
void Normalize();\r
b2Vec2 localCenter; ///< local center of mass position\r
b2Vec2 c0, c; ///< center world positions\r
float32 a0, a; ///< world angles\r
+\r
+ /// Fraction of the current time step in the range [0,1]\r
+ /// c0 and a0 are the positions at alpha0.\r
+ float32 alpha0;\r
};\r
\r
\r
return b2MulT(T.R, v - T.position);\r
}\r
\r
+// v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p)\r
+inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)\r
+{\r
+ b2Transform C;\r
+ C.R = b2MulT(A.R, B.R);\r
+ C.position = B.position - A.position;\r
+ return C;\r
+}\r
+\r
+template <typename T>\r
+inline T b2Abs(T a)\r
+{\r
+ return a > T(0) ? a : -a;\r
+}\r
+\r
inline b2Vec2 b2Abs(const b2Vec2& a)\r
{\r
return b2Vec2(b2Abs(a.x), b2Abs(a.y));\r
return result;\r
}\r
\r
-inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const\r
+inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const\r
{\r
- xf->position = (1.0f - alpha) * c0 + alpha * c;\r
- float32 angle = (1.0f - alpha) * a0 + alpha * a;\r
+ xf->position = (1.0f - beta) * c0 + beta * c;\r
+ float32 angle = (1.0f - beta) * a0 + beta * a;\r
xf->R.Set(angle);\r
\r
// Shift to origin\r
xf->position -= b2Mul(xf->R, localCenter);\r
}\r
\r
-inline void b2Sweep::Advance(float32 t)\r
+inline void b2Sweep::Advance(float32 alpha)\r
{\r
- c0 = (1.0f - t) * c0 + t * c;\r
- a0 = (1.0f - t) * a0 + t * a;\r
+ b2Assert(alpha0 < 1.0f);\r
+ float32 beta = (alpha - alpha0) / (1.0f - alpha0);\r
+ c0 = (1.0f - beta) * c0 + beta * c;\r
+ a0 = (1.0f - beta) * a0 + beta * a;\r
+ alpha0 = alpha;\r
}\r
\r
/// Normalize an angle in radians to be between -pi and pi\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 <Box2D/Common/b2Settings.h>\r
#include <cstdlib>\r
\r
-b2Version b2_version = {2, 1, 2};\r
+b2Version b2_version = {2, 2, 0};\r
\r
// Memory allocators. Modify these to use your own allocator.\r
void* b2Alloc(int32 size)\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
typedef unsigned short uint16;\r
typedef unsigned int uint32;\r
typedef float float32;\r
+typedef double float64;\r
\r
#define b2_maxFloat FLT_MAX\r
#define b2_epsilon FLT_EPSILON\r
/// The maximum number of contact points between two convex shapes.\r
#define b2_maxManifoldPoints 2\r
\r
-/// The maximum number of vertices on a convex polygon.\r
+/// The maximum number of vertices on a convex polygon. You cannot increase\r
+/// this too much because b2BlockAllocator has a maximum object size.\r
#define b2_maxPolygonVertices 8\r
\r
/// This is used to fatten AABBs in the dynamic tree. This allows proxies\r
/// Making it larger may create artifacts for vertex collision.\r
#define b2_polygonRadius (2.0f * b2_linearSlop)\r
\r
+/// Maximum number of sub-steps per contact in continuous physics simulation.\r
+#define b2_maxSubSteps 8\r
+\r
\r
// Dynamics\r
\r
/// to overshoot.\r
#define b2_contactBaumgarte 0.2f\r
\r
+\r
// Sleep\r
\r
/// The time that a body must be still before it will go to sleep.\r
/// Current version.\r
extern b2Version b2_version;\r
\r
-/// Friction mixing law. Feel free to customize this.\r
-inline float32 b2MixFriction(float32 friction1, float32 friction2)\r
-{\r
- return sqrtf(friction1 * friction2);\r
-}\r
-\r
-/// Restitution mixing law. Feel free to customize this.\r
-inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)\r
-{\r
- return restitution1 > restitution2 ? restitution1 : restitution2;\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
-* 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
--- /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/b2Timer.h>\r
+\r
+float64 b2Timer::s_invFrequency = 0.0f;\r
+\r
+#if defined(WIN32)\r
+\r
+#include <Windows.h>\r
+\r
+b2Timer::b2Timer()\r
+{\r
+ LARGE_INTEGER largeInteger;\r
+\r
+ if (s_invFrequency == 0.0f)\r
+ {\r
+ QueryPerformanceFrequency(&largeInteger);\r
+ s_invFrequency = float64(largeInteger.QuadPart);\r
+ if (s_invFrequency > 0.0f)\r
+ {\r
+ s_invFrequency = 1000.0f / s_invFrequency;\r
+ }\r
+ }\r
+\r
+ QueryPerformanceCounter(&largeInteger);\r
+ m_start = float64(largeInteger.QuadPart);\r
+}\r
+\r
+void b2Timer::Reset()\r
+{\r
+ LARGE_INTEGER largeInteger;\r
+ QueryPerformanceCounter(&largeInteger);\r
+ m_start = float64(largeInteger.QuadPart);\r
+}\r
+\r
+float32 b2Timer::GetMilliseconds() const\r
+{\r
+ LARGE_INTEGER largeInteger;\r
+ QueryPerformanceCounter(&largeInteger);\r
+ float64 count = float64(largeInteger.QuadPart);\r
+ float32 ms = float32(s_invFrequency * (count - m_start));\r
+ return ms;\r
+}\r
+\r
+#else\r
+\r
+b2Timer::b2Timer()\r
+{\r
+}\r
+\r
+void b2Timer::Reset()\r
+{\r
+}\r
+\r
+float32 b2Timer::GetMilliseconds() const\r
+{\r
+ return 0.0f;\r
+}\r
+\r
+#endif\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/b2Settings.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
+/// Timer for profiling. This has platform specific code and may\r
+/// not work on every platform.\r
+class b2Timer\r
{\r
public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
+ /// Constructor\r
+ b2Timer();\r
\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
+ /// Reset the timer.\r
+ void Reset();\r
+\r
+ /// Get the time since construction or the last reset.\r
+ float32 GetMilliseconds() const;\r
\r
private:\r
\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
+ float64 m_start;\r
+ static float64 s_invFrequency;\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
#include <Box2D/Collision/b2TimeOfImpact.h>\r
\r
#include <new>\r
+using namespace std;\r
\r
-b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)\r
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)\r
{\r
void* mem = allocator->Allocate(sizeof(b2CircleContact));\r
return new (mem) b2CircleContact(fixtureA, fixtureB);\r
}\r
\r
b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)\r
- : b2Contact(fixtureA, fixtureB)\r
+ : b2Contact(fixtureA, 0, fixtureB, 0)\r
{\r
b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);\r
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);\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
class b2CircleContact : public b2Contact\r
{\r
public:\r
- static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);\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 <Box2D/Dynamics/Contacts/b2CircleContact.h>\r
#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>\r
#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>\r
+#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>\r
+#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>\r
+#include <Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h>\r
+#include <Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h>\r
#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>\r
\r
#include <Box2D/Collision/b2Collision.h>\r
AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);\r
AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);\r
AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);\r
+ AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);\r
+ AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);\r
+ AddType(b2LoopAndCircleContact::Create, b2LoopAndCircleContact::Destroy, b2Shape::e_loop, b2Shape::e_circle);\r
+ AddType(b2LoopAndPolygonContact::Create, b2LoopAndPolygonContact::Destroy, b2Shape::e_loop, b2Shape::e_polygon);\r
}\r
\r
void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,\r
}\r
}\r
\r
-b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)\r
+b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)\r
{\r
if (s_initialized == false)\r
{\r
{\r
if (s_registers[type1][type2].primary)\r
{\r
- return createFcn(fixtureA, fixtureB, allocator);\r
+ return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);\r
}\r
else\r
{\r
- return createFcn(fixtureB, fixtureA, allocator);\r
+ return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);\r
}\r
}\r
else\r
destroyFcn(contact, allocator);\r
}\r
\r
-b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB)\r
+b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)\r
{\r
m_flags = e_enabledFlag;\r
\r
m_fixtureA = fA;\r
m_fixtureB = fB;\r
\r
+ m_indexA = indexA;\r
+ m_indexB = indexB;\r
+\r
m_manifold.pointCount = 0;\r
\r
m_prev = NULL;\r
m_nodeB.other = NULL;\r
\r
m_toiCount = 0;\r
+\r
+ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);\r
+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);\r
}\r
\r
// Update the contact manifold and touching status.\r
{\r
const b2Shape* shapeA = m_fixtureA->GetShape();\r
const b2Shape* shapeB = m_fixtureB->GetShape();\r
- touching = b2TestOverlap(shapeA, shapeB, xfA, xfB);\r
+ touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);\r
\r
// Sensors don't generate manifolds.\r
m_manifold.pointCount = 0;\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 <Box2D/Common/b2Math.h>\r
#include <Box2D/Collision/b2Collision.h>\r
#include <Box2D/Collision/Shapes/b2Shape.h>\r
-#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
#include <Box2D/Dynamics/b2Fixture.h>\r
\r
class b2Body;\r
class b2StackAllocator;\r
class b2ContactListener;\r
\r
-typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);\r
+/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero.\r
+/// For example, anything slides on ice.\r
+inline float32 b2MixFriction(float32 friction1, float32 friction2)\r
+{\r
+ return std::sqrt(friction1 * friction2);\r
+}\r
+\r
+/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.\r
+/// For example, a superball bounces on anything.\r
+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)\r
+{\r
+ return restitution1 > restitution2 ? restitution1 : restitution2;\r
+}\r
+\r
+typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB,\r
+ b2BlockAllocator* allocator);\r
typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
struct b2ContactRegister\r
b2Contact* GetNext();\r
const b2Contact* GetNext() const;\r
\r
- /// Get the first fixture in this contact.\r
+ /// Get fixture A in this contact.\r
b2Fixture* GetFixtureA();\r
const b2Fixture* GetFixtureA() const;\r
\r
- /// Get the second fixture in this contact.\r
+ /// Get the child primitive index for fixture A.\r
+ int32 GetChildIndexA() const;\r
+\r
+ /// Get fixture B in this contact.\r
b2Fixture* GetFixtureB();\r
const b2Fixture* GetFixtureB() const;\r
\r
+ /// Get the child primitive index for fixture B.\r
+ int32 GetChildIndexB() const;\r
+\r
+ /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.\r
+ /// This value persists until set or reset.\r
+ void SetFriction(float32 friction);\r
+\r
+ /// Get the friction.\r
+ float32 GetFriction() const;\r
+\r
+ /// Reset the friction mixture to the default value.\r
+ void ResetFriction();\r
+\r
+ /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.\r
+ /// The value persists until you set or reset.\r
+ void SetRestitution(float32 restitution);\r
+\r
+ /// Get the restitution.\r
+ float32 GetRestitution() const;\r
+\r
+ /// Reset the restitution to the default value.\r
+ void ResetRestitution();\r
+\r
/// Evaluate this contact with your own manifold and transforms.\r
virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;\r
\r
// This bullet contact had a TOI event\r
e_bulletHitFlag = 0x0010,\r
\r
+ // This contact has a valid TOI in m_toi\r
+ e_toiFlag = 0x0020\r
};\r
\r
/// Flag this contact for filtering. Filtering will occur the next time step.\r
static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,\r
b2Shape::Type typeA, b2Shape::Type typeB);\r
static void InitializeRegisters();\r
- static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);\r
+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);\r
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}\r
- b2Contact(b2Fixture* fixtureA, b2Fixture* fixtureB);\r
+ b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);\r
virtual ~b2Contact() {}\r
\r
void Update(b2ContactListener* listener);\r
b2Fixture* m_fixtureA;\r
b2Fixture* m_fixtureB;\r
\r
+ int32 m_indexA;\r
+ int32 m_indexB;\r
+\r
b2Manifold m_manifold;\r
\r
int32 m_toiCount;\r
-// float32 m_toi;\r
+ float32 m_toi;\r
+\r
+ float32 m_friction;\r
+ float32 m_restitution;\r
};\r
\r
inline b2Manifold* b2Contact::GetManifold()\r
return m_fixtureB;\r
}\r
\r
+inline int32 b2Contact::GetChildIndexA() const\r
+{\r
+ return m_indexA;\r
+}\r
+\r
inline const b2Fixture* b2Contact::GetFixtureB() const\r
{\r
return m_fixtureB;\r
}\r
\r
+inline int32 b2Contact::GetChildIndexB() const\r
+{\r
+ return m_indexB;\r
+}\r
+\r
inline void b2Contact::FlagForFiltering()\r
{\r
m_flags |= e_filterFlag;\r
}\r
\r
+inline void b2Contact::SetFriction(float32 friction)\r
+{\r
+ m_friction = friction;\r
+}\r
+\r
+inline float32 b2Contact::GetFriction() const\r
+{\r
+ return m_friction;\r
+}\r
+\r
+inline void b2Contact::ResetFriction()\r
+{\r
+ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);\r
+}\r
+\r
+inline void b2Contact::SetRestitution(float32 restitution)\r
+{\r
+ m_restitution = restitution;\r
+}\r
+\r
+inline float32 b2Contact::GetRestitution() const\r
+{\r
+ return m_restitution;\r
+}\r
+\r
+inline void b2Contact::ResetRestitution()\r
+{\r
+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);\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
#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>\r
+\r
+\r
#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
#include <Box2D/Dynamics/b2Body.h>\r
#include <Box2D/Dynamics/b2Fixture.h>\r
\r
#define B2_DEBUG_SOLVER 0\r
\r
-b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,\r
- b2StackAllocator* allocator, float32 impulseRatio)\r
+b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)\r
{\r
- m_allocator = allocator;\r
+ m_allocator = def->allocator;\r
\r
- m_constraintCount = contactCount;\r
- m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));\r
+ m_count = def->count;\r
+ m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactConstraint));\r
\r
- for (int32 i = 0; i < m_constraintCount; ++i)\r
+ // Initialize position independent portions of the constraints.\r
+ for (int32 i = 0; i < m_count; ++i)\r
{\r
- b2Contact* contact = contacts[i];\r
+ b2Contact* contact = def->contacts[i];\r
\r
b2Fixture* fixtureA = contact->m_fixtureA;\r
b2Fixture* fixtureB = contact->m_fixtureB;\r
b2Body* bodyB = fixtureB->GetBody();\r
b2Manifold* manifold = contact->GetManifold();\r
\r
- float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());\r
- float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());\r
-\r
- b2Vec2 vA = bodyA->m_linearVelocity;\r
- b2Vec2 vB = bodyB->m_linearVelocity;\r
- float32 wA = bodyA->m_angularVelocity;\r
- float32 wB = bodyB->m_angularVelocity;\r
-\r
b2Assert(manifold->pointCount > 0);\r
\r
- b2WorldManifold worldManifold;\r
- worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);\r
-\r
b2ContactConstraint* cc = m_constraints + i;\r
+ cc->friction = contact->m_friction;\r
+ cc->restitution = contact->m_restitution;\r
cc->bodyA = bodyA;\r
cc->bodyB = bodyB;\r
cc->manifold = manifold;\r
- cc->normal = worldManifold.normal;\r
+ cc->normal.SetZero();\r
cc->pointCount = manifold->pointCount;\r
- cc->friction = friction;\r
\r
cc->localNormal = manifold->localNormal;\r
cc->localPoint = manifold->localPoint;\r
- cc->radius = radiusA + radiusB;\r
+ cc->radiusA = radiusA;\r
+ cc->radiusB = radiusB;\r
cc->type = manifold->type;\r
\r
for (int32 j = 0; j < cc->pointCount; ++j)\r
b2ManifoldPoint* cp = manifold->points + j;\r
b2ContactConstraintPoint* ccp = cc->points + j;\r
\r
- ccp->normalImpulse = impulseRatio * cp->normalImpulse;\r
- ccp->tangentImpulse = impulseRatio * cp->tangentImpulse;\r
+ if (def->warmStarting)\r
+ {\r
+ ccp->normalImpulse = def->impulseRatio * cp->normalImpulse;\r
+ ccp->tangentImpulse = def->impulseRatio * cp->tangentImpulse;\r
+ }\r
+ else\r
+ {\r
+ ccp->normalImpulse = 0.0f;\r
+ ccp->tangentImpulse = 0.0f;\r
+ }\r
\r
ccp->localPoint = cp->localPoint;\r
+ ccp->rA.SetZero();\r
+ ccp->rB.SetZero();\r
+ ccp->normalMass = 0.0f;\r
+ ccp->tangentMass = 0.0f;\r
+ ccp->velocityBias = 0.0f;\r
+ }\r
+\r
+ cc->K.SetZero();\r
+ cc->normalMass.SetZero();\r
+ }\r
+}\r
+\r
+b2ContactSolver::~b2ContactSolver()\r
+{\r
+ m_allocator->Free(m_constraints);\r
+}\r
+\r
+// Initialize position dependent portions of the velocity constraints.\r
+void b2ContactSolver::InitializeVelocityConstraints()\r
+{\r
+ for (int32 i = 0; i < m_count; ++i)\r
+ {\r
+ b2ContactConstraint* cc = m_constraints + i;\r
+\r
+ float32 radiusA = cc->radiusA;\r
+ float32 radiusB = cc->radiusB;\r
+ b2Body* bodyA = cc->bodyA;\r
+ b2Body* bodyB = cc->bodyB;\r
+ b2Manifold* manifold = cc->manifold;\r
+\r
+ b2Vec2 vA = bodyA->m_linearVelocity;\r
+ b2Vec2 vB = bodyB->m_linearVelocity;\r
+ float32 wA = bodyA->m_angularVelocity;\r
+ float32 wB = bodyB->m_angularVelocity;\r
+\r
+ b2Assert(manifold->pointCount > 0);\r
+\r
+ b2WorldManifold worldManifold;\r
+ worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);\r
+\r
+ cc->normal = worldManifold.normal;\r
+\r
+ for (int32 j = 0; j < cc->pointCount; ++j)\r
+ {\r
+ b2ContactConstraintPoint* ccp = cc->points + j;\r
\r
ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;\r
ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;\r
float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));\r
if (vRel < -b2_velocityThreshold)\r
{\r
- ccp->velocityBias = -restitution * vRel;\r
+ ccp->velocityBias = -cc->restitution * vRel;\r
}\r
}\r
\r
{\r
b2ContactConstraintPoint* ccp1 = cc->points + 0;\r
b2ContactConstraintPoint* ccp2 = cc->points + 1;\r
- \r
+\r
float32 invMassA = bodyA->m_invMass;\r
float32 invIA = bodyA->m_invI;\r
float32 invMassB = bodyB->m_invMass;\r
float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;\r
\r
// Ensure a reasonable condition number.\r
- const float32 k_maxConditionNumber = 100.0f;\r
+ const float32 k_maxConditionNumber = 1000.0f;\r
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))\r
{\r
// K is safe to invert.\r
}\r
}\r
\r
-b2ContactSolver::~b2ContactSolver()\r
-{\r
- m_allocator->Free(m_constraints);\r
-}\r
-\r
void b2ContactSolver::WarmStart()\r
{\r
// Warm start.\r
- for (int32 i = 0; i < m_constraintCount; ++i)\r
+ for (int32 i = 0; i < m_count; ++i)\r
{\r
b2ContactConstraint* c = m_constraints + i;\r
\r
\r
void b2ContactSolver::SolveVelocityConstraints()\r
{\r
- for (int32 i = 0; i < m_constraintCount; ++i)\r
+ for (int32 i = 0; i < m_count; ++i)\r
{\r
b2ContactConstraint* c = m_constraints + i;\r
b2Body* bodyA = c->bodyA;\r
\r
void b2ContactSolver::StoreImpulses()\r
{\r
- for (int32 i = 0; i < m_constraintCount; ++i)\r
+ for (int32 i = 0; i < m_count; ++i)\r
{\r
b2ContactConstraint* c = m_constraints + i;\r
b2Manifold* m = c->manifold;\r
}\r
\r
point = 0.5f * (pointA + pointB);\r
- separation = b2Dot(pointB - pointA, normal) - cc->radius;\r
+ separation = b2Dot(pointB - pointA, normal) - cc->radiusA - cc->radiusB;\r
}\r
break;\r
\r
b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);\r
\r
b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint);\r
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;\r
+ separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB;\r
point = clipPoint;\r
}\r
break;\r
b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);\r
\r
b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint);\r
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;\r
+ separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB;\r
point = clipPoint;\r
\r
// Ensure normal points from A to B\r
{\r
float32 minSeparation = 0.0f;\r
\r
- for (int32 i = 0; i < m_constraintCount; ++i)\r
+ for (int32 i = 0; i < m_count; ++i)\r
{\r
b2ContactConstraint* c = m_constraints + i;\r
b2Body* bodyA = c->bodyA;\r
// push the separation above -b2_linearSlop.\r
return minSeparation >= -1.5f * b2_linearSlop;\r
}\r
+\r
+// Sequential position solver for position constraints.\r
+bool b2ContactSolver::SolveTOIPositionConstraints(float32 baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB)\r
+{\r
+ float32 minSeparation = 0.0f;\r
+\r
+ for (int32 i = 0; i < m_count; ++i)\r
+ {\r
+ b2ContactConstraint* c = m_constraints + i;\r
+ b2Body* bodyA = c->bodyA;\r
+ b2Body* bodyB = c->bodyB;\r
+\r
+ float32 massA = 0.0f;\r
+ if (bodyA == toiBodyA || bodyA == toiBodyB)\r
+ {\r
+ massA = bodyA->m_mass;\r
+ }\r
+\r
+ float32 massB = 0.0f;\r
+ if (bodyB == toiBodyA || bodyB == toiBodyB)\r
+ {\r
+ massB = bodyB->m_mass;\r
+ }\r
+\r
+ float32 invMassA = bodyA->m_mass * bodyA->m_invMass;\r
+ float32 invIA = bodyA->m_mass * bodyA->m_invI;\r
+ float32 invMassB = bodyB->m_mass * bodyB->m_invMass;\r
+ float32 invIB = bodyB->m_mass * bodyB->m_invI;\r
+\r
+ // Solve normal constraints\r
+ for (int32 j = 0; j < c->pointCount; ++j)\r
+ {\r
+ b2PositionSolverManifold psm;\r
+ psm.Initialize(c, j);\r
+ b2Vec2 normal = psm.normal;\r
+\r
+ b2Vec2 point = psm.point;\r
+ float32 separation = psm.separation;\r
+\r
+ b2Vec2 rA = point - bodyA->m_sweep.c;\r
+ b2Vec2 rB = point - bodyB->m_sweep.c;\r
+\r
+ // Track max constraint error.\r
+ minSeparation = b2Min(minSeparation, separation);\r
+\r
+ // Prevent large corrections and allow slop.\r
+ float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);\r
+\r
+ // Compute the effective mass.\r
+ float32 rnA = b2Cross(rA, normal);\r
+ float32 rnB = b2Cross(rB, normal);\r
+ float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;\r
+\r
+ // Compute normal impulse\r
+ float32 impulse = K > 0.0f ? - C / K : 0.0f;\r
+\r
+ b2Vec2 P = impulse * normal;\r
+\r
+ bodyA->m_sweep.c -= invMassA * P;\r
+ bodyA->m_sweep.a -= invIA * b2Cross(rA, P);\r
+ bodyA->SynchronizeTransform();\r
+\r
+ bodyB->m_sweep.c += invMassB * P;\r
+ bodyB->m_sweep.a += invIB * b2Cross(rB, P);\r
+ bodyB->SynchronizeTransform();\r
+ }\r
+ }\r
+\r
+ // We can't expect minSpeparation >= -b2_linearSlop because we don't\r
+ // push the separation above -b2_linearSlop.\r
+ return minSeparation >= -1.5f * b2_linearSlop;\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
b2Body* bodyA;\r
b2Body* bodyB;\r
b2Manifold::Type type;\r
- float32 radius;\r
+ float32 radiusA, radiusB;\r
float32 friction;\r
+ float32 restitution;\r
int32 pointCount;\r
b2Manifold* manifold;\r
};\r
\r
+struct b2ContactSolverDef\r
+{\r
+ b2Contact** contacts;\r
+ int32 count;\r
+ b2StackAllocator* allocator;\r
+ float32 impulseRatio;\r
+ bool warmStarting;\r
+};\r
+\r
class b2ContactSolver\r
{\r
public:\r
- b2ContactSolver(b2Contact** contacts, int32 contactCount,\r
- b2StackAllocator* allocator, float32 impulseRatio);\r
-\r
+ b2ContactSolver(b2ContactSolverDef* def);\r
~b2ContactSolver();\r
\r
+ void InitializeVelocityConstraints();\r
+\r
void WarmStart();\r
void SolveVelocityConstraints();\r
void StoreImpulses();\r
\r
bool SolvePositionConstraints(float32 baumgarte);\r
+ bool SolveTOIPositionConstraints(float32 baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB);\r
\r
b2StackAllocator* m_allocator;\r
b2ContactConstraint* m_constraints;\r
- int m_constraintCount;\r
+ int m_count;\r
};\r
\r
#endif\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
+#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>\r
+#include <Box2D/Common/b2BlockAllocator.h>\r
+#include <Box2D/Dynamics/b2Fixture.h>\r
+\r
+#include <new>\r
+using namespace std;\r
+\r
+b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));\r
+ return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);\r
+}\r
+\r
+void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)\r
+{\r
+ ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();\r
+ allocator->Free(contact, sizeof(b2EdgeAndCircleContact));\r
+}\r
+\r
+b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)\r
+: b2Contact(fixtureA, 0, fixtureB, 0)\r
+{\r
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);\r
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);\r
+}\r
+\r
+void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)\r
+{\r
+ b2CollideEdgeAndCircle( manifold,\r
+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,\r
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);\r
+}\r
/*\r
-* Copyright (c) 2006-2010 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
* 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
+#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H\r
+#define B2_EDGE_AND_CIRCLE_CONTACT_H\r
\r
-#include <Box2D/Common/b2Math.h>\r
+#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
\r
-class b2Contact;\r
-class b2Body;\r
-struct b2TOIConstraint;\r
-class b2StackAllocator;\r
+class b2BlockAllocator;\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
+class b2EdgeAndCircleContact : public b2Contact\r
{\r
public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
+ b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);\r
+ ~b2EdgeAndCircleContact() {}\r
\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
-\r
-private:\r
-\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);\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/Dynamics/Contacts/b2EdgeAndPolygonContact.h>\r
+#include <Box2D/Common/b2BlockAllocator.h>\r
+#include <Box2D/Dynamics/b2Fixture.h>\r
+\r
+#include <new>\r
+using namespace std;\r
+\r
+b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));\r
+ return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);\r
+}\r
+\r
+void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)\r
+{\r
+ ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();\r
+ allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));\r
+}\r
+\r
+b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)\r
+: b2Contact(fixtureA, 0, fixtureB, 0)\r
+{\r
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);\r
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);\r
+}\r
+\r
+void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)\r
+{\r
+ b2CollideEdgeAndPolygon( manifold,\r
+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,\r
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);\r
+}\r
/*\r
-* Copyright (c) 2006-2010 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
* 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
+#ifndef B2_EDGE_AND_POLYGON_CONTACT_H\r
+#define B2_EDGE_AND_POLYGON_CONTACT_H\r
\r
-#include <Box2D/Common/b2Math.h>\r
+#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
\r
-class b2Contact;\r
-class b2Body;\r
-struct b2TOIConstraint;\r
-class b2StackAllocator;\r
+class b2BlockAllocator;\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
+class b2EdgeAndPolygonContact : public b2Contact\r
{\r
public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
+ b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);\r
+ ~b2EdgeAndPolygonContact() {}\r
\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
-\r
-private:\r
-\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);\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/Dynamics/Contacts/b2LoopAndCircleContact.h>\r
+#include <Box2D/Common/b2BlockAllocator.h>\r
+#include <Box2D/Dynamics/b2Fixture.h>\r
+#include <Box2D/Collision/Shapes/b2LoopShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+\r
+#include <new>\r
+using namespace std;\r
+\r
+b2Contact* b2LoopAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2LoopAndCircleContact));\r
+ return new (mem) b2LoopAndCircleContact(fixtureA, indexA, fixtureB, indexB);\r
+}\r
+\r
+void b2LoopAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)\r
+{\r
+ ((b2LoopAndCircleContact*)contact)->~b2LoopAndCircleContact();\r
+ allocator->Free(contact, sizeof(b2LoopAndCircleContact));\r
+}\r
+\r
+b2LoopAndCircleContact::b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)\r
+: b2Contact(fixtureA, indexA, fixtureB, indexB)\r
+{\r
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_loop);\r
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);\r
+}\r
+\r
+void b2LoopAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)\r
+{\r
+ b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape();\r
+ b2EdgeShape edge;\r
+ loop->GetChildEdge(&edge, m_indexA);\r
+ b2CollideEdgeAndCircle( manifold, &edge, xfA,\r
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);\r
+}\r
/*\r
-* Copyright (c) 2006-2010 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
* 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
+#ifndef B2_LOOP_AND_CIRCLE_CONTACT_H\r
+#define B2_LOOP_AND_CIRCLE_CONTACT_H\r
\r
-#include <Box2D/Common/b2Math.h>\r
+#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
\r
-class b2Contact;\r
-class b2Body;\r
-struct b2TOIConstraint;\r
-class b2StackAllocator;\r
+class b2BlockAllocator;\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
+class b2LoopAndCircleContact : public b2Contact\r
{\r
public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
+ b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);\r
+ ~b2LoopAndCircleContact() {}\r
\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
-\r
-private:\r
-\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);\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/Dynamics/Contacts/b2LoopAndPolygonContact.h>\r
+#include <Box2D/Common/b2BlockAllocator.h>\r
+#include <Box2D/Dynamics/b2Fixture.h>\r
+#include <Box2D/Collision/Shapes/b2LoopShape.h>\r
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>\r
+\r
+#include <new>\r
+using namespace std;\r
+\r
+b2Contact* b2LoopAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)\r
+{\r
+ void* mem = allocator->Allocate(sizeof(b2LoopAndPolygonContact));\r
+ return new (mem) b2LoopAndPolygonContact(fixtureA, indexA, fixtureB, indexB);\r
+}\r
+\r
+void b2LoopAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)\r
+{\r
+ ((b2LoopAndPolygonContact*)contact)->~b2LoopAndPolygonContact();\r
+ allocator->Free(contact, sizeof(b2LoopAndPolygonContact));\r
+}\r
+\r
+b2LoopAndPolygonContact::b2LoopAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)\r
+: b2Contact(fixtureA, indexA, fixtureB, indexB)\r
+{\r
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_loop);\r
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);\r
+}\r
+\r
+void b2LoopAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)\r
+{\r
+ b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape();\r
+ b2EdgeShape edge;\r
+ loop->GetChildEdge(&edge, m_indexA);\r
+ b2CollideEdgeAndPolygon( manifold, &edge, xfA,\r
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);\r
+}\r
/*\r
-* Copyright (c) 2006-2010 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
* 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
+#ifndef B2_LOOP_AND_POLYGON_CONTACT_H\r
+#define B2_LOOP_AND_POLYGON_CONTACT_H\r
\r
-#include <Box2D/Common/b2Math.h>\r
+#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
\r
-class b2Contact;\r
-class b2Body;\r
-struct b2TOIConstraint;\r
-class b2StackAllocator;\r
+class b2BlockAllocator;\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
+class b2LoopAndPolygonContact : public b2Contact\r
{\r
public:\r
- b2TOISolver(b2StackAllocator* allocator);\r
- ~b2TOISolver();\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
- void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);\r
- void Clear();\r
+ b2LoopAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);\r
+ ~b2LoopAndPolygonContact() {}\r
\r
- // Perform one solver iteration. Returns true if converged.\r
- bool Solve(float32 baumgarte);\r
-\r
-private:\r
-\r
- b2TOIConstraint* m_constraints;\r
- int32 m_count;\r
- b2Body* m_toiBody;\r
- b2StackAllocator* m_allocator;\r
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);\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
#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>\r
#include <Box2D/Common/b2BlockAllocator.h>\r
-#include <Box2D/Collision/b2TimeOfImpact.h>\r
-#include <Box2D/Dynamics/b2Body.h>\r
#include <Box2D/Dynamics/b2Fixture.h>\r
-#include <Box2D/Dynamics/b2WorldCallbacks.h>\r
\r
#include <new>\r
+using namespace std;\r
\r
-b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)\r
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)\r
{\r
void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));\r
return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);\r
}\r
\r
b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)\r
-: b2Contact(fixtureA, fixtureB)\r
+: b2Contact(fixtureA, 0, fixtureB, 0)\r
{\r
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);\r
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);\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
class b2PolygonAndCircleContact : public b2Contact\r
{\r
public:\r
- static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);\r
+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);\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 <Box2D/Dynamics/b2WorldCallbacks.h>\r
\r
#include <new>\r
+using namespace std;\r
\r
-b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)\r
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)\r
{\r
void* mem = allocator->Allocate(sizeof(b2PolygonContact));\r
return new (mem) b2PolygonContact(fixtureA, fixtureB);\r
}\r
\r
b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)\r
- : b2Contact(fixtureA, fixtureB)\r
+ : b2Contact(fixtureA, 0, fixtureB, 0)\r
{\r
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);\r
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);\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
class b2PolygonContact : public b2Contact\r
{\r
public:\r
- static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);\r
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,\r
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);\r
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);\r
\r
b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);\r
+++ /dev/null
-/*\r
-* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com\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/Dynamics/Contacts/b2TOISolver.h>\r
-#include <Box2D/Dynamics/Contacts/b2Contact.h>\r
-#include <Box2D/Dynamics/b2Body.h>\r
-#include <Box2D/Dynamics/b2Fixture.h>\r
-#include <Box2D/Common/b2StackAllocator.h>\r
-\r
-struct b2TOIConstraint\r
-{\r
- b2Vec2 localPoints[b2_maxManifoldPoints];\r
- b2Vec2 localNormal;\r
- b2Vec2 localPoint;\r
- b2Manifold::Type type;\r
- float32 radius;\r
- int32 pointCount;\r
- b2Body* bodyA;\r
- b2Body* bodyB;\r
-};\r
-\r
-b2TOISolver::b2TOISolver(b2StackAllocator* allocator)\r
-{\r
- m_allocator = allocator;\r
- m_constraints = NULL;\r
- m_count = NULL;\r
- m_toiBody = NULL;\r
-}\r
-\r
-b2TOISolver::~b2TOISolver()\r
-{\r
- Clear();\r
-}\r
-\r
-void b2TOISolver::Clear()\r
-{\r
- if (m_allocator && m_constraints)\r
- {\r
- m_allocator->Free(m_constraints);\r
- m_constraints = NULL;\r
- }\r
-}\r
-\r
-void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)\r
-{\r
- Clear();\r
-\r
- m_count = count;\r
- m_toiBody = toiBody;\r
-\r
- m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));\r
-\r
- for (int32 i = 0; i < m_count; ++i)\r
- {\r
- b2Contact* contact = contacts[i];\r
-\r
- b2Fixture* fixtureA = contact->GetFixtureA();\r
- b2Fixture* fixtureB = contact->GetFixtureB();\r
- b2Shape* shapeA = fixtureA->GetShape();\r
- b2Shape* shapeB = fixtureB->GetShape();\r
- float32 radiusA = shapeA->m_radius;\r
- float32 radiusB = shapeB->m_radius;\r
- b2Body* bodyA = fixtureA->GetBody();\r
- b2Body* bodyB = fixtureB->GetBody();\r
- b2Manifold* manifold = contact->GetManifold();\r
-\r
- b2Assert(manifold->pointCount > 0);\r
-\r
- b2TOIConstraint* constraint = m_constraints + i;\r
- constraint->bodyA = bodyA;\r
- constraint->bodyB = bodyB;\r
- constraint->localNormal = manifold->localNormal;\r
- constraint->localPoint = manifold->localPoint;\r
- constraint->type = manifold->type;\r
- constraint->pointCount = manifold->pointCount;\r
- constraint->radius = radiusA + radiusB;\r
-\r
- for (int32 j = 0; j < constraint->pointCount; ++j)\r
- {\r
- b2ManifoldPoint* cp = manifold->points + j;\r
- constraint->localPoints[j] = cp->localPoint;\r
- }\r
- }\r
-}\r
-\r
-struct b2TOISolverManifold\r
-{\r
- void Initialize(b2TOIConstraint* cc, int32 index)\r
- {\r
- b2Assert(cc->pointCount > 0);\r
-\r
- switch (cc->type)\r
- {\r
- case b2Manifold::e_circles:\r
- {\r
- b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);\r
- b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);\r
- if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)\r
- {\r
- normal = pointB - pointA;\r
- normal.Normalize();\r
- }\r
- else\r
- {\r
- normal.Set(1.0f, 0.0f);\r
- }\r
-\r
- point = 0.5f * (pointA + pointB);\r
- separation = b2Dot(pointB - pointA, normal) - cc->radius;\r
- }\r
- break;\r
-\r
- case b2Manifold::e_faceA:\r
- {\r
- normal = cc->bodyA->GetWorldVector(cc->localNormal);\r
- b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);\r
-\r
- b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);\r
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;\r
- point = clipPoint;\r
- }\r
- break;\r
-\r
- case b2Manifold::e_faceB:\r
- {\r
- normal = cc->bodyB->GetWorldVector(cc->localNormal);\r
- b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);\r
-\r
- b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);\r
- separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;\r
- point = clipPoint;\r
-\r
- // Ensure normal points from A to B\r
- normal = -normal;\r
- }\r
- break;\r
- }\r
- }\r
-\r
- b2Vec2 normal;\r
- b2Vec2 point;\r
- float32 separation;\r
-};\r
-\r
-// Push out the toi body to provide clearance for further simulation.\r
-bool b2TOISolver::Solve(float32 baumgarte)\r
-{\r
- float32 minSeparation = 0.0f;\r
-\r
- for (int32 i = 0; i < m_count; ++i)\r
- {\r
- b2TOIConstraint* c = m_constraints + i;\r
- b2Body* bodyA = c->bodyA;\r
- b2Body* bodyB = c->bodyB;\r
-\r
- float32 massA = bodyA->m_mass;\r
- float32 massB = bodyB->m_mass;\r
-\r
- // Only the TOI body should move.\r
- if (bodyA == m_toiBody)\r
- {\r
- massB = 0.0f;\r
- }\r
- else\r
- {\r
- massA = 0.0f;\r
- }\r
-\r
- float32 invMassA = massA * bodyA->m_invMass;\r
- float32 invIA = massA * bodyA->m_invI;\r
- float32 invMassB = massB * bodyB->m_invMass;\r
- float32 invIB = massB * bodyB->m_invI;\r
-\r
- // Solve normal constraints\r
- for (int32 j = 0; j < c->pointCount; ++j)\r
- {\r
- b2TOISolverManifold psm;\r
- psm.Initialize(c, j);\r
- b2Vec2 normal = psm.normal;\r
-\r
- b2Vec2 point = psm.point;\r
- float32 separation = psm.separation;\r
-\r
- b2Vec2 rA = point - bodyA->m_sweep.c;\r
- b2Vec2 rB = point - bodyB->m_sweep.c;\r
-\r
- // Track max constraint error.\r
- minSeparation = b2Min(minSeparation, separation);\r
-\r
- // Prevent large corrections and allow slop.\r
- float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);\r
-\r
- // Compute the effective mass.\r
- float32 rnA = b2Cross(rA, normal);\r
- float32 rnB = b2Cross(rB, normal);\r
- float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;\r
-\r
- // Compute normal impulse\r
- float32 impulse = K > 0.0f ? - C / K : 0.0f;\r
-\r
- b2Vec2 P = impulse * normal;\r
-\r
- bodyA->m_sweep.c -= invMassA * P;\r
- bodyA->m_sweep.a -= invIA * b2Cross(rA, P);\r
- bodyA->SynchronizeTransform();\r
-\r
- bodyB->m_sweep.c += invMassB * P;\r
- bodyB->m_sweep.a += invIB * b2Cross(rB, P);\r
- bodyB->SynchronizeTransform();\r
- }\r
- }\r
-\r
- // We can't expect minSpeparation >= -b2_linearSlop because we don't\r
- // push the separation above -b2_linearSlop.\r
- return minSeparation >= -1.5f * b2_linearSlop;\r
-}\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
length = d.Length();\r
}\r
\r
-\r
b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)\r
: b2Joint(def)\r
{\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
b2Vec2 GetAnchorA() const;\r
b2Vec2 GetAnchorB() const;\r
\r
+ /// Get the reaction force given the inverse time step.\r
+ /// Unit is N.\r
b2Vec2 GetReactionForce(float32 inv_dt) const;\r
+\r
+ /// Get the reaction torque given the inverse time step.\r
+ /// Unit is N*m. This is always zero for a distance joint.\r
float32 GetReactionTorque(float32 inv_dt) const;\r
\r
/// Set/get the natural length.\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
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007 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
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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/Dynamics/Joints/b2Joint.h>\r
#include <Box2D/Dynamics/Joints/b2DistanceJoint.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/b2RevoluteJoint.h>\r
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>\r
#include <Box2D/Dynamics/Joints/b2GearJoint.h>\r
#include <Box2D/Dynamics/Joints/b2WeldJoint.h>\r
#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>\r
+#include <Box2D/Dynamics/Joints/b2RopeJoint.h>\r
#include <Box2D/Dynamics/b2Body.h>\r
#include <Box2D/Dynamics/b2World.h>\r
#include <Box2D/Common/b2BlockAllocator.h>\r
}\r
break;\r
\r
- case e_lineJoint:\r
+ case e_wheelJoint:\r
{\r
- void* mem = allocator->Allocate(sizeof(b2LineJoint));\r
- joint = new (mem) b2LineJoint((b2LineJointDef*)def);\r
+ void* mem = allocator->Allocate(sizeof(b2WheelJoint));\r
+ joint = new (mem) b2WheelJoint((b2WheelJointDef*)def);\r
}\r
break;\r
\r
}\r
break;\r
\r
+ case e_ropeJoint:\r
+ {\r
+ void* mem = allocator->Allocate(sizeof(b2RopeJoint));\r
+ joint = new (mem) b2RopeJoint((b2RopeJointDef*)def);\r
+ }\r
+ break;\r
+\r
default:\r
b2Assert(false);\r
break;\r
allocator->Free(joint, sizeof(b2GearJoint));\r
break;\r
\r
- case e_lineJoint:\r
- allocator->Free(joint, sizeof(b2LineJoint));\r
+ case e_wheelJoint:\r
+ allocator->Free(joint, sizeof(b2WheelJoint));\r
break;\r
\r
case e_weldJoint:\r
allocator->Free(joint, sizeof(b2FrictionJoint));\r
break;\r
\r
+ case e_ropeJoint:\r
+ allocator->Free(joint, sizeof(b2RopeJoint));\r
+ break;\r
+\r
default:\r
b2Assert(false);\r
break;\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
e_pulleyJoint,\r
e_mouseJoint,\r
e_gearJoint,\r
- e_lineJoint,\r
+ e_wheelJoint,\r
e_weldJoint,\r
e_frictionJoint,\r
+ e_ropeJoint\r
};\r
\r
enum b2LimitState\r
\r
/// Get the next joint the world joint list.\r
b2Joint* GetNext();\r
+ const b2Joint* GetNext() const;\r
\r
/// Get the user data pointer.\r
void* GetUserData() const;\r
/// Short-cut function to determine if either body is inactive.\r
bool IsActive() const;\r
\r
+ /// Get collide connected.\r
+ /// Note: modifying the collide connect flag won't work correctly because\r
+ /// the flag is only checked when fixture AABBs begin to overlap.\r
+ bool GetCollideConnected() const;\r
+\r
protected:\r
friend class b2World;\r
friend class b2Body;\r
void* m_userData;\r
\r
// Cache here per time step to reduce cache misses.\r
+ // TODO_ERIN nuke\r
b2Vec2 m_localCenterA, m_localCenterB;\r
float32 m_invMassA, m_invIA;\r
float32 m_invMassB, m_invIB;\r
return m_next;\r
}\r
\r
+inline const b2Joint* b2Joint::GetNext() const\r
+{\r
+ return m_next;\r
+}\r
+\r
inline void* b2Joint::GetUserData() const\r
{\r
return m_userData;\r
m_userData = data;\r
}\r
\r
+inline bool b2Joint::GetCollideConnected() const\r
+{\r
+ return m_collideConnected;\r
+}\r
+\r
#endif\r
+++ /dev/null
-/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\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/Dynamics/Joints/b2LineJoint.h>\r
-#include <Box2D/Dynamics/b2Body.h>\r
-#include <Box2D/Dynamics/b2TimeStep.h>\r
-\r
-// Linear constraint (point-to-line)\r
-// d = p2 - p1 = x2 + r2 - x1 - r1\r
-// C = dot(perp, d)\r
-// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))\r
-// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)\r
-// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]\r
-//\r
-// K = J * invM * JT\r
-//\r
-// J = [-a -s1 a s2]\r
-// a = perp\r
-// s1 = cross(d + r1, a) = cross(p2 - x1, a)\r
-// s2 = cross(r2, a) = cross(p2 - x2, a)\r
-\r
-\r
-// Motor/Limit linear constraint\r
-// C = dot(ax1, d)\r
-// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)\r
-// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]\r
-\r
-// Block Solver\r
-// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even\r
-// when the mass has poor distribution (leading to large torques about the joint anchor points).\r
-//\r
-// The Jacobian has 3 rows:\r
-// J = [-uT -s1 uT s2] // linear\r
-// [-vT -a1 vT a2] // limit\r
-//\r
-// u = perp\r
-// v = axis\r
-// s1 = cross(d + r1, u), s2 = cross(r2, u)\r
-// a1 = cross(d + r1, v), a2 = cross(r2, v)\r
-\r
-// M * (v2 - v1) = JT * df\r
-// J * v2 = bias\r
-//\r
-// v2 = v1 + invM * JT * df\r
-// J * (v1 + invM * JT * df) = bias\r
-// K * df = bias - J * v1 = -Cdot\r
-// K = J * invM * JT\r
-// Cdot = J * v1 - bias\r
-//\r
-// Now solve for f2.\r
-// df = f2 - f1\r
-// K * (f2 - f1) = -Cdot\r
-// f2 = invK * (-Cdot) + f1\r
-//\r
-// Clamp accumulated limit impulse.\r
-// lower: f2(2) = max(f2(2), 0)\r
-// upper: f2(2) = min(f2(2), 0)\r
-//\r
-// Solve for correct f2(1)\r
-// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1\r
-// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2)\r
-// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1)\r
-// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)\r
-//\r
-// Now compute impulse to be applied:\r
-// df = f2 - f1\r
-\r
-void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)\r
-{\r
- bodyA = b1;\r
- bodyB = b2;\r
- localAnchorA = bodyA->GetLocalPoint(anchor);\r
- localAnchorB = bodyB->GetLocalPoint(anchor);\r
- localAxisA = bodyA->GetLocalVector(axis);\r
-}\r
-\r
-b2LineJoint::b2LineJoint(const b2LineJointDef* def)\r
-: b2Joint(def)\r
-{\r
- m_localAnchor1 = def->localAnchorA;\r
- m_localAnchor2 = def->localAnchorB;\r
- m_localXAxis1 = def->localAxisA;\r
- m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);\r
-\r
- m_impulse.SetZero();\r
- m_motorMass = 0.0;\r
- m_motorImpulse = 0.0f;\r
-\r
- m_lowerTranslation = def->lowerTranslation;\r
- m_upperTranslation = def->upperTranslation;\r
- m_maxMotorForce = def->maxMotorForce;\r
- m_motorSpeed = def->motorSpeed;\r
- m_enableLimit = def->enableLimit;\r
- m_enableMotor = def->enableMotor;\r
- m_limitState = e_inactiveLimit;\r
-\r
- m_axis.SetZero();\r
- m_perp.SetZero();\r
-}\r
-\r
-void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)\r
-{\r
- b2Body* b1 = m_bodyA;\r
- b2Body* b2 = m_bodyB;\r
-\r
- m_localCenterA = b1->GetLocalCenter();\r
- m_localCenterB = b2->GetLocalCenter();\r
-\r
- b2Transform xf1 = b1->GetTransform();\r
- b2Transform xf2 = b2->GetTransform();\r
-\r
- // Compute the effective masses.\r
- b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);\r
- b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);\r
- b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;\r
-\r
- m_invMassA = b1->m_invMass;\r
- m_invIA = b1->m_invI;\r
- m_invMassB = b2->m_invMass;\r
- m_invIB = b2->m_invI;\r
-\r
- // Compute motor Jacobian and effective mass.\r
- {\r
- m_axis = b2Mul(xf1.R, m_localXAxis1);\r
- m_a1 = b2Cross(d + r1, m_axis);\r
- m_a2 = b2Cross(r2, m_axis);\r
-\r
- m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;\r
- if (m_motorMass > b2_epsilon)\r
- {\r
- m_motorMass = 1.0f / m_motorMass;\r
- }\r
- else\r
- {\r
- m_motorMass = 0.0f;\r
- }\r
- }\r
-\r
- // Prismatic constraint.\r
- {\r
- m_perp = b2Mul(xf1.R, m_localYAxis1);\r
-\r
- m_s1 = b2Cross(d + r1, m_perp);\r
- m_s2 = b2Cross(r2, m_perp);\r
-\r
- float32 m1 = m_invMassA, m2 = m_invMassB;\r
- float32 i1 = m_invIA, i2 = m_invIB;\r
-\r
- float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;\r
- float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;\r
- float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;\r
-\r
- m_K.col1.Set(k11, k12);\r
- m_K.col2.Set(k12, k22);\r
- }\r
-\r
- // Compute motor and limit terms.\r
- if (m_enableLimit)\r
- {\r
- float32 jointTranslation = b2Dot(m_axis, d);\r
- if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)\r
- {\r
- m_limitState = e_equalLimits;\r
- }\r
- else if (jointTranslation <= m_lowerTranslation)\r
- {\r
- if (m_limitState != e_atLowerLimit)\r
- {\r
- m_limitState = e_atLowerLimit;\r
- m_impulse.y = 0.0f;\r
- }\r
- }\r
- else if (jointTranslation >= m_upperTranslation)\r
- {\r
- if (m_limitState != e_atUpperLimit)\r
- {\r
- m_limitState = e_atUpperLimit;\r
- m_impulse.y = 0.0f;\r
- }\r
- }\r
- else\r
- {\r
- m_limitState = e_inactiveLimit;\r
- m_impulse.y = 0.0f;\r
- }\r
- }\r
- else\r
- {\r
- m_limitState = e_inactiveLimit;\r
- }\r
-\r
- if (m_enableMotor == false)\r
- {\r
- m_motorImpulse = 0.0f;\r
- }\r
-\r
- if (step.warmStarting)\r
- {\r
- // Account for variable time step.\r
- m_impulse *= step.dtRatio;\r
- m_motorImpulse *= step.dtRatio;\r
-\r
- b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;\r
- float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;\r
- float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;\r
-\r
- b1->m_linearVelocity -= m_invMassA * P;\r
- b1->m_angularVelocity -= m_invIA * L1;\r
-\r
- b2->m_linearVelocity += m_invMassB * P;\r
- b2->m_angularVelocity += m_invIB * L2;\r
- }\r
- else\r
- {\r
- m_impulse.SetZero();\r
- m_motorImpulse = 0.0f;\r
- }\r
-}\r
-\r
-void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)\r
-{\r
- b2Body* b1 = m_bodyA;\r
- b2Body* b2 = m_bodyB;\r
-\r
- b2Vec2 v1 = b1->m_linearVelocity;\r
- float32 w1 = b1->m_angularVelocity;\r
- b2Vec2 v2 = b2->m_linearVelocity;\r
- float32 w2 = b2->m_angularVelocity;\r
-\r
- // Solve linear motor constraint.\r
- if (m_enableMotor && m_limitState != e_equalLimits)\r
- {\r
- float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;\r
- float32 impulse = m_motorMass * (m_motorSpeed - Cdot);\r
- float32 oldImpulse = m_motorImpulse;\r
- float32 maxImpulse = step.dt * m_maxMotorForce;\r
- m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);\r
- impulse = m_motorImpulse - oldImpulse;\r
-\r
- b2Vec2 P = impulse * m_axis;\r
- float32 L1 = impulse * m_a1;\r
- float32 L2 = impulse * m_a2;\r
-\r
- v1 -= m_invMassA * P;\r
- w1 -= m_invIA * L1;\r
-\r
- v2 += m_invMassB * P;\r
- w2 += m_invIB * L2;\r
- }\r
-\r
- float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;\r
-\r
- if (m_enableLimit && m_limitState != e_inactiveLimit)\r
- {\r
- // Solve prismatic and limit constraint in block form.\r
- float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;\r
- b2Vec2 Cdot(Cdot1, Cdot2);\r
-\r
- b2Vec2 f1 = m_impulse;\r
- b2Vec2 df = m_K.Solve(-Cdot);\r
- m_impulse += df;\r
-\r
- if (m_limitState == e_atLowerLimit)\r
- {\r
- m_impulse.y = b2Max(m_impulse.y, 0.0f);\r
- }\r
- else if (m_limitState == e_atUpperLimit)\r
- {\r
- m_impulse.y = b2Min(m_impulse.y, 0.0f);\r
- }\r
-\r
- // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)\r
- float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;\r
- float32 f2r;\r
- if (m_K.col1.x != 0.0f)\r
- {\r
- f2r = b / m_K.col1.x + f1.x;\r
- }\r
- else\r
- {\r
- f2r = f1.x; \r
- }\r
-\r
- m_impulse.x = f2r;\r
-\r
- df = m_impulse - f1;\r
-\r
- b2Vec2 P = df.x * m_perp + df.y * m_axis;\r
- float32 L1 = df.x * m_s1 + df.y * m_a1;\r
- float32 L2 = df.x * m_s2 + df.y * m_a2;\r
-\r
- v1 -= m_invMassA * P;\r
- w1 -= m_invIA * L1;\r
-\r
- v2 += m_invMassB * P;\r
- w2 += m_invIB * L2;\r
- }\r
- else\r
- {\r
- // Limit is inactive, just solve the prismatic constraint in block form.\r
- float32 df;\r
- if (m_K.col1.x != 0.0f)\r
- {\r
- df = - Cdot1 / m_K.col1.x;\r
- }\r
- else\r
- {\r
- df = 0.0f;\r
- }\r
- m_impulse.x += df;\r
-\r
- b2Vec2 P = df * m_perp;\r
- float32 L1 = df * m_s1;\r
- float32 L2 = df * m_s2;\r
-\r
- v1 -= m_invMassA * P;\r
- w1 -= m_invIA * L1;\r
-\r
- v2 += m_invMassB * P;\r
- w2 += m_invIB * L2;\r
- }\r
-\r
- b1->m_linearVelocity = v1;\r
- b1->m_angularVelocity = w1;\r
- b2->m_linearVelocity = v2;\r
- b2->m_angularVelocity = w2;\r
-}\r
-\r
-bool b2LineJoint::SolvePositionConstraints(float32 baumgarte)\r
-{\r
- B2_NOT_USED(baumgarte);\r
-\r
- b2Body* b1 = m_bodyA;\r
- b2Body* b2 = m_bodyB;\r
-\r
- b2Vec2 c1 = b1->m_sweep.c;\r
- float32 a1 = b1->m_sweep.a;\r
-\r
- b2Vec2 c2 = b2->m_sweep.c;\r
- float32 a2 = b2->m_sweep.a;\r
-\r
- // Solve linear limit constraint.\r
- float32 linearError = 0.0f, angularError = 0.0f;\r
- bool active = false;\r
- float32 C2 = 0.0f;\r
-\r
- b2Mat22 R1(a1), R2(a2);\r
-\r
- b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);\r
- b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);\r
- b2Vec2 d = c2 + r2 - c1 - r1;\r
-\r
- if (m_enableLimit)\r
- {\r
- m_axis = b2Mul(R1, m_localXAxis1);\r
-\r
- m_a1 = b2Cross(d + r1, m_axis);\r
- m_a2 = b2Cross(r2, m_axis);\r
-\r
- float32 translation = b2Dot(m_axis, d);\r
- if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)\r
- {\r
- // Prevent large angular corrections\r
- C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);\r
- linearError = b2Abs(translation);\r
- active = true;\r
- }\r
- else if (translation <= m_lowerTranslation)\r
- {\r
- // Prevent large linear corrections and allow some slop.\r
- C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);\r
- linearError = m_lowerTranslation - translation;\r
- active = true;\r
- }\r
- else if (translation >= m_upperTranslation)\r
- {\r
- // Prevent large linear corrections and allow some slop.\r
- C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);\r
- linearError = translation - m_upperTranslation;\r
- active = true;\r
- }\r
- }\r
-\r
- m_perp = b2Mul(R1, m_localYAxis1);\r
-\r
- m_s1 = b2Cross(d + r1, m_perp);\r
- m_s2 = b2Cross(r2, m_perp);\r
-\r
- b2Vec2 impulse;\r
- float32 C1;\r
- C1 = b2Dot(m_perp, d);\r
-\r
- linearError = b2Max(linearError, b2Abs(C1));\r
- angularError = 0.0f;\r
-\r
- if (active)\r
- {\r
- float32 m1 = m_invMassA, m2 = m_invMassB;\r
- float32 i1 = m_invIA, i2 = m_invIB;\r
-\r
- float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;\r
- float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;\r
- float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;\r
-\r
- m_K.col1.Set(k11, k12);\r
- m_K.col2.Set(k12, k22);\r
-\r
- b2Vec2 C;\r
- C.x = C1;\r
- C.y = C2;\r
-\r
- impulse = m_K.Solve(-C);\r
- }\r
- else\r
- {\r
- float32 m1 = m_invMassA, m2 = m_invMassB;\r
- float32 i1 = m_invIA, i2 = m_invIB;\r
-\r
- float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;\r
-\r
- float32 impulse1;\r
- if (k11 != 0.0f)\r
- {\r
- impulse1 = - C1 / k11;\r
- }\r
- else\r
- {\r
- impulse1 = 0.0f;\r
- }\r
-\r
- impulse.x = impulse1;\r
- impulse.y = 0.0f;\r
- }\r
-\r
- b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;\r
- float32 L1 = impulse.x * m_s1 + impulse.y * m_a1;\r
- float32 L2 = impulse.x * m_s2 + impulse.y * m_a2;\r
-\r
- c1 -= m_invMassA * P;\r
- a1 -= m_invIA * L1;\r
- c2 += m_invMassB * P;\r
- a2 += m_invIB * L2;\r
-\r
- // TODO_ERIN remove need for this.\r
- b1->m_sweep.c = c1;\r
- b1->m_sweep.a = a1;\r
- b2->m_sweep.c = c2;\r
- b2->m_sweep.a = a2;\r
- b1->SynchronizeTransform();\r
- b2->SynchronizeTransform();\r
-\r
- return linearError <= b2_linearSlop && angularError <= b2_angularSlop;\r
-}\r
-\r
-b2Vec2 b2LineJoint::GetAnchorA() const\r
-{\r
- return m_bodyA->GetWorldPoint(m_localAnchor1);\r
-}\r
-\r
-b2Vec2 b2LineJoint::GetAnchorB() const\r
-{\r
- return m_bodyB->GetWorldPoint(m_localAnchor2);\r
-}\r
-\r
-b2Vec2 b2LineJoint::GetReactionForce(float32 inv_dt) const\r
-{\r
- return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis);\r
-}\r
-\r
-float32 b2LineJoint::GetReactionTorque(float32 inv_dt) const\r
-{\r
- B2_NOT_USED(inv_dt);\r
- return 0.0f;\r
-}\r
-\r
-float32 b2LineJoint::GetJointTranslation() const\r
-{\r
- b2Body* b1 = m_bodyA;\r
- b2Body* b2 = m_bodyB;\r
-\r
- b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);\r
- b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);\r
- b2Vec2 d = p2 - p1;\r
- b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);\r
-\r
- float32 translation = b2Dot(d, axis);\r
- return translation;\r
-}\r
-\r
-float32 b2LineJoint::GetJointSpeed() const\r
-{\r
- b2Body* b1 = m_bodyA;\r
- b2Body* b2 = m_bodyB;\r
-\r
- b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());\r
- b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());\r
- b2Vec2 p1 = b1->m_sweep.c + r1;\r
- b2Vec2 p2 = b2->m_sweep.c + r2;\r
- b2Vec2 d = p2 - p1;\r
- b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);\r
-\r
- b2Vec2 v1 = b1->m_linearVelocity;\r
- b2Vec2 v2 = b2->m_linearVelocity;\r
- float32 w1 = b1->m_angularVelocity;\r
- float32 w2 = b2->m_angularVelocity;\r
-\r
- float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));\r
- return speed;\r
-}\r
-\r
-bool b2LineJoint::IsLimitEnabled() const\r
-{\r
- return m_enableLimit;\r
-}\r
-\r
-void b2LineJoint::EnableLimit(bool flag)\r
-{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_enableLimit = flag;\r
-}\r
-\r
-float32 b2LineJoint::GetLowerLimit() const\r
-{\r
- return m_lowerTranslation;\r
-}\r
-\r
-float32 b2LineJoint::GetUpperLimit() const\r
-{\r
- return m_upperTranslation;\r
-}\r
-\r
-void b2LineJoint::SetLimits(float32 lower, float32 upper)\r
-{\r
- b2Assert(lower <= upper);\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_lowerTranslation = lower;\r
- m_upperTranslation = upper;\r
-}\r
-\r
-bool b2LineJoint::IsMotorEnabled() const\r
-{\r
- return m_enableMotor;\r
-}\r
-\r
-void b2LineJoint::EnableMotor(bool flag)\r
-{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_enableMotor = flag;\r
-}\r
-\r
-void b2LineJoint::SetMotorSpeed(float32 speed)\r
-{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_motorSpeed = speed;\r
-}\r
-\r
-void b2LineJoint::SetMaxMotorForce(float32 force)\r
-{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_maxMotorForce = force;\r
-}\r
-\r
-float32 b2LineJoint::GetMotorForce() const\r
-{\r
- return m_motorImpulse;\r
-}\r
-\r
-\r
-\r
-\r
-\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
m_a2 = b2Cross(r2, m_axis);\r
\r
m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;\r
- if (m_motorMass > b2_epsilon)\r
+ if (m_motorMass > 0.0f)\r
{\r
m_motorMass = 1.0f / m_motorMass;\r
}\r
float32 k12 = i1 * m_s1 + i2 * m_s2;\r
float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;\r
float32 k22 = i1 + i2;\r
+ if (k22 == 0.0f)\r
+ {\r
+ k22 = 1.0f;\r
+ }\r
float32 k23 = i1 * m_a1 + i2 * m_a2;\r
float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;\r
\r
float32 k12 = i1 * m_s1 + i2 * m_s2;\r
float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;\r
float32 k22 = i1 + i2;\r
+ if (k22 == 0.0f)\r
+ {\r
+ k22 = 1.0f;\r
+ }\r
float32 k23 = i1 * m_a1 + i2 * m_a2;\r
float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;\r
\r
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;\r
float32 k12 = i1 * m_s1 + i2 * m_s2;\r
float32 k22 = i1 + i2;\r
+ if (k22 == 0.0f)\r
+ {\r
+ k22 = 1.0f;\r
+ }\r
\r
m_K.col1.Set(k11, k12, 0.0f);\r
m_K.col2.Set(k12, k22, 0.0f);\r
\r
void b2PrismaticJoint::EnableLimit(bool flag)\r
{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_enableLimit = flag;\r
+ if (flag != m_enableLimit)\r
+ {\r
+ m_bodyA->SetAwake(true);\r
+ m_bodyB->SetAwake(true);\r
+ m_enableLimit = flag;\r
+ m_impulse.z = 0.0f;\r
+ }\r
}\r
\r
float32 b2PrismaticJoint::GetLowerLimit() const\r
void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)\r
{\r
b2Assert(lower <= upper);\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_lowerTranslation = lower;\r
- m_upperTranslation = upper;\r
+ if (lower != m_lowerTranslation || upper != m_upperTranslation)\r
+ {\r
+ m_bodyA->SetAwake(true);\r
+ m_bodyB->SetAwake(true);\r
+ m_lowerTranslation = lower;\r
+ m_upperTranslation = upper;\r
+ m_impulse.z = 0.0f;\r
+ }\r
}\r
\r
bool b2PrismaticJoint::IsMotorEnabled() const\r
m_maxMotorForce = force;\r
}\r
\r
-float32 b2PrismaticJoint::GetMotorForce() const\r
+float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const\r
{\r
- return m_motorImpulse;\r
+ return inv_dt * m_motorImpulse;\r
}\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
/// Set the maximum motor force, usually in N.\r
void SetMaxMotorForce(float32 force);\r
\r
- /// Get the current motor force, usually in N.\r
- float32 GetMotorForce() const;\r
+ /// Get the current motor force given the inverse time step, usually in N.\r
+ float32 GetMotorForce(float32 inv_dt) const;\r
\r
protected:\r
friend class b2Joint;\r
/*\r
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2007 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
// length1 = norm(p1 - s1)\r
// length2 = norm(p2 - s2)\r
// C0 = (length1 + ratio * length2)_initial\r
-// C = C0 - (length1 + ratio * length2) >= 0\r
+// C = C0 - (length1 + ratio * length2)\r
// u1 = (p1 - s1) / norm(p1 - s1)\r
// u2 = (p2 - s2) / norm(p2 - s2)\r
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))\r
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]\r
// K = J * invM * JT\r
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)\r
-//\r
-// Limit:\r
-// C = maxLength - length\r
-// u = (p - s) / norm(p - s)\r
-// Cdot = -dot(u, v + cross(w, r))\r
-// K = invMass + invI * cross(r, u)^2\r
-// 0 <= impulse\r
\r
void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,\r
const b2Vec2& ga1, const b2Vec2& ga2,\r
ratio = r;\r
b2Assert(ratio > b2_epsilon);\r
float32 C = lengthA + ratio * lengthB;\r
- maxLengthA = C - ratio * b2_minPulleyLength;\r
- maxLengthB = (C - b2_minPulleyLength) / ratio;\r
}\r
\r
b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)\r
\r
m_constant = def->lengthA + m_ratio * def->lengthB;\r
\r
- m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength);\r
- m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio);\r
-\r
m_impulse = 0.0f;\r
- m_limitImpulse1 = 0.0f;\r
- m_limitImpulse2 = 0.0f;\r
}\r
\r
void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)\r
float32 length1 = m_u1.Length();\r
float32 length2 = m_u2.Length();\r
\r
- if (length1 > b2_linearSlop)\r
+ if (length1 > 10.0f * b2_linearSlop)\r
{\r
m_u1 *= 1.0f / length1;\r
}\r
m_u1.SetZero();\r
}\r
\r
- if (length2 > b2_linearSlop)\r
+ if (length2 > 10.0f * b2_linearSlop)\r
{\r
m_u2 *= 1.0f / length2;\r
}\r
m_u2.SetZero();\r
}\r
\r
- float32 C = m_constant - length1 - m_ratio * length2;\r
- if (C > 0.0f)\r
- {\r
- m_state = e_inactiveLimit;\r
- m_impulse = 0.0f;\r
- }\r
- else\r
- {\r
- m_state = e_atUpperLimit;\r
- }\r
-\r
- if (length1 < m_maxLength1)\r
- {\r
- m_limitState1 = e_inactiveLimit;\r
- m_limitImpulse1 = 0.0f;\r
- }\r
- else\r
- {\r
- m_limitState1 = e_atUpperLimit;\r
- }\r
-\r
- if (length2 < m_maxLength2)\r
- {\r
- m_limitState2 = e_inactiveLimit;\r
- m_limitImpulse2 = 0.0f;\r
- }\r
- else\r
- {\r
- m_limitState2 = e_atUpperLimit;\r
- }\r
-\r
// Compute effective mass.\r
float32 cr1u1 = b2Cross(r1, m_u1);\r
float32 cr2u2 = b2Cross(r2, m_u2);\r
\r
- m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;\r
- m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;\r
- m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;\r
- b2Assert(m_limitMass1 > b2_epsilon);\r
- b2Assert(m_limitMass2 > b2_epsilon);\r
- b2Assert(m_pulleyMass > b2_epsilon);\r
- m_limitMass1 = 1.0f / m_limitMass1;\r
- m_limitMass2 = 1.0f / m_limitMass2;\r
- m_pulleyMass = 1.0f / m_pulleyMass;\r
+ float32 m1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;\r
+ float32 m2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;\r
+\r
+ m_pulleyMass = m1 + m_ratio * m_ratio * m2;\r
+\r
+ if (m_pulleyMass > 0.0f)\r
+ {\r
+ m_pulleyMass = 1.0f / m_pulleyMass;\r
+ }\r
\r
if (step.warmStarting)\r
{\r
// Scale impulses to support variable time steps.\r
m_impulse *= step.dtRatio;\r
- m_limitImpulse1 *= step.dtRatio;\r
- m_limitImpulse2 *= step.dtRatio;\r
\r
// Warm starting.\r
- b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;\r
- b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;\r
+ b2Vec2 P1 = -(m_impulse) * m_u1;\r
+ b2Vec2 P2 = (-m_ratio * m_impulse) * m_u2;\r
b1->m_linearVelocity += b1->m_invMass * P1;\r
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);\r
b2->m_linearVelocity += b2->m_invMass * P2;\r
else\r
{\r
m_impulse = 0.0f;\r
- m_limitImpulse1 = 0.0f;\r
- m_limitImpulse2 = 0.0f;\r
}\r
}\r
\r
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());\r
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());\r
\r
- if (m_state == e_atUpperLimit)\r
{\r
b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);\r
b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);\r
\r
float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);\r
float32 impulse = m_pulleyMass * (-Cdot);\r
- float32 oldImpulse = m_impulse;\r
- m_impulse = b2Max(0.0f, m_impulse + impulse);\r
- impulse = m_impulse - oldImpulse;\r
+ m_impulse += impulse;\r
\r
b2Vec2 P1 = -impulse * m_u1;\r
b2Vec2 P2 = -m_ratio * impulse * m_u2;\r
b2->m_linearVelocity += b2->m_invMass * P2;\r
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);\r
}\r
-\r
- if (m_limitState1 == e_atUpperLimit)\r
- {\r
- b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);\r
-\r
- float32 Cdot = -b2Dot(m_u1, v1);\r
- float32 impulse = -m_limitMass1 * Cdot;\r
- float32 oldImpulse = m_limitImpulse1;\r
- m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse);\r
- impulse = m_limitImpulse1 - oldImpulse;\r
-\r
- b2Vec2 P1 = -impulse * m_u1;\r
- b1->m_linearVelocity += b1->m_invMass * P1;\r
- b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);\r
- }\r
-\r
- if (m_limitState2 == e_atUpperLimit)\r
- {\r
- b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);\r
-\r
- float32 Cdot = -b2Dot(m_u2, v2);\r
- float32 impulse = -m_limitMass2 * Cdot;\r
- float32 oldImpulse = m_limitImpulse2;\r
- m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse);\r
- impulse = m_limitImpulse2 - oldImpulse;\r
-\r
- b2Vec2 P2 = -impulse * m_u2;\r
- b2->m_linearVelocity += b2->m_invMass * P2;\r
- b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);\r
- }\r
}\r
\r
bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)\r
b2Vec2 s1 = m_groundAnchor1;\r
b2Vec2 s2 = m_groundAnchor2;\r
\r
- float32 linearError = 0.0f;\r
+ b2Vec2 r1 = b2Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());\r
+ b2Vec2 r2 = b2Mul(b2->m_xf.R, m_localAnchor2 - b2->GetLocalCenter());\r
\r
- if (m_state == e_atUpperLimit)\r
- {\r
- b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());\r
- b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());\r
-\r
- b2Vec2 p1 = b1->m_sweep.c + r1;\r
- b2Vec2 p2 = b2->m_sweep.c + r2;\r
-\r
- // Get the pulley axes.\r
- m_u1 = p1 - s1;\r
- m_u2 = p2 - s2;\r
-\r
- float32 length1 = m_u1.Length();\r
- float32 length2 = m_u2.Length();\r
-\r
- if (length1 > b2_linearSlop)\r
- {\r
- m_u1 *= 1.0f / length1;\r
- }\r
- else\r
- {\r
- m_u1.SetZero();\r
- }\r
-\r
- if (length2 > b2_linearSlop)\r
- {\r
- m_u2 *= 1.0f / length2;\r
- }\r
- else\r
- {\r
- m_u2.SetZero();\r
- }\r
-\r
- float32 C = m_constant - length1 - m_ratio * length2;\r
- linearError = b2Max(linearError, -C);\r
-\r
- C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);\r
- float32 impulse = -m_pulleyMass * C;\r
+ b2Vec2 p1 = b1->m_sweep.c + r1;\r
+ b2Vec2 p2 = b2->m_sweep.c + r2;\r
\r
- b2Vec2 P1 = -impulse * m_u1;\r
- b2Vec2 P2 = -m_ratio * impulse * m_u2;\r
+ // Get the pulley axes.\r
+ b2Vec2 u1 = p1 - s1;\r
+ b2Vec2 u2 = p2 - s2;\r
\r
- b1->m_sweep.c += b1->m_invMass * P1;\r
- b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);\r
- b2->m_sweep.c += b2->m_invMass * P2;\r
- b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);\r
+ float32 length1 = u1.Length();\r
+ float32 length2 = u2.Length();\r
\r
- b1->SynchronizeTransform();\r
- b2->SynchronizeTransform();\r
+ if (length1 > 10.0f * b2_linearSlop)\r
+ {\r
+ u1 *= 1.0f / length1;\r
+ }\r
+ else\r
+ {\r
+ u1.SetZero();\r
}\r
\r
- if (m_limitState1 == e_atUpperLimit)\r
+ if (length2 > 10.0f * b2_linearSlop)\r
+ {\r
+ u2 *= 1.0f / length2;\r
+ }\r
+ else\r
{\r
- b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());\r
- b2Vec2 p1 = b1->m_sweep.c + r1;\r
-\r
- m_u1 = p1 - s1;\r
- float32 length1 = m_u1.Length();\r
-\r
- if (length1 > b2_linearSlop)\r
- {\r
- m_u1 *= 1.0f / length1;\r
- }\r
- else\r
- {\r
- m_u1.SetZero();\r
- }\r
-\r
- float32 C = m_maxLength1 - length1;\r
- linearError = b2Max(linearError, -C);\r
- C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);\r
- float32 impulse = -m_limitMass1 * C;\r
+ u2.SetZero();\r
+ }\r
\r
- b2Vec2 P1 = -impulse * m_u1;\r
- b1->m_sweep.c += b1->m_invMass * P1;\r
- b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);\r
+ // Compute effective mass.\r
+ float32 cr1u1 = b2Cross(r1, u1);\r
+ float32 cr2u2 = b2Cross(r2, u2);\r
\r
- b1->SynchronizeTransform();\r
- }\r
+ float32 m1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;\r
+ float32 m2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;\r
+\r
+ float32 mass = m1 + m_ratio * m_ratio * m2;\r
\r
- if (m_limitState2 == e_atUpperLimit)\r
+ if (mass > 0.0f)\r
{\r
- b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());\r
- b2Vec2 p2 = b2->m_sweep.c + r2;\r
-\r
- m_u2 = p2 - s2;\r
- float32 length2 = m_u2.Length();\r
-\r
- if (length2 > b2_linearSlop)\r
- {\r
- m_u2 *= 1.0f / length2;\r
- }\r
- else\r
- {\r
- m_u2.SetZero();\r
- }\r
-\r
- float32 C = m_maxLength2 - length2;\r
- linearError = b2Max(linearError, -C);\r
- C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);\r
- float32 impulse = -m_limitMass2 * C;\r
-\r
- b2Vec2 P2 = -impulse * m_u2;\r
- b2->m_sweep.c += b2->m_invMass * P2;\r
- b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);\r
-\r
- b2->SynchronizeTransform();\r
+ mass = 1.0f / mass;\r
}\r
\r
+ float32 C = m_constant - length1 - m_ratio * length2;\r
+ float32 linearError = b2Abs(C);\r
+\r
+ float32 impulse = -mass * C;\r
+\r
+ b2Vec2 P1 = -impulse * u1;\r
+ b2Vec2 P2 = -m_ratio * impulse * u2;\r
+\r
+ b1->m_sweep.c += b1->m_invMass * P1;\r
+ b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);\r
+ b2->m_sweep.c += b2->m_invMass * P2;\r
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);\r
+\r
+ b1->SynchronizeTransform();\r
+ b2->SynchronizeTransform();\r
+\r
return linearError < b2_linearSlop;\r
}\r
\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
const float32 b2_minPulleyLength = 2.0f;\r
\r
/// Pulley joint definition. This requires two ground anchors,\r
-/// two dynamic body anchor points, max lengths for each side,\r
-/// and a pulley ratio.\r
+/// two dynamic body anchor points, and a pulley ratio.\r
struct b2PulleyJointDef : public b2JointDef\r
{\r
b2PulleyJointDef()\r
localAnchorA.Set(-1.0f, 0.0f);\r
localAnchorB.Set(1.0f, 0.0f);\r
lengthA = 0.0f;\r
- maxLengthA = 0.0f;\r
lengthB = 0.0f;\r
- maxLengthB = 0.0f;\r
ratio = 1.0f;\r
collideConnected = true;\r
}\r
/// The a reference length for the segment attached to bodyA.\r
float32 lengthA;\r
\r
- /// The maximum length of the segment attached to bodyA.\r
- float32 maxLengthA;\r
-\r
/// The a reference length for the segment attached to bodyB.\r
float32 lengthB;\r
\r
- /// The maximum length of the segment attached to bodyB.\r
- float32 maxLengthB;\r
-\r
/// The pulley ratio, used to simulate a block-and-tackle.\r
float32 ratio;\r
};\r
/// The pulley supports a ratio such that:\r
/// length1 + ratio * length2 <= constant\r
/// Yes, the force transmitted is scaled by the ratio.\r
-/// The pulley also enforces a maximum length limit on both sides. This is\r
-/// useful to prevent one side of the pulley hitting the top.\r
+/// Warning: the pulley joint can get a bit squirrelly by itself. They often\r
+/// work better when combined with prismatic joints. You should also cover the\r
+/// the anchor points with static shapes to prevent one side from going to\r
+/// zero length.\r
class b2PulleyJoint : public b2Joint\r
{\r
public:\r
float32 m_constant;\r
float32 m_ratio;\r
\r
- float32 m_maxLength1;\r
- float32 m_maxLength2;\r
-\r
// Effective masses\r
float32 m_pulleyMass;\r
- float32 m_limitMass1;\r
- float32 m_limitMass2;\r
\r
// Impulses for accumulation/warm starting.\r
float32 m_impulse;\r
- float32 m_limitImpulse1;\r
- float32 m_limitImpulse2;\r
-\r
- b2LimitState m_state;\r
- b2LimitState m_limitState1;\r
- b2LimitState m_limitState2;\r
};\r
\r
#endif\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
float32 newImpulse = m_impulse.z + impulse.z;\r
if (newImpulse < 0.0f)\r
{\r
- b2Vec2 reduced = m_mass.Solve22(-Cdot1);\r
+ b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.col3.x, m_mass.col3.y);\r
+ b2Vec2 reduced = m_mass.Solve22(rhs);\r
impulse.x = reduced.x;\r
impulse.y = reduced.y;\r
impulse.z = -m_impulse.z;\r
m_impulse.y += reduced.y;\r
m_impulse.z = 0.0f;\r
}\r
+ else\r
+ {\r
+ m_impulse += impulse;\r
+ }\r
}\r
else if (m_limitState == e_atUpperLimit)\r
{\r
float32 newImpulse = m_impulse.z + impulse.z;\r
if (newImpulse > 0.0f)\r
{\r
- b2Vec2 reduced = m_mass.Solve22(-Cdot1);\r
+ b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.col3.x, m_mass.col3.y);\r
+ b2Vec2 reduced = m_mass.Solve22(rhs);\r
impulse.x = reduced.x;\r
impulse.y = reduced.y;\r
impulse.z = -m_impulse.z;\r
m_impulse.y += reduced.y;\r
m_impulse.z = 0.0f;\r
}\r
+ else\r
+ {\r
+ m_impulse += impulse;\r
+ }\r
}\r
\r
b2Vec2 P(impulse.x, impulse.y);\r
m_enableMotor = flag;\r
}\r
\r
-float32 b2RevoluteJoint::GetMotorTorque() const\r
+float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const\r
{\r
- return m_motorImpulse;\r
+ return inv_dt * m_motorImpulse;\r
}\r
\r
void b2RevoluteJoint::SetMotorSpeed(float32 speed)\r
\r
void b2RevoluteJoint::EnableLimit(bool flag)\r
{\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_enableLimit = flag;\r
+ if (flag != m_enableLimit)\r
+ {\r
+ m_bodyA->SetAwake(true);\r
+ m_bodyB->SetAwake(true);\r
+ m_enableLimit = flag;\r
+ m_impulse.z = 0.0f;\r
+ }\r
}\r
\r
float32 b2RevoluteJoint::GetLowerLimit() const\r
void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)\r
{\r
b2Assert(lower <= upper);\r
- m_bodyA->SetAwake(true);\r
- m_bodyB->SetAwake(true);\r
- m_lowerAngle = lower;\r
- m_upperAngle = upper;\r
+ \r
+ if (lower != m_lowerAngle || upper != m_upperAngle)\r
+ {\r
+ m_bodyA->SetAwake(true);\r
+ m_bodyB->SetAwake(true);\r
+ m_impulse.z = 0.0f;\r
+ m_lowerAngle = lower;\r
+ m_upperAngle = upper;\r
+ }\r
}\r
/*\r
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com\r
+* Copyright (c) 2006-2007 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
b2Vec2 GetAnchorA() const;\r
b2Vec2 GetAnchorB() const;\r
\r
- b2Vec2 GetReactionForce(float32 inv_dt) const;\r
- float32 GetReactionTorque(float32 inv_dt) const;\r
-\r
/// Get the current joint angle in radians.\r
float32 GetJointAngle() const;\r
\r
/// Set the maximum motor torque, usually in N-m.\r
void SetMaxMotorTorque(float32 torque);\r
\r
- /// Get the current motor torque, usually in N-m.\r
- float32 GetMotorTorque() const;\r
+ /// Get the reaction force given the inverse time step.\r
+ /// Unit is N.\r
+ b2Vec2 GetReactionForce(float32 inv_dt) const;\r
+\r
+ /// Get the reaction torque due to the joint limit given the inverse time step.\r
+ /// Unit is N*m.\r
+ float32 GetReactionTorque(float32 inv_dt) const;\r
+\r
+ /// Get the current motor torque given the inverse time step.\r
+ /// Unit is N*m.\r
+ float32 GetMotorTorque(float32 inv_dt) const;\r
\r
protected:\r
\r
--- /dev/null
+/*\r
+* Copyright (c) 2007-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/Dynamics/Joints/b2RopeJoint.h>\r
+#include <Box2D/Dynamics/b2Body.h>\r
+#include <Box2D/Dynamics/b2TimeStep.h>\r
+\r
+\r
+// Limit:\r
+// C = norm(pB - pA) - L\r
+// u = (pB - pA) / norm(pB - pA)\r
+// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))\r
+// J = [-u -cross(rA, u) u cross(rB, u)]\r
+// K = J * invM * JT\r
+// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2\r
+\r
+b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)\r
+: b2Joint(def)\r
+{\r
+ m_localAnchorA = def->localAnchorA;\r
+ m_localAnchorB = def->localAnchorB;\r
+\r
+ m_maxLength = def->maxLength;\r
+\r
+ m_mass = 0.0f;\r
+ m_impulse = 0.0f;\r
+ m_state = e_inactiveLimit;\r
+ m_length = 0.0f;\r
+}\r
+\r
+void b2RopeJoint::InitVelocityConstraints(const b2TimeStep& step)\r
+{\r
+ b2Body* bA = m_bodyA;\r
+ b2Body* bB = m_bodyB;\r
+\r
+ m_rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());\r
+ m_rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());\r
+\r
+ // Rope axis\r
+ m_u = bB->m_sweep.c + m_rB - bA->m_sweep.c - m_rA;\r
+\r
+ m_length = m_u.Length();\r
+\r
+ float32 C = m_length - m_maxLength;\r
+ if (C > 0.0f)\r
+ {\r
+ m_state = e_atUpperLimit;\r
+ }\r
+ else\r
+ {\r
+ m_state = e_inactiveLimit;\r
+ }\r
+\r
+ if (m_length > b2_linearSlop)\r
+ {\r
+ m_u *= 1.0f / m_length;\r
+ }\r
+ else\r
+ {\r
+ m_u.SetZero();\r
+ m_mass = 0.0f;\r
+ m_impulse = 0.0f;\r
+ return;\r
+ }\r
+\r
+ // Compute effective mass.\r
+ float32 crA = b2Cross(m_rA, m_u);\r
+ float32 crB = b2Cross(m_rB, m_u);\r
+ float32 invMass = bA->m_invMass + bA->m_invI * crA * crA + bB->m_invMass + bB->m_invI * crB * crB;\r
+\r
+ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;\r
+\r
+ if (step.warmStarting)\r
+ {\r
+ // Scale the impulse to support a variable time step.\r
+ m_impulse *= step.dtRatio;\r
+\r
+ b2Vec2 P = m_impulse * m_u;\r
+ bA->m_linearVelocity -= bA->m_invMass * P;\r
+ bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P);\r
+ bB->m_linearVelocity += bB->m_invMass * P;\r
+ bB->m_angularVelocity += bB->m_invI * b2Cross(m_rB, P);\r
+ }\r
+ else\r
+ {\r
+ m_impulse = 0.0f;\r
+ }\r
+}\r
+\r
+void b2RopeJoint::SolveVelocityConstraints(const b2TimeStep& step)\r
+{\r
+ B2_NOT_USED(step);\r
+\r
+ b2Body* bA = m_bodyA;\r
+ b2Body* bB = m_bodyB;\r
+\r
+ // Cdot = dot(u, v + cross(w, r))\r
+ b2Vec2 vA = bA->m_linearVelocity + b2Cross(bA->m_angularVelocity, m_rA);\r
+ b2Vec2 vB = bB->m_linearVelocity + b2Cross(bB->m_angularVelocity, m_rB);\r
+ float32 C = m_length - m_maxLength;\r
+ float32 Cdot = b2Dot(m_u, vB - vA);\r
+\r
+ // Predictive constraint.\r
+ if (C < 0.0f)\r
+ {\r
+ Cdot += step.inv_dt * C;\r
+ }\r
+\r
+ float32 impulse = -m_mass * Cdot;\r
+ float32 oldImpulse = m_impulse;\r
+ m_impulse = b2Min(0.0f, m_impulse + impulse);\r
+ impulse = m_impulse - oldImpulse;\r
+\r
+ b2Vec2 P = impulse * m_u;\r
+ bA->m_linearVelocity -= bA->m_invMass * P;\r
+ bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P);\r
+ bB->m_linearVelocity += bB->m_invMass * P;\r
+ bB->m_angularVelocity += bB->m_invI * b2Cross(m_rB, P);\r
+}\r
+\r
+bool b2RopeJoint::SolvePositionConstraints(float32 baumgarte)\r
+{\r
+ B2_NOT_USED(baumgarte);\r
+\r
+ b2Body* bA = m_bodyA;\r
+ b2Body* bB = m_bodyB;\r
+\r
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());\r
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());\r
+\r
+ b2Vec2 u = bB->m_sweep.c + rB - bA->m_sweep.c - rA;\r
+\r
+ float32 length = u.Normalize();\r
+ float32 C = length - m_maxLength;\r
+\r
+ C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);\r
+\r
+ float32 impulse = -m_mass * C;\r
+ b2Vec2 P = impulse * u;\r
+\r
+ bA->m_sweep.c -= bA->m_invMass * P;\r
+ bA->m_sweep.a -= bA->m_invI * b2Cross(rA, P);\r
+ bB->m_sweep.c += bB->m_invMass * P;\r
+ bB->m_sweep.a += bB->m_invI * b2Cross(rB, P);\r
+\r
+ bA->SynchronizeTransform();\r
+ bB->SynchronizeTransform();\r
+\r
+ return length - m_maxLength < b2_linearSlop;\r
+}\r
+\r
+b2Vec2 b2RopeJoint::GetAnchorA() const\r
+{\r
+ return m_bodyA->GetWorldPoint(m_localAnchorA);\r
+}\r
+\r
+b2Vec2 b2RopeJoint::GetAnchorB() const\r
+{\r
+ return m_bodyB->GetWorldPoint(m_localAnchorB);\r
+}\r
+\r
+b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const\r
+{\r
+ b2Vec2 F = (inv_dt * m_impulse) * m_u;\r
+ return F;\r
+}\r
+\r
+float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const\r
+{\r
+ B2_NOT_USED(inv_dt);\r
+ return 0.0f;\r
+}\r
+\r
+float32 b2RopeJoint::GetMaxLength() const\r
+{\r
+ return m_maxLength;\r
+}\r
+\r
+b2LimitState b2RopeJoint::GetLimitState() const\r
+{\r
+ return m_state;\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_ROPE_JOINT_H\r
+#define B2_ROPE_JOINT_H\r
+\r
+#include <Box2D/Dynamics/Joints/b2Joint.h>\r
+\r
+/// Rope joint definition. This requires two body anchor points and\r
+/// a maximum lengths.\r
+/// Note: by default the connected objects will not collide.\r
+/// see collideConnected in b2JointDef.\r
+struct b2RopeJointDef : public b2JointDef\r
+{\r
+ b2RopeJointDef()\r
+ {\r
+ type = e_ropeJoint;\r
+ localAnchorA.Set(-1.0f, 0.0f);\r
+ localAnchorB.Set(1.0f, 0.0f);\r
+ maxLength = 0.0f;\r
+ }\r
+\r
+ /// The local anchor point relative to bodyA's origin.\r
+ b2Vec2 localAnchorA;\r
+\r
+ /// The local anchor point relative to bodyB's origin.\r
+ b2Vec2 localAnchorB;\r
+\r
+ /// The maximum length of the rope.\r
+ /// Warning: this must be larger than b2_linearSlop or\r
+ /// the joint will have no effect.\r
+ float32 maxLength;\r
+};\r
+\r
+/// A rope joint enforces a maximum distance between two points\r
+/// on two bodies. It has no other effect.\r
+/// Warning: if you attempt to change the maximum length during\r
+/// the simulation you will get some non-physical behavior.\r
+/// A model that would allow you to dynamically modify the length\r
+/// would have some sponginess, so I chose not to implement it\r
+/// that way. See b2DistanceJoint if you want to dynamically\r
+/// control length.\r
+class b2RopeJoint : public b2Joint\r
+{\r
+public:\r
+ b2Vec2 GetAnchorA() const;\r
+ b2Vec2 GetAnchorB() const;\r
+\r
+ b2Vec2 GetReactionForce(float32 inv_dt) const;\r
+ float32 GetReactionTorque(float32 inv_dt) const;\r
+\r
+ /// Get the maximum length of the rope.\r
+ float32 GetMaxLength() const;\r
+\r
+ b2LimitState GetLimitState() const;\r
+\r
+protected:\r
+\r
+ friend class b2Joint;\r
+ b2RopeJoint(const b2RopeJointDef* data);\r
+\r
+ void InitVelocityConstraints(const b2TimeStep& step);\r
+ void SolveVelocityConstraints(const b2TimeStep& step);\r
+ bool SolvePositionConstraints(float32 baumgarte);\r
+\r
+ b2Vec2 m_localAnchorA;\r
+ b2Vec2 m_localAnchorB;\r
+\r
+ float32 m_maxLength;\r
+ float32 m_length;\r
+\r
+ // Jacobian info\r
+ b2Vec2 m_u, m_rA, m_rB;\r
+\r
+ // Effective mass\r
+ float32 m_mass;\r
+\r
+ // Impulses for accumulation/warm starting.\r
+ float32 m_impulse;\r
+\r
+ b2LimitState m_state;\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
-* 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
--- /dev/null
+/*\r
+* Copyright (c) 2006-2007 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/Dynamics/Joints/b2WheelJoint.h>\r
+#include <Box2D/Dynamics/b2Body.h>\r
+#include <Box2D/Dynamics/b2TimeStep.h>\r
+\r
+// Linear constraint (point-to-line)\r
+// d = pB - pA = xB + rB - xA - rA\r
+// C = dot(ay, d)\r
+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))\r
+// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)\r
+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]\r
+\r
+// Spring linear constraint\r
+// C = dot(ax, d)\r
+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)\r
+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]\r
+\r
+// Motor rotational constraint\r
+// Cdot = wB - wA\r
+// J = [0 0 -1 0 0 1]\r
+\r
+void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)\r
+{\r
+ bodyA = bA;\r
+ bodyB = bB;\r
+ localAnchorA = bodyA->GetLocalPoint(ancho
projects / tanks-ios.git / commitdiff