first commit
authorschutz <schutz@schutz.(none)>
Mon, 2 Aug 2010 15:40:12 +0000 (17:40 +0200)
committerschutz <schutz@schutz.(none)>
Mon, 2 Aug 2010 15:40:12 +0000 (17:40 +0200)
125 files changed:
Box2D/CMakeLists.txt [new file with mode: 0644]
Box2D/CMakeLists.txt~ [new file with mode: 0644]
Box2D/License.txt [new file with mode: 0644]
Box2D/Readme.txt [new file with mode: 0644]
Box2D/Source/Box2D.h [new file with mode: 0644]
Box2D/Source/CMakeLists.txt [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2CircleShape.cpp [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2CircleShape.h [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2PolygonShape.cpp [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2PolygonShape.h [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2Shape.cpp [new file with mode: 0644]
Box2D/Source/Collision/Shapes/b2Shape.h [new file with mode: 0644]
Box2D/Source/Collision/b2BroadPhase.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2BroadPhase.h [new file with mode: 0644]
Box2D/Source/Collision/b2CollideCircle.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2CollidePoly.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2Collision.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2Collision.h [new file with mode: 0644]
Box2D/Source/Collision/b2Distance.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2PairManager.cpp [new file with mode: 0644]
Box2D/Source/Collision/b2PairManager.h [new file with mode: 0644]
Box2D/Source/Collision/b2TimeOfImpact.cpp [new file with mode: 0644]
Box2D/Source/Common/Fixed.h [new file with mode: 0644]
Box2D/Source/Common/b2BlockAllocator.cpp [new file with mode: 0644]
Box2D/Source/Common/b2BlockAllocator.h [new file with mode: 0644]
Box2D/Source/Common/b2Math.cpp [new file with mode: 0644]
Box2D/Source/Common/b2Math.h [new file with mode: 0644]
Box2D/Source/Common/b2Settings.cpp [new file with mode: 0644]
Box2D/Source/Common/b2Settings.h [new file with mode: 0644]
Box2D/Source/Common/b2StackAllocator.cpp [new file with mode: 0644]
Box2D/Source/Common/b2StackAllocator.h [new file with mode: 0644]
Box2D/Source/Common/jtypes.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2CircleContact.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2CircleContact.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2Contact.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2Contact.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2ContactSolver.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2ContactSolver.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2NullContact.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.h [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2PolyContact.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Contacts/b2PolyContact.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2DistanceJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2DistanceJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2GearJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2GearJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2Joint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2Joint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2MouseJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2MouseJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2PrismaticJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2PrismaticJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2PulleyJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2PulleyJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2RevoluteJoint.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/Joints/b2RevoluteJoint.h [new file with mode: 0644]
Box2D/Source/Dynamics/b2Body.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/b2Body.h [new file with mode: 0644]
Box2D/Source/Dynamics/b2ContactManager.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/b2ContactManager.h [new file with mode: 0644]
Box2D/Source/Dynamics/b2Island.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/b2Island.h [new file with mode: 0644]
Box2D/Source/Dynamics/b2World.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/b2World.h [new file with mode: 0644]
Box2D/Source/Dynamics/b2WorldCallbacks.cpp [new file with mode: 0644]
Box2D/Source/Dynamics/b2WorldCallbacks.h [new file with mode: 0644]
Box2D/TODO.txt [new file with mode: 0644]
Box2D/box2d.pri [new file with mode: 0755]
Box2D/box2d.pri~ [new file with mode: 0755]
CMakeLists.txt [new file with mode: 0644]
athread.cpp [new file with mode: 0644]
athread.h [new file with mode: 0644]
blok.pro [new file with mode: 0644]
blokgameview.cpp [new file with mode: 0644]
blokgameview.h [new file with mode: 0644]
blokitem/blokitem.cpp [new file with mode: 0644]
blokitem/blokitem.h [new file with mode: 0644]
blokitem/blokitem.pri [new file with mode: 0644]
blokitem/blokitem.pri~ [new file with mode: 0755]
blokitem/box2d.pri [new file with mode: 0755]
blokitem/chimicblokitem.cpp [new file with mode: 0644]
blokitem/chimicblokitem.h [new file with mode: 0644]
blokitem/explodeblokitem.cpp [new file with mode: 0644]
blokitem/explodeblokitem.h [new file with mode: 0644]
blokitem/exploseblokitem.cpp [new file with mode: 0644]
blokitem/exploseblokitem.h [new file with mode: 0644]
blokitem/normalblokitem.cpp [new file with mode: 0644]
blokitem/normalblokitem.h [new file with mode: 0644]
blokitem/solidblokitem.cpp [new file with mode: 0644]
blokitem/solidblokitem.h [new file with mode: 0644]
blokitem/totemblokitem.cpp [new file with mode: 0644]
blokitem/totemblokitem.h [new file with mode: 0644]
boxitem.cpp [new file with mode: 0644]
boxitem.h [new file with mode: 0644]
data/sounds/explosion.wav [new file with mode: 0644]
data/sprites.qrc [new file with mode: 0644]
data/sprites/.directory [new file with mode: 0644]
data/sprites/blue_ground.png [new file with mode: 0755]
data/sprites/chimic_block.png [new file with mode: 0755]
data/sprites/clanbomber.png [new file with mode: 0644]
data/sprites/cloud.png [new file with mode: 0644]
data/sprites/cursor.png [new file with mode: 0644]
data/sprites/explode_block.png [new file with mode: 0644]
data/sprites/explose.png [new file with mode: 0644]
data/sprites/fire_wallpaper.png [new file with mode: 0644]
data/sprites/green_ground.png [new file with mode: 0755]
data/sprites/normal_block.png [new file with mode: 0755]
data/sprites/red_ground.png [new file with mode: 0755]
data/sprites/sky_wallpaper.png [new file with mode: 0755]
data/sprites/solid_block.png [new file with mode: 0755]
data/sprites/supermario_wallpaper.png [new file with mode: 0644]
data/sprites/text05.png [new file with mode: 0755]
data/sprites/text06.png [new file with mode: 0755]
data/sprites/text07.png [new file with mode: 0755]
data/sprites/totem1_block.png [new file with mode: 0755]
data/sprites/totem2_block.png [new file with mode: 0755]
data/sprites/totem3_block.png [new file with mode: 0755]
main.cpp [new file with mode: 0644]
mainwindow.cpp [new file with mode: 0644]
mainwindow.h [new file with mode: 0644]
physicsscene.cpp [new file with mode: 0644]
physicsscene.h [new file with mode: 0644]
totemblokitem.cpp [new file with mode: 0644]
totemblokitem.h [new file with mode: 0644]

diff --git a/Box2D/CMakeLists.txt b/Box2D/CMakeLists.txt
new file mode 100644 (file)
index 0000000..a2a89ce
--- /dev/null
@@ -0,0 +1,3 @@
+project(kglengine)
+add_subdirectory(Source)
+
diff --git a/Box2D/CMakeLists.txt~ b/Box2D/CMakeLists.txt~
new file mode 100644 (file)
index 0000000..121428f
--- /dev/null
@@ -0,0 +1,12 @@
+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})
+
diff --git a/Box2D/License.txt b/Box2D/License.txt
new file mode 100644 (file)
index 0000000..65ba0dc
--- /dev/null
@@ -0,0 +1,18 @@
+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.
+
diff --git a/Box2D/Readme.txt b/Box2D/Readme.txt
new file mode 100644 (file)
index 0000000..9ec1759
--- /dev/null
@@ -0,0 +1,19 @@
+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
diff --git a/Box2D/Source/Box2D.h b/Box2D/Source/Box2D.h
new file mode 100644 (file)
index 0000000..ebdb75e
--- /dev/null
@@ -0,0 +1,52 @@
+/*
+* 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
diff --git a/Box2D/Source/CMakeLists.txt b/Box2D/Source/CMakeLists.txt
new file mode 100644 (file)
index 0000000..dcb2484
--- /dev/null
@@ -0,0 +1,40 @@
+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" )
+
diff --git a/Box2D/Source/Collision/Shapes/b2CircleShape.cpp b/Box2D/Source/Collision/Shapes/b2CircleShape.cpp
new file mode 100644 (file)
index 0000000..3949628
--- /dev/null
@@ -0,0 +1,120 @@
+/*
+* 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));
+}
diff --git a/Box2D/Source/Collision/Shapes/b2CircleShape.h b/Box2D/Source/Collision/Shapes/b2CircleShape.h
new file mode 100644 (file)
index 0000000..4ce35ef
--- /dev/null
@@ -0,0 +1,92 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/Shapes/b2PolygonShape.cpp b/Box2D/Source/Collision/Shapes/b2PolygonShape.cpp
new file mode 100644 (file)
index 0000000..936fb51
--- /dev/null
@@ -0,0 +1,449 @@
+
+/*
+* 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]);
+}
diff --git a/Box2D/Source/Collision/Shapes/b2PolygonShape.h b/Box2D/Source/Collision/Shapes/b2PolygonShape.h
new file mode 100644 (file)
index 0000000..20834ce
--- /dev/null
@@ -0,0 +1,163 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/Shapes/b2Shape.cpp b/Box2D/Source/Collision/Shapes/b2Shape.cpp
new file mode 100644 (file)
index 0000000..e11b4fa
--- /dev/null
@@ -0,0 +1,167 @@
+/*
+* 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;
+       }
+}
diff --git a/Box2D/Source/Collision/Shapes/b2Shape.h b/Box2D/Source/Collision/Shapes/b2Shape.h
new file mode 100644 (file)
index 0000000..a55a0e4
--- /dev/null
@@ -0,0 +1,285 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/b2BroadPhase.cpp b/Box2D/Source/Collision/b2BroadPhase.cpp
new file mode 100644 (file)
index 0000000..007cc10
--- /dev/null
@@ -0,0 +1,668 @@
+/*
+* 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);
+               }
+       }
+}
diff --git a/Box2D/Source/Collision/b2BroadPhase.h b/Box2D/Source/Collision/b2BroadPhase.h
new file mode 100644 (file)
index 0000000..0106123
--- /dev/null
@@ -0,0 +1,146 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/b2CollideCircle.cpp b/Box2D/Source/Collision/b2CollideCircle.cpp
new file mode 100644 (file)
index 0000000..2b7030d
--- /dev/null
@@ -0,0 +1,168 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Collision/b2CollidePoly.cpp b/Box2D/Source/Collision/b2CollidePoly.cpp
new file mode 100644 (file)
index 0000000..155ebee
--- /dev/null
@@ -0,0 +1,352 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Collision/b2Collision.cpp b/Box2D/Source/Collision/b2Collision.cpp
new file mode 100644 (file)
index 0000000..884f247
--- /dev/null
@@ -0,0 +1,72 @@
+/*
+* 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;
+}
+
+
diff --git a/Box2D/Source/Collision/b2Collision.h b/Box2D/Source/Collision/b2Collision.h
new file mode 100644 (file)
index 0000000..09d975e
--- /dev/null
@@ -0,0 +1,154 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/b2Distance.cpp b/Box2D/Source/Collision/b2Distance.cpp
new file mode 100644 (file)
index 0000000..e9ba070
--- /dev/null
@@ -0,0 +1,364 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Collision/b2PairManager.cpp b/Box2D/Source/Collision/b2PairManager.cpp
new file mode 100644 (file)
index 0000000..5e1f7e6
--- /dev/null
@@ -0,0 +1,396 @@
+/*
+* 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
+}
diff --git a/Box2D/Source/Collision/b2PairManager.h b/Box2D/Source/Collision/b2PairManager.h
new file mode 100644 (file)
index 0000000..d275814
--- /dev/null
@@ -0,0 +1,121 @@
+/*
+* 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
diff --git a/Box2D/Source/Collision/b2TimeOfImpact.cpp b/Box2D/Source/Collision/b2TimeOfImpact.cpp
new file mode 100644 (file)
index 0000000..0681eda
--- /dev/null
@@ -0,0 +1,112 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Common/Fixed.h b/Box2D/Source/Common/Fixed.h
new file mode 100644 (file)
index 0000000..0e1d980
--- /dev/null
@@ -0,0 +1,477 @@
+/*
+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
diff --git a/Box2D/Source/Common/b2BlockAllocator.cpp b/Box2D/Source/Common/b2BlockAllocator.cpp
new file mode 100644 (file)
index 0000000..56d595a
--- /dev/null
@@ -0,0 +1,206 @@
+/*
+* 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));
+}
diff --git a/Box2D/Source/Common/b2BlockAllocator.h b/Box2D/Source/Common/b2BlockAllocator.h
new file mode 100644 (file)
index 0000000..b1c6265
--- /dev/null
@@ -0,0 +1,59 @@
+/*
+* 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
diff --git a/Box2D/Source/Common/b2Math.cpp b/Box2D/Source/Common/b2Math.cpp
new file mode 100644 (file)
index 0000000..e67195b
--- /dev/null
@@ -0,0 +1,54 @@
+/*
+* 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;
+       }
+}
diff --git a/Box2D/Source/Common/b2Math.h b/Box2D/Source/Common/b2Math.h
new file mode 100644 (file)
index 0000000..a253211
--- /dev/null
@@ -0,0 +1,636 @@
+/*
+* 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
diff --git a/Box2D/Source/Common/b2Settings.cpp b/Box2D/Source/Common/b2Settings.cpp
new file mode 100644 (file)
index 0000000..16ffe63
--- /dev/null
@@ -0,0 +1,51 @@
+/*
+* 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);
+}
diff --git a/Box2D/Source/Common/b2Settings.h b/Box2D/Source/Common/b2Settings.h
new file mode 100644 (file)
index 0000000..0c75823
--- /dev/null
@@ -0,0 +1,176 @@
+/*
+* 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
diff --git a/Box2D/Source/Common/b2StackAllocator.cpp b/Box2D/Source/Common/b2StackAllocator.cpp
new file mode 100644 (file)
index 0000000..aaa5e78
--- /dev/null
@@ -0,0 +1,83 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Common/b2StackAllocator.h b/Box2D/Source/Common/b2StackAllocator.h
new file mode 100644 (file)
index 0000000..bcdb8e5
--- /dev/null
@@ -0,0 +1,60 @@
+/*
+* 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
diff --git a/Box2D/Source/Common/jtypes.h b/Box2D/Source/Common/jtypes.h
new file mode 100644 (file)
index 0000000..9f22454
--- /dev/null
@@ -0,0 +1,139 @@
+/*---------------------------------------------------------------------------------
+       $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
+//---------------------------------------------------------------------------------
diff --git a/Box2D/Source/Dynamics/Contacts/b2CircleContact.cpp b/Box2D/Source/Dynamics/Contacts/b2CircleContact.cpp
new file mode 100644 (file)
index 0000000..af25138
--- /dev/null
@@ -0,0 +1,122 @@
+/*
+* 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);
+               }
+       }
+}
diff --git a/Box2D/Source/Dynamics/Contacts/b2CircleContact.h b/Box2D/Source/Dynamics/Contacts/b2CircleContact.h
new file mode 100644 (file)
index 0000000..647347b
--- /dev/null
@@ -0,0 +1,46 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Contacts/b2Contact.cpp b/Box2D/Source/Dynamics/Contacts/b2Contact.cpp
new file mode 100644 (file)
index 0000000..addbeb0
--- /dev/null
@@ -0,0 +1,172 @@
+/*
+* 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;
+       }
+}
diff --git a/Box2D/Source/Dynamics/Contacts/b2Contact.h b/Box2D/Source/Dynamics/Contacts/b2Contact.h
new file mode 100644 (file)
index 0000000..25cee8e
--- /dev/null
@@ -0,0 +1,183 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Contacts/b2ContactSolver.cpp b/Box2D/Source/Dynamics/Contacts/b2ContactSolver.cpp
new file mode 100644 (file)
index 0000000..e481546
--- /dev/null
@@ -0,0 +1,360 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Dynamics/Contacts/b2ContactSolver.h b/Box2D/Source/Dynamics/Contacts/b2ContactSolver.h
new file mode 100644 (file)
index 0000000..6e8b3d1
--- /dev/null
@@ -0,0 +1,77 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Contacts/b2NullContact.h b/Box2D/Source/Dynamics/Contacts/b2NullContact.h
new file mode 100644 (file)
index 0000000..f5b2d1d
--- /dev/null
@@ -0,0 +1,33 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.cpp b/Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.cpp
new file mode 100644 (file)
index 0000000..6d9287d
--- /dev/null
@@ -0,0 +1,158 @@
+/*
+* 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);
+       }
+}
diff --git a/Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.h b/Box2D/Source/Dynamics/Contacts/b2PolyAndCircleContact.h
new file mode 100644 (file)
index 0000000..120abdc
--- /dev/null
@@ -0,0 +1,44 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Contacts/b2PolyContact.cpp b/Box2D/Source/Dynamics/Contacts/b2PolyContact.cpp
new file mode 100644 (file)
index 0000000..b66899d
--- /dev/null
@@ -0,0 +1,157 @@
+/*
+* 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);
+       }
+}
diff --git a/Box2D/Source/Dynamics/Contacts/b2PolyContact.h b/Box2D/Source/Dynamics/Contacts/b2PolyContact.h
new file mode 100644 (file)
index 0000000..a2cddf1
--- /dev/null
@@ -0,0 +1,44 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Joints/b2DistanceJoint.cpp b/Box2D/Source/Dynamics/Joints/b2DistanceJoint.cpp
new file mode 100644 (file)
index 0000000..a164ab7
--- /dev/null
@@ -0,0 +1,206 @@
+/*
+* 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;
+}
diff --git a/Box2D/Source/Dynamics/Joints/b2DistanceJoint.h b/Box2D/Source/Dynamics/Joints/b2DistanceJoint.h
new file mode 100644 (file)
index 0000000..3d2536b
--- /dev/null
@@ -0,0 +1,96 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Joints/b2GearJoint.cpp b/Box2D/Source/Dynamics/Joints/b2GearJoint.cpp
new file mode 100644 (file)
index 0000000..cb92889
--- /dev/null
@@ -0,0 +1,253 @@
+/*
+* 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;
+}
+
+
diff --git a/Box2D/Source/Dynamics/Joints/b2GearJoint.h b/Box2D/Source/Dynamics/Joints/b2GearJoint.h
new file mode 100644 (file)
index 0000000..5c8e48d
--- /dev/null
@@ -0,0 +1,109 @@
+/*
+* 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
diff --git a/Box2D/Source/Dynamics/Joints/b2Joint.cpp b/Box2D/Source/Dynamics/Joints/b2Joint.cpp
new file mode 100644 (file)
index 0000000..867ba8e
--- /dev/null
@@ -0,0 +1,134 @@
+/*
+* 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 &q