--- /dev/null
+project(kglengine)
+add_subdirectory(Source)
+
--- /dev/null
+project(kglengine)
+
+# FIND_PACKAGE(GLUT REQUIRED)
+# FIND_PACKAGE(GLU REQUIRED)
+# FIND_PACKAGE(OpenGL REQUIRED)
+
+add_subdirectory(Source)
+
+
+
+# INCLUDE_DIRECTORIES(${GLUT_INCLUDE_DIR} ${GLU_INCLUDE_DIR})
+
--- /dev/null
+Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software
+in a product, an acknowledgment in the product documentation would be
+appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
--- /dev/null
+Box2D Version 2.0.1
+
+Welcome to Box2D!
+
+Box2D is a 2D physics engine for games.
+
+For help with Box2D, please visit http://www.box2d.org. There is a forum there where you may post your questions.
+
+The project files with this distribution work with Microsoft Visual C++ 2005 and above. You can download MSVC 2008 for free and it includes the necessary Platform SDK.
+
+To run the demos, set "Testbed" as your startup project and press F5. Some test bed commands are:
+- 'r' to reset the current test
+- SPACE to launch a bomb
+- arrow keys to pan
+- 'x' and 'z' to zoom in/out
+- use the mouse to click and drag objects
+
+Erin Catto
+http://www.box2d.org
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX2D_H
+#define BOX2D_H
+
+/**
+\mainpage Box2D API Documentation
+
+\section intro_sec Getting Started
+
+For tutorials please see http://www.box2d.org/manual.html
+
+For discussion please visit http://www.box2d.org/forum
+*/
+
+// These include files constitute the main Box2D API
+
+#include "Common/b2Settings.h"
+
+#include "Collision/Shapes/b2CircleShape.h"
+#include "Collision/Shapes/b2PolygonShape.h"
+#include "Collision/b2BroadPhase.h"
+#include "Dynamics/b2WorldCallbacks.h"
+#include "Dynamics/b2World.h"
+#include "Dynamics/b2Body.h"
+
+#include "Dynamics/Contacts/b2Contact.h"
+
+#include "Dynamics/Joints/b2DistanceJoint.h"
+#include "Dynamics/Joints/b2MouseJoint.h"
+#include "Dynamics/Joints/b2PrismaticJoint.h"
+#include "Dynamics/Joints/b2RevoluteJoint.h"
+#include "Dynamics/Joints/b2PulleyJoint.h"
+#include "Dynamics/Joints/b2GearJoint.h"
+
+#endif
\ No newline at end of file
--- /dev/null
+set(SOURCES
+./Common/b2StackAllocator.cpp
+./Common/b2BlockAllocator.cpp
+./Common/b2Settings.cpp
+./Common/b2Math.cpp
+./Collision/b2CollidePoly.cpp
+./Collision/b2PairManager.cpp
+./Collision/b2BroadPhase.cpp
+./Collision/b2CollideCircle.cpp
+./Collision/b2TimeOfImpact.cpp
+./Collision/b2Distance.cpp
+./Collision/Shapes/b2CircleShape.cpp
+./Collision/Shapes/b2Shape.cpp
+./Collision/Shapes/b2PolygonShape.cpp
+./Collision/b2Collision.cpp
+./Dynamics/Contacts/b2PolyContact.cpp
+./Dynamics/Contacts/b2CircleContact.cpp
+./Dynamics/Contacts/b2ContactSolver.cpp
+./Dynamics/Contacts/b2PolyAndCircleContact.cpp
+./Dynamics/Contacts/b2Contact.cpp
+./Dynamics/b2Body.cpp
+./Dynamics/Joints/b2Joint.cpp
+./Dynamics/Joints/b2DistanceJoint.cpp
+./Dynamics/Joints/b2RevoluteJoint.cpp
+./Dynamics/Joints/b2PulleyJoint.cpp
+./Dynamics/Joints/b2MouseJoint.cpp
+./Dynamics/Joints/b2PrismaticJoint.cpp
+./Dynamics/Joints/b2GearJoint.cpp
+./Dynamics/b2Island.cpp
+./Dynamics/b2ContactManager.cpp
+./Dynamics/b2World.cpp
+./Dynamics/b2WorldCallbacks.cpp
+)
+
+add_library( Box2D STATIC ${SOURCES} )
+
+if( CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64" )
+set_target_properties(Box2D PROPERTIES COMPILE_FLAGS "-fPIC")
+endif( CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64" )
+
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2CircleShape.h"
+
+b2CircleShape::b2CircleShape(const b2ShapeDef* def)
+: b2Shape(def)
+{
+ b2Assert(def->type == e_circleShape);
+ const b2CircleDef* circleDef = (const b2CircleDef*)def;
+
+ m_type = e_circleShape;
+ m_localPosition = circleDef->localPosition;
+ m_radius = circleDef->radius;
+}
+
+void b2CircleShape::UpdateSweepRadius(const b2Vec2& center)
+{
+ // Update the sweep radius (maximum radius) as measured from
+ // a local center point.
+ b2Vec2 d = m_localPosition - center;
+ m_sweepRadius = d.Length() + m_radius - b2_toiSlop;
+}
+
+bool b2CircleShape::TestPoint(const b2XForm& transform, const b2Vec2& p) const
+{
+ b2Vec2 center = transform.position + b2Mul(transform.R, m_localPosition);
+ b2Vec2 d = p - center;
+ return b2Dot(d, d) <= m_radius * m_radius;
+}
+
+// Collision Detection in Interactive 3D Environments by Gino van den Bergen
+// From Section 3.1.2
+// x = s + a * r
+// norm(x) = radius
+bool b2CircleShape::TestSegment(const b2XForm& transform,
+ float32* lambda,
+ b2Vec2* normal,
+ const b2Segment& segment,
+ float32 maxLambda) const
+{
+ b2Vec2 position = transform.position + b2Mul(transform.R, m_localPosition);
+ b2Vec2 s = segment.p1 - position;
+ float32 b = b2Dot(s, s) - m_radius * m_radius;
+
+ // Does the segment start inside the circle?
+ if (b < 0.0f)
+ {
+ return false;
+ }
+
+ // Solve quadratic equation.
+ b2Vec2 r = segment.p2 - segment.p1;
+ float32 c = b2Dot(s, r);
+ float32 rr = b2Dot(r, r);
+ float32 sigma = c * c - rr * b;
+
+ // Check for negative discriminant and short segment.
+ if (sigma < 0.0f || rr < B2_FLT_EPSILON)
+ {
+ return false;
+ }
+
+ // Find the point of intersection of the line with the circle.
+ float32 a = -(c + b2Sqrt(sigma));
+
+ // Is the intersection point on the segment?
+ if (0.0f <= a && a <= maxLambda * rr)
+ {
+ a /= rr;
+ *lambda = a;
+ *normal = s + a * r;
+ normal->Normalize();
+ return true;
+ }
+
+ return false;
+}
+
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2XForm& transform) const
+{
+ b2Vec2 p = transform.position + b2Mul(transform.R, m_localPosition);
+ aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
+ aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
+}
+
+void b2CircleShape::ComputeSweptAABB(b2AABB* aabb, const b2XForm& transform1, const b2XForm& transform2) const
+{
+ b2Vec2 p1 = transform1.position + b2Mul(transform1.R, m_localPosition);
+ b2Vec2 p2 = transform2.position + b2Mul(transform2.R, m_localPosition);
+ b2Vec2 lower = b2Min(p1, p2);
+ b2Vec2 upper = b2Max(p1, p2);
+
+ aabb->lowerBound.Set(lower.x - m_radius, lower.y - m_radius);
+ aabb->upperBound.Set(upper.x + m_radius, upper.y + m_radius);
+}
+
+void b2CircleShape::ComputeMass(b2MassData* massData) const
+{
+ massData->mass = m_density * b2_pi * m_radius * m_radius;
+ massData->center = m_localPosition;
+
+ // inertia about the local origin
+ massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_localPosition, m_localPosition));
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_CIRCLE_SHAPE_H
+#define B2_CIRCLE_SHAPE_H
+
+#include "b2Shape.h"
+
+/// This structure is used to build circle shapes.
+struct b2CircleDef : public b2ShapeDef
+{
+ b2CircleDef()
+ {
+ type = e_circleShape;
+ localPosition.SetZero();
+ radius = 1.0f;
+ }
+
+ b2Vec2 localPosition;
+ float32 radius;
+};
+
+/// A circle shape.
+class b2CircleShape : public b2Shape
+{
+public:
+ /// @see b2Shape::TestPoint
+ bool TestPoint(const b2XForm& transform, const b2Vec2& p) const;
+
+ /// @see b2Shape::TestSegment
+ bool TestSegment( const b2XForm& transform,
+ float32* lambda,
+ b2Vec2* normal,
+ const b2Segment& segment,
+ float32 maxLambda) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2XForm& transform) const;
+
+ /// @see b2Shape::ComputeSweptAABB
+ void ComputeSweptAABB( b2AABB* aabb,
+ const b2XForm& transform1,
+ const b2XForm& transform2) const;
+
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData) const;
+
+ /// Get the local position of this circle in its parent body.
+ const b2Vec2& GetLocalPosition() const;
+
+ /// Get the radius of this circle.
+ float32 GetRadius() const;
+
+private:
+
+ friend class b2Shape;
+
+ b2CircleShape(const b2ShapeDef* def);
+
+ void UpdateSweepRadius(const b2Vec2& center);
+
+ // Local position in parent body
+ b2Vec2 m_localPosition;
+ float32 m_radius;
+};
+
+inline const b2Vec2& b2CircleShape::GetLocalPosition() const
+{
+ return m_localPosition;
+}
+
+inline float32 b2CircleShape::GetRadius() const
+{
+ return m_radius;
+}
+
+#endif
--- /dev/null
+
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PolygonShape.h"
+
+void b2PolygonDef::SetAsBox(float32 hx, float32 hy)
+{
+ vertexCount = 4;
+ vertices[0].Set(-hx, -hy);
+ vertices[1].Set( hx, -hy);
+ vertices[2].Set( hx, hy);
+ vertices[3].Set(-hx, hy);
+}
+
+void b2PolygonDef::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+{
+ SetAsBox(hx, hy);
+ b2XForm xf;
+ xf.position = center;
+ xf.R.Set(angle);
+
+ for (int32 i = 0; i < vertexCount; ++i)
+ {
+ vertices[i] = b2Mul(xf, vertices[i]);
+ }
+}
+
+static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
+{
+ b2Assert(count >= 3);
+
+ b2Vec2 c; c.Set(0.0f, 0.0f);
+ float32 area = 0.0f;
+
+ // pRef is the reference point for forming triangles.
+ // It's location doesn't change the result (except for rounding error).
+ b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+ // This code would put the reference point inside the polygon.
+ for (int32 i = 0; i < count; ++i)
+ {
+ pRef += vs[i];
+ }
+ pRef *= 1.0f / count;
+#endif
+
+ const float32 inv3 = 1.0f / 3.0f;
+
+ for (int32 i = 0; i < count; ++i)
+ {
+ // Triangle vertices.
+ b2Vec2 p1 = pRef;
+ b2Vec2 p2 = vs[i];
+ b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
+
+ b2Vec2 e1 = p2 - p1;
+ b2Vec2 e2 = p3 - p1;
+
+ float32 D = b2Cross(e1, e2);
+
+ float32 triangleArea = 0.5f * D;
+ area += triangleArea;
+
+ // Area weighted centroid
+ c += triangleArea * inv3 * (p1 + p2 + p3);
+ }
+
+ // Centroid
+ b2Assert(area > B2_FLT_EPSILON);
+ c *= 1.0f / area;
+ return c;
+}
+
+// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
+static void ComputeOBB(b2OBB* obb, const b2Vec2* vs, int32 count)
+{
+ b2Assert(count <= b2_maxPolygonVertices);
+ b2Vec2 p[b2_maxPolygonVertices + 1];
+ for (int32 i = 0; i < count; ++i)
+ {
+ p[i] = vs[i];
+ }
+ p[count] = p[0];
+
+ float32 minArea = B2_FLT_MAX;
+
+ for (int32 i = 1; i <= count; ++i)
+ {
+ b2Vec2 root = p[i-1];
+ b2Vec2 ux = p[i] - root;
+ float32 length = ux.Normalize();
+ b2Assert(length > B2_FLT_EPSILON);
+ b2Vec2 uy(-ux.y, ux.x);
+ b2Vec2 lower(B2_FLT_MAX, B2_FLT_MAX);
+ b2Vec2 upper(-B2_FLT_MAX, -B2_FLT_MAX);
+
+ for (int32 j = 0; j < count; ++j)
+ {
+ b2Vec2 d = p[j] - root;
+ b2Vec2 r;
+ r.x = b2Dot(ux, d);
+ r.y = b2Dot(uy, d);
+ lower = b2Min(lower, r);
+ upper = b2Max(upper, r);
+ }
+
+ float32 area = (upper.x - lower.x) * (upper.y - lower.y);
+ if (area < 0.95f * minArea)
+ {
+ minArea = area;
+ obb->R.col1 = ux;
+ obb->R.col2 = uy;
+ b2Vec2 center = 0.5f * (lower + upper);
+ obb->center = root + b2Mul(obb->R, center);
+ obb->extents = 0.5f * (upper - lower);
+ }
+ }
+
+ b2Assert(minArea < B2_FLT_MAX);
+}
+
+b2PolygonShape::b2PolygonShape(const b2ShapeDef* def)
+ : b2Shape(def)
+{
+ b2Assert(def->type == e_polygonShape);
+ m_type = e_polygonShape;
+ const b2PolygonDef* poly = (const b2PolygonDef*)def;
+
+ // Get the vertices transformed into the body frame.
+ m_vertexCount = poly->vertexCount;
+ b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolygonVertices);
+
+ // Copy vertices.
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ m_vertices[i] = poly->vertices[i];
+ }
+
+ // Compute normals. Ensure the edges have non-zero length.
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ int32 i1 = i;
+ int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+ b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+ b2Assert(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON);
+ m_normals[i] = b2Cross(edge, 1.0f);
+ m_normals[i].Normalize();
+ }
+
+#ifdef _DEBUG
+ // Ensure the polygon is convex.
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ for (int32 j = 0; j < m_vertexCount; ++j)
+ {
+ // Don't check vertices on the current edge.
+ if (j == i || j == (i + 1) % m_vertexCount)
+ {
+ continue;
+ }
+
+ // Your polygon is non-convex (it has an indentation).
+ // Or your polygon is too skinny.
+ float32 s = b2Dot(m_normals[i], m_vertices[j] - m_vertices[i]);
+ b2Assert(s < -b2_linearSlop);
+ }
+ }
+
+ // Ensure the polygon is counter-clockwise.
+ for (int32 i = 1; i < m_vertexCount; ++i)
+ {
+ float32 cross = b2Cross(m_normals[i-1], m_normals[i]);
+
+ // Keep asinf happy.
+ cross = b2Clamp(cross, -1.0f, 1.0f);
+
+ // You have consecutive edges that are almost parallel on your polygon.
+ float32 angle = asinf(cross);
+ b2Assert(angle > b2_angularSlop);
+ }
+#endif
+
+ // Compute the polygon centroid.
+ m_centroid = ComputeCentroid(poly->vertices, poly->vertexCount);
+
+ // Compute the oriented bounding box.
+ ComputeOBB(&m_obb, m_vertices, m_vertexCount);
+
+ // Create core polygon shape by shifting edges inward.
+ // Also compute the min/max radius for CCD.
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ int32 i1 = i - 1 >= 0 ? i - 1 : m_vertexCount - 1;
+ int32 i2 = i;
+
+ b2Vec2 n1 = m_normals[i1];
+ b2Vec2 n2 = m_normals[i2];
+ b2Vec2 v = m_vertices[i] - m_centroid;;
+
+ b2Vec2 d;
+ d.x = b2Dot(n1, v) - b2_toiSlop;
+ d.y = b2Dot(n2, v) - b2_toiSlop;
+
+ // Shifting the edge inward by b2_toiSlop should
+ // not cause the plane to pass the centroid.
+
+ // Your shape has a radius/extent less than b2_toiSlop.
+ b2Assert(d.x >= 0.0f);
+ b2Assert(d.y >= 0.0f);
+ b2Mat22 A;
+ A.col1.x = n1.x; A.col2.x = n1.y;
+ A.col1.y = n2.x; A.col2.y = n2.y;
+ m_coreVertices[i] = A.Solve(d) + m_centroid;
+ }
+}
+
+void b2PolygonShape::UpdateSweepRadius(const b2Vec2& center)
+{
+ // Update the sweep radius (maximum radius) as measured from
+ // a local center point.
+ m_sweepRadius = 0.0f;
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ b2Vec2 d = m_coreVertices[i] - center;
+ m_sweepRadius = b2Max(m_sweepRadius, d.Length());
+ }
+}
+
+bool b2PolygonShape::TestPoint(const b2XForm& xf, const b2Vec2& p) const
+{
+ b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
+
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+ if (dot > 0.0f)
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool b2PolygonShape::TestSegment(
+ const b2XForm& xf,
+ float32* lambda,
+ b2Vec2* normal,
+ const b2Segment& segment,
+ float32 maxLambda) const
+{
+ float32 lower = 0.0f, upper = maxLambda;
+
+ b2Vec2 p1 = b2MulT(xf.R, segment.p1 - xf.position);
+ b2Vec2 p2 = b2MulT(xf.R, segment.p2 - xf.position);
+ b2Vec2 d = p2 - p1;
+ int32 index = -1;
+
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ // p = p1 + a * d
+ // dot(normal, p - v) = 0
+ // dot(normal, p1 - v) + a * dot(normal, d) = 0
+ float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+ float32 denominator = b2Dot(m_normals[i], d);
+
+ // Note: we want this predicate without division:
+ // lower < numerator / denominator, where denominator < 0
+ // Since denominator < 0, we have to flip the inequality:
+ // lower < numerator / denominator <==> denominator * lower > numerator.
+
+ if (denominator < 0.0f && numerator < lower * denominator)
+ {
+ // Increase lower.
+ // The segment enters this half-space.
+ lower = numerator / denominator;
+ index = i;
+ }
+ else if (denominator > 0.0f && numerator < upper * denominator)
+ {
+ // Decrease upper.
+ // The segment exits this half-space.
+ upper = numerator / denominator;
+ }
+
+ if (upper < lower)
+ {
+ return false;
+ }
+ }
+
+ b2Assert(0.0f <= lower && lower <= maxLambda);
+
+ if (index >= 0)
+ {
+ *lambda = lower;
+ *normal = b2Mul(xf.R, m_normals[index]);
+ return true;
+ }
+
+ return false;
+}
+
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2XForm& xf) const
+{
+ b2Mat22 R = b2Mul(xf.R, m_obb.R);
+ b2Mat22 absR = b2Abs(R);
+ b2Vec2 h = b2Mul(absR, m_obb.extents);
+ b2Vec2 position = xf.position + b2Mul(xf.R, m_obb.center);
+ aabb->lowerBound = position - h;
+ aabb->upperBound = position + h;
+}
+
+void b2PolygonShape::ComputeSweptAABB(b2AABB* aabb,
+ const b2XForm& transform1,
+ const b2XForm& transform2) const
+{
+ b2AABB aabb1, aabb2;
+ ComputeAABB(&aabb1, transform1);
+ ComputeAABB(&aabb2, transform2);
+ aabb->lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
+ aabb->upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
+}
+
+void b2PolygonShape::ComputeMass(b2MassData* massData) const
+{
+ // Polygon mass, centroid, and inertia.
+ // Let rho be the polygon density in mass per unit area.
+ // Then:
+ // mass = rho * int(dA)
+ // centroid.x = (1/mass) * rho * int(x * dA)
+ // centroid.y = (1/mass) * rho * int(y * dA)
+ // I = rho * int((x*x + y*y) * dA)
+ //
+ // We can compute these integrals by summing all the integrals
+ // for each triangle of the polygon. To evaluate the integral
+ // for a single triangle, we make a change of variables to
+ // the (u,v) coordinates of the triangle:
+ // x = x0 + e1x * u + e2x * v
+ // y = y0 + e1y * u + e2y * v
+ // where 0 <= u && 0 <= v && u + v <= 1.
+ //
+ // We integrate u from [0,1-v] and then v from [0,1].
+ // We also need to use the Jacobian of the transformation:
+ // D = cross(e1, e2)
+ //
+ // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
+ //
+ // The rest of the derivation is handled by computer algebra.
+
+ b2Assert(m_vertexCount >= 3);
+
+ b2Vec2 center; center.Set(0.0f, 0.0f);
+ float32 area = 0.0f;
+ float32 I = 0.0f;
+
+ // pRef is the reference point for forming triangles.
+ // It's location doesn't change the result (except for rounding error).
+ b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+ // This code would put the reference point inside the polygon.
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ pRef += m_vertices[i];
+ }
+ pRef *= 1.0f / count;
+#endif
+
+ const float32 k_inv3 = 1.0f / 3.0f;
+
+ for (int32 i = 0; i < m_vertexCount; ++i)
+ {
+ // Triangle vertices.
+ b2Vec2 p1 = pRef;
+ b2Vec2 p2 = m_vertices[i];
+ b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
+
+ b2Vec2 e1 = p2 - p1;
+ b2Vec2 e2 = p3 - p1;
+
+ float32 D = b2Cross(e1, e2);
+
+ float32 triangleArea = 0.5f * D;
+ area += triangleArea;
+
+ // Area weighted centroid
+ center += triangleArea * k_inv3 * (p1 + p2 + p3);
+
+ float32 px = p1.x, py = p1.y;
+ float32 ex1 = e1.x, ey1 = e1.y;
+ float32 ex2 = e2.x, ey2 = e2.y;
+
+ float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
+ float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
+
+ I += D * (intx2 + inty2);
+ }
+
+ // Total mass
+ massData->mass = m_density * area;
+
+ // Center of mass
+ b2Assert(area > B2_FLT_EPSILON);
+ center *= 1.0f / area;
+ massData->center = center;
+
+ // Inertia tensor relative to the local origin.
+ massData->I = m_density * I;
+}
+
+b2Vec2 b2PolygonShape::Centroid(const b2XForm& xf) const
+{
+ return b2Mul(xf, m_centroid);
+}
+
+b2Vec2 b2PolygonShape::Support(const b2XForm& xf, const b2Vec2& d) const
+{
+ b2Vec2 dLocal = b2MulT(xf.R, d);
+
+ int32 bestIndex = 0;
+ float32 bestValue = b2Dot(m_coreVertices[0], dLocal);
+ for (int32 i = 1; i < m_vertexCount; ++i)
+ {
+ float32 value = b2Dot(m_coreVertices[i], dLocal);
+ if (value > bestValue)
+ {
+ bestIndex = i;
+ bestValue = value;
+ }
+ }
+
+ return b2Mul(xf, m_coreVertices[bestIndex]);
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_POLYGON_SHAPE_H
+#define B2_POLYGON_SHAPE_H
+
+#include "b2Shape.h"
+
+/// Convex polygon. The vertices must be in CCW order for a right-handed
+/// coordinate system with the z-axis coming out of the screen.
+struct b2PolygonDef : public b2ShapeDef
+{
+ b2PolygonDef()
+ {
+ type = e_polygonShape;
+ vertexCount = 0;
+ }
+
+ /// Build vertices to represent an axis-aligned box.
+ /// @param hx the half-width.
+ /// @param hy the half-height.
+ void SetAsBox(float32 hx, float32 hy);
+
+ /// Build vertices to represent an oriented box.
+ /// @param hx the half-width.
+ /// @param hy the half-height.
+ /// @param center the center of the box in local coordinates.
+ /// @param angle the rotation of the box in local coordinates.
+ void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
+
+ /// The polygon vertices in local coordinates.
+ b2Vec2 vertices[b2_maxPolygonVertices];
+
+ /// The number of polygon vertices.
+ int32 vertexCount;
+};
+
+
+/// A convex polygon.
+class b2PolygonShape : public b2Shape
+{
+public:
+ /// @see b2Shape::TestPoint
+ bool TestPoint(const b2XForm& transform, const b2Vec2& p) const;
+
+ /// @see b2Shape::TestSegment
+ bool TestSegment( const b2XForm& transform,
+ float32* lambda,
+ b2Vec2* normal,
+ const b2Segment& segment,
+ float32 maxLambda) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2XForm& transform) const;
+
+ /// @see b2Shape::ComputeSweptAABB
+ void ComputeSweptAABB( b2AABB* aabb,
+ const b2XForm& transform1,
+ const b2XForm& transform2) const;
+
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData) const;
+
+ /// Get the oriented bounding box relative to the parent body.
+ const b2OBB& GetOBB() const;
+
+ /// Get local centroid relative to the parent body.
+ const b2Vec2& GetCentroid() const;
+
+ /// Get the vertex count.
+ int32 GetVertexCount() const;
+
+ /// Get the vertices in local coordinates.
+ const b2Vec2* GetVertices() const;
+
+ /// Get the core vertices in local coordinates. These vertices
+ /// represent a smaller polygon that is used for time of impact
+ /// computations.
+ const b2Vec2* GetCoreVertices() const;
+
+ /// Get the edge normal vectors. There is one for each vertex.
+ const b2Vec2* GetNormals() const;
+
+ /// Get the first vertex and apply the supplied transform.
+ b2Vec2 GetFirstVertex(const b2XForm& xf) const;
+
+ /// Get the centroid and apply the supplied transform.
+ b2Vec2 Centroid(const b2XForm& xf) const;
+
+ /// Get the support point in the given world direction.
+ /// Use the supplied transform.
+ b2Vec2 Support(const b2XForm& xf, const b2Vec2& d) const;
+
+private:
+
+ friend class b2Shape;
+
+ b2PolygonShape(const b2ShapeDef* def);
+
+ void UpdateSweepRadius(const b2Vec2& center);
+
+ // Local position of the polygon centroid.
+ b2Vec2 m_centroid;
+
+ b2OBB m_obb;
+
+ b2Vec2 m_vertices[b2_maxPolygonVertices];
+ b2Vec2 m_normals[b2_maxPolygonVertices];
+ b2Vec2 m_coreVertices[b2_maxPolygonVertices];
+ int32 m_vertexCount;
+};
+
+inline b2Vec2 b2PolygonShape::GetFirstVertex(const b2XForm& xf) const
+{
+ return b2Mul(xf, m_coreVertices[0]);
+}
+
+inline const b2OBB& b2PolygonShape::GetOBB() const
+{
+ return m_obb;
+}
+
+inline const b2Vec2& b2PolygonShape::GetCentroid() const
+{
+ return m_centroid;
+}
+
+inline int32 b2PolygonShape::GetVertexCount() const
+{
+ return m_vertexCount;
+}
+
+inline const b2Vec2* b2PolygonShape::GetVertices() const
+{
+ return m_vertices;
+}
+
+inline const b2Vec2* b2PolygonShape::GetCoreVertices() const
+{
+ return m_coreVertices;
+}
+
+inline const b2Vec2* b2PolygonShape::GetNormals() const
+{
+ return m_normals;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Shape.h"
+#include "b2CircleShape.h"
+#include "b2PolygonShape.h"
+#include "../b2Collision.h"
+#include "../b2BroadPhase.h"
+#include "../../Common/b2BlockAllocator.h"
+
+#include <new>
+
+b2Shape* b2Shape::Create(const b2ShapeDef* def, b2BlockAllocator* allocator)
+{
+ switch (def->type)
+ {
+ case e_circleShape:
+ {
+ void* mem = allocator->Allocate(sizeof(b2CircleShape));
+ return new (mem) b2CircleShape(def);
+ }
+
+ case e_polygonShape:
+ {
+ void* mem = allocator->Allocate(sizeof(b2PolygonShape));
+ return new (mem) b2PolygonShape(def);
+ }
+
+ default:
+ b2Assert(false);
+ return NULL;
+ }
+}
+
+void b2Shape::Destroy(b2Shape* s, b2BlockAllocator* allocator)
+{
+ switch (s->GetType())
+ {
+ case e_circleShape:
+ s->~b2Shape();
+ allocator->Free(s, sizeof(b2CircleShape));
+ break;
+
+ case e_polygonShape:
+ s->~b2Shape();
+ allocator->Free(s, sizeof(b2PolygonShape));
+ break;
+
+ default:
+ b2Assert(false);
+ }
+}
+
+b2Shape::b2Shape(const b2ShapeDef* def)
+{
+ m_userData = def->userData;
+ m_friction = def->friction;
+ m_restitution = def->restitution;
+ m_density = def->density;
+ m_body = NULL;
+ m_sweepRadius = 0.0f;
+
+ m_next = NULL;
+
+ m_proxyId = b2_nullProxy;
+
+ m_filter = def->filter;
+
+ m_isSensor = def->isSensor;
+}
+
+b2Shape::~b2Shape()
+{
+ b2Assert(m_proxyId == b2_nullProxy);
+}
+
+void b2Shape::CreateProxy(b2BroadPhase* broadPhase, const b2XForm& transform)
+{
+ b2Assert(m_proxyId == b2_nullProxy);
+
+ b2AABB aabb;
+ ComputeAABB(&aabb, transform);
+
+ bool inRange = broadPhase->InRange(aabb);
+
+ // You are creating a shape outside the world box.
+ b2Assert(inRange);
+
+ if (inRange)
+ {
+ m_proxyId = broadPhase->CreateProxy(aabb, this);
+ }
+ else
+ {
+ m_proxyId = b2_nullProxy;
+ }
+}
+
+void b2Shape::DestroyProxy(b2BroadPhase* broadPhase)
+{
+ if (m_proxyId != b2_nullProxy)
+ {
+ broadPhase->DestroyProxy(m_proxyId);
+ m_proxyId = b2_nullProxy;
+ }
+}
+
+bool b2Shape::Synchronize(b2BroadPhase* broadPhase, const b2XForm& transform1, const b2XForm& transform2)
+{
+ if (m_proxyId == b2_nullProxy)
+ {
+ return false;
+ }
+
+ // Compute an AABB that covers the swept shape (may miss some rotation effect).
+ b2AABB aabb;
+ ComputeSweptAABB(&aabb, transform1, transform2);
+
+ if (broadPhase->InRange(aabb))
+ {
+ broadPhase->MoveProxy(m_proxyId, aabb);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void b2Shape::RefilterProxy(b2BroadPhase* broadPhase, const b2XForm& transform)
+{
+ if (m_proxyId == b2_nullProxy)
+ {
+ return;
+ }
+
+ broadPhase->DestroyProxy(m_proxyId);
+
+ b2AABB aabb;
+ ComputeAABB(&aabb, transform);
+
+ bool inRange = broadPhase->InRange(aabb);
+
+ if (inRange)
+ {
+ m_proxyId = broadPhase->CreateProxy(aabb, this);
+ }
+ else
+ {
+ m_proxyId = b2_nullProxy;
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_SHAPE_H
+#define B2_SHAPE_H
+
+#include "../../Common/b2Math.h"
+#include "../b2Collision.h"
+
+class b2BlockAllocator;
+class b2Body;
+class b2BroadPhase;
+
+/// This holds the mass data computed for a shape.
+struct b2MassData
+{
+ /// The mass of the shape, usually in kilograms.
+ float32 mass;
+
+ /// The position of the shape's centroid relative to the shape's origin.
+ b2Vec2 center;
+
+ /// The rotational inertia of the shape.
+ float32 I;
+};
+
+/// This holds contact filtering data.
+struct b2FilterData
+{
+ /// The collision category bits. Normally you would just set one bit.
+ uint16 categoryBits;
+
+ /// The collision mask bits. This states the categories that this
+ /// shape would accept for collision.
+ uint16 maskBits;
+
+ /// Collision groups allow a certain group of objects to never collide (negative)
+ /// or always collide (positive). Zero means no collision group. Non-zero group
+ /// filtering always wins against the mask bits.
+ int16 groupIndex;
+};
+
+/// The various collision shape types supported by Box2D.
+enum b2ShapeType
+{
+ e_unknownShape = -1,
+ e_circleShape,
+ e_polygonShape,
+ e_shapeTypeCount,
+};
+
+/// A shape definition is used to construct a shape. This class defines an
+/// abstract shape definition. You can reuse shape definitions safely.
+struct b2ShapeDef
+{
+ /// The constructor sets the default shape definition values.
+ b2ShapeDef()
+ {
+ type = e_unknownShape;
+ userData = NULL;
+ friction = 0.2f;
+ restitution = 0.0f;
+ density = 0.0f;
+ filter.categoryBits = 0x0001;
+ filter.maskBits = 0xFFFF;
+ filter.groupIndex = 0;
+ isSensor = false;
+ }
+
+ virtual ~b2ShapeDef() {}
+
+ /// Holds the shape type for down-casting.
+ b2ShapeType type;
+
+ /// Use this to store application specify shape data.
+ void* userData;
+
+ /// The shape's friction coefficient, usually in the range [0,1].
+ float32 friction;
+
+ /// The shape's restitution (elasticity) usually in the range [0,1].
+ float32 restitution;
+
+ /// The shape's density, usually in kg/m^2.
+ float32 density;
+
+ /// A sensor shape collects contact information but never generates a collision
+ /// response.
+ bool isSensor;
+
+ /// Contact filtering data.
+ b2FilterData filter;
+};
+
+/// A shape is used for collision detection. Shapes are created in b2World.
+/// You can use shape for collision detection before they are attached to the world.
+/// @warning you cannot reuse shapes.
+class b2Shape
+{
+public:
+ /// Get the type of this shape. You can use this to down cast to the concrete shape.
+ /// @return the shape type.
+ b2ShapeType GetType() const;
+
+ /// Is this shape a sensor (non-solid)?
+ /// @return the true if the shape is a sensor.
+ bool IsSensor() const;
+
+ /// Set the contact filtering data. You must call b2World::Refilter to correct
+ /// existing contacts/non-contacts.
+ void SetFilterData(const b2FilterData& filter);
+
+ /// Get the contact filtering data.
+ const b2FilterData& GetFilterData() const;
+
+ /// Get the parent body of this shape. This is NULL if the shape is not attached.
+ /// @return the parent body.
+ b2Body* GetBody();
+
+ /// Get the next shape in the parent body's shape list.
+ /// @return the next shape.
+ b2Shape* GetNext();
+
+ /// Get the user data that was assigned in the shape definition. Use this to
+ /// store your application specific data.
+ void* GetUserData();
+
+ /// Set the user data. Use this to store your application specific data.
+ void SetUserData(void* data);
+
+ /// Test a point for containment in this shape. This only works for convex shapes.
+ /// @param xf the shape world transform.
+ /// @param p a point in world coordinates.
+ virtual bool TestPoint(const b2XForm& xf, const b2Vec2& p) const = 0;
+
+ /// Perform a ray cast against this shape.
+ /// @param xf the shape world transform.
+ /// @param lambda returns the hit fraction. You can use this to compute the contact point
+ /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
+ /// @param normal returns the normal at the contact point. If there is no intersection, the normal
+ /// is not set.
+ /// @param segment defines the begin and end point of the ray cast.
+ /// @param maxLambda a number typically in the range [0,1].
+ /// @return true if there was an intersection.
+ virtual bool TestSegment( const b2XForm& xf,
+ float32* lambda,
+ b2Vec2* normal,
+ const b2Segment& segment,
+ float32 maxLambda) const = 0;
+
+ /// Given a transform, compute the associated axis aligned bounding box for this shape.
+ /// @param aabb returns the axis aligned box.
+ /// @param xf the world transform of the shape.
+ virtual void ComputeAABB(b2AABB* aabb, const b2XForm& xf) const = 0;
+
+ /// Given two transforms, compute the associated swept axis aligned bounding box for this shape.
+ /// @param aabb returns the axis aligned box.
+ /// @param xf1 the starting shape world transform.
+ /// @param xf2 the ending shape world transform.
+ virtual void ComputeSweptAABB( b2AABB* aabb,
+ const b2XForm& xf1,
+ const b2XForm& xf2) const = 0;
+
+ /// Compute the mass properties of this shape using its dimensions and density.
+ /// The inertia tensor is computed about the local origin, not the centroid.
+ /// @param massData returns the mass data for this shape.
+ virtual void ComputeMass(b2MassData* massData) const = 0;
+
+ /// Get the maximum radius about the parent body's center of mass.
+ float32 GetSweepRadius() const;
+
+ /// Get the coefficient of friction.
+ float32 GetFriction() const;
+
+ /// Get the coefficient of restitution.
+ float32 GetRestitution() const;
+
+protected:
+
+ friend class b2Body;
+ friend class b2World;
+
+ static b2Shape* Create(const b2ShapeDef* def, b2BlockAllocator* allocator);
+ static void Destroy(b2Shape* shape, b2BlockAllocator* allocator);
+
+ b2Shape(const b2ShapeDef* def);
+ virtual ~b2Shape();
+
+ void CreateProxy(b2BroadPhase* broadPhase, const b2XForm& xf);
+ void DestroyProxy(b2BroadPhase* broadPhase);
+ bool Synchronize(b2BroadPhase* broadPhase, const b2XForm& xf1, const b2XForm& xf2);
+ void RefilterProxy(b2BroadPhase* broadPhase, const b2XForm& xf);
+
+ virtual void UpdateSweepRadius(const b2Vec2& center) = 0;
+
+ b2ShapeType m_type;
+ b2Shape* m_next;
+ b2Body* m_body;
+
+ // Sweep radius relative to the parent body's center of mass.
+ float32 m_sweepRadius;
+
+ float32 m_density;
+ float32 m_friction;
+ float32 m_restitution;
+
+ uint16 m_proxyId;
+ b2FilterData m_filter;
+
+ bool m_isSensor;
+
+ void* m_userData;
+};
+
+inline b2ShapeType b2Shape::GetType() const
+{
+ return m_type;
+}
+
+inline bool b2Shape::IsSensor() const
+{
+ return m_isSensor;
+}
+
+inline void b2Shape::SetFilterData(const b2FilterData& filter)
+{
+ m_filter = filter;
+}
+
+inline const b2FilterData& b2Shape::GetFilterData() const
+{
+ return m_filter;
+}
+
+inline void* b2Shape::GetUserData()
+{
+ return m_userData;
+}
+
+inline void b2Shape::SetUserData(void* data)
+{
+ m_userData = data;
+}
+
+inline b2Body* b2Shape::GetBody()
+{
+ return m_body;
+}
+
+inline b2Shape* b2Shape::GetNext()
+{
+ return m_next;
+}
+
+inline float32 b2Shape::GetSweepRadius() const
+{
+ return m_sweepRadius;
+}
+
+inline float32 b2Shape::GetFriction() const
+{
+ return m_friction;
+}
+
+inline float32 b2Shape::GetRestitution() const
+{
+ return m_restitution;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2BroadPhase.h"
+#include <algorithm>
+#include <string.h>
+
+// Notes:
+// - we use bound arrays instead of linked lists for cache coherence.
+// - we use quantized integral values for fast compares.
+// - we use short indices rather than pointers to save memory.
+// - we use a stabbing count for fast overlap queries (less than order N).
+// - we also use a time stamp on each proxy to speed up the registration of
+// overlap query results.
+// - where possible, we compare bound indices instead of values to reduce
+// cache misses (TODO_ERIN).
+// - no broadphase is perfect and neither is this one: it is not great for huge
+// worlds (use a multi-SAP instead), it is not great for large objects.
+
+bool b2BroadPhase::s_validate = false;
+
+struct b2BoundValues
+{
+ uint16 lowerValues[2];
+ uint16 upperValues[2];
+};
+
+static int32 BinarySearch(b2Bound* bounds, int32 count, uint16 value)
+{
+ int32 low = 0;
+ int32 high = count - 1;
+ while (low <= high)
+ {
+ int32 mid = (low + high) >> 1;
+ if (bounds[mid].value > value)
+ {
+ high = mid - 1;
+ }
+ else if (bounds[mid].value < value)
+ {
+ low = mid + 1;
+ }
+ else
+ {
+ return (uint16)mid;
+ }
+ }
+
+ return low;
+}
+
+b2BroadPhase::b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback)
+{
+ m_pairManager.Initialize(this, callback);
+
+ b2Assert(worldAABB.IsValid());
+ m_worldAABB = worldAABB;
+ m_proxyCount = 0;
+
+ b2Vec2 d = worldAABB.upperBound - worldAABB.lowerBound;
+ m_quantizationFactor.x = float32(B2BROADPHASE_MAX) / d.x;
+ m_quantizationFactor.y = float32(B2BROADPHASE_MAX) / d.y;
+
+ for (uint16 i = 0; i < b2_maxProxies - 1; ++i)
+ {
+ m_proxyPool[i].SetNext(i + 1);
+ m_proxyPool[i].timeStamp = 0;
+ m_proxyPool[i].overlapCount = b2_invalid;
+ m_proxyPool[i].userData = NULL;
+ }
+ m_proxyPool[b2_maxProxies-1].SetNext(b2_nullProxy);
+ m_proxyPool[b2_maxProxies-1].timeStamp = 0;
+ m_proxyPool[b2_maxProxies-1].overlapCount = b2_invalid;
+ m_proxyPool[b2_maxProxies-1].userData = NULL;
+ m_freeProxy = 0;
+
+ m_timeStamp = 1;
+ m_queryResultCount = 0;
+}
+
+b2BroadPhase::~b2BroadPhase()
+{
+}
+
+// This one is only used for validation.
+bool b2BroadPhase::TestOverlap(b2Proxy* p1, b2Proxy* p2)
+{
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+
+ b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
+ b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
+ b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
+ b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);
+
+ if (bounds[p1->lowerBounds[axis]].value > bounds[p2->upperBounds[axis]].value)
+ return false;
+
+ if (bounds[p1->upperBounds[axis]].value < bounds[p2->lowerBounds[axis]].value)
+ return false;
+ }
+
+ return true;
+}
+
+bool b2BroadPhase::TestOverlap(const b2BoundValues& b, b2Proxy* p)
+{
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+
+ b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
+ b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);
+
+ if (b.lowerValues[axis] > bounds[p->upperBounds[axis]].value)
+ return false;
+
+ if (b.upperValues[axis] < bounds[p->lowerBounds[axis]].value)
+ return false;
+ }
+
+ return true;
+}
+
+void b2BroadPhase::ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb)
+{
+ b2Assert(aabb.upperBound.x > aabb.lowerBound.x);
+ b2Assert(aabb.upperBound.y > aabb.lowerBound.y);
+
+ b2Vec2 minVertex = b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
+ b2Vec2 maxVertex = b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
+
+ // Bump lower bounds downs and upper bounds up. This ensures correct sorting of
+ // lower/upper bounds that would have equal values.
+ // TODO_ERIN implement fast float to uint16 conversion.
+ lowerValues[0] = (uint16)(m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x)) & (B2BROADPHASE_MAX - 1);
+ upperValues[0] = (uint16)(m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;
+
+ lowerValues[1] = (uint16)(m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y)) & (B2BROADPHASE_MAX - 1);
+ upperValues[1] = (uint16)(m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
+}
+
+void b2BroadPhase::IncrementTimeStamp()
+{
+ if (m_timeStamp == B2BROADPHASE_MAX)
+ {
+ for (uint16 i = 0; i < b2_maxProxies; ++i)
+ {
+ m_proxyPool[i].timeStamp = 0;
+ }
+ m_timeStamp = 1;
+ }
+ else
+ {
+ ++m_timeStamp;
+ }
+}
+
+void b2BroadPhase::IncrementOverlapCount(int32 proxyId)
+{
+ b2Proxy* proxy = m_proxyPool + proxyId;
+ if (proxy->timeStamp < m_timeStamp)
+ {
+ proxy->timeStamp = m_timeStamp;
+ proxy->overlapCount = 1;
+ }
+ else
+ {
+ proxy->overlapCount = 2;
+ b2Assert(m_queryResultCount < b2_maxProxies);
+ m_queryResults[m_queryResultCount] = (uint16)proxyId;
+ ++m_queryResultCount;
+ }
+}
+
+void b2BroadPhase::Query(int32* lowerQueryOut, int32* upperQueryOut,
+ uint16 lowerValue, uint16 upperValue,
+ b2Bound* bounds, int32 boundCount, int32 axis)
+{
+ int32 lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
+ int32 upperQuery = BinarySearch(bounds, boundCount, upperValue);
+
+ // Easy case: lowerQuery <= lowerIndex(i) < upperQuery
+ // Solution: search query range for min bounds.
+ for (int32 i = lowerQuery; i < upperQuery; ++i)
+ {
+ if (bounds[i].IsLower())
+ {
+ IncrementOverlapCount(bounds[i].proxyId);
+ }
+ }
+
+ // Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
+ // Solution: use the stabbing count to search down the bound array.
+ if (lowerQuery > 0)
+ {
+ int32 i = lowerQuery - 1;
+ int32 s = bounds[i].stabbingCount;
+
+ // Find the s overlaps.
+ while (s)
+ {
+ b2Assert(i >= 0);
+
+ if (bounds[i].IsLower())
+ {
+ b2Proxy* proxy = m_proxyPool + bounds[i].proxyId;
+ if (lowerQuery <= proxy->upperBounds[axis])
+ {
+ IncrementOverlapCount(bounds[i].proxyId);
+ --s;
+ }
+ }
+ --i;
+ }
+ }
+
+ *lowerQueryOut = lowerQuery;
+ *upperQueryOut = upperQuery;
+}
+
+uint16 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
+{
+ b2Assert(m_proxyCount < b2_maxProxies);
+ b2Assert(m_freeProxy != b2_nullProxy);
+
+ uint16 proxyId = m_freeProxy;
+ b2Proxy* proxy = m_proxyPool + proxyId;
+ m_freeProxy = proxy->GetNext();
+
+ proxy->overlapCount = 0;
+ proxy->userData = userData;
+
+ int32 boundCount = 2 * m_proxyCount;
+
+ uint16 lowerValues[2], upperValues[2];
+ ComputeBounds(lowerValues, upperValues, aabb);
+
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+ int32 lowerIndex, upperIndex;
+ Query(&lowerIndex, &upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
+
+ memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
+ memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
+
+ // The upper index has increased because of the lower bound insertion.
+ ++upperIndex;
+
+ // Copy in the new bounds.
+ bounds[lowerIndex].value = lowerValues[axis];
+ bounds[lowerIndex].proxyId = proxyId;
+ bounds[upperIndex].value = upperValues[axis];
+ bounds[upperIndex].proxyId = proxyId;
+
+ bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex-1].stabbingCount;
+ bounds[upperIndex].stabbingCount = bounds[upperIndex-1].stabbingCount;
+
+ // Adjust the stabbing count between the new bounds.
+ for (int32 index = lowerIndex; index < upperIndex; ++index)
+ {
+ ++bounds[index].stabbingCount;
+ }
+
+ // Adjust the all the affected bound indices.
+ for (int32 index = lowerIndex; index < boundCount + 2; ++index)
+ {
+ b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
+ if (bounds[index].IsLower())
+ {
+ proxy->lowerBounds[axis] = (uint16)index;
+ }
+ else
+ {
+ proxy->upperBounds[axis] = (uint16)index;
+ }
+ }
+ }
+
+ ++m_proxyCount;
+
+ b2Assert(m_queryResultCount < b2_maxProxies);
+
+ // Create pairs if the AABB is in range.
+ for (int32 i = 0; i < m_queryResultCount; ++i)
+ {
+ b2Assert(m_queryResults[i] < b2_maxProxies);
+ b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
+
+ m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
+ }
+
+ m_pairManager.Commit();
+
+ if (s_validate)
+ {
+ Validate();
+ }
+
+ // Prepare for next query.
+ m_queryResultCount = 0;
+ IncrementTimeStamp();
+
+ return proxyId;
+}
+
+void b2BroadPhase::DestroyProxy(int32 proxyId)
+{
+ b2Assert(0 < m_proxyCount && m_proxyCount <= b2_maxProxies);
+ b2Proxy* proxy = m_proxyPool + proxyId;
+ b2Assert(proxy->IsValid());
+
+ int32 boundCount = 2 * m_proxyCount;
+
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+
+ int32 lowerIndex = proxy->lowerBounds[axis];
+ int32 upperIndex = proxy->upperBounds[axis];
+ uint16 lowerValue = bounds[lowerIndex].value;
+ uint16 upperValue = bounds[upperIndex].value;
+
+ memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
+ memmove(bounds + upperIndex-1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
+
+ // Fix bound indices.
+ for (int32 index = lowerIndex; index < boundCount - 2; ++index)
+ {
+ b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
+ if (bounds[index].IsLower())
+ {
+ proxy->lowerBounds[axis] = (uint16)index;
+ }
+ else
+ {
+ proxy->upperBounds[axis] = (uint16)index;
+ }
+ }
+
+ // Fix stabbing count.
+ for (int32 index = lowerIndex; index < upperIndex - 1; ++index)
+ {
+ --bounds[index].stabbingCount;
+ }
+
+ // Query for pairs to be removed. lowerIndex and upperIndex are not needed.
+ Query(&lowerIndex, &upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
+ }
+
+ b2Assert(m_queryResultCount < b2_maxProxies);
+
+ for (int32 i = 0; i < m_queryResultCount; ++i)
+ {
+ b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
+ m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
+ }
+
+ m_pairManager.Commit();
+
+ // Prepare for next query.
+ m_queryResultCount = 0;
+ IncrementTimeStamp();
+
+ // Return the proxy to the pool.
+ proxy->userData = NULL;
+ proxy->overlapCount = b2_invalid;
+ proxy->lowerBounds[0] = b2_invalid;
+ proxy->lowerBounds[1] = b2_invalid;
+ proxy->upperBounds[0] = b2_invalid;
+ proxy->upperBounds[1] = b2_invalid;
+
+ proxy->SetNext(m_freeProxy);
+ m_freeProxy = (uint16)proxyId;
+ --m_proxyCount;
+
+ if (s_validate)
+ {
+ Validate();
+ }
+}
+
+void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb)
+{
+ if (proxyId == b2_nullProxy || b2_maxProxies <= proxyId)
+ {
+ b2Assert(false);
+ return;
+ }
+
+ if (aabb.IsValid() == false)
+ {
+ b2Assert(false);
+ return;
+ }
+
+ int32 boundCount = 2 * m_proxyCount;
+
+ b2Proxy* proxy = m_proxyPool + proxyId;
+
+ // Get new bound values
+ b2BoundValues newValues;
+ ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);
+
+ // Get old bound values
+ b2BoundValues oldValues;
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ oldValues.lowerValues[axis] = m_bounds[axis][proxy->lowerBounds[axis]].value;
+ oldValues.upperValues[axis] = m_bounds[axis][proxy->upperBounds[axis]].value;
+ }
+
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+
+ int32 lowerIndex = proxy->lowerBounds[axis];
+ int32 upperIndex = proxy->upperBounds[axis];
+
+ uint16 lowerValue = newValues.lowerValues[axis];
+ uint16 upperValue = newValues.upperValues[axis];
+
+ int32 deltaLower = lowerValue - bounds[lowerIndex].value;
+ int32 deltaUpper = upperValue - bounds[upperIndex].value;
+
+ bounds[lowerIndex].value = lowerValue;
+ bounds[upperIndex].value = upperValue;
+
+ //
+ // Expanding adds overlaps
+ //
+
+ // Should we move the lower bound down?
+ if (deltaLower < 0)
+ {
+ int32 index = lowerIndex;
+ while (index > 0 && lowerValue < bounds[index-1].value)
+ {
+ b2Bound* bound = bounds + index;
+ b2Bound* prevBound = bound - 1;
+
+ int32 prevProxyId = prevBound->proxyId;
+ b2Proxy* prevProxy = m_proxyPool + prevBound->proxyId;
+
+ ++prevBound->stabbingCount;
+
+ if (prevBound->IsUpper() == true)
+ {
+ if (TestOverlap(newValues, prevProxy))
+ {
+ m_pairManager.AddBufferedPair(proxyId, prevProxyId);
+ }
+
+ ++prevProxy->upperBounds[axis];
+ ++bound->stabbingCount;
+ }
+ else
+ {
+ ++prevProxy->lowerBounds[axis];
+ --bound->stabbingCount;
+ }
+
+ --proxy->lowerBounds[axis];
+ b2Swap(*bound, *prevBound);
+ --index;
+ }
+ }
+
+ // Should we move the upper bound up?
+ if (deltaUpper > 0)
+ {
+ int32 index = upperIndex;
+ while (index < boundCount-1 && bounds[index+1].value <= upperValue)
+ {
+ b2Bound* bound = bounds + index;
+ b2Bound* nextBound = bound + 1;
+ int32 nextProxyId = nextBound->proxyId;
+ b2Proxy* nextProxy = m_proxyPool + nextProxyId;
+
+ ++nextBound->stabbingCount;
+
+ if (nextBound->IsLower() == true)
+ {
+ if (TestOverlap(newValues, nextProxy))
+ {
+ m_pairManager.AddBufferedPair(proxyId, nextProxyId);
+ }
+
+ --nextProxy->lowerBounds[axis];
+ ++bound->stabbingCount;
+ }
+ else
+ {
+ --nextProxy->upperBounds[axis];
+ --bound->stabbingCount;
+ }
+
+ ++proxy->upperBounds[axis];
+ b2Swap(*bound, *nextBound);
+ ++index;
+ }
+ }
+
+ //
+ // Shrinking removes overlaps
+ //
+
+ // Should we move the lower bound up?
+ if (deltaLower > 0)
+ {
+ int32 index = lowerIndex;
+ while (index < boundCount-1 && bounds[index+1].value <= lowerValue)
+ {
+ b2Bound* bound = bounds + index;
+ b2Bound* nextBound = bound + 1;
+
+ int32 nextProxyId = nextBound->proxyId;
+ b2Proxy* nextProxy = m_proxyPool + nextProxyId;
+
+ --nextBound->stabbingCount;
+
+ if (nextBound->IsUpper())
+ {
+ if (TestOverlap(oldValues, nextProxy))
+ {
+ m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
+ }
+
+ --nextProxy->upperBounds[axis];
+ --bound->stabbingCount;
+ }
+ else
+ {
+ --nextProxy->lowerBounds[axis];
+ ++bound->stabbingCount;
+ }
+
+ ++proxy->lowerBounds[axis];
+ b2Swap(*bound, *nextBound);
+ ++index;
+ }
+ }
+
+ // Should we move the upper bound down?
+ if (deltaUpper < 0)
+ {
+ int32 index = upperIndex;
+ while (index > 0 && upperValue < bounds[index-1].value)
+ {
+ b2Bound* bound = bounds + index;
+ b2Bound* prevBound = bound - 1;
+
+ int32 prevProxyId = prevBound->proxyId;
+ b2Proxy* prevProxy = m_proxyPool + prevProxyId;
+
+ --prevBound->stabbingCount;
+
+ if (prevBound->IsLower() == true)
+ {
+ if (TestOverlap(oldValues, prevProxy))
+ {
+ m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
+ }
+
+ ++prevProxy->lowerBounds[axis];
+ --bound->stabbingCount;
+ }
+ else
+ {
+ ++prevProxy->upperBounds[axis];
+ ++bound->stabbingCount;
+ }
+
+ --proxy->upperBounds[axis];
+ b2Swap(*bound, *prevBound);
+ --index;
+ }
+ }
+ }
+
+ if (s_validate)
+ {
+ Validate();
+ }
+}
+
+void b2BroadPhase::Commit()
+{
+ m_pairManager.Commit();
+}
+
+int32 b2BroadPhase::Query(const b2AABB& aabb, void** userData, int32 maxCount)
+{
+ uint16 lowerValues[2];
+ uint16 upperValues[2];
+ ComputeBounds(lowerValues, upperValues, aabb);
+
+ int32 lowerIndex, upperIndex;
+
+ Query(&lowerIndex, &upperIndex, lowerValues[0], upperValues[0], m_bounds[0], 2*m_proxyCount, 0);
+ Query(&lowerIndex, &upperIndex, lowerValues[1], upperValues[1], m_bounds[1], 2*m_proxyCount, 1);
+
+ b2Assert(m_queryResultCount < b2_maxProxies);
+
+ int32 count = 0;
+ for (int32 i = 0; i < m_queryResultCount && count < maxCount; ++i, ++count)
+ {
+ b2Assert(m_queryResults[i] < b2_maxProxies);
+ b2Proxy* proxy = m_proxyPool + m_queryResults[i];
+ b2Assert(proxy->IsValid());
+ userData[i] = proxy->userData;
+ }
+
+ // Prepare for next query.
+ m_queryResultCount = 0;
+ IncrementTimeStamp();
+
+ return count;
+}
+
+void b2BroadPhase::Validate()
+{
+ for (int32 axis = 0; axis < 2; ++axis)
+ {
+ b2Bound* bounds = m_bounds[axis];
+
+ int32 boundCount = 2 * m_proxyCount;
+ uint16 stabbingCount = 0;
+
+ for (int32 i = 0; i < boundCount; ++i)
+ {
+ b2Bound* bound = bounds + i;
+ b2Assert(i == 0 || bounds[i-1].value <= bound->value);
+ b2Assert(bound->proxyId != b2_nullProxy);
+ b2Assert(m_proxyPool[bound->proxyId].IsValid());
+
+ if (bound->IsLower() == true)
+ {
+ b2Assert(m_proxyPool[bound->proxyId].lowerBounds[axis] == i);
+ ++stabbingCount;
+ }
+ else
+ {
+ b2Assert(m_proxyPool[bound->proxyId].upperBounds[axis] == i);
+ --stabbingCount;
+ }
+
+ b2Assert(bound->stabbingCount == stabbingCount);
+ }
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_BROAD_PHASE_H
+#define B2_BROAD_PHASE_H
+
+/*
+This broad phase uses the Sweep and Prune algorithm as described in:
+Collision Detection in Interactive 3D Environments by Gino van den Bergen
+Also, some ideas, such as using integral values for fast compares comes from
+Bullet (http:/www.bulletphysics.com).
+*/
+
+#include "../Common/b2Settings.h"
+#include "b2Collision.h"
+#include "b2PairManager.h"
+#include <climits>
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+#define B2BROADPHASE_MAX (USHRT_MAX/2)
+#else
+#define B2BROADPHASE_MAX USHRT_MAX
+
+#endif
+
+const uint16 b2_invalid = B2BROADPHASE_MAX;
+const uint16 b2_nullEdge = B2BROADPHASE_MAX;
+struct b2BoundValues;
+
+struct b2Bound
+{
+ bool IsLower() const { return (value & 1) == 0; }
+ bool IsUpper() const { return (value & 1) == 1; }
+
+ uint16 value;
+ uint16 proxyId;
+ uint16 stabbingCount;
+};
+
+struct b2Proxy
+{
+ uint16 GetNext() const { return lowerBounds[0]; }
+ void SetNext(uint16 next) { lowerBounds[0] = next; }
+ bool IsValid() const { return overlapCount != b2_invalid; }
+
+ uint16 lowerBounds[2], upperBounds[2];
+ uint16 overlapCount;
+ uint16 timeStamp;
+ void* userData;
+};
+
+class b2BroadPhase
+{
+public:
+ b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback);
+ ~b2BroadPhase();
+
+ // Use this to see if your proxy is in range. If it is not in range,
+ // it should be destroyed. Otherwise you may get O(m^2) pairs, where m
+ // is the number of proxies that are out of range.
+ bool InRange(const b2AABB& aabb) const;
+
+ // Create and destroy proxies. These call Flush first.
+ uint16 CreateProxy(const b2AABB& aabb, void* userData);
+ void DestroyProxy(int32 proxyId);
+
+ // Call MoveProxy as many times as you like, then when you are done
+ // call Commit to finalized the proxy pairs (for your time step).
+ void MoveProxy(int32 proxyId, const b2AABB& aabb);
+ void Commit();
+
+ // Get a single proxy. Returns NULL if the id is invalid.
+ b2Proxy* GetProxy(int32 proxyId);
+
+ // Query an AABB for overlapping proxies, returns the user data and
+ // the count, up to the supplied maximum count.
+ int32 Query(const b2AABB& aabb, void** userData, int32 maxCount);
+
+ void Validate();
+ void ValidatePairs();
+
+private:
+ void ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb);
+
+ bool TestOverlap(b2Proxy* p1, b2Proxy* p2);
+ bool TestOverlap(const b2BoundValues& b, b2Proxy* p);
+
+ void Query(int32* lowerIndex, int32* upperIndex, uint16 lowerValue, uint16 upperValue,
+ b2Bound* bounds, int32 boundCount, int32 axis);
+ void IncrementOverlapCount(int32 proxyId);
+ void IncrementTimeStamp();
+
+public:
+ friend class b2PairManager;
+
+ b2PairManager m_pairManager;
+
+ b2Proxy m_proxyPool[b2_maxProxies];
+ uint16 m_freeProxy;
+
+ b2Bound m_bounds[2][2*b2_maxProxies];
+
+ uint16 m_queryResults[b2_maxProxies];
+ int32 m_queryResultCount;
+
+ b2AABB m_worldAABB;
+ b2Vec2 m_quantizationFactor;
+ int32 m_proxyCount;
+ uint16 m_timeStamp;
+
+ static bool s_validate;
+};
+
+
+inline bool b2BroadPhase::InRange(const b2AABB& aabb) const
+{
+ b2Vec2 d = b2Max(aabb.lowerBound - m_worldAABB.upperBound, m_worldAABB.lowerBound - aabb.upperBound);
+ return b2Max(d.x, d.y) < 0.0f;
+}
+
+inline b2Proxy* b2BroadPhase::GetProxy(int32 proxyId)
+{
+ if (proxyId == b2_nullProxy || m_proxyPool[proxyId].IsValid() == false)
+ {
+ return NULL;
+ }
+
+ return m_proxyPool + proxyId;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+#include "Shapes/b2CircleShape.h"
+#include "Shapes/b2PolygonShape.h"
+
+void b2CollideCircles(
+ b2Manifold* manifold,
+ const b2CircleShape* circle1, const b2XForm& xf1,
+ const b2CircleShape* circle2, const b2XForm& xf2)
+{
+ manifold->pointCount = 0;
+
+ b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
+ b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
+
+ b2Vec2 d = p2 - p1;
+ float32 distSqr = b2Dot(d, d);
+ float32 r1 = circle1->GetRadius();
+ float32 r2 = circle2->GetRadius();
+ float32 radiusSum = r1 + r2;
+ if (distSqr > radiusSum * radiusSum)
+ {
+ return;
+ }
+
+ float32 separation;
+ if (distSqr < B2_FLT_EPSILON)
+ {
+ separation = -radiusSum;
+ manifold->normal.Set(0.0f, 1.0f);
+ }
+ else
+ {
+ float32 dist = b2Sqrt(distSqr);
+ separation = dist - radiusSum;
+ float32 a = 1.0f / dist;
+ manifold->normal.x = a * d.x;
+ manifold->normal.y = a * d.y;
+ }
+
+ manifold->pointCount = 1;
+ manifold->points[0].id.key = 0;
+ manifold->points[0].separation = separation;
+
+ p1 += r1 * manifold->normal;
+ p2 -= r2 * manifold->normal;
+
+ b2Vec2 p = 0.5f * (p1 + p2);
+
+ manifold->points[0].localPoint1 = b2MulT(xf1, p);
+ manifold->points[0].localPoint2 = b2MulT(xf2, p);
+}
+
+void b2CollidePolygonAndCircle(
+ b2Manifold* manifold,
+ const b2PolygonShape* polygon, const b2XForm& xf1,
+ const b2CircleShape* circle, const b2XForm& xf2)
+{
+ manifold->pointCount = 0;
+
+ // Compute circle position in the frame of the polygon.
+ b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
+ b2Vec2 cLocal = b2MulT(xf1, c);
+
+ // Find the min separating edge.
+ int32 normalIndex = 0;
+ float32 separation = -B2_FLT_MAX;
+ float32 radius = circle->GetRadius();
+ int32 vertexCount = polygon->GetVertexCount();
+ const b2Vec2* vertices = polygon->GetVertices();
+ const b2Vec2* normals = polygon->GetNormals();
+
+ for (int32 i = 0; i < vertexCount; ++i)
+ {
+ float32 s = b2Dot(normals[i], cLocal - vertices[i]);
+
+ if (s > radius)
+ {
+ // Early out.
+ return;
+ }
+
+ if (s > separation)
+ {
+ separation = s;
+ normalIndex = i;
+ }
+ }
+
+ // If the center is inside the polygon ...
+ if (separation < B2_FLT_EPSILON)
+ {
+ manifold->pointCount = 1;
+ manifold->normal = b2Mul(xf1.R, normals[normalIndex]);
+ manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
+ manifold->points[0].id.features.incidentVertex = b2_nullFeature;
+ manifold->points[0].id.features.referenceEdge = 0;
+ manifold->points[0].id.features.flip = 0;
+ b2Vec2 position = c - radius * manifold->normal;
+ manifold->points[0].localPoint1 = b2MulT(xf1, position);
+ manifold->points[0].localPoint2 = b2MulT(xf2, position);
+ manifold->points[0].separation = separation - radius;
+ return;
+ }
+
+ // Project the circle center onto the edge segment.
+ int32 vertIndex1 = normalIndex;
+ int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+ b2Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
+
+ float32 length = e.Normalize();
+ b2Assert(length > B2_FLT_EPSILON);
+
+ // Project the center onto the edge.
+ float32 u = b2Dot(cLocal - vertices[vertIndex1], e);
+ b2Vec2 p;
+ if (u <= 0.0f)
+ {
+ p = vertices[vertIndex1];
+ manifold->points[0].id.features.incidentEdge = b2_nullFeature;
+ manifold->points[0].id.features.incidentVertex = (uint8)vertIndex1;
+ }
+ else if (u >= length)
+ {
+ p = vertices[vertIndex2];
+ manifold->points[0].id.features.incidentEdge = b2_nullFeature;
+ manifold->points[0].id.features.incidentVertex = (uint8)vertIndex2;
+ }
+ else
+ {
+ p = vertices[vertIndex1] + u * e;
+ manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
+ manifold->points[0].id.features.incidentVertex = 0;
+ }
+
+ b2Vec2 d = cLocal - p;
+ float32 dist = d.Normalize();
+ if (dist > radius)
+ {
+ return;
+ }
+
+ manifold->pointCount = 1;
+ manifold->normal = b2Mul(xf1.R, d);
+ b2Vec2 position = c - radius * manifold->normal;
+ manifold->points[0].localPoint1 = b2MulT(xf1, position);
+ manifold->points[0].localPoint2 = b2MulT(xf2, position);
+ manifold->points[0].separation = dist - radius;
+ manifold->points[0].id.features.referenceEdge = 0;
+ manifold->points[0].id.features.flip = 0;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+#include "Shapes/b2PolygonShape.h"
+
+struct ClipVertex
+{
+ b2Vec2 v;
+ b2ContactID id;
+};
+
+static int32 ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
+ const b2Vec2& normal, float32 offset)
+{
+ // Start with no output points
+ int32 numOut = 0;
+
+ // Calculate the distance of end points to the line
+ float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
+ float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+ // If the points are behind the plane
+ if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+ if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+ // If the points are on different sides of the plane
+ if (distance0 * distance1 < 0.0f)
+ {
+ // Find intersection point of edge and plane
+ float32 interp = distance0 / (distance0 - distance1);
+ vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+ if (distance0 > 0.0f)
+ {
+ vOut[numOut].id = vIn[0].id;
+ }
+ else
+ {
+ vOut[numOut].id = vIn[1].id;
+ }
+ ++numOut;
+ }
+
+ return numOut;
+}
+
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
+ const b2PolygonShape* poly2, const b2XForm& xf2)
+{
+ int32 count1 = poly1->GetVertexCount();
+ const b2Vec2* vertices1 = poly1->GetVertices();
+ const b2Vec2* normals1 = poly1->GetNormals();
+
+ int32 count2 = poly2->GetVertexCount();
+ const b2Vec2* vertices2 = poly2->GetVertices();
+
+ b2Assert(0 <= edge1 && edge1 < count1);
+
+ // Convert normal from poly1's frame into poly2's frame.
+ b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
+ b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
+
+ // Find support vertex on poly2 for -normal.
+ int32 index = 0;
+ float32 minDot = B2_FLT_MAX;
+
+ for (int32 i = 0; i < count2; ++i)
+ {
+ float32 dot = b2Dot(vertices2[i], normal1);
+ if (dot < minDot)
+ {
+ minDot = dot;
+ index = i;
+ }
+ }
+
+ b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
+ b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
+ float32 separation = b2Dot(v2 - v1, normal1World);
+ return separation;
+}
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static float32 FindMaxSeparation(int32* edgeIndex,
+ const b2PolygonShape* poly1, const b2XForm& xf1,
+ const b2PolygonShape* poly2, const b2XForm& xf2)
+{
+ int32 count1 = poly1->GetVertexCount();
+ const b2Vec2* normals1 = poly1->GetNormals();
+
+ // Vector pointing from the centroid of poly1 to the centroid of poly2.
+ b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
+ b2Vec2 dLocal1 = b2MulT(xf1.R, d);
+
+ // Find edge normal on poly1 that has the largest projection onto d.
+ int32 edge = 0;
+ float32 maxDot = -B2_FLT_MAX;
+ for (int32 i = 0; i < count1; ++i)
+ {
+ float32 dot = b2Dot(normals1[i], dLocal1);
+ if (dot > maxDot)
+ {
+ maxDot = dot;
+ edge = i;
+ }
+ }
+
+ // Get the separation for the edge normal.
+ float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+ if (s > 0.0f)
+ {
+ return s;
+ }
+
+ // Check the separation for the previous edge normal.
+ int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+ float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+ if (sPrev > 0.0f)
+ {
+ return sPrev;
+ }
+
+ // Check the separation for the next edge normal.
+ int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+ float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+ if (sNext > 0.0f)
+ {
+ return sNext;
+ }
+
+ // Find the best edge and the search direction.
+ int32 bestEdge;
+ float32 bestSeparation;
+ int32 increment;
+ if (sPrev > s && sPrev > sNext)
+ {
+ increment = -1;
+ bestEdge = prevEdge;
+ bestSeparation = sPrev;
+ }
+ else if (sNext > s)
+ {
+ increment = 1;
+ bestEdge = nextEdge;
+ bestSeparation = sNext;
+ }
+ else
+ {
+ *edgeIndex = edge;
+ return s;
+ }
+
+ // Perform a local search for the best edge normal.
+ for ( ; ; )
+ {
+ if (increment == -1)
+ edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+ else
+ edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+ s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+ if (s > 0.0f)
+ {
+ return s;
+ }
+
+ if (s > bestSeparation)
+ {
+ bestEdge = edge;
+ bestSeparation = s;
+ }
+ else
+ {
+ break;
+ }
+ }
+
+ *edgeIndex = bestEdge;
+ return bestSeparation;
+}
+
+static void FindIncidentEdge(ClipVertex c[2],
+ const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
+ const b2PolygonShape* poly2, const b2XForm& xf2)
+{
+ int32 count1 = poly1->GetVertexCount();
+ const b2Vec2* normals1 = poly1->GetNormals();
+
+ int32 count2 = poly2->GetVertexCount();
+ const b2Vec2* vertices2 = poly2->GetVertices();
+ const b2Vec2* normals2 = poly2->GetNormals();
+
+ b2Assert(0 <= edge1 && edge1 < count1);
+
+ // Get the normal of the reference edge in poly2's frame.
+ b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
+
+ // Find the incident edge on poly2.
+ int32 index = 0;
+ float32 minDot = B2_FLT_MAX;
+ for (int32 i = 0; i < count2; ++i)
+ {
+ float32 dot = b2Dot(normal1, normals2[i]);
+ if (dot < minDot)
+ {
+ minDot = dot;
+ index = i;
+ }
+ }
+
+ // Build the clip vertices for the incident edge.
+ int32 i1 = index;
+ int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+ c[0].v = b2Mul(xf2, vertices2[i1]);
+ c[0].id.features.referenceEdge = (uint8)edge1;
+ c[0].id.features.incidentEdge = (uint8)i1;
+ c[0].id.features.incidentVertex = 0;
+
+ c[1].v = b2Mul(xf2, vertices2[i2]);
+ c[1].id.features.referenceEdge = (uint8)edge1;
+ c[1].id.features.incidentEdge = (uint8)i2;
+ c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(b2Manifold* manifold,
+ const b2PolygonShape* polyA, const b2XForm& xfA,
+ const b2PolygonShape* polyB, const b2XForm& xfB)
+{
+ manifold->pointCount = 0;
+
+ int32 edgeA = 0;
+ float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+ if (separationA > 0.0f)
+ return;
+
+ int32 edgeB = 0;
+ float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+ if (separationB > 0.0f)
+ return;
+
+ const b2PolygonShape* poly1; // reference poly
+ const b2PolygonShape* poly2; // incident poly
+ b2XForm xf1, xf2;
+ int32 edge1; // reference edge
+ uint8 flip;
+ const float32 k_relativeTol = 0.98f;
+ const float32 k_absoluteTol = 0.001f;
+
+ // TODO_ERIN use "radius" of poly for absolute tolerance.
+ if (separationB > k_relativeTol * separationA + k_absoluteTol)
+ {
+ poly1 = polyB;
+ poly2 = polyA;
+ xf1 = xfB;
+ xf2 = xfA;
+ edge1 = edgeB;
+ flip = 1;
+ }
+ else
+ {
+ poly1 = polyA;
+ poly2 = polyB;
+ xf1 = xfA;
+ xf2 = xfB;
+ edge1 = edgeA;
+ flip = 0;
+ }
+
+ ClipVertex incidentEdge[2];
+ FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+ int32 count1 = poly1->GetVertexCount();
+ const b2Vec2* vertices1 = poly1->GetVertices();
+
+ b2Vec2 v11 = vertices1[edge1];
+ b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+
+ b2Vec2 dv = v12 - v11;
+ b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
+ sideNormal.Normalize();
+ b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
+
+ v11 = b2Mul(xf1, v11);
+ v12 = b2Mul(xf1, v12);
+
+ float32 frontOffset = b2Dot(frontNormal, v11);
+ float32 sideOffset1 = -b2Dot(sideNormal, v11);
+ float32 sideOffset2 = b2Dot(sideNormal, v12);
+
+ // Clip incident edge against extruded edge1 side edges.
+ ClipVertex clipPoints1[2];
+ ClipVertex clipPoints2[2];
+ int np;
+
+ // Clip to box side 1
+ np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+
+ if (np < 2)
+ return;
+
+ // Clip to negative box side 1
+ np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
+
+ if (np < 2)
+ return;
+
+ // Now clipPoints2 contains the clipped points.
+ manifold->normal = flip ? -frontNormal : frontNormal;
+
+ int32 pointCount = 0;
+ for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+ {
+ float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+
+ if (separation <= 0.0f)
+ {
+ b2ManifoldPoint* cp = manifold->points + pointCount;
+ cp->separation = separation;
+ cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
+ cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+ cp->id = clipPoints2[i].id;
+ cp->id.features.flip = flip;
+ ++pointCount;
+ }
+ }
+
+ manifold->pointCount = pointCount;
+}
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+
+// Collision Detection in Interactive 3D Environments by Gino van den Bergen
+// From Section 3.4.1
+// x = mu1 * p1 + mu2 * p2
+// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
+// mu1 = 1 - mu2;
+// x = (1 - mu2) * p1 + mu2 * p2
+// = p1 + mu2 * (p2 - p1)
+// x = s + a * r (s := start, r := end - start)
+// s + a * r = p1 + mu2 * d (d := p2 - p1)
+// -a * r + mu2 * d = b (b := s - p1)
+// [-r d] * [a; mu2] = b
+// Cramer's rule:
+// denom = det[-r d]
+// a = det[b d] / denom
+// mu2 = det[-r b] / denom
+bool b2Segment::TestSegment(float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const
+{
+ b2Vec2 s = segment.p1;
+ b2Vec2 r = segment.p2 - s;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 n = b2Cross(d, 1.0f);
+
+ const float32 k_slop = 100.0f * B2_FLT_EPSILON;
+ float32 denom = -b2Dot(r, n);
+
+ // Cull back facing collision and ignore parallel segments.
+ if (denom > k_slop)
+ {
+ // Does the segment intersect the infinite line associated with this segment?
+ b2Vec2 b = s - p1;
+ float32 a = b2Dot(b, n);
+
+ if (0.0f <= a && a <= maxLambda * denom)
+ {
+ float32 mu2 = -r.x * b.y + r.y * b.x;
+
+ // Does the segment intersect this segment?
+ if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
+ {
+ a /= denom;
+ n.Normalize();
+ *lambda = a;
+ *normal = n;
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_COLLISION_H
+#define B2_COLLISION_H
+
+#include "../Common/b2Math.h"
+#include <climits>
+
+/// @file
+/// Structures and functions used for computing contact points, distance
+/// queries, and TOI queries.
+
+class b2Shape;
+class b2CircleShape;
+class b2PolygonShape;
+
+const uint8 b2_nullFeature = UCHAR_MAX;
+
+/// Contact ids to facilitate warm starting.
+union b2ContactID
+{
+ /// The features that intersect to form the contact point
+ struct Features
+ {
+ uint8 referenceEdge; ///< The edge that defines the outward contact normal.
+ uint8 incidentEdge; ///< The edge most anti-parallel to the reference edge.
+ uint8 incidentVertex; ///< The vertex (0 or 1) on the incident edge that was clipped.
+ uint8 flip; ///< A value of 1 indicates that the reference edge is on shape2.
+ } features;
+ uint32 key; ///< Used to quickly compare contact ids.
+};
+
+/// A manifold point is a contact point belonging to a contact
+/// manifold. It holds details related to the geometry and dynamics
+/// of the contact points.
+/// The point is stored in local coordinates because CCD
+/// requires sub-stepping in which the separation is stale.
+struct b2ManifoldPoint
+{
+ b2Vec2 localPoint1; ///< local position of the contact point in body1
+ b2Vec2 localPoint2; ///< local position of the contact point in body2
+ float32 separation; ///< the separation of the shapes along the normal vector
+ float32 normalImpulse; ///< the non-penetration impulse
+ float32 tangentImpulse; ///< the friction impulse
+ b2ContactID id; ///< uniquely identifies a contact point between two shapes
+};
+
+/// A manifold for two touching convex shapes.
+struct b2Manifold
+{
+ b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact
+ b2Vec2 normal; ///< the shared unit normal vector
+ int32 pointCount; ///< the number of manifold points
+};
+
+/// A line segment.
+struct b2Segment
+{
+ /// Ray cast against this segment with another segment.
+ bool TestSegment(float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const;
+
+ b2Vec2 p1; ///< the starting point
+ b2Vec2 p2; ///< the ending point
+};
+
+/// An axis aligned bounding box.
+struct b2AABB
+{
+ /// Verify that the bounds are sorted.
+ bool IsValid() const;
+
+ b2Vec2 lowerBound; ///< the lower vertex
+ b2Vec2 upperBound; ///< the upper vertex
+};
+
+/// An oriented bounding box.
+struct b2OBB
+{
+ b2Mat22 R; ///< the rotation matrix
+ b2Vec2 center; ///< the local centroid
+ b2Vec2 extents; ///< the half-widths
+};
+
+/// Compute the collision manifold between two circles.
+void b2CollideCircles(b2Manifold* manifold,
+ const b2CircleShape* circle1, const b2XForm& xf1,
+ const b2CircleShape* circle2, const b2XForm& xf2);
+
+/// Compute the collision manifold between a polygon and a circle.
+void b2CollidePolygonAndCircle(b2Manifold* manifold,
+ const b2PolygonShape* polygon, const b2XForm& xf1,
+ const b2CircleShape* circle, const b2XForm& xf2);
+
+/// Compute the collision manifold between two circles.
+void b2CollidePolygons(b2Manifold* manifold,
+ const b2PolygonShape* polygon1, const b2XForm& xf1,
+ const b2PolygonShape* polygon2, const b2XForm& xf2);
+
+/// Compute the distance between two shapes and the closest points.
+/// @return the distance between the shapes or zero if they are overlapped/touching.
+float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
+ const b2Shape* shape1, const b2XForm& xf1,
+ const b2Shape* shape2, const b2XForm& xf2);
+
+/// Compute the time when two shapes begin to touch or touch at a closer distance.
+/// @warning the sweeps must have the same time interval.
+/// @return the fraction between [0,1] in which the shapes first touch.
+/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
+float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
+ const b2Shape* shape2, const b2Sweep& sweep2);
+
+
+// ---------------- Inline Functions ------------------------------------------
+
+inline bool b2AABB::IsValid() const
+{
+ b2Vec2 d = upperBound - lowerBound;
+ bool valid = d.x >= 0.0f && d.y >= 0.0f;
+ valid = valid && lowerBound.IsValid() && upperBound.IsValid();
+ return valid;
+}
+
+inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)
+{
+ b2Vec2 d1, d2;
+ d1 = b.lowerBound - a.upperBound;
+ d2 = a.lowerBound - b.upperBound;
+
+ if (d1.x > 0.0f || d1.y > 0.0f)
+ return false;
+
+ if (d2.x > 0.0f || d2.y > 0.0f)
+ return false;
+
+ return true;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+#include "Shapes/b2CircleShape.h"
+#include "Shapes/b2PolygonShape.h"
+
+int32 g_GJK_Iterations = 0;
+
+// GJK using Voronoi regions (Christer Ericson) and region selection
+// optimizations (Casey Muratori).
+
+// The origin is either in the region of points[1] or in the edge region. The origin is
+// not in region of points[0] because that is the old point.
+static int32 ProcessTwo(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
+{
+ // If in point[1] region
+ b2Vec2 r = -points[1];
+ b2Vec2 d = points[0] - points[1];
+ float32 length = d.Normalize();
+ float32 lambda = b2Dot(r, d);
+ if (lambda <= 0.0f || length < B2_FLT_EPSILON)
+ {
+ // The simplex is reduced to a point.
+ *x1 = p1s[1];
+ *x2 = p2s[1];
+ p1s[0] = p1s[1];
+ p2s[0] = p2s[1];
+ points[0] = points[1];
+ return 1;
+ }
+
+ // Else in edge region
+ lambda /= length;
+ *x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
+ *x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
+ return 2;
+}
+
+// Possible regions:
+// - points[2]
+// - edge points[0]-points[2]
+// - edge points[1]-points[2]
+// - inside the triangle
+static int32 ProcessThree(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
+{
+ b2Vec2 a = points[0];
+ b2Vec2 b = points[1];
+ b2Vec2 c = points[2];
+
+ b2Vec2 ab = b - a;
+ b2Vec2 ac = c - a;
+ b2Vec2 bc = c - b;
+
+ float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
+ float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
+ float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);
+
+ // In vertex c region?
+ if (td <= 0.0f && ud <= 0.0f)
+ {
+ // Single point
+ *x1 = p1s[2];
+ *x2 = p2s[2];
+ p1s[0] = p1s[2];
+ p2s[0] = p2s[2];
+ points[0] = points[2];
+ return 1;
+ }
+
+ // Should not be in vertex a or b region.
+ B2_NOT_USED(sd);
+ B2_NOT_USED(sn);
+ b2Assert(sn > 0.0f || tn > 0.0f);
+ b2Assert(sd > 0.0f || un > 0.0f);
+
+ float32 n = b2Cross(ab, ac);
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+ n = (n < 0.0)? -1.0 : ((n > 0.0)? 1.0 : 0.0);
+#endif
+
+ // Should not be in edge ab region.
+ float32 vc = n * b2Cross(a, b);
+ b2Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
+
+ // In edge bc region?
+ float32 va = n * b2Cross(b, c);
+ if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un+ud) > 0.0f)
+ {
+ b2Assert(un + ud > 0.0f);
+ float32 lambda = un / (un + ud);
+ *x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
+ *x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
+ p1s[0] = p1s[2];
+ p2s[0] = p2s[2];
+ points[0] = points[2];
+ return 2;
+ }
+
+ // In edge ac region?
+ float32 vb = n * b2Cross(c, a);
+ if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn+td) > 0.0f)
+ {
+ b2Assert(tn + td > 0.0f);
+ float32 lambda = tn / (tn + td);
+ *x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
+ *x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
+ p1s[1] = p1s[2];
+ p2s[1] = p2s[2];
+ points[1] = points[2];
+ return 2;
+ }
+
+ // Inside the triangle, compute barycentric coordinates
+ float32 denom = va + vb + vc;
+ b2Assert(denom > 0.0f);
+ denom = 1.0f / denom;
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+ *x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
+ *x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
+#else
+ float32 u = va * denom;
+ float32 v = vb * denom;
+ float32 w = 1.0f - u - v;
+ *x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
+ *x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
+#endif
+ return 3;
+}
+
+static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
+{
+ const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
+ for (int32 i = 0; i < pointCount; ++i)
+ {
+ b2Vec2 d = b2Abs(w - points[i]);
+ b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
+
+ if (d.x < k_tolerance * (m.x + 1.0f) &&
+ d.y < k_tolerance * (m.y + 1.0f))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+template <typename T1, typename T2>
+float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
+ const T1* shape1, const b2XForm& xf1,
+ const T2* shape2, const b2XForm& xf2)
+{
+ b2Vec2 p1s[3], p2s[3];
+ b2Vec2 points[3];
+ int32 pointCount = 0;
+
+ *x1 = shape1->GetFirstVertex(xf1);
+ *x2 = shape2->GetFirstVertex(xf2);
+
+ float32 vSqr = 0.0f;
+ const int32 maxIterations = 20;
+ for (int32 iter = 0; iter < maxIterations; ++iter)
+ {
+ b2Vec2 v = *x2 - *x1;
+ b2Vec2 w1 = shape1->Support(xf1, v);
+ b2Vec2 w2 = shape2->Support(xf2, -v);
+
+ vSqr = b2Dot(v, v);
+ b2Vec2 w = w2 - w1;
+ float32 vw = b2Dot(v, w);
+ if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
+ {
+ if (pointCount == 0)
+ {
+ *x1 = w1;
+ *x2 = w2;
+ }
+ g_GJK_Iterations = iter;
+ return b2Sqrt(vSqr);
+ }
+
+ switch (pointCount)
+ {
+ case 0:
+ p1s[0] = w1;
+ p2s[0] = w2;
+ points[0] = w;
+ *x1 = p1s[0];
+ *x2 = p2s[0];
+ ++pointCount;
+ break;
+
+ case 1:
+ p1s[1] = w1;
+ p2s[1] = w2;
+ points[1] = w;
+ pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
+ break;
+
+ case 2:
+ p1s[2] = w1;
+ p2s[2] = w2;
+ points[2] = w;
+ pointCount = ProcessThree(x1, x2, p1s, p2s, points);
+ break;
+ }
+
+ // If we have three points, then the origin is in the corresponding triangle.
+ if (pointCount == 3)
+ {
+ g_GJK_Iterations = iter;
+ return 0.0f;
+ }
+
+ float32 maxSqr = -B2_FLT_MAX;
+ for (int32 i = 0; i < pointCount; ++i)
+ {
+ maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
+ }
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+ if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
+#else
+ if (pointCount == 3 || vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
+#endif
+ {
+ g_GJK_Iterations = iter;
+ v = *x2 - *x1;
+ vSqr = b2Dot(v, v);
+ return b2Sqrt(vSqr);
+ }
+ }
+
+ g_GJK_Iterations = maxIterations;
+ return b2Sqrt(vSqr);
+}
+
+static float32 DistanceCC(
+ b2Vec2* x1, b2Vec2* x2,
+ const b2CircleShape* circle1, const b2XForm& xf1,
+ const b2CircleShape* circle2, const b2XForm& xf2)
+{
+ b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
+ b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
+
+ b2Vec2 d = p2 - p1;
+ float32 dSqr = b2Dot(d, d);
+ float32 r1 = circle1->GetRadius() - b2_toiSlop;
+ float32 r2 = circle2->GetRadius() - b2_toiSlop;
+ float32 r = r1 + r2;
+ if (dSqr > r * r)
+ {
+ float32 dLen = d.Normalize();
+ float32 distance = dLen - r;
+ *x1 = p1 + r1 * d;
+ *x2 = p2 - r2 * d;
+ return distance;
+ }
+ else if (dSqr > B2_FLT_EPSILON * B2_FLT_EPSILON)
+ {
+ d.Normalize();
+ *x1 = p1 + r1 * d;
+ *x2 = *x1;
+ return 0.0f;
+ }
+
+ *x1 = p1;
+ *x2 = *x1;
+ return 0.0f;
+}
+
+// This is used for polygon-vs-circle distance.
+struct Point
+{
+ b2Vec2 Support(const b2XForm&, const b2Vec2&) const
+ {
+ return p;
+ }
+
+ b2Vec2 GetFirstVertex(const b2XForm&) const
+ {
+ return p;
+ }
+
+ b2Vec2 p;
+};
+
+// GJK is more robust with polygon-vs-point than polygon-vs-circle.
+// So we convert polygon-vs-circle to polygon-vs-point.
+static float32 DistancePC(
+ b2Vec2* x1, b2Vec2* x2,
+ const b2PolygonShape* polygon, const b2XForm& xf1,
+ const b2CircleShape* circle, const b2XForm& xf2)
+{
+ Point point;
+ point.p = b2Mul(xf2, circle->GetLocalPosition());
+
+ float32 distance = DistanceGeneric(x1, x2, polygon, xf1, &point, b2XForm_identity);
+
+ float32 r = circle->GetRadius() - b2_toiSlop;
+
+ if (distance > r)
+ {
+ distance -= r;
+ b2Vec2 d = *x2 - *x1;
+ d.Normalize();
+ *x2 -= r * d;
+ }
+ else
+ {
+ distance = 0.0f;
+ *x2 = *x1;
+ }
+
+ return distance;
+}
+
+float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
+ const b2Shape* shape1, const b2XForm& xf1,
+ const b2Shape* shape2, const b2XForm& xf2)
+{
+ b2ShapeType type1 = shape1->GetType();
+ b2ShapeType type2 = shape2->GetType();
+
+ if (type1 == e_circleShape && type2 == e_circleShape)
+ {
+ return DistanceCC(x1, x2, (b2CircleShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
+ }
+
+ if (type1 == e_polygonShape && type2 == e_circleShape)
+ {
+ return DistancePC(x1, x2, (b2PolygonShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
+ }
+
+ if (type1 == e_circleShape && type2 == e_polygonShape)
+ {
+ return DistancePC(x2, x1, (b2PolygonShape*)shape2, xf2, (b2CircleShape*)shape1, xf1);
+ }
+
+ if (type1 == e_polygonShape && type2 == e_polygonShape)
+ {
+ return DistanceGeneric(x1, x2, (b2PolygonShape*)shape1, xf1, (b2PolygonShape*)shape2, xf2);
+ }
+
+ return 0.0f;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PairManager.h"
+#include "b2BroadPhase.h"
+
+#include <algorithm>
+
+// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+// This assumes proxyId1 and proxyId2 are 16-bit.
+inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
+{
+ uint32 key = (proxyId2 << 16) | proxyId1;
+ key = ~key + (key << 15);
+ key = key ^ (key >> 12);
+ key = key + (key << 2);
+ key = key ^ (key >> 4);
+ key = key * 2057;
+ key = key ^ (key >> 16);
+ return key;
+}
+
+inline bool Equals(const b2Pair& pair, int32 proxyId1, int32 proxyId2)
+{
+ return pair.proxyId1 == proxyId1 && pair.proxyId2 == proxyId2;
+}
+
+inline bool Equals(const b2BufferedPair& pair1, const b2BufferedPair& pair2)
+{
+ return pair1.proxyId1 == pair2.proxyId1 && pair1.proxyId2 == pair2.proxyId2;
+}
+
+// For sorting.
+inline bool operator < (const b2BufferedPair& pair1, const b2BufferedPair& pair2)
+{
+ if (pair1.proxyId1 < pair2.proxyId1)
+ {
+ return true;
+ }
+
+ if (pair1.proxyId1 == pair2.proxyId1)
+ {
+ return pair1.proxyId2 < pair2.proxyId2;
+ }
+
+ return false;
+}
+
+
+b2PairManager::b2PairManager()
+{
+ b2Assert(b2IsPowerOfTwo(b2_tableCapacity) == true);
+ b2Assert(b2_tableCapacity >= b2_maxPairs);
+ for (int32 i = 0; i < b2_tableCapacity; ++i)
+ {
+ m_hashTable[i] = b2_nullPair;
+ }
+ m_freePair = 0;
+ for (int32 i = 0; i < b2_maxPairs; ++i)
+ {
+ m_pairs[i].proxyId1 = b2_nullProxy;
+ m_pairs[i].proxyId2 = b2_nullProxy;
+ m_pairs[i].userData = NULL;
+ m_pairs[i].status = 0;
+ m_pairs[i].next = uint16(i + 1);
+ }
+ m_pairs[b2_maxPairs-1].next = b2_nullPair;
+ m_pairCount = 0;
+ m_pairBufferCount = 0;
+}
+
+void b2PairManager::Initialize(b2BroadPhase* broadPhase, b2PairCallback* callback)
+{
+ m_broadPhase = broadPhase;
+ m_callback = callback;
+}
+
+b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2, uint32 hash)
+{
+ int32 index = m_hashTable[hash];
+
+ while (index != b2_nullPair && Equals(m_pairs[index], proxyId1, proxyId2) == false)
+ {
+ index = m_pairs[index].next;
+ }
+
+ if (index == b2_nullPair)
+ {
+ return NULL;
+ }
+
+ b2Assert(index < b2_maxPairs);
+
+ return m_pairs + index;
+}
+
+b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2)
+{
+ if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
+
+ int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+ return Find(proxyId1, proxyId2, hash);
+}
+
+// Returns existing pair or creates a new one.
+b2Pair* b2PairManager::AddPair(int32 proxyId1, int32 proxyId2)
+{
+ if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
+
+ int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+ b2Pair* pair = Find(proxyId1, proxyId2, hash);
+ if (pair != NULL)
+ {
+ return pair;
+ }
+
+ b2Assert(m_pairCount < b2_maxPairs && m_freePair != b2_nullPair);
+
+ uint16 pairIndex = m_freePair;
+ pair = m_pairs + pairIndex;
+ m_freePair = pair->next;
+
+ pair->proxyId1 = (uint16)proxyId1;
+ pair->proxyId2 = (uint16)proxyId2;
+ pair->status = 0;
+ pair->userData = NULL;
+ pair->next = m_hashTable[hash];
+
+ m_hashTable[hash] = pairIndex;
+
+ ++m_pairCount;
+
+ return pair;
+}
+
+// Removes a pair. The pair must exist.
+void* b2PairManager::RemovePair(int32 proxyId1, int32 proxyId2)
+{
+ b2Assert(m_pairCount > 0);
+
+ if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
+
+ int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
+
+ uint16* node = &m_hashTable[hash];
+ while (*node != b2_nullPair)
+ {
+ if (Equals(m_pairs[*node], proxyId1, proxyId2))
+ {
+ uint16 index = *node;
+ *node = m_pairs[*node].next;
+
+ b2Pair* pair = m_pairs + index;
+ void* userData = pair->userData;
+
+ // Scrub
+ pair->next = m_freePair;
+ pair->proxyId1 = b2_nullProxy;
+ pair->proxyId2 = b2_nullProxy;
+ pair->userData = NULL;
+ pair->status = 0;
+
+ m_freePair = index;
+ --m_pairCount;
+ return userData;
+ }
+ else
+ {
+ node = &m_pairs[*node].next;
+ }
+ }
+
+ b2Assert(false);
+ return NULL;
+}
+
+/*
+As proxies are created and moved, many pairs are created and destroyed. Even worse, the same
+pair may be added and removed multiple times in a single time step of the physics engine. To reduce
+traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the
+end of the physics step. This is done by buffering all the RemovePair requests. AddPair
+requests are processed immediately because we need the hash table entry for quick lookup.
+
+All user user callbacks are delayed until the buffered pairs are confirmed in Commit.
+This is very important because the user callbacks may be very expensive and client logic
+may be harmed if pairs are added and removed within the same time step.
+
+Buffer a pair for addition.
+We may add a pair that is not in the pair manager or pair buffer.
+We may add a pair that is already in the pair manager and pair buffer.
+If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called).
+*/
+void b2PairManager::AddBufferedPair(int32 id1, int32 id2)
+{
+ b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
+ b2Assert(m_pairBufferCount < b2_maxPairs);
+
+ b2Pair* pair = AddPair(id1, id2);
+
+ // If this pair is not in the pair buffer ...
+ if (pair->IsBuffered() == false)
+ {
+ // This must be a newly added pair.
+ b2Assert(pair->IsFinal() == false);
+
+ // Add it to the pair buffer.
+ pair->SetBuffered();
+ m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
+ m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
+ ++m_pairBufferCount;
+
+ b2Assert(m_pairBufferCount <= m_pairCount);
+ }
+
+ // Confirm this pair for the subsequent call to Commit.
+ pair->ClearRemoved();
+
+ if (b2BroadPhase::s_validate)
+ {
+ ValidateBuffer();
+ }
+}
+
+// Buffer a pair for removal.
+void b2PairManager::RemoveBufferedPair(int32 id1, int32 id2)
+{
+ b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
+ b2Assert(m_pairBufferCount < b2_maxPairs);
+
+ b2Pair* pair = Find(id1, id2);
+
+ if (pair == NULL)
+ {
+ // The pair never existed. This is legal (due to collision filtering).
+ return;
+ }
+
+ // If this pair is not in the pair buffer ...
+ if (pair->IsBuffered() == false)
+ {
+ // This must be an old pair.
+ b2Assert(pair->IsFinal() == true);
+
+ pair->SetBuffered();
+ m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
+ m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
+ ++m_pairBufferCount;
+
+ b2Assert(m_pairBufferCount <= m_pairCount);
+ }
+
+ pair->SetRemoved();
+
+ if (b2BroadPhase::s_validate)
+ {
+ ValidateBuffer();
+ }
+}
+
+void b2PairManager::Commit()
+{
+ int32 removeCount = 0;
+
+ b2Proxy* proxies = m_broadPhase->m_proxyPool;
+
+ for (int32 i = 0; i < m_pairBufferCount; ++i)
+ {
+ b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
+ b2Assert(pair->IsBuffered());
+ pair->ClearBuffered();
+
+ b2Assert(pair->proxyId1 < b2_maxProxies && pair->proxyId2 < b2_maxProxies);
+
+ b2Proxy* proxy1 = proxies + pair->proxyId1;
+ b2Proxy* proxy2 = proxies + pair->proxyId2;
+
+ b2Assert(proxy1->IsValid());
+ b2Assert(proxy2->IsValid());
+
+ if (pair->IsRemoved())
+ {
+ // It is possible a pair was added then removed before a commit. Therefore,
+ // we should be careful not to tell the user the pair was removed when the
+ // the user didn't receive a matching add.
+ if (pair->IsFinal() == true)
+ {
+ m_callback->PairRemoved(proxy1->userData, proxy2->userData, pair->userData);
+ }
+
+ // Store the ids so we can actually remove the pair below.
+ m_pairBuffer[removeCount].proxyId1 = pair->proxyId1;
+ m_pairBuffer[removeCount].proxyId2 = pair->proxyId2;
+ ++removeCount;
+ }
+ else
+ {
+ b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);
+
+ if (pair->IsFinal() == false)
+ {
+ pair->userData = m_callback->PairAdded(proxy1->userData, proxy2->userData);
+ pair->SetFinal();
+ }
+ }
+ }
+
+ for (int32 i = 0; i < removeCount; ++i)
+ {
+ RemovePair(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
+ }
+
+ m_pairBufferCount = 0;
+
+ if (b2BroadPhase::s_validate)
+ {
+ ValidateTable();
+ }
+}
+
+void b2PairManager::ValidateBuffer()
+{
+#ifdef _DEBUG
+ b2Assert(m_pairBufferCount <= m_pairCount);
+
+ std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
+
+ for (int32 i = 0; i < m_pairBufferCount; ++i)
+ {
+ if (i > 0)
+ {
+ b2Assert(Equals(m_pairBuffer[i], m_pairBuffer[i-1]) == false);
+ }
+
+ b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
+ b2Assert(pair->IsBuffered());
+
+ b2Assert(pair->proxyId1 != pair->proxyId2);
+ b2Assert(pair->proxyId1 < b2_maxProxies);
+ b2Assert(pair->proxyId2 < b2_maxProxies);
+
+ b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
+ b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;
+
+ b2Assert(proxy1->IsValid() == true);
+ b2Assert(proxy2->IsValid() == true);
+ }
+#endif
+}
+
+void b2PairManager::ValidateTable()
+{
+#ifdef _DEBUG
+ for (int32 i = 0; i < b2_tableCapacity; ++i)
+ {
+ uint16 index = m_hashTable[i];
+ while (index != b2_nullPair)
+ {
+ b2Pair* pair = m_pairs + index;
+ b2Assert(pair->IsBuffered() == false);
+ b2Assert(pair->IsFinal() == true);
+ b2Assert(pair->IsRemoved() == false);
+
+ b2Assert(pair->proxyId1 != pair->proxyId2);
+ b2Assert(pair->proxyId1 < b2_maxProxies);
+ b2Assert(pair->proxyId2 < b2_maxProxies);
+
+ b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
+ b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;
+
+ b2Assert(proxy1->IsValid() == true);
+ b2Assert(proxy2->IsValid() == true);
+
+ b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);
+
+ index = pair->next;
+ }
+ }
+#endif
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+// The pair manager is used by the broad-phase to quickly add/remove/find pairs
+// of overlapping proxies. It is based closely on code provided by Pierre Terdiman.
+// http://www.codercorner.com/IncrementalSAP.txt
+
+#ifndef B2_PAIR_MANAGER_H
+#define B2_PAIR_MANAGER_H
+
+#include "../Common/b2Settings.h"
+#include "../Common/b2Math.h"
+
+#include <climits>
+
+class b2BroadPhase;
+struct b2Proxy;
+
+const uint16 b2_nullPair = USHRT_MAX;
+const uint16 b2_nullProxy = USHRT_MAX;
+const int32 b2_tableCapacity = b2_maxPairs; // must be a power of two
+const int32 b2_tableMask = b2_tableCapacity - 1;
+
+struct b2Pair
+{
+ enum
+ {
+ e_pairBuffered = 0x0001,
+ e_pairRemoved = 0x0002,
+ e_pairFinal = 0x0004,
+ };
+
+ void SetBuffered() { status |= e_pairBuffered; }
+ void ClearBuffered() { status &= ~e_pairBuffered; }
+ bool IsBuffered() { return (status & e_pairBuffered) == e_pairBuffered; }
+
+ void SetRemoved() { status |= e_pairRemoved; }
+ void ClearRemoved() { status &= ~e_pairRemoved; }
+ bool IsRemoved() { return (status & e_pairRemoved) == e_pairRemoved; }
+
+ void SetFinal() { status |= e_pairFinal; }
+ bool IsFinal() { return (status & e_pairFinal) == e_pairFinal; }
+
+ void* userData;
+ uint16 proxyId1;
+ uint16 proxyId2;
+ uint16 next;
+ uint16 status;
+};
+
+struct b2BufferedPair
+{
+ uint16 proxyId1;
+ uint16 proxyId2;
+};
+
+class b2PairCallback
+{
+public:
+ virtual ~b2PairCallback() {}
+
+ // This should return the new pair user data. It is ok if the
+ // user data is null.
+ virtual void* PairAdded(void* proxyUserData1, void* proxyUserData2) = 0;
+
+ // This should free the pair's user data. In extreme circumstances, it is possible
+ // this will be called with null pairUserData because the pair never existed.
+ virtual void PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData) = 0;
+};
+
+class b2PairManager
+{
+public:
+ b2PairManager();
+
+ void Initialize(b2BroadPhase* broadPhase, b2PairCallback* callback);
+
+ void AddBufferedPair(int32 proxyId1, int32 proxyId2);
+ void RemoveBufferedPair(int32 proxyId1, int32 proxyId2);
+
+ void Commit();
+
+private:
+ b2Pair* Find(int32 proxyId1, int32 proxyId2);
+ b2Pair* Find(int32 proxyId1, int32 proxyId2, uint32 hashValue);
+
+ b2Pair* AddPair(int32 proxyId1, int32 proxyId2);
+ void* RemovePair(int32 proxyId1, int32 proxyId2);
+
+ void ValidateBuffer();
+ void ValidateTable();
+
+public:
+ b2BroadPhase *m_broadPhase;
+ b2PairCallback *m_callback;
+ b2Pair m_pairs[b2_maxPairs];
+ uint16 m_freePair;
+ int32 m_pairCount;
+
+ b2BufferedPair m_pairBuffer[b2_maxPairs];
+ int32 m_pairBufferCount;
+
+ uint16 m_hashTable[b2_tableCapacity];
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+#include "Shapes/b2Shape.h"
+
+// This algorithm uses conservative advancement to compute the time of
+// impact (TOI) of two shapes.
+// Refs: Bullet, Young Kim
+float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
+ const b2Shape* shape2, const b2Sweep& sweep2)
+{
+ float32 r1 = shape1->GetSweepRadius();
+ float32 r2 = shape2->GetSweepRadius();
+
+ b2Assert(sweep1.t0 == sweep2.t0);
+ b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
+
+ float32 t0 = sweep1.t0;
+ b2Vec2 v1 = sweep1.c - sweep1.c0;
+ b2Vec2 v2 = sweep2.c - sweep2.c0;
+ float32 omega1 = sweep1.a - sweep1.a0;
+ float32 omega2 = sweep2.a - sweep2.a0;
+
+ float32 alpha = 0.0f;
+
+ b2Vec2 p1, p2;
+ const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
+ int32 iter = 0;
+ b2Vec2 normal = b2Vec2_zero;
+ float32 distance = 0.0f;
+ float32 targetDistance = 0.0f;
+ for(;;)
+ {
+ float32 t = (1.0f - alpha) * t0 + alpha;
+ b2XForm xf1, xf2;
+ sweep1.GetXForm(&xf1, t);
+ sweep2.GetXForm(&xf2, t);
+
+ // Get the distance between shapes.
+ distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
+
+ if (iter == 0)
+ {
+ // Compute a reasonable target distance to give some breathing room
+ // for conservative advancement.
+ if (distance > 2.0f * b2_toiSlop)
+ {
+ targetDistance = 1.5f * b2_toiSlop;
+ }
+ else
+ {
+ targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
+ }
+ }
+
+ if (distance - targetDistance < 0.05f * b2_toiSlop || iter == k_maxIterations)
+ {
+ break;
+ }
+
+ normal = p2 - p1;
+ normal.Normalize();
+
+ // Compute upper bound on remaining movement.
+ float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
+ if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
+ {
+ alpha = 1.0f;
+ break;
+ }
+
+ // Get the conservative time increment. Don't advance all the way.
+ float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
+ //float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
+ float32 newAlpha = alpha + dAlpha;
+
+ // The shapes may be moving apart or a safe distance apart.
+ if (newAlpha < 0.0f || 1.0f < newAlpha)
+ {
+ alpha = 1.0f;
+ break;
+ }
+
+ // Ensure significant advancement.
+ if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
+ {
+ break;
+ }
+
+ alpha = newAlpha;
+
+ ++iter;
+ }
+
+ return alpha;
+}
--- /dev/null
+/*
+Copyright (c) 2006 Henry Strickland & Ryan Seto
+ 2007-2008 Tobias Weyand (modifications and extensions)
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+
+ (* http://www.opensource.org/licenses/mit-license.php *)
+*/
+
+#ifndef _FIXED_H_
+#define _FIXED_H_
+
+#include <stdio.h>
+
+#ifdef TARGET_IS_NDS
+
+#include "nds.h"
+
+#endif
+
+#define FIXED_BP 16
+#define FIXED_MAX ((1<<(32-FIXED_BP-1))-1)
+#define FIXED_MIN (-(1<<(32-FIXED_BP-1)))
+#define FIXED_EPSILON (Fixed(0.00007f))
+
+#define G_1_DIV_PI 20861
+
+class Fixed {
+
+ private:
+
+ int g; // the guts
+
+ const static int BP= FIXED_BP; // how many low bits are right of Binary Point
+ const static int BP2= BP*2; // how many low bits are right of Binary Point
+ const static int BPhalf= BP/2; // how many low bits are right of Binary Point
+
+ double STEP(); // smallest step we can represent
+
+ // for private construction via guts
+ enum FixedRaw { RAW };
+ Fixed(FixedRaw, int guts);
+
+ public:
+
+ Fixed();
+ Fixed(const Fixed &a);
+ Fixed(float a);
+ Fixed(double a);
+ Fixed(int a);
+ Fixed(long a);
+
+ Fixed& operator =(const Fixed a);
+ Fixed& operator =(float a);
+ Fixed& operator =(double a);
+ Fixed& operator =(int a);
+ Fixed& operator =(long a);
+
+ operator float();
+ operator double();
+ operator int();
+ operator long();
+ operator unsigned short();
+
+ operator float() const;
+
+ Fixed operator +() const;
+ Fixed operator -() const;
+
+ Fixed operator +(const Fixed a) const;
+ Fixed operator -(const Fixed a) const;
+#if 1
+ // more acurate, using long long
+ Fixed operator *(const Fixed a) const;
+#else
+ // faster, but with only half as many bits right of binary point
+ Fixed operator *(const Fixed a) const;
+#endif
+ Fixed operator /(const Fixed a) const;
+
+ Fixed operator *(unsigned short a) const;
+ Fixed operator *(int a) const;
+
+ Fixed operator +(float a) const;
+ Fixed operator -(float a) const;
+ Fixed operator *(float a) const;
+ Fixed operator /(float a) const;
+
+ Fixed operator +(double a) const;
+ Fixed operator -(double a) const;
+ Fixed operator *(double a) const;
+ Fixed operator /(double a) const;
+
+ Fixed operator >>(int a) const;
+ Fixed operator <<(int a) const;
+
+ Fixed& operator +=(Fixed a);
+ Fixed& operator -=(Fixed a);
+ Fixed& operator *=(Fixed a);
+ Fixed& operator /=(Fixed a);
+
+ Fixed& operator +=(int a);
+ Fixed& operator -=(int a);
+ Fixed& operator *=(int a);
+ Fixed& operator /=(int a);
+
+ Fixed& operator +=(long a);
+ Fixed& operator -=(long a);
+ Fixed& operator *=(long a);
+ Fixed& operator /=(long a);
+
+ Fixed& operator +=(float a);
+ Fixed& operator -=(float a);
+ Fixed& operator *=(float a);
+ Fixed& operator /=(float a);
+
+ Fixed& operator +=(double a);
+ Fixed& operator -=(double a);
+ Fixed& operator *=(double a);
+ Fixed& operator /=(double a);
+
+ bool operator ==(const Fixed a) const;
+ bool operator !=(const Fixed a) const;
+ bool operator <=(const Fixed a) const;
+ bool operator >=(const Fixed a) const;
+ bool operator <(const Fixed a) const;
+ bool operator >(const Fixed a) const;
+
+ bool operator ==(float a) const;
+ bool operator !=(float a) const;
+ bool operator <=(float a) const;
+ bool operator >=(float a) const;
+ bool operator <(float a) const;
+ bool operator >(float a) const;
+
+ bool operator ==(double a) const;
+ bool operator !=(double a) const;
+ bool operator <=(double a) const;
+ bool operator >=(double a) const;
+ bool operator <(double a) const;
+ bool operator >(double a) const;
+
+ bool operator >(int a) const;
+ bool operator <(int a) const;
+ bool operator >=(int a) const;
+ bool operator <=(int a) const;
+
+ Fixed abs();
+ Fixed sqrt();
+#ifdef TARGET_IS_NDS
+ Fixed cosf();
+ Fixed sinf();
+ Fixed tanf();
+#endif
+};
+
+//
+// Implementation
+//
+
+inline double Fixed::STEP() { return 1.0 / (1<<BP); } // smallest step we can represent
+
+// for private construction via guts
+inline Fixed::Fixed(FixedRaw, int guts) : g(guts) {}
+
+inline Fixed::Fixed() : g(0) {}
+inline Fixed::Fixed(const Fixed &a) : g( a.g ) {}
+inline Fixed::Fixed(float a) : g( int(a * (float)(1<<BP)) ) {}
+inline Fixed::Fixed(double a) : g( int(a * (double)(1<<BP) ) ) {}
+inline Fixed::Fixed(int a) : g( a << BP ) {}
+inline Fixed::Fixed(long a) : g( a << BP ) {}
+
+inline Fixed& Fixed::operator =(const Fixed a) { g= a.g; return *this; }
+inline Fixed& Fixed::operator =(float a) { g= Fixed(a).g; return *this; }
+inline Fixed& Fixed::operator =(double a) { g= Fixed(a).g; return *this; }
+inline Fixed& Fixed::operator =(int a) { g= Fixed(a).g; return *this; }
+inline Fixed& Fixed::operator =(long a) { g= Fixed(a).g; return *this; }
+
+inline Fixed::operator float() { return g * (float)STEP(); }
+inline Fixed::operator double() { return g * (double)STEP(); }
+inline Fixed::operator int() { return g>>BP; }
+inline Fixed::operator long() { return g>>BP; }
+//#pragma warning(disable: 4244) //HARDWIRE added pragma to prevent VS2005 compilation error
+inline Fixed::operator unsigned short() { return g>>BP; }
+inline Fixed::operator float() const { return g / (float)(1<<BP); }
+
+inline Fixed Fixed::operator +() const { return Fixed(RAW,g); }
+inline Fixed Fixed::operator -() const { return Fixed(RAW,-g); }
+
+inline Fixed Fixed::operator +(const Fixed a) const { return Fixed(RAW, g + a.g); }
+inline Fixed Fixed::operator -(const Fixed a) const { return Fixed(RAW, g - a.g); }
+
+#if 1
+// more acurate, using long long
+inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (int)( ((long long)g * (long long)a.g ) >> BP)); }
+
+#elif 0
+
+// check for overflow and figure out where. Must specify -rdynamic in linker
+#include <execinfo.h>
+#include <signal.h>
+#include <exception>
+
+inline Fixed Fixed::operator *(const Fixed a) const {
+ long long x = ((long long)g * (long long)a.g );
+ if(x > 0x7fffffffffffLL || x < -0x7fffffffffffLL) {
+ printf("overflow");
+ void *array[2];
+ int nSize = backtrace(array, 2);
+ char **symbols = backtrace_symbols(array, nSize);
+ for(int i=0; i<nSize; i++) {
+ printf(" %s", symbols[i]);
+ }
+ printf("\n");
+ }
+ return Fixed(RAW, (int)(x>>BP));
+}
+
+#else
+// faster, but with only half as many bits right of binary point
+inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (g>>BPhalf) * (a.g>>BPhalf) ); }
+#endif
+
+
+#ifdef TARGET_IS_NDS
+// Division using the DS's maths coprocessor
+inline Fixed Fixed::operator /(const Fixed a) const
+{
+ //printf("%d %d\n", (long long)g << BP, a.g);
+ return Fixed(RAW, int( div64((long long)g << BP, a.g) ) );
+}
+#else
+inline Fixed Fixed::operator /(const Fixed a) const
+{
+ return Fixed(RAW, int( (((long long)g << BP2) / (long long)(a.g)) >> BP) );
+ //return Fixed(RAW, int( (((long long)g << BP) / (long long)(a.g)) ) );
+}
+#endif
+
+inline Fixed Fixed::operator *(unsigned short a) const { return operator*(Fixed(a)); }
+inline Fixed Fixed::operator *(int a) const { return operator*(Fixed(a)); }
+
+inline Fixed Fixed::operator +(float a) const { return Fixed(RAW, g + Fixed(a).g); }
+inline Fixed Fixed::operator -(float a) const { return Fixed(RAW, g - Fixed(a).g); }
+inline Fixed Fixed::operator *(float a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
+//inline Fixed Fixed::operator /(float a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
+inline Fixed Fixed::operator /(float a) const { return operator/(Fixed(a)); }
+
+inline Fixed Fixed::operator +(double a) const { return Fixed(RAW, g + Fixed(a).g); }
+inline Fixed Fixed::operator -(double a) const { return Fixed(RAW, g - Fixed(a).g); }
+inline Fixed Fixed::operator *(double a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
+//inline Fixed Fixed::operator /(double a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
+inline Fixed Fixed::operator /(double a) const { return operator/(Fixed(a)); }
+
+inline Fixed Fixed::operator >>(int a) const { return Fixed(RAW, g >> a); }
+inline Fixed Fixed::operator <<(int a) const { return Fixed(RAW, g << a); }
+
+inline Fixed& Fixed::operator +=(Fixed a) { return *this = *this + a; }
+inline Fixed& Fixed::operator -=(Fixed a) { return *this = *this - a; }
+inline Fixed& Fixed::operator *=(Fixed a) { return *this = *this * a; }
+//inline Fixed& Fixed::operator /=(Fixed a) { return *this = *this / a; }
+inline Fixed& Fixed::operator /=(Fixed a) { return *this = operator/(a); }
+
+inline Fixed& Fixed::operator +=(int a) { return *this = *this + (Fixed)a; }
+inline Fixed& Fixed::operator -=(int a) { return *this = *this - (Fixed)a; }
+inline Fixed& Fixed::operator *=(int a) { return *this = *this * (Fixed)a; }
+//inline Fixed& Fixed::operator /=(int a) { return *this = *this / (Fixed)a; }
+inline Fixed& Fixed::operator /=(int a) { return *this = operator/((Fixed)a); }
+
+inline Fixed& Fixed::operator +=(long a) { return *this = *this + (Fixed)a; }
+inline Fixed& Fixed::operator -=(long a) { return *this = *this - (Fixed)a; }
+inline Fixed& Fixed::operator *=(long a) { return *this = *this * (Fixed)a; }
+//inline Fixed& Fixed::operator /=(long a) { return *this = *this / (Fixed)a; }
+inline Fixed& Fixed::operator /=(long a) { return *this = operator/((Fixed)a); }
+
+inline Fixed& Fixed::operator +=(float a) { return *this = *this + a; }
+inline Fixed& Fixed::operator -=(float a) { return *this = *this - a; }
+inline Fixed& Fixed::operator *=(float a) { return *this = *this * a; }
+//inline Fixed& Fixed::operator /=(float a) { return *this = *this / a; }
+inline Fixed& Fixed::operator /=(float a) { return *this = operator/(a); }
+
+inline Fixed& Fixed::operator +=(double a) { return *this = *this + a; }
+inline Fixed& Fixed::operator -=(double a) { return *this = *this - a; }
+inline Fixed& Fixed::operator *=(double a) { return *this = *this * a; }
+//inline Fixed& Fixed::operator /=(double a) { return *this = *this / a; }
+inline Fixed& Fixed::operator /=(double a) { return *this = operator/(a); }
+
+inline Fixed operator +(int a, const Fixed b) { return Fixed(a)+b; }
+inline Fixed operator -(int a, const Fixed b) { return Fixed(a)-b; }
+inline Fixed operator *(int a, const Fixed b) { return Fixed(a)*b; }
+inline Fixed operator /(int a, const Fixed b) { return Fixed(a)/b; };
+
+inline Fixed operator +(float a, const Fixed b) { return Fixed(a)+b; }
+inline Fixed operator -(float a, const Fixed b) { return Fixed(a)-b; }
+inline Fixed operator *(float a, const Fixed b) { return Fixed(a)*b; }
+inline Fixed operator /(float a, const Fixed b) { return Fixed(a)/b; }
+
+inline bool Fixed::operator ==(const Fixed a) const { return g == a.g; }
+inline bool Fixed::operator !=(const Fixed a) const { return g != a.g; }
+inline bool Fixed::operator <=(const Fixed a) const { return g <= a.g; }
+inline bool Fixed::operator >=(const Fixed a) const { return g >= a.g; }
+inline bool Fixed::operator <(const Fixed a) const { return g < a.g; }
+inline bool Fixed::operator >(const Fixed a) const { return g > a.g; }
+
+inline bool Fixed::operator ==(float a) const { return g == Fixed(a).g; }
+inline bool Fixed::operator !=(float a) const { return g != Fixed(a).g; }
+inline bool Fixed::operator <=(float a) const { return g <= Fixed(a).g; }
+inline bool Fixed::operator >=(float a) const { return g >= Fixed(a).g; }
+inline bool Fixed::operator <(float a) const { return g < Fixed(a).g; }
+inline bool Fixed::operator >(float a) const { return g > Fixed(a).g; }
+
+inline bool Fixed::operator ==(double a) const { return g == Fixed(a).g; }
+inline bool Fixed::operator !=(double a) const { return g != Fixed(a).g; }
+inline bool Fixed::operator <=(double a) const { return g <= Fixed(a).g; }
+inline bool Fixed::operator >=(double a) const { return g >= Fixed(a).g; }
+inline bool Fixed::operator <(double a) const { return g < Fixed(a).g; }
+inline bool Fixed::operator >(double a) const { return g > Fixed(a).g; }
+
+inline bool Fixed::operator >(int a) const { return g > Fixed(a).g; }
+inline bool Fixed::operator <(int a) const { return g < Fixed(a).g; }
+inline bool Fixed::operator >=(int a) const{ return g >= Fixed(a).g; };
+inline bool Fixed::operator <=(int a) const{ return g <= Fixed(a).g; };
+
+inline bool operator ==(float a, const Fixed b) { return Fixed(a) == b; }
+inline bool operator !=(float a, const Fixed b) { return Fixed(a) != b; }
+inline bool operator <=(float a, const Fixed b) { return Fixed(a) <= b; }
+inline bool operator >=(float a, const Fixed b) { return Fixed(a) >= b; }
+inline bool operator <(float a, const Fixed b) { return Fixed(a) < b; }
+inline bool operator >(float a, const Fixed b) { return Fixed(a) > b; }
+
+inline Fixed operator +(double a, const Fixed b) { return Fixed(a)+b; }
+inline Fixed operator -(double a, const Fixed b) { return Fixed(a)-b; }
+inline Fixed operator *(double a, const Fixed b) { return Fixed(a)*b; }
+inline Fixed operator /(double a, const Fixed b) { return Fixed(a)/b; }
+
+inline bool operator ==(double a, const Fixed b) { return Fixed(a) == b; }
+inline bool operator !=(double a, const Fixed b) { return Fixed(a) != b; }
+inline bool operator <=(double a, const Fixed b) { return Fixed(a) <= b; }
+inline bool operator >=(double a, const Fixed b) { return Fixed(a) >= b; }
+inline bool operator <(double a, const Fixed b) { return Fixed(a) < b; }
+inline bool operator >(double a, const Fixed b) { return Fixed(a) > b; }
+
+inline bool operator ==(int a, const Fixed b) { return Fixed(a) == b; }
+inline bool operator !=(int a, const Fixed b) { return Fixed(a) != b; }
+inline bool operator <=(int a, const Fixed b) { return Fixed(a) <= b; }
+inline bool operator >=(int a, const Fixed b) { return Fixed(a) >= b; }
+inline bool operator <(int a, const Fixed b) { return Fixed(a) < b; }
+inline bool operator >(int a, const Fixed b) { return Fixed(a) > b; }
+
+inline int& operator +=(int& a, const Fixed b) { a = (Fixed)a + b; return a; }
+inline int& operator -=(int& a, const Fixed b) { a = (Fixed)a - b; return a; }
+inline int& operator *=(int& a, const Fixed b) { a = (Fixed)a * b; return a; }
+inline int& operator /=(int& a, const Fixed b) { a = (Fixed)a / b; return a; }
+
+inline long& operator +=(long& a, const Fixed b) { a = (Fixed)a + b; return a; }
+inline long& operator -=(long& a, const Fixed b) { a = (Fixed)a - b; return a; }
+inline long& operator *=(long& a, const Fixed b) { a = (Fixed)a * b; return a; }
+inline long& operator /=(long& a, const Fixed b) { a = (Fixed)a / b; return a; }
+
+inline float& operator +=(float& a, const Fixed b) { a = a + b; return a; }
+inline float& operator -=(float& a, const Fixed b) { a = a - b; return a; }
+inline float& operator *=(float& a, const Fixed b) { a = a * b; return a; }
+inline float& operator /=(float& a, const Fixed b) { a = a / b; return a; }
+
+inline double& operator +=(double& a, const Fixed b) { a = a + b; return a; }
+inline double& operator -=(double& a, const Fixed b) { a = a - b; return a; }
+inline double& operator *=(double& a, const Fixed b) { a = a * b; return a; }
+inline double& operator /=(double& a, const Fixed b) { a = a / b; return a; }
+
+inline Fixed Fixed::abs() { return (g>0) ? Fixed(RAW, g) : Fixed(RAW, -g); }
+inline Fixed abs(Fixed f) { return f.abs(); }
+
+//inline Fixed atan2(Fixed a, Fixed b) { return atan2f((float) a, (float) b); }
+inline Fixed atan2(Fixed y, Fixed x)
+{
+ Fixed abs_y = y.abs() + FIXED_EPSILON; // avoid 0/0
+ Fixed r, angle;
+
+ if(x >= 0.0f) {
+ r = (x - abs_y) / (x + abs_y);
+ angle = 3.1415926/4.0;
+ } else {
+ r = (x + abs_y) / (abs_y - x);
+ angle = 3.0*3.1415926/4.0;
+ }
+ angle += Fixed(0.1963) * (r * r * r) - Fixed(0.9817) * r;
+ return (y < 0) ? -angle : angle;
+}
+
+#if TARGET_IS_NDS
+
+static inline long nds_sqrt64(long long a)
+{
+ SQRT_CR = SQRT_64;
+ while(SQRT_CR & SQRT_BUSY);
+ SQRT_PARAM64 = a;
+ while(SQRT_CR & SQRT_BUSY);
+
+ return SQRT_RESULT32;
+}
+
+static inline int32 div6464(int64 num, int64 den)
+{
+ DIV_CR = DIV_64_64;
+ while(DIV_CR & DIV_BUSY);
+ DIV_NUMERATOR64 = num;
+ DIV_DENOMINATOR64 = den;
+ while(DIV_CR & DIV_BUSY);
+
+ return (DIV_RESULT32);
+}
+
+inline Fixed Fixed::sqrt()
+{
+ return Fixed(RAW, nds_sqrt64(((long long)(g))<<BP));
+}
+#else
+inline Fixed Fixed::sqrt()
+{
+ long long m, root = 0, left = (long long)g<<FIXED_BP;
+ for ( m = (long long)1<<( (sizeof(long long)<<3) - 2); m; m >>= 2 )
+ {
+ if ( ( left & -m ) > root )
+ left -= ( root += m ), root += m;
+ root >>= 1;
+ }
+ return Fixed(RAW, root);
+}
+#endif
+
+inline Fixed sqrt(Fixed a) { return a.sqrt(); }
+inline Fixed sqrtf(Fixed a) { return a.sqrt(); }
+
+#endif
+
+#ifdef TARGET_IS_NDS
+// Use the libnds lookup tables for trigonometry functions
+inline Fixed Fixed::cosf() {
+ int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
+ if(idx < 0)
+ idx += 512;
+ return Fixed(RAW, COS_bin[idx] << 4);
+}
+inline Fixed cosf(Fixed x) { return x.cosf(); }
+inline Fixed Fixed::sinf() {
+ int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
+ if(idx < 0)
+ idx += 512;
+ return Fixed(RAW, SIN_bin[idx] << 4);
+}
+inline Fixed sinf(Fixed x) { return x.sinf(); }
+inline Fixed Fixed::tanf() {
+ int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
+ if(idx < 0)
+ idx += 512;
+ return Fixed(RAW, TAN_bin[idx] << 4);
+}
+inline Fixed tanf(Fixed x) { return x.tanf(); }
+
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2BlockAllocator.h"
+#include <cstdlib>
+#include <memory>
+#include <climits>
+#include <string.h>
+
+int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =
+{
+ 16, // 0
+ 32, // 1
+ 64, // 2
+ 96, // 3
+ 128, // 4
+ 160, // 5
+ 192, // 6
+ 224, // 7
+ 256, // 8
+ 320, // 9
+ 384, // 10
+ 448, // 11
+ 512, // 12
+ 640, // 13
+};
+uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
+bool b2BlockAllocator::s_blockSizeLookupInitialized;
+
+struct b2Chunk
+{
+ int32 blockSize;
+ b2Block* blocks;
+};
+
+struct b2Block
+{
+ b2Block* next;
+};
+
+b2BlockAllocator::b2BlockAllocator()
+{
+ b2Assert(b2_blockSizes < UCHAR_MAX);
+
+ m_chunkSpace = b2_chunkArrayIncrement;
+ m_chunkCount = 0;
+ m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+
+ memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+ memset(m_freeLists, 0, sizeof(m_freeLists));
+
+ if (s_blockSizeLookupInitialized == false)
+ {
+ int32 j = 0;
+ for (int32 i = 1; i <= b2_maxBlockSize; ++i)
+ {
+ b2Assert(j < b2_blockSizes);
+ if (i <= s_blockSizes[j])
+ {
+ s_blockSizeLookup[i] = (uint8)j;
+ }
+ else
+ {
+ ++j;
+ s_blockSizeLookup[i] = (uint8)j;
+ }
+ }
+
+ s_blockSizeLookupInitialized = true;
+ }
+}
+
+b2BlockAllocator::~b2BlockAllocator()
+{
+ for (int32 i = 0; i < m_chunkCount; ++i)
+ {
+ b2Free(m_chunks[i].blocks);
+ }
+
+ b2Free(m_chunks);
+}
+
+void* b2BlockAllocator::Allocate(int32 size)
+{
+ if (size == 0)
+ return NULL;
+
+ b2Assert(0 < size && size <= b2_maxBlockSize);
+
+ int32 index = s_blockSizeLookup[size];
+ b2Assert(0 <= index && index < b2_blockSizes);
+
+ if (m_freeLists[index])
+ {
+ b2Block* block = m_freeLists[index];
+ m_freeLists[index] = block->next;
+ return block;
+ }
+ else
+ {
+ if (m_chunkCount == m_chunkSpace)
+ {
+ b2Chunk* oldChunks = m_chunks;
+ m_chunkSpace += b2_chunkArrayIncrement;
+ m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+ memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
+ memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
+ b2Free(oldChunks);
+ }
+
+ b2Chunk* chunk = m_chunks + m_chunkCount;
+ chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
+#if defined(_DEBUG)
+ memset(chunk->blocks, 0xcd, b2_chunkSize);
+#endif
+ int32 blockSize = s_blockSizes[index];
+ chunk->blockSize = blockSize;
+ int32 blockCount = b2_chunkSize / blockSize;
+ b2Assert(blockCount * blockSize <= b2_chunkSize);
+ for (int32 i = 0; i < blockCount - 1; ++i)
+ {
+ b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
+ b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
+ block->next = next;
+ }
+ b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
+ last->next = NULL;
+
+ m_freeLists[index] = chunk->blocks->next;
+ ++m_chunkCount;
+
+ return chunk->blocks;
+ }
+}
+
+void b2BlockAllocator::Free(void* p, int32 size)
+{
+ if (size == 0)
+ {
+ return;
+ }
+
+ b2Assert(0 < size && size <= b2_maxBlockSize);
+
+ int32 index = s_blockSizeLookup[size];
+ b2Assert(0 <= index && index < b2_blockSizes);
+
+#ifdef _DEBUG
+ // Verify the memory address and size is valid.
+ int32 blockSize = s_blockSizes[index];
+ bool found = false;
+ int32 gap = (int32)((int8*)&m_chunks->blocks - (int8*)m_chunks);
+ for (int32 i = 0; i < m_chunkCount; ++i)
+ {
+ b2Chunk* chunk = m_chunks + i;
+ if (chunk->blockSize != blockSize)
+ {
+ b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks ||
+ (int8*)chunk->blocks + b2_chunkSize + gap <= (int8*)p);
+ }
+ else
+ {
+ if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
+ {
+ found = true;
+ }
+ }
+ }
+
+ b2Assert(found);
+
+ memset(p, 0xfd, blockSize);
+#endif
+
+ b2Block* block = (b2Block*)p;
+ block->next = m_freeLists[index];
+ m_freeLists[index] = block;
+}
+
+void b2BlockAllocator::Clear()
+{
+ for (int32 i = 0; i < m_chunkCount; ++i)
+ {
+ b2Free(m_chunks[i].blocks);
+ }
+
+ m_chunkCount = 0;
+ memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+
+ memset(m_freeLists, 0, sizeof(m_freeLists));
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_BLOCK_ALLOCATOR_H
+#define B2_BLOCK_ALLOCATOR_H
+
+#include "b2Settings.h"
+
+const int32 b2_chunkSize = 4096;
+const int32 b2_maxBlockSize = 640;
+const int32 b2_blockSizes = 14;
+const int32 b2_chunkArrayIncrement = 128;
+
+struct b2Block;
+struct b2Chunk;
+
+// This is a small object allocator used for allocating small
+// objects that persist for more than one time step.
+// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
+class b2BlockAllocator
+{
+public:
+ b2BlockAllocator();
+ ~b2BlockAllocator();
+
+ void* Allocate(int32 size);
+ void Free(void* p, int32 size);
+
+ void Clear();
+
+private:
+
+ b2Chunk* m_chunks;
+ int32 m_chunkCount;
+ int32 m_chunkSpace;
+
+ b2Block* m_freeLists[b2_blockSizes];
+
+ static int32 s_blockSizes[b2_blockSizes];
+ static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];
+ static bool s_blockSizeLookupInitialized;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Math.h"
+
+const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
+const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f);
+const b2XForm b2XForm_identity(b2Vec2_zero, b2Mat22_identity);
+
+void b2Sweep::GetXForm(b2XForm* xf, float32 t) const
+{
+ // center = p + R * localCenter
+ if (1.0f - t0 > B2_FLT_EPSILON)
+ {
+ float32 alpha = (t - t0) / (1.0f - t0);
+ xf->position = (1.0f - alpha) * c0 + alpha * c;
+ float32 angle = (1.0f - alpha) * a0 + alpha * a;
+ xf->R.Set(angle);
+ }
+ else
+ {
+ xf->position = c;
+ xf->R.Set(a);
+ }
+
+ // Shift to origin
+ xf->position -= b2Mul(xf->R, localCenter);
+}
+
+void b2Sweep::Advance(float32 t)
+{
+ if (t0 < t && 1.0f - t0 > B2_FLT_EPSILON)
+ {
+ float32 alpha = (t - t0) / (1.0f - t0);
+ c0 = (1.0f - alpha) * c0 + alpha * c;
+ a0 = (1.0f - alpha) * a0 + alpha * a;
+ t0 = t;
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_MATH_H
+#define B2_MATH_H
+
+#include "b2Settings.h"
+#include <cmath>
+#include <cfloat>
+#include <cstdlib>
+
+#include <stdio.h>
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+
+inline Fixed b2Min(const Fixed& a, const Fixed& b)
+{
+ return a < b ? a : b;
+}
+
+inline Fixed b2Max(const Fixed& a, const Fixed& b)
+{
+ return a > b ? a : b;
+}
+
+inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
+{
+ return b2Max(low, b2Min(a, high));
+}
+
+inline bool b2IsValid(Fixed x)
+{
+ return true;
+}
+
+#define b2Sqrt(x) sqrt(x)
+#define b2Atan2(y, x) atan2(y, x)
+
+#else
+
+/// This function is used to ensure that a floating point number is
+/// not a NaN or infinity.
+inline bool b2IsValid(float32 x)
+{
+#ifdef _MSC_VER
+ return _finite(x) != 0;
+#else
+ return finite(x) != 0;
+#endif
+}
+
+/// This is a approximate yet fast inverse square-root.
+inline float32 b2InvSqrt(float32 x)
+{
+ union
+ {
+ float32 x;
+ int32 i;
+ } convert;
+
+ convert.x = x;
+ float32 xhalf = 0.5f * x;
+ convert.i = 0x5f3759df - (convert.i >> 1);
+ x = convert.x;
+ x = x * (1.5f - xhalf * x * x);
+ return x;
+}
+
+#define b2Sqrt(x) sqrtf(x)
+#define b2Atan2(y, x) atan2f(y, x)
+
+#endif
+
+inline float32 b2Abs(float32 a)
+{
+ return a > 0.0f ? a : -a;
+}
+
+/// A 2D column vector.
+
+struct b2Vec2
+{
+ /// Default constructor does nothing (for performance).
+ b2Vec2() {}
+
+ /// Construct using coordinates.
+ b2Vec2(float32 x, float32 y) : x(x), y(y) {}
+
+ /// Set this vector to all zeros.
+ void SetZero() { x = 0.0f; y = 0.0f; }
+
+ /// Set this vector to some specified coordinates.
+ void Set(float32 x_, float32 y_) { x = x_; y = y_; }
+
+ /// Negate this vector.
+ b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
+
+ /// Add a vector to this vector.
+ void operator += (const b2Vec2& v)
+ {
+ x += v.x; y += v.y;
+ }
+
+ /// Subtract a vector from this vector.
+ void operator -= (const b2Vec2& v)
+ {
+ x -= v.x; y -= v.y;
+ }
+
+ /// Multiply this vector by a scalar.
+ void operator *= (float32 a)
+ {
+ x *= a; y *= a;
+ }
+
+ /// Get the length of this vector (the norm).
+ float32 Length() const
+ {
+#ifdef TARGET_FLOAT32_IS_FIXED
+ float est = b2Abs(x) + b2Abs(y);
+ if(est == 0.0f) {
+ return 0.0;
+ } else if(est < 0.1) {
+ return (1.0/256.0) * b2Vec2(x<<8, y<<8).Length();
+ } else if(est < 180.0f) {
+ return b2Sqrt(x * x + y * y);
+ } else {
+ return 256.0 * (b2Vec2(x>>8, y>>8).Length());
+ }
+#else
+ return b2Sqrt(x * x + y * y);
+#endif
+ }
+
+ /// Get the length squared. For performance, use this instead of
+ /// b2Vec2::Length (if possible).
+ float32 LengthSquared() const
+ {
+ return x * x + y * y;
+ }
+
+ /// Convert this vector into a unit vector. Returns the length.
+#ifdef TARGET_FLOAT32_IS_FIXED
+ float32 Normalize()
+ {
+ float32 length = Length();
+ if (length < B2_FLT_EPSILON)
+ {
+ return 0.0f;
+ }
+#ifdef NORMALIZE_BY_INVERT_MULTIPLY
+ if (length < (1.0/16.0)) {
+ x = x << 4;
+ y = y << 4;
+ return (1.0/16.0)*Normalize();
+ } else if(length > 16.0) {
+ x = x >> 4;
+ y = y >> 4;
+ return 16.0*Normalize();
+ }
+ float32 invLength = 1.0f / length;
+ x *= invLength;
+ y *= invLength;
+#else
+ x /= length;
+ y /= length;
+#endif
+ return length;
+ }
+#else
+ float32 Normalize()
+ {
+ float32 length = Length();
+ if (length < B2_FLT_EPSILON)
+ {
+ return 0.0f;
+ }
+ float32 invLength = 1.0f / length;
+ x *= invLength;
+ y *= invLength;
+
+ return length;
+ }
+#endif
+
+ /// Does this vector contain finite coordinates?
+ bool IsValid() const
+ {
+ return b2IsValid(x) && b2IsValid(y);
+ }
+
+ float32 x, y;
+};
+
+/// A 2-by-2 matrix. Stored in column-major order.
+struct b2Mat22
+{
+ /// The default constructor does nothing (for performance).
+ b2Mat22() {}
+
+ /// Construct this matrix using columns.
+ b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
+ {
+ col1 = c1;
+ col2 = c2;
+ }
+
+ /// Construct this matrix using scalars.
+ b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
+ {
+ col1.x = a11; col1.y = a21;
+ col2.x = a12; col2.y = a22;
+ }
+
+ /// Construct this matrix using an angle. This matrix becomes
+ /// an orthonormal rotation matrix.
+ explicit b2Mat22(float32 angle)
+ {
+ float32 c = cosf(angle), s = sinf(angle);
+ col1.x = c; col2.x = -s;
+ col1.y = s; col2.y = c;
+ }
+
+ /// Initialize this matrix using columns.
+ void Set(const b2Vec2& c1, const b2Vec2& c2)
+ {
+ col1 = c1;
+ col2 = c2;
+ }
+
+ /// Initialize this matrix using an angle. This matrix becomes
+ /// an orthonormal rotation matrix.
+ void Set(float32 angle)
+ {
+ float32 c = cosf(angle), s = sinf(angle);
+ col1.x = c; col2.x = -s;
+ col1.y = s; col2.y = c;
+ }
+
+ /// Set this to the identity matrix.
+ void SetIdentity()
+ {
+ col1.x = 1.0f; col2.x = 0.0f;
+ col1.y = 0.0f; col2.y = 1.0f;
+ }
+
+ /// Set this matrix to all zeros.
+ void SetZero()
+ {
+ col1.x = 0.0f; col2.x = 0.0f;
+ col1.y = 0.0f; col2.y = 0.0f;
+ }
+
+ /// Extract the angle from this matrix (assumed to be
+ /// a rotation matrix).
+ float32 GetAngle() const
+ {
+ return b2Atan2(col1.y, col1.x);
+ }
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+
+ /// Compute the inverse of this matrix, such that inv(A) * A = identity.
+ b2Mat22 Invert() const
+ {
+ float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
+ float32 det = a * d - b * c;
+ b2Mat22 B;
+ int n = 0;
+
+ if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
+ {
+ n = 3;
+ a = a<<n; b = b<<n;
+ c = c<<n; d = d<<n;
+ det = a * d - b * c;
+ b2Assert(det != 0.0f);
+ det = float32(1) / det;
+ B.col1.x = ( det * d) << n; B.col2.x = (-det * b) << n;
+ B.col1.y = (-det * c) << n; B.col2.y = ( det * a) << n;
+ }
+ else
+ {
+ n = (b2Abs(det) >= 16.0)? 4 : 0;
+ b2Assert(det != 0.0f);
+ det = float32(1<<n) / det;
+ B.col1.x = ( det * d) >> n; B.col2.x = (-det * b) >> n;
+ B.col1.y = (-det * c) >> n; B.col2.y = ( det * a) >> n;
+ }
+
+ return B;
+ }
+
+ // Solve A * x = b
+ b2Vec2 Solve(const b2Vec2& b) const
+ {
+ float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+ float32 det = a11 * a22 - a12 * a21;
+ int n = 0;
+ b2Vec2 x;
+
+
+ if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
+ {
+ n = 3;
+ a11 = col1.x<<n; a12 = col2.x<<n;
+ a21 = col1.y<<n; a22 = col2.y<<n;
+ det = a11 * a22 - a12 * a21;
+ b2Assert(det != 0.0f);
+ det = float32(1) / det;
+ x.x = (det * (a22 * b.x - a12 * b.y)) << n;
+ x.y = (det * (a11 * b.y - a21 * b.x)) << n;
+ }
+ else
+ {
+ n = (b2Abs(det) >= 16.0) ? 4 : 0;
+ b2Assert(det != 0.0f);
+ det = float32(1<<n) / det;
+ x.x = (det * (a22 * b.x - a12 * b.y)) >> n;
+ x.y = (det * (a11 * b.y - a21 * b.x)) >> n;
+ }
+
+ return x;
+ }
+
+#else
+ b2Mat22 Invert() const
+ {
+ float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
+ b2Mat22 B;
+ float32 det = a * d - b * c;
+ b2Assert(det != 0.0f);
+ det = float32(1.0f) / det;
+ B.col1.x = det * d; B.col2.x = -det * b;
+ B.col1.y = -det * c; B.col2.y = det * a;
+ return B;
+ }
+
+ /// Solve A * x = b, where b is a column vector. This is more efficient
+ /// than computing the inverse in one-shot cases.
+ b2Vec2 Solve(const b2Vec2& b) const
+ {
+ float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+ float32 det = a11 * a22 - a12 * a21;
+ b2Assert(det != 0.0f);
+ det = 1.0f / det;
+ b2Vec2 x;
+ x.x = det * (a22 * b.x - a12 * b.y);
+ x.y = det * (a11 * b.y - a21 * b.x);
+ return x;
+ }
+#endif
+
+ b2Vec2 col1, col2;
+};
+
+/// A transform contains translation and rotation. It is used to represent
+/// the position and orientation of rigid frames.
+struct b2XForm
+{
+ /// The default constructor does nothing (for performance).
+ b2XForm() {}
+
+ /// Initialize using a position vector and a rotation matrix.
+ b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
+
+ /// Set this to the identity transform.
+ void SetIdentity()
+ {
+ position.SetZero();
+ R.SetIdentity();
+ }
+
+ b2Vec2 position;
+ b2Mat22 R;
+};
+
+/// This describes the motion of a body/shape for TOI computation.
+/// Shapes are defined with respect to the body origin, which may
+/// no coincide with the center of mass. However, to support dynamics
+/// we must interpolate the center of mass position.
+struct b2Sweep
+{
+ /// Get the interpolated transform at a specific time.
+ /// @param t the normalized time in [0,1].
+ void GetXForm(b2XForm* xf, float32 t) const;
+
+ /// Advance the sweep forward, yielding a new initial state.
+ /// @param t the new initial time.
+ void Advance(float32 t);
+
+ b2Vec2 localCenter; ///< local center of mass position
+ b2Vec2 c0, c; ///< center world positions
+ float32 a0, a; ///< world angles
+ float32 t0; ///< time interval = [t0,1], where t0 is in [0,1]
+};
+
+
+extern const b2Vec2 b2Vec2_zero;
+extern const b2Mat22 b2Mat22_identity;
+extern const b2XForm b2XForm_identity;
+
+/// Peform the dot product on two vectors.
+inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+
+/// Perform the cross product on two vectors. In 2D this produces a scalar.
+inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
+{
+ return a.x * b.y - a.y * b.x;
+}
+
+/// Perform the cross product on a vector and a scalar. In 2D this produces
+/// a vector.
+inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
+{
+ b2Vec2 v; v.Set(s * a.y, -s * a.x);
+ return v;
+}
+
+/// Perform the cross product on a scalar and a vector. In 2D this produces
+/// a vector.
+inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
+{
+ b2Vec2 v; v.Set(-s * a.y, s * a.x);
+ return v;
+}
+
+/// Multiply a matrix times a vector. If a rotation matrix is provided,
+/// then this transforms the vector from one frame to another.
+inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
+{
+ b2Vec2 u;
+ u.Set(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
+ return u;
+}
+
+/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
+/// then this transforms the vector from one frame to another (inverse transform).
+inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
+{
+ b2Vec2 u;
+ u.Set(b2Dot(v, A.col1), b2Dot(v, A.col2));
+ return u;
+}
+
+/// Add two vectors component-wise.
+inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 v; v.Set(a.x + b.x, a.y + b.y);
+ return v;
+}
+
+/// Subtract two vectors component-wise.
+inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 v; v.Set(a.x - b.x, a.y - b.y);
+ return v;
+}
+
+inline b2Vec2 operator * (float32 s, const b2Vec2& a)
+{
+ b2Vec2 v; v.Set(s * a.x, s * a.y);
+ return v;
+}
+
+inline bool operator == (const b2Vec2& a, const b2Vec2& b)
+{
+ return a.x == b.x && a.y == b.y;
+}
+
+inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 c = a - b;
+ return c.Length();
+}
+
+inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 c = a - b;
+ return b2Dot(c, c);
+}
+
+inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
+{
+ b2Mat22 C;
+ C.Set(A.col1 + B.col1, A.col2 + B.col2);
+ return C;
+}
+
+// A * B
+inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
+{
+ b2Mat22 C;
+ C.Set(b2Mul(A, B.col1), b2Mul(A, B.col2));
+ return C;
+}
+
+// A^T * B
+inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
+{
+ b2Vec2 c1; c1.Set(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
+ b2Vec2 c2; c2.Set(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
+ b2Mat22 C;
+ C.Set(c1, c2);
+ return C;
+}
+
+inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
+{
+ return T.position + b2Mul(T.R, v);
+}
+
+inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& v)
+{
+ return b2MulT(T.R, v - T.position);
+}
+
+inline b2Vec2 b2Abs(const b2Vec2& a)
+{
+ b2Vec2 b; b.Set(b2Abs(a.x), b2Abs(a.y));
+ return b;
+}
+
+inline b2Mat22 b2Abs(const b2Mat22& A)
+{
+ b2Mat22 B;
+ B.Set(b2Abs(A.col1), b2Abs(A.col2));
+ return B;
+}
+
+template <typename T>
+inline T b2Min(T a, T b)
+{
+ return a < b ? a : b;
+}
+
+inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 c;
+ c.x = b2Min(a.x, b.x);
+ c.y = b2Min(a.y, b.y);
+ return c;
+}
+
+template <typename T>
+inline T b2Max(T a, T b)
+{
+ return a > b ? a : b;
+}
+
+inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
+{
+ b2Vec2 c;
+ c.x = b2Max(a.x, b.x);
+ c.y = b2Max(a.y, b.y);
+ return c;
+}
+
+template <typename T>
+inline T b2Clamp(T a, T low, T high)
+{
+ return b2Max(low, b2Min(a, high));
+}
+
+inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
+{
+ return b2Max(low, b2Min(a, high));
+}
+
+template<typename T> inline void b2Swap(T& a, T& b)
+{
+ T tmp = a;
+ a = b;
+ b = tmp;
+}
+
+#define RAND_LIMIT 32767
+
+// Random number in range [-1,1]
+inline float32 b2Random()
+{
+ float32 r = (float32)(rand() & (RAND_LIMIT));
+ r /= RAND_LIMIT;
+ r = 2.0f * r - 1.0f;
+ return r;
+}
+
+/// Random floating point number in range [lo, hi]
+inline float32 b2Random(float32 lo, float32 hi)
+{
+ float32 r = (float32)(rand() & (RAND_LIMIT));
+ r /= RAND_LIMIT;
+ r = (hi - lo) * r + lo;
+ return r;
+}
+
+/// "Next Largest Power of 2
+/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
+/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
+/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
+/// largest power of 2. For a 32-bit value:"
+inline uint32 b2NextPowerOfTwo(uint32 x)
+{
+ x |= (x >> 1);
+ x |= (x >> 2);
+ x |= (x >> 4);
+ x |= (x >> 8);
+ x |= (x >> 16);
+ return x + 1;
+}
+
+inline bool b2IsPowerOfTwo(uint32 x)
+{
+ bool result = x > 0 && (x & (x - 1)) == 0;
+ return result;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Settings.h"
+#include <cstdlib>
+
+b2Version b2_version = {2, 0, 1};
+
+int32 b2_byteCount = 0;
+
+
+
+// Memory allocators. Modify these to use your own allocator.
+void* b2Alloc(int32 size)
+{
+ size += 4;
+ b2_byteCount += size;
+ char* bytes = (char*)malloc(size);
+ *(int32*)bytes = size;
+ return bytes + 4;
+}
+
+void b2Free(void* mem)
+{
+ if (mem == NULL)
+ {
+ return;
+ }
+
+ char* bytes = (char*)mem;
+ bytes -= 4;
+ int32 size = *(int32*)bytes;
+ b2Assert(b2_byteCount >= size);
+ b2_byteCount -= size;
+ free(bytes);
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_SETTINGS_H
+#define B2_SETTINGS_H
+
+#include <assert.h>
+#include <math.h>
+
+#define B2_NOT_USED(x) x
+#define b2Assert(A) assert(A)
+
+
+// need to include NDS jtypes.h instead of
+// usual typedefs because NDS jtypes defines
+// them slightly differently, oh well.
+#ifdef TARGET_IS_NDS
+
+#include "jtypes.h"
+
+#else
+
+typedef signed char int8;
+typedef signed short int16;
+typedef signed int int32;
+typedef unsigned char uint8;
+typedef unsigned short uint16;
+typedef unsigned int uint32;
+
+#endif
+
+#ifdef TARGET_FLOAT32_IS_FIXED
+
+#include "Fixed.h"
+
+typedef Fixed float32;
+#define B2_FLT_MAX FIXED_MAX
+#define B2_FLT_EPSILON FIXED_EPSILON
+#define B2FORCE_SCALE(x) ((x)<<7)
+#define B2FORCE_INV_SCALE(x) ((x)>>7)
+
+#else
+
+typedef float float32;
+#define B2_FLT_MAX FLT_MAX
+#define B2_FLT_EPSILON FLT_EPSILON
+#define B2FORCE_SCALE(x) (x)
+#define B2FORCE_INV_SCALE(x) (x)
+
+#endif
+
+const float32 b2_pi = 3.14159265359f;
+
+/// @file
+/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
+///
+
+// Collision
+const int32 b2_maxManifoldPoints = 2;
+const int32 b2_maxPolygonVertices = 8;
+const int32 b2_maxProxies = 512; // this must be a power of two
+const int32 b2_maxPairs = 8 * b2_maxProxies; // this must be a power of two
+
+// Dynamics
+
+/// A small length used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant.
+const float32 b2_linearSlop = 0.005f; // 0.5 cm
+
+/// A small angle used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant.
+const float32 b2_angularSlop = 2.0f / 180.0f * b2_pi; // 2 degrees
+
+/// Continuous collision detection (CCD) works with core, shrunken shapes. This is the
+/// amount by which shapes are automatically shrunk to work with CCD. This must be
+/// larger than b2_linearSlop.
+const float32 b2_toiSlop = 8.0f * b2_linearSlop;
+
+/// Maximum number of contacts to be handled to solve a TOI island.
+const int32 b2_maxTOIContactsPerIsland = 32;
+
+/// A velocity threshold for elastic collisions. Any collision with a relative linear
+/// velocity below this threshold will be treated as inelastic.
+const float32 b2_velocityThreshold = 1.0f; // 1 m/s
+
+/// The maximum linear position correction used when solving constraints. This helps to
+/// prevent overshoot.
+const float32 b2_maxLinearCorrection = 0.2f; // 20 cm
+
+/// The maximum angular position correction used when solving constraints. This helps to
+/// prevent overshoot.
+const float32 b2_maxAngularCorrection = 8.0f / 180.0f * b2_pi; // 8 degrees
+
+/// The maximum linear velocity of a body. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this.
+#ifdef TARGET_FLOAT32_IS_FIXED
+const float32 b2_maxLinearVelocity = 100.0f;
+#else
+const float32 b2_maxLinearVelocity = 200.0f;
+const float32 b2_maxLinearVelocitySquared = b2_maxLinearVelocity * b2_maxLinearVelocity;
+#endif
+
+/// The maximum angular velocity of a body. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this.
+const float32 b2_maxAngularVelocity = 250.0f;
+#ifndef TARGET_FLOAT32_IS_FIXED
+const float32 b2_maxAngularVelocitySquared = b2_maxAngularVelocity * b2_maxAngularVelocity;
+#endif
+
+/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
+/// that overlap is removed in one time step. However using values close to 1 often lead
+/// to overshoot.
+const float32 b2_contactBaumgarte = 0.2f;
+
+// Sleep
+
+/// The time that a body must be still before it will go to sleep.
+const float32 b2_timeToSleep = 0.5f; // half a second
+
+/// A body cannot sleep if its linear velocity is above this tolerance.
+const float32 b2_linearSleepTolerance = 0.01f; // 1 cm/s
+
+/// A body cannot sleep if its angular velocity is above this tolerance.
+const float32 b2_angularSleepTolerance = 2.0f / 180.0f; // 2 degrees/s
+
+// Memory Allocation
+
+/// The current number of bytes allocated through b2Alloc.
+extern int32 b2_byteCount;
+
+/// Implement this function to use your own memory allocator.
+void* b2Alloc(int32 size);
+
+/// If you implement b2Alloc, you should also implement this function.
+void b2Free(void* mem);
+
+/// Version numbering scheme.
+/// See http://en.wikipedia.org/wiki/Software_versioning
+struct b2Version
+{
+ int32 major; ///< significant changes
+ int32 minor; ///< incremental changes
+ int32 revision; ///< bug fixes
+};
+
+/// Current version.
+extern b2Version b2_version;
+
+/// Friction mixing law. Feel free to customize this.
+inline float32 b2MixFriction(float32 friction1, float32 friction2)
+{
+ return sqrtf(friction1 * friction2);
+}
+
+/// Restitution mixing law. Feel free to customize this.
+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
+{
+ return restitution1 > restitution2 ? restitution1 : restitution2;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2StackAllocator.h"
+#include "b2Math.h"
+
+b2StackAllocator::b2StackAllocator()
+{
+ m_index = 0;
+ m_allocation = 0;
+ m_maxAllocation = 0;
+ m_entryCount = 0;
+}
+
+b2StackAllocator::~b2StackAllocator()
+{
+ b2Assert(m_index == 0);
+ b2Assert(m_entryCount == 0);
+}
+
+void* b2StackAllocator::Allocate(int32 size)
+{
+ b2Assert(m_entryCount < b2_maxStackEntries);
+
+ b2StackEntry* entry = m_entries + m_entryCount;
+ entry->size = size;
+ if (m_index + size > b2_stackSize)
+ {
+ entry->data = (char*)b2Alloc(size);
+ entry->usedMalloc = true;
+ }
+ else
+ {
+ entry->data = m_data + m_index;
+ entry->usedMalloc = false;
+ m_index += size;
+ }
+
+ m_allocation += size;
+ m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
+ ++m_entryCount;
+
+ return entry->data;
+}
+
+void b2StackAllocator::Free(void* p)
+{
+ b2Assert(m_entryCount > 0);
+ b2StackEntry* entry = m_entries + m_entryCount - 1;
+ b2Assert(p == entry->data);
+ if (entry->usedMalloc)
+ {
+ b2Free(p);
+ }
+ else
+ {
+ m_index -= entry->size;
+ }
+ m_allocation -= entry->size;
+ --m_entryCount;
+
+ p = NULL;
+}
+
+int32 b2StackAllocator::GetMaxAllocation() const
+{
+ return m_maxAllocation;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_STACK_ALLOCATOR_H
+#define B2_STACK_ALLOCATOR_H
+
+#include "b2Settings.h"
+
+const int32 b2_stackSize = 100 * 1024; // 100k
+const int32 b2_maxStackEntries = 32;
+
+struct b2StackEntry
+{
+ char* data;
+ int32 size;
+ bool usedMalloc;
+};
+
+// This is a stack allocator used for fast per step allocations.
+// You must nest allocate/free pairs. The code will assert
+// if you try to interleave multiple allocate/free pairs.
+class b2StackAllocator
+{
+public:
+ b2StackAllocator();
+ ~b2StackAllocator();
+
+ void* Allocate(int32 size);
+ void Free(void* p);
+
+ int32 GetMaxAllocation() const;
+
+private:
+
+ char m_data[b2_stackSize];
+ int32 m_index;
+
+ int32 m_allocation;
+ int32 m_maxAllocation;
+
+ b2StackEntry m_entries[b2_maxStackEntries];
+ int32 m_entryCount;
+};
+
+#endif
--- /dev/null
+/*---------------------------------------------------------------------------------
+ $Id: jtypes.h,v 1.17 2007/07/18 05:20:45 wntrmute Exp $
+
+ jtypes.h -- Common types (and a few useful macros)
+
+ Copyright (C) 2005
+ Michael Noland (joat)
+ Jason Rogers (dovoto)
+ Dave Murphy (WinterMute)
+ Chris Double (doublec)
+
+ This software is provided 'as-is', without any express or implied
+ warranty. In no event will the authors be held liable for any
+ damages arising from the use of this software.
+
+ Permission is granted to anyone to use this software for any
+ purpose, including commercial applications, and to alter it and
+ redistribute it freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you
+ must not claim that you wrote the original software. If you use
+ this software in a product, an acknowledgment in the product
+ documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and
+ must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+---------------------------------------------------------------------------------*/
+#ifndef NDS_JTYPES_INCLUDE
+#define NDS_JTYPES_INCLUDE
+//---------------------------------------------------------------------------------
+
+
+#define PACKED __attribute__ ((packed))
+#define packed_struct struct PACKED
+
+//---------------------------------------------------------------------------------
+// libgba compatible section macros
+//---------------------------------------------------------------------------------
+#define ITCM_CODE __attribute__((section(".itcm"), long_call))
+
+#define DTCM_DATA __attribute__((section(".dtcm")))
+#define DTCM_BSS __attribute__((section(".sbss")))
+#define ALIGN(m) __attribute__((aligned (m)))
+
+#define PACKED __attribute__ ((packed))
+#define packed_struct struct PACKED
+
+//---------------------------------------------------------------------------------
+// These are linked to the bin2o macro in the Makefile
+//---------------------------------------------------------------------------------
+#define GETRAW(name) (name)
+#define GETRAWSIZE(name) ((int)name##_size)
+#define GETRAWEND(name) ((int)name##_end)
+
+#ifndef TRUE
+#define TRUE 1
+#define FALSE 0
+#endif
+
+#define BIT(n) (1 << (n))
+
+// define libnds types in terms of stdint
+#include <stdint.h>
+
+typedef uint8_t uint8;
+typedef uint16_t uint16;
+typedef uint32_t uint32;
+typedef uint64_t uint64;
+
+typedef int8_t int8;
+typedef int16_t int16;
+typedef int32_t int32;
+typedef int64_t int64;
+
+//typedef float float32;
+typedef double float64;
+
+typedef volatile uint8_t vuint8;
+typedef volatile uint16_t vuint16;
+typedef volatile uint32_t vuint32;
+typedef volatile uint64_t vuint64;
+
+typedef volatile int8_t vint8;
+typedef volatile int16_t vint16;
+typedef volatile int32_t vint32;
+typedef volatile int64_t vint64;
+
+typedef volatile float vfloat32;
+typedef volatile float64 vfloat64;
+
+typedef uint8_t byte;
+
+typedef uint8_t u8;
+typedef uint16_t u16;
+typedef uint32_t u32;
+typedef uint64_t u64;
+
+typedef int8_t s8;
+typedef int16_t s16;
+typedef int32_t s32;
+typedef int64_t s64;
+
+typedef volatile u8 vu8;
+typedef volatile u16 vu16;
+typedef volatile u32 vu32;
+typedef volatile u64 vu64;
+
+typedef volatile s8 vs8;
+typedef volatile s16 vs16;
+typedef volatile s32 vs32;
+typedef volatile s64 vs64;
+
+typedef struct touchPosition {
+ int16 x;
+ int16 y;
+ int16 px;
+ int16 py;
+ int16 z1;
+ int16 z2;
+} touchPosition;
+
+
+#ifndef __cplusplus
+/** C++ compatible bool for C
+
+*/
+typedef enum { false, true } bool;
+#endif
+
+// Handy function pointer typedefs
+typedef void ( * IntFn)(void);
+typedef void (* VoidFunctionPointer)(void);
+typedef void (* fp)(void);
+
+//---------------------------------------------------------------------------------
+#endif
+//---------------------------------------------------------------------------------
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2CircleContact.h"
+#include "../b2Body.h"
+#include "../b2WorldCallbacks.h"
+#include "../../Common/b2BlockAllocator.h"
+
+#include <new>
+#include <string.h>
+
+b2Contact* b2CircleContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2CircleContact));
+ return new (mem) b2CircleContact(shape1, shape2);
+}
+
+void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2CircleContact*)contact)->~b2CircleContact();
+ allocator->Free(contact, sizeof(b2CircleContact));
+}
+
+b2CircleContact::b2CircleContact(b2Shape* s1, b2Shape* s2)
+: b2Contact(s1, s2)
+{
+ b2Assert(m_shape1->GetType() == e_circleShape);
+ b2Assert(m_shape2->GetType() == e_circleShape);
+ m_manifold.pointCount = 0;
+ m_manifold.points[0].normalImpulse = 0.0f;
+ m_manifold.points[0].tangentImpulse = 0.0f;
+}
+
+void b2CircleContact::Evaluate(b2ContactListener* listener)
+{
+ b2Body* b1 = m_shape1->GetBody();
+ b2Body* b2 = m_shape2->GetBody();
+
+ b2Manifold m0;
+ memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+
+ b2CollideCircles(&m_manifold, (b2CircleShape*)m_shape1, b1->GetXForm(), (b2CircleShape*)m_shape2, b2->GetXForm());
+
+ b2ContactPoint cp;
+ cp.shape1 = m_shape1;
+ cp.shape2 = m_shape2;
+ cp.friction = m_friction;
+ cp.restitution = m_restitution;
+
+ if (m_manifold.pointCount > 0)
+ {
+ m_manifoldCount = 1;
+ b2ManifoldPoint* mp = m_manifold.points + 0;
+
+ if (m0.pointCount == 0)
+ {
+ mp->normalImpulse = 0.0f;
+ mp->tangentImpulse = 0.0f;
+
+ if (listener)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = mp->id;
+ listener->Add(&cp);
+ }
+ }
+ else
+ {
+ b2ManifoldPoint* mp0 = m0.points + 0;
+ mp->normalImpulse = mp0->normalImpulse;
+ mp->tangentImpulse = mp0->tangentImpulse;
+
+ if (listener)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = mp->id;
+ listener->Persist(&cp);
+ }
+ }
+ }
+ else
+ {
+ m_manifoldCount = 0;
+ if (m0.pointCount > 0 && listener)
+ {
+ b2ManifoldPoint* mp0 = m0.points + 0;
+ cp.position = b1->GetWorldPoint(mp0->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m0.normal;
+ cp.separation = mp0->separation;
+ cp.id = mp0->id;
+ listener->Remove(&cp);
+ }
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CIRCLE_CONTACT_H
+#define CIRCLE_CONTACT_H
+
+#include "../../Common/b2Math.h"
+#include "../../Collision/b2Collision.h"
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2CircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2CircleContact(b2Shape* shape1, b2Shape* shape2);
+ ~b2CircleContact() {}
+
+ void Evaluate(b2ContactListener* listener);
+ b2Manifold* GetManifolds()
+ {
+ return &m_manifold;
+ }
+
+ b2Manifold m_manifold;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Contact.h"
+#include "b2CircleContact.h"
+#include "b2PolyAndCircleContact.h"
+#include "b2PolyContact.h"
+#include "b2ContactSolver.h"
+#include "../../Collision/b2Collision.h"
+#include "../../Collision/Shapes/b2Shape.h"
+#include "../../Common/b2BlockAllocator.h"
+#include "../../Dynamics/b2World.h"
+#include "../../Dynamics/b2Body.h"
+
+b2ContactRegister b2Contact::s_registers[e_shapeTypeCount][e_shapeTypeCount];
+bool b2Contact::s_initialized = false;
+
+void b2Contact::InitializeRegisters()
+{
+ AddType(b2CircleContact::Create, b2CircleContact::Destroy, e_circleShape, e_circleShape);
+ AddType(b2PolyAndCircleContact::Create, b2PolyAndCircleContact::Destroy, e_polygonShape, e_circleShape);
+ AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, e_polygonShape, e_polygonShape);
+}
+
+void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
+ b2ShapeType type1, b2ShapeType type2)
+{
+ b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
+ b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
+
+ s_registers[type1][type2].createFcn = createFcn;
+ s_registers[type1][type2].destroyFcn = destoryFcn;
+ s_registers[type1][type2].primary = true;
+
+ if (type1 != type2)
+ {
+ s_registers[type2][type1].createFcn = createFcn;
+ s_registers[type2][type1].destroyFcn = destoryFcn;
+ s_registers[type2][type1].primary = false;
+ }
+}
+
+b2Contact* b2Contact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
+{
+ if (s_initialized == false)
+ {
+ InitializeRegisters();
+ s_initialized = true;
+ }
+
+ b2ShapeType type1 = shape1->GetType();
+ b2ShapeType type2 = shape2->GetType();
+
+ b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
+ b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
+
+ b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
+ if (createFcn)
+ {
+ if (s_registers[type1][type2].primary)
+ {
+ return createFcn(shape1, shape2, allocator);
+ }
+ else
+ {
+ b2Contact* c = createFcn(shape2, shape1, allocator);
+ for (int32 i = 0; i < c->GetManifoldCount(); ++i)
+ {
+ b2Manifold* m = c->GetManifolds() + i;
+ m->normal = -m->normal;
+ }
+ return c;
+ }
+ }
+ else
+ {
+ return NULL;
+ }
+}
+
+void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ b2Assert(s_initialized == true);
+
+ if (contact->GetManifoldCount() > 0)
+ {
+ contact->GetShape1()->GetBody()->WakeUp();
+ contact->GetShape2()->GetBody()->WakeUp();
+ }
+
+ b2ShapeType type1 = contact->GetShape1()->GetType();
+ b2ShapeType type2 = contact->GetShape2()->GetType();
+
+ b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
+ b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
+
+ b2ContactDestroyFcn* destroyFcn = s_registers[type1][type2].destroyFcn;
+ destroyFcn(contact, allocator);
+}
+
+b2Contact::b2Contact(b2Shape* s1, b2Shape* s2)
+{
+ m_flags = 0;
+
+ if (s1->IsSensor() || s2->IsSensor())
+ {
+ m_flags |= e_nonSolidFlag;
+ }
+
+ m_shape1 = s1;
+ m_shape2 = s2;
+
+ m_manifoldCount = 0;
+
+ m_friction = b2MixFriction(m_shape1->GetFriction(), m_shape2->GetFriction());
+ m_restitution = b2MixRestitution(m_shape1->GetRestitution(), m_shape2->GetRestitution());
+ m_prev = NULL;
+ m_next = NULL;
+
+ m_node1.contact = NULL;
+ m_node1.prev = NULL;
+ m_node1.next = NULL;
+ m_node1.other = NULL;
+
+ m_node2.contact = NULL;
+ m_node2.prev = NULL;
+ m_node2.next = NULL;
+ m_node2.other = NULL;
+}
+
+void b2Contact::Update(b2ContactListener* listener)
+{
+ int32 oldCount = GetManifoldCount();
+
+ Evaluate(listener);
+
+ int32 newCount = GetManifoldCount();
+
+ b2Body* body1 = m_shape1->GetBody();
+ b2Body* body2 = m_shape2->GetBody();
+
+ if (newCount == 0 && oldCount > 0)
+ {
+ body1->WakeUp();
+ body2->WakeUp();
+ }
+
+ // Slow contacts don't generate TOI events.
+ if (body1->IsStatic() || body1->IsBullet() || body2->IsStatic() || body2->IsBullet())
+ {
+ m_flags &= ~e_slowFlag;
+ }
+ else
+ {
+ m_flags |= e_slowFlag;
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_H
+#define CONTACT_H
+
+#include "../../Common/b2Math.h"
+#include "../../Collision/b2Collision.h"
+#include "../../Collision/Shapes/b2Shape.h"
+
+class b2Body;
+class b2Contact;
+class b2World;
+class b2BlockAllocator;
+class b2StackAllocator;
+class b2ContactListener;
+
+typedef b2Contact* b2ContactCreateFcn(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
+
+struct b2ContactRegister
+{
+ b2ContactCreateFcn* createFcn;
+ b2ContactDestroyFcn* destroyFcn;
+ bool primary;
+};
+
+/// A contact edge is used to connect bodies and contacts together
+/// in a contact graph where each body is a node and each contact
+/// is an edge. A contact edge belongs to a doubly linked list
+/// maintained in each attached body. Each contact has two contact
+/// nodes, one for each attached body.
+struct b2ContactEdge
+{
+ b2Body* other; ///< provides quick access to the other body attached.
+ b2Contact* contact; ///< the contact
+ b2ContactEdge* prev; ///< the previous contact edge in the body's contact list
+ b2ContactEdge* next; ///< the next contact edge in the body's contact list
+};
+
+/// This structure is used to report contact points.
+struct b2ContactPoint
+{
+ b2Shape* shape1; ///< the first shape
+ b2Shape* shape2; ///< the second shape
+ b2Vec2 position; ///< position in world coordinates
+ b2Vec2 velocity; ///< velocity of point on body2 relative to point on body1 (pre-solver)
+ b2Vec2 normal; ///< points from shape1 to shape2
+ float32 separation; ///< the separation is negative when shapes are touching
+ float32 friction; ///< the combined friction coefficient
+ float32 restitution; ///< the combined restitution coefficient
+ b2ContactID id; ///< the contact id identifies the features in contact
+};
+
+/// This structure is used to report contact point results.
+struct b2ContactResult
+{
+ b2Shape* shape1; ///< the first shape
+ b2Shape* shape2; ///< the second shape
+ b2Vec2 position; ///< position in world coordinates
+ b2Vec2 normal; ///< points from shape1 to shape2
+ float32 normalImpulse; ///< the normal impulse applied to body2
+ float32 tangentImpulse; ///< the tangent impulse applied to body2
+ b2ContactID id; ///< the contact id identifies the features in contact
+};
+
+/// The class manages contact between two shapes. A contact exists for each overlapping
+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
+/// that has no contact points.
+class b2Contact
+{
+public:
+
+ /// Get the manifold array.
+ virtual b2Manifold* GetManifolds() = 0;
+
+ /// Get the number of manifolds. This is 0 or 1 between convex shapes.
+ /// This may be greater than 1 for convex-vs-concave shapes. Each
+ /// manifold holds up to two contact points with a shared contact normal.
+ int32 GetManifoldCount() const;
+
+ /// Is this contact solid?
+ /// @return true if this contact should generate a response.
+ bool IsSolid() const;
+
+ /// Get the next contact in the world's contact list.
+ b2Contact* GetNext();
+
+ /// Get the first shape in this contact.
+ b2Shape* GetShape1();
+
+ /// Get the second shape in this contact.
+ b2Shape* GetShape2();
+
+ //--------------- Internals Below -------------------
+public:
+
+ // m_flags
+ enum
+ {
+ e_nonSolidFlag = 0x0001,
+ e_slowFlag = 0x0002,
+ e_islandFlag = 0x0004,
+ e_toiFlag = 0x0008,
+ };
+
+ static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
+ b2ShapeType type1, b2ShapeType type2);
+ static void InitializeRegisters();
+ static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2Contact() : m_shape1(NULL), m_shape2(NULL) {}
+ b2Contact(b2Shape* shape1, b2Shape* shape2);
+ virtual ~b2Contact() {}
+
+ void Update(b2ContactListener* listener);
+ virtual void Evaluate(b2ContactListener* listener) = 0;
+ static b2ContactRegister s_registers[e_shapeTypeCount][e_shapeTypeCount];
+ static bool s_initialized;
+
+ uint32 m_flags;
+ int32 m_manifoldCount;
+
+ // World pool and list pointers.
+ b2Contact* m_prev;
+ b2Contact* m_next;
+
+ // Nodes for connecting bodies.
+ b2ContactEdge m_node1;
+ b2ContactEdge m_node2;
+
+ b2Shape* m_shape1;
+ b2Shape* m_shape2;
+
+ // Combined friction
+ float32 m_friction;
+ float32 m_restitution;
+
+ float32 m_toi;
+};
+
+inline int32 b2Contact::GetManifoldCount() const
+{
+ return m_manifoldCount;
+}
+
+inline bool b2Contact::IsSolid() const
+{
+ return (m_flags & e_nonSolidFlag) == 0;
+}
+
+inline b2Contact* b2Contact::GetNext()
+{
+ return m_next;
+}
+
+inline b2Shape* b2Contact::GetShape1()
+{
+ return m_shape1;
+}
+
+inline b2Shape* b2Contact::GetShape2()
+{
+ return m_shape2;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2ContactSolver.h"
+#include "b2Contact.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+#include "../../Common/b2StackAllocator.h"
+
+b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
+{
+ m_step = step;
+ m_allocator = allocator;
+
+ m_constraintCount = 0;
+ for (int32 i = 0; i < contactCount; ++i)
+ {
+ b2Assert(contacts[i]->IsSolid());
+ m_constraintCount += contacts[i]->GetManifoldCount();
+ }
+
+ m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
+
+ int32 count = 0;
+ for (int32 i = 0; i < contactCount; ++i)
+ {
+ b2Contact* contact = contacts[i];
+
+ b2Body* b1 = contact->m_shape1->GetBody();
+ b2Body* b2 = contact->m_shape2->GetBody();
+ int32 manifoldCount = contact->GetManifoldCount();
+ b2Manifold* manifolds = contact->GetManifolds();
+ float32 friction = contact->m_friction;
+ float32 restitution = contact->m_restitution;
+
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ float32 w1 = b1->m_angularVelocity;
+ float32 w2 = b2->m_angularVelocity;
+
+ for (int32 j = 0; j < manifoldCount; ++j)
+ {
+ b2Manifold* manifold = manifolds + j;
+
+ b2Assert(manifold->pointCount > 0);
+
+ const b2Vec2 normal = manifold->normal;
+
+ b2Assert(count < m_constraintCount);
+ b2ContactConstraint* c = m_constraints + count;
+ c->body1 = b1;
+ c->body2 = b2;
+ c->manifold = manifold;
+ c->normal = normal;
+ c->pointCount = manifold->pointCount;
+ c->friction = friction;
+ c->restitution = restitution;
+
+ for (int32 k = 0; k < c->pointCount; ++k)
+ {
+ b2ManifoldPoint* cp = manifold->points + k;
+ b2ContactConstraintPoint* ccp = c->points + k;
+
+ ccp->normalImpulse = cp->normalImpulse;
+ ccp->tangentImpulse = cp->tangentImpulse;
+ ccp->separation = cp->separation;
+ ccp->positionImpulse = 0.0f;
+
+ ccp->localAnchor1 = cp->localPoint1;
+ ccp->localAnchor2 = cp->localPoint2;
+ ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
+ ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
+
+ float32 r1Sqr = b2Dot(ccp->r1, ccp->r1);
+ float32 r2Sqr = b2Dot(ccp->r2, ccp->r2);
+ float32 rn1 = b2Dot(ccp->r1, normal);
+ float32 rn2 = b2Dot(ccp->r2, normal);
+
+ float32 kNormal = b1->m_invMass + b2->m_invMass;
+ kNormal += b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_invI * (r2Sqr - rn2 * rn2);
+
+ b2Assert(kNormal > B2_FLT_EPSILON);
+ ccp->normalMass = 1.0f / kNormal;
+
+ float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
+ kEqualized += b1->m_mass * b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_mass * b2->m_invI * (r2Sqr - rn2 * rn2);
+
+ b2Assert(kEqualized > B2_FLT_EPSILON);
+ ccp->equalizedMass = 1.0f / kEqualized;
+
+ b2Vec2 tangent = b2Cross(normal, 1.0f);
+
+ float32 rt1 = b2Dot(ccp->r1, tangent);
+ float32 rt2 = b2Dot(ccp->r2, tangent);
+ float32 kTangent = b1->m_invMass + b2->m_invMass;
+ kTangent += b1->m_invI * (r1Sqr - rt1 * rt1) + b2->m_invI * (r2Sqr - rt2 * rt2);
+
+ b2Assert(kTangent > B2_FLT_EPSILON);
+ ccp->tangentMass = 1.0f / kTangent;
+
+ // Setup a velocity bias for restitution.
+ ccp->velocityBias = 0.0f;
+ if (ccp->separation > 0.0f)
+ {
+ ccp->velocityBias = -60.0f * ccp->separation; // TODO_ERIN b2TimeStep
+ }
+
+ float32 vRel = b2Dot(c->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
+ if (vRel < -b2_velocityThreshold)
+ {
+ ccp->velocityBias += -c->restitution * vRel;
+ }
+ }
+
+ ++count;
+ }
+ }
+
+ b2Assert(count == m_constraintCount);
+}
+
+b2ContactSolver::~b2ContactSolver()
+{
+ m_allocator->Free(m_constraints);
+}
+
+void b2ContactSolver::InitVelocityConstraints(const b2TimeStep& step)
+{
+ // Warm start.
+ for (int32 i = 0; i < m_constraintCount; ++i)
+ {
+ b2ContactConstraint* c = m_constraints + i;
+
+ b2Body* b1 = c->body1;
+ b2Body* b2 = c->body2;
+ float32 invMass1 = b1->m_invMass;
+ float32 invI1 = b1->m_invI;
+ float32 invMass2 = b2->m_invMass;
+ float32 invI2 = b2->m_invI;
+ b2Vec2 normal = c->normal;
+ b2Vec2 tangent = b2Cross(normal, 1.0f);
+
+ if (step.warmStarting)
+ {
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ b2ContactConstraintPoint* ccp = c->points + j;
+ ccp->normalImpulse *= step.dtRatio;
+ ccp->tangentImpulse *= step.dtRatio;
+ b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
+ b1->m_angularVelocity -= invI1 * b2Cross(ccp->r1, P);
+ b1->m_linearVelocity -= invMass1 * P;
+ b2->m_angularVelocity += invI2 * b2Cross(ccp->r2, P);
+ b2->m_linearVelocity += invMass2 * P;
+ }
+ }
+ else
+ {
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ b2ContactConstraintPoint* ccp = c->points + j;
+ ccp->normalImpulse = 0.0f;
+ ccp->tangentImpulse = 0.0f;
+ }
+ }
+ }
+}
+
+void b2ContactSolver::SolveVelocityConstraints()
+{
+ for (int32 i = 0; i < m_constraintCount; ++i)
+ {
+ b2ContactConstraint* c = m_constraints + i;
+ b2Body* b1 = c->body1;
+ b2Body* b2 = c->body2;
+ float32 w1 = b1->m_angularVelocity;
+ float32 w2 = b2->m_angularVelocity;
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ float32 invMass1 = b1->m_invMass;
+ float32 invI1 = b1->m_invI;
+ float32 invMass2 = b2->m_invMass;
+ float32 invI2 = b2->m_invI;
+ b2Vec2 normal = c->normal;
+ b2Vec2 tangent = b2Cross(normal, 1.0f);
+ float32 friction = c->friction;
+//#define DEFERRED_UPDATE
+#ifdef DEFERRED_UPDATE
+ b2Vec2 b1_linearVelocity = b1->m_linearVelocity;
+ float32 b1_angularVelocity = b1->m_angularVelocity;
+ b2Vec2 b2_linearVelocity = b2->m_linearVelocity;
+ float32 b2_angularVelocity = b2->m_angularVelocity;
+#endif
+ // Solve normal constraints
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ b2ContactConstraintPoint* ccp = c->points + j;
+
+ // Relative velocity at contact
+ b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
+
+ // Compute normal impulse
+ float32 vn = b2Dot(dv, normal);
+ float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
+
+ // b2Clamp the accumulated impulse
+ float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
+ lambda = newImpulse - ccp->normalImpulse;
+
+ // Apply contact impulse
+ b2Vec2 P = lambda * normal;
+#ifdef DEFERRED_UPDATE
+ b1_linearVelocity -= invMass1 * P;
+ b1_angularVelocity -= invI1 * b2Cross(r1, P);
+
+ b2_linearVelocity += invMass2 * P;
+ b2_angularVelocity += invI2 * b2Cross(r2, P);
+#else
+ v1 -= invMass1 * P;
+ w1 -= invI1 * b2Cross(ccp->r1, P);
+
+ v2 += invMass2 * P;
+ w2 += invI2 * b2Cross(ccp->r2, P);
+#endif
+ ccp->normalImpulse = newImpulse;
+ }
+
+#ifdef DEFERRED_UPDATE
+ b1->m_linearVelocity = b1_linearVelocity;
+ b1->m_angularVelocity = b1_angularVelocity;
+ b2->m_linearVelocity = b2_linearVelocity;
+ b2->m_angularVelocity = b2_angularVelocity;
+#endif
+ // Solve tangent constraints
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ b2ContactConstraintPoint* ccp = c->points + j;
+
+ // Relative velocity at contact
+ b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
+
+ // Compute tangent force
+ float32 vt = b2Dot(dv, tangent);
+ float32 lambda = ccp->tangentMass * (-vt);
+
+ // b2Clamp the accumulated force
+ float32 maxFriction = friction * ccp->normalImpulse;
+ float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
+ lambda = newImpulse - ccp->tangentImpulse;
+
+ // Apply contact impulse
+ b2Vec2 P = lambda * tangent;
+
+ v1 -= invMass1 * P;
+ w1 -= invI1 * b2Cross(ccp->r1, P);
+
+ v2 += invMass2 * P;
+ w2 += invI2 * b2Cross(ccp->r2, P);
+
+ ccp->tangentImpulse = newImpulse;
+ }
+
+ b1->m_linearVelocity = v1;
+ b1->m_angularVelocity = w1;
+ b2->m_linearVelocity = v2;
+ b2->m_angularVelocity = w2;
+ }
+}
+
+void b2ContactSolver::FinalizeVelocityConstraints()
+{
+ for (int32 i = 0; i < m_constraintCount; ++i)
+ {
+ b2ContactConstraint* c = m_constraints + i;
+ b2Manifold* m = c->manifold;
+
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ m->points[j].normalImpulse = c->points[j].normalImpulse;
+ m->points[j].tangentImpulse = c->points[j].tangentImpulse;
+ }
+ }
+}
+
+bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
+{
+ float32 minSeparation = 0.0f;
+
+ for (int32 i = 0; i < m_constraintCount; ++i)
+ {
+ b2ContactConstraint* c = m_constraints + i;
+ b2Body* b1 = c->body1;
+ b2Body* b2 = c->body2;
+ float32 invMass1 = b1->m_mass * b1->m_invMass;
+ float32 invI1 = b1->m_mass * b1->m_invI;
+ float32 invMass2 = b2->m_mass * b2->m_invMass;
+ float32 invI2 = b2->m_mass * b2->m_invI;
+
+ b2Vec2 normal = c->normal;
+
+ // Solver normal constraints
+ for (int32 j = 0; j < c->pointCount; ++j)
+ {
+ b2ContactConstraintPoint* ccp = c->points + j;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 dp = p2 - p1;
+
+ // Approximate the current separation.
+ float32 separation = b2Dot(dp, normal) + ccp->separation;
+
+ // Track max constraint error.
+ minSeparation = b2Min(minSeparation, separation);
+
+ // Prevent large corrections and allow slop.
+ float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+
+ // Compute normal impulse
+ float32 dImpulse = -ccp->equalizedMass * C;
+
+ // b2Clamp the accumulated impulse
+ float32 impulse0 = ccp->positionImpulse;
+ ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f);
+ dImpulse = ccp->positionImpulse - impulse0;
+
+ b2Vec2 impulse = dImpulse * normal;
+
+ b1->m_sweep.c -= invMass1 * impulse;
+ b1->m_sweep.a -= invI1 * b2Cross(r1, impulse);
+ b1->SynchronizeTransform();
+
+ b2->m_sweep.c += invMass2 * impulse;
+ b2->m_sweep.a += invI2 * b2Cross(r2, impulse);
+ b2->SynchronizeTransform();
+ }
+ }
+
+ // We can't expect minSpeparation >= -b2_linearSlop because we don't
+ // push the separation above -b2_linearSlop.
+ return minSeparation >= -1.5f * b2_linearSlop;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_SOLVER_H
+#define CONTACT_SOLVER_H
+
+#include "../../Common/b2Math.h"
+#include "../../Collision/b2Collision.h"
+#include "../b2World.h"
+
+class b2Contact;
+class b2Body;
+class b2Island;
+class b2StackAllocator;
+
+struct b2ContactConstraintPoint
+{
+ b2Vec2 localAnchor1;
+ b2Vec2 localAnchor2;
+ b2Vec2 r1;
+ b2Vec2 r2;
+ float32 normalImpulse;
+ float32 tangentImpulse;
+ float32 positionImpulse;
+ float32 normalMass;
+ float32 tangentMass;
+ float32 equalizedMass;
+ float32 separation;
+ float32 velocityBias;
+};
+
+struct b2ContactConstraint
+{
+ b2ContactConstraintPoint points[b2_maxManifoldPoints];
+ b2Vec2 normal;
+ b2Manifold* manifold;
+ b2Body* body1;
+ b2Body* body2;
+ float32 friction;
+ float32 restitution;
+ int32 pointCount;
+};
+
+class b2ContactSolver
+{
+public:
+ b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator);
+ ~b2ContactSolver();
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints();
+ void FinalizeVelocityConstraints();
+
+ bool SolvePositionConstraints(float32 baumgarte);
+
+ b2TimeStep m_step;
+ b2StackAllocator* m_allocator;
+ b2ContactConstraint* m_constraints;
+ int m_constraintCount;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_NULL_CONTACT_H
+#define B2_NULL_CONTACT_H
+
+#include "../../Common/b2Math.h"
+#include "b2Contact.h"
+
+class b2NullContact : public b2Contact
+{
+public:
+ b2NullContact() {}
+ void Evaluate(b2ContactListener*) {}
+ b2Manifold* GetManifolds() { return NULL; }
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PolyAndCircleContact.h"
+#include "../b2Body.h"
+#include "../b2WorldCallbacks.h"
+#include "../../Common/b2BlockAllocator.h"
+
+#include <new>
+#include <string.h>
+
+b2Contact* b2PolyAndCircleContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2PolyAndCircleContact));
+ return new (mem) b2PolyAndCircleContact(shape1, shape2);
+}
+
+void b2PolyAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2PolyAndCircleContact*)contact)->~b2PolyAndCircleContact();
+ allocator->Free(contact, sizeof(b2PolyAndCircleContact));
+}
+
+b2PolyAndCircleContact::b2PolyAndCircleContact(b2Shape* s1, b2Shape* s2)
+: b2Contact(s1, s2)
+{
+ b2Assert(m_shape1->GetType() == e_polygonShape);
+ b2Assert(m_shape2->GetType() == e_circleShape);
+ m_manifold.pointCount = 0;
+ m_manifold.points[0].normalImpulse = 0.0f;
+ m_manifold.points[0].tangentImpulse = 0.0f;
+}
+
+void b2PolyAndCircleContact::Evaluate(b2ContactListener* listener)
+{
+ b2Body* b1 = m_shape1->GetBody();
+ b2Body* b2 = m_shape2->GetBody();
+
+ b2Manifold m0;
+ memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+
+ b2CollidePolygonAndCircle(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2CircleShape*)m_shape2, b2->GetXForm());
+
+ bool persisted[b2_maxManifoldPoints] = {false, false};
+
+ b2ContactPoint cp;
+ cp.shape1 = m_shape1;
+ cp.shape2 = m_shape2;
+ cp.friction = m_friction;
+ cp.restitution = m_restitution;
+
+ // Match contact ids to facilitate warm starting.
+ if (m_manifold.pointCount > 0)
+ {
+ // Match old contact ids to new contact ids and copy the
+ // stored impulses to warm start the solver.
+ for (int32 i = 0; i < m_manifold.pointCount; ++i)
+ {
+ b2ManifoldPoint* mp = m_manifold.points + i;
+ mp->normalImpulse = 0.0f;
+ mp->tangentImpulse = 0.0f;
+ bool found = false;
+ b2ContactID id = mp->id;
+
+ for (int32 j = 0; j < m0.pointCount; ++j)
+ {
+ if (persisted[j] == true)
+ {
+ continue;
+ }
+
+ b2ManifoldPoint* mp0 = m0.points + j;
+
+ if (mp0->id.key == id.key)
+ {
+ persisted[j] = true;
+ mp->normalImpulse = mp0->normalImpulse;
+ mp->tangentImpulse = mp0->tangentImpulse;
+
+ // A persistent point.
+ found = true;
+
+ // Report persistent point.
+ if (listener != NULL)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = id;
+ listener->Persist(&cp);
+ }
+ break;
+ }
+ }
+
+ // Report added point.
+ if (found == false && listener != NULL)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = id;
+ listener->Add(&cp);
+ }
+ }
+
+ m_manifoldCount = 1;
+ }
+ else
+ {
+ m_manifoldCount = 0;
+ }
+
+ if (listener == NULL)
+ {
+ return;
+ }
+
+ // Report removed points.
+ for (int32 i = 0; i < m0.pointCount; ++i)
+ {
+ if (persisted[i])
+ {
+ continue;
+ }
+
+ b2ManifoldPoint* mp0 = m0.points + i;
+ cp.position = b1->GetWorldPoint(mp0->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m0.normal;
+ cp.separation = mp0->separation;
+ cp.id = mp0->id;
+ listener->Remove(&cp);
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POLY_AND_CIRCLE_CONTACT_H
+#define POLY_AND_CIRCLE_CONTACT_H
+
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolyAndCircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2PolyAndCircleContact(b2Shape* shape1, b2Shape* shape2);
+ ~b2PolyAndCircleContact() {}
+
+ void Evaluate(b2ContactListener* listener);
+ b2Manifold* GetManifolds()
+ {
+ return &m_manifold;
+ }
+
+ b2Manifold m_manifold;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PolyContact.h"
+#include "../b2Body.h"
+#include "../b2WorldCallbacks.h"
+#include "../../Common/b2BlockAllocator.h"
+
+#include <memory>
+#include <new>
+#include <string.h>
+
+b2Contact* b2PolygonContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2PolygonContact));
+ return new (mem) b2PolygonContact(shape1, shape2);
+}
+
+void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2PolygonContact*)contact)->~b2PolygonContact();
+ allocator->Free(contact, sizeof(b2PolygonContact));
+}
+
+b2PolygonContact::b2PolygonContact(b2Shape* s1, b2Shape* s2)
+ : b2Contact(s1, s2)
+{
+ b2Assert(m_shape1->GetType() == e_polygonShape);
+ b2Assert(m_shape2->GetType() == e_polygonShape);
+ m_manifold.pointCount = 0;
+}
+
+void b2PolygonContact::Evaluate(b2ContactListener* listener)
+{
+ b2Body* b1 = m_shape1->GetBody();
+ b2Body* b2 = m_shape2->GetBody();
+
+ b2Manifold m0;
+ memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+
+ b2CollidePolygons(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2PolygonShape*)m_shape2, b2->GetXForm());
+
+ bool persisted[b2_maxManifoldPoints] = {false, false};
+
+ b2ContactPoint cp;
+ cp.shape1 = m_shape1;
+ cp.shape2 = m_shape2;
+ cp.friction = m_friction;
+ cp.restitution = m_restitution;
+
+ // Match contact ids to facilitate warm starting.
+ if (m_manifold.pointCount > 0)
+ {
+ // Match old contact ids to new contact ids and copy the
+ // stored impulses to warm start the solver.
+ for (int32 i = 0; i < m_manifold.pointCount; ++i)
+ {
+ b2ManifoldPoint* mp = m_manifold.points + i;
+ mp->normalImpulse = 0.0f;
+ mp->tangentImpulse = 0.0f;
+ bool found = false;
+ b2ContactID id = mp->id;
+
+ for (int32 j = 0; j < m0.pointCount; ++j)
+ {
+ if (persisted[j] == true)
+ {
+ continue;
+ }
+
+ b2ManifoldPoint* mp0 = m0.points + j;
+
+ if (mp0->id.key == id.key)
+ {
+ persisted[j] = true;
+ mp->normalImpulse = mp0->normalImpulse;
+ mp->tangentImpulse = mp0->tangentImpulse;
+
+ // A persistent point.
+ found = true;
+
+ // Report persistent point.
+ if (listener != NULL)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = id;
+ listener->Persist(&cp);
+ }
+ break;
+ }
+ }
+
+ // Report added point.
+ if (found == false && listener != NULL)
+ {
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m_manifold.normal;
+ cp.separation = mp->separation;
+ cp.id = id;
+ listener->Add(&cp);
+ }
+ }
+
+ m_manifoldCount = 1;
+ }
+ else
+ {
+ m_manifoldCount = 0;
+ }
+
+ if (listener == NULL)
+ {
+ return;
+ }
+
+ // Report removed points.
+ for (int32 i = 0; i < m0.pointCount; ++i)
+ {
+ if (persisted[i])
+ {
+ continue;
+ }
+
+ b2ManifoldPoint* mp0 = m0.points + i;
+ cp.position = b1->GetWorldPoint(mp0->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.normal = m0.normal;
+ cp.separation = mp0->separation;
+ cp.id = mp0->id;
+ listener->Remove(&cp);
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POLYCONTACT_H
+#define POLYCONTACT_H
+
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonContact : public b2Contact
+{
+public:
+ static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2PolygonContact(b2Shape* shape1, b2Shape* shape2);
+ ~b2PolygonContact() {}
+
+ void Evaluate(b2ContactListener* listener);
+ b2Manifold* GetManifolds()
+ {
+ return &m_manifold;
+ }
+
+ b2Manifold m_manifold;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2DistanceJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// 1-D constrained system
+// m (v2 - v1) = lambda
+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
+// x2 = x1 + h * v2
+
+// 1-D mass-damper-spring system
+// m (v2 - v1) + h * d * v2 + h * k *
+
+// C = norm(p2 - p1) - L
+// u = (p2 - p1) / norm(p2 - p1)
+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// J = [-u -cross(r1, u) u cross(r2, u)]
+// K = J * invM * JT
+// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
+
+void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2)
+{
+ body1 = b1;
+ body2 = b2;
+ localAnchor1 = body1->GetLocalPoint(anchor1);
+ localAnchor2 = body2->GetLocalPoint(anchor2);
+ b2Vec2 d = anchor2 - anchor1;
+ length = d.Length();
+}
+
+
+b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchor1 = def->localAnchor1;
+ m_localAnchor2 = def->localAnchor2;
+ m_length = def->length;
+ m_frequencyHz = def->frequencyHz;
+ m_dampingRatio = def->dampingRatio;
+ m_impulse = 0.0f;
+ m_gamma = 0.0f;
+ m_bias = 0.0f;
+ m_inv_dt = 0.0f;
+}
+
+void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ m_inv_dt = step.inv_dt;
+
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ // Compute the effective mass matrix.
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+ // Handle singularity.
+ float32 length = m_u.Length();
+ if (length > b2_linearSlop)
+ {
+ m_u *= 1.0f / length;
+ }
+ else
+ {
+ m_u.Set(0.0f, 0.0f);
+ }
+
+ float32 cr1u = b2Cross(r1, m_u);
+ float32 cr2u = b2Cross(r2, m_u);
+ float32 invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u;
+ b2Assert(invMass > B2_FLT_EPSILON);
+ m_mass = 1.0f / invMass;
+
+ if (m_frequencyHz > 0.0f)
+ {
+ float32 C = length - m_length;
+
+ // Frequency
+ float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+ // Damping coefficient
+ float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+
+ // Spring stiffness
+ float32 k = m_mass * omega * omega;
+
+ // magic formulas
+ m_gamma = 1.0f / (step.dt * (d + step.dt * k));
+ m_bias = C * step.dt * k * m_gamma;
+
+ m_mass = 1.0f / (invMass + m_gamma);
+ }
+
+ if (step.warmStarting)
+ {
+ m_impulse *= step.dtRatio;
+ b2Vec2 P = m_impulse * m_u;
+ b1->m_linearVelocity -= b1->m_invMass * P;
+ b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+ b2->m_linearVelocity += b2->m_invMass * P;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+ }
+ else
+ {
+ m_impulse = 0.0f;
+ }
+}
+
+void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ B2_NOT_USED(step);
+
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ // Cdot = dot(u, v + cross(w, r))
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+ float32 Cdot = b2Dot(m_u, v2 - v1);
+
+ float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+ m_impulse += impulse;
+
+ b2Vec2 P = impulse * m_u;
+ b1->m_linearVelocity -= b1->m_invMass * P;
+ b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+ b2->m_linearVelocity += b2->m_invMass * P;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+}
+
+bool b2DistanceJoint::SolvePositionConstraints()
+{
+ if (m_frequencyHz > 0.0f)
+ {
+ return true;
+ }
+
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+ float32 length = d.Normalize();
+ float32 C = length - m_length;
+ C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+
+ float32 impulse = -m_mass * C;
+ m_u = d;
+ b2Vec2 P = impulse * m_u;
+
+ b1->m_sweep.c -= b1->m_invMass * P;
+ b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
+ b2->m_sweep.c += b2->m_invMass * P;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ return b2Abs(C) < b2_linearSlop;
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2DistanceJoint::GetReactionForce() const
+{
+ b2Vec2 F = (m_inv_dt * m_impulse) * m_u;
+ return F;
+}
+
+float32 b2DistanceJoint::GetReactionTorque() const
+{
+ return 0.0f;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_DISTANCE_JOINT_H
+#define B2_DISTANCE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Distance joint definition. This requires defining an
+/// anchor point on both bodies and the non-zero length of the
+/// distance joint. The definition uses local anchor points
+/// so that the initial configuration can violate the constraint
+/// slightly. This helps when saving and loading a game.
+/// @warning Do not use a zero or short length.
+struct b2DistanceJointDef : public b2JointDef
+{
+ b2DistanceJointDef()
+ {
+ type = e_distanceJoint;
+ localAnchor1.Set(0.0f, 0.0f);
+ localAnchor2.Set(0.0f, 0.0f);
+ length = 1.0f;
+ frequencyHz = 0.0f;
+ dampingRatio = 0.0f;
+ }
+
+ /// Initialize the bodies, anchors, and length using the world
+ /// anchors.
+ void Initialize(b2Body* body1, b2Body* body2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2);
+
+ /// The local anchor point relative to body1's origin.
+ b2Vec2 localAnchor1;
+
+ /// The local anchor point relative to body2's origin.
+ b2Vec2 localAnchor2;
+
+ /// The equilibrium length between the anchor points.
+ float32 length;
+
+ /// The response speed.
+ float32 frequencyHz;
+
+ /// The damping ratio. 0 = no damping, 1 = critical damping.
+ float32 dampingRatio;
+};
+
+/// A distance joint constrains two points on two bodies
+/// to remain at a fixed distance from each other. You can view
+/// this as a massless, rigid rod.
+class b2DistanceJoint : public b2Joint
+{
+public:
+
+ b2Vec2 GetAnchor1() const;
+ b2Vec2 GetAnchor2() const;
+
+ b2Vec2 GetReactionForce() const;
+ float32 GetReactionTorque() const;
+
+ //--------------- Internals Below -------------------
+
+ b2DistanceJoint(const b2DistanceJointDef* data);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints();
+
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+ b2Vec2 m_u;
+ float32 m_frequencyHz;
+ float32 m_dampingRatio;
+ float32 m_gamma;
+ float32 m_bias;
+ float32 m_impulse;
+ float32 m_mass; // effective mass for the constraint.
+ float32 m_length;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2GearJoint.h"
+#include "b2RevoluteJoint.h"
+#include "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// C = C0 - (cordinate1 + ratio * coordinate2) = 0
+// Cdot = -(Cdot1 + ratio * Cdot2)
+// J = -[J1 ratio * J2]
+// K = J * invM * JT
+// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
+//
+// Revolute:
+// coordinate = rotation
+// Cdot = angularVelocity
+// J = [0 0 1]
+// K = J * invM * JT = invI
+//
+// Prismatic:
+// coordinate = dot(p - pg, ug)
+// Cdot = dot(v + cross(w, r), ug)
+// J = [ug cross(r, ug)]
+// K = J * invM * JT = invMass + invI * cross(r, ug)^2
+
+b2GearJoint::b2GearJoint(const b2GearJointDef* def)
+: b2Joint(def)
+{
+ b2JointType type1 = def->joint1->GetType();
+ b2JointType type2 = def->joint2->GetType();
+
+ b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
+ b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
+ b2Assert(def->joint1->GetBody1()->IsStatic());
+ b2Assert(def->joint2->GetBody1()->IsStatic());
+
+ m_revolute1 = NULL;
+ m_prismatic1 = NULL;
+ m_revolute2 = NULL;
+ m_prismatic2 = NULL;
+
+ float32 coordinate1, coordinate2;
+
+ m_ground1 = def->joint1->GetBody1();
+ m_body1 = def->joint1->GetBody2();
+ if (type1 == e_revoluteJoint)
+ {
+ m_revolute1 = (b2RevoluteJoint*)def->joint1;
+ m_groundAnchor1 = m_revolute1->m_localAnchor1;
+ m_localAnchor1 = m_revolute1->m_localAnchor2;
+ coordinate1 = m_revolute1->GetJointAngle();
+ }
+ else
+ {
+ m_prismatic1 = (b2PrismaticJoint*)def->joint1;
+ m_groundAnchor1 = m_prismatic1->m_localAnchor1;
+ m_localAnchor1 = m_prismatic1->m_localAnchor2;
+ coordinate1 = m_prismatic1->GetJointTranslation();
+ }
+
+ m_ground2 = def->joint2->GetBody1();
+ m_body2 = def->joint2->GetBody2();
+ if (type2 == e_revoluteJoint)
+ {
+ m_revolute2 = (b2RevoluteJoint*)def->joint2;
+ m_groundAnchor2 = m_revolute2->m_localAnchor1;
+ m_localAnchor2 = m_revolute2->m_localAnchor2;
+ coordinate2 = m_revolute2->GetJointAngle();
+ }
+ else
+ {
+ m_prismatic2 = (b2PrismaticJoint*)def->joint2;
+ m_groundAnchor2 = m_prismatic2->m_localAnchor1;
+ m_localAnchor2 = m_prismatic2->m_localAnchor2;
+ coordinate2 = m_prismatic2->GetJointTranslation();
+ }
+
+ m_ratio = def->ratio;
+
+ m_constant = coordinate1 + m_ratio * coordinate2;
+
+ m_force = 0.0f;
+}
+
+void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* g1 = m_ground1;
+ b2Body* g2 = m_ground2;
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 K = 0.0f;
+ m_J.SetZero();
+
+ if (m_revolute1)
+ {
+ m_J.angular1 = -1.0f;
+ K += b1->m_invI;
+ }
+ else
+ {
+ b2Vec2 ug = b2Mul(g1->GetXForm().R, m_prismatic1->m_localXAxis1);
+ b2Vec2 r = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ float32 crug = b2Cross(r, ug);
+ m_J.linear1 = -ug;
+ m_J.angular1 = -crug;
+ K += b1->m_invMass + b1->m_invI * crug * crug;
+ }
+
+ if (m_revolute2)
+ {
+ m_J.angular2 = -m_ratio;
+ K += m_ratio * m_ratio * b2->m_invI;
+ }
+ else
+ {
+ b2Vec2 ug = b2Mul(g2->GetXForm().R, m_prismatic2->m_localXAxis1);
+ b2Vec2 r = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ float32 crug = b2Cross(r, ug);
+ m_J.linear2 = -m_ratio * ug;
+ m_J.angular2 = -m_ratio * crug;
+ K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
+ }
+
+ // Compute effective mass.
+ b2Assert(K > 0.0f);
+ m_mass = 1.0f / K;
+
+ if (step.warmStarting)
+ {
+ // Warm starting.
+ float32 P = B2FORCE_SCALE(step.dt) * m_force;
+ b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
+ b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
+ b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
+ b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+ }
+ else
+ {
+ m_force = 0.0f;
+ }
+}
+
+void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 Cdot = m_J.Compute( b1->m_linearVelocity, b1->m_angularVelocity,
+ b2->m_linearVelocity, b2->m_angularVelocity);
+
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_mass * Cdot;
+ m_force += force;
+
+ float32 P = B2FORCE_SCALE(step.dt) * force;
+ b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
+ b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
+ b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
+ b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+}
+
+bool b2GearJoint::SolvePositionConstraints()
+{
+ float32 linearError = 0.0f;
+
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 coordinate1, coordinate2;
+ if (m_revolute1)
+ {
+ coordinate1 = m_revolute1->GetJointAngle();
+ }
+ else
+ {
+ coordinate1 = m_prismatic1->GetJointTranslation();
+ }
+
+ if (m_revolute2)
+ {
+ coordinate2 = m_revolute2->GetJointAngle();
+ }
+ else
+ {
+ coordinate2 = m_prismatic2->GetJointTranslation();
+ }
+
+ float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
+
+ float32 impulse = -m_mass * C;
+
+ b1->m_sweep.c += b1->m_invMass * impulse * m_J.linear1;
+ b1->m_sweep.a += b1->m_invI * impulse * m_J.angular1;
+ b2->m_sweep.c += b2->m_invMass * impulse * m_J.linear2;
+ b2->m_sweep.a += b2->m_invI * impulse * m_J.angular2;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2GearJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2GearJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2GearJoint::GetReactionForce() const
+{
+ // TODO_ERIN not tested
+ b2Vec2 F = B2FORCE_SCALE(m_force) * m_J.linear2;
+ return F;
+}
+
+float32 b2GearJoint::GetReactionTorque() const
+{
+ // TODO_ERIN not tested
+ b2Vec2 r = b2Mul(m_body2->GetXForm().R, m_localAnchor2 - m_body2->GetLocalCenter());
+ b2Vec2 F = m_force * m_J.linear2;
+ float32 T = B2FORCE_SCALE(m_force * m_J.angular2 - b2Cross(r, F));
+ return T;
+}
+
+float32 b2GearJoint::GetRatio() const
+{
+ return m_ratio;
+}
+
+
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_GEAR_JOINT_H
+#define B2_GEAR_JOINT_H
+
+#include "b2Joint.h"
+
+class b2RevoluteJoint;
+class b2PrismaticJoint;
+
+/// Gear joint definition. This definition requires two existing
+/// revolute or prismatic joints (any combination will work).
+/// The provided joints must attach a dynamic body to a static body.
+struct b2GearJointDef : public b2JointDef
+{
+ b2GearJointDef()
+ {
+ type = e_gearJoint;
+ joint1 = NULL;
+ joint2 = NULL;
+ ratio = 1.0f;
+ }
+
+ /// The first revolute/prismatic joint attached to the gear joint.
+ b2Joint* joint1;
+
+ /// The second revolute/prismatic joint attached to the gear joint.
+ b2Joint* joint2;
+
+ /// The gear ratio.
+ /// @see b2GearJoint for explanation.
+ float32 ratio;
+};
+
+/// A gear joint is used to connect two joints together. Either joint
+/// can be a revolute or prismatic joint. You specify a gear ratio
+/// to bind the motions together:
+/// coordinate1 + ratio * coordinate2 = constant
+/// The ratio can be negative or positive. If one joint is a revolute joint
+/// and the other joint is a prismatic joint, then the ratio will have units
+/// of length or units of 1/length.
+/// @warning The revolute and prismatic joints must be attached to
+/// fixed bodies (which must be body1 on those joints).
+class b2GearJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchor1() const;
+ b2Vec2 GetAnchor2() const;
+
+ b2Vec2 GetReactionForce() const;
+ float32 GetReactionTorque() const;
+
+ /// Get the gear ratio.
+ float32 GetRatio() const;
+
+ //--------------- Internals Below -------------------
+
+ b2GearJoint(const b2GearJointDef* data);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints();
+
+ b2Body* m_ground1;
+ b2Body* m_ground2;
+
+ // One of these is NULL.
+ b2RevoluteJoint* m_revolute1;
+ b2PrismaticJoint* m_prismatic1;
+
+ // One of these is NULL.
+ b2RevoluteJoint* m_revolute2;
+ b2PrismaticJoint* m_prismatic2;
+
+ b2Vec2 m_groundAnchor1;
+ b2Vec2 m_groundAnchor2;
+
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+
+ b2Jacobian m_J;
+
+ float32 m_constant;
+ float32 m_ratio;
+
+ // Effective mass
+ float32 m_mass;
+
+ // Impulse for accumulation/warm starting.
+ float32 m_force;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Joint.h"
+#include "b2DistanceJoint.h"
+#include "b2MouseJoint.h"
+#include "b2RevoluteJoint.h"
+#include "b2PrismaticJoint.h"
+#include "b2PulleyJoint.h"
+#include "b2GearJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+#include "../../Common/b2BlockAllocator.h"
+#include "../../Collision/b2BroadPhase.h"
+
+#include <new>
+
+b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
+{
+ b2Joint* joint = NULL;
+
+ switch (def->type)
+ {
+ case e_distanceJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
+ joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
+ }
+ break;
+
+ case e_mouseJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2MouseJoint));
+ joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
+ }
+ break;
+
+ case e_prismaticJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
+ joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
+ }
+ break;
+
+ case e_revoluteJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
+ joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
+ }
+ break;
+
+ case e_pulleyJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
+ joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
+ }
+ break;
+
+ case e_gearJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2GearJoint));
+ joint = new (mem) b2GearJoint((b2GearJointDef*)def);
+ }
+ break;
+
+ default:
+ b2Assert(false);
+ break;
+ }
+
+ return joint;
+}
+
+void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
+{
+ joint->~b2Joint();
+ switch (joint->m_type)
+ {
+ case e_distanceJoint:
+ allocator->Free(joint, sizeof(b2DistanceJoint));
+ break;
+
+ case e_mouseJoint:
+ allocator->Free(joint, sizeof(b2MouseJoint));
+ break;
+
+ case e_prismaticJoint:
+ allocator->Free(joint, sizeof(b2PrismaticJoint));
+ break;
+
+ case e_revoluteJoint:
+ allocator->Free(joint, sizeof(b2RevoluteJoint));
+ break;
+
+ case e_pulleyJoint:
+ allocator->Free(joint, sizeof(b2PulleyJoint));
+ break;
+
+ case e_gearJoint:
+ allocator->Free(joint, sizeof(b2GearJoint));
+ break;
+
+ default:
+ b2Assert(false);
+ break;
+ }
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+ m_type = def->type;
+ m_prev = NULL;
+ m_next = NULL;
+ m_body1 = def->body1;
+ m_body2 = def->body2;
+ m_collideConnected = def->collideConnected;
+ m_islandFlag = false;
+ m_userData = def->userData;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef JOINT_H
+#define JOINT_H
+
+#include "../../Common/b2Math.h"
+
+class b2Body;
+class b2Joint;
+struct b2TimeStep;
+class b2BlockAllocator;
+
+enum b2JointType
+{
+ e_unknownJoint,
+ e_revoluteJoint,
+ e_prismaticJoint,
+ e_distanceJoint,
+ e_pulleyJoint,
+ e_mouseJoint,
+ e_gearJoint
+};
+
+enum b2LimitState
+{
+ e_inactiveLimit,
+ e_atLowerLimit,
+ e_atUpperLimit,
+ e_equalLimits
+};
+
+struct b2Jacobian
+{
+ b2Vec2 linear1;
+ float32 angular1;
+ b2Vec2 linear2;
+ float32 angular2;
+
+ void SetZero();
+ void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+ float32 Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+};
+
+/// A joint edge is used to connect bodies and joints together
+/// in a joint graph where each body is a node and each joint
+/// is an edge. A joint edge belongs to a doubly linked list
+/// maintained in each attached body. Each joint has two joint
+/// nodes, one for each attached body.
+struct b2JointEdge
+{
+ b2Body* other; ///< provides quick access to the other body attached.
+ b2Joint* joint; ///< the joint
+ b2JointEdge* prev; ///< the previous joint edge in the body's joint list
+ b2JointEdge* next; ///< the next joint edge in the body's joint list
+};
+
+/// Joint definitions are used to construct joints.
+struct b2JointDef
+{
+ b2JointDef()
+ {
+ type = e_unknownJoint;
+ userData = NULL;
+ body1 = NULL;
+ body2 = NULL;
+ collideConnected = false;
+ }
+
+ /// The joint type is set automatically for concrete joint types.
+ b2JointType type;
+
+ /// Use this to attach application specific data to your joints.
+ void* userData;
+
+ /// The first attached body.
+ b2Body* body1;
+
+ /// The second attached body.
+ b2Body* body2;
+
+ /// Set this flag to true if the attached bodies should collide.
+ bool collideConnected;
+};
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+class b2Joint
+{
+public:
+
+ /// Get the type of the concrete joint.
+ b2JointType GetType() const;
+
+ /// Get the first body attached to this joint.
+ b2Body* GetBody1();
+
+ /// Get the second body attached to this joint.
+ b2Body* GetBody2();
+
+ /// Get the anchor point on body1 in world coordinates.
+ virtual b2Vec2 GetAnchor1() const = 0;
+
+ /// Get the anchor point on body2 in world coordinates.
+ virtual b2Vec2 GetAnchor2() const = 0;
+
+ /// Get the reaction force on body2 at the joint anchor.
+ virtual b2Vec2 GetReactionForce() const = 0;
+
+ /// Get the reaction torque on body2.
+ virtual float32 GetReactionTorque() const = 0;
+
+ /// Get the next joint the world joint list.
+ b2Joint* GetNext();
+
+ /// Get the user data pointer.
+ void* GetUserData();
+
+ /// Set the user data pointer.
+ void SetUserData(void* data);
+
+ //--------------- Internals Below -------------------
+protected:
+ friend class b2World;
+ friend class b2Body;
+ friend class b2Island;
+
+ static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
+ static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
+
+ b2Joint(const b2JointDef* def);
+ virtual ~b2Joint() {}
+
+ virtual void InitVelocityConstraints(const b2TimeStep& step) = 0;
+ virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
+
+ // This returns true if the position errors are within tolerance.
+ virtual void InitPositionConstraints() {}
+ virtual bool SolvePositionConstraints() = 0;
+
+ b2JointType m_type;
+ b2Joint* m_prev;
+ b2Joint* m_next;
+ b2JointEdge m_node1;
+ b2JointEdge m_node2;
+ b2Body* m_body1;
+ b2Body* m_body2;
+
+ float32 m_inv_dt;
+
+ bool m_islandFlag;
+ bool m_collideConnected;
+
+ void* m_userData;
+};
+
+inline void b2Jacobian::SetZero()
+{
+ linear1.SetZero(); angular1 = 0.0f;
+ linear2.SetZero(); angular2 = 0.0f;
+}
+
+inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+ linear1 = x1; angular1 = a1;
+ linear2 = x2; angular2 = a2;
+}
+
+inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+ return b2Dot(linear1, x1) + angular1 * a1 + b2Dot(linear2, x2) + angular2 * a2;
+}
+
+inline b2JointType b2Joint::GetType() const
+{
+ return m_type;
+}
+
+inline b2Body* b2Joint::GetBody1()
+{
+ return m_body1;
+}
+
+inline b2Body* b2Joint::GetBody2()
+{
+ return m_body2;
+}
+
+inline b2Joint* b2Joint::GetNext()
+{
+ return m_next;
+}
+
+inline void* b2Joint::GetUserData()
+{
+ return m_userData;
+}
+
+inline void b2Joint::SetUserData(void* data)
+{
+ m_userData = data;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2MouseJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// p = attached point, m = mouse point
+// C = p - m
+// Cdot = v
+// = v + cross(w, r)
+// J = [I r_skew]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
+: b2Joint(def)
+{
+ m_target = def->target;
+ m_localAnchor = b2MulT(m_body2->GetXForm(), m_target);
+
+ m_maxForce = B2FORCE_INV_SCALE(def->maxForce);
+ m_impulse.SetZero();
+
+ float32 mass = m_body2->m_mass;
+
+ // Frequency
+ float32 omega = 2.0f * b2_pi * def->frequencyHz;
+
+ // Damping coefficient
+ float32 d = 2.0f * mass * def->dampingRatio * omega;
+
+ // Spring stiffness
+ float32 k = (def->timeStep * mass) * (omega * omega);
+
+ // magic formulas
+ b2Assert(d + k > B2_FLT_EPSILON);
+ m_gamma = 1.0f / (d + k);
+ m_beta = k / (d + k);
+}
+
+void b2MouseJoint::SetTarget(const b2Vec2& target)
+{
+ if (m_body2->IsSleeping())
+ {
+ m_body2->WakeUp();
+ }
+ m_target = target;
+}
+
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b = m_body2;
+
+ // Compute the effective mass matrix.
+ b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
+
+ // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+ // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+ // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
+ float32 invMass = b->m_invMass;
+ float32 invI = b->m_invI;
+
+ b2Mat22 K1;
+ K1.col1.x = invMass; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = invMass;
+
+ b2Mat22 K2;
+ K2.col1.x = invI * r.y * r.y; K2.col2.x = -invI * r.x * r.y;
+ K2.col1.y = -invI * r.x * r.y; K2.col2.y = invI * r.x * r.x;
+
+ b2Mat22 K = K1 + K2;
+ K.col1.x += m_gamma;
+ K.col2.y += m_gamma;
+
+ m_mass = K.Invert();
+
+ m_C = b->m_sweep.c + r - m_target;
+
+ // Cheat with some damping
+ b->m_angularVelocity *= 0.98f;
+
+ // Warm starting.
+ b2Vec2 P = B2FORCE_SCALE(step.dt) * m_impulse;
+ b->m_linearVelocity += invMass * P;
+ b->m_angularVelocity += invI * b2Cross(r, P);
+}
+
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b = m_body2;
+
+ b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
+
+ // Cdot = v + cross(w, r)
+ b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+ b2Vec2 force = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C + B2FORCE_SCALE(step.dt) * (m_gamma * m_impulse));
+
+ b2Vec2 oldForce = m_impulse;
+ m_impulse += force;
+ float32 forceMagnitude = m_impulse.Length();
+ if (forceMagnitude > m_maxForce)
+ {
+ m_impulse *= m_maxForce / forceMagnitude;
+ }
+ force = m_impulse - oldForce;
+
+ b2Vec2 P = B2FORCE_SCALE(step.dt) * force;
+ b->m_linearVelocity += b->m_invMass * P;
+ b->m_angularVelocity += b->m_invI * b2Cross(r, P);
+}
+
+b2Vec2 b2MouseJoint::GetAnchor1() const
+{
+ return m_target;
+}
+
+b2Vec2 b2MouseJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor);
+}
+
+b2Vec2 b2MouseJoint::GetReactionForce() const
+{
+ return B2FORCE_SCALE(float32(1.0))*m_impulse;
+}
+
+float32 b2MouseJoint::GetReactionTorque() const
+{
+ return 0.0f;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_MOUSE_JOINT_H
+#define B2_MOUSE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Mouse joint definition. This requires a world target point,
+/// tuning parameters, and the time step.
+struct b2MouseJointDef : public b2JointDef
+{
+ b2MouseJointDef()
+ {
+ type = e_mouseJoint;
+ target.Set(0.0f, 0.0f);
+ maxForce = 0.0f;
+ frequencyHz = 5.0f;
+ dampingRatio = 0.7f;
+ timeStep = 1.0f / 60.0f;
+ }
+
+ /// The initial world target point. This is assumed
+ /// to coincide with the body anchor initially.
+ b2Vec2 target;
+
+ /// The maximum constraint force that can be exerted
+ /// to move the candidate body. Usually you will express
+ /// as some multiple of the weight (multiplier * mass * gravity).
+ float32 maxForce;
+
+ /// The response speed.
+ float32 frequencyHz;
+
+ /// The damping ratio. 0 = no damping, 1 = critical damping.
+ float32 dampingRatio;
+
+ /// The time step used in the simulation.
+ float32 timeStep;
+};
+
+/// A mouse joint is used to make a point on a body track a
+/// specified world point. This a soft constraint with a maximum
+/// force. This allows the constraint to stretch and without
+/// applying huge forces.
+class b2MouseJoint : public b2Joint
+{
+public:
+
+ /// Implements b2Joint.
+ b2Vec2 GetAnchor1() const;
+
+ /// Implements b2Joint.
+ b2Vec2 GetAnchor2() const;
+
+ /// Implements b2Joint.
+ b2Vec2 GetReactionForce() const;
+
+ /// Implements b2Joint.
+ float32 GetReactionTorque() const;
+
+ /// Use this to update the target point.
+ void SetTarget(const b2Vec2& target);
+
+ //--------------- Internals Below -------------------
+
+ b2MouseJoint(const b2MouseJointDef* def);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints()
+ {
+ return true;
+ }
+
+ b2Vec2 m_localAnchor;
+ b2Vec2 m_target;
+ b2Vec2 m_impulse;
+
+ b2Mat22 m_mass; // effective mass for point-to-point constraint.
+ b2Vec2 m_C; // position error
+ float32 m_maxForce;
+ float32 m_beta; // bias factor
+ float32 m_gamma; // softness
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(ay1, d)
+// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
+// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+ body1 = b1;
+ body2 = b2;
+ localAnchor1 = body1->GetLocalPoint(anchor);
+ localAnchor2 = body2->GetLocalPoint(anchor);
+ localAxis1 = body1->GetLocalVector(axis);
+ referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchor1 = def->localAnchor1;
+ m_localAnchor2 = def->localAnchor2;
+ m_localXAxis1 = def->localAxis1;
+ m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+ m_refAngle = def->referenceAngle;
+
+ m_linearJacobian.SetZero();
+ m_linearMass = 0.0f;
+ m_force = 0.0f;
+
+ m_angularMass = 0.0f;
+ m_torque = 0.0f;
+
+ m_motorJacobian.SetZero();
+ m_motorMass = 0.0;
+ m_motorForce = 0.0f;
+ m_limitForce = 0.0f;
+ m_limitPositionImpulse = 0.0f;
+
+ m_lowerTranslation = def->lowerTranslation;
+ m_upperTranslation = def->upperTranslation;
+ m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
+ m_motorSpeed = def->motorSpeed;
+ m_enableLimit = def->enableLimit;
+ m_enableMotor = def->enableMotor;
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ // Compute the effective masses.
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ // Compute point to line constraint effective mass.
+ // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+ b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+ b2Vec2 e = b2->m_sweep.c + r2 - b1->m_sweep.c; // e = d + r1
+
+ m_linearJacobian.Set(-ay1, -b2Cross(e, ay1), ay1, b2Cross(r2, ay1));
+ m_linearMass = invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 +
+ invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
+ b2Assert(m_linearMass > B2_FLT_EPSILON);
+ m_linearMass = 1.0f / m_linearMass;
+
+ // Compute angular constraint effective mass.
+ m_angularMass = invI1 + invI2;
+ if (m_angularMass > B2_FLT_EPSILON)
+ {
+ m_angularMass = 1.0f / m_angularMass;
+ }
+
+ // Compute motor and limit terms.
+ if (m_enableLimit || m_enableMotor)
+ {
+ // The motor and limit share a Jacobian and effective mass.
+ b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+ m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
+ m_motorMass = invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
+ invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
+ b2Assert(m_motorMass > B2_FLT_EPSILON);
+ m_motorMass = 1.0f / m_motorMass;
+
+ if (m_enableLimit)
+ {
+ b2Vec2 d = e - r1; // p2 - p1
+ float32 jointTranslation = b2Dot(ax1, d);
+ if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+ {
+ m_limitState = e_equalLimits;
+ }
+ else if (jointTranslation <= m_lowerTranslation)
+ {
+ if (m_limitState != e_atLowerLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atLowerLimit;
+ }
+ else if (jointTranslation >= m_upperTranslation)
+ {
+ if (m_limitState != e_atUpperLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atUpperLimit;
+ }
+ else
+ {
+ m_limitState = e_inactiveLimit;
+ m_limitForce = 0.0f;
+ }
+ }
+ }
+
+ if (m_enableMotor == false)
+ {
+ m_motorForce = 0.0f;
+ }
+
+ if (m_enableLimit == false)
+ {
+ m_limitForce = 0.0f;
+ }
+
+ if (step.warmStarting)
+ {
+ b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
+ b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
+ float32 L1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
+ float32 L2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
+
+ b1->m_linearVelocity += invMass1 * P1;
+ b1->m_angularVelocity += invI1 * L1;
+
+ b2->m_linearVelocity += invMass2 * P2;
+ b2->m_angularVelocity += invI2 * L2;
+ }
+ else
+ {
+ m_force = 0.0f;
+ m_torque = 0.0f;
+ m_limitForce = 0.0f;
+ m_motorForce = 0.0f;
+ }
+
+ m_limitPositionImpulse = 0.0f;
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ // Solve linear constraint.
+ float32 linearCdot = m_linearJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_linearMass * linearCdot;
+ m_force += force;
+
+ float32 P = B2FORCE_SCALE(step.dt) * force;
+ b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
+
+ // Solve angular constraint.
+ float32 angularCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+ float32 torque = -B2FORCE_INV_SCALE(step.inv_dt) * m_angularMass * angularCdot;
+ m_torque += torque;
+
+ float32 L = B2FORCE_SCALE(step.dt) * torque;
+ b1->m_angularVelocity -= invI1 * L;
+ b2->m_angularVelocity += invI2 * L;
+
+ // Solve linear motor constraint.
+ if (m_enableMotor && m_limitState != e_equalLimits)
+ {
+ float32 motorCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity) - m_motorSpeed;
+ float32 motorForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * motorCdot;
+ float32 oldMotorForce = m_motorForce;
+ m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
+ motorForce = m_motorForce - oldMotorForce;
+
+ float32 P = B2FORCE_SCALE(step.dt) * motorForce;
+ b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+ }
+
+ // Solve linear limit constraint.
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ float32 limitCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+ float32 limitForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * limitCdot;
+
+ if (m_limitState == e_equalLimits)
+ {
+ m_limitForce += limitForce;
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+
+ float32 P = B2FORCE_SCALE(step.dt) * limitForce;
+
+ b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+ }
+}
+
+bool b2PrismaticJoint::SolvePositionConstraints()
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+
+ // Solve linear (point-to-line) constraint.
+ float32 linearC = b2Dot(ay1, d);
+ // Prevent overly large corrections.
+ linearC = b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+ float32 linearImpulse = -m_linearMass * linearC;
+
+ b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
+ b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
+ //b1->SynchronizeTransform(); // updated by angular constraint
+ b2->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
+ b2->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
+ //b2->SynchronizeTransform(); // updated by angular constraint
+
+ float32 positionError = b2Abs(linearC);
+
+ // Solve angular constraint.
+ float32 angularC = b2->m_sweep.a - b1->m_sweep.a - m_refAngle;
+ // Prevent overly large corrections.
+ angularC = b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+ float32 angularImpulse = -m_angularMass * angularC;
+
+ b1->m_sweep.a -= b1->m_invI * angularImpulse;
+ b2->m_sweep.a += b2->m_invI * angularImpulse;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ float32 angularError = b2Abs(angularC);
+
+ // Solve linear limit constraint.
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+
+ float32 translation = b2Dot(ax1, d);
+ float32 limitImpulse = 0.0f;
+
+ if (m_limitState == e_equalLimits)
+ {
+ // Prevent large angular corrections
+ float32 limitC = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ positionError = b2Max(positionError, b2Abs(angularC));
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 limitC = translation - m_lowerTranslation;
+ positionError = b2Max(positionError, -limitC);
+
+ // Prevent large linear corrections and allow some slop.
+ limitC = b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 limitC = translation - m_upperTranslation;
+ positionError = b2Max(positionError, limitC);
+
+ // Prevent large linear corrections and allow some slop.
+ limitC = b2Clamp(limitC - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+
+ b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
+ b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
+ b2->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
+ b2->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
+
+ return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce() const
+{
+ b2Vec2 ax1 = b2Mul(m_body1->GetXForm().R, m_localXAxis1);
+ b2Vec2 ay1 = b2Mul(m_body1->GetXForm().R, m_localYAxis1);
+
+ return B2FORCE_SCALE(float32(1.0))*(m_limitForce * ax1 + m_force * ay1);
+}
+
+float32 b2PrismaticJoint::GetReactionTorque() const
+{
+ return B2FORCE_SCALE(m_torque);
+}
+
+float32 b2PrismaticJoint::GetJointTranslation() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+ b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ float32 translation = b2Dot(d, axis);
+ return translation;
+}
+
+float32 b2PrismaticJoint::GetJointSpeed() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ float32 w1 = b1->m_angularVelocity;
+ float32 w2 = b2->m_angularVelocity;
+
+ float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+ return speed;
+}
+
+bool b2PrismaticJoint::IsLimitEnabled() const
+{
+ return m_enableLimit;
+}
+
+void b2PrismaticJoint::EnableLimit(bool flag)
+{
+ m_enableLimit = flag;
+}
+
+float32 b2PrismaticJoint::GetLowerLimit() const
+{
+ return m_lowerTranslation;
+}
+
+float32 b2PrismaticJoint::GetUpperLimit() const
+{
+ return m_upperTranslation;
+}
+
+void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
+{
+ b2Assert(lower <= upper);
+ m_lowerTranslation = lower;
+ m_upperTranslation = upper;
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+ return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+ m_enableMotor = flag;
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+{
+ m_motorSpeed = speed;
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+{
+ m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
+}
+
+float32 b2PrismaticJoint::GetMotorForce() const
+{
+ return m_motorForce;
+}
+
+
+
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_PRISMATIC_JOINT_H
+#define B2_PRISMATIC_JOINT_H
+
+#include "b2Joint.h"
+
+/// Prismatic joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space. Using local
+/// anchors and a local axis helps when saving and loading a game.
+struct b2PrismaticJointDef : public b2JointDef
+{
+ b2PrismaticJointDef()
+ {
+ type = e_prismaticJoint;
+ localAnchor1.SetZero();
+ localAnchor2.SetZero();
+ localAxis1.Set(1.0f, 0.0f);
+ referenceAngle = 0.0f;
+ enableLimit = false;
+ lowerTranslation = 0.0f;
+ upperTranslation = 0.0f;
+ enableMotor = false;
+ maxMotorForce = 0.0f;
+ motorSpeed = 0.0f;
+ }
+
+ /// Initialize the bodies, anchors, axis, and reference angle using the world
+ /// anchor and world axis.
+ void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor, const b2Vec2& axis);
+
+ /// The local anchor point relative to body1's origin.
+ b2Vec2 localAnchor1;
+
+ /// The local anchor point relative to body2's origin.
+ b2Vec2 localAnchor2;
+
+ /// The local translation axis in body1.
+ b2Vec2 localAxis1;
+
+ /// The constrained angle between the bodies: body2_angle - body1_angle.
+ float32 referenceAngle;
+
+ /// Enable/disable the joint limit.
+ bool enableLimit;
+
+ /// The lower translation limit, usually in meters.
+ float32 lowerTranslation;
+
+ /// The upper translation limit, usually in meters.
+ float32 upperTranslation;
+
+ /// Enable/disable the joint motor.
+ bool enableMotor;
+
+ /// The maximum motor torque, usually in N-m.
+ float32 maxMotorForce;
+
+ /// The desired motor speed in radians per second.
+ float32 motorSpeed;
+};
+
+/// A prismatic joint. This joint provides one degree of freedom: translation
+/// along an axis fixed in body1. Relative rotation is prevented. You can
+/// use a joint limit to restrict the range of motion and a joint motor to
+/// drive the motion or to model joint friction.
+class b2PrismaticJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchor1() const;
+ b2Vec2 GetAnchor2() const;
+
+ b2Vec2 GetReactionForce() const;
+ float32 GetReactionTorque() const;
+
+ /// Get the current joint translation, usually in meters.
+ float32 GetJointTranslation() const;
+
+ /// Get the current joint translation speed, usually in meters per second.
+ float32 GetJointSpeed() const;
+
+ /// Is the joint limit enabled?
+ bool IsLimitEnabled() const;
+
+ /// Enable/disable the joint limit.
+ void EnableLimit(bool flag);
+
+ /// Get the lower joint limit, usually in meters.
+ float32 GetLowerLimit() const;
+
+ /// Get the upper joint limit, usually in meters.
+ float32 GetUpperLimit() const;
+
+ /// Set the joint limits, usually in meters.
+ void SetLimits(float32 lower, float32 upper);
+
+ /// Is the joint motor enabled?
+ bool IsMotorEnabled() const;
+
+ /// Enable/disable the joint motor.
+ void EnableMotor(bool flag);
+
+ /// Set the motor speed, usually in meters per second.
+ void SetMotorSpeed(float32 speed);
+
+ /// Get the motor speed, usually in meters per second.
+ float32 GetMotorSpeed() const;
+
+ /// Set the maximum motor force, usually in N.
+ void SetMaxMotorForce(float32 force);
+
+ /// Get the current motor force, usually in N.
+ float32 GetMotorForce() const;
+
+ //--------------- Internals Below -------------------
+
+ b2PrismaticJoint(const b2PrismaticJointDef* def);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints();
+
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+ b2Vec2 m_localXAxis1;
+ b2Vec2 m_localYAxis1;
+ float32 m_refAngle;
+
+ b2Jacobian m_linearJacobian;
+ float32 m_linearMass; // effective mass for point-to-line constraint.
+ float32 m_force;
+
+ float32 m_angularMass; // effective mass for angular constraint.
+ float32 m_torque;
+
+ b2Jacobian m_motorJacobian;
+ float32 m_motorMass; // effective mass for motor/limit translational constraint.
+ float32 m_motorForce;
+ float32 m_limitForce;
+ float32 m_limitPositionImpulse;
+
+ float32 m_lowerTranslation;
+ float32 m_upperTranslation;
+ float32 m_maxMotorForce;
+ float32 m_motorSpeed;
+
+ bool m_enableLimit;
+ bool m_enableMotor;
+ b2LimitState m_limitState;
+};
+
+inline float32 b2PrismaticJoint::GetMotorSpeed() const
+{
+ return m_motorSpeed;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PulleyJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Pulley:
+// length1 = norm(p1 - s1)
+// length2 = norm(p2 - s2)
+// C0 = (length1 + ratio * length2)_initial
+// C = C0 - (length1 + ratio * length2) >= 0
+// u1 = (p1 - s1) / norm(p1 - s1)
+// u2 = (p2 - s2) / norm(p2 - s2)
+// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
+// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
+// K = J * invM * JT
+// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
+//
+// Limit:
+// C = maxLength - length
+// u = (p - s) / norm(p - s)
+// Cdot = -dot(u, v + cross(w, r))
+// K = invMass + invI * cross(r, u)^2
+// 0 <= impulse
+
+void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
+ const b2Vec2& ga1, const b2Vec2& ga2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2,
+ float32 r)
+{
+ body1 = b1;
+ body2 = b2;
+ groundAnchor1 = ga1;
+ groundAnchor2 = ga2;
+ localAnchor1 = body1->GetLocalPoint(anchor1);
+ localAnchor2 = body2->GetLocalPoint(anchor2);
+ b2Vec2 d1 = anchor1 - ga1;
+ length1 = d1.Length();
+ b2Vec2 d2 = anchor2 - ga2;
+ length2 = d2.Length();
+ ratio = r;
+ b2Assert(ratio > B2_FLT_EPSILON);
+ float32 C = length1 + ratio * length2;
+ maxLength1 = C - ratio * b2_minPulleyLength;
+ maxLength2 = (C - b2_minPulleyLength) / ratio;
+}
+
+b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
+: b2Joint(def)
+{
+ m_ground = m_body1->GetWorld()->GetGroundBody();
+ m_groundAnchor1 = def->groundAnchor1 - m_ground->GetXForm().position;
+ m_groundAnchor2 = def->groundAnchor2 - m_ground->GetXForm().position;
+ m_localAnchor1 = def->localAnchor1;
+ m_localAnchor2 = def->localAnchor2;
+
+ b2Assert(def->ratio != 0.0f);
+ m_ratio = def->ratio;
+
+ m_constant = def->length1 + m_ratio * def->length2;
+
+ m_maxLength1 = b2Min(def->maxLength1, m_constant - m_ratio * b2_minPulleyLength);
+ m_maxLength2 = b2Min(def->maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
+
+ m_force = 0.0f;
+ m_limitForce1 = 0.0f;
+ m_limitForce2 = 0.0f;
+}
+
+void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
+ b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+
+ // Get the pulley axes.
+ m_u1 = p1 - s1;
+ m_u2 = p2 - s2;
+
+ float32 length1 = m_u1.Length();
+ float32 length2 = m_u2.Length();
+
+ if (length1 > b2_linearSlop)
+ {
+ m_u1 *= 1.0f / length1;
+ }
+ else
+ {
+ m_u1.SetZero();
+ }
+
+ if (length2 > b2_linearSlop)
+ {
+ m_u2 *= 1.0f / length2;
+ }
+ else
+ {
+ m_u2.SetZero();
+ }
+
+ float32 C = m_constant - length1 - m_ratio * length2;
+ if (C > 0.0f)
+ {
+ m_state = e_inactiveLimit;
+ m_force = 0.0f;
+ }
+ else
+ {
+ m_state = e_atUpperLimit;
+ m_positionImpulse = 0.0f;
+ }
+
+ if (length1 < m_maxLength1)
+ {
+ m_limitState1 = e_inactiveLimit;
+ m_limitForce1 = 0.0f;
+ }
+ else
+ {
+ m_limitState1 = e_atUpperLimit;
+ m_limitPositionImpulse1 = 0.0f;
+ }
+
+ if (length2 < m_maxLength2)
+ {
+ m_limitState2 = e_inactiveLimit;
+ m_limitForce2 = 0.0f;
+ }
+ else
+ {
+ m_limitState2 = e_atUpperLimit;
+ m_limitPositionImpulse2 = 0.0f;
+ }
+
+ // Compute effective mass.
+ float32 cr1u1 = b2Cross(r1, m_u1);
+ float32 cr2u2 = b2Cross(r2, m_u2);
+
+ m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
+ m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
+ m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
+ b2Assert(m_limitMass1 > B2_FLT_EPSILON);
+ b2Assert(m_limitMass2 > B2_FLT_EPSILON);
+ b2Assert(m_pulleyMass > B2_FLT_EPSILON);
+ m_limitMass1 = 1.0f / m_limitMass1;
+ m_limitMass2 = 1.0f / m_limitMass2;
+ m_pulleyMass = 1.0f / m_pulleyMass;
+
+ if (step.warmStarting)
+ {
+ // Warm starting.
+ b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
+ b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+ }
+ else
+ {
+ m_force = 0.0f;
+ m_limitForce1 = 0.0f;
+ m_limitForce2 = 0.0f;
+ }
+}
+
+void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ if (m_state == e_atUpperLimit)
+ {
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+
+ float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_pulleyMass * Cdot;
+ float32 oldForce = m_force;
+ m_force = b2Max(0.0f, m_force + force);
+ force = m_force - oldForce;
+
+ b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+ b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * m_u2;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+ }
+
+ if (m_limitState1 == e_atUpperLimit)
+ {
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+
+ float32 Cdot = -b2Dot(m_u1, v1);
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass1 * Cdot;
+ float32 oldForce = m_limitForce1;
+ m_limitForce1 = b2Max(0.0f, m_limitForce1 + force);
+ force = m_limitForce1 - oldForce;
+
+ b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+ }
+
+ if (m_limitState2 == e_atUpperLimit)
+ {
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+
+ float32 Cdot = -b2Dot(m_u2, v2);
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass2 * Cdot;
+ float32 oldForce = m_limitForce2;
+ m_limitForce2 = b2Max(0.0f, m_limitForce2 + force);
+ force = m_limitForce2 - oldForce;
+
+ b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+ }
+}
+
+bool b2PulleyJoint::SolvePositionConstraints()
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
+ b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+
+ float32 linearError = 0.0f;
+
+ if (m_state == e_atUpperLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ // Get the pulley axes.
+ m_u1 = p1 - s1;
+ m_u2 = p2 - s2;
+
+ float32 length1 = m_u1.Length();
+ float32 length2 = m_u2.Length();
+
+ if (length1 > b2_linearSlop)
+ {
+ m_u1 *= 1.0f / length1;
+ }
+ else
+ {
+ m_u1.SetZero();
+ }
+
+ if (length2 > b2_linearSlop)
+ {
+ m_u2 *= 1.0f / length2;
+ }
+ else
+ {
+ m_u2.SetZero();
+ }
+
+ float32 C = m_constant - length1 - m_ratio * length2;
+ linearError = b2Max(linearError, -C);
+
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ float32 impulse = -m_pulleyMass * C;
+ float32 oldImpulse = m_positionImpulse;
+ m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse);
+ impulse = m_positionImpulse - oldImpulse;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b2Vec2 P2 = -m_ratio * impulse * m_u2;
+
+ b1->m_sweep.c += b1->m_invMass * P1;
+ b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+ b2->m_sweep.c += b2->m_invMass * P2;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
+
+ if (m_limitState1 == e_atUpperLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+
+ m_u1 = p1 - s1;
+ float32 length1 = m_u1.Length();
+
+ if (length1 > b2_linearSlop)
+ {
+ m_u1 *= 1.0f / length1;
+ }
+ else
+ {
+ m_u1.SetZero();
+ }
+
+ float32 C = m_maxLength1 - length1;
+ linearError = b2Max(linearError, -C);
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ float32 impulse = -m_limitMass1 * C;
+ float32 oldLimitPositionImpulse = m_limitPositionImpulse1;
+ m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse);
+ impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b1->m_sweep.c += b1->m_invMass * P1;
+ b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+
+ b1->SynchronizeTransform();
+ }
+
+ if (m_limitState2 == e_atUpperLimit)
+ {
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ m_u2 = p2 - s2;
+ float32 length2 = m_u2.Length();
+
+ if (length2 > b2_linearSlop)
+ {
+ m_u2 *= 1.0f / length2;
+ }
+ else
+ {
+ m_u2.SetZero();
+ }
+
+ float32 C = m_maxLength2 - length2;
+ linearError = b2Max(linearError, -C);
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ float32 impulse = -m_limitMass2 * C;
+ float32 oldLimitPositionImpulse = m_limitPositionImpulse2;
+ m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse);
+ impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse;
+
+ b2Vec2 P2 = -impulse * m_u2;
+ b2->m_sweep.c += b2->m_invMass * P2;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+ b2->SynchronizeTransform();
+ }
+
+ return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PulleyJoint::GetReactionForce() const
+{
+ b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
+ return F;
+}
+
+float32 b2PulleyJoint::GetReactionTorque() const
+{
+ return 0.0f;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor1() const
+{
+ return m_ground->GetXForm().position + m_groundAnchor1;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor2() const
+{
+ return m_ground->GetXForm().position + m_groundAnchor2;
+}
+
+float32 b2PulleyJoint::GetLength1() const
+{
+ b2Vec2 p = m_body1->GetWorldPoint(m_localAnchor1);
+ b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor1;
+ b2Vec2 d = p - s;
+ return d.Length();
+}
+
+float32 b2PulleyJoint::GetLength2() const
+{
+ b2Vec2 p = m_body2->GetWorldPoint(m_localAnchor2);
+ b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor2;
+ b2Vec2 d = p - s;
+ return d.Length();
+}
+
+float32 b2PulleyJoint::GetRatio() const
+{
+ return m_ratio;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_PULLEY_JOINT_H
+#define B2_PULLEY_JOINT_H
+
+#include "b2Joint.h"
+
+const float32 b2_minPulleyLength = 2.0f;
+
+/// Pulley joint definition. This requires two ground anchors,
+/// two dynamic body anchor points, max lengths for each side,
+/// and a pulley ratio.
+struct b2PulleyJointDef : public b2JointDef
+{
+ b2PulleyJointDef()
+ {
+ type = e_pulleyJoint;
+ groundAnchor1.Set(-1.0f, 1.0f);
+ groundAnchor2.Set(1.0f, 1.0f);
+ localAnchor1.Set(-1.0f, 0.0f);
+ localAnchor2.Set(1.0f, 0.0f);
+ length1 = 0.0f;
+ maxLength1 = 0.0f;
+ length2 = 0.0f;
+ maxLength2 = 0.0f;
+ ratio = 1.0f;
+ collideConnected = true;
+ }
+
+ /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
+ void Initialize(b2Body* body1, b2Body* body2,
+ const b2Vec2& groundAnchor1, const b2Vec2& groundAnchor2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2,
+ float32 ratio);
+
+ /// The first ground anchor in world coordinates. This point never moves.
+ b2Vec2 groundAnchor1;
+
+ /// The second ground anchor in world coordinates. This point never moves.
+ b2Vec2 groundAnchor2;
+
+ /// The local anchor point relative to body1's origin.
+ b2Vec2 localAnchor1;
+
+ /// The local anchor point relative to body2's origin.
+ b2Vec2 localAnchor2;
+
+ /// The a reference length for the segment attached to body1.
+ float32 length1;
+
+ /// The maximum length of the segment attached to body1.
+ float32 maxLength1;
+
+ /// The a reference length for the segment attached to body2.
+ float32 length2;
+
+ /// The maximum length of the segment attached to body2.
+ float32 maxLength2;
+
+ /// The pulley ratio, used to simulate a block-and-tackle.
+ float32 ratio;
+};
+
+/// The pulley joint is connected to two bodies and two fixed ground points.
+/// The pulley supports a ratio such that:
+/// length1 + ratio * length2 <= constant
+/// Yes, the force transmitted is scaled by the ratio.
+/// The pulley also enforces a maximum length limit on both sides. This is
+/// useful to prevent one side of the pulley hitting the top.
+class b2PulleyJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchor1() const;
+ b2Vec2 GetAnchor2() const;
+
+ b2Vec2 GetReactionForce() const;
+ float32 GetReactionTorque() const;
+
+ /// Get the first ground anchor.
+ b2Vec2 GetGroundAnchor1() const;
+
+ /// Get the second ground anchor.
+ b2Vec2 GetGroundAnchor2() const;
+
+ /// Get the current length of the segment attached to body1.
+ float32 GetLength1() const;
+
+ /// Get the current length of the segment attached to body2.
+ float32 GetLength2() const;
+
+ /// Get the pulley ratio.
+ float32 GetRatio() const;
+
+ //--------------- Internals Below -------------------
+
+ b2PulleyJoint(const b2PulleyJointDef* data);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints();
+
+ b2Body* m_ground;
+ b2Vec2 m_groundAnchor1;
+ b2Vec2 m_groundAnchor2;
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+
+ b2Vec2 m_u1;
+ b2Vec2 m_u2;
+
+ float32 m_constant;
+ float32 m_ratio;
+
+ float32 m_maxLength1;
+ float32 m_maxLength2;
+
+ // Effective masses
+ float32 m_pulleyMass;
+ float32 m_limitMass1;
+ float32 m_limitMass2;
+
+ // Impulses for accumulation/warm starting.
+ float32 m_force;
+ float32 m_limitForce1;
+ float32 m_limitForce2;
+
+ // Position impulses for accumulation.
+ float32 m_positionImpulse;
+ float32 m_limitPositionImpulse1;
+ float32 m_limitPositionImpulse2;
+
+ b2LimitState m_state;
+ b2LimitState m_limitState1;
+ b2LimitState m_limitState2;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2RevoluteJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+#include "../b2Island.h"
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
+{
+ body1 = b1;
+ body2 = b2;
+ localAnchor1 = body1->GetLocalPoint(anchor);
+ localAnchor2 = body2->GetLocalPoint(anchor);
+ referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchor1 = def->localAnchor1;
+ m_localAnchor2 = def->localAnchor2;
+ m_referenceAngle = def->referenceAngle;
+
+ m_pivotForce.Set(0.0f, 0.0f);
+ m_motorForce = 0.0f;
+ m_limitForce = 0.0f;
+ m_limitPositionImpulse = 0.0f;
+
+ m_lowerAngle = def->lowerAngle;
+ m_upperAngle = def->upperAngle;
+ m_maxMotorTorque = def->maxMotorTorque;
+ m_motorSpeed = def->motorSpeed;
+ m_enableLimit = def->enableLimit;
+ m_enableMotor = def->enableMotor;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ // Compute the effective mass matrix.
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+ // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+ // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ b2Mat22 K1;
+ K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
+
+ b2Mat22 K2;
+ K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y;
+ K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x;
+
+ b2Mat22 K3;
+ K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y;
+ K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x;
+
+ b2Mat22 K = K1 + K2 + K3;
+ m_pivotMass = K.Invert();
+
+ m_motorMass = 1.0f / (invI1 + invI2);
+
+ if (m_enableMotor == false)
+ {
+ m_motorForce = 0.0f;
+ }
+
+ if (m_enableLimit)
+ {
+ float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+ if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
+ {
+ m_limitState = e_equalLimits;
+ }
+ else if (jointAngle <= m_lowerAngle)
+ {
+ if (m_limitState != e_atLowerLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atLowerLimit;
+ }
+ else if (jointAngle >= m_upperAngle)
+ {
+ if (m_limitState != e_atUpperLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atUpperLimit;
+ }
+ else
+ {
+ m_limitState = e_inactiveLimit;
+ m_limitForce = 0.0f;
+ }
+ }
+ else
+ {
+ m_limitForce = 0.0f;
+ }
+
+ if (step.warmStarting)
+ {
+ b1->m_linearVelocity -= B2FORCE_SCALE(step.dt) * invMass1 * m_pivotForce;
+ b1->m_angularVelocity -= B2FORCE_SCALE(step.dt) * invI1 * (b2Cross(r1, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+
+ b2->m_linearVelocity += B2FORCE_SCALE(step.dt) * invMass2 * m_pivotForce;
+ b2->m_angularVelocity += B2FORCE_SCALE(step.dt) * invI2 * (b2Cross(r2, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+ }
+ else
+ {
+ m_pivotForce.SetZero();
+ m_motorForce = 0.0f;
+ m_limitForce = 0.0f;
+ }
+
+ m_limitPositionImpulse = 0.0f;
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ // Solve point-to-point constraint
+ b2Vec2 pivotCdot = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
+ b2Vec2 pivotForce = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_pivotMass, pivotCdot);
+ m_pivotForce += pivotForce;
+
+ b2Vec2 P = B2FORCE_SCALE(step.dt) * pivotForce;
+ b1->m_linearVelocity -= b1->m_invMass * P;
+ b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+
+ b2->m_linearVelocity += b2->m_invMass * P;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+
+ if (m_enableMotor && m_limitState != e_equalLimits)
+ {
+ float32 motorCdot = b2->m_angularVelocity - b1->m_angularVelocity - m_motorSpeed;
+ float32 motorForce = -step.inv_dt * m_motorMass * motorCdot;
+ float32 oldMotorForce = m_motorForce;
+ m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
+ motorForce = m_motorForce - oldMotorForce;
+
+ float32 P = step.dt * motorForce;
+ b1->m_angularVelocity -= b1->m_invI * P;
+ b2->m_angularVelocity += b2->m_invI * P;
+ }
+
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ float32 limitCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+ float32 limitForce = -step.inv_dt * m_motorMass * limitCdot;
+
+ if (m_limitState == e_equalLimits)
+ {
+ m_limitForce += limitForce;
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+
+ float32 P = step.dt * limitForce;
+ b1->m_angularVelocity -= b1->m_invI * P;
+ b2->m_angularVelocity += b2->m_invI * P;
+ }
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints()
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 positionError = 0.0f;
+
+ // Solve point-to-point position error.
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 ptpC = p2 - p1;
+
+ positionError = ptpC.Length();
+
+ // Prevent overly large corrections.
+ //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
+ //ptpC = b2Clamp(ptpC, -dpMax, dpMax);
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ b2Mat22 K1;
+ K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
+
+ b2Mat22 K2;
+ K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y;
+ K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x;
+
+ b2Mat22 K3;
+ K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y;
+ K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x;
+
+ b2Mat22 K = K1 + K2 + K3;
+ b2Vec2 impulse = K.Solve(-ptpC);
+
+ b1->m_sweep.c -= b1->m_invMass * impulse;
+ b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
+
+ b2->m_sweep.c += b2->m_invMass * impulse;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ // Handle limits.
+ float32 angularError = 0.0f;
+
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+ float32 limitImpulse = 0.0f;
+
+ if (m_limitState == e_equalLimits)
+ {
+ // Prevent large angular corrections
+ float32 limitC = b2Clamp(angle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ angularError = b2Abs(limitC);
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 limitC = angle - m_lowerAngle;
+ angularError = b2Max(0.0f, -limitC);
+
+ // Prevent large angular corrections and allow some slop.
+ limitC = b2Clamp(limitC + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 limitC = angle - m_upperAngle;
+ angularError = b2Max(0.0f, limitC);
+
+ // Prevent large angular corrections and allow some slop.
+ limitC = b2Clamp(limitC - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+
+ b1->m_sweep.a -= b1->m_invI * limitImpulse;
+ b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
+
+ return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce() const
+{
+ return B2FORCE_SCALE(float32(1.0))*m_pivotForce;
+}
+
+float32 b2RevoluteJoint::GetReactionTorque() const
+{
+ return m_limitForce;
+}
+
+float32 b2RevoluteJoint::GetJointAngle() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+ return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+}
+
+float32 b2RevoluteJoint::GetJointSpeed() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+ return b2->m_angularVelocity - b1->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+ return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+ m_enableMotor = flag;
+}
+
+float32 b2RevoluteJoint::GetMotorTorque() const
+{
+ return m_motorForce;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+ m_motorSpeed = speed;
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+ m_maxMotorTorque = torque;
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+ return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+ m_enableLimit = flag;
+}
+
+float32 b2RevoluteJoint::GetLowerLimit() const
+{
+ return m_lowerAngle;
+}
+
+float32 b2RevoluteJoint::GetUpperLimit() const
+{
+ return m_upperAngle;
+}
+
+void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
+{
+ b2Assert(lower <= upper);
+ m_lowerAngle = lower;
+ m_upperAngle = upper;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_REVOLUTE_JOINT_H
+#define B2_REVOLUTE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Revolute joint definition. This requires defining an
+/// anchor point where the bodies are joined. The definition
+/// uses local anchor points so that the initial configuration
+/// can violate the constraint slightly. You also need to
+/// specify the initial relative angle for joint limits. This
+/// helps when saving and loading a game.
+/// The local anchor points are measured from the body's origin
+/// rather than the center of mass because:
+/// 1. you might not know where the center of mass will be.
+/// 2. if you add/remove shapes from a body and recompute the mass,
+/// the joints will be broken.
+struct b2RevoluteJointDef : public b2JointDef
+{
+ b2RevoluteJointDef()
+ {
+ type = e_revoluteJoint;
+ localAnchor1.Set(0.0f, 0.0f);
+ localAnchor2.Set(0.0f, 0.0f);
+ referenceAngle = 0.0f;
+ lowerAngle = 0.0f;
+ upperAngle = 0.0f;
+ maxMotorTorque = 0.0f;
+ motorSpeed = 0.0f;
+ enableLimit = false;
+ enableMotor = false;
+ }
+
+ /// Initialize the bodies, anchors, and reference angle using the world
+ /// anchor.
+ void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+
+ /// The local anchor point relative to body1's origin.
+ b2Vec2 localAnchor1;
+
+ /// The local anchor point relative to body2's origin.
+ b2Vec2 localAnchor2;
+
+ /// The body2 angle minus body1 angle in the reference state (radians).
+ float32 referenceAngle;
+
+ /// A flag to enable joint limits.
+ bool enableLimit;
+
+ /// The lower angle for the joint limit (radians).
+ float32 lowerAngle;
+
+ /// The upper angle for the joint limit (radians).
+ float32 upperAngle;
+
+ /// A flag to enable the joint motor.
+ bool enableMotor;
+
+ /// The desired motor speed. Usually in radians per second.
+ float32 motorSpeed;
+
+ /// The maximum motor torque used to achieve the desired motor speed.
+ /// Usually in N-m.
+ float32 maxMotorTorque;
+};
+
+/// A revolute joint constrains to bodies to share a common point while they
+/// are free to rotate about the point. The relative rotation about the shared
+/// point is the joint angle. You can limit the relative rotation with
+/// a joint limit that specifies a lower and upper angle. You can use a motor
+/// to drive the relative rotation about the shared point. A maximum motor torque
+/// is provided so that infinite forces are not generated.
+class b2RevoluteJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchor1() const;
+ b2Vec2 GetAnchor2() const;
+
+ b2Vec2 GetReactionForce() const;
+ float32 GetReactionTorque() const;
+
+ /// Get the current joint angle in radians.
+ float32 GetJointAngle() const;
+
+ /// Get the current joint angle speed in radians per second.
+ float32 GetJointSpeed() const;
+
+ /// Is the joint limit enabled?
+ bool IsLimitEnabled() const;
+
+ /// Enable/disable the joint limit.
+ void EnableLimit(bool flag);
+
+ /// Get the lower joint limit in radians.
+ float32 GetLowerLimit() const;
+
+ /// Get the upper joint limit in radians.
+ float32 GetUpperLimit() const;
+
+ /// Set the joint limits in radians.
+ void SetLimits(float32 lower, float32 upper);
+
+ /// Is the joint motor enabled?
+ bool IsMotorEnabled() const;
+
+ /// Enable/disable the joint motor.
+ void EnableMotor(bool flag);
+
+ /// Set the motor speed in radians per second.
+ void SetMotorSpeed(float32 speed);
+
+ /// Get the motor speed in radians per second.
+ float32 GetMotorSpeed() const;
+
+ /// Set the maximum motor torque, usually in N-m.
+ void SetMaxMotorTorque(float32 torque);
+
+ /// Get the current motor torque, usually in N-m.
+ float32 GetMotorTorque() const;
+
+ //--------------- Internals Below -------------------
+ b2RevoluteJoint(const b2RevoluteJointDef* def);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+
+ bool SolvePositionConstraints();
+
+ b2Vec2 m_localAnchor1; // relative
+ b2Vec2 m_localAnchor2;
+ b2Vec2 m_pivotForce;
+ float32 m_motorForce;
+ float32 m_limitForce;
+ float32 m_limitPositionImpulse;
+
+ b2Mat22 m_pivotMass; // effective mass for point-to-point constraint.
+ float32 m_motorMass; // effective mass for motor/limit angular constraint.
+
+ bool m_enableMotor;
+ float32 m_maxMotorTorque;
+ float32 m_motorSpeed;
+
+ bool m_enableLimit;
+ float32 m_referenceAngle;
+ float32 m_lowerAngle;
+ float32 m_upperAngle;
+ b2LimitState m_limitState;
+};
+
+inline float32 b2RevoluteJoint::GetMotorSpeed() const
+{
+ return m_motorSpeed;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Body.h"
+#include "b2World.h"
+#include "Joints/b2Joint.h"
+#include "../Collision/Shapes/b2Shape.h"
+
+b2Body::b2Body(const b2BodyDef* bd, b2World* world)
+{
+ b2Assert(world->m_lock == false);
+
+ m_flags = 0;
+
+ if (bd->isBullet)
+ {
+ m_flags |= e_bulletFlag;
+ }
+ if (bd->fixedRotation)
+ {
+ m_flags |= e_fixedRotationFlag;
+ }
+ if (bd->allowSleep)
+ {
+ m_flags |= e_allowSleepFlag;
+ }
+ if (bd->isSleeping)
+ {
+ m_flags |= e_sleepFlag;
+ }
+
+ m_world = world;
+
+ m_xf.position = bd->position;
+ m_xf.R.Set(bd->angle);
+
+ m_sweep.localCenter = bd->massData.center;
+ m_sweep.t0 = 1.0f;
+ m_sweep.a0 = m_sweep.a = bd->angle;
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+ m_jointList = NULL;
+ m_contactList = NULL;
+ m_prev = NULL;
+ m_next = NULL;
+
+ m_linearDamping = bd->linearDamping;
+ m_angularDamping = bd->angularDamping;
+
+ m_force.Set(0.0f, 0.0f);
+ m_torque = 0.0f;
+
+ m_linearVelocity.SetZero();
+ m_angularVelocity = 0.0f;
+
+ m_sleepTime = 0.0f;
+
+ m_invMass = 0.0f;
+ m_I = 0.0f;
+ m_invI = 0.0f;
+
+ m_mass = bd->massData.mass;
+
+ if (m_mass > 0.0f)
+ {
+ m_invMass = 1.0f / m_mass;
+ }
+
+ if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
+ {
+ m_I = bd->massData.I;
+ }
+
+ if (m_I > 0.0f)
+ {
+ m_invI = 1.0f / m_I;
+ }
+
+ if (m_invMass == 0.0f && m_invI == 0.0f)
+ {
+ m_type = e_staticType;
+ }
+ else
+ {
+ m_type = e_dynamicType;
+ }
+
+ m_userData = bd->userData;
+
+ m_shapeList = NULL;
+ m_shapeCount = 0;
+}
+
+b2Body::~b2Body()
+{
+ b2Assert(m_world->m_lock == false);
+ // shapes and joints are destroyed in b2World::Destroy
+}
+
+b2Shape* b2Body::CreateShape(b2ShapeDef* def)
+{
+ b2Assert(m_world->m_lock == false);
+ if (m_world->m_lock == true)
+ {
+ return NULL;
+ }
+
+ b2Shape* s = b2Shape::Create(def, &m_world->m_blockAllocator);
+
+ s->m_next = m_shapeList;
+ m_shapeList = s;
+ ++m_shapeCount;
+
+ s->m_body = this;
+
+ // Add the shape to the world's broad-phase.
+ s->CreateProxy(m_world->m_broadPhase, m_xf);
+
+ // Compute the sweep radius for CCD.
+ s->UpdateSweepRadius(m_sweep.localCenter);
+
+ return s;
+}
+
+void b2Body::DestroyShape(b2Shape* s)
+{
+ b2Assert(m_world->m_lock == false);
+ if (m_world->m_lock == true)
+ {
+ return;
+ }
+
+ b2Assert(s->GetBody() == this);
+ s->DestroyProxy(m_world->m_broadPhase);
+
+ b2Assert(m_shapeCount > 0);
+ b2Shape** node = &m_shapeList;
+ bool found = false;
+ while (*node != NULL)
+ {
+ if (*node == s)
+ {
+ *node = s->m_next;
+ found = true;
+ break;
+ }
+
+ node = &(*node)->m_next;
+ }
+
+ // You tried to remove a shape that is not attached to this body.
+ b2Assert(found);
+
+ s->m_body = NULL;
+ s->m_next = NULL;
+
+ --m_shapeCount;
+
+ b2Shape::Destroy(s, &m_world->m_blockAllocator);
+}
+
+// TODO_ERIN adjust linear velocity and torque to account for movement of center.
+void b2Body::SetMass(const b2MassData* massData)
+{
+ b2Assert(m_world->m_lock == false);
+ if (m_world->m_lock == true)
+ {
+ return;
+ }
+
+ m_invMass = 0.0f;
+ m_I = 0.0f;
+ m_invI = 0.0f;
+
+ m_mass = massData->mass;
+
+ if (m_mass > 0.0f)
+ {
+ m_invMass = 1.0f / m_mass;
+ }
+
+ if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
+ {
+ m_I = massData->I;
+ }
+
+ if (m_I > 0.0f)
+ {
+ m_invI = 1.0f / m_I;
+ }
+
+ // Move center of mass.
+ m_sweep.localCenter = massData->center;
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+ // Update the sweep radii of all child shapes.
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->UpdateSweepRadius(m_sweep.localCenter);
+ }
+
+ int16 oldType = m_type;
+ if (m_invMass == 0.0f && m_invI == 0.0f)
+ {
+ m_type = e_staticType;
+ }
+ else
+ {
+ m_type = e_dynamicType;
+ }
+
+ // If the body type changed, we need to refilter the broad-phase proxies.
+ if (oldType != m_type)
+ {
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->RefilterProxy(m_world->m_broadPhase, m_xf);
+ }
+ }
+}
+
+// TODO_ERIN adjust linear velocity and torque to account for movement of center.
+void b2Body::SetMassFromShapes()
+{
+ b2Assert(m_world->m_lock == false);
+ if (m_world->m_lock == true)
+ {
+ return;
+ }
+
+ // Compute mass data from shapes. Each shape has its own density.
+ m_mass = 0.0f;
+ m_invMass = 0.0f;
+ m_I = 0.0f;
+ m_invI = 0.0f;
+
+ b2Vec2 center = b2Vec2_zero;
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ b2MassData massData;
+ s->ComputeMass(&massData);
+ m_mass += massData.mass;
+ center += massData.mass * massData.center;
+ m_I += massData.I;
+ }
+
+ // Compute center of mass, and shift the origin to the COM.
+ if (m_mass > 0.0f)
+ {
+ m_invMass = 1.0f / m_mass;
+ center *= m_invMass;
+ }
+
+ if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
+ {
+ // Center the inertia about the center of mass.
+ m_I -= m_mass * b2Dot(center, center);
+ b2Assert(m_I > 0.0f);
+ m_invI = 1.0f / m_I;
+ }
+ else
+ {
+ m_I = 0.0f;
+ m_invI = 0.0f;
+ }
+
+ // Move center of mass.
+ m_sweep.localCenter = center;
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+ // Update the sweep radii of all child shapes.
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->UpdateSweepRadius(m_sweep.localCenter);
+ }
+
+ int16 oldType = m_type;
+ if (m_invMass == 0.0f && m_invI == 0.0f)
+ {
+ m_type = e_staticType;
+ }
+ else
+ {
+ m_type = e_dynamicType;
+ }
+
+ // If the body type changed, we need to refilter the broad-phase proxies.
+ if (oldType != m_type)
+ {
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->RefilterProxy(m_world->m_broadPhase, m_xf);
+ }
+ }
+}
+
+bool b2Body::SetXForm(const b2Vec2& position, float32 angle)
+{
+ b2Assert(m_world->m_lock == false);
+ if (m_world->m_lock == true)
+ {
+ return true;
+ }
+
+ if (IsFrozen())
+ {
+ return false;
+ }
+
+ m_xf.R.Set(angle);
+ m_xf.position = position;
+
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+ m_sweep.a0 = m_sweep.a = angle;
+
+ bool freeze = false;
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ bool inRange = s->Synchronize(m_world->m_broadPhase, m_xf, m_xf);
+
+ if (inRange == false)
+ {
+ freeze = true;
+ break;
+ }
+ }
+
+ if (freeze == true)
+ {
+ m_flags |= e_frozenFlag;
+ m_linearVelocity.SetZero();
+ m_angularVelocity = 0.0f;
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->DestroyProxy(m_world->m_broadPhase);
+ }
+
+ // Failure
+ return false;
+ }
+
+ // Success
+ m_world->m_broadPhase->Commit();
+ return true;
+}
+
+bool b2Body::SynchronizeShapes()
+{
+ b2XForm xf1;
+ xf1.R.Set(m_sweep.a0);
+ xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
+
+ bool inRange = true;
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ inRange = s->Synchronize(m_world->m_broadPhase, xf1, m_xf);
+ if (inRange == false)
+ {
+ break;
+ }
+ }
+
+ if (inRange == false)
+ {
+ m_flags |= e_frozenFlag;
+ m_linearVelocity.SetZero();
+ m_angularVelocity = 0.0f;
+ for (b2Shape* s = m_shapeList; s; s = s->m_next)
+ {
+ s->DestroyProxy(m_world->m_broadPhase);
+ }
+
+ // Failure
+ return false;
+ }
+
+ // Success
+ return true;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_BODY_H
+#define B2_BODY_H
+
+#include "../Common/b2Math.h"
+#include "../Collision/Shapes/b2Shape.h"
+#include "Joints/b2Joint.h"
+
+#include <memory>
+
+class b2Joint;
+class b2Contact;
+class b2World;
+struct b2JointEdge;
+struct b2ContactEdge;
+
+/// A body definition holds all the data needed to construct a rigid body.
+/// You can safely re-use body definitions.
+struct b2BodyDef
+{
+ /// This constructor sets the body definition default values.
+ b2BodyDef()
+ {
+ massData.center.SetZero();
+ massData.mass = 0.0f;
+ massData.I = 0.0f;
+ userData = NULL;
+ position.Set(0.0f, 0.0f);
+ angle = 0.0f;
+ linearDamping = 0.0f;
+ angularDamping = 0.0f;
+ allowSleep = true;
+ isSleeping = false;
+ fixedRotation = false;
+ isBullet = false;
+ }
+
+ /// You can use this to initialized the mass properties of the body.
+ /// If you prefer, you can set the mass properties after the shapes
+ /// have been added using b2Body::SetMassFromShapes.
+ b2MassData massData;
+
+ /// Use this to store application specific body data.
+ void* userData;
+
+ /// The world position of the body. Avoid creating bodies at the origin
+ /// since this can lead to many overlapping shapes.
+ b2Vec2 position;
+
+ /// The world angle of the body in radians.
+ float32 angle;
+
+ /// Linear damping is use to reduce the linear velocity. The damping parameter
+ /// can be larger than 1.0f but the damping effect becomes sensitive to the
+ /// time step when the damping parameter is large.
+ float32 linearDamping;
+
+ /// Angular damping is use to reduce the angular velocity. The damping parameter
+ /// can be larger than 1.0f but the damping effect becomes sensitive to the
+ /// time step when the damping parameter is large.
+ float32 angularDamping;
+
+ /// Set this flag to false if this body should never fall asleep. Note that
+ /// this increases CPU usage.
+ bool allowSleep;
+
+ /// Is this body initially sleeping?
+ bool isSleeping;
+
+ /// Should this body be prevented from rotating? Useful for characters.
+ bool fixedRotation;
+
+ /// Is this a fast moving body that should be prevented from tunneling through
+ /// other moving bodies? Note that all bodies are prevented from tunneling through
+ /// static bodies.
+ /// @warning You should use this flag sparingly since it increases processing time.
+ bool isBullet;
+};
+
+/// A rigid body.
+class b2Body
+{
+public:
+ /// Creates a shape and attach it to this body.
+ /// @param shapeDef the shape definition.
+ /// @warning This function is locked during callbacks.
+ b2Shape* CreateShape(b2ShapeDef* shapeDef);
+
+ /// Destroy a shape. This removes the shape from the broad-phase and
+ /// therefore destroys any contacts associated with this shape. All shapes
+ /// attached to a body are implicitly destroyed when the body is destroyed.
+ /// @param shape the shape to be removed.
+ /// @warning This function is locked during callbacks.
+ void DestroyShape(b2Shape* shape);
+
+ /// Set the mass properties. Note that this changes the center of mass position.
+ /// If you are not sure how to compute mass properties, use SetMassFromShapes.
+ /// The inertia tensor is assumed to be relative to the center of mass.
+ /// @param massData the mass properties.
+ void SetMass(const b2MassData* massData);
+
+ /// Compute the mass properties from the attached shapes. You typically call this
+ /// after adding all the shapes. If you add or remove shapes later, you may want
+ /// to call this again. Note that this changes the center of mass position.
+ void SetMassFromShapes();
+
+ /// Set the position of the body's origin and rotation (radians).
+ /// This breaks any contacts and wakes the other bodies.
+ /// @param position the new world position of the body's origin (not necessarily
+ /// the center of mass).
+ /// @param angle the new world rotation angle of the body in radians.
+ /// @return false if the movement put a shape outside the world. In this case the
+ /// body is automatically frozen.
+ bool SetXForm(const b2Vec2& position, float32 angle);
+
+ /// Get the body transform for the body's origin.
+ /// @return the world transform of the body's origin.
+ const b2XForm& GetXForm() const;
+
+ /// Get the world body origin position.
+ /// @return the world position of the body's origin.
+ const b2Vec2& GetPosition() const;
+
+ /// Get the angle in radians.
+ /// @return the current world rotation angle in radians.
+ float32 GetAngle() const;
+
+ /// Get the world position of the center of mass.
+ const b2Vec2& GetWorldCenter() const;
+
+ /// Get the local position of the center of mass.
+ const b2Vec2& GetLocalCenter() const;
+
+ /// Set the linear velocity of the center of mass.
+ /// @param v the new linear velocity of the center of mass.
+ void SetLinearVelocity(const b2Vec2& v);
+
+ /// Get the linear velocity of the center of mass.
+ /// @return the linear velocity of the center of mass.
+ b2Vec2 GetLinearVelocity() const;
+
+ /// Set the angular velocity.
+ /// @param omega the new angular velocity in radians/second.
+ void SetAngularVelocity(float32 omega);
+
+ /// Get the angular velocity.
+ /// @return the angular velocity in radians/second.
+ float32 GetAngularVelocity() const;
+
+ /// Apply a force at a world point. If the force is not
+ /// applied at the center of mass, it will generate a torque and
+ /// affect the angular velocity. This wakes up the body.
+ /// @param force the world force vector, usually in Newtons (N).
+ /// @param point the world position of the point of application.
+ void ApplyForce(const b2Vec2& force, const b2Vec2& point);
+
+ /// Apply a torque. This affects the angular velocity
+ /// without affecting the linear velocity of the center of mass.
+ /// This wakes up the body.
+ /// @param torque about the z-axis (out of the screen), usually in N-m.
+ void ApplyTorque(float32 torque);
+
+ /// Apply an impulse at a point. This immediately modifies the velocity.
+ /// It also modifies the angular velocity if the point of application
+ /// is not at the center of mass. This wakes up the body.
+ /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
+ /// @param point the world position of the point of application.
+ void ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point);
+
+ /// Get the total mass of the body.
+ /// @return the mass, usually in kilograms (kg).
+ float32 GetMass() const;
+
+ /// Get the central rotational inertia of the body.
+ /// @return the rotational inertia, usually in kg-m^2.
+ float32 GetInertia() const;
+
+ /// Get the world coordinates of a point given the local coordinates.
+ /// @param localPoint a point on the body measured relative the the body's origin.
+ /// @return the same point expressed in world coordinates.
+ b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const;
+
+ /// Get the world coordinates of a vector given the local coordinates.
+ /// @param localVector a vector fixed in the body.
+ /// @return the same vector expressed in world coordinates.
+ b2Vec2 GetWorldVector(const b2Vec2& localVector) const;
+
+ /// Gets a local point relative to the body's origin given a world point.
+ /// @param a point in world coordinates.
+ /// @return the corresponding local point relative to the body's origin.
+ b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const;
+
+ /// Gets a local vector given a world vector.
+ /// @param a vector in world coordinates.
+ /// @return the corresponding local vector.
+ b2Vec2 GetLocalVector(const b2Vec2& worldVector) const;
+
+ /// Get the world linear velocity of a world point attached to this body.
+ /// @param a point in world coordinates.
+ /// @return the world velocity of a point.
+ b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;
+
+ /// Get the world velocity of a local point.
+ /// @param a point in local coordinates.
+ /// @return the world velocity of a point.
+ b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;
+
+ /// Is this body treated like a bullet for continuous collision detection?
+ bool IsBullet() const;
+
+ /// Should this body be treated like a bullet for continuous collision detection?
+ void SetBullet(bool flag);
+
+ /// Is this body static (immovable)?
+ bool IsStatic() const;
+
+ /// Is this body dynamic (movable)?
+ bool IsDynamic() const;
+
+ /// Is this body frozen?
+ bool IsFrozen() const;
+
+ /// Is this body sleeping (not simulating).
+ bool IsSleeping() const;
+
+ /// You can disable sleeping on this body.
+ void AllowSleeping(bool flag);
+
+ /// Wake up this body so it will begin simulating.
+ void WakeUp();
+
+ /// Put this body to sleep so it will stop simulating.
+ /// This also sets the velocity to zero.
+ void PutToSleep();
+
+ /// Get the list of all shapes attached to this body.
+ b2Shape* GetShapeList();
+
+ /// Get the list of all joints attached to this body.
+ b2JointEdge* GetJointList();
+
+ /// Get the next body in the world's body list.
+ b2Body* GetNext();
+
+ /// Get the user data pointer that was provided in the body definition.
+ void* GetUserData();
+
+ /// Set the user data. Use this to store your application specific data.
+ void SetUserData(void* data);
+
+ /// Get the parent world of this body.
+ b2World* GetWorld();
+
+private:
+
+ friend class b2World;
+ friend class b2Island;
+ friend class b2ContactManager;
+ friend class b2ContactSolver;
+
+ friend class b2DistanceJoint;
+ friend class b2GearJoint;
+ friend class b2MouseJoint;
+ friend class b2PrismaticJoint;
+ friend class b2PulleyJoint;
+ friend class b2RevoluteJoint;
+
+ // m_flags
+ enum
+ {
+ e_frozenFlag = 0x0002,
+ e_islandFlag = 0x0004,
+ e_sleepFlag = 0x0008,
+ e_allowSleepFlag = 0x0010,
+ e_bulletFlag = 0x0020,
+ e_fixedRotationFlag = 0x0040,
+ };
+
+ // m_type
+ enum
+ {
+ e_staticType,
+ e_dynamicType,
+ e_maxTypes,
+ };
+
+ b2Body(const b2BodyDef* bd, b2World* world);
+ ~b2Body();
+
+ bool SynchronizeShapes();
+
+ void SynchronizeTransform();
+
+ // This is used to prevent connected bodies from colliding.
+ // It may lie, depending on the collideConnected flag.
+ bool IsConnected(const b2Body* other) const;
+
+ void Advance(float32 t);
+
+ uint16 m_flags;
+ int16 m_type;
+
+ b2XForm m_xf; // the body origin transform
+
+ b2Sweep m_sweep; // the swept motion for CCD
+
+ b2Vec2 m_linearVelocity;
+ float32 m_angularVelocity;
+
+ b2Vec2 m_force;
+ float32 m_torque;
+
+ b2World* m_world;
+ b2Body* m_prev;
+ b2Body* m_next;
+
+ b2Shape* m_shapeList;
+ int32 m_shapeCount;
+
+ b2JointEdge* m_jointList;
+ b2ContactEdge* m_contactList;
+
+ float32 m_mass, m_invMass;
+ float32 m_I, m_invI;
+
+ float32 m_linearDamping;
+ float32 m_angularDamping;
+
+ float32 m_sleepTime;
+
+ void* m_userData;
+};
+
+inline const b2XForm& b2Body::GetXForm() const
+{
+ return m_xf;
+}
+
+inline const b2Vec2& b2Body::GetPosition() const
+{
+ return m_xf.position;
+}
+
+inline float32 b2Body::GetAngle() const
+{
+ return m_sweep.a;
+}
+
+inline const b2Vec2& b2Body::GetWorldCenter() const
+{
+ return m_sweep.c;
+}
+
+inline const b2Vec2& b2Body::GetLocalCenter() const
+{
+ return m_sweep.localCenter;
+}
+
+inline void b2Body::SetLinearVelocity(const b2Vec2& v)
+{
+ m_linearVelocity = v;
+}
+
+inline b2Vec2 b2Body::GetLinearVelocity() const
+{
+ return m_linearVelocity;
+}
+
+inline void b2Body::SetAngularVelocity(float32 w)
+{
+ m_angularVelocity = w;
+}
+
+inline float32 b2Body::GetAngularVelocity() const
+{
+ return m_angularVelocity;
+}
+
+inline float32 b2Body::GetMass() const
+{
+ return m_mass;
+}
+
+inline float32 b2Body::GetInertia() const
+{
+ return m_I;
+}
+
+inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const
+{
+ return b2Mul(m_xf, localPoint);
+}
+
+inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const
+{
+ return b2Mul(m_xf.R, localVector);
+}
+
+inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
+{
+ return b2MulT(m_xf, worldPoint);
+}
+
+inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const
+{
+ return b2MulT(m_xf.R, worldVector);
+}
+
+inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
+{
+ return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
+}
+
+inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const
+{
+ return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
+}
+
+inline bool b2Body::IsBullet() const
+{
+ return (m_flags & e_bulletFlag) == e_bulletFlag;
+}
+
+inline void b2Body::SetBullet(bool flag)
+{
+ if (flag)
+ {
+ m_flags |= e_bulletFlag;
+ }
+ else
+ {
+ m_flags &= ~e_bulletFlag;
+ }
+}
+
+inline bool b2Body::IsStatic() const
+{
+ return m_type == e_staticType;
+}
+
+inline bool b2Body::IsDynamic() const
+{
+ return m_type == e_dynamicType;
+}
+
+inline bool b2Body::IsFrozen() const
+{
+ return (m_flags & e_frozenFlag) == e_frozenFlag;
+}
+
+inline bool b2Body::IsSleeping() const
+{
+ return (m_flags & e_sleepFlag) == e_sleepFlag;
+}
+
+inline void b2Body::AllowSleeping(bool flag)
+{
+ if (flag)
+ {
+ m_flags |= e_allowSleepFlag;
+ }
+ else
+ {
+ m_flags &= ~e_allowSleepFlag;
+ WakeUp();
+ }
+}
+
+inline void b2Body::WakeUp()
+{
+ m_flags &= ~e_sleepFlag;
+ m_sleepTime = 0.0f;
+}
+
+inline void b2Body::PutToSleep()
+{
+ m_flags |= e_sleepFlag;
+ m_sleepTime = 0.0f;
+ m_linearVelocity.SetZero();
+ m_angularVelocity = 0.0f;
+ m_force.SetZero();
+ m_torque = 0.0f;
+}
+
+inline b2Shape* b2Body::GetShapeList()
+{
+ return m_shapeList;
+}
+
+inline b2JointEdge* b2Body::GetJointList()
+{
+ return m_jointList;
+}
+
+inline b2Body* b2Body::GetNext()
+{
+ return m_next;
+}
+
+inline void* b2Body::GetUserData()
+{
+ return m_userData;
+}
+
+inline void b2Body::SetUserData(void* data)
+{
+ m_userData = data;
+}
+
+inline bool b2Body::IsConnected(const b2Body* other) const
+{
+ for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
+ {
+ if (jn->other == other)
+ return jn->joint->m_collideConnected == false;
+ }
+
+ return false;
+}
+
+inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
+{
+ if (IsSleeping())
+ {
+ WakeUp();
+ }
+ m_force += force;
+ m_torque += b2Cross(point - m_sweep.c, force);
+}
+
+inline void b2Body::ApplyTorque(float32 torque)
+{
+ if (IsSleeping())
+ {
+ WakeUp();
+ }
+ m_torque += torque;
+}
+
+inline void b2Body::ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point)
+{
+ if (IsSleeping())
+ {
+ WakeUp();
+ }
+ m_linearVelocity += m_invMass * impulse;
+ m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
+}
+
+inline void b2Body::SynchronizeTransform()
+{
+ m_xf.R.Set(m_sweep.a);
+ m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
+}
+
+inline void b2Body::Advance(float32 t)
+{
+ // Advance to the new safe time.
+ m_sweep.Advance(t);
+ m_sweep.c = m_sweep.c0;
+ m_sweep.a = m_sweep.a0;
+ SynchronizeTransform();
+}
+
+inline b2World* b2Body::GetWorld()
+{
+ return m_world;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2ContactManager.h"
+#include "b2World.h"
+#include "b2Body.h"
+
+// This is a callback from the broadphase when two AABB proxies begin
+// to overlap. We create a b2Contact to manage the narrow phase.
+void* b2ContactManager::PairAdded(void* proxyUserData1, void* proxyUserData2)
+{
+ b2Shape* shape1 = (b2Shape*)proxyUserData1;
+ b2Shape* shape2 = (b2Shape*)proxyUserData2;
+
+ b2Body* body1 = shape1->GetBody();
+ b2Body* body2 = shape2->GetBody();
+
+ if (body1->IsStatic() && body2->IsStatic())
+ {
+ return &m_nullContact;
+ }
+
+ if (shape1->GetBody() == shape2->GetBody())
+ {
+ return &m_nullContact;
+ }
+
+ if (body2->IsConnected(body1))
+ {
+ return &m_nullContact;
+ }
+
+ if (m_world->m_contactFilter != NULL && m_world->m_contactFilter->ShouldCollide(shape1, shape2) == false)
+ {
+ return &m_nullContact;
+ }
+
+ // Call the factory.
+ b2Contact* c = b2Contact::Create(shape1, shape2, &m_world->m_blockAllocator);
+
+ if (c == NULL)
+ {
+ return &m_nullContact;
+ }
+
+ // Contact creation may swap shapes.
+ shape1 = c->GetShape1();
+ shape2 = c->GetShape2();
+ body1 = shape1->GetBody();
+ body2 = shape2->GetBody();
+
+ // Insert into the world.
+ c->m_prev = NULL;
+ c->m_next = m_world->m_contactList;
+ if (m_world->m_contactList != NULL)
+ {
+ m_world->m_contactList->m_prev = c;
+ }
+ m_world->m_contactList = c;
+
+ // Connect to island graph.
+
+ // Connect to body 1
+ c->m_node1.contact = c;
+ c->m_node1.other = body2;
+
+ c->m_node1.prev = NULL;
+ c->m_node1.next = body1->m_contactList;
+ if (body1->m_contactList != NULL)
+ {
+ body1->m_contactList->prev = &c->m_node1;
+ }
+ body1->m_contactList = &c->m_node1;
+
+ // Connect to body 2
+ c->m_node2.contact = c;
+ c->m_node2.other = body1;
+
+ c->m_node2.prev = NULL;
+ c->m_node2.next = body2->m_contactList;
+ if (body2->m_contactList != NULL)
+ {
+ body2->m_contactList->prev = &c->m_node2;
+ }
+ body2->m_contactList = &c->m_node2;
+
+ ++m_world->m_contactCount;
+ return c;
+}
+
+// This is a callback from the broadphase when two AABB proxies cease
+// to overlap. We retire the b2Contact.
+void b2ContactManager::PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData)
+{
+ B2_NOT_USED(proxyUserData1);
+ B2_NOT_USED(proxyUserData2);
+
+ if (pairUserData == NULL)
+ {
+ return;
+ }
+
+ b2Contact* c = (b2Contact*)pairUserData;
+ if (c == &m_nullContact)
+ {
+ return;
+ }
+
+ // An attached body is being destroyed, we must destroy this contact
+ // immediately to avoid orphaned shape pointers.
+ Destroy(c);
+}
+
+void b2ContactManager::Destroy(b2Contact* c)
+{
+ b2Shape* shape1 = c->GetShape1();
+ b2Shape* shape2 = c->GetShape2();
+
+ // Inform the user that this contact is ending.
+ int32 manifoldCount = c->GetManifoldCount();
+ if (manifoldCount > 0 && m_world->m_contactListener)
+ {
+ b2Body* b1 = shape1->GetBody();
+ b2Body* b2 = shape2->GetBody();
+
+ b2Manifold* manifolds = c->GetManifolds();
+ b2ContactPoint cp;
+ cp.shape1 = c->GetShape1();
+ cp.shape2 = c->GetShape2();
+ cp.friction = c->m_friction;
+ cp.restitution = c->m_restitution;
+
+ for (int32 i = 0; i < manifoldCount; ++i)
+ {
+ b2Manifold* manifold = manifolds + i;
+ cp.normal = manifold->normal;
+
+ for (int32 j = 0; j < manifold->pointCount; ++j)
+ {
+ b2ManifoldPoint* mp = manifold->points + j;
+ cp.position = b1->GetWorldPoint(mp->localPoint1);
+ b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+ b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+ cp.velocity = v2 - v1;
+ cp.separation = mp->separation;
+ cp.id = mp->id;
+ m_world->m_contactListener->Remove(&cp);
+ }
+ }
+ }
+
+ // Remove from the world.
+ if (c->m_prev)
+ {
+ c->m_prev->m_next = c->m_next;
+ }
+
+ if (c->m_next)
+ {
+ c->m_next->m_prev = c->m_prev;
+ }
+
+ if (c == m_world->m_contactList)
+ {
+ m_world->m_contactList = c->m_next;
+ }
+
+ b2Body* body1 = shape1->GetBody();
+ b2Body* body2 = shape2->GetBody();
+
+ // Remove from body 1
+ if (c->m_node1.prev)
+ {
+ c->m_node1.prev->next = c->m_node1.next;
+ }
+
+ if (c->m_node1.next)
+ {
+ c->m_node1.next->prev = c->m_node1.prev;
+ }
+
+ if (&c->m_node1 == body1->m_contactList)
+ {
+ body1->m_contactList = c->m_node1.next;
+ }
+
+ // Remove from body 2
+ if (c->m_node2.prev)
+ {
+ c->m_node2.prev->next = c->m_node2.next;
+ }
+
+ if (c->m_node2.next)
+ {
+ c->m_node2.next->prev = c->m_node2.prev;
+ }
+
+ if (&c->m_node2 == body2->m_contactList)
+ {
+ body2->m_contactList = c->m_node2.next;
+ }
+
+ // Call the factory.
+ b2Contact::Destroy(c, &m_world->m_blockAllocator);
+ --m_world->m_contactCount;
+}
+
+// This is the top level collision call for the time step. Here
+// all the narrow phase collision is processed for the world
+// contact list.
+void b2ContactManager::Collide()
+{
+ // Update awake contacts.
+ for (b2Contact* c = m_world->m_contactList; c; c = c->GetNext())
+ {
+ b2Body* body1 = c->GetShape1()->GetBody();
+ b2Body* body2 = c->GetShape2()->GetBody();
+ if (body1->IsSleeping() && body2->IsSleeping())
+ {
+ continue;
+ }
+
+ c->Update(m_world->m_contactListener);
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_CONTACT_MANAGER_H
+#define B2_CONTACT_MANAGER_H
+
+#include "../Collision/b2BroadPhase.h"
+#include "../Dynamics/Contacts/b2NullContact.h"
+
+class b2World;
+class b2Contact;
+struct b2TimeStep;
+
+// Delegate of b2World.
+class b2ContactManager : public b2PairCallback
+{
+public:
+ b2ContactManager() : m_world(NULL), m_destroyImmediate(false) {}
+
+ // Implements PairCallback
+ void* PairAdded(void* proxyUserData1, void* proxyUserData2);
+
+ // Implements PairCallback
+ void PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData);
+
+ void Destroy(b2Contact* c);
+
+ void Collide();
+
+ b2World* m_world;
+
+ // This lets us provide broadphase proxy pair user data for
+ // contacts that shouldn't exist.
+ b2NullContact m_nullContact;
+
+ bool m_destroyImmediate;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Island.h"
+#include "b2Body.h"
+#include "b2World.h"
+#include "Contacts/b2Contact.h"
+#include "Contacts/b2ContactSolver.h"
+#include "Joints/b2Joint.h"
+#include "../Common/b2StackAllocator.h"
+
+/*
+Position Correction Notes
+=========================
+I tried the several algorithms for position correction of the 2D revolute joint.
+I looked at these systems:
+- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
+- suspension bridge with 30 1m long planks of length 1m.
+- multi-link chain with 30 1m long links.
+
+Here are the algorithms:
+
+Baumgarte - A fraction of the position error is added to the velocity error. There is no
+separate position solver.
+
+Pseudo Velocities - After the velocity solver and position integration,
+the position error, Jacobian, and effective mass are recomputed. Then
+the velocity constraints are solved with pseudo velocities and a fraction
+of the position error is added to the pseudo velocity error. The pseudo
+velocities are initialized to zero and there is no warm-starting. After
+the position solver, the pseudo velocities are added to the positions.
+This is also called the First Order World method or the Position LCP method.
+
+Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
+position error is re-computed for each constraint and the positions are updated
+after the constraint is solved. The radius vectors (aka Jacobians) are
+re-computed too (otherwise the algorithm has horrible instability). The pseudo
+velocity states are not needed because they are effectively zero at the beginning
+of each iteration. Since we have the current position error, we allow the
+iterations to terminate early if the error becomes smaller than b2_linearSlop.
+
+Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
+each time a constraint is solved.
+
+Here are the results:
+Baumgarte - this is the cheapest algorithm but it has some stability problems,
+especially with the bridge. The chain links separate easily close to the root
+and they jitter as they struggle to pull together. This is one of the most common
+methods in the field. The big drawback is that the position correction artificially
+affects the momentum, thus leading to instabilities and false bounce. I used a
+bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
+factor makes joints and contacts more spongy.
+
+Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
+stable. However, joints still separate with large angular velocities. Drag the
+simple pendulum in a circle quickly and the joint will separate. The chain separates
+easily and does not recover. I used a bias factor of 0.2. A larger value lead to
+the bridge collapsing when a heavy cube drops on it.
+
+Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
+Velocities, but in other ways it is worse. The bridge and chain are much more
+stable, but the simple pendulum goes unstable at high angular velocities.
+
+Full NGS - stable in all tests. The joints display good stiffness. The bridge
+still sags, but this is better than infinite forces.
+
+Recommendations
+Pseudo Velocities are not really worthwhile because the bridge and chain cannot
+recover from joint separation. In other cases the benefit over Baumgarte is small.
+
+Modified NGS is not a robust method for the revolute joint due to the violent
+instability seen in the simple pendulum. Perhaps it is viable with other constraint
+types, especially scalar constraints where the effective mass is a scalar.
+
+This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
+and is very fast. I don't think we can escape Baumgarte, especially in highly
+demanding cases where high constraint fidelity is not needed.
+
+Full NGS is robust and easy on the eyes. I recommend this as an option for
+higher fidelity simulation and certainly for suspension bridges and long chains.
+Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
+joint separation can be problematic. The number of NGS iterations can be reduced
+for better performance without harming robustness much.
+
+Each joint in a can be handled differently in the position solver. So I recommend
+a system where the user can select the algorithm on a per joint basis. I would
+probably default to the slower Full NGS and let the user select the faster
+Baumgarte method in performance critical scenarios.
+*/
+
+b2Island::b2Island(
+ int32 bodyCapacity,
+ int32 contactCapacity,
+ int32 jointCapacity,
+ b2StackAllocator* allocator,
+ b2ContactListener* listener)
+{
+ m_bodyCapacity = bodyCapacity;
+ m_contactCapacity = contactCapacity;
+ m_jointCapacity = jointCapacity;
+ m_bodyCount = 0;
+ m_contactCount = 0;
+ m_jointCount = 0;
+
+ m_allocator = allocator;
+ m_listener = listener;
+
+ m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
+ m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
+ m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
+
+ m_positionIterationCount = 0;
+}
+
+b2Island::~b2Island()
+{
+ // Warning: the order should reverse the constructor order.
+ m_allocator->Free(m_joints);
+ m_allocator->Free(m_contacts);
+ m_allocator->Free(m_bodies);
+}
+
+void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep)
+{
+ // Integrate velocities and apply damping.
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ b2Body* b = m_bodies[i];
+
+ if (b->IsStatic())
+ continue;
+
+ // Integrate velocities.
+ b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
+ b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
+
+ // Reset forces.
+ b->m_force.Set(0.0f, 0.0f);
+ b->m_torque = 0.0f;
+
+ // Apply damping.
+ // ODE: dv/dt + c * v = 0
+ // Solution: v(t) = v0 * exp(-c * t)
+ // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+ // v2 = exp(-c * dt) * v1
+ // Taylor expansion:
+ // v2 = (1.0f - c * dt) * v1
+ b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
+ b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
+
+ // Check for large velocities.
+#ifdef TARGET_FLOAT32_IS_FIXED
+ // Fixed point code written this way to prevent
+ // overflows, float code is optimized for speed
+
+ float32 vMagnitude = b->m_linearVelocity.Length();
+ if(vMagnitude > b2_maxLinearVelocity) {
+ b->m_linearVelocity *= b2_maxLinearVelocity/vMagnitude;
+ }
+ b->m_angularVelocity = b2Clamp(b->m_angularVelocity,
+ -b2_maxAngularVelocity, b2_maxAngularVelocity);
+
+#else
+
+ if (b2Dot(b->m_linearVelocity, b->m_linearVelocity) > b2_maxLinearVelocitySquared)
+ {
+ b->m_linearVelocity.Normalize();
+ b->m_linearVelocity *= b2_maxLinearVelocity;
+ }
+ if (b->m_angularVelocity * b->m_angularVelocity > b2_maxAngularVelocitySquared)
+ {
+ if (b->m_angularVelocity < 0.0f)
+ {
+ b->m_angularVelocity = -b2_maxAngularVelocity;
+ }
+ else
+ {
+ b->m_angularVelocity = b2_maxAngularVelocity;
+ }
+ }
+#endif
+
+ }
+
+ b2ContactSolver contactSolver(step, m_contacts, m_contactCount, m_allocator);
+
+ // Initialize velocity constraints.
+ contactSolver.InitVelocityConstraints(step);
+
+ for (int32 i = 0; i < m_jointCount; ++i)
+ {
+ m_joints[i]->InitVelocityConstraints(step);
+ }
+
+ // Solve velocity constraints.
+ for (int32 i = 0; i < step.maxIterations; ++i)
+ {
+ contactSolver.SolveVelocityConstraints();
+
+ for (int32 j = 0; j < m_jointCount; ++j)
+ {
+ m_joints[j]->SolveVelocityConstraints(step);
+ }
+ }
+
+ // Post-solve (store impulses for warm starting).
+ contactSolver.FinalizeVelocityConstraints();
+
+ // Integrate positions.
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ b2Body* b = m_bodies[i];
+
+ if (b->IsStatic())
+ continue;
+
+ // Store positions for continuous collision.
+ b->m_sweep.c0 = b->m_sweep.c;
+ b->m_sweep.a0 = b->m_sweep.a;
+
+ // Integrate
+ b->m_sweep.c += step.dt * b->m_linearVelocity;
+ b->m_sweep.a += step.dt * b->m_angularVelocity;
+
+ // Compute new transform
+ b->SynchronizeTransform();
+
+ // Note: shapes are synchronized later.
+ }
+
+ if (correctPositions)
+ {
+ // Initialize position constraints.
+ // Contacts don't need initialization.
+ for (int32 i = 0; i < m_jointCount; ++i)
+ {
+ m_joints[i]->InitPositionConstraints();
+ }
+
+ // Iterate over constraints.
+ for (m_positionIterationCount = 0; m_positionIterationCount < step.maxIterations; ++m_positionIterationCount)
+ {
+ bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
+
+ bool jointsOkay = true;
+ for (int i = 0; i < m_jointCount; ++i)
+ {
+ bool jointOkay = m_joints[i]->SolvePositionConstraints();
+ jointsOkay = jointsOkay && jointOkay;
+ }
+
+ if (contactsOkay && jointsOkay)
+ {
+ break;
+ }
+ }
+ }
+
+ Report(contactSolver.m_constraints);
+
+ if (allowSleep)
+ {
+ float32 minSleepTime = B2_FLT_MAX;
+
+#ifndef TARGET_FLOAT32_IS_FIXED
+ const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
+ const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
+#endif
+
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ b2Body* b = m_bodies[i];
+ if (b->m_invMass == 0.0f)
+ {
+ continue;
+ }
+
+ if ((b->m_flags & b2Body::e_allowSleepFlag) == 0)
+ {
+ b->m_sleepTime = 0.0f;
+ minSleepTime = 0.0f;
+ }
+
+ if ((b->m_flags & b2Body::e_allowSleepFlag) == 0 ||
+#ifdef TARGET_FLOAT32_IS_FIXED
+ b2Abs(b->m_angularVelocity) > b2_angularSleepTolerance ||
+ b2Abs(b->m_linearVelocity.x) > b2_linearSleepTolerance ||
+ b2Abs(b->m_linearVelocity.y) > b2_linearSleepTolerance)
+#else
+ b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
+ b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
+#endif
+ {
+ b->m_sleepTime = 0.0f;
+ minSleepTime = 0.0f;
+ }
+ else
+ {
+ b->m_sleepTime += step.dt;
+ minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
+ }
+ }
+
+ if (minSleepTime >= b2_timeToSleep)
+ {
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ b2Body* b = m_bodies[i];
+ b->m_flags |= b2Body::e_sleepFlag;
+ b->m_linearVelocity = b2Vec2_zero;
+ b->m_angularVelocity = 0.0f;
+ }
+ }
+ }
+}
+
+void b2Island::SolveTOI(const b2TimeStep& subStep)
+{
+ b2ContactSolver contactSolver(subStep, m_contacts, m_contactCount, m_allocator);
+
+ // No warm starting needed for TOI events.
+
+ // Solve velocity constraints.
+ for (int32 i = 0; i < subStep.maxIterations; ++i)
+ {
+ contactSolver.SolveVelocityConstraints();
+ }
+
+ // Don't store the TOI contact forces for warm starting
+ // because they can be quite large.
+
+ // Integrate positions.
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ b2Body* b = m_bodies[i];
+
+ if (b->IsStatic())
+ continue;
+
+ // Store positions for continuous collision.
+ b->m_sweep.c0 = b->m_sweep.c;
+ b->m_sweep.a0 = b->m_sweep.a;
+
+ // Integrate
+ b->m_sweep.c += subStep.dt * b->m_linearVelocity;
+ b->m_sweep.a += subStep.dt * b->m_angularVelocity;
+
+ // Compute new transform
+ b->SynchronizeTransform();
+
+ // Note: shapes are synchronized later.
+ }
+
+ // Solve position constraints.
+ const float32 k_toiBaumgarte = 0.75f;
+ for (int32 i = 0; i < subStep.maxIterations; ++i)
+ {
+ bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
+ if (contactsOkay)
+ {
+ break;
+ }
+ }
+
+ Report(contactSolver.m_constraints);
+}
+
+void b2Island::Report(b2ContactConstraint* constraints)
+{
+ if (m_listener == NULL)
+ {
+ return;
+ }
+
+ for (int32 i = 0; i < m_contactCount; ++i)
+ {
+ b2Contact* c = m_contacts[i];
+ b2ContactConstraint* cc = constraints + i;
+ b2ContactResult cr;
+ cr.shape1 = c->GetShape1();
+ cr.shape2 = c->GetShape2();
+ b2Body* b1 = cr.shape1->GetBody();
+ int32 manifoldCount = c->GetManifoldCount();
+ b2Manifold* manifolds = c->GetManifolds();
+ for (int32 j = 0; j < manifoldCount; ++j)
+ {
+ b2Manifold* manifold = manifolds + j;
+ cr.normal = manifold->normal;
+ for (int32 k = 0; k < manifold->pointCount; ++k)
+ {
+ b2ManifoldPoint* point = manifold->points + k;
+ b2ContactConstraintPoint* ccp = cc->points + k;
+ cr.position = b1->GetWorldPoint(point->localPoint1);
+
+ // TOI constraint results are not stored, so get
+ // the result from the constraint.
+ cr.normalImpulse = ccp->normalImpulse;
+ cr.tangentImpulse = ccp->tangentImpulse;
+ cr.id = point->id;
+
+ m_listener->Result(&cr);
+ }
+ }
+ }
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_ISLAND_H
+#define B2_ISLAND_H
+
+#include "../Common/b2Math.h"
+
+class b2Contact;
+class b2Body;
+class b2Joint;
+class b2StackAllocator;
+class b2ContactListener;
+struct b2ContactConstraint;
+struct b2TimeStep;
+
+class b2Island
+{
+public:
+ b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity,
+ b2StackAllocator* allocator, b2ContactListener* listener);
+ ~b2Island();
+
+ void Clear()
+ {
+ m_bodyCount = 0;
+ m_contactCount = 0;
+ m_jointCount = 0;
+ }
+
+ void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep);
+
+ void SolveTOI(const b2TimeStep& subStep);
+
+ void Add(b2Body* body)
+ {
+ b2Assert(m_bodyCount < m_bodyCapacity);
+ m_bodies[m_bodyCount++] = body;
+ }
+
+ void Add(b2Contact* contact)
+ {
+ b2Assert(m_contactCount < m_contactCapacity);
+ m_contacts[m_contactCount++] = contact;
+ }
+
+ void Add(b2Joint* joint)
+ {
+ b2Assert(m_jointCount < m_jointCapacity);
+ m_joints[m_jointCount++] = joint;
+ }
+
+ void Report(b2ContactConstraint* constraints);
+
+ b2StackAllocator* m_allocator;
+ b2ContactListener* m_listener;
+
+ b2Body** m_bodies;
+ b2Contact** m_contacts;
+ b2Joint** m_joints;
+
+ int32 m_bodyCount;
+ int32 m_jointCount;
+ int32 m_contactCount;
+
+ int32 m_bodyCapacity;
+ int32 m_contactCapacity;
+ int32 m_jointCapacity;
+
+ int32 m_positionIterationCount;
+};
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2World.h"
+#include "b2Body.h"
+#include "b2Island.h"
+#include "Joints/b2PulleyJoint.h"
+#include "Contacts/b2Contact.h"
+#include "Contacts/b2ContactSolver.h"
+#include "../Collision/b2Collision.h"
+#include "../Collision/Shapes/b2CircleShape.h"
+#include "../Collision/Shapes/b2PolygonShape.h"
+#include <new>
+
+b2World::b2World(const b2AABB& worldAABB, const b2Vec2& gravity, bool doSleep)
+{
+ m_destructionListener = NULL;
+ m_boundaryListener = NULL;
+ m_contactFilter = &b2_defaultFilter;
+ m_contactListener = NULL;
+ m_debugDraw = NULL;
+
+ m_bodyList = NULL;
+ m_contactList = NULL;
+ m_jointList = NULL;
+
+ m_bodyCount = 0;
+ m_contactCount = 0;
+ m_jointCount = 0;
+
+ m_positionCorrection = true;
+ m_warmStarting = true;
+ m_continuousPhysics = true;
+
+ m_allowSleep = doSleep;
+ m_gravity = gravity;
+
+ m_lock = false;
+
+ m_inv_dt0 = 0.0f;
+
+ m_contactManager.m_world = this;
+ void* mem = b2Alloc(sizeof(b2BroadPhase));
+ m_broadPhase = new (mem) b2BroadPhase(worldAABB, &m_contactManager);
+
+ b2BodyDef bd;
+ m_groundBody = CreateBody(&bd);
+}
+
+b2World::~b2World()
+{
+ DestroyBody(m_groundBody);
+ m_broadPhase->~b2BroadPhase();
+ b2Free(m_broadPhase);
+}
+
+void b2World::SetDestructionListener(b2DestructionListener* listener)
+{
+ m_destructionListener = listener;
+}
+
+void b2World::SetBoundaryListener(b2BoundaryListener* listener)
+{
+ m_boundaryListener = listener;
+}
+
+void b2World::SetContactFilter(b2ContactFilter* filter)
+{
+ m_contactFilter = filter;
+}
+
+void b2World::SetContactListener(b2ContactListener* listener)
+{
+ m_contactListener = listener;
+}
+
+void b2World::SetDebugDraw(b2DebugDraw* debugDraw)
+{
+ m_debugDraw = debugDraw;
+}
+
+b2Body* b2World::CreateBody(const b2BodyDef* def)
+{
+ b2Assert(m_lock == false);
+ if (m_lock == true)
+ {
+ return NULL;
+ }
+
+ void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
+ b2Body* b = new (mem) b2Body(def, this);
+
+ // Add to world doubly linked list.
+ b->m_prev = NULL;
+ b->m_next = m_bodyList;
+ if (m_bodyList)
+ {
+ m_bodyList->m_prev = b;
+ }
+ m_bodyList = b;
+ ++m_bodyCount;
+
+ return b;
+}
+
+void b2World::DestroyBody(b2Body* b)
+{
+ b2Assert(m_bodyCount > 0);
+ b2Assert(m_lock == false);
+ if (m_lock == true)
+ {
+ return;
+ }
+
+ // Delete the attached joints.
+ b2JointEdge* jn = b->m_jointList;
+ while (jn)
+ {
+ b2JointEdge* jn0 = jn;
+ jn = jn->next;
+
+ if (m_destructionListener)
+ {
+ m_destructionListener->SayGoodbye(jn0->joint);
+ }
+
+ DestroyJoint(jn0->joint);
+ }
+
+ // Delete the attached shapes. This destroys broad-phase
+ // proxies and pairs, leading to the destruction of contacts.
+ b2Shape* s = b->m_shapeList;
+ while (s)
+ {
+ b2Shape* s0 = s;
+ s = s->m_next;
+
+ if (m_destructionListener)
+ {
+ m_destructionListener->SayGoodbye(s0);
+ }
+
+ s0->DestroyProxy(m_broadPhase);
+ b2Shape::Destroy(s0, &m_blockAllocator);
+ }
+
+ // Remove world body list.
+ if (b->m_prev)
+ {
+ b->m_prev->m_next = b->m_next;
+ }
+
+ if (b->m_next)
+ {
+ b->m_next->m_prev = b->m_prev;
+ }
+
+ if (b == m_bodyList)
+ {
+ m_bodyList = b->m_next;
+ }
+
+ --m_bodyCount;
+ b->~b2Body();
+ m_blockAllocator.Free(b, sizeof(b2Body));
+}
+
+b2Joint* b2World::CreateJoint(const b2JointDef* def)
+{
+ b2Assert(m_lock == false);
+
+ b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
+
+ // Connect to the world list.
+ j->m_prev = NULL;
+ j->m_next = m_jointList;
+ if (m_jointList)
+ {
+ m_jointList->m_prev = j;
+ }
+ m_jointList = j;
+ ++m_jointCount;
+
+ // Connect to the bodies' doubly linked lists.
+ j->m_node1.joint = j;
+ j->m_node1.other = j->m_body2;
+ j->m_node1.prev = NULL;
+ j->m_node1.next = j->m_body1->m_jointList;
+ if (j->m_body1->m_jointList) j->m_body1->m_jointList->prev = &j->m_node1;
+ j->m_body1->m_jointList = &j->m_node1;
+
+ j->m_node2.joint = j;
+ j->m_node2.other = j->m_body1;
+ j->m_node2.prev = NULL;
+ j->m_node2.next = j->m_body2->m_jointList;
+ if (j->m_body2->m_jointList) j->m_body2->m_jointList->prev = &j->m_node2;
+ j->m_body2->m_jointList = &j->m_node2;
+
+ // If the joint prevents collisions, then reset collision filtering.
+ if (def->collideConnected == false)
+ {
+ // Reset the proxies on the body with the minimum number of shapes.
+ b2Body* b = def->body1->m_shapeCount < def->body2->m_shapeCount ? def->body1 : def->body2;
+ for (b2Shape* s = b->m_shapeList; s; s = s->m_next)
+ {
+ s->RefilterProxy(m_broadPhase, b->GetXForm());
+ }
+ }
+
+ return j;
+}
+
+void b2World::DestroyJoint(b2Joint* j)
+{
+ b2Assert(m_lock == false);
+
+ bool collideConnected = j->m_collideConnected;
+
+ // Remove from the doubly linked list.
+ if (j->m_prev)
+ {
+ j->m_prev->m_next = j->m_next;
+ }
+
+ if (j->m_next)
+ {
+ j->m_next->m_prev = j->m_prev;
+ }
+
+ if (j == m_jointList)
+ {
+ m_jointList = j->m_next;
+ }
+
+ // Disconnect from island graph.
+ b2Body* body1 = j->m_body1;
+ b2Body* body2 = j->m_body2;
+
+ // Wake up connected bodies.
+ body1->WakeUp();
+ body2->WakeUp();
+
+ // Remove from body 1.
+ if (j->m_node1.prev)
+ {
+ j->m_node1.prev->next = j->m_node1.next;
+ }
+
+ if (j->m_node1.next)
+ {
+ j->m_node1.next->prev = j->m_node1.prev;
+ }
+
+ if (&j->m_node1 == body1->m_jointList)
+ {
+ body1->m_jointList = j->m_node1.next;
+ }
+
+ j->m_node1.prev = NULL;
+ j->m_node1.next = NULL;
+
+ // Remove from body 2
+ if (j->m_node2.prev)
+ {
+ j->m_node2.prev->next = j->m_node2.next;
+ }
+
+ if (j->m_node2.next)
+ {
+ j->m_node2.next->prev = j->m_node2.prev;
+ }
+
+ if (&j->m_node2 == body2->m_jointList)
+ {
+ body2->m_jointList = j->m_node2.next;
+ }
+
+ j->m_node2.prev = NULL;
+ j->m_node2.next = NULL;
+
+ b2Joint::Destroy(j, &m_blockAllocator);
+
+ b2Assert(m_jointCount > 0);
+ --m_jointCount;
+
+ // If the joint prevents collisions, then reset collision filtering.
+ if (collideConnected == false)
+ {
+ // Reset the proxies on the body with the minimum number of shapes.
+ b2Body* b = body1->m_shapeCount < body2->m_shapeCount ? body1 : body2;
+ for (b2Shape* s = b->m_shapeList; s; s = s->m_next)
+ {
+ s->RefilterProxy(m_broadPhase, b->GetXForm());
+ }
+ }
+}
+
+void b2World::Refilter(b2Shape* shape)
+{
+ shape->RefilterProxy(m_broadPhase, shape->GetBody()->GetXForm());
+}
+
+// Find islands, integrate and solve constraints, solve position constraints
+void b2World::Solve(const b2TimeStep& step)
+{
+ m_positionIterationCount = 0;
+
+ // Size the island for the worst case.
+ b2Island island(m_bodyCount, m_contactCount, m_jointCount, &m_stackAllocator, m_contactListener);
+
+ // Clear all the island flags.
+ for (b2Body* b = m_bodyList; b; b = b->m_next)
+ {
+ b->m_flags &= ~b2Body::e_islandFlag;
+ }
+ for (b2Contact* c = m_contactList; c; c = c->m_next)
+ {
+ c->m_flags &= ~b2Contact::e_islandFlag;
+ }
+ for (b2Joint* j = m_jointList; j; j = j->m_next)
+ {
+ j->m_islandFlag = false;
+ }
+
+ // Build and simulate all awake islands.
+ int32 stackSize = m_bodyCount;
+ b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
+ for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
+ {
+ if (seed->m_flags & (b2Body::e_islandFlag | b2Body::e_sleepFlag | b2Body::e_frozenFlag))
+ {
+ continue;
+ }
+
+ if (seed->IsStatic())
+ {
+ continue;
+ }
+
+ // Reset island and stack.
+ island.Clear();
+ int32 stackCount = 0;
+ stack[stackCount++] = seed;
+ seed->m_flags |= b2Body::e_islandFlag;
+
+ // Perform a depth first search (DFS) on the constraint graph.
+ while (stackCount > 0)
+ {
+ // Grab the next body off the stack and add it to the island.
+ b2Body* b = stack[--stackCount];
+ island.Add(b);
+
+ // Make sure the body is awake.
+ b->m_flags &= ~b2Body::e_sleepFlag;
+
+ // To keep islands as small as possible, we don't
+ // propagate islands across static bodies.
+ if (b->IsStatic())
+ {
+ continue;
+ }
+
+ // Search all contacts connected to this body.
+ for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
+ {
+ // Has this contact already been added to an island?
+ if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_nonSolidFlag))
+ {
+ continue;
+ }
+
+ // Is this contact touching?
+ if (cn->contact->GetManifoldCount() == 0)
+ {
+ continue;
+ }
+
+ island.Add(cn->contact);
+ cn->contact->m_flags |= b2Contact::e_islandFlag;
+
+ b2Body* other = cn->other;
+
+ // Was the other body already added to this island?
+ if (other->m_flags & b2Body::e_islandFlag)
+ {
+ continue;
+ }
+
+ b2Assert(stackCount < stackSize);
+ stack[stackCount++] = other;
+ other->m_flags |= b2Body::e_islandFlag;
+ }
+
+ // Search all joints connect to this body.
+ for (b2JointEdge* jn = b->m_jointList; jn; jn = jn->next)
+ {
+ if (jn->joint->m_islandFlag == true)
+ {
+ continue;
+ }
+
+ island.Add(jn->joint);
+ jn->joint->m_islandFlag = true;
+
+ b2Body* other = jn->other;
+ if (other->m_flags & b2Body::e_islandFlag)
+ {
+ continue;
+ }
+
+ b2Assert(stackCount < stackSize);
+ stack[stackCount++] = other;
+ other->m_flags |= b2Body::e_islandFlag;
+ }
+ }
+
+ island.Solve(step, m_gravity, m_positionCorrection, m_allowSleep);
+ m_positionIterationCount = b2Max(m_positionIterationCount, island.m_positionIterationCount);
+
+ // Post solve cleanup.
+ for (int32 i = 0; i < island.m_bodyCount; ++i)
+ {
+ // Allow static bodies to participate in other islands.
+ b2Body* b = island.m_bodies[i];
+ if (b->IsStatic())
+ {
+ b->m_flags &= ~b2Body::e_islandFlag;
+ }
+ }
+ }
+
+ m_stackAllocator.Free(stack);
+
+ // Synchronize shapes, check for out of range bodies.
+ for (b2Body* b = m_bodyList; b; b = b->GetNext())
+ {
+ if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
+ {
+ continue;
+ }
+
+ if (b->IsStatic())
+ {
+ continue;
+ }
+
+ // Update shapes (for broad-phase). If the shapes go out of
+ // the world AABB then shapes and contacts may be destroyed,
+ // including contacts that are
+ bool inRange = b->SynchronizeShapes();
+
+ // Did the body's shapes leave the world?
+ if (inRange == false && m_boundaryListener != NULL)
+ {
+ m_boundaryListener->Violation(b);
+ }
+ }
+
+ // Commit shape proxy movements to the broad-phase so that new contacts are created.
+ // Also, some contacts can be destroyed.
+ m_broadPhase->Commit();
+}
+
+// Find TOI contacts and solve them.
+void b2World::SolveTOI(const b2TimeStep& step)
+{
+ // Reserve an island and a stack for TOI island solution.
+ b2Island island(m_bodyCount, b2_maxTOIContactsPerIsland, 0, &m_stackAllocator, m_contactListener);
+ int32 stackSize = m_bodyCount;
+ b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
+
+ for (b2Body* b = m_bodyList; b; b = b->m_next)
+ {
+ b->m_flags &= ~b2Body::e_islandFlag;
+ b->m_sweep.t0 = 0.0f;
+ }
+
+ for (b2Contact* c = m_contactList; c; c = c->m_next)
+ {
+ // Invalidate TOI
+ c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
+ }
+
+ // Find TOI events and solve them.
+ for (;;)
+ {
+ // Find the first TOI.
+ b2Contact* minContact = NULL;
+ float32 minTOI = 1.0f;
+
+ for (b2Contact* c = m_contactList; c; c = c->m_next)
+ {
+ if (c->m_flags & (b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
+ {
+ continue;
+ }
+
+ // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
+
+ float32 toi = 1.0f;
+ if (c->m_flags & b2Contact::e_toiFlag)
+ {
+ // This contact has a valid cached TOI.
+ toi = c->m_toi;
+ }
+ else
+ {
+ // Compute the TOI for this contact.
+ b2Shape* s1 = c->GetShape1();
+ b2Shape* s2 = c->GetShape2();
+ b2Body* b1 = s1->GetBody();
+ b2Body* b2 = s2->GetBody();
+
+ if ((b1->IsStatic() || b1->IsSleeping()) && (b2->IsStatic() || b2->IsSleeping()))
+ {
+ continue;
+ }
+
+ // Put the sweeps onto the same time interval.
+ float32 t0 = b1->m_sweep.t0;
+
+ if (b1->m_sweep.t0 < b2->m_sweep.t0)
+ {
+ t0 = b2->m_sweep.t0;
+ b1->m_sweep.Advance(t0);
+ }
+ else if (b2->m_sweep.t0 < b1->m_sweep.t0)
+ {
+ t0 = b1->m_sweep.t0;
+ b2->m_sweep.Advance(t0);
+ }
+
+ b2Assert(t0 < 1.0f);
+
+ // Compute the time of impact.
+ toi = b2TimeOfImpact(c->m_shape1, b1->m_sweep, c->m_shape2, b2->m_sweep);
+
+ b2Assert(0.0f <= toi && toi <= 1.0f);
+
+ if (toi > 0.0f && toi < 1.0f)
+ {
+ toi = b2Min((1.0f - toi) * t0 + toi, 1.0f);
+ }
+
+
+ c->m_toi = toi;
+ c->m_flags |= b2Contact::e_toiFlag;
+ }
+
+ if (B2_FLT_EPSILON < toi && toi < minTOI)
+ {
+ // This is the minimum TOI found so far.
+ minContact = c;
+ minTOI = toi;
+ }
+ }
+
+ if (minContact == NULL || 1.0f - 100.0f * B2_FLT_EPSILON < minTOI)
+ {
+ // No more TOI events. Done!
+ break;
+ }
+
+ // Advance the bodies to the TOI.
+ b2Shape* s1 = minContact->GetShape1();
+ b2Shape* s2 = minContact->GetShape2();
+ b2Body* b1 = s1->GetBody();
+ b2Body* b2 = s2->GetBody();
+ b1->Advance(minTOI);
+ b2->Advance(minTOI);
+
+ // The TOI contact likely has some new contact points.
+ minContact->Update(m_contactListener);
+ minContact->m_flags &= ~b2Contact::e_toiFlag;
+
+ if (minContact->GetManifoldCount() == 0)
+ {
+ // This shouldn't happen. Numerical error?
+ //b2Assert(false);
+ continue;
+ }
+
+ // Build the TOI island. We need a dynamic seed.
+ b2Body* seed = b1;
+ if (seed->IsStatic())
+ {
+ seed = b2;
+ }
+
+ // Reset island and stack.
+ island.Clear();
+ int32 stackCount = 0;
+ stack[stackCount++] = seed;
+ seed->m_flags |= b2Body::e_islandFlag;
+
+ // Perform a depth first search (DFS) on the contact graph.
+ while (stackCount > 0)
+ {
+ // Grab the next body off the stack and add it to the island.
+ b2Body* b = stack[--stackCount];
+ island.Add(b);
+
+ // Make sure the body is awake.
+ b->m_flags &= ~b2Body::e_sleepFlag;
+
+ // To keep islands as small as possible, we don't
+ // propagate islands across static bodies.
+ if (b->IsStatic())
+ {
+ continue;
+ }
+
+ // Search all contacts connected to this body.
+ for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
+ {
+ // Does the TOI island still have space for contacts?
+ if (island.m_contactCount == island.m_contactCapacity)
+ {
+ continue;
+ }
+
+ // Has this contact already been added to an island? Skip slow or non-solid contacts.
+ if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
+ {
+ continue;
+ }
+
+ // Is this contact touching? For performance we are not updating this contact.
+ if (cn->contact->GetManifoldCount() == 0)
+ {
+ continue;
+ }
+
+ island.Add(cn->contact);
+ cn->contact->m_flags |= b2Contact::e_islandFlag;
+
+ // Update other body.
+ b2Body* other = cn->other;
+
+ // Was the other body already added to this island?
+ if (other->m_flags & b2Body::e_islandFlag)
+ {
+ continue;
+ }
+
+ // March forward, this can do no harm since this is the min TOI.
+ if (other->IsStatic() == false)
+ {
+ other->Advance(minTOI);
+ other->WakeUp();
+ }
+
+ b2Assert(stackCount < stackSize);
+ stack[stackCount++] = other;
+ other->m_flags |= b2Body::e_islandFlag;
+ }
+ }
+
+ b2TimeStep subStep;
+ subStep.dt = (1.0f - minTOI) * step.dt;
+ b2Assert(subStep.dt > B2_FLT_EPSILON);
+ subStep.inv_dt = 1.0f / subStep.dt;
+ subStep.maxIterations = step.maxIterations;
+
+ island.SolveTOI(subStep);
+
+ // Post solve cleanup.
+ for (int32 i = 0; i < island.m_bodyCount; ++i)
+ {
+ // Allow bodies to participate in future TOI islands.
+ b2Body* b = island.m_bodies[i];
+ b->m_flags &= ~b2Body::e_islandFlag;
+
+ if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
+ {
+ continue;
+ }
+
+ if (b->IsStatic())
+ {
+ continue;
+ }
+
+ // Update shapes (for broad-phase). If the shapes go out of
+ // the world AABB then shapes and contacts may be destroyed,
+ // including contacts that are
+ bool inRange = b->SynchronizeShapes();
+
+ // Did the body's shapes leave the world?
+ if (inRange == false && m_boundaryListener != NULL)
+ {
+ m_boundaryListener->Violation(b);
+ }
+
+ // Invalidate all contact TOIs associated with this body. Some of these
+ // may not be in the island because they were not touching.
+ for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
+ {
+ cn->contact->m_flags &= ~b2Contact::e_toiFlag;
+ }
+ }
+
+ for (int32 i = 0; i < island.m_contactCount; ++i)
+ {
+ // Allow contacts to participate in future TOI islands.
+ b2Contact* c = island.m_contacts[i];
+ c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
+ }
+
+ // Commit shape proxy movements to the broad-phase so that new contacts are created.
+ // Also, some contacts can be destroyed.
+ m_broadPhase->Commit();
+ }
+
+ m_stackAllocator.Free(stack);
+}
+
+void b2World::Step(float32 dt, int32 iterations)
+{
+ m_lock = true;
+
+ b2TimeStep step;
+ step.dt = dt;
+ step.maxIterations = iterations;
+ if (dt > 0.0f)
+ {
+ step.inv_dt = 1.0f / dt;
+ }
+ else
+ {
+ step.inv_dt = 0.0f;
+ }
+
+ step.dtRatio = m_inv_dt0 * dt;
+
+ step.positionCorrection = m_positionCorrection;
+ step.warmStarting = m_warmStarting;
+
+ // Update contacts.
+ m_contactManager.Collide();
+
+ // Integrate velocities, solve velocity constraints, and integrate positions.
+ if (step.dt > 0.0f)
+ {
+ Solve(step);
+ }
+
+ // Handle TOI events.
+ if (m_continuousPhysics && step.dt > 0.0f)
+ {
+ SolveTOI(step);
+ }
+
+ // Draw debug information.
+ DrawDebugData();
+
+ m_inv_dt0 = step.inv_dt;
+ m_lock = false;
+}
+
+int32 b2World::Query(const b2AABB& aabb, b2Shape** shapes, int32 maxCount)
+{
+ void** results = (void**)m_stackAllocator.Allocate(maxCount * sizeof(void*));
+
+ int32 count = m_broadPhase->Query(aabb, results, maxCount);
+
+ for (int32 i = 0; i < count; ++i)
+ {
+ shapes[i] = (b2Shape*)results[i];
+ }
+
+ m_stackAllocator.Free(results);
+ return count;
+}
+
+void b2World::DrawShape(b2Shape* shape, const b2XForm& xf, const b2Color& color, bool core)
+{
+ b2Color coreColor(0.9f, 0.6f, 0.6f);
+
+ switch (shape->GetType())
+ {
+ case e_circleShape:
+ {
+ b2CircleShape* circle = (b2CircleShape*)shape;
+
+ b2Vec2 center = b2Mul(xf, circle->GetLocalPosition());
+ float32 radius = circle->GetRadius();
+ b2Vec2 axis = xf.R.col1;
+
+ m_debugDraw->DrawSolidCircle(center, radius, axis, color);
+
+ if (core)
+ {
+ m_debugDraw->DrawCircle(center, radius - b2_toiSlop, coreColor);
+ }
+ }
+ break;
+
+ case e_polygonShape:
+ {
+ b2PolygonShape* poly = (b2PolygonShape*)shape;
+ int32 vertexCount = poly->GetVertexCount();
+ const b2Vec2* localVertices = poly->GetVertices();
+
+ b2Assert(vertexCount <= b2_maxPolygonVertices);
+ b2Vec2 vertices[b2_maxPolygonVertices];
+
+ for (int32 i = 0; i < vertexCount; ++i)
+ {
+ vertices[i] = b2Mul(xf, localVertices[i]);
+ }
+
+ m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
+
+ if (core)
+ {
+ const b2Vec2* localCoreVertices = poly->GetCoreVertices();
+ for (int32 i = 0; i < vertexCount; ++i)
+ {
+ vertices[i] = b2Mul(xf, localCoreVertices[i]);
+ }
+ m_debugDraw->DrawPolygon(vertices, vertexCount, coreColor);
+ }
+ }
+ break;
+ }
+}
+
+void b2World::DrawJoint(b2Joint* joint)
+{
+ b2Body* b1 = joint->GetBody1();
+ b2Body* b2 = joint->GetBody2();
+ const b2XForm& xf1 = b1->GetXForm();
+ const b2XForm& xf2 = b2->GetXForm();
+ b2Vec2 x1 = xf1.position;
+ b2Vec2 x2 = xf2.position;
+ b2Vec2 p1 = joint->GetAnchor1();
+ b2Vec2 p2 = joint->GetAnchor2();
+
+ b2Color color(0.5f, 0.8f, 0.8f);
+
+ switch (joint->GetType())
+ {
+ case e_distanceJoint:
+ m_debugDraw->DrawSegment(p1, p2, color);
+ break;
+
+ case e_pulleyJoint:
+ {
+ b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
+ b2Vec2 s1 = pulley->GetGroundAnchor1();
+ b2Vec2 s2 = pulley->GetGroundAnchor2();
+ m_debugDraw->DrawSegment(s1, p1, color);
+ m_debugDraw->DrawSegment(s2, p2, color);
+ m_debugDraw->DrawSegment(s1, s2, color);
+ }
+ break;
+
+ case e_mouseJoint:
+ // don't draw this
+ break;
+
+ default:
+ m_debugDraw->DrawSegment(x1, p1, color);
+ m_debugDraw->DrawSegment(p1, p2, color);
+ m_debugDraw->DrawSegment(x2, p2, color);
+ }
+}
+
+void b2World::DrawDebugData()
+{
+ if (m_debugDraw == NULL)
+ {
+ return;
+ }
+
+ uint32 flags = m_debugDraw->GetFlags();
+
+ if (flags & b2DebugDraw::e_shapeBit)
+ {
+ bool core = (flags & b2DebugDraw::e_coreShapeBit) == b2DebugDraw::e_coreShapeBit;
+
+ for (b2Body* b = m_bodyList; b; b = b->GetNext())
+ {
+ const b2XForm& xf = b->GetXForm();
+ for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
+ {
+ if (b->IsStatic())
+ {
+ DrawShape(s, xf, b2Color(0.5f, 0.9f, 0.5f), core);
+ }
+ else if (b->IsSleeping())
+ {
+ DrawShape(s, xf, b2Color(0.5f, 0.5f, 0.9f), core);
+ }
+ else
+ {
+ DrawShape(s, xf, b2Color(0.9f, 0.9f, 0.9f), core);
+ }
+ }
+ }
+ }
+
+ if (flags & b2DebugDraw::e_jointBit)
+ {
+ for (b2Joint* j = m_jointList; j; j = j->GetNext())
+ {
+ if (j->GetType() != e_mouseJoint)
+ {
+ DrawJoint(j);
+ }
+ }
+ }
+
+ if (flags & b2DebugDraw::e_pairBit)
+ {
+ b2BroadPhase* bp = m_broadPhase;
+ b2Vec2 invQ;
+ invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
+ b2Color color(0.9f, 0.9f, 0.3f);
+
+ for (int32 i = 0; i < b2_tableCapacity; ++i)
+ {
+ uint16 index = bp->m_pairManager.m_hashTable[i];
+ while (index != b2_nullPair)
+ {
+ b2Pair* pair = bp->m_pairManager.m_pairs + index;
+ b2Proxy* p1 = bp->m_proxyPool + pair->proxyId1;
+ b2Proxy* p2 = bp->m_proxyPool + pair->proxyId2;
+
+ b2AABB b1, b2;
+ b1.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->lowerBounds[0]].value;
+ b1.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->lowerBounds[1]].value;
+ b1.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->upperBounds[0]].value;
+ b1.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->upperBounds[1]].value;
+ b2.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->lowerBounds[0]].value;
+ b2.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->lowerBounds[1]].value;
+ b2.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->upperBounds[0]].value;
+ b2.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->upperBounds[1]].value;
+
+ b2Vec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
+ b2Vec2 x2 = 0.5f * (b2.lowerBound + b2.upperBound);
+
+ m_debugDraw->DrawSegment(x1, x2, color);
+
+ index = pair->next;
+ }
+ }
+ }
+
+ if (flags & b2DebugDraw::e_aabbBit)
+ {
+ b2BroadPhase* bp = m_broadPhase;
+ b2Vec2 worldLower = bp->m_worldAABB.lowerBound;
+ b2Vec2 worldUpper = bp->m_worldAABB.upperBound;
+
+ b2Vec2 invQ;
+ invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
+ b2Color color(0.9f, 0.3f, 0.9f);
+ for (int32 i = 0; i < b2_maxProxies; ++i)
+ {
+ b2Proxy* p = bp->m_proxyPool + i;
+ if (p->IsValid() == false)
+ {
+ continue;
+ }
+
+ b2AABB b;
+ b.lowerBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->lowerBounds[0]].value;
+ b.lowerBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->lowerBounds[1]].value;
+ b.upperBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->upperBounds[0]].value;
+ b.upperBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->upperBounds[1]].value;
+
+ b2Vec2 vs[4];
+ vs[0].Set(b.lowerBound.x, b.lowerBound.y);
+ vs[1].Set(b.upperBound.x, b.lowerBound.y);
+ vs[2].Set(b.upperBound.x, b.upperBound.y);
+ vs[3].Set(b.lowerBound.x, b.upperBound.y);
+
+ m_debugDraw->DrawPolygon(vs, 4, color);
+ }
+
+ b2Vec2 vs[4];
+ vs[0].Set(worldLower.x, worldLower.y);
+ vs[1].Set(worldUpper.x, worldLower.y);
+ vs[2].Set(worldUpper.x, worldUpper.y);
+ vs[3].Set(worldLower.x, worldUpper.y);
+ m_debugDraw->DrawPolygon(vs, 4, b2Color(0.3f, 0.9f, 0.9f));
+ }
+
+ if (flags & b2DebugDraw::e_obbBit)
+ {
+ b2Color color(0.5f, 0.3f, 0.5f);
+
+ for (b2Body* b = m_bodyList; b; b = b->GetNext())
+ {
+ const b2XForm& xf = b->GetXForm();
+ for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
+ {
+ if (s->GetType() != e_polygonShape)
+ {
+ continue;
+ }
+
+ b2PolygonShape* poly = (b2PolygonShape*)s;
+ const b2OBB& obb = poly->GetOBB();
+ b2Vec2 h = obb.extents;
+ b2Vec2 vs[4];
+ vs[0].Set(-h.x, -h.y);
+ vs[1].Set( h.x, -h.y);
+ vs[2].Set( h.x, h.y);
+ vs[3].Set(-h.x, h.y);
+
+ for (int32 i = 0; i < 4; ++i)
+ {
+ vs[i] = obb.center + b2Mul(obb.R, vs[i]);
+ vs[i] = b2Mul(xf, vs[i]);
+ }
+
+ m_debugDraw->DrawPolygon(vs, 4, color);
+ }
+ }
+ }
+
+ if (flags & b2DebugDraw::e_centerOfMassBit)
+ {
+ for (b2Body* b = m_bodyList; b; b = b->GetNext())
+ {
+ b2XForm xf = b->GetXForm();
+ xf.position = b->GetWorldCenter();
+ m_debugDraw->DrawXForm(xf);
+ }
+ }
+}
+
+void b2World::Validate()
+{
+ m_broadPhase->Validate();
+}
+
+int32 b2World::GetProxyCount() const
+{
+ return m_broadPhase->m_proxyCount;
+}
+
+int32 b2World::GetPairCount() const
+{
+ return m_broadPhase->m_pairManager.m_pairCount;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_WORLD_H
+#define B2_WORLD_H
+
+#include "../Common/b2Math.h"
+#include "../Common/b2BlockAllocator.h"
+#include "../Common/b2StackAllocator.h"
+#include "b2ContactManager.h"
+#include "b2WorldCallbacks.h"
+
+struct b2AABB;
+struct b2ShapeDef;
+struct b2BodyDef;
+struct b2JointDef;
+class b2Body;
+class b2Joint;
+class b2Shape;
+class b2Contact;
+class b2BroadPhase;
+
+struct b2TimeStep
+{
+ float32 dt; // time step
+ float32 inv_dt; // inverse time step (0 if dt == 0).
+ float32 dtRatio; // dt * inv_dt0
+ int32 maxIterations;
+ bool warmStarting;
+ bool positionCorrection;
+};
+
+/// The world class manages all physics entities, dynamic simulation,
+/// and asynchronous queries. The world also contains efficient memory
+/// management facilities.
+class b2World
+{
+public:
+ /// Construct a world object.
+ /// @param worldAABB a bounding box that completely encompasses all your shapes.
+ /// @param gravity the world gravity vector.
+ /// @param doSleep improve performance by not simulating inactive bodies.
+ b2World(const b2AABB& worldAABB, const b2Vec2& gravity, bool doSleep);
+
+ /// Destruct the world. All physics entities are destroyed and all heap memory is released.
+ ~b2World();
+
+ /// Register a destruction listener.
+ void SetDestructionListener(b2DestructionListener* listener);
+
+ /// Register a broad-phase boundary listener.
+ void SetBoundaryListener(b2BoundaryListener* listener);
+
+ /// Register a contact filter to provide specific control over collision.
+ /// Otherwise the default filter is used (b2_defaultFilter).
+ void SetContactFilter(b2ContactFilter* filter);
+
+ /// Register a contact event listener
+ void SetContactListener(b2ContactListener* listener);
+
+ /// Register a routine for debug drawing. The debug draw functions are called
+ /// inside the b2World::Step method, so make sure your renderer is ready to
+ /// consume draw commands when you call Step().
+ void SetDebugDraw(b2DebugDraw* debugDraw);
+
+ /// Create a rigid body given a definition. No reference to the definition
+ /// is retained.
+ /// @warning This function is locked during callbacks.
+ b2Body* CreateBody(const b2BodyDef* def);
+
+ /// Destroy a rigid body given a definition. No reference to the definition
+ /// is retained. This function is locked during callbacks.
+ /// @warning This automatically deletes all associated shapes and joints.
+ /// @warning This function is locked during callbacks.
+ void DestroyBody(b2Body* body);
+
+ /// Create a joint to constrain bodies together. No reference to the definition
+ /// is retained. This may cause the connected bodies to cease colliding.
+ /// @warning This function is locked during callbacks.
+ b2Joint* CreateJoint(const b2JointDef* def);
+
+ /// Destroy a joint. This may cause the connected bodies to begin colliding.
+ /// @warning This function is locked during callbacks.
+ void DestroyJoint(b2Joint* joint);
+
+ /// The world provides a single static ground body with no collision shapes.
+ /// You can use this to simplify the creation of joints and static shapes.
+ b2Body* GetGroundBody();
+
+ /// Take a time step. This performs collision detection, integration,
+ /// and constraint solution.
+ /// @param timeStep the amount of time to simulate, this should not vary.
+ /// @param iterations the number of iterations to be used by the constraint solver.
+ void Step(float32 timeStep, int32 iterations);
+
+ /// Query the world for all shapes that potentially overlap the
+ /// provided AABB. You provide a shape pointer buffer of specified
+ /// size. The number of shapes found is returned.
+ /// @param aabb the query box.
+ /// @param shapes a user allocated shape pointer array of size maxCount (or greater).
+ /// @param maxCount the capacity of the shapes array.
+ /// @return the number of shapes found in aabb.
+ int32 Query(const b2AABB& aabb, b2Shape** shapes, int32 maxCount);
+
+ /// Get the world body list. With the returned body, use b2Body::GetNext to get
+ /// the next body in the world list. A NULL body indicates the end of the list.
+ /// @return the head of the world body list.
+ b2Body* GetBodyList();
+
+ /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
+ /// the next joint in the world list. A NULL joint indicates the end of the list.
+ /// @return the head of the world joint list.
+ b2Joint* GetJointList();
+
+ /// Re-filter a shape. This re-runs contact filtering on a shape.
+ void Refilter(b2Shape* shape);
+
+ /// Enable/disable warm starting. For testing.
+ void SetWarmStarting(bool flag) { m_warmStarting = flag; }
+
+ /// Enable/disable position correction. For testing.
+ void SetPositionCorrection(bool flag) { m_positionCorrection = flag; }
+
+ /// Enable/disable continuous physics. For testing.
+ void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
+
+ /// Perform validation of internal data structures.
+ void Validate();
+
+ /// Get the number of broad-phase proxies.
+ int32 GetProxyCount() const;
+
+ /// Get the number of broad-phase pairs.
+ int32 GetPairCount() const;
+
+ /// Get the number of bodies.
+ int32 GetBodyCount() const;
+
+ /// Get the number joints.
+ int32 GetJointCount() const;
+
+ /// Get the number of contacts (each may have 0 or more contact points).
+ int32 GetContactCount() const;
+
+ /// Change the global gravity vector.
+ void SetGravity(const b2Vec2& gravity);
+
+private:
+
+ friend class b2Body;
+ friend class b2ContactManager;
+
+ void Solve(const b2TimeStep& step);
+ void SolveTOI(const b2TimeStep& step);
+
+ void DrawJoint(b2Joint* joint);
+ void DrawShape(b2Shape* shape, const b2XForm& xf, const b2Color& color, bool core);
+ void DrawDebugData();
+
+ b2BlockAllocator m_blockAllocator;
+ b2StackAllocator m_stackAllocator;
+
+ bool m_lock;
+
+ b2BroadPhase* m_broadPhase;
+ b2ContactManager m_contactManager;
+
+ b2Body* m_bodyList;
+ b2Joint* m_jointList;
+
+ // Do not access
+ b2Contact* m_contactList;
+
+ int32 m_bodyCount;
+ int32 m_contactCount;
+ int32 m_jointCount;
+
+ b2Vec2 m_gravity;
+ bool m_allowSleep;
+
+ b2Body* m_groundBody;
+
+ b2DestructionListener* m_destructionListener;
+ b2BoundaryListener* m_boundaryListener;
+ b2ContactFilter* m_contactFilter;
+ b2ContactListener* m_contactListener;
+ b2DebugDraw* m_debugDraw;
+
+ float32 m_inv_dt0;
+
+ int32 m_positionIterationCount;
+
+ // This is for debugging the solver.
+ bool m_positionCorrection;
+
+ // This is for debugging the solver.
+ bool m_warmStarting;
+
+ // This is for debugging the solver.
+ bool m_continuousPhysics;
+};
+
+inline b2Body* b2World::GetGroundBody()
+{
+ return m_groundBody;
+}
+
+inline b2Body* b2World::GetBodyList()
+{
+ return m_bodyList;
+}
+
+inline b2Joint* b2World::GetJointList()
+{
+ return m_jointList;
+}
+
+inline int32 b2World::GetBodyCount() const
+{
+ return m_bodyCount;
+}
+
+inline int32 b2World::GetJointCount() const
+{
+ return m_jointCount;
+}
+
+inline int32 b2World::GetContactCount() const
+{
+ return m_contactCount;
+}
+
+inline void b2World::SetGravity(const b2Vec2& gravity)
+{
+ m_gravity = gravity;
+}
+
+#endif
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2WorldCallbacks.h"
+#include "../Collision/Shapes/b2Shape.h"
+
+b2ContactFilter b2_defaultFilter;
+
+// Return true if contact calculations should be performed between these two shapes.
+// If you implement your own collision filter you may want to build from this implementation.
+bool b2ContactFilter::ShouldCollide(b2Shape* shape1, b2Shape* shape2)
+{
+ const b2FilterData& filter1 = shape1->GetFilterData();
+ const b2FilterData& filter2 = shape2->GetFilterData();
+
+ if (filter1.groupIndex == filter2.groupIndex && filter1.groupIndex != 0)
+ {
+ return filter1.groupIndex > 0;
+ }
+
+ bool collide = (filter1.maskBits & filter2.categoryBits) != 0 && (filter1.categoryBits & filter2.maskBits) != 0;
+ return collide;
+}
+
+b2DebugDraw::b2DebugDraw()
+{
+ m_drawFlags = 0;
+}
+
+void b2DebugDraw::SetFlags(uint32 flags)
+{
+ m_drawFlags = flags;
+}
+
+uint32 b2DebugDraw::GetFlags() const
+{
+ return m_drawFlags;
+}
+
+void b2DebugDraw::AppendFlags(uint32 flags)
+{
+ m_drawFlags |= flags;
+}
+
+void b2DebugDraw::ClearFlags(uint32 flags)
+{
+ m_drawFlags &= ~flags;
+}
--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_WORLD_CALLBACKS_H
+#define B2_WORLD_CALLBACKS_H
+
+#include "../Common/b2Settings.h"
+
+struct b2Vec2;
+struct b2XForm;
+class b2Shape;
+class b2Body;
+class b2Joint;
+class b2Contact;
+struct b2ContactPoint;
+struct b2ContactResult;
+
+/// Joints and shapes are destroyed when their associated
+/// body is destroyed. Implement this listener so that you
+/// may nullify references to these joints and shapes.
+class b2DestructionListener
+{
+public:
+ virtual ~b2DestructionListener() {}
+
+ /// Called when any joint is about to be destroyed due
+ /// to the destruction of one of its attached bodies.
+ virtual void SayGoodbye(b2Joint* joint) = 0;
+
+ /// Called when any shape is about to be destroyed due
+ /// to the destruction of its parent body.
+ virtual void SayGoodbye(b2Shape* shape) = 0;
+};
+
+
+/// This is called when a body's shape passes outside of the world boundary.
+class b2BoundaryListener
+{
+public:
+ virtual ~b2BoundaryListener() {}
+
+ /// This is called for each body that leaves the world boundary.
+ /// @warning you can't modify the world inside this callback.
+ virtual void Violation(b2Body* body) = 0;
+};
+
+
+/// Implement this class to provide collision filtering. In other words, you can implement
+/// this class if you want finer control over contact creation.
+class b2ContactFilter
+{
+public:
+ virtual ~b2ContactFilter() {}
+
+ /// Return true if contact calculations should be performed between these two shapes.
+ /// @warning for performance reasons this is only called when the AABBs begin to overlap.
+ virtual bool ShouldCollide(b2Shape* shape1, b2Shape* shape2);
+};
+
+/// The default contact filter.
+extern b2ContactFilter b2_defaultFilter;
+
+/// Implement this class to get collision results. You can use these results for
+/// things like sounds and game logic. You can also get contact results by
+/// traversing the contact lists after the time step. However, you might miss
+/// some contacts because continuous physics leads to sub-stepping.
+/// Additionally you may receive multiple callbacks for the same contact in a
+/// single time step.
+/// You should strive to make your callbacks efficient because there may be
+/// many callbacks per time step.
+/// @warning The contact separation is the last computed value.
+/// @warning You cannot create/destroy Box2D entities inside these callbacks.
+class b2ContactListener
+{
+public:
+ virtual ~b2ContactListener() {}
+
+ /// Called when a contact point is added. This includes the geometry
+ /// and the forces.
+ virtual void Add(const b2ContactPoint* point) { B2_NOT_USED(point); }
+
+ /// Called when a contact point persists. This includes the geometry
+ /// and the forces.
+ virtual void Persist(const b2ContactPoint* point) { B2_NOT_USED(point); }
+
+ /// Called when a contact point is removed. This includes the last
+ /// computed geometry and forces.
+ virtual void Remove(const b2ContactPoint* point) { B2_NOT_USED(point); }
+
+ /// Called after a contact point is solved.
+ virtual void Result(const b2ContactResult* point) { B2_NOT_USED(point); }
+};
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+ b2Color() {}
+ b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
+ float32 r, g, b;
+};
+
+/// Implement and register this class with a b2World to provide debug drawing of physics
+/// entities in your game.
+class b2DebugDraw
+{
+public:
+ b2DebugDraw();
+
+ virtual ~b2DebugDraw() {}
+
+ enum
+ {
+ e_shapeBit = 0x0001, ///< draw shapes
+ e_jointBit = 0x0002, ///< draw joint connections
+ e_coreShapeBit = 0x0004, ///< draw core (TOI) shapes
+ e_aabbBit = 0x0008, ///< draw axis aligned bounding boxes
+ e_obbBit = 0x0010, ///< draw oriented bounding boxes
+ e_pairBit = 0x0020, ///< draw broad-phase pairs
+ e_centerOfMassBit = 0x0040, ///< draw center of mass frame
+ };
+
+ /// Set the drawing flags.
+ void SetFlags(uint32 flags);
+
+ /// Get the drawing flags.
+ uint32 GetFlags() const;
+
+ /// Append flags to the current flags.
+ void AppendFlags(uint32 flags);
+
+ /// Clear flags from the current flags.
+ void ClearFlags(uint32 flags);
+
+ /// Draw a closed polygon provided in CCW order.
+ virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+ /// Draw a solid closed polygon provided in CCW order.
+ virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+ /// Draw a circle.
+ virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
+
+ /// Draw a solid circle.
+ virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
+
+ /// Draw a line segment.
+ virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
+
+ /// Draw a transform. Choose your own length scale.
+ /// @param xf a transform.
+ virtual void DrawXForm(const b2XForm& xf) = 0;
+
+protected:
+ uint32 m_drawFlags;
+};
+
+#endif
--- /dev/null
+Version 1.0.0
+- Clamp large rotations. DONE
+- Restitution. DONE
+- Prismatic joint. DONE
+- Limits and motors. DONE
+- Handle equal limits. DONE
+- Fast broadphase. DONE
+- Collide connected. DONE
+- Island sleeping. DONE
+- Custom allocators. DONE
+- Convex polygons. DONE
+- Compound shapes. DONE
+- Constraint graph. DONE
+- Contact solver. DONE
+- Joint solver. DONE
+- Improved stability. DONE
+- Improve contact matching. DONE
+- Contact solver. DONE
+- Joint solver. DONE
+- Mouse picking. DONE
+- SAP AABB query. DONE
+- Distance joint. DONE
+
+Version 1.0.1
+- Joint limit bug fix. DONE
+
+Version 1.1.0
+- Handle dangling joint pointers. DONE
+- Collision filters. DONE
+- User data. DONE
+- Improved API. DONE
+- Orphaned mouse joint. DONE
+
+Version 1.2.0
+- Rename engine.lib to box2d.lib. DONE
+- Code layout. DONE
+- Circles. DONE
+
+Version 1.2.1
+- Handle proxies hitting the world boundary. DONE
+- Freeze out of bounds bodies, notify user. DONE
+- Handle running out of pairs. DONE
+
+Version 1.3.0
+- Documentation phase 1. DONE
+- Add "no rotation" option to body def. DONE
+- b2Settings configuration for pixel coordinates. DONE
+- Slider-crank test. DONE
+- Notify client when a body is frozen. DONE
+- Fix collide-connected. DONE
+- Fix compound polygons. DONE
+- Check degenerate containment resolution. DONE (no problem found)
+- Access to contacts and joints. DONE
+
+Version 1.4.0
+- Gear joint. DONE
+- Pulley joint. DONE
+- Personalize the phpbb header. DONE
+- Document contacts and new joint types. DONE
+
+Version 1.4.1
+- Fix contact list. DONE
+- Damping. DONE
+- Poly-circle ids. DONE
+- Improve mouse joint. DONE
+
+Version 1.4.2
+- Improve joint stability. DONE.
+
+Version 1.4.3
+- Fix thin polygon collision. DONE
+- Simplify broad-phase internally. DONE
+- OSX patch. DONE
+- Fix compound body freezing. DONE
+- Collision filter callback. DONE
+
+Version 2.0.0
+- Collision doesn't include any files from Dynamics. DONE
+- User created shapes. DONE
+- Shape ray casts. DONE
+- Verify polygon convex and CCW. DONE
+- Deferred body destruction is gone. DONE
+- Deferred contact destruction is gone. DONE
+- JointDefs in local coordinates so that save/load works with limits. DONE
+- Wake up bonies on apply force/torque/impulse. DONE
+- Improve polygon collision performance. DONE
+- Optimal OBBs for polygons. DONE
+- Debug display interface. DONE
+- Access version number. DONE
+- Improved TestBed camera. DONE
+- Continuous physics with TOI islands. DONE
+- Max velocities. DONE
+- Body add/remove shape. DONE
+- Damping now works better with a variable time step. DONE
+- Time of Impact (TOI) callbacks. DONE
+- Contact solver tweak callbacks. DONE
+- Contact callbacks with impulses. DONE
+- Safe contact callback example. DONE
+- Draw world AABB in TestBed. DONE
+- Use forces instead of impulses to handle variable time step. DONE
+- Ragdoll contribution. DONE
+- Car example. DONE
+- Constraint graph and shapes support changes in center of mass. DONE
+- Sensors with overlap report. DONE
+- Doxygen. DONE
+- Update manual. DONE
+
+Version 2.1.0
+- Meshes (segment soup).
+- Ray casts.
+- Wiki. DONE
+- Pairs and proxies use small block allocator?
+- Bounds use growable array?
+- Don't sleep until position error is low?
+- Allow non-solid polygon edges.
+- User refilter.
+- CMake.
+- Kequalized crash.
+- Soft distance constraint. DONE
+-
+
+Version 2.2.0
+- Handle orphaned gear joint.
+- Friction/motor joint for top down games.
+- Rolling resistance for circles.
+- Add comments to examples and testbed.
+
+Version 2.3.0
+- Convex hull tool.
+- Angle joint.
+- Weld joint.
+- Speculative contacts.
+
+Version 2.4.0
+- Spline joints.
+- Contact block solver.
+
+Version 2.5.0
+- Convex decomposition.
+- Kinematic bodies.
+
+Version 2.6.0
+- Ropes and particles.
+- Growable vertex arrays.
+
+Version 3+
+- Performance.
+- Try position correction clamping and larger Baumgarte.
+- Characters.
+- Vehicles.
+- Tree constraints.
+- Buoyancy.
+- Heightfields.
+- Static point-grids.
+- Revolute 3x3 effective mass.
+- Use velocity lambda to control NGS clamping.
+- No globals or statics.
\ No newline at end of file
--- /dev/null
+DEPENDPATH += $$PWD
+
+HEADERS += \
+ Box2D/Source/Collision/b2PairManager.h \
+ Box2D/Source/Collision/b2Collision.h \
+ Box2D/Source/Collision/b2BroadPhase.h \
+ Box2D/Source/Collision/Shapes/b2Shape.h \
+ Box2D/Source/Collision/Shapes/b2PolygonShape.h \
+ Box2D/Source/Collision/Shapes/b2CircleShape.h \
+ Box2D/Source/Common/jtypes.h \
+ Box2D/Source/Common/Fixed.h \
+ Box2D/Source/Common/b2StackAllocator.h \
+ Box2D/Source/Common/b2Settings.h \
+ Box2D/Source/Common/b2Math.h \
+ Box2D/Source/Common/b2BlockAllocator.h \
+ Box2D/Source/Dynamics/b2WorldCallbacks.h \
+ Box2D/Source/Dynamics/b2World.h \
+ Box2D/Source/Dynamics/b2Island.h \
+ Box2D/Source/Dynamics/b2ContactManager.h \
+ Box2D/Source/Dynamics/b2Body.h \
+ Box2D/Source/Dynamics/Contacts/b2PolyContact.h \
+ Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.h \
+ Box2D/Source/Dynamics/Contacts/b2NullContact.h \
+ Box2D/Source/Dynamics/Contacts/b2ContactSolver.h \
+ Box2D/Source/Dynamics/Contacts/b2Contact.h \
+ Box2D/Source/Dynamics/Contacts/b2CircleContact.h \
+ Box2D/Source/Dynamics/Joints/b2RevoluteJoint.h \
+ Box2D/Source/Dynamics/Joints/b2PulleyJoint.h \
+ Box2D/Source/Dynamics/Joints/b2PrismaticJoint.h \
+ Box2D/Source/Dynamics/Joints/b2MouseJoint.h \
+ Box2D/Source/Dynamics/Joints/b2Joint.h \
+ Box2D/Source/Dynamics/Joints/b2GearJoint.h \
+ Box2D/Source/Dynamics/Joints/b2DistanceJoint.h \
+ Box2D/Source/Box2D.h
+
+SOURCES += \
+ Box2D/Source/Collision/b2TimeOfImpact.cpp \
+ Box2D/Source/Collision/b2PairManager.cpp \
+ Box2D/Source/Collision/b2Distance.cpp \
+ Box2D/Source/Collision/b2Collision.cpp \
+ Box2D/Source/Collision/b2CollidePoly.cpp \
+ Box2D/Source/Collision/b2CollideCircle.cpp \
+ Box2D/Source/Collision/b2BroadPhase.cpp \
+ Box2D/Source/Collision/Shapes/b2Shape.cpp \
+ Box2D/Source/Collision/Shapes/b2PolygonShape.cpp \
+ Box2D/Source/Collision/Shapes/b2CircleShape.cpp \
+ Box2D/Source/Common/b2StackAllocator.cpp \
+ Box2D/Source/Common/b2Settings.cpp \
+ Box2D/Source/Common/b2Math.cpp \
+ Box2D/Source/Common/b2BlockAllocator.cpp \
+ Box2D/Source/Dynamics/b2WorldCallbacks.cpp \
+ Box2D/Source/Dynamics/b2World.cpp \
+ Box2D/Source/Dynamics/b2Island.cpp \
+ Box2D/Source/Dynamics/b2ContactManager.cpp \
+ Box2D/Source/Dynamics/b2Body.cpp \
+ Box2D/Source/Dynamics/Contacts/b2PolyContact.cpp \
+ Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.cpp \
+ Box2D/Source/Dynamics/Contacts/b2ContactSolver.cpp \
+ Box2D/Source/Dynamics/Contacts/b2Contact.cpp \
+ Box2D/Source/Dynamics/Contacts/b2CircleContact.cpp \
+ Box2D/Source/Dynamics/Joints/b2RevoluteJoint.cpp \
+ Box2D/Source/Dynamics/Joints/b2PulleyJoint.cpp \
+ Box2D/Source/Dynamics/Joints/b2PrismaticJoint.cpp \
+ Box2D/Source/Dynamics/Joints/b2MouseJoint.cpp \
+ Box2D/Source/Dynamics/Joints/b2Joint.cpp \
+ Box2D/Source/Dynamics/Joints/b2GearJoint.cpp \
+ Box2D/Source/Dynamics/Joints/b2DistanceJoint.cpp
--- /dev/null
+DEPENDPATH += $$PWD
+
+HEADERS += \
+ wizard/accountwizard.h \
+ wizard/annuairepage.h
+
+SOURCES += \
+ wizard/accountwizard.cpp
+
+FORMS +=
--- /dev/null
+project(maemoblok)
+
+cmake_minimum_required(VERSION 2.4.0)
+find_package(Qt4 REQUIRED)
+
+
+INCLUDE_DIRECTORIES( ${QT_INCLUDE_DIR} ${QT_QTCORE_INCLUDE_DIR} ${QT_QTGUI_INCLUDE_DIR})
+ADD_DEFINITIONS( ${QT_DEFINITIONS} )
+
+
+set(maemoblok_SRCS
+ main.cpp)
+
+add_executable(qtproject ${maemoblok_SRCS})
+
+target_link_libraries(qtproject ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY})
+
+add_subdirectory(Box2D)
\ No newline at end of file
--- /dev/null
+#include <QFile>
+#include <QTextStream>
+#include <QTimer>
+#include <QStringList>
+
+#include "athread.h"
+
+aThread::aThread(QObject *parent)
+ : QThread(parent)
+{
+}
+
+void aThread::run()
+{
+ QTimer timer;
+ connect(&timer, SIGNAL(timeout()), this, SLOT(updateCoords()));
+ timer.start(10); // 50 Hz update rate
+ exec();
+}
+
+void aThread::updateCoords()
+{
+ QFile file("/sys/class/i2c-adapter/i2c-3/3-001d/coord");
+ if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
+ return;
+ QTextStream in(&file);
+ QString data = in.readAll();
+ processCoords(data);
+}
+
+void aThread::processCoords(QString &data)
+{
+ QStringList data_splited = data.split(" ");
+
+ x = data_splited[0];
+ y = data_splited[1];
+ z = data_splited[2];
+
+ emit deviceOrientation(x, y, z);
+}
+
+
--- /dev/null
+#ifndef ATHREAD_H
+#define ATHREAD_H
+
+#include <QThread>
+#include <QString>
+
+class aThread : public QThread
+{
+ Q_OBJECT
+
+public:
+ aThread(QObject *parent = 0);
+ void run();
+
+ QString x;
+ QString y;
+ QString z;
+
+public slots:
+ void updateCoords();
+ void processCoords(QString &data);
+
+signals:
+ void deviceOrientation(QString x, QString y, QString z);
+};
+
+#endif // ATHREAD_H
--- /dev/null
+
+QT += phonon
+TARGET = blok
+include (Box2D/box2d.pri)
+include (blokitem/blokitem.pri)
+
+INCLUDEPATH += Box2D/Source/
+
+
+SOURCES += \
+ main.cpp \
+ physicsscene.cpp \
+ boxitem.cpp \
+ mainwindow.cpp \
+ blokgameview.cpp \
+ athread.cpp
+
+
+HEADERS += \
+ physicsscene.h \
+ boxitem.h \
+ mainwindow.h \
+ blokgameview.h \
+ athread.h
+
+
+RESOURCES += \
+ data/sprites.qrc
--- /dev/null
+#include "blokgameview.h"
+
+BlokGameView::BlokGameView(QWidget *parent) :
+ QGraphicsView(parent)
+{
+
+ mScene = new PhysicsScene;
+ mWallpaperItem = new QGraphicsPixmapItem;
+ mGroundItem = new BoxItem(QRectF(0,430,800,50),true);
+
+ setScene(mScene);
+
+
+ mScene->addPhysicsItem(new BoxItem(QRectF(0,0,10,480),true));
+ mScene->addPhysicsItem(new BoxItem(QRectF(790,0,10,480),true));
+ mScene->addPhysicsItem(new BoxItem(QRectF(0,0,800,10),true));
+ mScene->addPhysicsItem(new BoxItem(QRectF(0,470,800,10),true));
+
+
+ //Level
+
+
+
+
+
+
+
+
+ mScene->addItem(mWallpaperItem);
+ mScene->addPhysicsItem(mGroundItem);
+
+ setWallpaper(":sprites/sky_wallpaper.png");
+ setGround(":sprites/green_ground.png");
+
+
+
+
+
+
+
+
+ for ( int i=0; i<5; ++i)
+ {
+
+ NormalBlokItem * item =new NormalBlokItem(1,1);
+ item->setPos(32,i*32);
+ mScene->addPhysicsItem(item);
+
+ NormalBlokItem * item2 =new NormalBlokItem(1,1);
+ item2->setPos(40, i*32);
+ mScene->addPhysicsItem(item2);
+
+
+
+ }
+
+
+
+mScene->addPhysicsItem(new SolidBlokItem(2,2));
+mScene->addPhysicsItem(new SolidBlokItem(1,4));
+
+
+
+ mScene->start();
+
+}
+
+void BlokGameView::setGround(const QString& path)
+{
+ mGroundItem->setBrush(QBrush(QPixmap(path)));
+}
+
+void BlokGameView::setWallpaper(const QString& path)
+{
+
+ mWallpaperItem->setPixmap(QPixmap(path));
+
+
+}
+
+void BlokGameView::mousePressEvent(QMouseEvent *event)
+{
+
+ if (event->button() == Qt::RightButton)
+ mScene->stop();
+
+ else
+ mScene->start();
+
+
+
+}
+
+void BlokGameView::loadMap(const QString& path)
+{
+
+
+}
--- /dev/null
+#ifndef BLOKGAMEVIEW_H
+#define BLOKGAMEVIEW_H
+
+#include <QGraphicsView>
+#include <QMouseEvent>
+#include "physicsscene.h"
+#include "boxitem.h"
+
+#include "blokitem/chimicblokitem.h"
+#include "blokitem/solidblokitem.h"
+#include "blokitem/normalblokitem.h"
+#include "blokitem/explodeblokitem.h"
+#include "blokitem/totemblokitem.h"
+
+
+
+class BlokGameView : public QGraphicsView
+{
+ Q_OBJECT
+public:
+ explicit BlokGameView(QWidget *parent = 0);
+void setWallpaper(const QString& path);
+void setGround(const QString& path);
+
+signals:
+
+public slots:
+void loadMap(const QString& path);
+
+
+protected:
+void mousePressEvent(QMouseEvent *event);
+
+private:
+PhysicsScene * mScene;
+QGraphicsPixmapItem * mWallpaperItem;
+BoxItem * mGroundItem;
+
+};
+
+#endif // BLOKGAMEVIEW_H
--- /dev/null
+#include "blokitem.h"
+
+BlokItem::BlokItem(int width, int height,QObject *parent):
+ BoxItem(QRectF(0,0,width*32,height*32))
+{
+}
--- /dev/null
+#ifndef BLOKITEM_H
+#define BLOKITEM_H
+#include <QObject>
+#include "boxitem.h"
+
+class BlokItem :public QObject, public BoxItem
+{
+ Q_OBJECT
+public:
+ enum BlokType{NORMAL_BLOK, SOLID_BLOK, CHIMIC_BLOK, EXPLOSE_BLOK};
+ Q_ENUMS(BlokType);
+ explicit BlokItem(int width=1, int height=1,QObject *parent = 0);
+ BlokType blokType(){return mBlokType;}
+protected:
+ void setType(BlokType type){mBlokType = type;}
+
+signals:
+
+public slots:
+
+private:
+ BlokType mBlokType;
+
+};
+
+#endif // BLOKITEM_H
--- /dev/null
+DEPENDPATH += $$PWD
+
+HEADERS += \
+ blokitem/blokitem.h \
+ blokitem/totemblokitem.h \
+ blokitem/normalblokitem.h \
+ blokitem/explodeblokitem.h \
+ blokitem/chimicblokitem.h \
+ blokitem/solidblokitem.h
+
+SOURCES += \
+ blokitem/blokitem.cpp \
+ blokitem/totemblokitem.cpp \
+ blokitem/normalblokitem.cpp \
+ blokitem/explodeblokitem.cpp \
+ blokitem/chimicblokitem.cpp \
+ blokitem/solidblokitem.cpp
--- /dev/null
+DEPENDPATH += $$PWD
--- /dev/null
+DEPENDPATH += $$PWD
+
+HEADERS += \
+ blokitem/blokitem.h \
+ blokitem/totemblokitem.h \
+ blokitem/normalblokitem.h \
+ blokitem/explodeblokitem.h \
+ blokitem/chimicblokitem.h \
+ blokitem/solidblokitem.h
+
+SOURCES += \
+ blokitem/blokitem.cpp \
+ blokitem/totemblokitem.cpp \
+ blokitem/normalblokitem.cpp \
+ blokitem/explodeblokitem.cpp \
+ blokitem/chimicblokitem.cpp \
+ blokitem/solidblokitem.cpp
--- /dev/null
+#include "chimicblokitem.h"
+
+ChimicBlokItem::ChimicBlokItem(int width, int height,QObject *parent) :
+ BlokItem(width, height,parent)
+{
+
+ setBrush(QBrush(QPixmap(":sprites/chimic_block.png")));
+
+
+
+
+}
--- /dev/null
+#ifndef CHIMICBLOKITEM_H
+#define CHIMICBLOKITEM_H
+#include "blokitem.h"
+class ChimicBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ explicit ChimicBlokItem(int width=1, int height=1,QObject *parent = 0);
+
+signals:
+
+public slots:
+
+};
+
+#endif // CHIMICBLOKITEM_H
--- /dev/null
+#include "explodeblokitem.h"
+
+ExplodeBlokItem::ExplodeBlokItem(int width, int height,QObject *parent) :
+ BlokItem(width,height,parent)
+{
+
+ setBrush(QBrush(QPixmap(":sprites/explode_block.png")));
+
+}
--- /dev/null
+#ifndef EXPLOSEBLOKITEM_H
+#define EXPLOSEBLOKITEM_H
+#include "blokitem.h"
+class ExplodeBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ explicit ExplodeBlokItem(int width=1, int height=1,QObject *parent = 0);
+
+signals:
+
+public slots:
+
+};
+
+#endif // EXPLOSEBLOKITEM_H
--- /dev/null
+#include "explodeblokitem.h"
+
+ExplodeBlokItem::ExplodeBlokItem(int width, int height,QObject *parent) :
+ BlokItem(width,height,parent)
+{
+
+ setBrush(QBrush(QPixmap(":sprites/explode_block.png")));
+
+}
--- /dev/null
+#ifndef EXPLOSEBLOKITEM_H
+#define EXPLOSEBLOKITEM_H
+#include "blokitem.h"
+class ExplodeBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ explicit ExplodeBlokItem(int width=32, int height=32,QObject *parent = 0);
+
+signals:
+
+public slots:
+
+};
+
+#endif // EXPLOSEBLOKITEM_H
--- /dev/null
+#include "normalblokitem.h"
+#include <QDebug>
+
+NormalBlokItem::NormalBlokItem(int width, int height,QObject *parent) :
+ BlokItem(width,height,parent)
+{
+
+ mAnimationTimeLine = new QTimeLine;
+ mAnimationTimeLine->setFrameRange(0,10);
+ setBrush(QBrush(QPixmap(":sprites/normal_block.png")));
+
+ connect(mAnimationTimeLine,SIGNAL(frameChanged(int)),this,SLOT(anim(int)));
+
+}
+
+void NormalBlokItem::mousePressEvent(QGraphicsSceneMouseEvent *event)
+{
+
+
+
+ mAnimationTimeLine->start();
+
+ setBrush(Qt::white);
+ QPen pen;
+ pen.setColor(Qt::transparent);
+ setPen(pen);
+
+}
+
+void NormalBlokItem::anim(int frame)
+{
+ qDebug()<<"anim";
+
+ QColor c= brush().color();
+ int alpha = c.alpha();
+
+ alpha-= 255/9;
+
+ qDebug()<<alpha;
+ if ( alpha <=0)
+ {
+ qDebug()<<"remove";
+ PhysicsScene * s = qobject_cast<PhysicsScene*>(scene());
+ s->world()->DestroyBody(body());
+ s->removeItem(this);
+ return;
+ }
+
+ c.setAlpha(alpha);
+ setBrush(QBrush(c));
+
+}
--- /dev/null
+#ifndef NORMALBLOKITEM_H
+#define NORMALBLOKITEM_H
+#include "blokitem.h"
+#include <QMouseEvent>
+#include <QTimeLine>
+#include "physicsscene.h"
+class NormalBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ explicit NormalBlokItem(int width=1, int height=1,QObject *parent = 0);
+
+
+protected:
+ void mousePressEvent(QGraphicsSceneMouseEvent *event);
+signals:
+
+public slots:
+void anim(int frame);
+
+private:
+QTimeLine *mAnimationTimeLine;
+};
+
+#endif // NORMALBLOKITEM_H
--- /dev/null
+#include "solidblokitem.h"
+
+SolidBlokItem::SolidBlokItem(int width, int height,QObject * parent)
+ :BlokItem(width,height,parent)
+{
+
+ setBrush(QBrush(QPixmap(":sprites/solid_block.png")));
+
+}
--- /dev/null
+#ifndef SOLIDBLOKITEM_H
+#define SOLIDBLOKITEM_H
+#include "blokitem.h"
+
+class SolidBlokItem : public BlokItem
+{
+public:
+ SolidBlokItem(int width=1, int height=1,QObject * parent=0);
+};
+
+#endif // SOLIDBLOKITEM_H
--- /dev/null
+#include "totemblokitem.h"
+
+TotemBlokItem::TotemBlokItem(int width,int height,QObject *parent) :
+ BlokItem(width, height,parent)
+{
+ setTotem(YELLOW_TOTEM);
+ QPen pen;
+ pen.setColor(Qt::transparent);
+ setPen(pen);
+}
+
+
+void TotemBlokItem::setTotem(Totem _totem)
+{
+ mCurrentTotem = _totem;
+
+ if ( mCurrentTotem == YELLOW_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem1_block.png")));
+ }
+
+ if ( mCurrentTotem == BLUE_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem2_block.png")));
+
+ }
+
+ if ( mCurrentTotem == RED_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem3_block.png")));
+
+ }
+
+
+
+}
--- /dev/null
+#ifndef TOTEMBLOKITEM_H
+#define TOTEMBLOKITEM_H
+#include "blokitem.h"
+class TotemBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ enum Totem{YELLOW_TOTEM, BLUE_TOTEM, RED_TOTEM};
+ Q_ENUMS (Totem);
+ explicit TotemBlokItem(int width=1,int height=1,QObject *parent = 0);
+
+ void setTotem(Totem _totem);
+signals:
+
+public slots:
+
+private:
+ Totem mCurrentTotem;
+
+};
+
+#endif // TOTEMBLOKITEM_H
--- /dev/null
+#include "boxitem.h"
+#include <QDebug>
+#include <cmath>
+#include <QPainter>
+
+#define MAEMO_WIDTH 800
+#define MAEMO_HEIGHT 480
+
+#define BOX2D_WIDTH 10
+#define BOX2D_HEIGHT 6
+
+
+BoxItem::BoxItem(QRectF rect, bool isStatic)
+ :QGraphicsPolygonItem()
+{
+ setPolygon(QPolygonF(rect));
+ mIsStatic = isStatic;
+
+ m_body = 0;
+ mNeedRemoveFromWorld = false;
+
+
+}
+
+BoxItem::BoxItem(QPolygonF poly, bool isStatic )
+ :QGraphicsPolygonItem()
+{
+ setPolygon(poly);
+ mIsStatic = isStatic;
+}
+
+
+void BoxItem::setup(b2World *world)
+{
+ if ( polygon().size() > 8)
+ {
+ qDebug()<<"Error! Cannot build a physics polygon with more 8 vertex...";
+ return;
+ }
+
+ double angleRad = rotation() * M_PI / 180;
+
+
+ //#define MAEMO_WIDTH 800
+ //#define MAEMO_HEIGHT 480
+
+ //#define BOX2D_WIDTH 10
+ //#define BOX2D_HEIGHT 6
+
+
+
+ m_bodyDef.userData = this;
+ m_bodyDef.position.Set(pos().x() * 10/800, pos().y() * 6/480);
+ m_bodyDef.angle = angleRad;
+
+ m_polygonDef.friction = 0.3f;
+ m_polygonDef.density = 1.0f;
+ m_polygonDef.restitution = 0.5f;
+ m_polygonDef.vertexCount = polygon().count() - 1;
+ //---CONSTRUCT A POYLGON SHAPE
+ int i=0;
+ QPolygonF poly = polygon();
+ poly.remove(poly.count()-1);
+ foreach (QPointF p, poly)
+ {
+ m_polygonDef.vertices[i].Set(p.x()* 10/800,p.y()* 6/480);
+ ++i;
+ }
+
+ m_body = world->CreateBody(&m_bodyDef);
+ m_body->CreateShape(&m_polygonDef);
+ if (!mIsStatic)
+ m_body->SetMassFromShapes();
+
+
+}
+void BoxItem::updatePhysics()
+{
+ resetTransform();
+
+
+ double factorX =BOX2D_WIDTH / MAEMO_WIDTH;
+ double factorY =BOX2D_HEIGHT / MAEMO_HEIGHT;
+
+ double angleDeg = m_body->GetAngle() * 180 / M_PI;
+ setPos(m_body->GetPosition().x*800/10, m_body->GetPosition().y*480/6);
+ setRotation(angleDeg);
+
+}
+
+void BoxItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
+{
+
+ painter->setBrush(brush());
+ painter->setPen(pen());
+ painter->drawPolygon(polygon());
+
+
+}
+
+b2Body* BoxItem::body()
+{
+if ( m_body == 0)
+ return 0;
+
+return m_body;
+
+}
--- /dev/null
+#ifndef BOXITEM_H
+#define BOXITEM_H
+
+#include <QGraphicsPolygonItem>
+#include <QPixmap>
+#include <QPainter>
+#include "Box2D/Source/Box2D.h"
+
+#define MAEMO_WIDTH 800
+#define MAEMO_HEIGHT 480
+
+class BoxItem : public QGraphicsPolygonItem
+{
+public:
+ BoxItem(QRectF rect, bool isStatic = false);
+ BoxItem(QPolygonF poly, bool isStatic = false);
+
+ void setPolygonDef(b2PolygonDef p){m_polygonDef = p;}
+ void setBodyDef(b2BodyDef b){m_bodyDef = b;}
+ void initDensity(const float32 &d ){m_polygonDef.density = d;}
+ void initFriction(const float32 &f){m_polygonDef.friction = f;}
+ void initRestitution(const float32 &r){m_polygonDef.restitution = r;}
+ void initSensor(bool b){m_polygonDef.isSensor = b;}
+ void setMass(const float32 &m){m_bodyDef.massData.mass = m;}
+ void initLinearDamping(const float32 &l){m_bodyDef.linearDamping = l;}
+ void initAngularDamping(const float32 &a){m_bodyDef.angularDamping = a;}
+ void initCategoryBits(const uint16 &c){m_polygonDef.filter.categoryBits = c;}
+ void initMaskBits(const uint16 &c){m_polygonDef.filter.maskBits = c;}
+ void initStatic(bool isStatic=true){mIsStatic=isStatic;}
+
+
+ b2PolygonDef *polygonDef(){return &m_polygonDef;}
+ b2BodyDef * bodyDef(){return &m_bodyDef;}
+ inline const float32 &density(){return m_polygonDef.density;}
+ inline const float32 &friction(){return m_polygonDef.friction;}
+ inline const float32 &restition(){return m_polygonDef.restitution;}
+ inline const float32 &mass(){return m_bodyDef.massData.mass; }
+ inline bool isSensor(){return m_polygonDef.isSensor;}
+ inline const float32 &linearDamping(){return m_bodyDef.linearDamping;}
+ inline const float32 &angularDamping(){return m_bodyDef.angularDamping;}
+ inline const uint16 &categoryBits(){return m_polygonDef.filter.categoryBits;}
+ inline const uint16 &maskBits(){return m_polygonDef.filter.maskBits;}
+
+ b2Body * body();
+
+
+
+ void setup(b2World *world);
+ void updatePhysics();
+
+ void removeFromWorld()
+ {
+ mNeedRemoveFromWorld = true;
+ }
+
+ bool needRemoveFromWorld()
+ {
+ return mNeedRemoveFromWorld;
+ }
+
+
+protected:
+ void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget);
+
+private:
+ b2PolygonDef m_polygonDef;
+ b2BodyDef m_bodyDef;
+ b2Body *m_body;
+ bool mIsStatic;
+ bool mNeedRemoveFromWorld;
+ QPixmap mPixmap;
+};
+
+#endif // BOXITEM_H
--- /dev/null
+<RCC>
+ <qresource prefix="/">
+ <file>sprites/blue_ground.png</file>
+ <file>sprites/chimic_block.png</file>
+ <file>sprites/clanbomber.png</file>
+ <file>sprites/cloud.png</file>
+ <file>sprites/cursor.png</file>
+ <file>sprites/explode_block.png</file>
+ <file>sprites/explose.png</file>
+ <file>sprites/fire_wallpaper.png</file>
+ <file>sprites/green_ground.png</file>
+ <file>sprites/normal_block.png</file>
+ <file>sprites/red_ground.png</file>
+ <file>sprites/solid_block.png</file>
+ <file>sprites/supermario_wallpaper.png</file>
+ <file>sprites/text05.png</file>
+ <file>sprites/text06.png</file>
+ <file>sprites/text07.png</file>
+ <file>sprites/totem1_block.png</file>
+ <file>sprites/totem2_block.png</file>
+ <file>sprites/totem3_block.png</file>
+ <file>sprites/sky_wallpaper.png</file>
+ <file>sounds/explosion.wav</file>
+ </qresource>
+</RCC>
--- /dev/null
+[Dolphin]
+ShowPreview=true
+Timestamp=2010,7,8,22,24,40
--- /dev/null
+#include <QApplication>
+#include <QWidget>
+#include "mainwindow.h"
+int main(int argc, char **argv)
+{
+
+ QApplication app(argc,argv);
+
+
+ MainWindow * win = new MainWindow;
+
+ win->show();
+
+ app.exec();
+
+
+// defined(Q_WS_MAEMO_5)
+
+
+}
--- /dev/null
+#include "mainwindow.h"
+#include <QPixmap>
+MainWindow::MainWindow(QWidget *parent) :
+ QMainWindow(parent)
+{
+ mView = new BlokGameView;
+
+ setCentralWidget(mView);
+
+ setFixedSize(800,480);
+
+ showFullScreen();
+}
+
--- /dev/null
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include <QGraphicsView>
+#include "physicsscene.h"
+
+#include "blokgameview.h"
+
+
+class MainWindow : public QMainWindow
+{
+ Q_OBJECT
+public:
+ explicit MainWindow(QWidget *parent = 0);
+signals:
+
+public slots:
+
+
+private:
+ BlokGameView * mView;
+};
+
+#endif // MAINWINDOW_H
--- /dev/null
+#include "physicsscene.h"
+#include <QDebug>
+
+
+#include "athread.h"
+
+PhysicsScene::PhysicsScene(QObject *parent) :
+ QGraphicsScene(parent)
+{
+ mGravity= b2Vec2(0.0f,10.0f);
+ createWorld();
+ mTimer = new QTimer;
+ mTimer->setInterval(20);
+ mTime = QTime::currentTime();
+ connect(mTimer,SIGNAL(timeout()),this,SLOT(mainLoop()));
+ mFps = 20;
+
+ aThread * accelerometer = new aThread(this);
+ connect(accelerometer,SIGNAL(deviceOrientation(QString,QString,QString)),this,SLOT(moveXYZ(QString,QString,QString)));
+
+
+ accelerometer->start(QThread::NormalPriority);
+}
+PhysicsScene::~PhysicsScene()
+{
+
+ delete mWorld;
+}
+
+void PhysicsScene::createWorld()
+{
+
+ b2AABB worldAABB;
+ worldAABB.lowerBound.Set(-100.0f, -100.0f);
+ worldAABB.upperBound.Set(100.0f, 100.0f);
+ bool doSleep = false;
+ mWorld = new b2World(worldAABB, mGravity, doSleep);
+
+}
+void PhysicsScene::addPhysicsItem(BoxItem * item)
+{
+ addItem(item);
+ item->setup(mWorld);
+ mBoxList.append(item);
+}
+void PhysicsScene::removePhysicsItem(BoxItem * item)
+{
+ mBoxList.removeOne(item);
+ mWorld->DestroyBody(item->body());
+ removeItem(item);
+
+}
+
+void PhysicsScene::computeSimulation(int32 iterations)
+{
+
+ double R = mFps/1000;
+
+ float32 timeStep = R;
+ mWorld->Step(timeStep, iterations);
+
+ b2Body* node = mWorld->GetBodyList();
+ while (node)
+ {
+ b2Body* b = node;
+ node = node->GetNext();
+
+ if ( b->GetUserData()!=NULL)
+ {
+ // qDebug()<<"positionx:"<<b->GetPosition().x;
+ // q#include <phonon/MediaObject>
+//qDebug()<<"positiony:"<<b->GetPosition().y;
+
+
+ BoxItem* item = (BoxItem*)b->GetUserData();
+ if (!item->needRemoveFromWorld())
+ item->updatePhysics();
+
+ else
+ {
+ mWorld->DestroyBody(item->body());
+ }
+
+ }
+ }
+}
+
+void PhysicsScene::keyPressEvent(QKeyEvent *event)
+{
+ qDebug()<<"key press";
+
+ computeSimulation();
+
+}
+
+void PhysicsScene::mainLoop()
+{
+ mFps = mTime.elapsed();
+ mTime = QTime::currentTime();
+
+ computeSimulation();
+
+
+}
+
+void PhysicsScene::start()
+{
+ mTimer->start();
+}
+
+void PhysicsScene::stop()
+{
+
+
+
+
+
+ mTimer->stop();
+
+}
+
+void PhysicsScene::moveXYZ(QString x, QString y, QString z)
+{
+#if defined(Q_WS_MAEMO_5)
+
+
+
+
+
+ mWorld->SetGravity(b2Vec2(-x.toDouble()/10, -y.toDouble() / 10));
+
+#endif
+
+ }
+
+
+
--- /dev/null
+#ifndef PHYSICSSCENE_H
+#define PHYSICSSCENE_H
+
+#include <QWidget>
+#include <QGraphicsScene>
+#include <QTimer>
+#include <QTime>
+#include <QList>
+
+#include "Box2D/Source/Box2D.h"
+
+#include "boxitem.h"
+
+#include <QKeyEvent>
+class PhysicsScene : public QGraphicsScene
+{
+ Q_OBJECT
+public:
+ explicit PhysicsScene(QObject *parent = 0);
+ ~PhysicsScene();
+ void createWorld();
+ void addPhysicsItem(BoxItem * item);
+ void removePhysicsItem(BoxItem * item);
+ void start();
+ void stop();
+ b2World * world(){return mWorld;}
+
+signals:
+
+public slots:
+ virtual void mainLoop();
+ void moveXYZ(QString x, QString y, QString z);
+
+protected:
+ void keyPressEvent(QKeyEvent *event);
+ void computeSimulation(int32 iterations = 10);
+
+private:
+ b2World *mWorld;
+ b2Vec2 mGravity;
+ QTimer* mTimer;
+ QTime mTime;
+ double mFps;
+ QList <BoxItem*> mBoxList;
+
+ BoxItem * ground;
+
+
+};
+
+#endif // PHYSICSSCENE_H
--- /dev/null
+#include "totemblokitem.h"
+
+TotemBlokItem::TotemBlokItem(int width,int height,QObject *parent) :
+ BlokItem(width, height,parent)
+{
+ setTotem(YELLOW_TOTEM);
+}
+
+
+void TotemBlokItem::setTotem(Totem _totem)
+{
+ mCurrentTotem = _totem;
+
+ if ( mCurrentTotem == YELLOW_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem1_block.png")));
+ }
+
+ if ( mCurrentTotem == BLUE_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem2_block.png")));
+
+ }
+
+ if ( mCurrentTotem == RED_TOTEM)
+ {
+ setBrush(QBrush(QPixmap(":sprites/totem3_block.png")));
+
+ }
+
+
+
+}
--- /dev/null
+#ifndef TOTEMBLOKITEM_H
+#define TOTEMBLOKITEM_H
+#include "blokitem.h"
+class TotemBlokItem : public BlokItem
+{
+ Q_OBJECT
+public:
+ enum Totem{YELLOW_TOTEM, BLUE_TOTEM, RED_TOTEM};
+ Q_ENUMS (Totem);
+ explicit TotemBlokItem(int width=64,int height=64,QObject *parent = 0);
+
+ void setTotem(Totem _totem);
+signals:
+
+public slots:
+
+private:
+ Totem mCurrentTotem;
+
+};
+
+#endif // TOTEMBLOKITEM_H