Starting box2d update

CMakeLists.txt

@@ -1068,126 +1068,77 @@ set(LOVE_SRC_3P_BOX2D_ROOT
 )
 
 set(LOVE_SRC_3P_BOX2D_COLLISION
-	src/libraries/Box2D/Collision/b2BroadPhase.cpp
-	src/libraries/Box2D/Collision/b2BroadPhase.h
-	src/libraries/Box2D/Collision/b2CollideCircle.cpp
-	src/libraries/Box2D/Collision/b2CollideEdge.cpp
-	src/libraries/Box2D/Collision/b2CollidePolygon.cpp
-	src/libraries/Box2D/Collision/b2Collision.cpp
-	src/libraries/Box2D/Collision/b2Collision.h
-	src/libraries/Box2D/Collision/b2Distance.cpp
-	src/libraries/Box2D/Collision/b2Distance.h
-	src/libraries/Box2D/Collision/b2DynamicTree.cpp
-	src/libraries/Box2D/Collision/b2DynamicTree.h
-	src/libraries/Box2D/Collision/b2TimeOfImpact.cpp
-	src/libraries/Box2D/Collision/b2TimeOfImpact.h
-)
-
-set(LOVE_SRC_3P_BOX2D_COLLISION_SHAPES
-	src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp
-	src/libraries/Box2D/Collision/Shapes/b2ChainShape.h
-	src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp
-	src/libraries/Box2D/Collision/Shapes/b2CircleShape.h
-	src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp
-	src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h
-	src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp
-	src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h
-	src/libraries/Box2D/Collision/Shapes/b2Shape.h
+    src/libraries/Box2D/collision/b2_broad_phase.cpp
+    src/libraries/Box2D/collision/b2_chain_shape.cpp
+    src/libraries/Box2D/collision/b2_circle_shape.cpp
+    src/libraries/Box2D/collision/b2_collide_circle.cpp
+    src/libraries/Box2D/collision/b2_collide_edge.cpp
+    src/libraries/Box2D/collision/b2_collide_polygon.cpp
+    src/libraries/Box2D/collision/b2_collision.cpp
+    src/libraries/Box2D/collision/b2_distance.cpp
+    src/libraries/Box2D/collision/b2_dynamic_tree.cpp
+    src/libraries/Box2D/collision/b2_edge_shape.cpp
+    src/libraries/Box2D/collision/b2_polygon_shape.cpp
+    src/libraries/Box2D/collision/b2_time_of_impact.cpp
 )
 
 set(LOVE_SRC_3P_BOX2D_COMMON
-	src/libraries/Box2D/Common/b2BlockAllocator.cpp
-	src/libraries/Box2D/Common/b2BlockAllocator.h
-	src/libraries/Box2D/Common/b2Draw.cpp
-	src/libraries/Box2D/Common/b2Draw.h
-	src/libraries/Box2D/Common/b2GrowableStack.h
-	src/libraries/Box2D/Common/b2Math.cpp
-	src/libraries/Box2D/Common/b2Math.h
-	src/libraries/Box2D/Common/b2Settings.cpp
-	src/libraries/Box2D/Common/b2Settings.h
-	src/libraries/Box2D/Common/b2StackAllocator.cpp
-	src/libraries/Box2D/Common/b2StackAllocator.h
-	src/libraries/Box2D/Common/b2Timer.cpp
-	src/libraries/Box2D/Common/b2Timer.h
+    src/libraries/Box2D/common/b2_block_allocator.cpp
+    src/libraries/Box2D/common/b2_draw.cpp
+    src/libraries/Box2D/common/b2_math.cpp
+    src/libraries/Box2D/common/b2_settings.cpp
+    src/libraries/Box2D/common/b2_stack_allocator.cpp
+    src/libraries/Box2D/common/b2_timer.cpp
 )
 
 set(LOVE_SRC_3P_BOX2D_DYNAMICS
-	src/libraries/Box2D/Dynamics/b2Body.cpp
-	src/libraries/Box2D/Dynamics/b2Body.h
-	src/libraries/Box2D/Dynamics/b2ContactManager.cpp
-	src/libraries/Box2D/Dynamics/b2ContactManager.h
-	src/libraries/Box2D/Dynamics/b2Fixture.cpp
-	src/libraries/Box2D/Dynamics/b2Fixture.h
-	src/libraries/Box2D/Dynamics/b2Island.cpp
-	src/libraries/Box2D/Dynamics/b2Island.h
-	src/libraries/Box2D/Dynamics/b2TimeStep.h
-	src/libraries/Box2D/Dynamics/b2World.cpp
-	src/libraries/Box2D/Dynamics/b2World.h
-	src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp
-	src/libraries/Box2D/Dynamics/b2WorldCallbacks.h
-)
-
-set(LOVE_SRC_3P_BOX2D_DYNAMICS_CONTACTS
-	src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2Contact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h
-	src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h
-	src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp
-	src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h
-)
-
-set(LOVE_SRC_3P_BOX2D_DYNAMICS_JOINTS
-	src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2Joint.h
-	src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h
-	src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp
-	src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h
+    src/libraries/Box2D/dynamics/b2_body.cpp
+    src/libraries/Box2D/dynamics/b2_chain_circle_contact.cpp
+    src/libraries/Box2D/dynamics/b2_chain_circle_contact.h
+    src/libraries/Box2D/dynamics/b2_chain_polygon_contact.cpp
+    src/libraries/Box2D/dynamics/b2_chain_polygon_contact.h
+    src/libraries/Box2D/dynamics/b2_circle_contact.cpp
+    src/libraries/Box2D/dynamics/b2_circle_contact.h
+    src/libraries/Box2D/dynamics/b2_contact.cpp
+    src/libraries/Box2D/dynamics/b2_contact_manager.cpp
+    src/libraries/Box2D/dynamics/b2_contact_solver.cpp
+    src/libraries/Box2D/dynamics/b2_contact_solver.h
+    src/libraries/Box2D/dynamics/b2_distance_joint.cpp
+    src/libraries/Box2D/dynamics/b2_edge_circle_contact.cpp
+    src/libraries/Box2D/dynamics/b2_edge_circle_contact.h
+    src/libraries/Box2D/dynamics/b2_edge_polygon_contact.cpp
+    src/libraries/Box2D/dynamics/b2_edge_polygon_contact.h
+    src/libraries/Box2D/dynamics/b2_fixture.cpp
+    src/libraries/Box2D/dynamics/b2_friction_joint.cpp
+    src/libraries/Box2D/dynamics/b2_gear_joint.cpp
+    src/libraries/Box2D/dynamics/b2_island.cpp
+    src/libraries/Box2D/dynamics/b2_island.h
+    src/libraries/Box2D/dynamics/b2_joint.cpp
+    src/libraries/Box2D/dynamics/b2_motor_joint.cpp
+    src/libraries/Box2D/dynamics/b2_mouse_joint.cpp
+    src/libraries/Box2D/dynamics/b2_polygon_circle_contact.cpp
+    src/libraries/Box2D/dynamics/b2_polygon_circle_contact.h
+    src/libraries/Box2D/dynamics/b2_polygon_contact.cpp
+    src/libraries/Box2D/dynamics/b2_polygon_contact.h
+    src/libraries/Box2D/dynamics/b2_prismatic_joint.cpp
+    src/libraries/Box2D/dynamics/b2_pulley_joint.cpp
+    src/libraries/Box2D/dynamics/b2_revolute_joint.cpp
+    src/libraries/Box2D/dynamics/b2_rope_joint.cpp
+    src/libraries/Box2D/dynamics/b2_weld_joint.cpp
+    src/libraries/Box2D/dynamics/b2_wheel_joint.cpp
+    src/libraries/Box2D/dynamics/b2_world.cpp
+    src/libraries/Box2D/dynamics/b2_world_callbacks.cpp
 )
 
 set(LOVE_SRC_3P_BOX2D_ROPE
-	src/libraries/Box2D/Rope/b2Rope.cpp
-	src/libraries/Box2D/Rope/b2Rope.h
+	src/libraries/Box2D/rope/b2_rope.cpp
 )
 
 set(LOVE_SRC_3P_BOX2D
 	${LOVE_SRC_3P_BOX2D_ROOT}
 	${LOVE_SRC_3P_BOX2D_COLLISION}
-	${LOVE_SRC_3P_BOX2D_COLLISION_SHAPES}
 	${LOVE_SRC_3P_BOX2D_COMMON}
 	${LOVE_SRC_3P_BOX2D_DYNAMICS}
-	${LOVE_SRC_3P_BOX2D_DYNAMICS_CONTACTS}
-	${LOVE_SRC_3P_BOX2D_DYNAMICS_JOINTS}
 	${LOVE_SRC_3P_BOX2D_ROPE}
 )
 
@@ -1726,6 +1677,7 @@ set(LOVE_LIB_SRC
 include_directories(
 	src
 	src/libraries
+	src/libraries/Box2D
 	src/modules
 	${LOVE_INCLUDE_DIRS}
 )

src/libraries/Box2D/Box2D.h

@@ -1,68 +1,59 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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.
-*/
+// MIT License
 
-#ifndef BOX2D_H
-#define BOX2D_H
+// Copyright (c) 2019 Erin Catto
 
-/**
-\mainpage Box2D API Documentation
+// 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:
 
-\section intro_sec Getting Started
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
 
-For documentation please see http://box2d.org/documentation.html
+// 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.
 
-For discussion please visit http://box2d.org/forum
-*/
+#ifndef BOX2D_H
+#define BOX2D_H
 
 // These include files constitute the main Box2D API
 
-#include <Box2D/Common/b2Settings.h>
-#include <Box2D/Common/b2Draw.h>
-#include <Box2D/Common/b2Timer.h>
-
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
-
-#include <Box2D/Collision/b2BroadPhase.h>
-#include <Box2D/Collision/b2Distance.h>
-#include <Box2D/Collision/b2DynamicTree.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-#include <Box2D/Dynamics/b2World.h>
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
-#include <Box2D/Dynamics/Joints/b2GearJoint.h>
-#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
-#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
-#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
-#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
-#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
-#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
-#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
+#include "b2_settings.h"
+#include "b2_draw.h"
+#include "b2_timer.h"
+
+#include "b2_chain_shape.h"
+#include "b2_circle_shape.h"
+#include "b2_edge_shape.h"
+#include "b2_polygon_shape.h"
+
+#include "b2_broad_phase.h"
+#include "b2_dynamic_tree.h"
+
+#include "b2_body.h"
+#include "b2_contact.h"
+#include "b2_fixture.h"
+#include "b2_time_step.h"
+#include "b2_world.h"
+#include "b2_world_callbacks.h"
+
+#include "b2_distance_joint.h"
+#include "b2_friction_joint.h"
+#include "b2_gear_joint.h"
+#include "b2_motor_joint.h"
+#include "b2_mouse_joint.h"
+#include "b2_prismatic_joint.h"
+#include "b2_pulley_joint.h"
+#include "b2_revolute_joint.h"
+#include "b2_rope_joint.h"
+#include "b2_weld_joint.h"
+#include "b2_wheel_joint.h"
 
 #endif

src/libraries/Box2D/Collision/Shapes/b2ChainShape.h

@@ -1,105 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_CHAIN_SHAPE_H
-#define B2_CHAIN_SHAPE_H
-
-#include <Box2D/Collision/Shapes/b2Shape.h>
-
-class b2EdgeShape;
-
-/// A chain shape is a free form sequence of line segments.
-/// The chain has two-sided collision, so you can use inside and outside collision.
-/// Therefore, you may use any winding order.
-/// Since there may be many vertices, they are allocated using b2Alloc.
-/// Connectivity information is used to create smooth collisions.
-/// WARNING: The chain will not collide properly if there are self-intersections.
-class b2ChainShape : public b2Shape
-{
-public:
-	b2ChainShape();
-
-	/// The destructor frees the vertices using b2Free.
-	~b2ChainShape();
-
-	/// Clear all data.
-	void Clear();
-
-	/// Create a loop. This automatically adjusts connectivity.
-	/// @param vertices an array of vertices, these are copied
-	/// @param count the vertex count
-	void CreateLoop(const b2Vec2* vertices, int32 count);
-
-	/// Create a chain with isolated end vertices.
-	/// @param vertices an array of vertices, these are copied
-	/// @param count the vertex count
-	void CreateChain(const b2Vec2* vertices, int32 count);
-
-	/// Establish connectivity to a vertex that precedes the first vertex.
-	/// Don't call this for loops.
-	void SetPrevVertex(const b2Vec2& prevVertex);
-
-	/// Establish connectivity to a vertex that follows the last vertex.
-	/// Don't call this for loops.
-	void SetNextVertex(const b2Vec2& nextVertex);
-
-	/// Implement b2Shape. Vertices are cloned using b2Alloc.
-	b2Shape* Clone(b2BlockAllocator* allocator) const;
-
-	/// @see b2Shape::GetChildCount
-	int32 GetChildCount() const;
-
-	/// Get a child edge.
-	void GetChildEdge(b2EdgeShape* edge, int32 index) const;
-
-	/// This always return false.
-	/// @see b2Shape::TestPoint
-	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
-
-	/// Implement b2Shape.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
-					const b2Transform& transform, int32 childIndex) const;
-
-	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
-
-	/// Chains have zero mass.
-	/// @see b2Shape::ComputeMass
-	void ComputeMass(b2MassData* massData, float32 density) const;
-
-	/// The vertices. Owned by this class.
-	b2Vec2* m_vertices;
-
-	/// The vertex count.
-	int32 m_count;
-
-	b2Vec2 m_prevVertex, m_nextVertex;
-	bool m_hasPrevVertex, m_hasNextVertex;
-};
-
-inline b2ChainShape::b2ChainShape()
-{
-	m_type = e_chain;
-	m_radius = b2_polygonRadius;
-	m_vertices = NULL;
-	m_count = 0;
-	m_hasPrevVertex = false;
-	m_hasNextVertex = false;
-}
-
-#endif

src/libraries/Box2D/Collision/Shapes/b2CircleShape.h

@@ -1,91 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/Shapes/b2Shape.h>
-
-/// A circle shape.
-class b2CircleShape : public b2Shape
-{
-public:
-	b2CircleShape();
-
-	/// Implement b2Shape.
-	b2Shape* Clone(b2BlockAllocator* allocator) const;
-
-	/// @see b2Shape::GetChildCount
-	int32 GetChildCount() const;
-
-	/// Implement b2Shape.
-	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
-
-	/// Implement b2Shape.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
-				const b2Transform& transform, int32 childIndex) const;
-
-	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
-
-	/// @see b2Shape::ComputeMass
-	void ComputeMass(b2MassData* massData, float32 density) const;
-
-	/// Get the supporting vertex index in the given direction.
-	int32 GetSupport(const b2Vec2& d) const;
-
-	/// Get the supporting vertex in the given direction.
-	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
-
-	/// Get the vertex count.
-	int32 GetVertexCount() const { return 1; }
-
-	/// Get a vertex by index. Used by b2Distance.
-	const b2Vec2& GetVertex(int32 index) const;
-
-	/// Position
-	b2Vec2 m_p;
-};
-
-inline b2CircleShape::b2CircleShape()
-{
-	m_type = e_circle;
-	m_radius = 0.0f;
-	m_p.SetZero();
-}
-
-inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const
-{
-	B2_NOT_USED(d);
-	return 0;
-}
-
-inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const
-{
-	B2_NOT_USED(d);
-	return m_p;
-}
-
-inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const
-{
-	B2_NOT_USED(index);
-	b2Assert(index == 0);
-	return m_p;
-}
-
-#endif

src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h

@@ -1,74 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_EDGE_SHAPE_H
-#define B2_EDGE_SHAPE_H
-
-#include <Box2D/Collision/Shapes/b2Shape.h>
-
-/// A line segment (edge) shape. These can be connected in chains or loops
-/// to other edge shapes. The connectivity information is used to ensure
-/// correct contact normals.
-class b2EdgeShape : public b2Shape
-{
-public:
-	b2EdgeShape();
-
-	/// Set this as an isolated edge.
-	void Set(const b2Vec2& v1, const b2Vec2& v2);
-
-	/// Implement b2Shape.
-	b2Shape* Clone(b2BlockAllocator* allocator) const;
-
-	/// @see b2Shape::GetChildCount
-	int32 GetChildCount() const;
-
-	/// @see b2Shape::TestPoint
-	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
-
-	/// Implement b2Shape.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
-				const b2Transform& transform, int32 childIndex) const;
-
-	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
-
-	/// @see b2Shape::ComputeMass
-	void ComputeMass(b2MassData* massData, float32 density) const;
-	
-	/// These are the edge vertices
-	b2Vec2 m_vertex1, m_vertex2;
-
-	/// Optional adjacent vertices. These are used for smooth collision.
-	b2Vec2 m_vertex0, m_vertex3;
-	bool m_hasVertex0, m_hasVertex3;
-};
-
-inline b2EdgeShape::b2EdgeShape()
-{
-	m_type = e_edge;
-	m_radius = b2_polygonRadius;
-	m_vertex0.x = 0.0f;
-	m_vertex0.y = 0.0f;
-	m_vertex3.x = 0.0f;
-	m_vertex3.y = 0.0f;
-	m_hasVertex0 = false;
-	m_hasVertex3 = false;
-}
-
-#endif

src/libraries/Box2D/Collision/b2CollideEdge.cpp

@@ -1,698 +0,0 @@
-/*
- * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
- *
- * This software is provided 'as-is', without any express or implied
- * warranty.  In no event will the authors be held liable for any damages
- * arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it
- * freely, subject to the following restrictions:
- * 1. The origin of this software must not be misrepresented; you must not
- * claim that you wrote the original software. If you use this software
- * in a product, an acknowledgment in the product documentation would be
- * appreciated but is not required.
- * 2. Altered source versions must be plainly marked as such, and must not be
- * misrepresented as being the original software.
- * 3. This notice may not be removed or altered from any source distribution.
- */
-
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
-
-
-// Compute contact points for edge versus circle.
-// This accounts for edge connectivity.
-void b2CollideEdgeAndCircle(b2Manifold* manifold,
-							const b2EdgeShape* edgeA, const b2Transform& xfA,
-							const b2CircleShape* circleB, const b2Transform& xfB)
-{
-	manifold->pointCount = 0;
-	
-	// Compute circle in frame of edge
-	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
-	
-	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
-	b2Vec2 e = B - A;
-	
-	// Barycentric coordinates
-	float32 u = b2Dot(e, B - Q);
-	float32 v = b2Dot(e, Q - A);
-	
-	float32 radius = edgeA->m_radius + circleB->m_radius;
-	
-	b2ContactFeature cf;
-	cf.indexB = 0;
-	cf.typeB = b2ContactFeature::e_vertex;
-	
-	// Region A
-	if (v <= 0.0f)
-	{
-		b2Vec2 P = A;
-		b2Vec2 d = Q - P;
-		float32 dd = b2Dot(d, d);
-		if (dd > radius * radius)
-		{
-			return;
-		}
-		
-		// Is there an edge connected to A?
-		if (edgeA->m_hasVertex0)
-		{
-			b2Vec2 A1 = edgeA->m_vertex0;
-			b2Vec2 B1 = A;
-			b2Vec2 e1 = B1 - A1;
-			float32 u1 = b2Dot(e1, B1 - Q);
-			
-			// Is the circle in Region AB of the previous edge?
-			if (u1 > 0.0f)
-			{
-				return;
-			}
-		}
-		
-		cf.indexA = 0;
-		cf.typeA = b2ContactFeature::e_vertex;
-		manifold->pointCount = 1;
-		manifold->type = b2Manifold::e_circles;
-		manifold->localNormal.SetZero();
-		manifold->localPoint = P;
-		manifold->points[0].id.key = 0;
-		manifold->points[0].id.cf = cf;
-		manifold->points[0].localPoint = circleB->m_p;
-		return;
-	}
-	
-	// Region B
-	if (u <= 0.0f)
-	{
-		b2Vec2 P = B;
-		b2Vec2 d = Q - P;
-		float32 dd = b2Dot(d, d);
-		if (dd > radius * radius)
-		{
-			return;
-		}
-		
-		// Is there an edge connected to B?
-		if (edgeA->m_hasVertex3)
-		{
-			b2Vec2 B2 = edgeA->m_vertex3;
-			b2Vec2 A2 = B;
-			b2Vec2 e2 = B2 - A2;
-			float32 v2 = b2Dot(e2, Q - A2);
-			
-			// Is the circle in Region AB of the next edge?
-			if (v2 > 0.0f)
-			{
-				return;
-			}
-		}
-		
-		cf.indexA = 1;
-		cf.typeA = b2ContactFeature::e_vertex;
-		manifold->pointCount = 1;
-		manifold->type = b2Manifold::e_circles;
-		manifold->localNormal.SetZero();
-		manifold->localPoint = P;
-		manifold->points[0].id.key = 0;
-		manifold->points[0].id.cf = cf;
-		manifold->points[0].localPoint = circleB->m_p;
-		return;
-	}
-	
-	// Region AB
-	float32 den = b2Dot(e, e);
-	b2Assert(den > 0.0f);
-	b2Vec2 P = (1.0f / den) * (u * A + v * B);
-	b2Vec2 d = Q - P;
-	float32 dd = b2Dot(d, d);
-	if (dd > radius * radius)
-	{
-		return;
-	}
-	
-	b2Vec2 n(-e.y, e.x);
-	if (b2Dot(n, Q - A) < 0.0f)
-	{
-		n.Set(-n.x, -n.y);
-	}
-	n.Normalize();
-	
-	cf.indexA = 0;
-	cf.typeA = b2ContactFeature::e_face;
-	manifold->pointCount = 1;
-	manifold->type = b2Manifold::e_faceA;
-	manifold->localNormal = n;
-	manifold->localPoint = A;
-	manifold->points[0].id.key = 0;
-	manifold->points[0].id.cf = cf;
-	manifold->points[0].localPoint = circleB->m_p;
-}
-
-// This structure is used to keep track of the best separating axis.
-struct b2EPAxis
-{
-	enum Type
-	{
-		e_unknown,
-		e_edgeA,
-		e_edgeB
-	};
-	
-	Type type;
-	int32 index;
-	float32 separation;
-};
-
-// This holds polygon B expressed in frame A.
-struct b2TempPolygon
-{
-	b2Vec2 vertices[b2_maxPolygonVertices];
-	b2Vec2 normals[b2_maxPolygonVertices];
-	int32 count;
-};
-
-// Reference face used for clipping
-struct b2ReferenceFace
-{
-	int32 i1, i2;
-	
-	b2Vec2 v1, v2;
-	
-	b2Vec2 normal;
-	
-	b2Vec2 sideNormal1;
-	float32 sideOffset1;
-	
-	b2Vec2 sideNormal2;
-	float32 sideOffset2;
-};
-
-// This class collides and edge and a polygon, taking into account edge adjacency.
-struct b2EPCollider
-{
-	void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
-				 const b2PolygonShape* polygonB, const b2Transform& xfB);
-	b2EPAxis ComputeEdgeSeparation();
-	b2EPAxis ComputePolygonSeparation();
-	
-	enum VertexType
-	{
-		e_isolated,
-		e_concave,
-		e_convex
-	};
-	
-	b2TempPolygon m_polygonB;
-	
-	b2Transform m_xf;
-	b2Vec2 m_centroidB;
-	b2Vec2 m_v0, m_v1, m_v2, m_v3;
-	b2Vec2 m_normal0, m_normal1, m_normal2;
-	b2Vec2 m_normal;
-	VertexType m_type1, m_type2;
-	b2Vec2 m_lowerLimit, m_upperLimit;
-	float32 m_radius;
-	bool m_front;
-};
-
-// Algorithm:
-// 1. Classify v1 and v2
-// 2. Classify polygon centroid as front or back
-// 3. Flip normal if necessary
-// 4. Initialize normal range to [-pi, pi] about face normal
-// 5. Adjust normal range according to adjacent edges
-// 6. Visit each separating axes, only accept axes within the range
-// 7. Return if _any_ axis indicates separation
-// 8. Clip
-void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
-						   const b2PolygonShape* polygonB, const b2Transform& xfB)
-{
-	m_xf = b2MulT(xfA, xfB);
-	
-	m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
-	
-	m_v0 = edgeA->m_vertex0;
-	m_v1 = edgeA->m_vertex1;
-	m_v2 = edgeA->m_vertex2;
-	m_v3 = edgeA->m_vertex3;
-	
-	bool hasVertex0 = edgeA->m_hasVertex0;
-	bool hasVertex3 = edgeA->m_hasVertex3;
-	
-	b2Vec2 edge1 = m_v2 - m_v1;
-	edge1.Normalize();
-	m_normal1.Set(edge1.y, -edge1.x);
-	float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
-	float32 offset0 = 0.0f, offset2 = 0.0f;
-	bool convex1 = false, convex2 = false;
-	
-	// Is there a preceding edge?
-	if (hasVertex0)
-	{
-		b2Vec2 edge0 = m_v1 - m_v0;
-		edge0.Normalize();
-		m_normal0.Set(edge0.y, -edge0.x);
-		convex1 = b2Cross(edge0, edge1) >= 0.0f;
-		offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
-	}
-	
-	// Is there a following edge?
-	if (hasVertex3)
-	{
-		b2Vec2 edge2 = m_v3 - m_v2;
-		edge2.Normalize();
-		m_normal2.Set(edge2.y, -edge2.x);
-		convex2 = b2Cross(edge1, edge2) > 0.0f;
-		offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
-	}
-	
-	// Determine front or back collision. Determine collision normal limits.
-	if (hasVertex0 && hasVertex3)
-	{
-		if (convex1 && convex2)
-		{
-			m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal0;
-				m_upperLimit = m_normal2;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal1;
-				m_upperLimit = -m_normal1;
-			}
-		}
-		else if (convex1)
-		{
-			m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal0;
-				m_upperLimit = m_normal1;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal2;
-				m_upperLimit = -m_normal1;
-			}
-		}
-		else if (convex2)
-		{
-			m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal1;
-				m_upperLimit = m_normal2;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal1;
-				m_upperLimit = -m_normal0;
-			}
-		}
-		else
-		{
-			m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal1;
-				m_upperLimit = m_normal1;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal2;
-				m_upperLimit = -m_normal0;
-			}
-		}
-	}
-	else if (hasVertex0)
-	{
-		if (convex1)
-		{
-			m_front = offset0 >= 0.0f || offset1 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal0;
-				m_upperLimit = -m_normal1;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = m_normal1;
-				m_upperLimit = -m_normal1;
-			}
-		}
-		else
-		{
-			m_front = offset0 >= 0.0f && offset1 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = m_normal1;
-				m_upperLimit = -m_normal1;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = m_normal1;
-				m_upperLimit = -m_normal0;
-			}
-		}
-	}
-	else if (hasVertex3)
-	{
-		if (convex2)
-		{
-			m_front = offset1 >= 0.0f || offset2 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = -m_normal1;
-				m_upperLimit = m_normal2;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal1;
-				m_upperLimit = m_normal1;
-			}
-		}
-		else
-		{
-			m_front = offset1 >= 0.0f && offset2 >= 0.0f;
-			if (m_front)
-			{
-				m_normal = m_normal1;
-				m_lowerLimit = -m_normal1;
-				m_upperLimit = m_normal1;
-			}
-			else
-			{
-				m_normal = -m_normal1;
-				m_lowerLimit = -m_normal2;
-				m_upperLimit = m_normal1;
-			}
-		}		
-	}
-	else
-	{
-		m_front = offset1 >= 0.0f;
-		if (m_front)
-		{
-			m_normal = m_normal1;
-			m_lowerLimit = -m_normal1;
-			m_upperLimit = -m_normal1;
-		}
-		else
-		{
-			m_normal = -m_normal1;
-			m_lowerLimit = m_normal1;
-			m_upperLimit = m_normal1;
-		}
-	}
-	
-	// Get polygonB in frameA
-	m_polygonB.count = polygonB->m_count;
-	for (int32 i = 0; i < polygonB->m_count; ++i)
-	{
-		m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
-		m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
-	}
-	
-	m_radius = 2.0f * b2_polygonRadius;
-	
-	manifold->pointCount = 0;
-	
-	b2EPAxis edgeAxis = ComputeEdgeSeparation();
-	
-	// If no valid normal can be found than this edge should not collide.
-	if (edgeAxis.type == b2EPAxis::e_unknown)
-	{
-		return;
-	}
-	
-	if (edgeAxis.separation > m_radius)
-	{
-		return;
-	}
-	
-	b2EPAxis polygonAxis = ComputePolygonSeparation();
-	if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
-	{
-		return;
-	}
-	
-	// Use hysteresis for jitter reduction.
-	const float32 k_relativeTol = 0.98f;
-	const float32 k_absoluteTol = 0.001f;
-	
-	b2EPAxis primaryAxis;
-	if (polygonAxis.type == b2EPAxis::e_unknown)
-	{
-		primaryAxis = edgeAxis;
-	}
-	else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
-	{
-		primaryAxis = polygonAxis;
-	}
-	else
-	{
-		primaryAxis = edgeAxis;
-	}
-	
-	b2ClipVertex ie[2];
-	b2ReferenceFace rf;
-	if (primaryAxis.type == b2EPAxis::e_edgeA)
-	{
-		manifold->type = b2Manifold::e_faceA;
-		
-		// Search for the polygon normal that is most anti-parallel to the edge normal.
-		int32 bestIndex = 0;
-		float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
-		for (int32 i = 1; i < m_polygonB.count; ++i)
-		{
-			float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
-			if (value < bestValue)
-			{
-				bestValue = value;
-				bestIndex = i;
-			}
-		}
-		
-		int32 i1 = bestIndex;
-		int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
-		
-		ie[0].v = m_polygonB.vertices[i1];
-		ie[0].id.cf.indexA = 0;
-		ie[0].id.cf.indexB = static_cast<uint8>(i1);
-		ie[0].id.cf.typeA = b2ContactFeature::e_face;
-		ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
-		
-		ie[1].v = m_polygonB.vertices[i2];
-		ie[1].id.cf.indexA = 0;
-		ie[1].id.cf.indexB = static_cast<uint8>(i2);
-		ie[1].id.cf.typeA = b2ContactFeature::e_face;
-		ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
-		
-		if (m_front)
-		{
-			rf.i1 = 0;
-			rf.i2 = 1;
-			rf.v1 = m_v1;
-			rf.v2 = m_v2;
-			rf.normal = m_normal1;
-		}
-		else
-		{
-			rf.i1 = 1;
-			rf.i2 = 0;
-			rf.v1 = m_v2;
-			rf.v2 = m_v1;
-			rf.normal = -m_normal1;
-		}		
-	}
-	else
-	{
-		manifold->type = b2Manifold::e_faceB;
-		
-		ie[0].v = m_v1;
-		ie[0].id.cf.indexA = 0;
-		ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
-		ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
-		ie[0].id.cf.typeB = b2ContactFeature::e_face;
-		
-		ie[1].v = m_v2;
-		ie[1].id.cf.indexA = 0;
-		ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
-		ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
-		ie[1].id.cf.typeB = b2ContactFeature::e_face;
-		
-		rf.i1 = primaryAxis.index;
-		rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
-		rf.v1 = m_polygonB.vertices[rf.i1];
-		rf.v2 = m_polygonB.vertices[rf.i2];
-		rf.normal = m_polygonB.normals[rf.i1];
-	}
-	
-	rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
-	rf.sideNormal2 = -rf.sideNormal1;
-	rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
-	rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
-	
-	// Clip incident edge against extruded edge1 side edges.
-	b2ClipVertex clipPoints1[2];
-	b2ClipVertex clipPoints2[2];
-	int32 np;
-	
-	// Clip to box side 1
-	np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
-	
-	if (np < b2_maxManifoldPoints)
-	{
-		return;
-	}
-	
-	// Clip to negative box side 1
-	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
-	
-	if (np < b2_maxManifoldPoints)
-	{
-		return;
-	}
-	
-	// Now clipPoints2 contains the clipped points.
-	if (primaryAxis.type == b2EPAxis::e_edgeA)
-	{
-		manifold->localNormal = rf.normal;
-		manifold->localPoint = rf.v1;
-	}
-	else
-	{
-		manifold->localNormal = polygonB->m_normals[rf.i1];
-		manifold->localPoint = polygonB->m_vertices[rf.i1];
-	}
-	
-	int32 pointCount = 0;
-	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
-	{
-		float32 separation;
-		
-		separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
-		
-		if (separation <= m_radius)
-		{
-			b2ManifoldPoint* cp = manifold->points + pointCount;
-			
-			if (primaryAxis.type == b2EPAxis::e_edgeA)
-			{
-				cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
-				cp->id = clipPoints2[i].id;
-			}
-			else
-			{
-				cp->localPoint = clipPoints2[i].v;
-				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
-				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
-				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
-				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
-			}
-			
-			++pointCount;
-		}
-	}
-	
-	manifold->pointCount = pointCount;
-}
-
-b2EPAxis b2EPCollider::ComputeEdgeSeparation()
-{
-	b2EPAxis axis;
-	axis.type = b2EPAxis::e_edgeA;
-	axis.index = m_front ? 0 : 1;
-	axis.separation = FLT_MAX;
-	
-	for (int32 i = 0; i < m_polygonB.count; ++i)
-	{
-		float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
-		if (s < axis.separation)
-		{
-			axis.separation = s;
-		}
-	}
-	
-	return axis;
-}
-
-b2EPAxis b2EPCollider::ComputePolygonSeparation()
-{
-	b2EPAxis axis;
-	axis.type = b2EPAxis::e_unknown;
-	axis.index = -1;
-	axis.separation = -FLT_MAX;
-
-	b2Vec2 perp(-m_normal.y, m_normal.x);
-
-	for (int32 i = 0; i < m_polygonB.count; ++i)
-	{
-		b2Vec2 n = -m_polygonB.normals[i];
-		
-		float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
-		float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
-		float32 s = b2Min(s1, s2);
-		
-		if (s > m_radius)
-		{
-			// No collision
-			axis.type = b2EPAxis::e_edgeB;
-			axis.index = i;
-			axis.separation = s;
-			return axis;
-		}
-		
-		// Adjacency
-		if (b2Dot(n, perp) >= 0.0f)
-		{
-			if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
-			{
-				continue;
-			}
-		}
-		else
-		{
-			if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
-			{
-				continue;
-			}
-		}
-		
-		if (s > axis.separation)
-		{
-			axis.type = b2EPAxis::e_edgeB;
-			axis.index = i;
-			axis.separation = s;
-		}
-	}
-	
-	return axis;
-}
-
-void b2CollideEdgeAndPolygon(	b2Manifold* manifold,
-							 const b2EdgeShape* edgeA, const b2Transform& xfA,
-							 const b2PolygonShape* polygonB, const b2Transform& xfB)
-{
-	b2EPCollider collider;
-	collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
-}

src/libraries/Box2D/Collision/b2TimeOfImpact.h

@@ -1,58 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_TIME_OF_IMPACT_H
-#define B2_TIME_OF_IMPACT_H
-
-#include <Box2D/Common/b2Math.h>
-#include <Box2D/Collision/b2Distance.h>
-
-/// Input parameters for b2TimeOfImpact
-struct b2TOIInput
-{
-	b2DistanceProxy proxyA;
-	b2DistanceProxy proxyB;
-	b2Sweep sweepA;
-	b2Sweep sweepB;
-	float32 tMax;		// defines sweep interval [0, tMax]
-};
-
-// Output parameters for b2TimeOfImpact.
-struct b2TOIOutput
-{
-	enum State
-	{
-		e_unknown,
-		e_failed,
-		e_overlapped,
-		e_touching,
-		e_separated
-	};
-
-	State state;
-	float32 t;
-};
-
-/// Compute the upper bound on time before two shapes penetrate. Time is represented as
-/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
-/// non-tunneling collision. If you change the time interval, you should call this function
-/// again.
-/// Note: use b2Distance to compute the contact point and normal at the time of impact.
-void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
-
-#endif

src/libraries/Box2D/Collision/b2BroadPhase.cpp → src/libraries/Box2D/Collision/b2_broad_phase.cpp

@@ -1,22 +1,26 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/b2BroadPhase.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_broad_phase.h"
 #include <string.h>
 
 b2BroadPhase::b2BroadPhase()
@@ -102,11 +106,18 @@ bool b2BroadPhase::QueryCallback(int32 proxyId)
 		return true;
 	}
 
+	const bool moved = m_tree.WasMoved(proxyId);
+	if (moved && proxyId > m_queryProxyId)
+	{
+		// Both proxies are moving. Avoid duplicate pairs.
+		return true;
+	}
+
 	// Grow the pair buffer as needed.
 	if (m_pairCount == m_pairCapacity)
 	{
 		b2Pair* oldBuffer = m_pairBuffer;
-		m_pairCapacity *= 2;
+		m_pairCapacity = m_pairCapacity + (m_pairCapacity >> 1);
 		m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
 		memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
 		b2Free(oldBuffer);

src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp → src/libraries/Box2D/Collision/b2_chain_shape.cpp

@@ -1,23 +1,30 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_edge_shape.h"
+
+#include "box2d/b2_block_allocator.h"
+
 #include <new>
 #include <string.h>
 
@@ -29,14 +36,19 @@ b2ChainShape::~b2ChainShape()
 void b2ChainShape::Clear()
 {
 	b2Free(m_vertices);
-	m_vertices = NULL;
+	m_vertices = nullptr;
 	m_count = 0;
 }
 
 void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
 {
-	b2Assert(m_vertices == NULL && m_count == 0);
+	b2Assert(m_vertices == nullptr && m_count == 0);
 	b2Assert(count >= 3);
+	if (count < 3)
+	{
+		return;
+	}
+
 	for (int32 i = 1; i < count; ++i)
 	{
 		b2Vec2 v1 = vertices[i-1];
@@ -51,13 +63,11 @@ void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
 	m_vertices[count] = m_vertices[0];
 	m_prevVertex = m_vertices[m_count - 2];
 	m_nextVertex = m_vertices[1];
-	m_hasPrevVertex = true;
-	m_hasNextVertex = true;
 }
 
-void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
+void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count,	const b2Vec2& prevVertex, const b2Vec2& nextVertex)
 {
-	b2Assert(m_vertices == NULL && m_count == 0);
+	b2Assert(m_vertices == nullptr && m_count == 0);
 	b2Assert(count >= 2);
 	for (int32 i = 1; i < count; ++i)
 	{
@@ -69,34 +79,15 @@ void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
 	m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
 	memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
 
-	m_hasPrevVertex = false;
-	m_hasNextVertex = false;
-
-	m_prevVertex.SetZero();
-	m_nextVertex.SetZero();
-}
-
-void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
-{
 	m_prevVertex = prevVertex;
-	m_hasPrevVertex = true;
-}
-
-void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
-{
 	m_nextVertex = nextVertex;
-	m_hasNextVertex = true;
 }
 
 b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
 {
 	void* mem = allocator->Allocate(sizeof(b2ChainShape));
 	b2ChainShape* clone = new (mem) b2ChainShape;
-	clone->CreateChain(m_vertices, m_count);
-	clone->m_prevVertex = m_prevVertex;
-	clone->m_nextVertex = m_nextVertex;
-	clone->m_hasPrevVertex = m_hasPrevVertex;
-	clone->m_hasNextVertex = m_hasNextVertex;
+	clone->CreateChain(m_vertices, m_count, m_prevVertex, m_nextVertex);
 	return clone;
 }
 
@@ -114,27 +105,24 @@ void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
 
 	edge->m_vertex1 = m_vertices[index + 0];
 	edge->m_vertex2 = m_vertices[index + 1];
+	edge->m_oneSided = true;
 
 	if (index > 0)
 	{
 		edge->m_vertex0 = m_vertices[index - 1];
-		edge->m_hasVertex0 = true;
 	}
 	else
 	{
 		edge->m_vertex0 = m_prevVertex;
-		edge->m_hasVertex0 = m_hasPrevVertex;
 	}
 
 	if (index < m_count - 2)
 	{
 		edge->m_vertex3 = m_vertices[index + 2];
-		edge->m_hasVertex3 = true;
 	}
 	else
 	{
 		edge->m_vertex3 = m_nextVertex;
-		edge->m_hasVertex3 = m_hasNextVertex;
 	}
 }
 
@@ -179,11 +167,15 @@ void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childI
 	b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
 	b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
 
-	aabb->lowerBound = b2Min(v1, v2);
-	aabb->upperBound = b2Max(v1, v2);
+	b2Vec2 lower = b2Min(v1, v2);
+	b2Vec2 upper = b2Max(v1, v2);
+
+	b2Vec2 r(m_radius, m_radius);
+	aabb->lowerBound = lower - r;
+	aabb->upperBound = upper + r;
 }
 
-void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2ChainShape::ComputeMass(b2MassData* massData, float density) const
 {
 	B2_NOT_USED(density);
 

src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp → src/libraries/Box2D/Collision/b2_circle_shape.cpp

@@ -1,22 +1,28 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/Shapes/b2CircleShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_block_allocator.h"
+
 #include <new>
 
 b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
@@ -50,13 +56,13 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input
 
 	b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
 	b2Vec2 s = input.p1 - position;
-	float32 b = b2Dot(s, s) - m_radius * m_radius;
+	float b = b2Dot(s, s) - m_radius * m_radius;
 
 	// Solve quadratic equation.
 	b2Vec2 r = input.p2 - input.p1;
-	float32 c =  b2Dot(s, r);
-	float32 rr = b2Dot(r, r);
-	float32 sigma = c * c - rr * b;
+	float c =  b2Dot(s, r);
+	float rr = b2Dot(r, r);
+	float sigma = c * c - rr * b;
 
 	// Check for negative discriminant and short segment.
 	if (sigma < 0.0f || rr < b2_epsilon)
@@ -65,7 +71,7 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input
 	}
 
 	// Find the point of intersection of the line with the circle.
-	float32 a = -(c + b2Sqrt(sigma));
+	float a = -(c + b2Sqrt(sigma));
 
 	// Is the intersection point on the segment?
 	if (0.0f <= a && a <= input.maxFraction * rr)
@@ -89,7 +95,7 @@ void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int3
 	aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
 }
 
-void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2CircleShape::ComputeMass(b2MassData* massData, float density) const
 {
 	massData->mass = density * b2_pi * m_radius * m_radius;
 	massData->center = m_p;

src/libraries/Box2D/Collision/b2CollideCircle.cpp → src/libraries/Box2D/Collision/b2_collide_circle.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_polygon_shape.h"
 
 void b2CollideCircles(
 	b2Manifold* manifold,
@@ -31,9 +35,9 @@ void b2CollideCircles(
 	b2Vec2 pB = b2Mul(xfB, circleB->m_p);
 
 	b2Vec2 d = pB - pA;
-	float32 distSqr = b2Dot(d, d);
-	float32 rA = circleA->m_radius, rB = circleB->m_radius;
-	float32 radius = rA + rB;
+	float distSqr = b2Dot(d, d);
+	float rA = circleA->m_radius, rB = circleB->m_radius;
+	float radius = rA + rB;
 	if (distSqr > radius * radius)
 	{
 		return;
@@ -61,15 +65,15 @@ void b2CollidePolygonAndCircle(
 
 	// Find the min separating edge.
 	int32 normalIndex = 0;
-	float32 separation = -b2_maxFloat;
-	float32 radius = polygonA->m_radius + circleB->m_radius;
+	float separation = -b2_maxFloat;
+	float radius = polygonA->m_radius + circleB->m_radius;
 	int32 vertexCount = polygonA->m_count;
 	const b2Vec2* vertices = polygonA->m_vertices;
 	const b2Vec2* normals = polygonA->m_normals;
 
 	for (int32 i = 0; i < vertexCount; ++i)
 	{
-		float32 s = b2Dot(normals[i], cLocal - vertices[i]);
+		float s = b2Dot(normals[i], cLocal - vertices[i]);
 
 		if (s > radius)
 		{
@@ -103,8 +107,8 @@ void b2CollidePolygonAndCircle(
 	}
 
 	// Compute barycentric coordinates
-	float32 u1 = b2Dot(cLocal - v1, v2 - v1);
-	float32 u2 = b2Dot(cLocal - v2, v1 - v2);
+	float u1 = b2Dot(cLocal - v1, v2 - v1);
+	float u2 = b2Dot(cLocal - v2, v1 - v2);
 	if (u1 <= 0.0f)
 	{
 		if (b2DistanceSquared(cLocal, v1) > radius * radius)
@@ -138,8 +142,8 @@ void b2CollidePolygonAndCircle(
 	else
 	{
 		b2Vec2 faceCenter = 0.5f * (v1 + v2);
-		float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
-		if (separation > radius)
+		float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+		if (s > radius)
 		{
 			return;
 		}

src/libraries/Box2D/Collision/b2_collide_edge.cpp

@@ -0,0 +1,524 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_edge_shape.h"
+#include "box2d/b2_polygon_shape.h"
+
+
+// Compute contact points for edge versus circle.
+// This accounts for edge connectivity.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							const b2EdgeShape* edgeA, const b2Transform& xfA,
+							const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	
+	// Compute circle in frame of edge
+	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
+	
+	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
+	b2Vec2 e = B - A;
+	
+	// Normal points to the right for a CCW winding
+	b2Vec2 n(e.y, -e.x);
+	float offset = b2Dot(n, Q - A);
+
+	bool oneSided = edgeA->m_oneSided;
+	if (oneSided && offset < 0.0f)
+	{
+		return;
+	}
+
+	// Barycentric coordinates
+	float u = b2Dot(e, B - Q);
+	float v = b2Dot(e, Q - A);
+	
+	float radius = edgeA->m_radius + circleB->m_radius;
+	
+	b2ContactFeature cf;
+	cf.indexB = 0;
+	cf.typeB = b2ContactFeature::e_vertex;
+	
+	// Region A
+	if (v <= 0.0f)
+	{
+		b2Vec2 P = A;
+		b2Vec2 d = Q - P;
+		float dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to A?
+		if (edgeA->m_oneSided)
+		{
+			b2Vec2 A1 = edgeA->m_vertex0;
+			b2Vec2 B1 = A;
+			b2Vec2 e1 = B1 - A1;
+			float u1 = b2Dot(e1, B1 - Q);
+			
+			// Is the circle in Region AB of the previous edge?
+			if (u1 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 0;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region B
+	if (u <= 0.0f)
+	{
+		b2Vec2 P = B;
+		b2Vec2 d = Q - P;
+		float dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to B?
+		if (edgeA->m_oneSided)
+		{
+			b2Vec2 B2 = edgeA->m_vertex3;
+			b2Vec2 A2 = B;
+			b2Vec2 e2 = B2 - A2;
+			float v2 = b2Dot(e2, Q - A2);
+			
+			// Is the circle in Region AB of the next edge?
+			if (v2 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 1;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region AB
+	float den = b2Dot(e, e);
+	b2Assert(den > 0.0f);
+	b2Vec2 P = (1.0f / den) * (u * A + v * B);
+	b2Vec2 d = Q - P;
+	float dd = b2Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return;
+	}
+	
+	if (offset < 0.0f)
+	{
+		n.Set(-n.x, -n.y);
+	}
+	n.Normalize();
+	
+	cf.indexA = 0;
+	cf.typeA = b2ContactFeature::e_face;
+	manifold->pointCount = 1;
+	manifold->type = b2Manifold::e_faceA;
+	manifold->localNormal = n;
+	manifold->localPoint = A;
+	manifold->points[0].id.key = 0;
+	manifold->points[0].id.cf = cf;
+	manifold->points[0].localPoint = circleB->m_p;
+}
+
+// This structure is used to keep track of the best separating axis.
+struct b2EPAxis
+{
+	enum Type
+	{
+		e_unknown,
+		e_edgeA,
+		e_edgeB
+	};
+	
+	b2Vec2 normal;
+	Type type;
+	int32 index;
+	float separation;
+};
+
+// This holds polygon B expressed in frame A.
+struct b2TempPolygon
+{
+	b2Vec2 vertices[b2_maxPolygonVertices];
+	b2Vec2 normals[b2_maxPolygonVertices];
+	int32 count;
+};
+
+// Reference face used for clipping
+struct b2ReferenceFace
+{
+	int32 i1, i2;
+	b2Vec2 v1, v2;
+	b2Vec2 normal;
+	
+	b2Vec2 sideNormal1;
+	float sideOffset1;
+	
+	b2Vec2 sideNormal2;
+	float sideOffset2;
+};
+
+static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1)
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_edgeA;
+	axis.index = -1;
+	axis.separation = -FLT_MAX;
+	axis.normal.SetZero();
+
+	b2Vec2 axes[2] = { normal1, -normal1 };
+
+	// Find axis with least overlap (min-max problem)
+	for (int32 j = 0; j < 2; ++j)
+	{
+		float sj = FLT_MAX;
+
+		// Find deepest polygon vertex along axis j
+		for (int32 i = 0; i < polygonB.count; ++i)
+		{
+			float si = b2Dot(axes[j], polygonB.vertices[i] - v1);
+			if (si < sj)
+			{
+				sj = si;
+			}
+		}
+
+		if (sj > axis.separation)
+		{
+			axis.index = j;
+			axis.separation = sj;
+			axis.normal = axes[j];
+		}
+	}
+
+	return axis;
+}
+
+static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2)
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_unknown;
+	axis.index = -1;
+	axis.separation = -FLT_MAX;
+	axis.normal.SetZero();
+
+	for (int32 i = 0; i < polygonB.count; ++i)
+	{
+		b2Vec2 n = -polygonB.normals[i];
+
+		float s1 = b2Dot(n, polygonB.vertices[i] - v1);
+		float s2 = b2Dot(n, polygonB.vertices[i] - v2);
+		float s = b2Min(s1, s2);
+
+		if (s > axis.separation)
+		{
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+			axis.normal = n;
+		}
+	}
+
+	return axis;
+}
+
+void b2CollideEdgeAndPolygon(b2Manifold* manifold,
+							const b2EdgeShape* edgeA, const b2Transform& xfA,
+							const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+
+	b2Transform xf = b2MulT(xfA, xfB);
+
+	b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid);
+
+	b2Vec2 v1 = edgeA->m_vertex1;
+	b2Vec2 v2 = edgeA->m_vertex2;
+
+	b2Vec2 edge1 = v2 - v1;
+	edge1.Normalize();
+
+	// Normal points to the right for a CCW winding
+	b2Vec2 normal1(edge1.y, -edge1.x);
+	float offset1 = b2Dot(normal1, centroidB - v1);
+
+	bool oneSided = edgeA->m_oneSided;
+	if (oneSided && offset1 < 0.0f)
+	{
+		return;
+	}
+
+	// Get polygonB in frameA
+	b2TempPolygon tempPolygonB;
+	tempPolygonB.count = polygonB->m_count;
+	for (int32 i = 0; i < polygonB->m_count; ++i)
+	{
+		tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]);
+		tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]);
+	}
+
+	float radius = polygonB->m_radius + edgeA->m_radius;
+
+	b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1);
+	if (edgeAxis.separation > radius)
+	{
+		return;
+	}
+
+	b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2);
+	if (polygonAxis.separation > radius)
+	{
+		return;
+	}
+
+	// Use hysteresis for jitter reduction.
+	const float k_relativeTol = 0.98f;
+	const float k_absoluteTol = 0.001f;
+
+	b2EPAxis primaryAxis;
+	if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol)
+	{
+		primaryAxis = polygonAxis;
+	}
+	else
+	{
+		primaryAxis = edgeAxis;
+	}
+
+	if (oneSided)
+	{
+		// Smooth collision
+		// See https://box2d.org/posts/2020/06/ghost-collisions/
+
+		b2Vec2 edge0 = v1 - edgeA->m_vertex0;
+		edge0.Normalize();
+		b2Vec2 normal0(edge0.y, -edge0.x);
+		bool convex1 = b2Cross(edge0, edge1) >= 0.0f;
+
+		b2Vec2 edge2 = edgeA->m_vertex3 - v2;
+		edge2.Normalize();
+		b2Vec2 normal2(edge2.y, -edge2.x);
+		bool convex2 = b2Cross(edge1, edge2) >= 0.0f;
+
+		const float sinTol = 0.1f;
+		bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f;
+
+		// Check Gauss Map
+		if (side1)
+		{
+			if (convex1)
+			{
+				if (b2Cross(primaryAxis.normal, normal0) > sinTol)
+				{
+					// Skip region
+					return;
+				}
+
+				// Admit region
+			}
+			else
+			{
+				// Snap region
+				primaryAxis = edgeAxis;
+			}
+		}
+		else
+		{
+			if (convex2)
+			{
+				if (b2Cross(normal2, primaryAxis.normal) > sinTol)
+				{
+					// Skip region
+					return;
+				}
+
+				// Admit region
+			}
+			else
+			{
+				// Snap region
+				primaryAxis = edgeAxis;
+			}
+		}
+	}
+
+	b2ClipVertex clipPoints[2];
+	b2ReferenceFace ref;
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->type = b2Manifold::e_faceA;
+
+		// Search for the polygon normal that is most anti-parallel to the edge normal.
+		int32 bestIndex = 0;
+		float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]);
+		for (int32 i = 1; i < tempPolygonB.count; ++i)
+		{
+			float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]);
+			if (value < bestValue)
+			{
+				bestValue = value;
+				bestIndex = i;
+			}
+		}
+
+		int32 i1 = bestIndex;
+		int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0;
+
+		clipPoints[0].v = tempPolygonB.vertices[i1];
+		clipPoints[0].id.cf.indexA = 0;
+		clipPoints[0].id.cf.indexB = static_cast<uint8>(i1);
+		clipPoints[0].id.cf.typeA = b2ContactFeature::e_face;
+		clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex;
+
+		clipPoints[1].v = tempPolygonB.vertices[i2];
+		clipPoints[1].id.cf.indexA = 0;
+		clipPoints[1].id.cf.indexB = static_cast<uint8>(i2);
+		clipPoints[1].id.cf.typeA = b2ContactFeature::e_face;
+		clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex;
+
+		ref.i1 = 0;
+		ref.i2 = 1;
+		ref.v1 = v1;
+		ref.v2 = v2;
+		ref.normal = primaryAxis.normal;
+		ref.sideNormal1 = -edge1;
+		ref.sideNormal2 = edge1;
+	}
+	else
+	{
+		manifold->type = b2Manifold::e_faceB;
+
+		clipPoints[0].v = v2;
+		clipPoints[0].id.cf.indexA = 1;
+		clipPoints[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
+		clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex;
+		clipPoints[0].id.cf.typeB = b2ContactFeature::e_face;
+
+		clipPoints[1].v = v1;
+		clipPoints[1].id.cf.indexA = 0;
+		clipPoints[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
+		clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex;
+		clipPoints[1].id.cf.typeB = b2ContactFeature::e_face;
+
+		ref.i1 = primaryAxis.index;
+		ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0;
+		ref.v1 = tempPolygonB.vertices[ref.i1];
+		ref.v2 = tempPolygonB.vertices[ref.i2];
+		ref.normal = tempPolygonB.normals[ref.i1];
+
+		// CCW winding
+		ref.sideNormal1.Set(ref.normal.y, -ref.normal.x);
+		ref.sideNormal2 = -ref.sideNormal1;
+	}
+
+	ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1);
+	ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2);
+
+	// Clip incident edge against reference face side planes
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int32 np;
+
+	// Clip to side 1
+	np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1);
+
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+
+	// Clip to side 2
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2);
+
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+
+	// Now clipPoints2 contains the clipped points.
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->localNormal = ref.normal;
+		manifold->localPoint = ref.v1;
+	}
+	else
+	{
+		manifold->localNormal = polygonB->m_normals[ref.i1];
+		manifold->localPoint = polygonB->m_vertices[ref.i1];
+	}
+
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float separation;
+
+		separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1);
+
+		if (separation <= radius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+
+			if (primaryAxis.type == b2EPAxis::e_edgeA)
+			{
+				cp->localPoint = b2MulT(xf, clipPoints2[i].v);
+				cp->id = clipPoints2[i].id;
+			}
+			else
+			{
+				cp->localPoint = clipPoints2[i].v;
+				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
+				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
+				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
+				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
+			}
+
+			++pointCount;
+		}
+	}
+
+	manifold->pointCount = pointCount;
+}

src/libraries/Box2D/Collision/b2CollidePolygon.cpp → src/libraries/Box2D/Collision/b2_collide_polygon.cpp

@@ -1,26 +1,30 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_polygon_shape.h"
 
 // Find the max separation between poly1 and poly2 using edge normals from poly1.
-static float32 b2FindMaxSeparation(int32* edgeIndex,
+static float b2FindMaxSeparation(int32* edgeIndex,
 								 const b2PolygonShape* poly1, const b2Transform& xf1,
 								 const b2PolygonShape* poly2, const b2Transform& xf2)
 {
@@ -32,7 +36,7 @@ static float32 b2FindMaxSeparation(int32* edgeIndex,
 	b2Transform xf = b2MulT(xf2, xf1);
 
 	int32 bestIndex = 0;
-	float32 maxSeparation = -b2_maxFloat;
+	float maxSeparation = -b2_maxFloat;
 	for (int32 i = 0; i < count1; ++i)
 	{
 		// Get poly1 normal in frame2.
@@ -40,10 +44,10 @@ static float32 b2FindMaxSeparation(int32* edgeIndex,
 		b2Vec2 v1 = b2Mul(xf, v1s[i]);
 
 		// Find deepest point for normal i.
-		float32 si = b2_maxFloat;
+		float si = b2_maxFloat;
 		for (int32 j = 0; j < count2; ++j)
 		{
-			float32 sij = b2Dot(n, v2s[j] - v1);
+			float sij = b2Dot(n, v2s[j] - v1);
 			if (sij < si)
 			{
 				si = sij;
@@ -78,10 +82,10 @@ static void b2FindIncidentEdge(b2ClipVertex c[2],
 
 	// Find the incident edge on poly2.
 	int32 index = 0;
-	float32 minDot = b2_maxFloat;
+	float minDot = b2_maxFloat;
 	for (int32 i = 0; i < count2; ++i)
 	{
-		float32 dot = b2Dot(normal1, normals2[i]);
+		float dot = b2Dot(normal1, normals2[i]);
 		if (dot < minDot)
 		{
 			minDot = dot;
@@ -118,15 +122,15 @@ void b2CollidePolygons(b2Manifold* manifold,
 					  const b2PolygonShape* polyB, const b2Transform& xfB)
 {
 	manifold->pointCount = 0;
-	float32 totalRadius = polyA->m_radius + polyB->m_radius;
+	float totalRadius = polyA->m_radius + polyB->m_radius;
 
 	int32 edgeA = 0;
-	float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	float separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
 	if (separationA > totalRadius)
 		return;
 
 	int32 edgeB = 0;
-	float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	float separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
 	if (separationB > totalRadius)
 		return;
 
@@ -135,7 +139,7 @@ void b2CollidePolygons(b2Manifold* manifold,
 	b2Transform xf1, xf2;
 	int32 edge1;					// reference edge
 	uint8 flip;
-	const float32 k_tol = 0.1f * b2_linearSlop;
+	const float k_tol = 0.1f * b2_linearSlop;
 
 	if (separationB > separationA + k_tol)
 	{
@@ -183,11 +187,11 @@ void b2CollidePolygons(b2Manifold* manifold,
 	v12 = b2Mul(xf1, v12);
 
 	// Face offset.
-	float32 frontOffset = b2Dot(normal, v11);
+	float frontOffset = b2Dot(normal, v11);
 
 	// Side offsets, extended by polytope skin thickness.
-	float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
-	float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
+	float sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
+	float sideOffset2 = b2Dot(tangent, v12) + totalRadius;
 
 	// Clip incident edge against extruded edge1 side edges.
 	b2ClipVertex clipPoints1[2];
@@ -215,7 +219,7 @@ void b2CollidePolygons(b2Manifold* manifold,
 	int32 pointCount = 0;
 	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
 	{
-		float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
+		float separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
 
 		if (separation <= totalRadius)
 		{

src/libraries/Box2D/Collision/b2Collision.cpp → src/libraries/Box2D/Collision/b2_collision.cpp

@@ -1,27 +1,31 @@
-/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/b2Distance.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_distance.h"
 
 void b2WorldManifold::Initialize(const b2Manifold* manifold,
-						  const b2Transform& xfA, float32 radiusA,
-						  const b2Transform& xfB, float32 radiusB)
+						  const b2Transform& xfA, float radiusA,
+						  const b2Transform& xfB, float radiusB)
 {
 	if (manifold->pointCount == 0)
 	{
@@ -132,8 +136,8 @@ void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState st
 // From Real-time Collision Detection, p179.
 bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
 {
-	float32 tmin = -b2_maxFloat;
-	float32 tmax = b2_maxFloat;
+	float tmin = -b2_maxFloat;
+	float tmax = b2_maxFloat;
 
 	b2Vec2 p = input.p1;
 	b2Vec2 d = input.p2 - input.p1;
@@ -153,12 +157,12 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
 		}
 		else
 		{
-			float32 inv_d = 1.0f / d(i);
-			float32 t1 = (lowerBound(i) - p(i)) * inv_d;
-			float32 t2 = (upperBound(i) - p(i)) * inv_d;
+			float inv_d = 1.0f / d(i);
+			float t1 = (lowerBound(i) - p(i)) * inv_d;
+			float t2 = (upperBound(i) - p(i)) * inv_d;
 
 			// Sign of the normal vector.
-			float32 s = -1.0f;
+			float s = -1.0f;
 
 			if (t1 > t2)
 			{
@@ -199,35 +203,37 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
 
 // Sutherland-Hodgman clipping.
 int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
-						const b2Vec2& normal, float32 offset, int32 vertexIndexA)
+						const b2Vec2& normal, float offset, int32 vertexIndexA)
 {
 	// Start with no output points
-	int32 numOut = 0;
+	int32 count = 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;
+	float distance0 = b2Dot(normal, vIn[0].v) - offset;
+	float 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 (distance0 <= 0.0f) vOut[count++] = vIn[0];
+	if (distance1 <= 0.0f) vOut[count++] = 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);
+		float interp = distance0 / (distance0 - distance1);
+		vOut[count].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
 
 		// VertexA is hitting edgeB.
-		vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
-		vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
-		vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
-		vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
-		++numOut;
+		vOut[count].id.cf.indexA = static_cast<uint8>(vertexIndexA);
+		vOut[count].id.cf.indexB = vIn[0].id.cf.indexB;
+		vOut[count].id.cf.typeA = b2ContactFeature::e_vertex;
+		vOut[count].id.cf.typeB = b2ContactFeature::e_face;
+		++count;
+
+		b2Assert(count == 2);
 	}
 
-	return numOut;
+	return count;
 }
 
 bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,

src/libraries/Box2D/Collision/b2Distance.cpp → src/libraries/Box2D/Collision/b2_distance.cpp

@@ -1,26 +1,30 @@
-/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/b2Distance.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_distance.h"
+#include "box2d/b2_edge_shape.h"
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_polygon_shape.h"
 
 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
 int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
@@ -82,13 +86,19 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 	}
 }
 
+void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float radius)
+{
+    m_vertices = vertices;
+    m_count = count;
+    m_radius = radius;
+}
 
 struct b2SimplexVertex
 {
 	b2Vec2 wA;		// support point in proxyA
 	b2Vec2 wB;		// support point in proxyB
 	b2Vec2 w;		// wB - wA
-	float32 a;		// barycentric coordinate for closest point
+	float a;		// barycentric coordinate for closest point
 	int32 indexA;	// wA index
 	int32 indexB;	// wB index
 };
@@ -121,8 +131,8 @@ struct b2Simplex
 		// old metric then flush the simplex.
 		if (m_count > 1)
 		{
-			float32 metric1 = cache->metric;
-			float32 metric2 = GetMetric();
+			float metric1 = cache->metric;
+			float metric2 = GetMetric();
 			if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
 			{
 				// Reset the simplex.
@@ -168,7 +178,7 @@ struct b2Simplex
 		case 2:
 			{
 				b2Vec2 e12 = m_v2.w - m_v1.w;
-				float32 sgn = b2Cross(e12, -m_v1.w);
+				float sgn = b2Cross(e12, -m_v1.w);
 				if (sgn > 0.0f)
 				{
 					// Origin is left of e12.
@@ -239,7 +249,7 @@ struct b2Simplex
 		}
 	}
 
-	float32 GetMetric() const
+	float GetMetric() const
 	{
 		switch (m_count)
 		{
@@ -300,7 +310,7 @@ void b2Simplex::Solve2()
 	b2Vec2 e12 = w2 - w1;
 
 	// w1 region
-	float32 d12_2 = -b2Dot(w1, e12);
+	float d12_2 = -b2Dot(w1, e12);
 	if (d12_2 <= 0.0f)
 	{
 		// a2 <= 0, so we clamp it to 0
@@ -310,7 +320,7 @@ void b2Simplex::Solve2()
 	}
 
 	// w2 region
-	float32 d12_1 = b2Dot(w2, e12);
+	float d12_1 = b2Dot(w2, e12);
 	if (d12_1 <= 0.0f)
 	{
 		// a1 <= 0, so we clamp it to 0
@@ -321,7 +331,7 @@ void b2Simplex::Solve2()
 	}
 
 	// Must be in e12 region.
-	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+	float inv_d12 = 1.0f / (d12_1 + d12_2);
 	m_v1.a = d12_1 * inv_d12;
 	m_v2.a = d12_2 * inv_d12;
 	m_count = 2;
@@ -343,37 +353,37 @@ void b2Simplex::Solve3()
 	// [w1.e12 w2.e12][a2] = [0]
 	// a3 = 0
 	b2Vec2 e12 = w2 - w1;
-	float32 w1e12 = b2Dot(w1, e12);
-	float32 w2e12 = b2Dot(w2, e12);
-	float32 d12_1 = w2e12;
-	float32 d12_2 = -w1e12;
+	float w1e12 = b2Dot(w1, e12);
+	float w2e12 = b2Dot(w2, e12);
+	float d12_1 = w2e12;
+	float d12_2 = -w1e12;
 
 	// Edge13
 	// [1      1     ][a1] = [1]
 	// [w1.e13 w3.e13][a3] = [0]
 	// a2 = 0
 	b2Vec2 e13 = w3 - w1;
-	float32 w1e13 = b2Dot(w1, e13);
-	float32 w3e13 = b2Dot(w3, e13);
-	float32 d13_1 = w3e13;
-	float32 d13_2 = -w1e13;
+	float w1e13 = b2Dot(w1, e13);
+	float w3e13 = b2Dot(w3, e13);
+	float d13_1 = w3e13;
+	float d13_2 = -w1e13;
 
 	// Edge23
 	// [1      1     ][a2] = [1]
 	// [w2.e23 w3.e23][a3] = [0]
 	// a1 = 0
 	b2Vec2 e23 = w3 - w2;
-	float32 w2e23 = b2Dot(w2, e23);
-	float32 w3e23 = b2Dot(w3, e23);
-	float32 d23_1 = w3e23;
-	float32 d23_2 = -w2e23;
+	float w2e23 = b2Dot(w2, e23);
+	float w3e23 = b2Dot(w3, e23);
+	float d23_1 = w3e23;
+	float d23_2 = -w2e23;
 	
 	// Triangle123
-	float32 n123 = b2Cross(e12, e13);
+	float n123 = b2Cross(e12, e13);
 
-	float32 d123_1 = n123 * b2Cross(w2, w3);
-	float32 d123_2 = n123 * b2Cross(w3, w1);
-	float32 d123_3 = n123 * b2Cross(w1, w2);
+	float d123_1 = n123 * b2Cross(w2, w3);
+	float d123_2 = n123 * b2Cross(w3, w1);
+	float d123_3 = n123 * b2Cross(w1, w2);
 
 	// w1 region
 	if (d12_2 <= 0.0f && d13_2 <= 0.0f)
@@ -386,7 +396,7 @@ void b2Simplex::Solve3()
 	// e12
 	if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
 	{
-		float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+		float inv_d12 = 1.0f / (d12_1 + d12_2);
 		m_v1.a = d12_1 * inv_d12;
 		m_v2.a = d12_2 * inv_d12;
 		m_count = 2;
@@ -396,7 +406,7 @@ void b2Simplex::Solve3()
 	// e13
 	if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
 	{
-		float32 inv_d13 = 1.0f / (d13_1 + d13_2);
+		float inv_d13 = 1.0f / (d13_1 + d13_2);
 		m_v1.a = d13_1 * inv_d13;
 		m_v3.a = d13_2 * inv_d13;
 		m_count = 2;
@@ -425,7 +435,7 @@ void b2Simplex::Solve3()
 	// e23
 	if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
 	{
-		float32 inv_d23 = 1.0f / (d23_1 + d23_2);
+		float inv_d23 = 1.0f / (d23_1 + d23_2);
 		m_v2.a = d23_1 * inv_d23;
 		m_v3.a = d23_2 * inv_d23;
 		m_count = 2;
@@ -434,7 +444,7 @@ void b2Simplex::Solve3()
 	}
 
 	// Must be in triangle123
-	float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
+	float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
 	m_v1.a = d123_1 * inv_d123;
 	m_v2.a = d123_2 * inv_d123;
 	m_v3.a = d123_3 * inv_d123;
@@ -466,9 +476,6 @@ void b2Distance(b2DistanceOutput* output,
 	int32 saveA[3], saveB[3];
 	int32 saveCount = 0;
 
-	float32 distanceSqr1 = b2_maxFloat;
-	float32 distanceSqr2 = distanceSqr1;
-
 	// Main iteration loop.
 	int32 iter = 0;
 	while (iter < k_maxIters)
@@ -504,17 +511,6 @@ void b2Distance(b2DistanceOutput* output,
 			break;
 		}
 
-		// Compute closest point.
-		b2Vec2 p = simplex.GetClosestPoint();
-		distanceSqr2 = p.LengthSquared();
-
-		// Ensure progress
-		if (distanceSqr2 >= distanceSqr1)
-		{
-			//break;
-		}
-		distanceSqr1 = distanceSqr2;
-
 		// Get search direction.
 		b2Vec2 d = simplex.GetSearchDirection();
 
@@ -534,7 +530,6 @@ void b2Distance(b2DistanceOutput* output,
 		b2SimplexVertex* vertex = vertices + simplex.m_count;
 		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
 		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
-		b2Vec2 wBLocal;
 		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
 		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
 		vertex->w = vertex->wB - vertex->wA;
@@ -577,8 +572,8 @@ void b2Distance(b2DistanceOutput* output,
 	// Apply radii if requested.
 	if (input->useRadii)
 	{
-		float32 rA = proxyA->m_radius;
-		float32 rB = proxyB->m_radius;
+		float rA = proxyA->m_radius;
+		float rB = proxyB->m_radius;
 
 		if (output->distance > rA + rB && output->distance > b2_epsilon)
 		{
@@ -601,3 +596,151 @@ void b2Distance(b2DistanceOutput* output,
 		}
 	}
 }
+
+// GJK-raycast
+// Algorithm by Gino van den Bergen.
+// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
+bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input)
+{
+    output->iterations = 0;
+    output->lambda = 1.0f;
+    output->normal.SetZero();
+    output->point.SetZero();
+
+	const b2DistanceProxy* proxyA = &input->proxyA;
+	const b2DistanceProxy* proxyB = &input->proxyB;
+
+    float radiusA = b2Max(proxyA->m_radius, b2_polygonRadius);
+    float radiusB = b2Max(proxyB->m_radius, b2_polygonRadius);
+    float radius = radiusA + radiusB;
+
+	b2Transform xfA = input->transformA;
+	b2Transform xfB = input->transformB;
+
+	b2Vec2 r = input->translationB;
+	b2Vec2 n(0.0f, 0.0f);
+	float lambda = 0.0f;
+
+	// Initial simplex
+	b2Simplex simplex;
+	simplex.m_count = 0;
+
+	// Get simplex vertices as an array.
+	b2SimplexVertex* vertices = &simplex.m_v1;
+
+	// Get support point in -r direction
+	int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r));
+	b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
+	int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r));
+	b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
+    b2Vec2 v = wA - wB;
+
+    // Sigma is the target distance between polygons
+    float sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius);
+	const float tolerance = 0.5f * b2_linearSlop;
+
+	// Main iteration loop.
+	const int32 k_maxIters = 20;
+	int32 iter = 0;
+	while (iter < k_maxIters && v.Length() - sigma > tolerance)
+	{
+		b2Assert(simplex.m_count < 3);
+
+        output->iterations += 1;
+
+		// Support in direction -v (A - B)
+		indexA = proxyA->GetSupport(b2MulT(xfA.q, -v));
+		wA = b2Mul(xfA, proxyA->GetVertex(indexA));
+		indexB = proxyB->GetSupport(b2MulT(xfB.q, v));
+		wB = b2Mul(xfB, proxyB->GetVertex(indexB));
+        b2Vec2 p = wA - wB;
+
+        // -v is a normal at p
+        v.Normalize();
+
+        // Intersect ray with plane
+		float vp = b2Dot(v, p);
+        float vr = b2Dot(v, r);
+		if (vp - sigma > lambda * vr)
+		{
+			if (vr <= 0.0f)
+			{
+				return false;
+			}
+
+			lambda = (vp - sigma) / vr;
+			if (lambda > 1.0f)
+			{
+				return false;
+			}
+
+            n = -v;
+            simplex.m_count = 0;
+		}
+
+        // Reverse simplex since it works with B - A.
+        // Shift by lambda * r because we want the closest point to the current clip point.
+        // Note that the support point p is not shifted because we want the plane equation
+        // to be formed in unshifted space.
+		b2SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = indexB;
+		vertex->wA = wB + lambda * r;
+		vertex->indexB = indexA;
+		vertex->wB = wA;
+		vertex->w = vertex->wB - vertex->wA;
+		vertex->a = 1.0f;
+		simplex.m_count += 1;
+
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+
+		case 2:
+			simplex.Solve2();
+			break;
+
+		case 3:
+			simplex.Solve3();
+			break;
+
+		default:
+			b2Assert(false);
+		}
+		
+		// If we have 3 points, then the origin is in the corresponding triangle.
+		if (simplex.m_count == 3)
+		{
+			// Overlap
+			return false;
+		}
+
+		// Get search direction.
+		v = simplex.GetClosestPoint();
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+	}
+
+	if (iter == 0)
+	{
+		// Initial overlap
+		return false;
+	}
+
+	// Prepare output.
+	b2Vec2 pointA, pointB;
+	simplex.GetWitnessPoints(&pointB, &pointA);
+
+	if (v.LengthSquared() > 0.0f)
+	{
+        n = -v;
+		n.Normalize();
+	}
+
+    output->point = pointA + radiusA * n;
+	output->normal = n;
+	output->lambda = lambda;
+	output->iterations = iter;
+	return true;
+}

src/libraries/Box2D/Collision/b2DynamicTree.cpp → src/libraries/Box2D/Collision/b2_dynamic_tree.cpp

@@ -1,22 +1,25 @@
-/*
-* Copyright (c) 2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/b2DynamicTree.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+#include "box2d/b2_dynamic_tree.h"
 #include <string.h>
 
 b2DynamicTree::b2DynamicTree()
@@ -83,7 +86,8 @@ int32 b2DynamicTree::AllocateNode()
 	m_nodes[nodeId].child1 = b2_nullNode;
 	m_nodes[nodeId].child2 = b2_nullNode;
 	m_nodes[nodeId].height = 0;
-	m_nodes[nodeId].userData = NULL;
+	m_nodes[nodeId].userData = nullptr;
+	m_nodes[nodeId].moved = false;
 	++m_nodeCount;
 	return nodeId;
 }
@@ -112,6 +116,7 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
 	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
 	m_nodes[proxyId].userData = userData;
 	m_nodes[proxyId].height = 0;
+	m_nodes[proxyId].moved = true;
 
 	InsertLeaf(proxyId);
 
@@ -133,43 +138,61 @@ bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& d
 
 	b2Assert(m_nodes[proxyId].IsLeaf());
 
-	if (m_nodes[proxyId].aabb.Contains(aabb))
-	{
-		return false;
-	}
-
-	RemoveLeaf(proxyId);
-
-	// Extend AABB.
-	b2AABB b = aabb;
+	// Extend AABB
+	b2AABB fatAABB;
 	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
-	b.lowerBound = b.lowerBound - r;
-	b.upperBound = b.upperBound + r;
+	fatAABB.lowerBound = aabb.lowerBound - r;
+	fatAABB.upperBound = aabb.upperBound + r;
 
-	// Predict AABB displacement.
+	// Predict AABB movement
 	b2Vec2 d = b2_aabbMultiplier * displacement;
 
 	if (d.x < 0.0f)
 	{
-		b.lowerBound.x += d.x;
+		fatAABB.lowerBound.x += d.x;
 	}
 	else
 	{
-		b.upperBound.x += d.x;
+		fatAABB.upperBound.x += d.x;
 	}
 
 	if (d.y < 0.0f)
 	{
-		b.lowerBound.y += d.y;
+		fatAABB.lowerBound.y += d.y;
 	}
 	else
 	{
-		b.upperBound.y += d.y;
+		fatAABB.upperBound.y += d.y;
+	}
+
+	const b2AABB& treeAABB = m_nodes[proxyId].aabb;
+	if (treeAABB.Contains(aabb))
+	{
+		// The tree AABB still contains the object, but it might be too large.
+		// Perhaps the object was moving fast but has since gone to sleep.
+		// The huge AABB is larger than the new fat AABB.
+		b2AABB hugeAABB;
+		hugeAABB.lowerBound = fatAABB.lowerBound - 4.0f * r;
+		hugeAABB.upperBound = fatAABB.upperBound + 4.0f * r;
+
+		if (hugeAABB.Contains(treeAABB))
+		{
+			// The tree AABB contains the object AABB and the tree AABB is
+			// not too large. No tree update needed.
+			return false;
+		}
+
+		// Otherwise the tree AABB is huge and needs to be shrunk
 	}
 
-	m_nodes[proxyId].aabb = b;
+	RemoveLeaf(proxyId);
+
+	m_nodes[proxyId].aabb = fatAABB;
 
 	InsertLeaf(proxyId);
+
+	m_nodes[proxyId].moved = true;
+
 	return true;
 }
 
@@ -192,20 +215,20 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
 		int32 child1 = m_nodes[index].child1;
 		int32 child2 = m_nodes[index].child2;
 
-		float32 area = m_nodes[index].aabb.GetPerimeter();
+		float area = m_nodes[index].aabb.GetPerimeter();
 
 		b2AABB combinedAABB;
 		combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
-		float32 combinedArea = combinedAABB.GetPerimeter();
+		float combinedArea = combinedAABB.GetPerimeter();
 
 		// Cost of creating a new parent for this node and the new leaf
-		float32 cost = 2.0f * combinedArea;
+		float cost = 2.0f * combinedArea;
 
 		// Minimum cost of pushing the leaf further down the tree
-		float32 inheritanceCost = 2.0f * (combinedArea - area);
+		float inheritanceCost = 2.0f * (combinedArea - area);
 
 		// Cost of descending into child1
-		float32 cost1;
+		float cost1;
 		if (m_nodes[child1].IsLeaf())
 		{
 			b2AABB aabb;
@@ -216,13 +239,13 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
 		{
 			b2AABB aabb;
 			aabb.Combine(leafAABB, m_nodes[child1].aabb);
-			float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
-			float32 newArea = aabb.GetPerimeter();
+			float oldArea = m_nodes[child1].aabb.GetPerimeter();
+			float newArea = aabb.GetPerimeter();
 			cost1 = (newArea - oldArea) + inheritanceCost;
 		}
 
 		// Cost of descending into child2
-		float32 cost2;
+		float cost2;
 		if (m_nodes[child2].IsLeaf())
 		{
 			b2AABB aabb;
@@ -233,8 +256,8 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
 		{
 			b2AABB aabb;
 			aabb.Combine(leafAABB, m_nodes[child2].aabb);
-			float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
-			float32 newArea = aabb.GetPerimeter();
+			float oldArea = m_nodes[child2].aabb.GetPerimeter();
+			float newArea = aabb.GetPerimeter();
 			cost2 = newArea - oldArea + inheritanceCost;
 		}
 
@@ -261,7 +284,7 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
 	int32 oldParent = m_nodes[sibling].parent;
 	int32 newParent = AllocateNode();
 	m_nodes[newParent].parent = oldParent;
-	m_nodes[newParent].userData = NULL;
+	m_nodes[newParent].userData = nullptr;
 	m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
 	m_nodes[newParent].height = m_nodes[sibling].height + 1;
 
@@ -528,7 +551,7 @@ int32 b2DynamicTree::GetHeight() const
 }
 
 //
-float32 b2DynamicTree::GetAreaRatio() const
+float b2DynamicTree::GetAreaRatio() const
 {
 	if (m_root == b2_nullNode)
 	{
@@ -536,9 +559,9 @@ float32 b2DynamicTree::GetAreaRatio() const
 	}
 
 	const b2TreeNode* root = m_nodes + m_root;
-	float32 rootArea = root->aabb.GetPerimeter();
+	float rootArea = root->aabb.GetPerimeter();
 
-	float32 totalArea = 0.0f;
+	float totalArea = 0.0f;
 	for (int32 i = 0; i < m_nodeCapacity; ++i)
 	{
 		const b2TreeNode* node = m_nodes + i;
@@ -652,6 +675,7 @@ void b2DynamicTree::ValidateMetrics(int32 index) const
 
 void b2DynamicTree::Validate() const
 {
+#if defined(b2DEBUG)
 	ValidateStructure(m_root);
 	ValidateMetrics(m_root);
 
@@ -667,6 +691,7 @@ void b2DynamicTree::Validate() const
 	b2Assert(GetHeight() == ComputeHeight());
 
 	b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
+#endif
 }
 
 int32 b2DynamicTree::GetMaxBalance() const
@@ -719,7 +744,7 @@ void b2DynamicTree::RebuildBottomUp()
 
 	while (count > 1)
 	{
-		float32 minCost = b2_maxFloat;
+		float minCost = b2_maxFloat;
 		int32 iMin = -1, jMin = -1;
 		for (int32 i = 0; i < count; ++i)
 		{
@@ -730,7 +755,7 @@ void b2DynamicTree::RebuildBottomUp()
 				b2AABB aabbj = m_nodes[nodes[j]].aabb;
 				b2AABB b;
 				b.Combine(aabbi, aabbj);
-				float32 cost = b.GetPerimeter();
+				float cost = b.GetPerimeter();
 				if (cost < minCost)
 				{
 					iMin = i;

src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp → src/libraries/Box2D/Collision/b2_edge_shape.cpp

@@ -1,30 +1,43 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_edge_shape.h"
+#include "box2d/b2_block_allocator.h"
 #include <new>
 
-void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
+void b2EdgeShape::SetOneSided(const b2Vec2& v0, const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& v3)
 {
+	m_vertex0 = v0;
 	m_vertex1 = v1;
 	m_vertex2 = v2;
-	m_hasVertex0 = false;
-	m_hasVertex3 = false;
+	m_vertex3 = v3;
+	m_oneSided = true;
+}
+
+void b2EdgeShape::SetTwoSided(const b2Vec2& v1, const b2Vec2& v2)
+{
+	m_vertex1 = v1;
+	m_vertex2 = v2;
+	m_oneSided = false;
 }
 
 b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
@@ -64,21 +77,28 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
 	b2Vec2 v1 = m_vertex1;
 	b2Vec2 v2 = m_vertex2;
 	b2Vec2 e = v2 - v1;
+
+	// Normal points to the right, looking from v1 at v2
 	b2Vec2 normal(e.y, -e.x);
 	normal.Normalize();
 
 	// q = p1 + t * d
 	// dot(normal, q - v1) = 0
 	// dot(normal, p1 - v1) + t * dot(normal, d) = 0
-	float32 numerator = b2Dot(normal, v1 - p1);
-	float32 denominator = b2Dot(normal, d);
+	float numerator = b2Dot(normal, v1 - p1);
+	if (m_oneSided && numerator > 0.0f)
+	{
+		return false;
+	}
+
+	float denominator = b2Dot(normal, d);
 
 	if (denominator == 0.0f)
 	{
 		return false;
 	}
 
-	float32 t = numerator / denominator;
+	float t = numerator / denominator;
 	if (t < 0.0f || input.maxFraction < t)
 	{
 		return false;
@@ -89,13 +109,13 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
 	// q = v1 + s * r
 	// s = dot(q - v1, r) / dot(r, r)
 	b2Vec2 r = v2 - v1;
-	float32 rr = b2Dot(r, r);
+	float rr = b2Dot(r, r);
 	if (rr == 0.0f)
 	{
 		return false;
 	}
 
-	float32 s = b2Dot(q - v1, r) / rr;
+	float s = b2Dot(q - v1, r) / rr;
 	if (s < 0.0f || 1.0f < s)
 	{
 		return false;
@@ -128,7 +148,7 @@ void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIn
 	aabb->upperBound = upper + r;
 }
 
-void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2EdgeShape::ComputeMass(b2MassData* massData, float density) const
 {
 	B2_NOT_USED(density);
 

src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp → src/libraries/Box2D/Collision/b2_polygon_shape.cpp

@@ -1,22 +1,28 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Collision/Shapes/b2PolygonShape.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_polygon_shape.h"
+#include "box2d/b2_block_allocator.h"
+
 #include <new>
 
 b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
@@ -27,7 +33,7 @@ b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
 	return clone;
 }
 
-void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
+void b2PolygonShape::SetAsBox(float hx, float hy)
 {
 	m_count = 4;
 	m_vertices[0].Set(-hx, -hy);
@@ -41,7 +47,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
 	m_centroid.SetZero();
 }
 
-void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+void b2PolygonShape::SetAsBox(float hx, float hy, const b2Vec2& center, float angle)
 {
 	m_count = 4;
 	m_vertices[0].Set(-hx, -hy);
@@ -75,36 +81,28 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 {
 	b2Assert(count >= 3);
 
-	b2Vec2 c; c.Set(0.0f, 0.0f);
-	float32 area = 0.0f;
+	b2Vec2 c(0.0f, 0.0f);
+	float 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
+	// Get a reference point for forming triangles.
+	// Use the first vertex to reduce round-off errors.
+	b2Vec2 s = vs[0];
 
-	const float32 inv3 = 1.0f / 3.0f;
+	const float 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 p1 = vs[0] - s;
+		b2Vec2 p2 = vs[i] - s;
+		b2Vec2 p3 = i + 1 < count ? vs[i+1] - s : vs[0] - s;
 
 		b2Vec2 e1 = p2 - p1;
 		b2Vec2 e2 = p3 - p1;
 
-		float32 D = b2Cross(e1, e2);
+		float D = b2Cross(e1, e2);
 
-		float32 triangleArea = 0.5f * D;
+		float triangleArea = 0.5f * D;
 		area += triangleArea;
 
 		// Area weighted centroid
@@ -113,7 +111,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 
 	// Centroid
 	b2Assert(area > b2_epsilon);
-	c *= 1.0f / area;
+	c = (1.0f / area) * c + s;
 	return c;
 }
 
@@ -165,10 +163,10 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 
 	// Find the right most point on the hull
 	int32 i0 = 0;
-	float32 x0 = ps[0].x;
+	float x0 = ps[0].x;
 	for (int32 i = 1; i < n; ++i)
 	{
-		float32 x = ps[i].x;
+		float x = ps[i].x;
 		if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
 		{
 			i0 = i;
@@ -182,6 +180,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 
 	for (;;)
 	{
+		b2Assert(m < b2_maxPolygonVertices);
 		hull[m] = ih;
 
 		int32 ie = 0;
@@ -195,7 +194,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 
 			b2Vec2 r = ps[ie] - ps[hull[m]];
 			b2Vec2 v = ps[j] - ps[hull[m]];
-			float32 c = b2Cross(r, v);
+			float c = b2Cross(r, v);
 			if (c < 0.0f)
 			{
 				ie = j;
@@ -254,7 +253,7 @@ bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
 
 	for (int32 i = 0; i < m_count; ++i)
 	{
-		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+		float dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
 		if (dot > 0.0f)
 		{
 			return false;
@@ -274,7 +273,7 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu
 	b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
 	b2Vec2 d = p2 - p1;
 
-	float32 lower = 0.0f, upper = input.maxFraction;
+	float lower = 0.0f, upper = input.maxFraction;
 
 	int32 index = -1;
 
@@ -283,8 +282,8 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu
 		// 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);
+		float numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+		float denominator = b2Dot(m_normals[i], d);
 
 		if (denominator == 0.0f)
 		{	
@@ -355,7 +354,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil
 	aabb->upperBound = upper + r;
 }
 
-void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2PolygonShape::ComputeMass(b2MassData* massData, float density) const
 {
 	// Polygon mass, centroid, and inertia.
 	// Let rho be the polygon density in mass per unit area.
@@ -383,22 +382,15 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 
 	b2Assert(m_count >= 3);
 
-	b2Vec2 center; center.Set(0.0f, 0.0f);
-	float32 area = 0.0f;
-	float32 I = 0.0f;
-
-	// s is the reference point for forming triangles.
-	// It's location doesn't change the result (except for rounding error).
-	b2Vec2 s(0.0f, 0.0f);
+	b2Vec2 center(0.0f, 0.0f);
+	float area = 0.0f;
+	float I = 0.0f;
 
-	// This code would put the reference point inside the polygon.
-	for (int32 i = 0; i < m_count; ++i)
-	{
-		s += m_vertices[i];
-	}
-	s *= 1.0f / m_count;
+	// Get a reference point for forming triangles.
+	// Use the first vertex to reduce round-off errors.
+	b2Vec2 s = m_vertices[0];
 
-	const float32 k_inv3 = 1.0f / 3.0f;
+	const float k_inv3 = 1.0f / 3.0f;
 
 	for (int32 i = 0; i < m_count; ++i)
 	{
@@ -406,19 +398,19 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 		b2Vec2 e1 = m_vertices[i] - s;
 		b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
 
-		float32 D = b2Cross(e1, e2);
+		float D = b2Cross(e1, e2);
 
-		float32 triangleArea = 0.5f * D;
+		float triangleArea = 0.5f * D;
 		area += triangleArea;
 
 		// Area weighted centroid
 		center += triangleArea * k_inv3 * (e1 + e2);
 
-		float32 ex1 = e1.x, ey1 = e1.y;
-		float32 ex2 = e2.x, ey2 = e2.y;
+		float ex1 = e1.x, ey1 = e1.y;
+		float ex2 = e2.x, ey2 = e2.y;
 
-		float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
-		float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
+		float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
+		float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
 
 		I += (0.25f * k_inv3 * D) * (intx2 + inty2);
 	}
@@ -455,7 +447,7 @@ bool b2PolygonShape::Validate() const
 			}
 
 			b2Vec2 v = m_vertices[j] - p;
-			float32 c = b2Cross(e, v);
+			float c = b2Cross(e, v);
 			if (c < 0.0f)
 			{
 				return false;

src/libraries/Box2D/Collision/b2TimeOfImpact.cpp → src/libraries/Box2D/Collision/b2_time_of_impact.cpp

@@ -1,31 +1,35 @@
-/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/b2Distance.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
-#include <Box2D/Common/b2Timer.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_distance.h"
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_polygon_shape.h"
+#include "box2d/b2_time_of_impact.h"
+#include "box2d/b2_timer.h"
 
 #include <stdio.h>
 
-float32 b2_toiTime, b2_toiMaxTime;
+float b2_toiTime, b2_toiMaxTime;
 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
 int32 b2_toiRootIters, b2_toiMaxRootIters;
 
@@ -41,10 +45,10 @@ struct b2SeparationFunction
 
 	// TODO_ERIN might not need to return the separation
 
-	float32 Initialize(const b2SimplexCache* cache,
+	float Initialize(const b2SimplexCache* cache,
 		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
 		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
-		float32 t1)
+		float t1)
 	{
 		m_proxyA = proxyA;
 		m_proxyB = proxyB;
@@ -66,7 +70,7 @@ struct b2SeparationFunction
 			b2Vec2 pointA = b2Mul(xfA, localPointA);
 			b2Vec2 pointB = b2Mul(xfB, localPointB);
 			m_axis = pointB - pointA;
-			float32 s = m_axis.Normalize();
+			float s = m_axis.Normalize();
 			return s;
 		}
 		else if (cache->indexA[0] == cache->indexA[1])
@@ -86,7 +90,7 @@ struct b2SeparationFunction
 			b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
 			b2Vec2 pointA = b2Mul(xfA, localPointA);
 
-			float32 s = b2Dot(pointA - pointB, normal);
+			float s = b2Dot(pointA - pointB, normal);
 			if (s < 0.0f)
 			{
 				m_axis = -m_axis;
@@ -111,7 +115,7 @@ struct b2SeparationFunction
 			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
 			b2Vec2 pointB = b2Mul(xfB, localPointB);
 
-			float32 s = b2Dot(pointB - pointA, normal);
+			float s = b2Dot(pointB - pointA, normal);
 			if (s < 0.0f)
 			{
 				m_axis = -m_axis;
@@ -122,7 +126,7 @@ struct b2SeparationFunction
 	}
 
 	//
-	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
+	float FindMinSeparation(int32* indexA, int32* indexB, float t) const
 	{
 		b2Transform xfA, xfB;
 		m_sweepA.GetTransform(&xfA, t);
@@ -144,7 +148,7 @@ struct b2SeparationFunction
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
 
-				float32 separation = b2Dot(pointB - pointA, m_axis);
+				float separation = b2Dot(pointB - pointA, m_axis);
 				return separation;
 			}
 
@@ -161,7 +165,7 @@ struct b2SeparationFunction
 				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
 
-				float32 separation = b2Dot(pointB - pointA, normal);
+				float separation = b2Dot(pointB - pointA, normal);
 				return separation;
 			}
 
@@ -178,7 +182,7 @@ struct b2SeparationFunction
 				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 
-				float32 separation = b2Dot(pointA - pointB, normal);
+				float separation = b2Dot(pointA - pointB, normal);
 				return separation;
 			}
 
@@ -191,7 +195,7 @@ struct b2SeparationFunction
 	}
 
 	//
-	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
+	float Evaluate(int32 indexA, int32 indexB, float t) const
 	{
 		b2Transform xfA, xfB;
 		m_sweepA.GetTransform(&xfA, t);
@@ -206,7 +210,7 @@ struct b2SeparationFunction
 
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
-				float32 separation = b2Dot(pointB - pointA, m_axis);
+				float separation = b2Dot(pointB - pointA, m_axis);
 
 				return separation;
 			}
@@ -219,7 +223,7 @@ struct b2SeparationFunction
 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
 
-				float32 separation = b2Dot(pointB - pointA, normal);
+				float separation = b2Dot(pointB - pointA, normal);
 				return separation;
 			}
 
@@ -231,7 +235,7 @@ struct b2SeparationFunction
 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 
-				float32 separation = b2Dot(pointA - pointB, normal);
+				float separation = b2Dot(pointA - pointB, normal);
 				return separation;
 			}
 
@@ -271,14 +275,14 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 	sweepA.Normalize();
 	sweepB.Normalize();
 
-	float32 tMax = input->tMax;
+	float tMax = input->tMax;
 
-	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
-	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
-	float32 tolerance = 0.25f * b2_linearSlop;
+	float totalRadius = proxyA->m_radius + proxyB->m_radius;
+	float target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
+	float tolerance = 0.25f * b2_linearSlop;
 	b2Assert(target > tolerance);
 
-	float32 t1 = 0.0f;
+	float t1 = 0.0f;
 	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
 	int32 iter = 0;
 
@@ -329,17 +333,17 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 		// Dump the curve seen by the root finder
 		{
 			const int32 N = 100;
-			float32 dx = 1.0f / N;
-			float32 xs[N+1];
-			float32 fs[N+1];
+			float dx = 1.0f / N;
+			float xs[N+1];
+			float fs[N+1];
 
-			float32 x = 0.0f;
+			float x = 0.0f;
 
 			for (int32 i = 0; i <= N; ++i)
 			{
 				sweepA.GetTransform(&xfA, x);
 				sweepB.GetTransform(&xfB, x);
-				float32 f = fcn.Evaluate(xfA, xfB) - target;
+				float f = fcn.Evaluate(xfA, xfB) - target;
 
 				printf("%g %g\n", x, f);
 
@@ -354,13 +358,13 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 		// Compute the TOI on the separating axis. We do this by successively
 		// resolving the deepest point. This loop is bounded by the number of vertices.
 		bool done = false;
-		float32 t2 = tMax;
+		float t2 = tMax;
 		int32 pushBackIter = 0;
 		for (;;)
 		{
 			// Find the deepest point at t2. Store the witness point indices.
 			int32 indexA, indexB;
-			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
+			float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
 
 			// Is the final configuration separated?
 			if (s2 > target + tolerance)
@@ -381,7 +385,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 			}
 
 			// Compute the initial separation of the witness points.
-			float32 s1 = fcn.Evaluate(indexA, indexB, t1);
+			float s1 = fcn.Evaluate(indexA, indexB, t1);
 
 			// Check for initial overlap. This might happen if the root finder
 			// runs out of iterations.
@@ -405,11 +409,11 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 
 			// Compute 1D root of: f(x) - target = 0
 			int32 rootIterCount = 0;
-			float32 a1 = t1, a2 = t2;
+			float a1 = t1, a2 = t2;
 			for (;;)
 			{
 				// Use a mix of the secant rule and bisection.
-				float32 t;
+				float t;
 				if (rootIterCount & 1)
 				{
 					// Secant rule to improve convergence.
@@ -424,7 +428,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 				++rootIterCount;
 				++b2_toiRootIters;
 
-				float32 s = fcn.Evaluate(indexA, indexB, t);
+				float s = fcn.Evaluate(indexA, indexB, t);
 
 				if (b2Abs(s - target) < tolerance)
 				{
@@ -480,7 +484,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 
 	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
 
-	float32 time = timer.GetMilliseconds();
+	float time = timer.GetMilliseconds();
 	b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
 	b2_toiTime += time;
 }

src/libraries/Box2D/Common/b2BlockAllocator.h

@@ -1,62 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Common/b2Settings.h>
-
-const int32 b2_chunkSize = 16 * 1024;
-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();
-
-	/// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
-	void* Allocate(int32 size);
-
-	/// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
-	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

src/libraries/Box2D/Common/b2Draw.cpp

@@ -1,44 +0,0 @@
-/*
-* Copyright (c) 2011 Erin Catto http://box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Common/b2Draw.h>
-
-b2Draw::b2Draw()
-{
-	m_drawFlags = 0;
-}
-
-void b2Draw::SetFlags(uint32 flags)
-{
-	m_drawFlags = flags;
-}
-
-uint32 b2Draw::GetFlags() const
-{
-	return m_drawFlags;
-}
-
-void b2Draw::AppendFlags(uint32 flags)
-{
-	m_drawFlags |= flags;
-}
-
-void b2Draw::ClearFlags(uint32 flags)
-{
-	m_drawFlags &= ~flags;
-}

src/libraries/Box2D/Common/b2Draw.h

@@ -1,86 +0,0 @@
-/*
-* Copyright (c) 2011 Erin Catto http://box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_DRAW_H
-#define B2_DRAW_H
-
-#include <Box2D/Common/b2Math.h>
-
-/// Color for debug drawing. Each value has the range [0,1].
-struct b2Color
-{
-	b2Color() {}
-	b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {}
-	void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; }
-	float32 r, g, b, a;
-};
-
-/// Implement and register this class with a b2World to provide debug drawing of physics
-/// entities in your game.
-class b2Draw
-{
-public:
-	b2Draw();
-
-	virtual ~b2Draw() {}
-
-	enum
-	{
-		e_shapeBit				= 0x0001,	///< draw shapes
-		e_jointBit				= 0x0002,	///< draw joint connections
-		e_aabbBit				= 0x0004,	///< draw axis aligned bounding boxes
-		e_pairBit				= 0x0008,	///< draw broad-phase pairs
-		e_centerOfMassBit		= 0x0010	///< draw center of mass frame
-	};
-
-	/// Set the drawing flags.
-	void SetFlags(uint32 flags);
-
-	/// Get the drawing flags.
-	uint32 GetFlags() const;
-	
-	/// Append flags to the current flags.
-	void AppendFlags(uint32 flags);
-
-	/// Clear flags from the current flags.
-	void ClearFlags(uint32 flags);
-
-	/// Draw a closed polygon provided in CCW order.
-	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
-
-	/// Draw a solid closed polygon provided in CCW order.
-	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
-
-	/// Draw a circle.
-	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
-	
-	/// Draw a solid circle.
-	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
-	
-	/// Draw a line segment.
-	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
-
-	/// Draw a transform. Choose your own length scale.
-	/// @param xf a transform.
-	virtual void DrawTransform(const b2Transform& xf) = 0;
-
-protected:
-	uint32 m_drawFlags;
-};
-
-#endif

src/libraries/Box2D/Common/b2GrowableStack.h

@@ -1,85 +0,0 @@
-/*
-* Copyright (c) 2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_GROWABLE_STACK_H
-#define B2_GROWABLE_STACK_H
-#include <Box2D/Common/b2Settings.h>
-#include <string.h>
-
-/// This is a growable LIFO stack with an initial capacity of N.
-/// If the stack size exceeds the initial capacity, the heap is used
-/// to increase the size of the stack.
-template <typename T, int32 N>
-class b2GrowableStack
-{
-public:
-	b2GrowableStack()
-	{
-		m_stack = m_array;
-		m_count = 0;
-		m_capacity = N;
-	}
-
-	~b2GrowableStack()
-	{
-		if (m_stack != m_array)
-		{
-			b2Free(m_stack);
-			m_stack = NULL;
-		}
-	}
-
-	void Push(const T& element)
-	{
-		if (m_count == m_capacity)
-		{
-			T* old = m_stack;
-			m_capacity *= 2;
-			m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
-			memcpy(m_stack, old, m_count * sizeof(T));
-			if (old != m_array)
-			{
-				b2Free(old);
-			}
-		}
-
-		m_stack[m_count] = element;
-		++m_count;
-	}
-
-	T Pop()
-	{
-		b2Assert(m_count > 0);
-		--m_count;
-		return m_stack[m_count];
-	}
-
-	int32 GetCount()
-	{
-		return m_count;
-	}
-
-private:
-	T* m_stack;
-	T m_array[N];
-	int32 m_count;
-	int32 m_capacity;
-};
-
-
-#endif

src/libraries/Box2D/Common/b2Math.cpp

@@ -1,94 +0,0 @@
-/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Common/b2Math.h>
-
-const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
-
-/// Solve A * x = b, where b is a column vector. This is more efficient
-/// than computing the inverse in one-shot cases.
-b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
-{
-	float32 det = b2Dot(ex, b2Cross(ey, ez));
-	if (det != 0.0f)
-	{
-		det = 1.0f / det;
-	}
-	b2Vec3 x;
-	x.x = det * b2Dot(b, b2Cross(ey, ez));
-	x.y = det * b2Dot(ex, b2Cross(b, ez));
-	x.z = det * b2Dot(ex, b2Cross(ey, b));
-	return x;
-}
-
-/// Solve A * x = b, where b is a column vector. This is more efficient
-/// than computing the inverse in one-shot cases.
-b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
-{
-	float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
-	float32 det = a11 * a22 - a12 * a21;
-	if (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;
-}
-
-///
-void b2Mat33::GetInverse22(b2Mat33* M) const
-{
-	float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
-	float32 det = a * d - b * c;
-	if (det != 0.0f)
-	{
-		det = 1.0f / det;
-	}
-
-	M->ex.x =  det * d;	M->ey.x = -det * b; M->ex.z = 0.0f;
-	M->ex.y = -det * c;	M->ey.y =  det * a; M->ey.z = 0.0f;
-	M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
-}
-
-/// Returns the zero matrix if singular.
-void b2Mat33::GetSymInverse33(b2Mat33* M) const
-{
-	float32 det = b2Dot(ex, b2Cross(ey, ez));
-	if (det != 0.0f)
-	{
-		det = 1.0f / det;
-	}
-
-	float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
-	float32 a22 = ey.y, a23 = ez.y;
-	float32 a33 = ez.z;
-
-	M->ex.x = det * (a22 * a33 - a23 * a23);
-	M->ex.y = det * (a13 * a23 - a12 * a33);
-	M->ex.z = det * (a12 * a23 - a13 * a22);
-
-	M->ey.x = M->ex.y;
-	M->ey.y = det * (a11 * a33 - a13 * a13);
-	M->ey.z = det * (a13 * a12 - a11 * a23);
-
-	M->ez.x = M->ex.z;
-	M->ez.y = M->ey.z;
-	M->ez.z = det * (a11 * a22 - a12 * a12);
-}

src/libraries/Box2D/Common/b2Settings.cpp

@@ -1,52 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Common/b2Settings.h>
-#include <stdio.h>
-#include <stdarg.h>
-#include <stdlib.h>
-
-#include "common/Exception.h"
-
-b2Version b2_version = {2, 3, 2};
-
-// Memory allocators. Modify these to use your own allocator.
-void* b2Alloc(int32 size)
-{
-	return malloc(size);
-}
-
-void b2Free(void* mem)
-{
-	free(mem);
-}
-
-// You can modify this to use your logging facility.
-void b2Log(const char* string, ...)
-{
-	va_list args;
-	va_start(args, string);
-	vprintf(string, args);
-	va_end(args);
-}
-
-void loveAssert(bool test, const char *teststr)
-{
-	if (!test)
-		throw love::Exception("Box2D assertion failed: %s", teststr);
-}

src/libraries/Box2D/Common/b2StackAllocator.cpp

@@ -1,83 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Common/b2StackAllocator.h>
-#include <Box2D/Common/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;
-}

src/libraries/Box2D/Common/b2StackAllocator.h

@@ -1,60 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Common/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

src/libraries/Box2D/Common/b2Timer.cpp

@@ -1,101 +0,0 @@
-/*
-* Copyright (c) 2011 Erin Catto http://box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Common/b2Timer.h>
-
-#if defined(_WIN32)
-
-float64 b2Timer::s_invFrequency = 0.0f;
-
-#define WIN32_LEAN_AND_MEAN
-#include <windows.h>
-
-b2Timer::b2Timer()
-{
-	LARGE_INTEGER largeInteger;
-
-	if (s_invFrequency == 0.0f)
-	{
-		QueryPerformanceFrequency(&largeInteger);
-		s_invFrequency = float64(largeInteger.QuadPart);
-		if (s_invFrequency > 0.0f)
-		{
-			s_invFrequency = 1000.0f / s_invFrequency;
-		}
-	}
-
-	QueryPerformanceCounter(&largeInteger);
-	m_start = float64(largeInteger.QuadPart);
-}
-
-void b2Timer::Reset()
-{
-	LARGE_INTEGER largeInteger;
-	QueryPerformanceCounter(&largeInteger);
-	m_start = float64(largeInteger.QuadPart);
-}
-
-float32 b2Timer::GetMilliseconds() const
-{
-	LARGE_INTEGER largeInteger;
-	QueryPerformanceCounter(&largeInteger);
-	float64 count = float64(largeInteger.QuadPart);
-	float32 ms = float32(s_invFrequency * (count - m_start));
-	return ms;
-}
-
-#elif defined(__linux__) || defined (__APPLE__)
-
-#include <sys/time.h>
-
-b2Timer::b2Timer()
-{
-    Reset();
-}
-
-void b2Timer::Reset()
-{
-    timeval t;
-    gettimeofday(&t, 0);
-    m_start_sec = t.tv_sec;
-    m_start_usec = t.tv_usec;
-}
-
-float32 b2Timer::GetMilliseconds() const
-{
-    timeval t;
-    gettimeofday(&t, 0);
-    return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);
-}
-
-#else
-
-b2Timer::b2Timer()
-{
-}
-
-void b2Timer::Reset()
-{
-}
-
-float32 b2Timer::GetMilliseconds() const
-{
-	return 0.0f;
-}
-
-#endif

src/libraries/Box2D/Common/b2Timer.h

@@ -1,50 +0,0 @@
-/*
-* Copyright (c) 2011 Erin Catto http://box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_TIMER_H
-#define B2_TIMER_H
-
-#include <Box2D/Common/b2Settings.h>
-
-/// Timer for profiling. This has platform specific code and may
-/// not work on every platform.
-class b2Timer
-{
-public:
-
-	/// Constructor
-	b2Timer();
-
-	/// Reset the timer.
-	void Reset();
-
-	/// Get the time since construction or the last reset.
-	float32 GetMilliseconds() const;
-
-private:
-
-#if defined(_WIN32)
-	float64 m_start;
-	static float64 s_invFrequency;
-#elif defined(__linux__) || defined (__APPLE__)
-	unsigned long m_start_sec;
-	unsigned long m_start_usec;
-#endif
-};
-
-#endif

src/libraries/Box2D/Common/b2BlockAllocator.cpp → src/libraries/Box2D/Common/b2_block_allocator.cpp

@@ -1,28 +1,36 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Common/b2BlockAllocator.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_block_allocator.h"
 #include <limits.h>
 #include <string.h>
 #include <stddef.h>
-#include <cstring>
 
-int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
+static const int32 b2_chunkSize = 16 * 1024;
+static const int32 b2_maxBlockSize = 640;
+static const int32 b2_chunkArrayIncrement = 128;
+
+// These are the supported object sizes. Actual allocations are rounded up the next size.
+static const int32 b2_blockSizes[b2_blockSizeCount] =
 {
 	16,		// 0
 	32,		// 1
@@ -39,8 +47,33 @@ int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =
 	512,	// 12
 	640,	// 13
 };
-uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
-bool b2BlockAllocator::s_blockSizeLookupInitialized;
+
+// This maps an arbitrary allocation size to a suitable slot in b2_blockSizes.
+struct b2SizeMap
+{
+	b2SizeMap()
+	{
+		int32 j = 0;
+		values[0] = 0;
+		for (int32 i = 1; i <= b2_maxBlockSize; ++i)
+		{
+			b2Assert(j < b2_blockSizeCount);
+			if (i <= b2_blockSizes[j])
+			{
+				values[i] = (uint8)j;
+			}
+			else
+			{
+				++j;
+				values[i] = (uint8)j;
+			}
+		}
+	}
+
+	uint8 values[b2_maxBlockSize + 1];
+};
+
+static const b2SizeMap b2_sizeMap;
 
 struct b2Chunk
 {
@@ -55,7 +88,7 @@ struct b2Block
 
 b2BlockAllocator::b2BlockAllocator()
 {
-	b2Assert(b2_blockSizes < UCHAR_MAX);
+	b2Assert(b2_blockSizeCount < UCHAR_MAX);
 
 	m_chunkSpace = b2_chunkArrayIncrement;
 	m_chunkCount = 0;
@@ -63,26 +96,6 @@ b2BlockAllocator::b2BlockAllocator()
 	
 	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()
@@ -98,7 +111,9 @@ b2BlockAllocator::~b2BlockAllocator()
 void* b2BlockAllocator::Allocate(int32 size)
 {
 	if (size == 0)
-		return NULL;
+	{
+		return nullptr;
+	}
 
 	b2Assert(0 < size);
 
@@ -107,8 +122,8 @@ void* b2BlockAllocator::Allocate(int32 size)
 		return b2Alloc(size);
 	}
 
-	int32 index = s_blockSizeLookup[size];
-	b2Assert(0 <= index && index < b2_blockSizes);
+	int32 index = b2_sizeMap.values[size];
+	b2Assert(0 <= index && index < b2_blockSizeCount);
 
 	if (m_freeLists[index])
 	{
@@ -133,7 +148,7 @@ void* b2BlockAllocator::Allocate(int32 size)
 #if defined(_DEBUG)
 		memset(chunk->blocks, 0xcd, b2_chunkSize);
 #endif
-		int32 blockSize = s_blockSizes[index];
+		int32 blockSize = b2_blockSizes[index];
 		chunk->blockSize = blockSize;
 		int32 blockCount = b2_chunkSize / blockSize;
 		b2Assert(blockCount * blockSize <= b2_chunkSize);
@@ -144,7 +159,7 @@ void* b2BlockAllocator::Allocate(int32 size)
 			block->next = next;
 		}
 		b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
-		last->next = NULL;
+		last->next = nullptr;
 
 		m_freeLists[index] = chunk->blocks->next;
 		++m_chunkCount;
@@ -168,12 +183,12 @@ void b2BlockAllocator::Free(void* p, int32 size)
 		return;
 	}
 
-	int32 index = s_blockSizeLookup[size];
-	b2Assert(0 <= index && index < b2_blockSizes);
+	int32 index = b2_sizeMap.values[size];
+	b2Assert(0 <= index && index < b2_blockSizeCount);
 
-#ifdef _DEBUG
+#if defined(_DEBUG)
 	// Verify the memory address and size is valid.
-	int32 blockSize = s_blockSizes[index];
+	int32 blockSize = b2_blockSizes[index];
 	bool found = false;
 	for (int32 i = 0; i < m_chunkCount; ++i)
 	{
@@ -211,6 +226,5 @@ void b2BlockAllocator::Clear()
 
 	m_chunkCount = 0;
 	memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
-
 	memset(m_freeLists, 0, sizeof(m_freeLists));
 }

src/libraries/Box2D/Common/b2_draw.cpp

@@ -0,0 +1,47 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+#include "box2d/b2_draw.h"
+
+b2Draw::b2Draw()
+{
+	m_drawFlags = 0;
+}
+
+void b2Draw::SetFlags(uint32 flags)
+{
+	m_drawFlags = flags;
+}
+
+uint32 b2Draw::GetFlags() const
+{
+	return m_drawFlags;
+}
+
+void b2Draw::AppendFlags(uint32 flags)
+{
+	m_drawFlags |= flags;
+}
+
+void b2Draw::ClearFlags(uint32 flags)
+{
+	m_drawFlags &= ~flags;
+}

src/libraries/Box2D/Common/b2_math.cpp

@@ -0,0 +1,98 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_math.h"
+
+const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
+{
+	float det = b2Dot(ex, b2Cross(ey, ez));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	b2Vec3 x;
+	x.x = det * b2Dot(b, b2Cross(ey, ez));
+	x.y = det * b2Dot(ex, b2Cross(b, ez));
+	x.z = det * b2Dot(ex, b2Cross(ey, b));
+	return x;
+}
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
+{
+	float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
+	float det = a11 * a22 - a12 * a21;
+	if (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;
+}
+
+///
+void b2Mat33::GetInverse22(b2Mat33* M) const
+{
+	float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+	float det = a * d - b * c;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	M->ex.x =  det * d;	M->ey.x = -det * b; M->ex.z = 0.0f;
+	M->ex.y = -det * c;	M->ey.y =  det * a; M->ey.z = 0.0f;
+	M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
+}
+
+/// Returns the zero matrix if singular.
+void b2Mat33::GetSymInverse33(b2Mat33* M) const
+{
+	float det = b2Dot(ex, b2Cross(ey, ez));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	float a11 = ex.x, a12 = ey.x, a13 = ez.x;
+	float a22 = ey.y, a23 = ez.y;
+	float a33 = ez.z;
+
+	M->ex.x = det * (a22 * a33 - a23 * a23);
+	M->ex.y = det * (a13 * a23 - a12 * a33);
+	M->ex.z = det * (a12 * a23 - a13 * a22);
+
+	M->ey.x = M->ex.y;
+	M->ey.y = det * (a11 * a33 - a13 * a13);
+	M->ey.z = det * (a13 * a12 - a11 * a23);
+
+	M->ez.x = M->ex.z;
+	M->ez.y = M->ey.z;
+	M->ez.z = det * (a11 * a22 - a12 * a12);
+}

src/libraries/Box2D/Common/b2_settings.cpp

@@ -0,0 +1,77 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#define _CRT_SECURE_NO_WARNINGS
+
+#include "box2d/b2_settings.h"
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+b2Version b2_version = {2, 4, 0};
+
+// Memory allocators. Modify these to use your own allocator.
+void* b2Alloc(int32 size)
+{
+	return malloc(size);
+}
+
+void b2Free(void* mem)
+{
+	free(mem);
+}
+
+// You can modify this to use your logging facility.
+void b2Log(const char* string, ...)
+{
+	va_list args;
+	va_start(args, string);
+	vprintf(string, args);
+	va_end(args);
+}
+
+FILE* b2_dumpFile = nullptr;
+
+void b2OpenDump(const char* fileName)
+{
+	b2Assert(b2_dumpFile == nullptr);
+	b2_dumpFile = fopen(fileName, "w");
+}
+
+void b2Dump(const char* string, ...)
+{
+	if (b2_dumpFile == nullptr)
+	{
+		return;
+	}
+
+	va_list args;
+	va_start(args, string);
+	vfprintf(b2_dumpFile, string, args);
+	va_end(args);
+}
+
+void b2CloseDump()
+{
+	fclose(b2_dumpFile);
+	b2_dumpFile = nullptr;
+}

src/libraries/Box2D/Common/b2_stack_allocator.cpp

@@ -0,0 +1,87 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_stack_allocator.h"
+#include "box2d/b2_math.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 = nullptr;
+}
+
+int32 b2StackAllocator::GetMaxAllocation() const
+{
+	return m_maxAllocation;
+}

src/libraries/Box2D/Common/b2_timer.cpp

@@ -0,0 +1,125 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_timer.h"
+
+#if defined(_WIN32)
+
+double b2Timer::s_invFrequency = 0.0;
+
+#ifndef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN
+#endif
+
+#include <windows.h>
+
+b2Timer::b2Timer()
+{
+	LARGE_INTEGER largeInteger;
+
+	if (s_invFrequency == 0.0)
+	{
+		QueryPerformanceFrequency(&largeInteger);
+		s_invFrequency = double(largeInteger.QuadPart);
+		if (s_invFrequency > 0.0)
+		{
+			s_invFrequency = 1000.0 / s_invFrequency;
+		}
+	}
+
+	QueryPerformanceCounter(&largeInteger);
+	m_start = double(largeInteger.QuadPart);
+}
+
+void b2Timer::Reset()
+{
+	LARGE_INTEGER largeInteger;
+	QueryPerformanceCounter(&largeInteger);
+	m_start = double(largeInteger.QuadPart);
+}
+
+float b2Timer::GetMilliseconds() const
+{
+	LARGE_INTEGER largeInteger;
+	QueryPerformanceCounter(&largeInteger);
+	double count = double(largeInteger.QuadPart);
+	float ms = float(s_invFrequency * (count - m_start));
+	return ms;
+}
+
+#elif defined(__linux__) || defined (__APPLE__)
+
+#include <sys/time.h>
+
+b2Timer::b2Timer()
+{
+    Reset();
+}
+
+void b2Timer::Reset()
+{
+    timeval t;
+    gettimeofday(&t, 0);
+    m_start_sec = t.tv_sec;
+    m_start_usec = t.tv_usec;
+}
+
+float b2Timer::GetMilliseconds() const
+{
+    timeval t;
+    gettimeofday(&t, 0);
+	time_t start_sec = m_start_sec;
+	suseconds_t start_usec = m_start_usec;
+	
+	// http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
+	if (t.tv_usec < start_usec)
+	{
+		int nsec = (start_usec - t.tv_usec) / 1000000 + 1;
+		start_usec -= 1000000 * nsec;
+		start_sec += nsec;
+	}
+	
+	if (t.tv_usec - start_usec > 1000000)
+	{
+		int nsec = (t.tv_usec - start_usec) / 1000000;
+		start_usec += 1000000 * nsec;
+		start_sec -= nsec;
+	}
+	return 1000.0f * (t.tv_sec - start_sec) + 0.001f * (t.tv_usec - start_usec);
+}
+
+#else
+
+b2Timer::b2Timer()
+{
+}
+
+void b2Timer::Reset()
+{
+}
+
+float b2Timer::GetMilliseconds() const
+{
+	return 0.0f;
+}
+
+#endif

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp

@@ -1,53 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-
-#include <new>
-
-b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
-	return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
-}
-
-void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
-	allocator->Free(contact, sizeof(b2ChainAndCircleContact));
-}
-
-b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
-: b2Contact(fixtureA, indexA, fixtureB, indexB)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
-}
-
-void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
-	b2EdgeShape edge;
-	chain->GetChildEdge(&edge, m_indexA);
-	b2CollideEdgeAndCircle(	manifold, &edge, xfA,
-							(b2CircleShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_CHAIN_AND_CIRCLE_CONTACT_H
-#define B2_CHAIN_AND_CIRCLE_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2ChainAndCircleContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
-	~b2ChainAndCircleContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp

@@ -1,53 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-
-#include <new>
-
-b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
-	return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
-}
-
-void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
-	allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
-}
-
-b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
-: b2Contact(fixtureA, indexA, fixtureB, indexB)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
-}
-
-void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
-	b2EdgeShape edge;
-	chain->GetChildEdge(&edge, m_indexA);
-	b2CollideEdgeAndPolygon(	manifold, &edge, xfA,
-								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_CHAIN_AND_POLYGON_CONTACT_H
-#define B2_CHAIN_AND_POLYGON_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2ChainAndPolygonContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
-	~b2ChainAndPolygonContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp

@@ -1,52 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/Contacts/b2CircleContact.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-
-#include <new>
-
-b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2CircleContact));
-	return new (mem) b2CircleContact(fixtureA, fixtureB);
-}
-
-void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2CircleContact*)contact)->~b2CircleContact();
-	allocator->Free(contact, sizeof(b2CircleContact));
-}
-
-b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-	: b2Contact(fixtureA, 0, fixtureB, 0)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
-}
-
-void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2CollideCircles(manifold,
-					(b2CircleShape*)m_fixtureA->GetShape(), xfA,
-					(b2CircleShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_CONTACT_H
-#define B2_CIRCLE_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2CircleContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
-	~b2CircleContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h

@@ -1,95 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_CONTACT_SOLVER_H
-#define B2_CONTACT_SOLVER_H
-
-#include <Box2D/Common/b2Math.h>
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-
-class b2Contact;
-class b2Body;
-class b2StackAllocator;
-struct b2ContactPositionConstraint;
-
-struct b2VelocityConstraintPoint
-{
-	b2Vec2 rA;
-	b2Vec2 rB;
-	float32 normalImpulse;
-	float32 tangentImpulse;
-	float32 normalMass;
-	float32 tangentMass;
-	float32 velocityBias;
-};
-
-struct b2ContactVelocityConstraint
-{
-	b2VelocityConstraintPoint points[b2_maxManifoldPoints];
-	b2Vec2 normal;
-	b2Mat22 normalMass;
-	b2Mat22 K;
-	int32 indexA;
-	int32 indexB;
-	float32 invMassA, invMassB;
-	float32 invIA, invIB;
-	float32 friction;
-	float32 restitution;
-	float32 tangentSpeed;
-	int32 pointCount;
-	int32 contactIndex;
-};
-
-struct b2ContactSolverDef
-{
-	b2TimeStep step;
-	b2Contact** contacts;
-	int32 count;
-	b2Position* positions;
-	b2Velocity* velocities;
-	b2StackAllocator* allocator;
-};
-
-class b2ContactSolver
-{
-public:
-	b2ContactSolver(b2ContactSolverDef* def);
-	~b2ContactSolver();
-
-	void InitializeVelocityConstraints();
-
-	void WarmStart();
-	void SolveVelocityConstraints();
-	void StoreImpulses();
-
-	bool SolvePositionConstraints();
-	bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
-
-	b2TimeStep m_step;
-	b2Position* m_positions;
-	b2Velocity* m_velocities;
-	b2StackAllocator* m_allocator;
-	b2ContactPositionConstraint* m_positionConstraints;
-	b2ContactVelocityConstraint* m_velocityConstraints;
-	b2Contact** m_contacts;
-	int m_count;
-};
-
-#endif
-

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp

@@ -1,49 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-
-#include <new>
-
-b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
-	return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
-}
-
-void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
-	allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
-}
-
-b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-: b2Contact(fixtureA, 0, fixtureB, 0)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
-}
-
-void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2CollideEdgeAndCircle(	manifold,
-								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
-								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H
-#define B2_EDGE_AND_CIRCLE_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2EdgeAndCircleContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
-	~b2EdgeAndCircleContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp

@@ -1,49 +0,0 @@
-/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-
-#include <new>
-
-b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
-	return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
-}
-
-void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
-	allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
-}
-
-b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-: b2Contact(fixtureA, 0, fixtureB, 0)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
-}
-
-void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2CollideEdgeAndPolygon(	manifold,
-								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
-								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_EDGE_AND_POLYGON_CONTACT_H
-#define B2_EDGE_AND_POLYGON_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2EdgeAndPolygonContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
-	~b2EdgeAndPolygonContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp

@@ -1,49 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-
-#include <new>
-
-b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
-	return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
-}
-
-void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
-	allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
-}
-
-b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-: b2Contact(fixtureA, 0, fixtureB, 0)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
-}
-
-void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2CollidePolygonAndCircle(	manifold,
-								(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
-								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h

@@ -1,38 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_AND_CIRCLE_CONTACT_H
-#define B2_POLYGON_AND_CIRCLE_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2PolygonAndCircleContact : public b2Contact
-{
-public:
-	static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
-	~b2PolygonAndCircleContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp

@@ -1,52 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/Contacts/b2PolygonContact.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
-
-#include <new>
-
-b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
-	return new (mem) b2PolygonContact(fixtureA, fixtureB);
-}
-
-void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2PolygonContact*)contact)->~b2PolygonContact();
-	allocator->Free(contact, sizeof(b2PolygonContact));
-}
-
-b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-	: b2Contact(fixtureA, 0, fixtureB, 0)
-{
-	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
-	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
-}
-
-void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
-{
-	b2CollidePolygons(	manifold,
-						(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
-						(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
-}

src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h

@@ -1,39 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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_CONTACT_H
-#define B2_POLYGON_CONTACT_H
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-class b2BlockAllocator;
-
-class b2PolygonContact : public b2Contact
-{
-public:
-	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
-								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
-	~b2PolygonContact() {}
-
-	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h

@@ -1,169 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_DISTANCE_JOINT_H
-#define B2_DISTANCE_JOINT_H
-
-#include <Box2D/Dynamics/Joints/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;
-		localAnchorA.Set(0.0f, 0.0f);
-		localAnchorB.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* bodyA, b2Body* bodyB,
-					const b2Vec2& anchorA, const b2Vec2& anchorB);
-
-	/// The local anchor point relative to bodyA's origin.
-	b2Vec2 localAnchorA;
-
-	/// The local anchor point relative to bodyB's origin.
-	b2Vec2 localAnchorB;
-
-	/// The natural length between the anchor points.
-	float32 length;
-
-	/// The mass-spring-damper frequency in Hertz. A value of 0
-	/// disables softness.
-	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 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	/// Get the reaction force given the inverse time step.
-	/// Unit is N.
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-
-	/// Get the reaction torque given the inverse time step.
-	/// Unit is N*m. This is always zero for a distance joint.
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// The local anchor point relative to bodyA's origin.
-	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
-
-	/// The local anchor point relative to bodyB's origin.
-	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
-
-	/// Set/get the natural length.
-	/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
-	void SetLength(float32 length);
-	float32 GetLength() const;
-
-	/// Set/get frequency in Hz.
-	void SetFrequency(float32 hz);
-	float32 GetFrequency() const;
-
-	/// Set/get damping ratio.
-	void SetDampingRatio(float32 ratio);
-	float32 GetDampingRatio() const;
-
-	/// Dump joint to dmLog
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-	b2DistanceJoint(const b2DistanceJointDef* data);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	float32 m_frequencyHz;
-	float32 m_dampingRatio;
-	float32 m_bias;
-
-	// Solver shared
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-	float32 m_gamma;
-	float32 m_impulse;
-	float32 m_length;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_u;
-	b2Vec2 m_rA;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-	float32 m_mass;
-};
-
-inline void b2DistanceJoint::SetLength(float32 length)
-{
-	m_length = length;
-}
-
-inline float32 b2DistanceJoint::GetLength() const
-{
-	return m_length;
-}
-
-inline void b2DistanceJoint::SetFrequency(float32 hz)
-{
-	m_frequencyHz = hz;
-}
-
-inline float32 b2DistanceJoint::GetFrequency() const
-{
-	return m_frequencyHz;
-}
-
-inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
-{
-	m_dampingRatio = ratio;
-}
-
-inline float32 b2DistanceJoint::GetDampingRatio() const
-{
-	return m_dampingRatio;
-}
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h

@@ -1,119 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_FRICTION_JOINT_H
-#define B2_FRICTION_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Friction joint definition.
-struct b2FrictionJointDef : public b2JointDef
-{
-	b2FrictionJointDef()
-	{
-		type = e_frictionJoint;
-		localAnchorA.SetZero();
-		localAnchorB.SetZero();
-		maxForce = 0.0f;
-		maxTorque = 0.0f;
-	}
-
-	/// Initialize the bodies, anchors, axis, and reference angle using the world
-	/// anchor and world axis.
-	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
-
-	/// The local anchor point relative to bodyA's origin.
-	b2Vec2 localAnchorA;
-
-	/// The local anchor point relative to bodyB's origin.
-	b2Vec2 localAnchorB;
-
-	/// The maximum friction force in N.
-	float32 maxForce;
-
-	/// The maximum friction torque in N-m.
-	float32 maxTorque;
-};
-
-/// Friction joint. This is used for top-down friction.
-/// It provides 2D translational friction and angular friction.
-class b2FrictionJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// The local anchor point relative to bodyA's origin.
-	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
-
-	/// The local anchor point relative to bodyB's origin.
-	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
-
-	/// Set the maximum friction force in N.
-	void SetMaxForce(float32 force);
-
-	/// Get the maximum friction force in N.
-	float32 GetMaxForce() const;
-
-	/// Set the maximum friction torque in N*m.
-	void SetMaxTorque(float32 torque);
-
-	/// Get the maximum friction torque in N*m.
-	float32 GetMaxTorque() const;
-
-	/// Dump joint to dmLog
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-
-	b2FrictionJoint(const b2FrictionJointDef* def);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-
-	// Solver shared
-	b2Vec2 m_linearImpulse;
-	float32 m_angularImpulse;
-	float32 m_maxForce;
-	float32 m_maxTorque;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_rA;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-	b2Mat22 m_linearMass;
-	float32 m_angularMass;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h

@@ -1,125 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_GEAR_JOINT_H
-#define B2_GEAR_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Gear joint definition. This definition requires two existing
-/// revolute or prismatic joints (any combination will work).
-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 You have to manually destroy the gear joint if joint1 or joint2
-/// is destroyed.
-class b2GearJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// Get the first joint.
-	b2Joint* GetJoint1() { return m_joint1; }
-
-	/// Get the second joint.
-	b2Joint* GetJoint2() { return m_joint2; }
-
-	/// Set/Get the gear ratio.
-	void SetRatio(float32 ratio);
-	float32 GetRatio() const;
-
-	/// Dump joint to dmLog
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-	b2GearJoint(const b2GearJointDef* data);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	b2Joint* m_joint1;
-	b2Joint* m_joint2;
-
-	b2JointType m_typeA;
-	b2JointType m_typeB;
-
-	// Body A is connected to body C
-	// Body B is connected to body D
-	b2Body* m_bodyC;
-	b2Body* m_bodyD;
-
-	// Solver shared
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-	b2Vec2 m_localAnchorC;
-	b2Vec2 m_localAnchorD;
-
-	b2Vec2 m_localAxisC;
-	b2Vec2 m_localAxisD;
-
-	float32 m_referenceAngleA;
-	float32 m_referenceAngleB;
-
-	float32 m_constant;
-	float32 m_ratio;
-
-	float32 m_impulse;
-
-	// Solver temp
-	int32 m_indexA, m_indexB, m_indexC, m_indexD;
-	b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
-	float32 m_mA, m_mB, m_mC, m_mD;
-	float32 m_iA, m_iB, m_iC, m_iD;
-	b2Vec2 m_JvAC, m_JvBD;
-	float32 m_JwA, m_JwB, m_JwC, m_JwD;
-	float32 m_mass;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp

@@ -1,211 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
-#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
-#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
-#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
-#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
-#include <Box2D/Dynamics/Joints/b2GearJoint.h>
-#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
-#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
-#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
-#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-
-#include <new>
-
-b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
-{
-	b2Joint* joint = NULL;
-
-	switch (def->type)
-	{
-	case e_distanceJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
-			joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
-		}
-		break;
-
-	case e_mouseJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2MouseJoint));
-			joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
-		}
-		break;
-
-	case e_prismaticJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
-			joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
-		}
-		break;
-
-	case e_revoluteJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
-			joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
-		}
-		break;
-
-	case e_pulleyJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
-			joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
-		}
-		break;
-
-	case e_gearJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2GearJoint));
-			joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
-		}
-		break;
-
-	case e_wheelJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2WheelJoint));
-			joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
-		}
-		break;
-
-	case e_weldJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2WeldJoint));
-			joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
-		}
-		break;
-        
-	case e_frictionJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
-			joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
-		}
-		break;
-
-	case e_ropeJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2RopeJoint));
-			joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));
-		}
-		break;
-
-	case e_motorJoint:
-		{
-			void* mem = allocator->Allocate(sizeof(b2MotorJoint));
-			joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
-		}
-		break;
-
-	default:
-		b2Assert(false);
-		break;
-	}
-
-	return joint;
-}
-
-void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
-{
-	joint->~b2Joint();
-	switch (joint->m_type)
-	{
-	case e_distanceJoint:
-		allocator->Free(joint, sizeof(b2DistanceJoint));
-		break;
-
-	case e_mouseJoint:
-		allocator->Free(joint, sizeof(b2MouseJoint));
-		break;
-
-	case e_prismaticJoint:
-		allocator->Free(joint, sizeof(b2PrismaticJoint));
-		break;
-
-	case e_revoluteJoint:
-		allocator->Free(joint, sizeof(b2RevoluteJoint));
-		break;
-
-	case e_pulleyJoint:
-		allocator->Free(joint, sizeof(b2PulleyJoint));
-		break;
-
-	case e_gearJoint:
-		allocator->Free(joint, sizeof(b2GearJoint));
-		break;
-
-	case e_wheelJoint:
-		allocator->Free(joint, sizeof(b2WheelJoint));
-		break;
-    
-	case e_weldJoint:
-		allocator->Free(joint, sizeof(b2WeldJoint));
-		break;
-
-	case e_frictionJoint:
-		allocator->Free(joint, sizeof(b2FrictionJoint));
-		break;
-
-	case e_ropeJoint:
-		allocator->Free(joint, sizeof(b2RopeJoint));
-		break;
-
-	case e_motorJoint:
-		allocator->Free(joint, sizeof(b2MotorJoint));
-		break;
-
-	default:
-		b2Assert(false);
-		break;
-	}
-}
-
-b2Joint::b2Joint(const b2JointDef* def)
-{
-	b2Assert(def->bodyA != def->bodyB);
-
-	m_type = def->type;
-	m_prev = NULL;
-	m_next = NULL;
-	m_bodyA = def->bodyA;
-	m_bodyB = def->bodyB;
-	m_index = 0;
-	m_collideConnected = def->collideConnected;
-	m_islandFlag = false;
-	m_userData = def->userData;
-
-	m_edgeA.joint = NULL;
-	m_edgeA.other = NULL;
-	m_edgeA.prev = NULL;
-	m_edgeA.next = NULL;
-
-	m_edgeB.joint = NULL;
-	m_edgeB.other = NULL;
-	m_edgeB.prev = NULL;
-	m_edgeB.next = NULL;
-}
-
-bool b2Joint::IsActive() const
-{
-	return m_bodyA->IsActive() && m_bodyB->IsActive();
-}

src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h

@@ -1,133 +0,0 @@
-/*
-* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_MOTOR_JOINT_H
-#define B2_MOTOR_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Motor joint definition.
-struct b2MotorJointDef : public b2JointDef
-{
-	b2MotorJointDef()
-	{
-		type = e_motorJoint;
-		linearOffset.SetZero();
-		angularOffset = 0.0f;
-		maxForce = 1.0f;
-		maxTorque = 1.0f;
-		correctionFactor = 0.3f;
-	}
-
-	/// Initialize the bodies and offsets using the current transforms.
-	void Initialize(b2Body* bodyA, b2Body* bodyB);
-
-	/// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
-	b2Vec2 linearOffset;
-
-	/// The bodyB angle minus bodyA angle in radians.
-	float32 angularOffset;
-	
-	/// The maximum motor force in N.
-	float32 maxForce;
-
-	/// The maximum motor torque in N-m.
-	float32 maxTorque;
-
-	/// Position correction factor in the range [0,1].
-	float32 correctionFactor;
-};
-
-/// A motor joint is used to control the relative motion
-/// between two bodies. A typical usage is to control the movement
-/// of a dynamic body with respect to the ground.
-class b2MotorJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// Set/get the target linear offset, in frame A, in meters.
-	void SetLinearOffset(const b2Vec2& linearOffset);
-	const b2Vec2& GetLinearOffset() const;
-
-	/// Set/get the target angular offset, in radians.
-	void SetAngularOffset(float32 angularOffset);
-	float32 GetAngularOffset() const;
-
-	/// Set the maximum friction force in N.
-	void SetMaxForce(float32 force);
-
-	/// Get the maximum friction force in N.
-	float32 GetMaxForce() const;
-
-	/// Set the maximum friction torque in N*m.
-	void SetMaxTorque(float32 torque);
-
-	/// Get the maximum friction torque in N*m.
-	float32 GetMaxTorque() const;
-
-	/// Set the position correction factor in the range [0,1].
-	void SetCorrectionFactor(float32 factor);
-
-	/// Get the position correction factor in the range [0,1].
-	float32 GetCorrectionFactor() const;
-
-	/// Dump to b2Log
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-
-	b2MotorJoint(const b2MotorJointDef* def);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	// Solver shared
-	b2Vec2 m_linearOffset;
-	float32 m_angularOffset;
-	b2Vec2 m_linearImpulse;
-	float32 m_angularImpulse;
-	float32 m_maxForce;
-	float32 m_maxTorque;
-	float32 m_correctionFactor;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_rA;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	b2Vec2 m_linearError;
-	float32 m_angularError;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-	b2Mat22 m_linearMass;
-	float32 m_angularMass;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h

@@ -1,129 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_MOUSE_JOINT_H
-#define B2_MOUSE_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Mouse joint definition. This requires a world target point,
-/// tuning parameters, and the time step.
-struct b2MouseJointDef : public b2JointDef
-{
-	b2MouseJointDef()
-	{
-		type = e_mouseJoint;
-		target.Set(0.0f, 0.0f);
-		maxForce = 0.0f;
-		frequencyHz = 5.0f;
-		dampingRatio = 0.7f;
-	}
-
-	/// The initial world target point. This is assumed
-	/// to coincide with the body anchor initially.
-	b2Vec2 target;
-
-	/// The maximum constraint force that can be exerted
-	/// to move the candidate body. Usually you will express
-	/// as some multiple of the weight (multiplier * mass * gravity).
-	float32 maxForce;
-
-	/// The response speed.
-	float32 frequencyHz;
-
-	/// The damping ratio. 0 = no damping, 1 = critical damping.
-	float32 dampingRatio;
-};
-
-/// A mouse joint is used to make a point on a body track a
-/// specified world point. This a soft constraint with a maximum
-/// force. This allows the constraint to stretch and without
-/// applying huge forces.
-/// NOTE: this joint is not documented in the manual because it was
-/// developed to be used in the testbed. If you want to learn how to
-/// use the mouse joint, look at the testbed.
-class b2MouseJoint : public b2Joint
-{
-public:
-
-	/// Implements b2Joint.
-	b2Vec2 GetAnchorA() const;
-
-	/// Implements b2Joint.
-	b2Vec2 GetAnchorB() const;
-
-	/// Implements b2Joint.
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-
-	/// Implements b2Joint.
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// Use this to update the target point.
-	void SetTarget(const b2Vec2& target);
-	const b2Vec2& GetTarget() const;
-
-	/// Set/get the maximum force in Newtons.
-	void SetMaxForce(float32 force);
-	float32 GetMaxForce() const;
-
-	/// Set/get the frequency in Hertz.
-	void SetFrequency(float32 hz);
-	float32 GetFrequency() const;
-
-	/// Set/get the damping ratio (dimensionless).
-	void SetDampingRatio(float32 ratio);
-	float32 GetDampingRatio() const;
-
-	/// The mouse joint does not support dumping.
-	void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }
-
-	/// Implement b2Joint::ShiftOrigin
-	void ShiftOrigin(const b2Vec2& newOrigin);
-
-protected:
-	friend class b2Joint;
-
-	b2MouseJoint(const b2MouseJointDef* def);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	b2Vec2 m_localAnchorB;
-	b2Vec2 m_targetA;
-	float32 m_frequencyHz;
-	float32 m_dampingRatio;
-	float32 m_beta;
-	
-	// Solver shared
-	b2Vec2 m_impulse;
-	float32 m_maxForce;
-	float32 m_gamma;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassB;
-	float32 m_invIB;
-	b2Mat22 m_mass;
-	b2Vec2 m_C;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp

@@ -1,629 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-
-// Linear constraint (point-to-line)
-// d = p2 - p1 = x2 + r2 - x1 - r1
-// C = dot(perp, d)
-// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
-// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
-//
-// Angular constraint
-// C = a2 - a1 + a_initial
-// Cdot = w2 - w1
-// J = [0 0 -1 0 0 1]
-//
-// K = J * invM * JT
-//
-// J = [-a -s1 a s2]
-//     [0  -1  0  1]
-// a = perp
-// s1 = cross(d + r1, a) = cross(p2 - x1, a)
-// s2 = cross(r2, a) = cross(p2 - x2, a)
-
-
-// Motor/Limit linear constraint
-// C = dot(ax1, d)
-// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
-// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
-
-// Block Solver
-// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
-// when the mass has poor distribution (leading to large torques about the joint anchor points).
-//
-// The Jacobian has 3 rows:
-// J = [-uT -s1 uT s2] // linear
-//     [0   -1   0  1] // angular
-//     [-vT -a1 vT a2] // limit
-//
-// u = perp
-// v = axis
-// s1 = cross(d + r1, u), s2 = cross(r2, u)
-// a1 = cross(d + r1, v), a2 = cross(r2, v)
-
-// M * (v2 - v1) = JT * df
-// J * v2 = bias
-//
-// v2 = v1 + invM * JT * df
-// J * (v1 + invM * JT * df) = bias
-// K * df = bias - J * v1 = -Cdot
-// K = J * invM * JT
-// Cdot = J * v1 - bias
-//
-// Now solve for f2.
-// df = f2 - f1
-// K * (f2 - f1) = -Cdot
-// f2 = invK * (-Cdot) + f1
-//
-// Clamp accumulated limit impulse.
-// lower: f2(3) = max(f2(3), 0)
-// upper: f2(3) = min(f2(3), 0)
-//
-// Solve for correct f2(1:2)
-// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
-//                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
-// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
-// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-//
-// Now compute impulse to be applied:
-// df = f2 - f1
-
-void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
-{
-	bodyA = bA;
-	bodyB = bB;
-	localAnchorA = bodyA->GetLocalPoint(anchor);
-	localAnchorB = bodyB->GetLocalPoint(anchor);
-	localAxisA = bodyA->GetLocalVector(axis);
-	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
-}
-
-b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
-: b2Joint(def)
-{
-	m_localAnchorA = def->localAnchorA;
-	m_localAnchorB = def->localAnchorB;
-	m_localXAxisA = def->localAxisA;
-	m_localXAxisA.Normalize();
-	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
-	m_referenceAngle = def->referenceAngle;
-
-	m_impulse.SetZero();
-	m_motorMass = 0.0f;
-	m_motorImpulse = 0.0f;
-
-	m_lowerTranslation = def->lowerTranslation;
-	m_upperTranslation = def->upperTranslation;
-	m_maxMotorForce = def->maxMotorForce;
-	m_motorSpeed = def->motorSpeed;
-	m_enableLimit = def->enableLimit;
-	m_enableMotor = def->enableMotor;
-	m_limitState = e_inactiveLimit;
-
-	m_axis.SetZero();
-	m_perp.SetZero();
-}
-
-void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
-{
-	m_indexA = m_bodyA->m_islandIndex;
-	m_indexB = m_bodyB->m_islandIndex;
-	m_localCenterA = m_bodyA->m_sweep.localCenter;
-	m_localCenterB = m_bodyB->m_sweep.localCenter;
-	m_invMassA = m_bodyA->m_invMass;
-	m_invMassB = m_bodyB->m_invMass;
-	m_invIA = m_bodyA->m_invI;
-	m_invIB = m_bodyB->m_invI;
-
-	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-
-	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	b2Rot qA(aA), qB(aB);
-
-	// Compute the effective masses.
-	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-	b2Vec2 d = (cB - cA) + rB - rA;
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	// Compute motor Jacobian and effective mass.
-	{
-		m_axis = b2Mul(qA, m_localXAxisA);
-		m_a1 = b2Cross(d + rA, m_axis);
-		m_a2 = b2Cross(rB, m_axis);
-
-		m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
-		if (m_motorMass > 0.0f)
-		{
-			m_motorMass = 1.0f / m_motorMass;
-		}
-	}
-
-	// Prismatic constraint.
-	{
-		m_perp = b2Mul(qA, m_localYAxisA);
-
-		m_s1 = b2Cross(d + rA, m_perp);
-		m_s2 = b2Cross(rB, m_perp);
-
-        float32 s1test;
-        s1test = b2Cross(rA, m_perp);
-
-		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
-		float32 k12 = iA * m_s1 + iB * m_s2;
-		float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
-		float32 k22 = iA + iB;
-		if (k22 == 0.0f)
-		{
-			// For bodies with fixed rotation.
-			k22 = 1.0f;
-		}
-		float32 k23 = iA * m_a1 + iB * m_a2;
-		float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
-
-		m_K.ex.Set(k11, k12, k13);
-		m_K.ey.Set(k12, k22, k23);
-		m_K.ez.Set(k13, k23, k33);
-	}
-
-	// Compute motor and limit terms.
-	if (m_enableLimit)
-	{
-		float32 jointTranslation = b2Dot(m_axis, d);
-		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
-		{
-			m_limitState = e_equalLimits;
-		}
-		else if (jointTranslation <= m_lowerTranslation)
-		{
-			if (m_limitState != e_atLowerLimit)
-			{
-				m_limitState = e_atLowerLimit;
-				m_impulse.z = 0.0f;
-			}
-		}
-		else if (jointTranslation >= m_upperTranslation)
-		{
-			if (m_limitState != e_atUpperLimit)
-			{
-				m_limitState = e_atUpperLimit;
-				m_impulse.z = 0.0f;
-			}
-		}
-		else
-		{
-			m_limitState = e_inactiveLimit;
-			m_impulse.z = 0.0f;
-		}
-	}
-	else
-	{
-		m_limitState = e_inactiveLimit;
-		m_impulse.z = 0.0f;
-	}
-
-	if (m_enableMotor == false)
-	{
-		m_motorImpulse = 0.0f;
-	}
-
-	if (data.step.warmStarting)
-	{
-		// Account for variable time step.
-		m_impulse *= data.step.dtRatio;
-		m_motorImpulse *= data.step.dtRatio;
-
-		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
-		float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
-		float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-	else
-	{
-		m_impulse.SetZero();
-		m_motorImpulse = 0.0f;
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
-{
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	// Solve linear motor constraint.
-	if (m_enableMotor && m_limitState != e_equalLimits)
-	{
-		float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
-		float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
-		float32 oldImpulse = m_motorImpulse;
-		float32 maxImpulse = data.step.dt * m_maxMotorForce;
-		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
-		impulse = m_motorImpulse - oldImpulse;
-
-		b2Vec2 P = impulse * m_axis;
-		float32 LA = impulse * m_a1;
-		float32 LB = impulse * m_a2;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-
-	b2Vec2 Cdot1;
-	Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
-	Cdot1.y = wB - wA;
-
-	if (m_enableLimit && m_limitState != e_inactiveLimit)
-	{
-		// Solve prismatic and limit constraint in block form.
-		float32 Cdot2;
-		Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
-		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
-
-		b2Vec3 f1 = m_impulse;
-		b2Vec3 df =  m_K.Solve33(-Cdot);
-		m_impulse += df;
-
-		if (m_limitState == e_atLowerLimit)
-		{
-			m_impulse.z = b2Max(m_impulse.z, 0.0f);
-		}
-		else if (m_limitState == e_atUpperLimit)
-		{
-			m_impulse.z = b2Min(m_impulse.z, 0.0f);
-		}
-
-		// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-		b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
-		b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
-		m_impulse.x = f2r.x;
-		m_impulse.y = f2r.y;
-
-		df = m_impulse - f1;
-
-		b2Vec2 P = df.x * m_perp + df.z * m_axis;
-		float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
-		float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-	else
-	{
-		// Limit is inactive, just solve the prismatic constraint in block form.
-		b2Vec2 df = m_K.Solve22(-Cdot1);
-		m_impulse.x += df.x;
-		m_impulse.y += df.y;
-
-		b2Vec2 P = df.x * m_perp;
-		float32 LA = df.x * m_s1 + df.y;
-		float32 LB = df.x * m_s2 + df.y;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
-{
-	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
-
-	b2Rot qA(aA), qB(aB);
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	// Compute fresh Jacobians
-	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-	b2Vec2 d = cB + rB - cA - rA;
-
-	b2Vec2 axis = b2Mul(qA, m_localXAxisA);
-	float32 a1 = b2Cross(d + rA, axis);
-	float32 a2 = b2Cross(rB, axis);
-	b2Vec2 perp = b2Mul(qA, m_localYAxisA);
-
-	float32 s1 = b2Cross(d + rA, perp);
-	float32 s2 = b2Cross(rB, perp);
-
-	b2Vec3 impulse;
-	b2Vec2 C1;
-	C1.x = b2Dot(perp, d);
-	C1.y = aB - aA - m_referenceAngle;
-
-	float32 linearError = b2Abs(C1.x);
-	float32 angularError = b2Abs(C1.y);
-
-	bool active = false;
-	float32 C2 = 0.0f;
-	if (m_enableLimit)
-	{
-		float32 translation = b2Dot(axis, d);
-		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
-		{
-			// Prevent large angular corrections
-			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
-			linearError = b2Max(linearError, b2Abs(translation));
-			active = true;
-		}
-		else if (translation <= m_lowerTranslation)
-		{
-			// Prevent large linear corrections and allow some slop.
-			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-			linearError = b2Max(linearError, m_lowerTranslation - translation);
-			active = true;
-		}
-		else if (translation >= m_upperTranslation)
-		{
-			// Prevent large linear corrections and allow some slop.
-			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
-			linearError = b2Max(linearError, translation - m_upperTranslation);
-			active = true;
-		}
-	}
-
-	if (active)
-	{
-		float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
-		float32 k12 = iA * s1 + iB * s2;
-		float32 k13 = iA * s1 * a1 + iB * s2 * a2;
-		float32 k22 = iA + iB;
-		if (k22 == 0.0f)
-		{
-			// For fixed rotation
-			k22 = 1.0f;
-		}
-		float32 k23 = iA * a1 + iB * a2;
-		float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
-
-		b2Mat33 K;
-		K.ex.Set(k11, k12, k13);
-		K.ey.Set(k12, k22, k23);
-		K.ez.Set(k13, k23, k33);
-
-		b2Vec3 C;
-		C.x = C1.x;
-		C.y = C1.y;
-		C.z = C2;
-
-		impulse = K.Solve33(-C);
-	}
-	else
-	{
-		float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
-		float32 k12 = iA * s1 + iB * s2;
-		float32 k22 = iA + iB;
-		if (k22 == 0.0f)
-		{
-			k22 = 1.0f;
-		}
-
-		b2Mat22 K;
-		K.ex.Set(k11, k12);
-		K.ey.Set(k12, k22);
-
-		b2Vec2 impulse1 = K.Solve(-C1);
-		impulse.x = impulse1.x;
-		impulse.y = impulse1.y;
-		impulse.z = 0.0f;
-	}
-
-	b2Vec2 P = impulse.x * perp + impulse.z * axis;
-	float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
-	float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;
-
-	cA -= mA * P;
-	aA -= iA * LA;
-	cB += mB * P;
-	aB += iB * LB;
-
-	data.positions[m_indexA].c = cA;
-	data.positions[m_indexA].a = aA;
-	data.positions[m_indexB].c = cB;
-	data.positions[m_indexB].a = aB;
-
-	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
-}
-
-b2Vec2 b2PrismaticJoint::GetAnchorA() const
-{
-	return m_bodyA->GetWorldPoint(m_localAnchorA);
-}
-
-b2Vec2 b2PrismaticJoint::GetAnchorB() const
-{
-	return m_bodyB->GetWorldPoint(m_localAnchorB);
-}
-
-b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const
-{
-	return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis);
-}
-
-float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const
-{
-	return inv_dt * m_impulse.y;
-}
-
-float32 b2PrismaticJoint::GetJointTranslation() const
-{
-	b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
-	b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
-	b2Vec2 d = pB - pA;
-	b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);
-
-	float32 translation = b2Dot(d, axis);
-	return translation;
-}
-
-float32 b2PrismaticJoint::GetJointSpeed() const
-{
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
-
-	b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
-	b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
-	b2Vec2 p1 = bA->m_sweep.c + rA;
-	b2Vec2 p2 = bB->m_sweep.c + rB;
-	b2Vec2 d = p2 - p1;
-	b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
-
-	b2Vec2 vA = bA->m_linearVelocity;
-	b2Vec2 vB = bB->m_linearVelocity;
-	float32 wA = bA->m_angularVelocity;
-	float32 wB = bB->m_angularVelocity;
-
-	float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
-	return speed;
-}
-
-bool b2PrismaticJoint::IsLimitEnabled() const
-{
-	return m_enableLimit;
-}
-
-void b2PrismaticJoint::EnableLimit(bool flag)
-{
-	if (flag != m_enableLimit)
-	{
-		m_bodyA->SetAwake(true);
-		m_bodyB->SetAwake(true);
-		m_enableLimit = flag;
-		m_impulse.z = 0.0f;
-	}
-}
-
-float32 b2PrismaticJoint::GetLowerLimit() const
-{
-	return m_lowerTranslation;
-}
-
-float32 b2PrismaticJoint::GetUpperLimit() const
-{
-	return m_upperTranslation;
-}
-
-void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
-{
-	b2Assert(lower <= upper);
-	if (lower != m_lowerTranslation || upper != m_upperTranslation)
-	{
-		m_bodyA->SetAwake(true);
-		m_bodyB->SetAwake(true);
-		m_lowerTranslation = lower;
-		m_upperTranslation = upper;
-		m_impulse.z = 0.0f;
-	}
-}
-
-bool b2PrismaticJoint::IsMotorEnabled() const
-{
-	return m_enableMotor;
-}
-
-void b2PrismaticJoint::EnableMotor(bool flag)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_enableMotor = flag;
-}
-
-void b2PrismaticJoint::SetMotorSpeed(float32 speed)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_motorSpeed = speed;
-}
-
-void b2PrismaticJoint::SetMaxMotorForce(float32 force)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_maxMotorForce = force;
-}
-
-float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const
-{
-	return inv_dt * m_motorImpulse;
-}
-
-void b2PrismaticJoint::Dump()
-{
-	int32 indexA = m_bodyA->m_islandIndex;
-	int32 indexB = m_bodyB->m_islandIndex;
-
-	b2Log("  b2PrismaticJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
-	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
-	b2Log("  jd.enableLimit = bool(%d);\n", m_enableLimit);
-	b2Log("  jd.lowerTranslation = %.15lef;\n", m_lowerTranslation);
-	b2Log("  jd.upperTranslation = %.15lef;\n", m_upperTranslation);
-	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
-	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
-	b2Log("  jd.maxMotorForce = %.15lef;\n", m_maxMotorForce);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
-}

src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp

@@ -1,502 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-
-// Point-to-point constraint
-// C = p2 - p1
-// Cdot = v2 - v1
-//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
-// J = [-I -r1_skew I r2_skew ]
-// Identity used:
-// w k % (rx i + ry j) = w * (-ry i + rx j)
-
-// Motor constraint
-// Cdot = w2 - w1
-// J = [0 0 -1 0 0 1]
-// K = invI1 + invI2
-
-void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
-{
-	bodyA = bA;
-	bodyB = bB;
-	localAnchorA = bodyA->GetLocalPoint(anchor);
-	localAnchorB = bodyB->GetLocalPoint(anchor);
-	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
-}
-
-b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
-: b2Joint(def)
-{
-	m_localAnchorA = def->localAnchorA;
-	m_localAnchorB = def->localAnchorB;
-	m_referenceAngle = def->referenceAngle;
-
-	m_impulse.SetZero();
-	m_motorImpulse = 0.0f;
-
-	m_lowerAngle = def->lowerAngle;
-	m_upperAngle = def->upperAngle;
-	m_maxMotorTorque = def->maxMotorTorque;
-	m_motorSpeed = def->motorSpeed;
-	m_enableLimit = def->enableLimit;
-	m_enableMotor = def->enableMotor;
-	m_limitState = e_inactiveLimit;
-}
-
-void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
-{
-	m_indexA = m_bodyA->m_islandIndex;
-	m_indexB = m_bodyB->m_islandIndex;
-	m_localCenterA = m_bodyA->m_sweep.localCenter;
-	m_localCenterB = m_bodyB->m_sweep.localCenter;
-	m_invMassA = m_bodyA->m_invMass;
-	m_invMassB = m_bodyB->m_invMass;
-	m_invIA = m_bodyA->m_invI;
-	m_invIB = m_bodyB->m_invI;
-
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-
-	float32 aB = data.positions[m_indexB].a;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	b2Rot qA(aA), qB(aB);
-
-	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-
-	// J = [-I -r1_skew I r2_skew]
-	//     [ 0       -1 0       1]
-	// r_skew = [-ry; rx]
-
-	// Matlab
-	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
-	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
-	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	bool fixedRotation = (iA + iB == 0.0f);
-
-	m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
-	m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
-	m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
-	m_mass.ex.y = m_mass.ey.x;
-	m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
-	m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
-	m_mass.ex.z = m_mass.ez.x;
-	m_mass.ey.z = m_mass.ez.y;
-	m_mass.ez.z = iA + iB;
-
-	m_motorMass = iA + iB;
-	if (m_motorMass > 0.0f)
-	{
-		m_motorMass = 1.0f / m_motorMass;
-	}
-
-	if (m_enableMotor == false || fixedRotation)
-	{
-		m_motorImpulse = 0.0f;
-	}
-
-	if (m_enableLimit && fixedRotation == false)
-	{
-		float32 jointAngle = aB - aA - m_referenceAngle;
-		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
-		{
-			m_limitState = e_equalLimits;
-		}
-		else if (jointAngle <= m_lowerAngle)
-		{
-			if (m_limitState != e_atLowerLimit)
-			{
-				m_impulse.z = 0.0f;
-			}
-			m_limitState = e_atLowerLimit;
-		}
-		else if (jointAngle >= m_upperAngle)
-		{
-			if (m_limitState != e_atUpperLimit)
-			{
-				m_impulse.z = 0.0f;
-			}
-			m_limitState = e_atUpperLimit;
-		}
-		else
-		{
-			m_limitState = e_inactiveLimit;
-			m_impulse.z = 0.0f;
-		}
-	}
-	else
-	{
-		m_limitState = e_inactiveLimit;
-	}
-
-	if (data.step.warmStarting)
-	{
-		// Scale impulses to support a variable time step.
-		m_impulse *= data.step.dtRatio;
-		m_motorImpulse *= data.step.dtRatio;
-
-		b2Vec2 P(m_impulse.x, m_impulse.y);
-
-		vA -= mA * P;
-		wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
-
-		vB += mB * P;
-		wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
-	}
-	else
-	{
-		m_impulse.SetZero();
-		m_motorImpulse = 0.0f;
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
-{
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	bool fixedRotation = (iA + iB == 0.0f);
-
-	// Solve motor constraint.
-	if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
-	{
-		float32 Cdot = wB - wA - m_motorSpeed;
-		float32 impulse = -m_motorMass * Cdot;
-		float32 oldImpulse = m_motorImpulse;
-		float32 maxImpulse = data.step.dt * m_maxMotorTorque;
-		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
-		impulse = m_motorImpulse - oldImpulse;
-
-		wA -= iA * impulse;
-		wB += iB * impulse;
-	}
-
-	// Solve limit constraint.
-	if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
-	{
-		b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
-		float32 Cdot2 = wB - wA;
-		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
-
-		b2Vec3 impulse = -m_mass.Solve33(Cdot);
-
-		if (m_limitState == e_equalLimits)
-		{
-			m_impulse += impulse;
-		}
-		else if (m_limitState == e_atLowerLimit)
-		{
-			float32 newImpulse = m_impulse.z + impulse.z;
-			if (newImpulse < 0.0f)
-			{
-				b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
-				b2Vec2 reduced = m_mass.Solve22(rhs);
-				impulse.x = reduced.x;
-				impulse.y = reduced.y;
-				impulse.z = -m_impulse.z;
-				m_impulse.x += reduced.x;
-				m_impulse.y += reduced.y;
-				m_impulse.z = 0.0f;
-			}
-			else
-			{
-				m_impulse += impulse;
-			}
-		}
-		else if (m_limitState == e_atUpperLimit)
-		{
-			float32 newImpulse = m_impulse.z + impulse.z;
-			if (newImpulse > 0.0f)
-			{
-				b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
-				b2Vec2 reduced = m_mass.Solve22(rhs);
-				impulse.x = reduced.x;
-				impulse.y = reduced.y;
-				impulse.z = -m_impulse.z;
-				m_impulse.x += reduced.x;
-				m_impulse.y += reduced.y;
-				m_impulse.z = 0.0f;
-			}
-			else
-			{
-				m_impulse += impulse;
-			}
-		}
-
-		b2Vec2 P(impulse.x, impulse.y);
-
-		vA -= mA * P;
-		wA -= iA * (b2Cross(m_rA, P) + impulse.z);
-
-		vB += mB * P;
-		wB += iB * (b2Cross(m_rB, P) + impulse.z);
-	}
-	else
-	{
-		// Solve point-to-point constraint
-		b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
-		b2Vec2 impulse = m_mass.Solve22(-Cdot);
-
-		m_impulse.x += impulse.x;
-		m_impulse.y += impulse.y;
-
-		vA -= mA * impulse;
-		wA -= iA * b2Cross(m_rA, impulse);
-
-		vB += mB * impulse;
-		wB += iB * b2Cross(m_rB, impulse);
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
-{
-	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
-
-	b2Rot qA(aA), qB(aB);
-
-	float32 angularError = 0.0f;
-	float32 positionError = 0.0f;
-
-	bool fixedRotation = (m_invIA + m_invIB == 0.0f);
-
-	// Solve angular limit constraint.
-	if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
-	{
-		float32 angle = aB - aA - m_referenceAngle;
-		float32 limitImpulse = 0.0f;
-
-		if (m_limitState == e_equalLimits)
-		{
-			// Prevent large angular corrections
-			float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
-			limitImpulse = -m_motorMass * C;
-			angularError = b2Abs(C);
-		}
-		else if (m_limitState == e_atLowerLimit)
-		{
-			float32 C = angle - m_lowerAngle;
-			angularError = -C;
-
-			// Prevent large angular corrections and allow some slop.
-			C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
-			limitImpulse = -m_motorMass * C;
-		}
-		else if (m_limitState == e_atUpperLimit)
-		{
-			float32 C = angle - m_upperAngle;
-			angularError = C;
-
-			// Prevent large angular corrections and allow some slop.
-			C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
-			limitImpulse = -m_motorMass * C;
-		}
-
-		aA -= m_invIA * limitImpulse;
-		aB += m_invIB * limitImpulse;
-	}
-
-	// Solve point-to-point constraint.
-	{
-		qA.Set(aA);
-		qB.Set(aB);
-		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-
-		b2Vec2 C = cB + rB - cA - rA;
-		positionError = C.Length();
-
-		float32 mA = m_invMassA, mB = m_invMassB;
-		float32 iA = m_invIA, iB = m_invIB;
-
-		b2Mat22 K;
-		K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
-		K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
-		K.ey.x = K.ex.y;
-		K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
-
-		b2Vec2 impulse = -K.Solve(C);
-
-		cA -= mA * impulse;
-		aA -= iA * b2Cross(rA, impulse);
-
-		cB += mB * impulse;
-		aB += iB * b2Cross(rB, impulse);
-	}
-
-	data.positions[m_indexA].c = cA;
-	data.positions[m_indexA].a = aA;
-	data.positions[m_indexB].c = cB;
-	data.positions[m_indexB].a = aB;
-	
-	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
-}
-
-b2Vec2 b2RevoluteJoint::GetAnchorA() const
-{
-	return m_bodyA->GetWorldPoint(m_localAnchorA);
-}
-
-b2Vec2 b2RevoluteJoint::GetAnchorB() const
-{
-	return m_bodyB->GetWorldPoint(m_localAnchorB);
-}
-
-b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
-{
-	b2Vec2 P(m_impulse.x, m_impulse.y);
-	return inv_dt * P;
-}
-
-float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
-{
-	return inv_dt * m_impulse.z;
-}
-
-float32 b2RevoluteJoint::GetJointAngle() const
-{
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
-	return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
-}
-
-float32 b2RevoluteJoint::GetJointSpeed() const
-{
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
-	return bB->m_angularVelocity - bA->m_angularVelocity;
-}
-
-bool b2RevoluteJoint::IsMotorEnabled() const
-{
-	return m_enableMotor;
-}
-
-void b2RevoluteJoint::EnableMotor(bool flag)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_enableMotor = flag;
-}
-
-float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
-{
-	return inv_dt * m_motorImpulse;
-}
-
-void b2RevoluteJoint::SetMotorSpeed(float32 speed)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_motorSpeed = speed;
-}
-
-void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_maxMotorTorque = torque;
-}
-
-bool b2RevoluteJoint::IsLimitEnabled() const
-{
-	return m_enableLimit;
-}
-
-void b2RevoluteJoint::EnableLimit(bool flag)
-{
-	if (flag != m_enableLimit)
-	{
-		m_bodyA->SetAwake(true);
-		m_bodyB->SetAwake(true);
-		m_enableLimit = flag;
-		m_impulse.z = 0.0f;
-	}
-}
-
-float32 b2RevoluteJoint::GetLowerLimit() const
-{
-	return m_lowerAngle;
-}
-
-float32 b2RevoluteJoint::GetUpperLimit() const
-{
-	return m_upperAngle;
-}
-
-void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
-{
-	b2Assert(lower <= upper);
-	
-	if (lower != m_lowerAngle || upper != m_upperAngle)
-	{
-		m_bodyA->SetAwake(true);
-		m_bodyB->SetAwake(true);
-		m_impulse.z = 0.0f;
-		m_lowerAngle = lower;
-		m_upperAngle = upper;
-	}
-}
-
-void b2RevoluteJoint::Dump()
-{
-	int32 indexA = m_bodyA->m_islandIndex;
-	int32 indexB = m_bodyB->m_islandIndex;
-
-	b2Log("  b2RevoluteJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
-	b2Log("  jd.enableLimit = bool(%d);\n", m_enableLimit);
-	b2Log("  jd.lowerAngle = %.15lef;\n", m_lowerAngle);
-	b2Log("  jd.upperAngle = %.15lef;\n", m_upperAngle);
-	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
-	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
-	b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
-}

src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h

@@ -1,114 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_ROPE_JOINT_H
-#define B2_ROPE_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Rope joint definition. This requires two body anchor points and
-/// a maximum lengths.
-/// Note: by default the connected objects will not collide.
-/// see collideConnected in b2JointDef.
-struct b2RopeJointDef : public b2JointDef
-{
-	b2RopeJointDef()
-	{
-		type = e_ropeJoint;
-		localAnchorA.Set(-1.0f, 0.0f);
-		localAnchorB.Set(1.0f, 0.0f);
-		maxLength = 0.0f;
-	}
-
-	/// The local anchor point relative to bodyA's origin.
-	b2Vec2 localAnchorA;
-
-	/// The local anchor point relative to bodyB's origin.
-	b2Vec2 localAnchorB;
-
-	/// The maximum length of the rope.
-	/// Warning: this must be larger than b2_linearSlop or
-	/// the joint will have no effect.
-	float32 maxLength;
-};
-
-/// A rope joint enforces a maximum distance between two points
-/// on two bodies. It has no other effect.
-/// Warning: if you attempt to change the maximum length during
-/// the simulation you will get some non-physical behavior.
-/// A model that would allow you to dynamically modify the length
-/// would have some sponginess, so I chose not to implement it
-/// that way. See b2DistanceJoint if you want to dynamically
-/// control length.
-class b2RopeJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// The local anchor point relative to bodyA's origin.
-	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
-
-	/// The local anchor point relative to bodyB's origin.
-	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
-
-	/// Set/Get the maximum length of the rope.
-	void SetMaxLength(float32 length) { m_maxLength = length; }
-	float32 GetMaxLength() const;
-
-	b2LimitState GetLimitState() const;
-
-	/// Dump joint to dmLog
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-	b2RopeJoint(const b2RopeJointDef* data);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	// Solver shared
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-	float32 m_maxLength;
-	float32 m_length;
-	float32 m_impulse;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_u;
-	b2Vec2 m_rA;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-	float32 m_mass;
-	b2LimitState m_state;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h

@@ -1,126 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_WELD_JOINT_H
-#define B2_WELD_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Weld joint definition. You need to specify local anchor points
-/// where they are attached and the relative body angle. The position
-/// of the anchor points is important for computing the reaction torque.
-struct b2WeldJointDef : public b2JointDef
-{
-	b2WeldJointDef()
-	{
-		type = e_weldJoint;
-		localAnchorA.Set(0.0f, 0.0f);
-		localAnchorB.Set(0.0f, 0.0f);
-		referenceAngle = 0.0f;
-		frequencyHz = 0.0f;
-		dampingRatio = 0.0f;
-	}
-
-	/// Initialize the bodies, anchors, and reference angle using a world
-	/// anchor point.
-	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
-
-	/// The local anchor point relative to bodyA's origin.
-	b2Vec2 localAnchorA;
-
-	/// The local anchor point relative to bodyB's origin.
-	b2Vec2 localAnchorB;
-
-	/// The bodyB angle minus bodyA angle in the reference state (radians).
-	float32 referenceAngle;
-	
-	/// The mass-spring-damper frequency in Hertz. Rotation only.
-	/// Disable softness with a value of 0.
-	float32 frequencyHz;
-
-	/// The damping ratio. 0 = no damping, 1 = critical damping.
-	float32 dampingRatio;
-};
-
-/// A weld joint essentially glues two bodies together. A weld joint may
-/// distort somewhat because the island constraint solver is approximate.
-class b2WeldJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// The local anchor point relative to bodyA's origin.
-	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
-
-	/// The local anchor point relative to bodyB's origin.
-	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
-
-	/// Get the reference angle.
-	float32 GetReferenceAngle() const { return m_referenceAngle; }
-
-	/// Set/get frequency in Hz.
-	void SetFrequency(float32 hz) { m_frequencyHz = hz; }
-	float32 GetFrequency() const { return m_frequencyHz; }
-
-	/// Set/get damping ratio.
-	void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; }
-	float32 GetDampingRatio() const { return m_dampingRatio; }
-
-	/// Dump to b2Log
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-
-	b2WeldJoint(const b2WeldJointDef* def);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	float32 m_frequencyHz;
-	float32 m_dampingRatio;
-	float32 m_bias;
-
-	// Solver shared
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-	float32 m_referenceAngle;
-	float32 m_gamma;
-	b2Vec3 m_impulse;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_rA;
-	b2Vec2 m_rB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-	b2Mat33 m_mass;
-};
-
-#endif

src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp

@@ -1,419 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-
-// Linear constraint (point-to-line)
-// d = pB - pA = xB + rB - xA - rA
-// C = dot(ay, d)
-// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
-//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
-// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
-
-// Spring linear constraint
-// C = dot(ax, d)
-// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
-// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
-
-// Motor rotational constraint
-// Cdot = wB - wA
-// J = [0 0 -1 0 0 1]
-
-void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
-{
-	bodyA = bA;
-	bodyB = bB;
-	localAnchorA = bodyA->GetLocalPoint(anchor);
-	localAnchorB = bodyB->GetLocalPoint(anchor);
-	localAxisA = bodyA->GetLocalVector(axis);
-}
-
-b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
-: b2Joint(def)
-{
-	m_localAnchorA = def->localAnchorA;
-	m_localAnchorB = def->localAnchorB;
-	m_localXAxisA = def->localAxisA;
-	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
-
-	m_mass = 0.0f;
-	m_impulse = 0.0f;
-	m_motorMass = 0.0f;
-	m_motorImpulse = 0.0f;
-	m_springMass = 0.0f;
-	m_springImpulse = 0.0f;
-
-	m_maxMotorTorque = def->maxMotorTorque;
-	m_motorSpeed = def->motorSpeed;
-	m_enableMotor = def->enableMotor;
-
-	m_frequencyHz = def->frequencyHz;
-	m_dampingRatio = def->dampingRatio;
-
-	m_bias = 0.0f;
-	m_gamma = 0.0f;
-
-	m_ax.SetZero();
-	m_ay.SetZero();
-}
-
-void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)
-{
-	m_indexA = m_bodyA->m_islandIndex;
-	m_indexB = m_bodyB->m_islandIndex;
-	m_localCenterA = m_bodyA->m_sweep.localCenter;
-	m_localCenterB = m_bodyB->m_sweep.localCenter;
-	m_invMassA = m_bodyA->m_invMass;
-	m_invMassB = m_bodyB->m_invMass;
-	m_invIA = m_bodyA->m_invI;
-	m_invIB = m_bodyB->m_invI;
-
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-
-	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	b2Rot qA(aA), qB(aB);
-
-	// Compute the effective masses.
-	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-	b2Vec2 d = cB + rB - cA - rA;
-
-	// Point to line constraint
-	{
-		m_ay = b2Mul(qA, m_localYAxisA);
-		m_sAy = b2Cross(d + rA, m_ay);
-		m_sBy = b2Cross(rB, m_ay);
-
-		m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
-
-		if (m_mass > 0.0f)
-		{
-			m_mass = 1.0f / m_mass;
-		}
-	}
-
-	// Spring constraint
-	m_springMass = 0.0f;
-	m_bias = 0.0f;
-	m_gamma = 0.0f;
-	if (m_frequencyHz > 0.0f)
-	{
-		m_ax = b2Mul(qA, m_localXAxisA);
-		m_sAx = b2Cross(d + rA, m_ax);
-		m_sBx = b2Cross(rB, m_ax);
-
-		float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
-
-		if (invMass > 0.0f)
-		{
-			m_springMass = 1.0f / invMass;
-
-			float32 C = b2Dot(d, m_ax);
-
-			// Frequency
-			float32 omega = 2.0f * b2_pi * m_frequencyHz;
-
-			// Damping coefficient
-			float32 d = 2.0f * m_springMass * m_dampingRatio * omega;
-
-			// Spring stiffness
-			float32 k = m_springMass * omega * omega;
-
-			// magic formulas
-			float32 h = data.step.dt;
-			m_gamma = h * (d + h * k);
-			if (m_gamma > 0.0f)
-			{
-				m_gamma = 1.0f / m_gamma;
-			}
-
-			m_bias = C * h * k * m_gamma;
-
-			m_springMass = invMass + m_gamma;
-			if (m_springMass > 0.0f)
-			{
-				m_springMass = 1.0f / m_springMass;
-			}
-		}
-	}
-	else
-	{
-		m_springImpulse = 0.0f;
-	}
-
-	// Rotational motor
-	if (m_enableMotor)
-	{
-		m_motorMass = iA + iB;
-		if (m_motorMass > 0.0f)
-		{
-			m_motorMass = 1.0f / m_motorMass;
-		}
-	}
-	else
-	{
-		m_motorMass = 0.0f;
-		m_motorImpulse = 0.0f;
-	}
-
-	if (data.step.warmStarting)
-	{
-		// Account for variable time step.
-		m_impulse *= data.step.dtRatio;
-		m_springImpulse *= data.step.dtRatio;
-		m_motorImpulse *= data.step.dtRatio;
-
-		b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
-		float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
-		float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
-
-		vA -= m_invMassA * P;
-		wA -= m_invIA * LA;
-
-		vB += m_invMassB * P;
-		wB += m_invIB * LB;
-	}
-	else
-	{
-		m_impulse = 0.0f;
-		m_springImpulse = 0.0f;
-		m_motorImpulse = 0.0f;
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
-{
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
-
-	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
-	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
-
-	// Solve spring constraint
-	{
-		float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
-		float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
-		m_springImpulse += impulse;
-
-		b2Vec2 P = impulse * m_ax;
-		float32 LA = impulse * m_sAx;
-		float32 LB = impulse * m_sBx;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-
-	// Solve rotational motor constraint
-	{
-		float32 Cdot = wB - wA - m_motorSpeed;
-		float32 impulse = -m_motorMass * Cdot;
-
-		float32 oldImpulse = m_motorImpulse;
-		float32 maxImpulse = data.step.dt * m_maxMotorTorque;
-		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
-		impulse = m_motorImpulse - oldImpulse;
-
-		wA -= iA * impulse;
-		wB += iB * impulse;
-	}
-
-	// Solve point to line constraint
-	{
-		float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
-		float32 impulse = -m_mass * Cdot;
-		m_impulse += impulse;
-
-		b2Vec2 P = impulse * m_ay;
-		float32 LA = impulse * m_sAy;
-		float32 LB = impulse * m_sBy;
-
-		vA -= mA * P;
-		wA -= iA * LA;
-
-		vB += mB * P;
-		wB += iB * LB;
-	}
-
-	data.velocities[m_indexA].v = vA;
-	data.velocities[m_indexA].w = wA;
-	data.velocities[m_indexB].v = vB;
-	data.velocities[m_indexB].w = wB;
-}
-
-bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
-{
-	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
-	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
-
-	b2Rot qA(aA), qB(aB);
-
-	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
-	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
-	b2Vec2 d = (cB - cA) + rB - rA;
-
-	b2Vec2 ay = b2Mul(qA, m_localYAxisA);
-
-	float32 sAy = b2Cross(d + rA, ay);
-	float32 sBy = b2Cross(rB, ay);
-
-	float32 C = b2Dot(d, ay);
-
-	float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
-
-	float32 impulse;
-	if (k != 0.0f)
-	{
-		impulse = - C / k;
-	}
-	else
-	{
-		impulse = 0.0f;
-	}
-
-	b2Vec2 P = impulse * ay;
-	float32 LA = impulse * sAy;
-	float32 LB = impulse * sBy;
-
-	cA -= m_invMassA * P;
-	aA -= m_invIA * LA;
-	cB += m_invMassB * P;
-	aB += m_invIB * LB;
-
-	data.positions[m_indexA].c = cA;
-	data.positions[m_indexA].a = aA;
-	data.positions[m_indexB].c = cB;
-	data.positions[m_indexB].a = aB;
-
-	return b2Abs(C) <= b2_linearSlop;
-}
-
-b2Vec2 b2WheelJoint::GetAnchorA() const
-{
-	return m_bodyA->GetWorldPoint(m_localAnchorA);
-}
-
-b2Vec2 b2WheelJoint::GetAnchorB() const
-{
-	return m_bodyB->GetWorldPoint(m_localAnchorB);
-}
-
-b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
-{
-	return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
-}
-
-float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
-{
-	return inv_dt * m_motorImpulse;
-}
-
-float32 b2WheelJoint::GetJointTranslation() const
-{
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
-
-	b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
-	b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
-	b2Vec2 d = pB - pA;
-	b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
-
-	float32 translation = b2Dot(d, axis);
-	return translation;
-}
-
-float32 b2WheelJoint::GetJointSpeed() const
-{
-	float32 wA = m_bodyA->m_angularVelocity;
-	float32 wB = m_bodyB->m_angularVelocity;
-	return wB - wA;
-}
-
-bool b2WheelJoint::IsMotorEnabled() const
-{
-	return m_enableMotor;
-}
-
-void b2WheelJoint::EnableMotor(bool flag)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_enableMotor = flag;
-}
-
-void b2WheelJoint::SetMotorSpeed(float32 speed)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_motorSpeed = speed;
-}
-
-void b2WheelJoint::SetMaxMotorTorque(float32 torque)
-{
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_maxMotorTorque = torque;
-}
-
-float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
-{
-	return inv_dt * m_motorImpulse;
-}
-
-void b2WheelJoint::Dump()
-{
-	int32 indexA = m_bodyA->m_islandIndex;
-	int32 indexB = m_bodyB->m_islandIndex;
-
-	b2Log("  b2WheelJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
-	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
-	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
-	b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
-	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
-	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
-}

src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h

@@ -1,210 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_WHEEL_JOINT_H
-#define B2_WHEEL_JOINT_H
-
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-
-/// Wheel joint definition. This requires defining a line of
-/// motion using an axis and an anchor point. The definition uses local
-/// anchor points and a local axis so that the initial configuration
-/// can violate the constraint slightly. The joint translation is zero
-/// when the local anchor points coincide in world space. Using local
-/// anchors and a local axis helps when saving and loading a game.
-struct b2WheelJointDef : public b2JointDef
-{
-	b2WheelJointDef()
-	{
-		type = e_wheelJoint;
-		localAnchorA.SetZero();
-		localAnchorB.SetZero();
-		localAxisA.Set(1.0f, 0.0f);
-		enableMotor = false;
-		maxMotorTorque = 0.0f;
-		motorSpeed = 0.0f;
-		frequencyHz = 2.0f;
-		dampingRatio = 0.7f;
-	}
-
-	/// Initialize the bodies, anchors, axis, and reference angle using the world
-	/// anchor and world axis.
-	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
-
-	/// The local anchor point relative to bodyA's origin.
-	b2Vec2 localAnchorA;
-
-	/// The local anchor point relative to bodyB's origin.
-	b2Vec2 localAnchorB;
-
-	/// The local translation axis in bodyA.
-	b2Vec2 localAxisA;
-
-	/// Enable/disable the joint motor.
-	bool enableMotor;
-
-	/// The maximum motor torque, usually in N-m.
-	float32 maxMotorTorque;
-
-	/// The desired motor speed in radians per second.
-	float32 motorSpeed;
-
-	/// Suspension frequency, zero indicates no suspension
-	float32 frequencyHz;
-
-	/// Suspension damping ratio, one indicates critical damping
-	float32 dampingRatio;
-};
-
-/// A wheel joint. This joint provides two degrees of freedom: translation
-/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to
-/// line constraint with a rotational motor and a linear spring/damper.
-/// This joint is designed for vehicle suspensions.
-class b2WheelJoint : public b2Joint
-{
-public:
-	b2Vec2 GetAnchorA() const;
-	b2Vec2 GetAnchorB() const;
-
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
-	/// The local anchor point relative to bodyA's origin.
-	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
-
-	/// The local anchor point relative to bodyB's origin.
-	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
-
-	/// The local joint axis relative to bodyA.
-	const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
-
-	/// Get the current joint translation, usually in meters.
-	float32 GetJointTranslation() const;
-
-	/// Get the current joint translation speed, usually in meters per second.
-	float32 GetJointSpeed() const;
-
-	/// Is the joint motor enabled?
-	bool IsMotorEnabled() const;
-
-	/// Enable/disable the joint motor.
-	void EnableMotor(bool flag);
-
-	/// Set the motor speed, usually in radians per second.
-	void SetMotorSpeed(float32 speed);
-
-	/// Get the motor speed, usually in radians per second.
-	float32 GetMotorSpeed() const;
-
-	/// Set/Get the maximum motor force, usually in N-m.
-	void SetMaxMotorTorque(float32 torque);
-	float32 GetMaxMotorTorque() const;
-
-	/// Get the current motor torque given the inverse time step, usually in N-m.
-	float32 GetMotorTorque(float32 inv_dt) const;
-
-	/// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring.
-	void SetSpringFrequencyHz(float32 hz);
-	float32 GetSpringFrequencyHz() const;
-
-	/// Set/Get the spring damping ratio
-	void SetSpringDampingRatio(float32 ratio);
-	float32 GetSpringDampingRatio() const;
-
-	/// Dump to b2Log
-	void Dump();
-
-protected:
-
-	friend class b2Joint;
-	b2WheelJoint(const b2WheelJointDef* def);
-
-	void InitVelocityConstraints(const b2SolverData& data);
-	void SolveVelocityConstraints(const b2SolverData& data);
-	bool SolvePositionConstraints(const b2SolverData& data);
-
-	float32 m_frequencyHz;
-	float32 m_dampingRatio;
-
-	// Solver shared
-	b2Vec2 m_localAnchorA;
-	b2Vec2 m_localAnchorB;
-	b2Vec2 m_localXAxisA;
-	b2Vec2 m_localYAxisA;
-
-	float32 m_impulse;
-	float32 m_motorImpulse;
-	float32 m_springImpulse;
-
-	float32 m_maxMotorTorque;
-	float32 m_motorSpeed;
-	bool m_enableMotor;
-
-	// Solver temp
-	int32 m_indexA;
-	int32 m_indexB;
-	b2Vec2 m_localCenterA;
-	b2Vec2 m_localCenterB;
-	float32 m_invMassA;
-	float32 m_invMassB;
-	float32 m_invIA;
-	float32 m_invIB;
-
-	b2Vec2 m_ax, m_ay;
-	float32 m_sAx, m_sBx;
-	float32 m_sAy, m_sBy;
-
-	float32 m_mass;
-	float32 m_motorMass;
-	float32 m_springMass;
-
-	float32 m_bias;
-	float32 m_gamma;
-};
-
-inline float32 b2WheelJoint::GetMotorSpeed() const
-{
-	return m_motorSpeed;
-}
-
-inline float32 b2WheelJoint::GetMaxMotorTorque() const
-{
-	return m_maxMotorTorque;
-}
-
-inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz)
-{
-	m_frequencyHz = hz;
-}
-
-inline float32 b2WheelJoint::GetSpringFrequencyHz() const
-{
-	return m_frequencyHz;
-}
-
-inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio)
-{
-	m_dampingRatio = ratio;
-}
-
-inline float32 b2WheelJoint::GetSpringDampingRatio() const
-{
-	return m_dampingRatio;
-}
-
-#endif

src/libraries/Box2D/Dynamics/b2ContactManager.h

@@ -1,52 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_CONTACT_MANAGER_H
-#define B2_CONTACT_MANAGER_H
-
-#include <Box2D/Collision/b2BroadPhase.h>
-
-class b2Contact;
-class b2ContactFilter;
-class b2ContactListener;
-class b2BlockAllocator;
-
-// Delegate of b2World.
-class b2ContactManager
-{
-public:
-	b2ContactManager();
-
-	// Broad-phase callback.
-	void AddPair(void* proxyUserDataA, void* proxyUserDataB);
-
-	void FindNewContacts();
-
-	void Destroy(b2Contact* c);
-
-	void Collide();
-            
-	b2BroadPhase m_broadPhase;
-	b2Contact* m_contactList;
-	int32 m_contactCount;
-	b2ContactFilter* m_contactFilter;
-	b2ContactListener* m_contactListener;
-	b2BlockAllocator* m_allocator;
-};
-
-#endif

src/libraries/Box2D/Dynamics/b2TimeStep.h

@@ -1,70 +0,0 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef B2_TIME_STEP_H
-#define B2_TIME_STEP_H
-
-#include <Box2D/Common/b2Math.h>
-
-/// Profiling data. Times are in milliseconds.
-struct b2Profile
-{
-	float32 step;
-	float32 collide;
-	float32 solve;
-	float32 solveInit;
-	float32 solveVelocity;
-	float32 solvePosition;
-	float32 broadphase;
-	float32 solveTOI;
-};
-
-/// This is an internal structure.
-struct b2TimeStep
-{
-	float32 dt;			// time step
-	float32 inv_dt;		// inverse time step (0 if dt == 0).
-	float32 dtRatio;	// dt * inv_dt0
-	int32 velocityIterations;
-	int32 positionIterations;
-	bool warmStarting;
-};
-
-/// This is an internal structure.
-struct b2Position
-{
-	b2Vec2 c;
-	float32 a;
-};
-
-/// This is an internal structure.
-struct b2Velocity
-{
-	b2Vec2 v;
-	float32 w;
-};
-
-/// Solver Data
-struct b2SolverData
-{
-	b2TimeStep step;
-	b2Position* positions;
-	b2Velocity* velocities;
-};
-
-#endif

src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp

@@ -1,36 +0,0 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-
-// Return true if contact calculations should be performed between these two shapes.
-// If you implement your own collision filter you may want to build from this implementation.
-bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
-{
-	const b2Filter& filterA = fixtureA->GetFilterData();
-	const b2Filter& filterB = fixtureB->GetFilterData();
-
-	if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
-	{
-		return filterA.groupIndex > 0;
-	}
-
-	bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
-	return collide;
-}

src/libraries/Box2D/Dynamics/b2Body.cpp → src/libraries/Box2D/Dynamics/b2_body.cpp

@@ -1,26 +1,32 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/Joints/b2Joint.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_joint.h"
+#include "box2d/b2_world.h"
+
+#include <new>
 
 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 {
@@ -45,13 +51,13 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 	{
 		m_flags |= e_autoSleepFlag;
 	}
-	if (bd->awake)
+	if (bd->awake && bd->type != b2_staticBody)
 	{
 		m_flags |= e_awakeFlag;
 	}
-	if (bd->active)
+	if (bd->enabled)
 	{
-		m_flags |= e_activeFlag;
+		m_flags |= e_enabledFlag;
 	}
 
 	m_world = world;
@@ -66,10 +72,10 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 	m_sweep.a = bd->angle;
 	m_sweep.alpha0 = 0.0f;
 
-	m_jointList = NULL;
-	m_contactList = NULL;
-	m_prev = NULL;
-	m_next = NULL;
+	m_jointList = nullptr;
+	m_contactList = nullptr;
+	m_prev = nullptr;
+	m_next = nullptr;
 
 	m_linearVelocity = bd->linearVelocity;
 	m_angularVelocity = bd->angularVelocity;
@@ -85,23 +91,15 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 
 	m_type = bd->type;
 
-	if (m_type == b2_dynamicBody)
-	{
-		m_mass = 1.0f;
-		m_invMass = 1.0f;
-	}
-	else
-	{
-		m_mass = 0.0f;
-		m_invMass = 0.0f;
-	}
+	m_mass = 0.0f;
+	m_invMass = 0.0f;
 
 	m_I = 0.0f;
 	m_invI = 0.0f;
 
 	m_userData = bd->userData;
 
-	m_fixtureList = NULL;
+	m_fixtureList = nullptr;
 	m_fixtureCount = 0;
 }
 
@@ -133,6 +131,7 @@ void b2Body::SetType(b2BodyType type)
 		m_angularVelocity = 0.0f;
 		m_sweep.a0 = m_sweep.a;
 		m_sweep.c0 = m_sweep.c;
+		m_flags &= ~e_awakeFlag;
 		SynchronizeFixtures();
 	}
 
@@ -149,7 +148,7 @@ void b2Body::SetType(b2BodyType type)
 		ce = ce->next;
 		m_world->m_contactManager.Destroy(ce0->contact);
 	}
-	m_contactList = NULL;
+	m_contactList = nullptr;
 
 	// Touch the proxies so that new contacts will be created (when appropriate)
 	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
@@ -168,7 +167,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
 	b2Assert(m_world->IsLocked() == false);
 	if (m_world->IsLocked() == true)
 	{
-		return NULL;
+		return nullptr;
 	}
 
 	b2BlockAllocator* allocator = &m_world->m_blockAllocator;
@@ -177,7 +176,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
 	b2Fixture* fixture = new (memory) b2Fixture;
 	fixture->Create(allocator, this, def);
 
-	if (m_flags & e_activeFlag)
+	if (m_flags & e_enabledFlag)
 	{
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 		fixture->CreateProxies(broadPhase, m_xf);
@@ -197,12 +196,12 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
 
 	// Let the world know we have a new fixture. This will cause new contacts
 	// to be created at the beginning of the next time step.
-	m_world->m_flags |= b2World::e_newFixture;
+	m_world->m_newContacts = true;
 
 	return fixture;
 }
 
-b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
+b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float density)
 {
 	b2FixtureDef def;
 	def.shape = shape;
@@ -213,6 +212,11 @@ b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
 
 void b2Body::DestroyFixture(b2Fixture* fixture)
 {
+	if (fixture == NULL)
+	{
+		return;
+	}
+
 	b2Assert(m_world->IsLocked() == false);
 	if (m_world->IsLocked() == true)
 	{
@@ -225,7 +229,7 @@ void b2Body::DestroyFixture(b2Fixture* fixture)
 	b2Assert(m_fixtureCount > 0);
 	b2Fixture** node = &m_fixtureList;
 	bool found = false;
-	while (*node != NULL)
+	while (*node != nullptr)
 	{
 		if (*node == fixture)
 		{
@@ -260,15 +264,15 @@ void b2Body::DestroyFixture(b2Fixture* fixture)
 
 	b2BlockAllocator* allocator = &m_world->m_blockAllocator;
 
-	if (m_flags & e_activeFlag)
+	if (m_flags & e_enabledFlag)
 	{
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 		fixture->DestroyProxies(broadPhase);
 	}
 
+	fixture->m_body = nullptr;
+	fixture->m_next = nullptr;
 	fixture->Destroy(allocator);
-	fixture->m_body = NULL;
-	fixture->m_next = NULL;
 	fixture->~b2Fixture();
 	allocator->Free(fixture, sizeof(b2Fixture));
 
@@ -320,12 +324,6 @@ void b2Body::ResetMassData()
 		m_invMass = 1.0f / m_mass;
 		localCenter *= m_invMass;
 	}
-	else
-	{
-		// Force all dynamic bodies to have a positive mass.
-		m_mass = 1.0f;
-		m_invMass = 1.0f;
-	}
 
 	if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
 	{
@@ -414,7 +412,7 @@ bool b2Body::ShouldCollide(const b2Body* other) const
 	return true;
 }
 
-void b2Body::SetTransform(const b2Vec2& position, float32 angle)
+void b2Body::SetTransform(const b2Vec2& position, float angle)
 {
 	b2Assert(m_world->IsLocked() == false);
 	if (m_world->IsLocked() == true)
@@ -440,29 +438,40 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle)
 
 void b2Body::SynchronizeFixtures()
 {
-	b2Transform xf1;
-	xf1.q.Set(m_sweep.a0);
-	xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
-
 	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
-	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+
+	if (m_flags & b2Body::e_awakeFlag)
 	{
-		f->Synchronize(broadPhase, xf1, m_xf);
+		b2Transform xf1;
+		xf1.q.Set(m_sweep.a0);
+		xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
+
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->Synchronize(broadPhase, xf1, m_xf);
+		}
+	}
+	else
+	{
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->Synchronize(broadPhase, m_xf, m_xf);
+		}
 	}
 }
 
-void b2Body::SetActive(bool flag)
+void b2Body::SetEnabled(bool flag)
 {
 	b2Assert(m_world->IsLocked() == false);
 
-	if (flag == IsActive())
+	if (flag == IsEnabled())
 	{
 		return;
 	}
 
 	if (flag)
 	{
-		m_flags |= e_activeFlag;
+		m_flags |= e_enabledFlag;
 
 		// Create all proxies.
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
@@ -471,11 +480,12 @@ void b2Body::SetActive(bool flag)
 			f->CreateProxies(broadPhase, m_xf);
 		}
 
-		// Contacts are created the next time step.
+		// Contacts are created at the beginning of the next
+		m_world->m_newContacts = true;
 	}
 	else
 	{
-		m_flags &= ~e_activeFlag;
+		m_flags &= ~e_enabledFlag;
 
 		// Destroy all proxies.
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
@@ -492,7 +502,7 @@ void b2Body::SetActive(bool flag)
 			ce = ce->next;
 			m_world->m_contactManager.Destroy(ce0->contact);
 		}
-		m_contactList = NULL;
+		m_contactList = nullptr;
 	}
 }
 
@@ -522,28 +532,31 @@ void b2Body::Dump()
 {
 	int32 bodyIndex = m_islandIndex;
 
-	b2Log("{\n");
-	b2Log("  b2BodyDef bd;\n");
-	b2Log("  bd.type = b2BodyType(%d);\n", m_type);
-	b2Log("  bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
-	b2Log("  bd.angle = %.15lef;\n", m_sweep.a);
-	b2Log("  bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
-	b2Log("  bd.angularVelocity = %.15lef;\n", m_angularVelocity);
-	b2Log("  bd.linearDamping = %.15lef;\n", m_linearDamping);
-	b2Log("  bd.angularDamping = %.15lef;\n", m_angularDamping);
-	b2Log("  bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
-	b2Log("  bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
-	b2Log("  bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
-	b2Log("  bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
-	b2Log("  bd.active = bool(%d);\n", m_flags & e_activeFlag);
-	b2Log("  bd.gravityScale = %.15lef;\n", m_gravityScale);
-	b2Log("  bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
-	b2Log("\n");
+	// %.9g is sufficient to save and load the same value using text
+	// FLT_DECIMAL_DIG == 9
+
+	b2Dump("{\n");
+	b2Dump("  b2BodyDef bd;\n");
+	b2Dump("  bd.type = b2BodyType(%d);\n", m_type);
+	b2Dump("  bd.position.Set(%.9g, %.9g);\n", m_xf.p.x, m_xf.p.y);
+	b2Dump("  bd.angle = %.9g;\n", m_sweep.a);
+	b2Dump("  bd.linearVelocity.Set(%.9g, %.9g);\n", m_linearVelocity.x, m_linearVelocity.y);
+	b2Dump("  bd.angularVelocity = %.9g;\n", m_angularVelocity);
+	b2Dump("  bd.linearDamping = %.9g;\n", m_linearDamping);
+	b2Dump("  bd.angularDamping = %.9g;\n", m_angularDamping);
+	b2Dump("  bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
+	b2Dump("  bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
+	b2Dump("  bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
+	b2Dump("  bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
+	b2Dump("  bd.enabled = bool(%d);\n", m_flags & e_enabledFlag);
+	b2Dump("  bd.gravityScale = %.9g;\n", m_gravityScale);
+	b2Dump("  bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
+	b2Dump("\n");
 	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 	{
-		b2Log("  {\n");
+		b2Dump("  {\n");
 		f->Dump(bodyIndex);
-		b2Log("  }\n");
+		b2Dump("  }\n");
 	}
-	b2Log("}\n");
+	b2Dump("}\n");
 }

src/libraries/Box2D/Dynamics/b2_chain_circle_contact.cpp

@@ -0,0 +1,57 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_chain_circle_contact.h"
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_edge_shape.h"
+
+#include <new>
+
+b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
+	return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
+	allocator->Free(contact, sizeof(b2ChainAndCircleContact));
+}
+
+b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+	b2EdgeShape edge;
+	chain->GetChildEdge(&edge, m_indexA);
+	b2CollideEdgeAndCircle(	manifold, &edge, xfA,
+							(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_chain_circle_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H
+#define B2_CHAIN_AND_CIRCLE_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2ChainAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+	~b2ChainAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.cpp

@@ -0,0 +1,57 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_chain_polygon_contact.h"
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_edge_shape.h"
+
+#include <new>
+
+b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
+	return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
+	allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
+}
+
+b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+	b2EdgeShape edge;
+	chain->GetChildEdge(&edge, m_indexA);
+	b2CollideEdgeAndPolygon(	manifold, &edge, xfA,
+								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H
+#define B2_CHAIN_AND_POLYGON_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2ChainAndPolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+	~b2ChainAndPolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2_circle_contact.cpp

@@ -0,0 +1,56 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_circle_contact.h"
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_time_of_impact.h"
+#include "box2d/b2_world_callbacks.h"
+
+#include <new>
+
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2CircleContact));
+	return new (mem) b2CircleContact(fixtureA, fixtureB);
+}
+
+void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2CircleContact*)contact)->~b2CircleContact();
+	allocator->Free(contact, sizeof(b2CircleContact));
+}
+
+b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideCircles(manifold,
+					(b2CircleShape*)m_fixtureA->GetShape(), xfA,
+					(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_circle_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_CIRCLE_CONTACT_H
+#define B2_CIRCLE_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2CircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2CircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp → src/libraries/Box2D/Dynamics/b2_contact.cpp

@@ -1,38 +1,42 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/Contacts/b2CircleContact.h>
-#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
-#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
-#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
-#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
-#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
-#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>
-#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Collision/Shapes/b2Shape.h>
-#include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2World.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_chain_circle_contact.h"
+#include "b2_chain_polygon_contact.h"
+#include "b2_circle_contact.h"
+#include "b2_contact_solver.h"
+#include "b2_edge_circle_contact.h"
+#include "b2_edge_polygon_contact.h"
+#include "b2_polygon_circle_contact.h"
+#include "b2_polygon_contact.h"
+
+#include "box2d/b2_contact.h"
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_collision.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_shape.h"
+#include "box2d/b2_time_of_impact.h"
+#include "box2d/b2_world.h"
 
 b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
 bool b2Contact::s_initialized = false;
@@ -94,7 +98,7 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtu
 	}
 	else
 	{
-		return NULL;
+		return nullptr;
 	}
 }
 
@@ -116,8 +120,8 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
 	b2Shape::Type typeA = fixtureA->GetType();
 	b2Shape::Type typeB = fixtureB->GetType();
 
-	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
-	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
+	b2Assert(0 <= typeA && typeA < b2Shape::e_typeCount);
+	b2Assert(0 <= typeB && typeB < b2Shape::e_typeCount);
 
 	b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
 	destroyFcn(contact, allocator);
@@ -135,18 +139,18 @@ b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
 
 	m_manifold.pointCount = 0;
 
-	m_prev = NULL;
-	m_next = NULL;
+	m_prev = nullptr;
+	m_next = nullptr;
 
-	m_nodeA.contact = NULL;
-	m_nodeA.prev = NULL;
-	m_nodeA.next = NULL;
-	m_nodeA.other = NULL;
+	m_nodeA.contact = nullptr;
+	m_nodeA.prev = nullptr;
+	m_nodeA.next = nullptr;
+	m_nodeA.other = nullptr;
 
-	m_nodeB.contact = NULL;
-	m_nodeB.prev = NULL;
-	m_nodeB.next = NULL;
-	m_nodeB.other = NULL;
+	m_nodeB.contact = nullptr;
+	m_nodeB.prev = nullptr;
+	m_nodeB.next = nullptr;
+	m_nodeB.other = nullptr;
 
 	m_toiCount = 0;
 

src/libraries/Box2D/Dynamics/b2ContactManager.cpp → src/libraries/Box2D/Dynamics/b2_contact_manager.cpp

@@ -1,37 +1,41 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/b2ContactManager.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_contact_manager.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_world_callbacks.h"
 
 b2ContactFilter b2_defaultFilter;
 b2ContactListener b2_defaultListener;
 
 b2ContactManager::b2ContactManager()
 {
-	m_contactList = NULL;
+	m_contactList = nullptr;
 	m_contactCount = 0;
 	m_contactFilter = &b2_defaultFilter;
 	m_contactListener = &b2_defaultListener;
-	m_allocator = NULL;
+	m_allocator = nullptr;
 }
 
 void b2ContactManager::Destroy(b2Contact* c)
@@ -237,7 +241,7 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 
 	// Call the factory.
 	b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
-	if (c == NULL)
+	if (c == nullptr)
 	{
 		return;
 	}
@@ -251,9 +255,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	bodyB = fixtureB->GetBody();
 
 	// Insert into the world.
-	c->m_prev = NULL;
+	c->m_prev = nullptr;
 	c->m_next = m_contactList;
-	if (m_contactList != NULL)
+	if (m_contactList != nullptr)
 	{
 		m_contactList->m_prev = c;
 	}
@@ -265,9 +269,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	c->m_nodeA.contact = c;
 	c->m_nodeA.other = bodyB;
 
-	c->m_nodeA.prev = NULL;
+	c->m_nodeA.prev = nullptr;
 	c->m_nodeA.next = bodyA->m_contactList;
-	if (bodyA->m_contactList != NULL)
+	if (bodyA->m_contactList != nullptr)
 	{
 		bodyA->m_contactList->prev = &c->m_nodeA;
 	}
@@ -277,20 +281,13 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	c->m_nodeB.contact = c;
 	c->m_nodeB.other = bodyA;
 
-	c->m_nodeB.prev = NULL;
+	c->m_nodeB.prev = nullptr;
 	c->m_nodeB.next = bodyB->m_contactList;
-	if (bodyB->m_contactList != NULL)
+	if (bodyB->m_contactList != nullptr)
 	{
 		bodyB->m_contactList->prev = &c->m_nodeB;
 	}
 	bodyB->m_contactList = &c->m_nodeB;
 
-	// Wake up the bodies
-	if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)
-	{
-		bodyA->SetAwake(true);
-		bodyB->SetAwake(true);
-	}
-
 	++m_contactCount;
 }

src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp → src/libraries/Box2D/Dynamics/b2_contact_solver.cpp

@@ -1,29 +1,34 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Common/b2StackAllocator.h>
+// MIT License
 
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_contact_solver.h"
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_stack_allocator.h"
+#include "box2d/b2_world.h"
+
+// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix.
 #define B2_DEBUG_SOLVER 0
 
 bool g_blockSolve = true;
@@ -35,11 +40,11 @@ struct b2ContactPositionConstraint
 	b2Vec2 localPoint;
 	int32 indexA;
 	int32 indexB;
-	float32 invMassA, invMassB;
+	float invMassA, invMassB;
 	b2Vec2 localCenterA, localCenterB;
-	float32 invIA, invIB;
+	float invIA, invIB;
 	b2Manifold::Type type;
-	float32 radiusA, radiusB;
+	float radiusA, radiusB;
 	int32 pointCount;
 };
 
@@ -63,8 +68,8 @@ b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
 		b2Fixture* fixtureB = contact->m_fixtureB;
 		b2Shape* shapeA = fixtureA->GetShape();
 		b2Shape* shapeB = fixtureB->GetShape();
-		float32 radiusA = shapeA->m_radius;
-		float32 radiusB = shapeB->m_radius;
+		float radiusA = shapeA->m_radius;
+		float radiusB = shapeB->m_radius;
 		b2Body* bodyA = fixtureA->GetBody();
 		b2Body* bodyB = fixtureB->GetBody();
 		b2Manifold* manifold = contact->GetManifold();
@@ -144,29 +149,29 @@ void b2ContactSolver::InitializeVelocityConstraints()
 		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
 		b2ContactPositionConstraint* pc = m_positionConstraints + i;
 
-		float32 radiusA = pc->radiusA;
-		float32 radiusB = pc->radiusB;
+		float radiusA = pc->radiusA;
+		float radiusB = pc->radiusB;
 		b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
 
 		int32 indexA = vc->indexA;
 		int32 indexB = vc->indexB;
 
-		float32 mA = vc->invMassA;
-		float32 mB = vc->invMassB;
-		float32 iA = vc->invIA;
-		float32 iB = vc->invIB;
+		float mA = vc->invMassA;
+		float mB = vc->invMassB;
+		float iA = vc->invIA;
+		float iB = vc->invIB;
 		b2Vec2 localCenterA = pc->localCenterA;
 		b2Vec2 localCenterB = pc->localCenterB;
 
 		b2Vec2 cA = m_positions[indexA].c;
-		float32 aA = m_positions[indexA].a;
+		float aA = m_positions[indexA].a;
 		b2Vec2 vA = m_velocities[indexA].v;
-		float32 wA = m_velocities[indexA].w;
+		float wA = m_velocities[indexA].w;
 
 		b2Vec2 cB = m_positions[indexB].c;
-		float32 aB = m_positions[indexB].a;
+		float aB = m_positions[indexB].a;
 		b2Vec2 vB = m_velocities[indexB].v;
-		float32 wB = m_velocities[indexB].w;
+		float wB = m_velocities[indexB].w;
 
 		b2Assert(manifold->pointCount > 0);
 
@@ -189,25 +194,25 @@ void b2ContactSolver::InitializeVelocityConstraints()
 			vcp->rA = worldManifold.points[j] - cA;
 			vcp->rB = worldManifold.points[j] - cB;
 
-			float32 rnA = b2Cross(vcp->rA, vc->normal);
-			float32 rnB = b2Cross(vcp->rB, vc->normal);
+			float rnA = b2Cross(vcp->rA, vc->normal);
+			float rnB = b2Cross(vcp->rB, vc->normal);
 
-			float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+			float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
 			vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
 
 			b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
 
-			float32 rtA = b2Cross(vcp->rA, tangent);
-			float32 rtB = b2Cross(vcp->rB, tangent);
+			float rtA = b2Cross(vcp->rA, tangent);
+			float rtB = b2Cross(vcp->rB, tangent);
 
-			float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+			float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
 
 			vcp->tangentMass = kTangent > 0.0f ? 1.0f /  kTangent : 0.0f;
 
 			// Setup a velocity bias for restitution.
 			vcp->velocityBias = 0.0f;
-			float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
+			float vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
 			if (vRel < -b2_velocityThreshold)
 			{
 				vcp->velocityBias = -vc->restitution * vRel;
@@ -220,17 +225,17 @@ void b2ContactSolver::InitializeVelocityConstraints()
 			b2VelocityConstraintPoint* vcp1 = vc->points + 0;
 			b2VelocityConstraintPoint* vcp2 = vc->points + 1;
 
-			float32 rn1A = b2Cross(vcp1->rA, vc->normal);
-			float32 rn1B = b2Cross(vcp1->rB, vc->normal);
-			float32 rn2A = b2Cross(vcp2->rA, vc->normal);
-			float32 rn2B = b2Cross(vcp2->rB, vc->normal);
+			float rn1A = b2Cross(vcp1->rA, vc->normal);
+			float rn1B = b2Cross(vcp1->rB, vc->normal);
+			float rn2A = b2Cross(vcp2->rA, vc->normal);
+			float rn2B = b2Cross(vcp2->rB, vc->normal);
 
-			float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
-			float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
-			float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
+			float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
+			float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
+			float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
 
 			// Ensure a reasonable condition number.
-			const float32 k_maxConditionNumber = 1000.0f;
+			const float k_maxConditionNumber = 1000.0f;
 			if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
 			{
 				// K is safe to invert.
@@ -257,16 +262,16 @@ void b2ContactSolver::WarmStart()
 
 		int32 indexA = vc->indexA;
 		int32 indexB = vc->indexB;
-		float32 mA = vc->invMassA;
-		float32 iA = vc->invIA;
-		float32 mB = vc->invMassB;
-		float32 iB = vc->invIB;
+		float mA = vc->invMassA;
+		float iA = vc->invIA;
+		float mB = vc->invMassB;
+		float iB = vc->invIB;
 		int32 pointCount = vc->pointCount;
 
 		b2Vec2 vA = m_velocities[indexA].v;
-		float32 wA = m_velocities[indexA].w;
+		float wA = m_velocities[indexA].w;
 		b2Vec2 vB = m_velocities[indexB].v;
-		float32 wB = m_velocities[indexB].w;
+		float wB = m_velocities[indexB].w;
 
 		b2Vec2 normal = vc->normal;
 		b2Vec2 tangent = b2Cross(normal, 1.0f);
@@ -296,20 +301,20 @@ void b2ContactSolver::SolveVelocityConstraints()
 
 		int32 indexA = vc->indexA;
 		int32 indexB = vc->indexB;
-		float32 mA = vc->invMassA;
-		float32 iA = vc->invIA;
-		float32 mB = vc->invMassB;
-		float32 iB = vc->invIB;
+		float mA = vc->invMassA;
+		float iA = vc->invIA;
+		float mB = vc->invMassB;
+		float iB = vc->invIB;
 		int32 pointCount = vc->pointCount;
 
 		b2Vec2 vA = m_velocities[indexA].v;
-		float32 wA = m_velocities[indexA].w;
+		float wA = m_velocities[indexA].w;
 		b2Vec2 vB = m_velocities[indexB].v;
-		float32 wB = m_velocities[indexB].w;
+		float wB = m_velocities[indexB].w;
 
 		b2Vec2 normal = vc->normal;
 		b2Vec2 tangent = b2Cross(normal, 1.0f);
-		float32 friction = vc->friction;
+		float friction = vc->friction;
 
 		b2Assert(pointCount == 1 || pointCount == 2);
 
@@ -323,12 +328,12 @@ void b2ContactSolver::SolveVelocityConstraints()
 			b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
 
 			// Compute tangent force
-			float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
-			float32 lambda = vcp->tangentMass * (-vt);
+			float vt = b2Dot(dv, tangent) - vc->tangentSpeed;
+			float lambda = vcp->tangentMass * (-vt);
 
 			// b2Clamp the accumulated force
-			float32 maxFriction = friction * vcp->normalImpulse;
-			float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
+			float maxFriction = friction * vcp->normalImpulse;
+			float newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
 			lambda = newImpulse - vcp->tangentImpulse;
 			vcp->tangentImpulse = newImpulse;
 
@@ -345,19 +350,19 @@ void b2ContactSolver::SolveVelocityConstraints()
 		// Solve normal constraints
 		if (pointCount == 1 || g_blockSolve == false)
 		{
-			for (int32 i = 0; i < pointCount; ++i)
+			for (int32 j = 0; j < pointCount; ++j)
 			{
-				b2VelocityConstraintPoint* vcp = vc->points + i;
+				b2VelocityConstraintPoint* vcp = vc->points + j;
 
 				// Relative velocity at contact
 				b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
 
 				// Compute normal impulse
-				float32 vn = b2Dot(dv, normal);
-				float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
+				float vn = b2Dot(dv, normal);
+				float lambda = -vcp->normalMass * (vn - vcp->velocityBias);
 
 				// b2Clamp the accumulated impulse
-				float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
+				float newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
 				lambda = newImpulse - vcp->normalImpulse;
 				vcp->normalImpulse = newImpulse;
 
@@ -375,7 +380,7 @@ void b2ContactSolver::SolveVelocityConstraints()
 			// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
 			// Build the mini LCP for this contact patch
 			//
-			// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+			// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
 			//
 			// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
 			// b = vn0 - velocityBias
@@ -416,8 +421,8 @@ void b2ContactSolver::SolveVelocityConstraints()
 			b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
 
 			// Compute normal velocity
-			float32 vn1 = b2Dot(dv1, normal);
-			float32 vn2 = b2Dot(dv2, normal);
+			float vn1 = b2Dot(dv1, normal);
+			float vn2 = b2Dot(dv2, normal);
 
 			b2Vec2 b;
 			b.x = vn1 - cp1->velocityBias;
@@ -426,7 +431,7 @@ void b2ContactSolver::SolveVelocityConstraints()
 			// Compute b'
 			b -= b2Mul(vc->K, a);
 
-			const float32 k_errorTol = 1e-3f;
+			const float k_errorTol = 1e-3f;
 			B2_NOT_USED(k_errorTol);
 
 			for (;;)
@@ -485,7 +490,6 @@ void b2ContactSolver::SolveVelocityConstraints()
 				x.y = 0.0f;
 				vn1 = 0.0f;
 				vn2 = vc->K.ex.y * x.x + b.y;
-
 				if (x.x >= 0.0f && vn2 >= 0.0f)
 				{
 					// Get the incremental impulse
@@ -664,13 +668,13 @@ struct b2PositionSolverManifold
 
 	b2Vec2 normal;
 	b2Vec2 point;
-	float32 separation;
+	float separation;
 };
 
 // Sequential solver.
 bool b2ContactSolver::SolvePositionConstraints()
 {
-	float32 minSeparation = 0.0f;
+	float minSeparation = 0.0f;
 
 	for (int32 i = 0; i < m_count; ++i)
 	{
@@ -679,18 +683,18 @@ bool b2ContactSolver::SolvePositionConstraints()
 		int32 indexA = pc->indexA;
 		int32 indexB = pc->indexB;
 		b2Vec2 localCenterA = pc->localCenterA;
-		float32 mA = pc->invMassA;
-		float32 iA = pc->invIA;
+		float mA = pc->invMassA;
+		float iA = pc->invIA;
 		b2Vec2 localCenterB = pc->localCenterB;
-		float32 mB = pc->invMassB;
-		float32 iB = pc->invIB;
+		float mB = pc->invMassB;
+		float iB = pc->invIB;
 		int32 pointCount = pc->pointCount;
 
 		b2Vec2 cA = m_positions[indexA].c;
-		float32 aA = m_positions[indexA].a;
+		float aA = m_positions[indexA].a;
 
 		b2Vec2 cB = m_positions[indexB].c;
-		float32 aB = m_positions[indexB].a;
+		float aB = m_positions[indexB].a;
 
 		// Solve normal constraints
 		for (int32 j = 0; j < pointCount; ++j)
@@ -706,7 +710,7 @@ bool b2ContactSolver::SolvePositionConstraints()
 			b2Vec2 normal = psm.normal;
 
 			b2Vec2 point = psm.point;
-			float32 separation = psm.separation;
+			float separation = psm.separation;
 
 			b2Vec2 rA = point - cA;
 			b2Vec2 rB = point - cB;
@@ -715,15 +719,15 @@ bool b2ContactSolver::SolvePositionConstraints()
 			minSeparation = b2Min(minSeparation, separation);
 
 			// Prevent large corrections and allow slop.
-			float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+			float C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
 
 			// Compute the effective mass.
-			float32 rnA = b2Cross(rA, normal);
-			float32 rnB = b2Cross(rB, normal);
-			float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+			float rnA = b2Cross(rA, normal);
+			float rnB = b2Cross(rB, normal);
+			float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
 			// Compute normal impulse
-			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+			float impulse = K > 0.0f ? - C / K : 0.0f;
 
 			b2Vec2 P = impulse * normal;
 
@@ -749,7 +753,7 @@ bool b2ContactSolver::SolvePositionConstraints()
 // Sequential position solver for position constraints.
 bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
 {
-	float32 minSeparation = 0.0f;
+	float minSeparation = 0.0f;
 
 	for (int32 i = 0; i < m_count; ++i)
 	{
@@ -761,16 +765,16 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde
 		b2Vec2 localCenterB = pc->localCenterB;
 		int32 pointCount = pc->pointCount;
 
-		float32 mA = 0.0f;
-		float32 iA = 0.0f;
+		float mA = 0.0f;
+		float iA = 0.0f;
 		if (indexA == toiIndexA || indexA == toiIndexB)
 		{
 			mA = pc->invMassA;
 			iA = pc->invIA;
 		}
 
-		float32 mB = 0.0f;
-		float32 iB = 0.;
+		float mB = 0.0f;
+		float iB = 0.;
 		if (indexB == toiIndexA || indexB == toiIndexB)
 		{
 			mB = pc->invMassB;
@@ -778,10 +782,10 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde
 		}
 
 		b2Vec2 cA = m_positions[indexA].c;
-		float32 aA = m_positions[indexA].a;
+		float aA = m_positions[indexA].a;
 
 		b2Vec2 cB = m_positions[indexB].c;
-		float32 aB = m_positions[indexB].a;
+		float aB = m_positions[indexB].a;
 
 		// Solve normal constraints
 		for (int32 j = 0; j < pointCount; ++j)
@@ -797,7 +801,7 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde
 			b2Vec2 normal = psm.normal;
 
 			b2Vec2 point = psm.point;
-			float32 separation = psm.separation;
+			float separation = psm.separation;
 
 			b2Vec2 rA = point - cA;
 			b2Vec2 rB = point - cB;
@@ -806,15 +810,15 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde
 			minSeparation = b2Min(minSeparation, separation);
 
 			// Prevent large corrections and allow slop.
-			float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+			float C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
 
 			// Compute the effective mass.
-			float32 rnA = b2Cross(rA, normal);
-			float32 rnB = b2Cross(rB, normal);
-			float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+			float rnA = b2Cross(rA, normal);
+			float rnB = b2Cross(rB, normal);
+			float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
 			// Compute normal impulse
-			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+			float impulse = K > 0.0f ? - C / K : 0.0f;
 
 			b2Vec2 P = impulse * normal;
 

src/libraries/Box2D/Dynamics/b2_contact_solver.h

@@ -0,0 +1,99 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_CONTACT_SOLVER_H
+#define B2_CONTACT_SOLVER_H
+
+#include "box2d/b2_collision.h"
+#include "box2d/b2_math.h"
+#include "box2d/b2_time_step.h"
+
+class b2Contact;
+class b2Body;
+class b2StackAllocator;
+struct b2ContactPositionConstraint;
+
+struct b2VelocityConstraintPoint
+{
+	b2Vec2 rA;
+	b2Vec2 rB;
+	float normalImpulse;
+	float tangentImpulse;
+	float normalMass;
+	float tangentMass;
+	float velocityBias;
+};
+
+struct b2ContactVelocityConstraint
+{
+	b2VelocityConstraintPoint points[b2_maxManifoldPoints];
+	b2Vec2 normal;
+	b2Mat22 normalMass;
+	b2Mat22 K;
+	int32 indexA;
+	int32 indexB;
+	float invMassA, invMassB;
+	float invIA, invIB;
+	float friction;
+	float restitution;
+	float tangentSpeed;
+	int32 pointCount;
+	int32 contactIndex;
+};
+
+struct b2ContactSolverDef
+{
+	b2TimeStep step;
+	b2Contact** contacts;
+	int32 count;
+	b2Position* positions;
+	b2Velocity* velocities;
+	b2StackAllocator* allocator;
+};
+
+class b2ContactSolver
+{
+public:
+	b2ContactSolver(b2ContactSolverDef* def);
+	~b2ContactSolver();
+
+	void InitializeVelocityConstraints();
+
+	void WarmStart();
+	void SolveVelocityConstraints();
+	void StoreImpulses();
+
+	bool SolvePositionConstraints();
+	bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
+
+	b2TimeStep m_step;
+	b2Position* m_positions;
+	b2Velocity* m_velocities;
+	b2StackAllocator* m_allocator;
+	b2ContactPositionConstraint* m_positionConstraints;
+	b2ContactVelocityConstraint* m_velocityConstraints;
+	b2Contact** m_contacts;
+	int m_count;
+};
+
+#endif
+

src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp → src/libraries/Box2D/Dynamics/b2_distance_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_distance_joint.h"
+#include "box2d/b2_time_step.h"
 
 // 1-D constrained system
 // m (v2 - v1) = lambda
@@ -35,6 +39,7 @@
 // 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)
 {
@@ -52,8 +57,9 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
 	m_localAnchorA = def->localAnchorA;
 	m_localAnchorB = def->localAnchorB;
 	m_length = def->length;
-	m_frequencyHz = def->frequencyHz;
-	m_dampingRatio = def->dampingRatio;
+	m_stiffness = def->stiffness;
+	m_damping = def->damping;
+
 	m_impulse = 0.0f;
 	m_gamma = 0.0f;
 	m_bias = 0.0f;
@@ -71,14 +77,14 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIB = m_bodyB->m_invI;
 
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -87,7 +93,7 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_u = cB + m_rB - cA - m_rA;
 
 	// Handle singularity.
-	float32 length = m_u.Length();
+	float length = m_u.Length();
 	if (length > b2_linearSlop)
 	{
 		m_u *= 1.0f / length;
@@ -97,28 +103,21 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 		m_u.Set(0.0f, 0.0f);
 	}
 
-	float32 crAu = b2Cross(m_rA, m_u);
-	float32 crBu = b2Cross(m_rB, m_u);
-	float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
-
-	// Compute the effective mass matrix.
-	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+	float crAu = b2Cross(m_rA, m_u);
+	float crBu = b2Cross(m_rB, m_u);
+	float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
 
-	if (m_frequencyHz > 0.0f)
+	if (m_stiffness > 0.0f)
 	{
-		float32 C = length - m_length;
-
-		// Frequency
-		float32 omega = 2.0f * b2_pi * m_frequencyHz;
+		float C = length - m_length;
 
-		// Damping coefficient
-		float32 d = 2.0f * m_mass * m_dampingRatio * omega;
-
-		// Spring stiffness
-		float32 k = m_mass * omega * omega;
+		float d = m_damping;
+		float k = m_stiffness;
 
 		// magic formulas
-		float32 h = data.step.dt;
+		float h = data.step.dt;
+
+		// gamma = 1 / (h * (d + h * k)), the extra factor of h in the denominator is since the lambda is an impulse, not a force
 		m_gamma = h * (d + h * k);
 		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
 		m_bias = C * h * k * m_gamma;
@@ -130,6 +129,7 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 	{
 		m_gamma = 0.0f;
 		m_bias = 0.0f;
+		m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
 	}
 
 	if (data.step.warmStarting)
@@ -157,16 +157,16 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	// Cdot = dot(u, v + cross(w, r))
 	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
 	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
-	float32 Cdot = b2Dot(m_u, vpB - vpA);
+	float Cdot = b2Dot(m_u, vpB - vpA);
 
-	float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+	float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
 	m_impulse += impulse;
 
 	b2Vec2 P = impulse * m_u;
@@ -183,16 +183,16 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
 
 bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	if (m_frequencyHz > 0.0f)
+	if (m_stiffness > 0.0f)
 	{
 		// There is no position correction for soft distance constraints.
 		return true;
 	}
 
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -200,11 +200,11 @@ bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
 	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 	b2Vec2 u = cB + rB - cA - rA;
 
-	float32 length = u.Normalize();
-	float32 C = length - m_length;
+	float length = u.Normalize();
+	float C = length - m_length;
 	C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
 
-	float32 impulse = -m_mass * C;
+	float impulse = -m_mass * C;
 	b2Vec2 P = impulse * u;
 
 	cA -= m_invMassA * P;
@@ -230,13 +230,13 @@ b2Vec2 b2DistanceJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2DistanceJoint::GetReactionForce(float inv_dt) const
 {
 	b2Vec2 F = (inv_dt * m_impulse) * m_u;
 	return F;
 }
 
-float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
+float b2DistanceJoint::GetReactionTorque(float inv_dt) const
 {
 	B2_NOT_USED(inv_dt);
 	return 0.0f;
@@ -247,14 +247,14 @@ void b2DistanceJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2DistanceJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.length = %.15lef;\n", m_length);
-	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
-	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2DistanceJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.length = %.9g;\n", m_length);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/b2_edge_circle_contact.cpp

@@ -0,0 +1,54 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_edge_circle_contact.h"
+
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_fixture.h"
+
+#include <new>
+
+b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
+	return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
+	allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
+}
+
+b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideEdgeAndCircle(	manifold,
+								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_edge_circle_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H
+#define B2_EDGE_AND_CIRCLE_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2EdgeAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2EdgeAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.cpp

@@ -0,0 +1,54 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_edge_polygon_contact.h"
+
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_fixture.h"
+
+#include <new>
+
+b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
+	return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
+	allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
+}
+
+b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideEdgeAndPolygon(	manifold,
+								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_EDGE_AND_POLYGON_CONTACT_H
+#define B2_EDGE_AND_POLYGON_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2EdgeAndPolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2EdgeAndPolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2Fixture.cpp → src/libraries/Box2D/Dynamics/b2_fixture.cpp

@@ -1,40 +1,44 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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 <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/b2BroadPhase.h>
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Common/b2BlockAllocator.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_broad_phase.h"
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_collision.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_edge_shape.h"
+#include "box2d/b2_polygon_shape.h"
+#include "box2d/b2_world.h"
 
 b2Fixture::b2Fixture()
 {
-	m_userData = NULL;
-	m_body = NULL;
-	m_next = NULL;
-	m_proxies = NULL;
+	m_userData = nullptr;
+	m_body = nullptr;
+	m_next = nullptr;
+	m_proxies = nullptr;
 	m_proxyCount = 0;
-	m_shape = NULL;
+	m_shape = nullptr;
 	m_density = 0.0f;
 }
 
@@ -45,7 +49,7 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur
 	m_restitution = def->restitution;
 
 	m_body = body;
-	m_next = NULL;
+	m_next = nullptr;
 
 	m_filter = def->filter;
 
@@ -58,7 +62,7 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur
 	m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy));
 	for (int32 i = 0; i < childCount; ++i)
 	{
-		m_proxies[i].fixture = NULL;
+		m_proxies[i].fixture = nullptr;
 		m_proxies[i].proxyId = b2BroadPhase::e_nullProxy;
 	}
 	m_proxyCount = 0;
@@ -74,7 +78,7 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
 	// Free the proxy array.
 	int32 childCount = m_shape->GetChildCount();
 	allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy));
-	m_proxies = NULL;
+	m_proxies = nullptr;
 
 	// Free the child shape.
 	switch (m_shape->m_type)
@@ -116,7 +120,7 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
 		break;
 	}
 
-	m_shape = NULL;
+	m_shape = nullptr;
 }
 
 void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf)
@@ -167,7 +171,7 @@ void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transfo
 	
 		proxy->aabb.Combine(aabb1, aabb2);
 
-		b2Vec2 displacement = transform2.p - transform1.p;
+		b2Vec2 displacement = aabb2.GetCenter() - aabb1.GetCenter();
 
 		broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
 	}
@@ -182,7 +186,7 @@ void b2Fixture::SetFilterData(const b2Filter& filter)
 
 void b2Fixture::Refilter()
 {
-	if (m_body == NULL)
+	if (m_body == nullptr)
 	{
 		return;
 	}
@@ -204,7 +208,7 @@ void b2Fixture::Refilter()
 
 	b2World* world = m_body->GetWorld();
 
-	if (world == NULL)
+	if (world == nullptr)
 	{
 		return;
 	}
@@ -228,67 +232,64 @@ void b2Fixture::SetSensor(bool sensor)
 
 void b2Fixture::Dump(int32 bodyIndex)
 {
-	b2Log("    b2FixtureDef fd;\n");
-	b2Log("    fd.friction = %.15lef;\n", m_friction);
-	b2Log("    fd.restitution = %.15lef;\n", m_restitution);
-	b2Log("    fd.density = %.15lef;\n", m_density);
-	b2Log("    fd.isSensor = bool(%d);\n", m_isSensor);
-	b2Log("    fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);
-	b2Log("    fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);
-	b2Log("    fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);
+	b2Dump("    b2FixtureDef fd;\n");
+	b2Dump("    fd.friction = %.9g;\n", m_friction);
+	b2Dump("    fd.restitution = %.9g;\n", m_restitution);
+	b2Dump("    fd.density = %.9g;\n", m_density);
+	b2Dump("    fd.isSensor = bool(%d);\n", m_isSensor);
+	b2Dump("    fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);
+	b2Dump("    fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);
+	b2Dump("    fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);
 
 	switch (m_shape->m_type)
 	{
 	case b2Shape::e_circle:
 		{
 			b2CircleShape* s = (b2CircleShape*)m_shape;
-			b2Log("    b2CircleShape shape;\n");
-			b2Log("    shape.m_radius = %.15lef;\n", s->m_radius);
-			b2Log("    shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y);
+			b2Dump("    b2CircleShape shape;\n");
+			b2Dump("    shape.m_radius = %.9g;\n", s->m_radius);
+			b2Dump("    shape.m_p.Set(%.9g, %.9g);\n", s->m_p.x, s->m_p.y);
 		}
 		break;
 
 	case b2Shape::e_edge:
 		{
 			b2EdgeShape* s = (b2EdgeShape*)m_shape;
-			b2Log("    b2EdgeShape shape;\n");
-			b2Log("    shape.m_radius = %.15lef;\n", s->m_radius);
-			b2Log("    shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y);
-			b2Log("    shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y);
-			b2Log("    shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y);
-			b2Log("    shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y);
-			b2Log("    shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0);
-			b2Log("    shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3);
+			b2Dump("    b2EdgeShape shape;\n");
+			b2Dump("    shape.m_radius = %.9g;\n", s->m_radius);
+			b2Dump("    shape.m_vertex0.Set(%.9g, %.9g);\n", s->m_vertex0.x, s->m_vertex0.y);
+			b2Dump("    shape.m_vertex1.Set(%.9g, %.9g);\n", s->m_vertex1.x, s->m_vertex1.y);
+			b2Dump("    shape.m_vertex2.Set(%.9g, %.9g);\n", s->m_vertex2.x, s->m_vertex2.y);
+			b2Dump("    shape.m_vertex3.Set(%.9g, %.9g);\n", s->m_vertex3.x, s->m_vertex3.y);
+			b2Dump("    shape.m_oneSided = bool(%d);\n", s->m_oneSided);
 		}
 		break;
 
 	case b2Shape::e_polygon:
 		{
 			b2PolygonShape* s = (b2PolygonShape*)m_shape;
-			b2Log("    b2PolygonShape shape;\n");
-			b2Log("    b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
+			b2Dump("    b2PolygonShape shape;\n");
+			b2Dump("    b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
 			for (int32 i = 0; i < s->m_count; ++i)
 			{
-				b2Log("    vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
+				b2Dump("    vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
 			}
-			b2Log("    shape.Set(vs, %d);\n", s->m_count);
+			b2Dump("    shape.Set(vs, %d);\n", s->m_count);
 		}
 		break;
 
 	case b2Shape::e_chain:
 		{
 			b2ChainShape* s = (b2ChainShape*)m_shape;
-			b2Log("    b2ChainShape shape;\n");
-			b2Log("    b2Vec2 vs[%d];\n", s->m_count);
+			b2Dump("    b2ChainShape shape;\n");
+			b2Dump("    b2Vec2 vs[%d];\n", s->m_count);
 			for (int32 i = 0; i < s->m_count; ++i)
 			{
-				b2Log("    vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
+				b2Dump("    vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
 			}
-			b2Log("    shape.CreateChain(vs, %d);\n", s->m_count);
-			b2Log("    shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y);
-			b2Log("    shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y);
-			b2Log("    shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex);
-			b2Log("    shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex);
+			b2Dump("    shape.CreateChain(vs, %d);\n", s->m_count);
+			b2Dump("    shape.m_prevVertex.Set(%.9g, %.9g);\n", s->m_prevVertex.x, s->m_prevVertex.y);
+			b2Dump("    shape.m_nextVertex.Set(%.9g, %.9g);\n", s->m_nextVertex.x, s->m_nextVertex.y);
 		}
 		break;
 
@@ -296,8 +297,8 @@ void b2Fixture::Dump(int32 bodyIndex)
 		return;
 	}
 
-	b2Log("\n");
-	b2Log("    fd.shape = &shape;\n");
-	b2Log("\n");
-	b2Log("    bodies[%d]->CreateFixture(&fd);\n", bodyIndex);
+	b2Dump("\n");
+	b2Dump("    fd.shape = &shape;\n");
+	b2Dump("\n");
+	b2Dump("    bodies[%d]->CreateFixture(&fd);\n", bodyIndex);
 }

src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp → src/libraries/Box2D/Dynamics/b2_friction_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_friction_joint.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_time_step.h"
 
 // Point-to-point constraint
 // Cdot = v2 - v1
@@ -64,13 +68,13 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIA = m_bodyA->m_invI;
 	m_invIB = m_bodyB->m_invI;
 
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -87,8 +91,8 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
 	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
 	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
 	b2Mat22 K;
 	K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
@@ -131,22 +135,22 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
-	float32 h = data.step.dt;
+	float h = data.step.dt;
 
 	// Solve angular friction
 	{
-		float32 Cdot = wB - wA;
-		float32 impulse = -m_angularMass * Cdot;
+		float Cdot = wB - wA;
+		float impulse = -m_angularMass * Cdot;
 
-		float32 oldImpulse = m_angularImpulse;
-		float32 maxImpulse = h * m_maxTorque;
+		float oldImpulse = m_angularImpulse;
+		float maxImpulse = h * m_maxTorque;
 		m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = m_angularImpulse - oldImpulse;
 
@@ -162,7 +166,7 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
 		b2Vec2 oldImpulse = m_linearImpulse;
 		m_linearImpulse += impulse;
 
-		float32 maxImpulse = h * m_maxForce;
+		float maxImpulse = h * m_maxForce;
 
 		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
 		{
@@ -202,34 +206,34 @@ b2Vec2 b2FrictionJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2FrictionJoint::GetReactionForce(float inv_dt) const
 {
 	return inv_dt * m_linearImpulse;
 }
 
-float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
+float b2FrictionJoint::GetReactionTorque(float inv_dt) const
 {
 	return inv_dt * m_angularImpulse;
 }
 
-void b2FrictionJoint::SetMaxForce(float32 force)
+void b2FrictionJoint::SetMaxForce(float force)
 {
 	b2Assert(b2IsValid(force) && force >= 0.0f);
 	m_maxForce = force;
 }
 
-float32 b2FrictionJoint::GetMaxForce() const
+float b2FrictionJoint::GetMaxForce() const
 {
 	return m_maxForce;
 }
 
-void b2FrictionJoint::SetMaxTorque(float32 torque)
+void b2FrictionJoint::SetMaxTorque(float torque)
 {
 	b2Assert(b2IsValid(torque) && torque >= 0.0f);
 	m_maxTorque = torque;
 }
 
-float32 b2FrictionJoint::GetMaxTorque() const
+float b2FrictionJoint::GetMaxTorque() const
 {
 	return m_maxTorque;
 }
@@ -239,13 +243,13 @@ void b2FrictionJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2FrictionJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.maxForce = %.15lef;\n", m_maxForce);
-	b2Log("  jd.maxTorque = %.15lef;\n", m_maxTorque);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2FrictionJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.maxForce = %.9g;\n", m_maxForce);
+	b2Dump("  jd.maxTorque = %.9g;\n", m_maxTorque);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp → src/libraries/Box2D/Dynamics/b2_gear_joint.cpp

@@ -1,26 +1,30 @@
-/*
-* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2GearJoint.h>
-#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
-#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_gear_joint.h"
+#include "box2d/b2_revolute_joint.h"
+#include "box2d/b2_prismatic_joint.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_time_step.h"
 
 // Gear Joint:
 // C0 = (coordinate1 + ratio * coordinate2)_initial
@@ -53,7 +57,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 	b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
 	b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
 
-	float32 coordinateA, coordinateB;
+	float coordinateA, coordinateB;
 
 	// TODO_ERIN there might be some problem with the joint edges in b2Joint.
 
@@ -62,9 +66,9 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 
 	// Get geometry of joint1
 	b2Transform xfA = m_bodyA->m_xf;
-	float32 aA = m_bodyA->m_sweep.a;
+	float aA = m_bodyA->m_sweep.a;
 	b2Transform xfC = m_bodyC->m_xf;
-	float32 aC = m_bodyC->m_sweep.a;
+	float aC = m_bodyC->m_sweep.a;
 
 	if (m_typeA == e_revoluteJoint)
 	{
@@ -94,9 +98,9 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 
 	// Get geometry of joint2
 	b2Transform xfB = m_bodyB->m_xf;
-	float32 aB = m_bodyB->m_sweep.a;
+	float aB = m_bodyB->m_sweep.a;
 	b2Transform xfD = m_bodyD->m_xf;
-	float32 aD = m_bodyD->m_sweep.a;
+	float aD = m_bodyD->m_sweep.a;
 
 	if (m_typeB == e_revoluteJoint)
 	{
@@ -147,21 +151,21 @@ void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_iC = m_bodyC->m_invI;
 	m_iD = m_bodyD->m_invI;
 
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
-	float32 aC = data.positions[m_indexC].a;
+	float aC = data.positions[m_indexC].a;
 	b2Vec2 vC = data.velocities[m_indexC].v;
-	float32 wC = data.velocities[m_indexC].w;
+	float wC = data.velocities[m_indexC].w;
 
-	float32 aD = data.positions[m_indexD].a;
+	float aD = data.positions[m_indexD].a;
 	b2Vec2 vD = data.velocities[m_indexD].v;
-	float32 wD = data.velocities[m_indexD].w;
+	float wD = data.velocities[m_indexD].w;
 
 	b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
 
@@ -235,18 +239,18 @@ void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 	b2Vec2 vC = data.velocities[m_indexC].v;
-	float32 wC = data.velocities[m_indexC].w;
+	float wC = data.velocities[m_indexC].w;
 	b2Vec2 vD = data.velocities[m_indexD].v;
-	float32 wD = data.velocities[m_indexD].w;
+	float wD = data.velocities[m_indexD].w;
 
-	float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
+	float Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
 	Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
 
-	float32 impulse = -m_mass * Cdot;
+	float impulse = -m_mass * Cdot;
 	m_impulse += impulse;
 
 	vA += (m_mA * impulse) * m_JvAC;
@@ -271,23 +275,23 @@ void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
 bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
 {
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 cC = data.positions[m_indexC].c;
-	float32 aC = data.positions[m_indexC].a;
+	float aC = data.positions[m_indexC].a;
 	b2Vec2 cD = data.positions[m_indexD].c;
-	float32 aD = data.positions[m_indexD].a;
+	float aD = data.positions[m_indexD].a;
 
 	b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
 
-	float32 linearError = 0.0f;
+	float linearError = 0.0f;
 
-	float32 coordinateA, coordinateB;
+	float coordinateA, coordinateB;
 
 	b2Vec2 JvAC, JvBD;
-	float32 JwA, JwB, JwC, JwD;
-	float32 mass = 0.0f;
+	float JwA, JwB, JwC, JwD;
+	float mass = 0.0f;
 
 	if (m_typeA == e_revoluteJoint)
 	{
@@ -337,9 +341,9 @@ bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
 		coordinateB = b2Dot(pB - pD, m_localAxisD);
 	}
 
-	float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
+	float C = (coordinateA + m_ratio * coordinateB) - m_constant;
 
-	float32 impulse = 0.0f;
+	float impulse = 0.0f;
 	if (mass > 0.0f)
 	{
 		impulse = -C / mass;
@@ -377,25 +381,25 @@ b2Vec2 b2GearJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2GearJoint::GetReactionForce(float inv_dt) const
 {
 	b2Vec2 P = m_impulse * m_JvAC;
 	return inv_dt * P;
 }
 
-float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
+float b2GearJoint::GetReactionTorque(float inv_dt) const
 {
-	float32 L = m_impulse * m_JwA;
+	float L = m_impulse * m_JwA;
 	return inv_dt * L;
 }
 
-void b2GearJoint::SetRatio(float32 ratio)
+void b2GearJoint::SetRatio(float ratio)
 {
 	b2Assert(b2IsValid(ratio));
 	m_ratio = ratio;
 }
 
-float32 b2GearJoint::GetRatio() const
+float b2GearJoint::GetRatio() const
 {
 	return m_ratio;
 }
@@ -408,12 +412,12 @@ void b2GearJoint::Dump()
 	int32 index1 = m_joint1->m_index;
 	int32 index2 = m_joint2->m_index;
 
-	b2Log("  b2GearJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.joint1 = joints[%d];\n", index1);
-	b2Log("  jd.joint2 = joints[%d];\n", index2);
-	b2Log("  jd.ratio = %.15lef;\n", m_ratio);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2GearJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.joint1 = joints[%d];\n", index1);
+	b2Dump("  jd.joint2 = joints[%d];\n", index2);
+	b2Dump("  jd.ratio = %.9g;\n", m_ratio);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/b2Island.cpp → src/libraries/Box2D/Dynamics/b2_island.cpp

@@ -1,31 +1,36 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Collision/b2Distance.h>
-#include <Box2D/Dynamics/b2Island.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-#include <Box2D/Common/b2StackAllocator.h>
-#include <Box2D/Common/b2Timer.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_distance.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_joint.h"
+#include "box2d/b2_stack_allocator.h"
+#include "box2d/b2_timer.h"
+#include "box2d/b2_world.h"
+
+#include "b2_island.h"
+#include "dynamics/b2_contact_solver.h"
 
 /*
 Position Correction Notes
@@ -184,7 +189,7 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 {
 	b2Timer timer;
 
-	float32 h = step.dt;
+	float h = step.dt;
 
 	// Integrate velocities and apply damping. Initialize the body state.
 	for (int32 i = 0; i < m_bodyCount; ++i)
@@ -192,9 +197,9 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 		b2Body* b = m_bodies[i];
 
 		b2Vec2 c = b->m_sweep.c;
-		float32 a = b->m_sweep.a;
+		float a = b->m_sweep.a;
 		b2Vec2 v = b->m_linearVelocity;
-		float32 w = b->m_angularVelocity;
+		float w = b->m_angularVelocity;
 
 		// Store positions for continuous collision.
 		b->m_sweep.c0 = b->m_sweep.c;
@@ -203,7 +208,7 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 		if (b->m_type == b2_dynamicBody)
 		{
 			// Integrate velocities.
-			v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
+			v += h * b->m_invMass * (b->m_gravityScale * b->m_mass * gravity + b->m_force);
 			w += h * b->m_invI * b->m_torque;
 
 			// Apply damping.
@@ -275,22 +280,22 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 	for (int32 i = 0; i < m_bodyCount; ++i)
 	{
 		b2Vec2 c = m_positions[i].c;
-		float32 a = m_positions[i].a;
+		float a = m_positions[i].a;
 		b2Vec2 v = m_velocities[i].v;
-		float32 w = m_velocities[i].w;
+		float w = m_velocities[i].w;
 
 		// Check for large velocities
 		b2Vec2 translation = h * v;
 		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
 		{
-			float32 ratio = b2_maxTranslation / translation.Length();
+			float ratio = b2_maxTranslation / translation.Length();
 			v *= ratio;
 		}
 
-		float32 rotation = h * w;
+		float rotation = h * w;
 		if (rotation * rotation > b2_maxRotationSquared)
 		{
-			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			float ratio = b2_maxRotation / b2Abs(rotation);
 			w *= ratio;
 		}
 
@@ -312,9 +317,9 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 		bool contactsOkay = contactSolver.SolvePositionConstraints();
 
 		bool jointsOkay = true;
-		for (int32 i = 0; i < m_jointCount; ++i)
+		for (int32 j = 0; j < m_jointCount; ++j)
 		{
-			bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
+			bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData);
 			jointsOkay = jointsOkay && jointOkay;
 		}
 
@@ -343,10 +348,10 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 
 	if (allowSleep)
 	{
-		float32 minSleepTime = b2_maxFloat;
+		float minSleepTime = b2_maxFloat;
 
-		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
-		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
+		const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
+		const float angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
 
 		for (int32 i = 0; i < m_bodyCount; ++i)
 		{
@@ -467,28 +472,28 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd
 	// Don't store the TOI contact forces for warm starting
 	// because they can be quite large.
 
-	float32 h = subStep.dt;
+	float h = subStep.dt;
 
 	// Integrate positions
 	for (int32 i = 0; i < m_bodyCount; ++i)
 	{
 		b2Vec2 c = m_positions[i].c;
-		float32 a = m_positions[i].a;
+		float a = m_positions[i].a;
 		b2Vec2 v = m_velocities[i].v;
-		float32 w = m_velocities[i].w;
+		float w = m_velocities[i].w;
 
 		// Check for large velocities
 		b2Vec2 translation = h * v;
 		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
 		{
-			float32 ratio = b2_maxTranslation / translation.Length();
+			float ratio = b2_maxTranslation / translation.Length();
 			v *= ratio;
 		}
 
-		float32 rotation = h * w;
+		float rotation = h * w;
 		if (rotation * rotation > b2_maxRotationSquared)
 		{
-			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			float ratio = b2_maxRotation / b2Abs(rotation);
 			w *= ratio;
 		}
 
@@ -515,7 +520,7 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd
 
 void b2Island::Report(const b2ContactVelocityConstraint* constraints)
 {
-	if (m_listener == NULL)
+	if (m_listener == nullptr)
 	{
 		return;
 	}

src/libraries/Box2D/Dynamics/b2Island.h → src/libraries/Box2D/Dynamics/b2_island.h

@@ -1,27 +1,31 @@
-/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* 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.
-*/
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
 
 #ifndef B2_ISLAND_H
 #define B2_ISLAND_H
 
-#include <Box2D/Common/b2Math.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+#include "box2d/b2_body.h"
+#include "box2d/b2_math.h"
+#include "box2d/b2_time_step.h"
 
 class b2Contact;
 class b2Joint;

src/libraries/Box2D/Dynamics/b2_joint.cpp

@@ -0,0 +1,313 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_distance_joint.h"
+#include "box2d/b2_draw.h"
+#include "box2d/b2_friction_joint.h"
+#include "box2d/b2_gear_joint.h"
+#include "box2d/b2_motor_joint.h"
+#include "box2d/b2_mouse_joint.h"
+#include "box2d/b2_prismatic_joint.h"
+#include "box2d/b2_pulley_joint.h"
+#include "box2d/b2_revolute_joint.h"
+#include "box2d/b2_rope_joint.h"
+#include "box2d/b2_weld_joint.h"
+#include "box2d/b2_wheel_joint.h"
+#include "box2d/b2_world.h"
+
+#include <new>
+
+void b2LinearStiffness(float& stiffness, float& damping,
+	float frequencyHertz, float dampingRatio,
+	const b2Body* bodyA, const b2Body* bodyB)
+{
+	float massA = bodyA->GetMass();
+	float massB = bodyB->GetMass();
+	float mass;
+	if (massA > 0.0f && massB > 0.0f)
+	{
+		mass = massA * massB / (massA + massB);
+	}
+	else if (massA > 0.0f)
+	{
+		mass = massA;
+	}
+	else
+	{
+		mass = massB;
+	}
+
+	float omega = 2.0f * b2_pi * frequencyHertz;
+	stiffness = mass * omega * omega;
+	damping = 2.0f * mass * dampingRatio * omega;
+}
+
+void b2AngularStiffness(float& stiffness, float& damping,
+	float frequencyHertz, float dampingRatio,
+	const b2Body* bodyA, const b2Body* bodyB)
+{
+	float IA = bodyA->GetInertia();
+	float IB = bodyB->GetInertia();
+	float I;
+	if (IA > 0.0f && IB > 0.0f)
+	{
+		I = IA * IB / (IA + IB);
+	}
+	else if (IA > 0.0f)
+	{
+		I = IA;
+	}
+	else
+	{
+		I = IB;
+	}
+
+	float omega = 2.0f * b2_pi * frequencyHertz;
+	stiffness = I * omega * omega;
+	damping = 2.0f * I * dampingRatio * omega;
+}
+
+b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
+{
+	b2Joint* joint = nullptr;
+
+	switch (def->type)
+	{
+	case e_distanceJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
+			joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
+		}
+		break;
+
+	case e_mouseJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2MouseJoint));
+			joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
+		}
+		break;
+
+	case e_prismaticJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
+			joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
+		}
+		break;
+
+	case e_revoluteJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
+			joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
+		}
+		break;
+
+	case e_pulleyJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
+			joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
+		}
+		break;
+
+	case e_gearJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2GearJoint));
+			joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
+		}
+		break;
+
+	case e_wheelJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2WheelJoint));
+			joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
+		}
+		break;
+
+	case e_weldJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2WeldJoint));
+			joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
+		}
+		break;
+        
+	case e_frictionJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
+			joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
+		}
+		break;
+
+	case e_ropeJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2RopeJoint));
+			joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));
+		}
+		break;
+
+	case e_motorJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2MotorJoint));
+			joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	return joint;
+}
+
+void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
+{
+	joint->~b2Joint();
+	switch (joint->m_type)
+	{
+	case e_distanceJoint:
+		allocator->Free(joint, sizeof(b2DistanceJoint));
+		break;
+
+	case e_mouseJoint:
+		allocator->Free(joint, sizeof(b2MouseJoint));
+		break;
+
+	case e_prismaticJoint:
+		allocator->Free(joint, sizeof(b2PrismaticJoint));
+		break;
+
+	case e_revoluteJoint:
+		allocator->Free(joint, sizeof(b2RevoluteJoint));
+		break;
+
+	case e_pulleyJoint:
+		allocator->Free(joint, sizeof(b2PulleyJoint));
+		break;
+
+	case e_gearJoint:
+		allocator->Free(joint, sizeof(b2GearJoint));
+		break;
+
+	case e_wheelJoint:
+		allocator->Free(joint, sizeof(b2WheelJoint));
+		break;
+    
+	case e_weldJoint:
+		allocator->Free(joint, sizeof(b2WeldJoint));
+		break;
+
+	case e_frictionJoint:
+		allocator->Free(joint, sizeof(b2FrictionJoint));
+		break;
+
+	case e_ropeJoint:
+		allocator->Free(joint, sizeof(b2RopeJoint));
+		break;
+
+	case e_motorJoint:
+		allocator->Free(joint, sizeof(b2MotorJoint));
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+	b2Assert(def->bodyA != def->bodyB);
+
+	m_type = def->type;
+	m_prev = nullptr;
+	m_next = nullptr;
+	m_bodyA = def->bodyA;
+	m_bodyB = def->bodyB;
+	m_index = 0;
+	m_collideConnected = def->collideConnected;
+	m_islandFlag = false;
+	m_userData = def->userData;
+
+	m_edgeA.joint = nullptr;
+	m_edgeA.other = nullptr;
+	m_edgeA.prev = nullptr;
+	m_edgeA.next = nullptr;
+
+	m_edgeB.joint = nullptr;
+	m_edgeB.other = nullptr;
+	m_edgeB.prev = nullptr;
+	m_edgeB.next = nullptr;
+}
+
+bool b2Joint::IsEnabled() const
+{
+	return m_bodyA->IsEnabled() && m_bodyB->IsEnabled();
+}
+
+void b2Joint::Draw(b2Draw* draw) const
+{
+	const b2Transform& xf1 = m_bodyA->GetTransform();
+	const b2Transform& xf2 = m_bodyB->GetTransform();
+	b2Vec2 x1 = xf1.p;
+	b2Vec2 x2 = xf2.p;
+	b2Vec2 p1 = GetAnchorA();
+	b2Vec2 p2 = GetAnchorB();
+
+	b2Color color(0.5f, 0.8f, 0.8f);
+
+	switch (m_type)
+	{
+	case e_distanceJoint:
+		draw->DrawSegment(p1, p2, color);
+		break;
+
+	case e_pulleyJoint:
+	{
+		b2PulleyJoint* pulley = (b2PulleyJoint*)this;
+		b2Vec2 s1 = pulley->GetGroundAnchorA();
+		b2Vec2 s2 = pulley->GetGroundAnchorB();
+		draw->DrawSegment(s1, p1, color);
+		draw->DrawSegment(s2, p2, color);
+		draw->DrawSegment(s1, s2, color);
+	}
+	break;
+
+	case e_mouseJoint:
+	{
+		b2Color c;
+		c.Set(0.0f, 1.0f, 0.0f);
+		draw->DrawPoint(p1, 4.0f, c);
+		draw->DrawPoint(p2, 4.0f, c);
+
+		c.Set(0.8f, 0.8f, 0.8f);
+		draw->DrawSegment(p1, p2, c);
+
+	}
+	break;
+
+	default:
+		draw->DrawSegment(x1, p1, color);
+		draw->DrawSegment(p1, p2, color);
+		draw->DrawSegment(x2, p2, color);
+	}
+}

src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp → src/libraries/Box2D/Dynamics/b2_motor_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_motor_joint.h"
+#include "box2d/b2_time_step.h"
 
 // Point-to-point constraint
 // Cdot = v2 - v1
@@ -26,6 +30,9 @@
 // J = [-I -r1_skew I r2_skew ]
 // Identity used:
 // w k % (rx i + ry j) = w * (-ry i + rx j)
+//
+// r1 = offset - c1
+// r2 = -c2
 
 // Angle constraint
 // Cdot = w2 - w1
@@ -39,8 +46,8 @@ void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)
 	b2Vec2 xB = bodyB->GetPosition();
 	linearOffset = bodyA->GetLocalPoint(xB);
 
-	float32 angleA = bodyA->GetAngle();
-	float32 angleB = bodyB->GetAngle();
+	float angleA = bodyA->GetAngle();
+	float angleB = bodyB->GetAngle();
 	angularOffset = angleB - angleA;
 }
 
@@ -70,23 +77,22 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIB = m_bodyB->m_invI;
 
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
 	// Compute the effective mass matrix.
-	m_rA = b2Mul(qA, -m_localCenterA);
+	m_rA = b2Mul(qA, m_linearOffset - m_localCenterA);
 	m_rB = b2Mul(qB, -m_localCenterB);
 
 	// J = [-I -r1_skew I r2_skew]
-	//     [ 0       -1 0       1]
 	// r_skew = [-ry; rx]
 
 	// Matlab
@@ -94,9 +100,10 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
 	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
 	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
+	// Upper 2 by 2 of K for point to point
 	b2Mat22 K;
 	K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
 	K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
@@ -111,7 +118,7 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
 		m_angularMass = 1.0f / m_angularMass;
 	}
 
-	m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
+	m_linearError = cB + m_rB - cA - m_rA;
 	m_angularError = aB - aA - m_angularOffset;
 
 	if (data.step.warmStarting)
@@ -141,23 +148,23 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
-	float32 h = data.step.dt;
-	float32 inv_h = data.step.inv_dt;
+	float h = data.step.dt;
+	float inv_h = data.step.inv_dt;
 
 	// Solve angular friction
 	{
-		float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
-		float32 impulse = -m_angularMass * Cdot;
+		float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
+		float impulse = -m_angularMass * Cdot;
 
-		float32 oldImpulse = m_angularImpulse;
-		float32 maxImpulse = h * m_maxTorque;
+		float oldImpulse = m_angularImpulse;
+		float maxImpulse = h * m_maxTorque;
 		m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = m_angularImpulse - oldImpulse;
 
@@ -173,7 +180,7 @@ void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
 		b2Vec2 oldImpulse = m_linearImpulse;
 		m_linearImpulse += impulse;
 
-		float32 maxImpulse = h * m_maxForce;
+		float maxImpulse = h * m_maxForce;
 
 		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
 		{
@@ -213,45 +220,45 @@ b2Vec2 b2MotorJoint::GetAnchorB() const
 	return m_bodyB->GetPosition();
 }
 
-b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2MotorJoint::GetReactionForce(float inv_dt) const
 {
 	return inv_dt * m_linearImpulse;
 }
 
-float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const
+float b2MotorJoint::GetReactionTorque(float inv_dt) const
 {
 	return inv_dt * m_angularImpulse;
 }
 
-void b2MotorJoint::SetMaxForce(float32 force)
+void b2MotorJoint::SetMaxForce(float force)
 {
 	b2Assert(b2IsValid(force) && force >= 0.0f);
 	m_maxForce = force;
 }
 
-float32 b2MotorJoint::GetMaxForce() const
+float b2MotorJoint::GetMaxForce() const
 {
 	return m_maxForce;
 }
 
-void b2MotorJoint::SetMaxTorque(float32 torque)
+void b2MotorJoint::SetMaxTorque(float torque)
 {
 	b2Assert(b2IsValid(torque) && torque >= 0.0f);
 	m_maxTorque = torque;
 }
 
-float32 b2MotorJoint::GetMaxTorque() const
+float b2MotorJoint::GetMaxTorque() const
 {
 	return m_maxTorque;
 }
 
-void b2MotorJoint::SetCorrectionFactor(float32 factor)
+void b2MotorJoint::SetCorrectionFactor(float factor)
 {
 	b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
 	m_correctionFactor = factor;
 }
 
-float32 b2MotorJoint::GetCorrectionFactor() const
+float b2MotorJoint::GetCorrectionFactor() const
 {
 	return m_correctionFactor;
 }
@@ -271,7 +278,7 @@ const b2Vec2& b2MotorJoint::GetLinearOffset() const
 	return m_linearOffset;
 }
 
-void b2MotorJoint::SetAngularOffset(float32 angularOffset)
+void b2MotorJoint::SetAngularOffset(float angularOffset)
 {
 	if (angularOffset != m_angularOffset)
 	{
@@ -281,7 +288,7 @@ void b2MotorJoint::SetAngularOffset(float32 angularOffset)
 	}
 }
 
-float32 b2MotorJoint::GetAngularOffset() const
+float b2MotorJoint::GetAngularOffset() const
 {
 	return m_angularOffset;
 }
@@ -291,14 +298,14 @@ void b2MotorJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2MotorJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);
-	b2Log("  jd.angularOffset = %.15lef;\n", m_angularOffset);
-	b2Log("  jd.maxForce = %.15lef;\n", m_maxForce);
-	b2Log("  jd.maxTorque = %.15lef;\n", m_maxTorque);
-	b2Log("  jd.correctionFactor = %.15lef;\n", m_correctionFactor);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2MotorJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.linearOffset.Set(%.9g, %.9g);\n", m_linearOffset.x, m_linearOffset.y);
+	b2Dump("  jd.angularOffset = %.9g;\n", m_angularOffset);
+	b2Dump("  jd.maxForce = %.9g;\n", m_maxForce);
+	b2Dump("  jd.maxTorque = %.9g;\n", m_maxTorque);
+	b2Dump("  jd.correctionFactor = %.9g;\n", m_correctionFactor);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp → src/libraries/Box2D/Dynamics/b2_mouse_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_mouse_joint.h"
+#include "box2d/b2_time_step.h"
 
 // p = attached point, m = mouse point
 // C = p - m
@@ -31,31 +35,24 @@
 b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
 : b2Joint(def)
 {
-	b2Assert(def->target.IsValid());
-	b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
-	b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
-	b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
-
 	m_targetA = def->target;
 	m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
-
 	m_maxForce = def->maxForce;
-	m_impulse.SetZero();
-
-	m_frequencyHz = def->frequencyHz;
-	m_dampingRatio = def->dampingRatio;
+	m_stiffness = def->stiffness;
+	m_damping = def->damping;
 
+	m_impulse.SetZero();
 	m_beta = 0.0f;
 	m_gamma = 0.0f;
 }
 
 void b2MouseJoint::SetTarget(const b2Vec2& target)
 {
-	if (m_bodyB->IsAwake() == false)
+	if (target != m_targetA)
 	{
 		m_bodyB->SetAwake(true);
+		m_targetA = target;
 	}
-	m_targetA = target;
 }
 
 const b2Vec2& b2MouseJoint::GetTarget() const
@@ -63,36 +60,16 @@ const b2Vec2& b2MouseJoint::GetTarget() const
 	return m_targetA;
 }
 
-void b2MouseJoint::SetMaxForce(float32 force)
+void b2MouseJoint::SetMaxForce(float force)
 {
 	m_maxForce = force;
 }
 
-float32 b2MouseJoint::GetMaxForce() const
+float b2MouseJoint::GetMaxForce() const
 {
 	return m_maxForce;
 }
 
-void b2MouseJoint::SetFrequency(float32 hz)
-{
-	m_frequencyHz = hz;
-}
-
-float32 b2MouseJoint::GetFrequency() const
-{
-	return m_frequencyHz;
-}
-
-void b2MouseJoint::SetDampingRatio(float32 ratio)
-{
-	m_dampingRatio = ratio;
-}
-
-float32 b2MouseJoint::GetDampingRatio() const
-{
-	return m_dampingRatio;
-}
-
 void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
 {
 	m_indexB = m_bodyB->m_islandIndex;
@@ -101,28 +78,21 @@ void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIB = m_bodyB->m_invI;
 
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qB(aB);
 
-	float32 mass = m_bodyB->GetMass();
-
-	// Frequency
-	float32 omega = 2.0f * b2_pi * m_frequencyHz;
-
-	// Damping coefficient
-	float32 d = 2.0f * mass * m_dampingRatio * omega;
+	float mass = m_bodyB->GetMass();
 
-	// Spring stiffness
-	float32 k = mass * (omega * omega);
+	float d = m_damping;
+	float k = m_stiffness;
 
 	// magic formulas
 	// gamma has units of inverse mass.
 	// beta has units of inverse time.
-	float32 h = data.step.dt;
-	b2Assert(d + h * k > b2_epsilon);
+	float h = data.step.dt;
 	m_gamma = h * (d + h * k);
 	if (m_gamma != 0.0f)
 	{
@@ -168,7 +138,7 @@ void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	// Cdot = v + cross(w, r)
 	b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
@@ -176,7 +146,7 @@ void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
 
 	b2Vec2 oldImpulse = m_impulse;
 	m_impulse += impulse;
-	float32 maxImpulse = data.step.dt * m_maxForce;
+	float maxImpulse = data.step.dt * m_maxForce;
 	if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
 	{
 		m_impulse *= maxImpulse / m_impulse.Length();
@@ -206,12 +176,12 @@ b2Vec2 b2MouseJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const
 {
 	return inv_dt * m_impulse;
 }
 
-float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
+float b2MouseJoint::GetReactionTorque(float inv_dt) const
 {
 	return inv_dt * 0.0f;
 }

src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.cpp

@@ -0,0 +1,54 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_polygon_circle_contact.h"
+
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_fixture.h"
+
+#include <new>
+
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
+	return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
+	allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
+}
+
+b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygonAndCircle(	manifold,
+								(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.h

@@ -0,0 +1,42 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H
+#define B2_POLYGON_AND_CIRCLE_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2PolygonAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2_polygon_contact.cpp

@@ -0,0 +1,57 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_polygon_contact.h"
+
+#include "box2d/b2_block_allocator.h"
+#include "box2d/b2_body.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_time_of_impact.h"
+#include "box2d/b2_world_callbacks.h"
+
+#include <new>
+
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
+	return new (mem) b2PolygonContact(fixtureA, fixtureB);
+}
+
+void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonContact*)contact)->~b2PolygonContact();
+	allocator->Free(contact, sizeof(b2PolygonContact));
+}
+
+b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygons(	manifold,
+						(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+						(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/libraries/Box2D/Dynamics/b2_polygon_contact.h

@@ -0,0 +1,43 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#ifndef B2_POLYGON_CONTACT_H
+#define B2_POLYGON_CONTACT_H
+
+#include "box2d/b2_contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2PolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
+};
+
+#endif

src/libraries/Box2D/Dynamics/b2_prismatic_joint.cpp

@@ -0,0 +1,644 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_draw.h"
+#include "box2d/b2_prismatic_joint.h"
+#include "box2d/b2_time_step.h"
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(perp, d)
+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+//     [0  -1  0  1]
+// a = perp
+// s1 = cross(d + r1, a) = cross(p2 - x1, a)
+// s2 = cross(r2, a) = cross(p2 - x2, a)
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+// Predictive limit is applied even when the limit is not active.
+// Prevents a constraint speed that can lead to a constraint error in one time step.
+// Want C2 = C1 + h * Cdot >= 0
+// Or:
+// Cdot + C1/h >= 0
+// I do not apply a negative constraint error because that is handled in position correction.
+// So:
+// Cdot + max(C1, 0)/h >= 0
+
+// Block Solver
+// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.
+//
+// The Jacobian has 2 rows:
+// J = [-uT -s1 uT s2] // linear
+//     [0   -1   0  1] // angular
+//
+// u = perp
+// s1 = cross(d + r1, u), s2 = cross(r2, u)
+// a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	localAxisA = bodyA->GetLocalVector(axis);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_localXAxisA = def->localAxisA;
+	m_localXAxisA.Normalize();
+	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+	m_axialMass = 0.0f;
+	m_motorImpulse = 0.0f;
+	m_lowerImpulse = 0.0f;
+	m_upperImpulse = 0.0f;
+
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+
+	b2Assert(m_lowerTranslation <= m_upperTranslation);
+
+	m_maxMotorForce = def->maxMotorForce;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+
+	m_translation = 0.0f;
+	m_axis.SetZero();
+	m_perp.SetZero();
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+	m_indexA = m_bodyA->m_islandIndex;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	m_invMassA = m_bodyA->m_invMass;
+	m_invMassB = m_bodyB->m_invMass;
+	m_invIA = m_bodyA->m_invI;
+	m_invIB = m_bodyB->m_invI;
+
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float aB = data.positions[m_indexB].a;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	b2Rot qA(aA), qB(aB);
+
+	// Compute the effective masses.
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = (cB - cA) + rB - rA;
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	// Compute motor Jacobian and effective mass.
+	{
+		m_axis = b2Mul(qA, m_localXAxisA);
+		m_a1 = b2Cross(d + rA, m_axis);
+		m_a2 = b2Cross(rB, m_axis);
+
+		m_axialMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
+		if (m_axialMass > 0.0f)
+		{
+			m_axialMass = 1.0f / m_axialMass;
+		}
+	}
+
+	// Prismatic constraint.
+	{
+		m_perp = b2Mul(qA, m_localYAxisA);
+
+		m_s1 = b2Cross(d + rA, m_perp);
+		m_s2 = b2Cross(rB, m_perp);
+
+		float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
+		float k12 = iA * m_s1 + iB * m_s2;
+		float k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			// For bodies with fixed rotation.
+			k22 = 1.0f;
+		}
+
+		m_K.ex.Set(k11, k12);
+		m_K.ey.Set(k12, k22);
+	}
+
+	if (m_enableLimit)
+	{
+		m_translation = b2Dot(m_axis, d);
+	}
+	else
+	{
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (data.step.warmStarting)
+	{
+		// Account for variable time step.
+		m_impulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
+		m_lowerImpulse *= data.step.dtRatio;
+		m_upperImpulse *= data.step.dtRatio;
+
+		float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse;
+		b2Vec2 P = m_impulse.x * m_perp + axialImpulse * m_axis;
+		float LA = m_impulse.x * m_s1 + m_impulse.y + axialImpulse * m_a1;
+		float LB = m_impulse.x * m_s2 + m_impulse.y + axialImpulse * m_a2;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	// Solve linear motor constraint
+	if (m_enableMotor)
+	{
+		float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
+		float impulse = m_axialMass * (m_motorSpeed - Cdot);
+		float oldImpulse = m_motorImpulse;
+		float maxImpulse = data.step.dt * m_maxMotorForce;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		b2Vec2 P = impulse * m_axis;
+		float LA = impulse * m_a1;
+		float LB = impulse * m_a2;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	if (m_enableLimit)
+	{
+		// Lower limit
+		{
+			float C = m_translation - m_lowerTranslation;
+			float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_lowerImpulse;
+			m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
+			impulse = m_lowerImpulse - oldImpulse;
+
+			b2Vec2 P = impulse * m_axis;
+			float LA = impulse * m_a1;
+			float LB = impulse * m_a2;
+
+			vA -= mA * P;
+			wA -= iA * LA;
+			vB += mB * P;
+			wB += iB * LB;
+		}
+
+		// Upper limit
+		// Note: signs are flipped to keep C positive when the constraint is satisfied.
+		// This also keeps the impulse positive when the limit is active.
+		{
+			float C = m_upperTranslation - m_translation;
+			float Cdot = b2Dot(m_axis, vA - vB) + m_a1 * wA - m_a2 * wB;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_upperImpulse;
+			m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
+			impulse = m_upperImpulse - oldImpulse;
+
+			b2Vec2 P = impulse * m_axis;
+			float LA = impulse * m_a1;
+			float LB = impulse * m_a2;
+
+			vA += mA * P;
+			wA += iA * LA;
+			vB -= mB * P;
+			wB -= iB * LB;
+		}
+	}
+
+	// Solve the prismatic constraint in block form.
+	{
+		b2Vec2 Cdot;
+		Cdot.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
+		Cdot.y = wB - wA;
+
+		b2Vec2 df = m_K.Solve(-Cdot);
+		m_impulse += df;
+
+		b2Vec2 P = df.x * m_perp;
+		float LA = df.x * m_s1 + df.y;
+		float LB = df.x * m_s2 + df.y;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+// A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context,
+// the position solver is not there to resolve forces.It is only there to cope with integration error.
+//
+// Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck.
+//
+// We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity
+// solver indicates the limit is inactive.
+bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float aB = data.positions[m_indexB].a;
+
+	b2Rot qA(aA), qB(aB);
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	// Compute fresh Jacobians
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = cB + rB - cA - rA;
+
+	b2Vec2 axis = b2Mul(qA, m_localXAxisA);
+	float a1 = b2Cross(d + rA, axis);
+	float a2 = b2Cross(rB, axis);
+	b2Vec2 perp = b2Mul(qA, m_localYAxisA);
+
+	float s1 = b2Cross(d + rA, perp);
+	float s2 = b2Cross(rB, perp);
+
+	b2Vec3 impulse;
+	b2Vec2 C1;
+	C1.x = b2Dot(perp, d);
+	C1.y = aB - aA - m_referenceAngle;
+
+	float linearError = b2Abs(C1.x);
+	float angularError = b2Abs(C1.y);
+
+	bool active = false;
+	float C2 = 0.0f;
+	if (m_enableLimit)
+	{
+		float translation = b2Dot(axis, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			C2 = translation;
+			linearError = b2Max(linearError, b2Abs(translation));
+			active = true;
+		}
+		else if (translation <= m_lowerTranslation)
+		{
+			C2 = b2Min(translation - m_lowerTranslation, 0.0f);
+			linearError = b2Max(linearError, m_lowerTranslation - translation);
+			active = true;
+		}
+		else if (translation >= m_upperTranslation)
+		{
+			C2 = b2Max(translation - m_upperTranslation, 0.0f);
+			linearError = b2Max(linearError, translation - m_upperTranslation);
+			active = true;
+		}
+	}
+
+	if (active)
+	{
+		float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+		float k12 = iA * s1 + iB * s2;
+		float k13 = iA * s1 * a1 + iB * s2 * a2;
+		float k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			// For fixed rotation
+			k22 = 1.0f;
+		}
+		float k23 = iA * a1 + iB * a2;
+		float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
+
+		b2Mat33 K;
+		K.ex.Set(k11, k12, k13);
+		K.ey.Set(k12, k22, k23);
+		K.ez.Set(k13, k23, k33);
+
+		b2Vec3 C;
+		C.x = C1.x;
+		C.y = C1.y;
+		C.z = C2;
+
+		impulse = K.Solve33(-C);
+	}
+	else
+	{
+		float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+		float k12 = iA * s1 + iB * s2;
+		float k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			k22 = 1.0f;
+		}
+
+		b2Mat22 K;
+		K.ex.Set(k11, k12);
+		K.ey.Set(k12, k22);
+
+		b2Vec2 impulse1 = K.Solve(-C1);
+		impulse.x = impulse1.x;
+		impulse.y = impulse1.y;
+		impulse.z = 0.0f;
+	}
+
+	b2Vec2 P = impulse.x * perp + impulse.z * axis;
+	float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
+	float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
+
+	cA -= mA * P;
+	aA -= iA * LA;
+	cB += mB * P;
+	aB += iB * LB;
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
+	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce(float inv_dt) const
+{
+	return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_lowerImpulse + m_upperImpulse) * m_axis);
+}
+
+float b2PrismaticJoint::GetReactionTorque(float inv_dt) const
+{
+	return inv_dt * m_impulse.y;
+}
+
+float b2PrismaticJoint::GetJointTranslation() const
+{
+	b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
+	b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
+	b2Vec2 d = pB - pA;
+	b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);
+
+	float translation = b2Dot(d, axis);
+	return translation;
+}
+
+float b2PrismaticJoint::GetJointSpeed() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
+	b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
+	b2Vec2 p1 = bA->m_sweep.c + rA;
+	b2Vec2 p2 = bB->m_sweep.c + rB;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float wA = bA->m_angularVelocity;
+	float wB = bB->m_angularVelocity;
+
+	float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
+	return speed;
+}
+
+bool b2PrismaticJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2PrismaticJoint::EnableLimit(bool flag)
+{
+	if (flag != m_enableLimit)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableLimit = flag;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+}
+
+float b2PrismaticJoint::GetLowerLimit() const
+{
+	return m_lowerTranslation;
+}
+
+float b2PrismaticJoint::GetUpperLimit() const
+{
+	return m_upperTranslation;
+}
+
+void b2PrismaticJoint::SetLimits(float lower, float upper)
+{
+	b2Assert(lower <= upper);
+	if (lower != m_lowerTranslation || upper != m_upperTranslation)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_lowerTranslation = lower;
+		m_upperTranslation = upper;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+	if (flag != m_enableMotor)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableMotor = flag;
+	}
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float speed)
+{
+	if (speed != m_motorSpeed)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_motorSpeed = speed;
+	}
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float force)
+{
+	if (force != m_maxMotorForce)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_maxMotorForce = force;
+	}
+}
+
+float b2PrismaticJoint::GetMotorForce(float inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+void b2PrismaticJoint::Dump()
+{
+	// FLT_DECIMAL_DIG == 9
+
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2PrismaticJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.enableLimit = bool(%d);\n", m_enableLimit);
+	b2Dump("  jd.lowerTranslation = %.9g;\n", m_lowerTranslation);
+	b2Dump("  jd.upperTranslation = %.9g;\n", m_upperTranslation);
+	b2Dump("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Dump("  jd.motorSpeed = %.9g;\n", m_motorSpeed);
+	b2Dump("  jd.maxMotorForce = %.9g;\n", m_maxMotorForce);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+
+///
+void b2PrismaticJoint::Draw(b2Draw* draw) const
+{
+	const b2Transform& xfA = m_bodyA->GetTransform();
+	const b2Transform& xfB = m_bodyB->GetTransform();
+	b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
+	b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
+
+	b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA);
+
+	b2Color c1(0.7f, 0.7f, 0.7f);
+	b2Color c2(0.3f, 0.9f, 0.3f);
+	b2Color c3(0.9f, 0.3f, 0.3f);
+	b2Color c4(0.3f, 0.3f, 0.9f);
+	b2Color c5(0.4f, 0.4f, 0.4f);
+
+	draw->DrawSegment(pA, pB, c5);
+
+	if (m_enableLimit)
+	{
+		b2Vec2 lower = pA + m_lowerTranslation * axis;
+		b2Vec2 upper = pA + m_upperTranslation * axis;
+		b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA);
+		draw->DrawSegment(lower, upper, c1);
+		draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2);
+		draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3);
+	}
+	else
+	{
+		draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1);
+	}
+
+	draw->DrawPoint(pA, 5.0f, c1);
+	draw->DrawPoint(pB, 5.0f, c4);
+}

src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp → src/libraries/Box2D/Dynamics/b2_pulley_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_pulley_joint.h"
+#include "box2d/b2_time_step.h"
 
 // Pulley:
 // length1 = norm(p1 - s1)
@@ -35,7 +39,7 @@
 void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,
 				const b2Vec2& groundA, const b2Vec2& groundB,
 				const b2Vec2& anchorA, const b2Vec2& anchorB,
-				float32 r)
+				float r)
 {
 	bodyA = bA;
 	bodyB = bB;
@@ -82,14 +86,14 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIB = m_bodyB->m_invI;
 
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -100,8 +104,8 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_uA = cA + m_rA - m_groundAnchorA;
 	m_uB = cB + m_rB - m_groundAnchorB;
 
-	float32 lengthA = m_uA.Length();
-	float32 lengthB = m_uB.Length();
+	float lengthA = m_uA.Length();
+	float lengthB = m_uB.Length();
 
 	if (lengthA > 10.0f * b2_linearSlop)
 	{
@@ -122,11 +126,11 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
 	}
 
 	// Compute effective mass.
-	float32 ruA = b2Cross(m_rA, m_uA);
-	float32 ruB = b2Cross(m_rB, m_uB);
+	float ruA = b2Cross(m_rA, m_uA);
+	float ruB = b2Cross(m_rB, m_uB);
 
-	float32 mA = m_invMassA + m_invIA * ruA * ruA;
-	float32 mB = m_invMassB + m_invIB * ruB * ruB;
+	float mA = m_invMassA + m_invIA * ruA * ruA;
+	float mB = m_invMassB + m_invIB * ruB * ruB;
 
 	m_mass = mA + m_ratio * m_ratio * mB;
 
@@ -163,15 +167,15 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
 	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
 
-	float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
-	float32 impulse = -m_mass * Cdot;
+	float Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
+	float impulse = -m_mass * Cdot;
 	m_impulse += impulse;
 
 	b2Vec2 PA = -impulse * m_uA;
@@ -190,9 +194,9 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)
 bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
 {
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -203,8 +207,8 @@ bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
 	b2Vec2 uA = cA + rA - m_groundAnchorA;
 	b2Vec2 uB = cB + rB - m_groundAnchorB;
 
-	float32 lengthA = uA.Length();
-	float32 lengthB = uB.Length();
+	float lengthA = uA.Length();
+	float lengthB = uB.Length();
 
 	if (lengthA > 10.0f * b2_linearSlop)
 	{
@@ -225,23 +229,23 @@ bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
 	}
 
 	// Compute effective mass.
-	float32 ruA = b2Cross(rA, uA);
-	float32 ruB = b2Cross(rB, uB);
+	float ruA = b2Cross(rA, uA);
+	float ruB = b2Cross(rB, uB);
 
-	float32 mA = m_invMassA + m_invIA * ruA * ruA;
-	float32 mB = m_invMassB + m_invIB * ruB * ruB;
+	float mA = m_invMassA + m_invIA * ruA * ruA;
+	float mB = m_invMassB + m_invIB * ruB * ruB;
 
-	float32 mass = mA + m_ratio * m_ratio * mB;
+	float mass = mA + m_ratio * m_ratio * mB;
 
 	if (mass > 0.0f)
 	{
 		mass = 1.0f / mass;
 	}
 
-	float32 C = m_constant - lengthA - m_ratio * lengthB;
-	float32 linearError = b2Abs(C);
+	float C = m_constant - lengthA - m_ratio * lengthB;
+	float linearError = b2Abs(C);
 
-	float32 impulse = -mass * C;
+	float impulse = -mass * C;
 
 	b2Vec2 PA = -impulse * uA;
 	b2Vec2 PB = -m_ratio * impulse * uB;
@@ -269,13 +273,13 @@ b2Vec2 b2PulleyJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2PulleyJoint::GetReactionForce(float inv_dt) const
 {
 	b2Vec2 P = m_impulse * m_uB;
 	return inv_dt * P;
 }
 
-float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
+float b2PulleyJoint::GetReactionTorque(float inv_dt) const
 {
 	B2_NOT_USED(inv_dt);
 	return 0.0f;
@@ -291,22 +295,22 @@ b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
 	return m_groundAnchorB;
 }
 
-float32 b2PulleyJoint::GetLengthA() const
+float b2PulleyJoint::GetLengthA() const
 {
 	return m_lengthA;
 }
 
-float32 b2PulleyJoint::GetLengthB() const
+float b2PulleyJoint::GetLengthB() const
 {
 	return m_lengthB;
 }
 
-float32 b2PulleyJoint::GetRatio() const
+float b2PulleyJoint::GetRatio() const
 {
 	return m_ratio;
 }
 
-float32 b2PulleyJoint::GetCurrentLengthA() const
+float b2PulleyJoint::GetCurrentLengthA() const
 {
 	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
 	b2Vec2 s = m_groundAnchorA;
@@ -314,7 +318,7 @@ float32 b2PulleyJoint::GetCurrentLengthA() const
 	return d.Length();
 }
 
-float32 b2PulleyJoint::GetCurrentLengthB() const
+float b2PulleyJoint::GetCurrentLengthB() const
 {
 	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
 	b2Vec2 s = m_groundAnchorB;
@@ -327,18 +331,18 @@ void b2PulleyJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2PulleyJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);
-	b2Log("  jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.lengthA = %.15lef;\n", m_lengthA);
-	b2Log("  jd.lengthB = %.15lef;\n", m_lengthB);
-	b2Log("  jd.ratio = %.15lef;\n", m_ratio);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2PulleyJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.groundAnchorA.Set(%.9g, %.9g);\n", m_groundAnchorA.x, m_groundAnchorA.y);
+	b2Dump("  jd.groundAnchorB.Set(%.9g, %.9g);\n", m_groundAnchorB.x, m_groundAnchorB.y);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.lengthA = %.9g;\n", m_lengthA);
+	b2Dump("  jd.lengthB = %.9g;\n", m_lengthB);
+	b2Dump("  jd.ratio = %.9g;\n", m_ratio);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }
 
 void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)

src/libraries/Box2D/Dynamics/b2_revolute_joint.cpp

@@ -0,0 +1,501 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_draw.h"
+#include "box2d/b2_revolute_joint.h"
+#include "box2d/b2_time_step.h"
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+	m_axialMass = 0.0f;
+	m_motorImpulse = 0.0f;
+	m_lowerImpulse = 0.0f;
+	m_upperImpulse = 0.0f;
+
+	m_lowerAngle = def->lowerAngle;
+	m_upperAngle = def->upperAngle;
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+
+	m_angle = 0.0f;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+	m_indexA = m_bodyA->m_islandIndex;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	m_invMassA = m_bodyA->m_invMass;
+	m_invMassB = m_bodyB->m_invMass;
+	m_invIA = m_bodyA->m_invI;
+	m_invIB = m_bodyB->m_invI;
+
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+
+	float aB = data.positions[m_indexB].a;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	b2Rot qA(aA), qB(aB);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+
+	// J = [-I -r1_skew I r2_skew]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x]
+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB]
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	m_K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
+	m_K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
+	m_K.ex.y = m_K.ey.x;
+	m_K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
+
+	m_axialMass = iA + iB;
+	bool fixedRotation;
+	if (m_axialMass > 0.0f)
+	{
+		m_axialMass = 1.0f / m_axialMass;
+		fixedRotation = false;
+	}
+	else
+	{
+		fixedRotation = true;
+	}
+
+	m_angle = aB - aA - m_referenceAngle;
+	if (m_enableLimit == false || fixedRotation)
+	{
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	if (m_enableMotor == false || fixedRotation)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (data.step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_impulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
+		m_lowerImpulse *= data.step.dtRatio;
+		m_upperImpulse *= data.step.dtRatio;
+
+		float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse;
+		b2Vec2 P(m_impulse.x, m_impulse.y);
+
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + axialImpulse);
+
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + axialImpulse);
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	bool fixedRotation = (iA + iB == 0.0f);
+
+	// Solve motor constraint.
+	if (m_enableMotor && fixedRotation == false)
+	{
+		float Cdot = wB - wA - m_motorSpeed;
+		float impulse = -m_axialMass * Cdot;
+		float oldImpulse = m_motorImpulse;
+		float maxImpulse = data.step.dt * m_maxMotorTorque;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	if (m_enableLimit && fixedRotation == false)
+	{
+		// Lower limit
+		{
+			float C = m_angle - m_lowerAngle;
+			float Cdot = wB - wA;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_lowerImpulse;
+			m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
+			impulse = m_lowerImpulse - oldImpulse;
+
+			wA -= iA * impulse;
+			wB += iB * impulse;
+		}
+
+		// Upper limit
+		// Note: signs are flipped to keep C positive when the constraint is satisfied.
+		// This also keeps the impulse positive when the limit is active.
+		{
+			float C = m_upperAngle - m_angle;
+			float Cdot = wA - wB;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_upperImpulse;
+			m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
+			impulse = m_upperImpulse - oldImpulse;
+
+			wA += iA * impulse;
+			wB -= iB * impulse;
+		}
+	}
+
+	// Solve point-to-point constraint
+	{
+		b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+		b2Vec2 impulse = m_K.Solve(-Cdot);
+
+		m_impulse.x += impulse.x;
+		m_impulse.y += impulse.y;
+
+		vA -= mA * impulse;
+		wA -= iA * b2Cross(m_rA, impulse);
+
+		vB += mB * impulse;
+		wB += iB * b2Cross(m_rB, impulse);
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float aB = data.positions[m_indexB].a;
+
+	b2Rot qA(aA), qB(aB);
+
+	float angularError = 0.0f;
+	float positionError = 0.0f;
+
+	bool fixedRotation = (m_invIA + m_invIB == 0.0f);
+
+	// Solve angular limit constraint
+	if (m_enableLimit && fixedRotation == false)
+	{
+		float angle = aB - aA - m_referenceAngle;
+		float C = 0.0f;
+
+		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
+		{
+			// Prevent large angular corrections
+			C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+		}
+		else if (angle <= m_lowerAngle)
+		{
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(angle - m_lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+		}
+		else if (angle >= m_upperAngle)
+		{
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(angle - m_upperAngle - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+		}
+
+		float limitImpulse = -m_axialMass * C;
+		aA -= m_invIA * limitImpulse;
+		aB += m_invIB * limitImpulse;
+		angularError = b2Abs(C);
+	}
+
+	// Solve point-to-point constraint.
+	{
+		qA.Set(aA);
+		qB.Set(aB);
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+
+		b2Vec2 C = cB + rB - cA - rA;
+		positionError = C.Length();
+
+		float mA = m_invMassA, mB = m_invMassB;
+		float iA = m_invIA, iB = m_invIB;
+
+		b2Mat22 K;
+		K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
+		K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
+		K.ey.x = K.ex.y;
+		K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
+
+		b2Vec2 impulse = -K.Solve(C);
+
+		cA -= mA * impulse;
+		aA -= iA * b2Cross(rA, impulse);
+
+		cB += mB * impulse;
+		aB += iB * b2Cross(rB, impulse);
+	}
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce(float inv_dt) const
+{
+	b2Vec2 P(m_impulse.x, m_impulse.y);
+	return inv_dt * P;
+}
+
+float b2RevoluteJoint::GetReactionTorque(float inv_dt) const
+{
+	return inv_dt * (m_lowerImpulse + m_upperImpulse);
+}
+
+float b2RevoluteJoint::GetJointAngle() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+	return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
+}
+
+float b2RevoluteJoint::GetJointSpeed() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+	return bB->m_angularVelocity - bA->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+	if (flag != m_enableMotor)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableMotor = flag;
+	}
+}
+
+float b2RevoluteJoint::GetMotorTorque(float inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float speed)
+{
+	if (speed != m_motorSpeed)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_motorSpeed = speed;
+	}
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float torque)
+{
+	if (torque != m_maxMotorTorque)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_maxMotorTorque = torque;
+	}
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+	if (flag != m_enableLimit)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableLimit = flag;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+}
+
+float b2RevoluteJoint::GetLowerLimit() const
+{
+	return m_lowerAngle;
+}
+
+float b2RevoluteJoint::GetUpperLimit() const
+{
+	return m_upperAngle;
+}
+
+void b2RevoluteJoint::SetLimits(float lower, float upper)
+{
+	b2Assert(lower <= upper);
+	
+	if (lower != m_lowerAngle || upper != m_upperAngle)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+		m_lowerAngle = lower;
+		m_upperAngle = upper;
+	}
+}
+
+void b2RevoluteJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2RevoluteJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.enableLimit = bool(%d);\n", m_enableLimit);
+	b2Dump("  jd.lowerAngle = %.9g;\n", m_lowerAngle);
+	b2Dump("  jd.upperAngle = %.9g;\n", m_upperAngle);
+	b2Dump("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Dump("  jd.motorSpeed = %.9g;\n", m_motorSpeed);
+	b2Dump("  jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+
+///
+void b2RevoluteJoint::Draw(b2Draw* draw) const
+{
+	const b2Transform& xfA = m_bodyA->GetTransform();
+	const b2Transform& xfB = m_bodyB->GetTransform();
+	b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
+	b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
+
+	b2Color c1(0.7f, 0.7f, 0.7f);
+	b2Color c2(0.3f, 0.9f, 0.3f);
+	b2Color c3(0.9f, 0.3f, 0.3f);
+	b2Color c4(0.3f, 0.3f, 0.9f);
+	b2Color c5(0.4f, 0.4f, 0.4f);
+
+	draw->DrawPoint(pA, 5.0f, c4);
+	draw->DrawPoint(pB, 5.0f, c5);
+
+	float aA = m_bodyA->GetAngle();
+	float aB = m_bodyB->GetAngle();
+	float angle = aB - aA - m_referenceAngle;
+
+	const float L = 0.5f;
+
+	b2Vec2 r = L * b2Vec2(cosf(angle), sinf(angle));
+	draw->DrawSegment(pB, pB + r, c1);
+	draw->DrawCircle(pB, L, c1);
+
+	if (m_enableLimit)
+	{
+		b2Vec2 rlo = L * b2Vec2(cosf(m_lowerAngle), sinf(m_lowerAngle));
+		b2Vec2 rhi = L * b2Vec2(cosf(m_upperAngle), sinf(m_upperAngle));
+
+		draw->DrawSegment(pB, pB + rlo, c2);
+		draw->DrawSegment(pB, pB + rhi, c3);
+	}
+
+	b2Color color(0.5f, 0.8f, 0.8f);
+	draw->DrawSegment(xfA.p, pA, color);
+	draw->DrawSegment(pA, pB, color);
+	draw->DrawSegment(xfB.p, pB, color);
+}

src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp → src/libraries/Box2D/Dynamics/b2_rope_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_rope_joint.h"
+#include "box2d/b2_time_step.h"
 
 
 // Limit:
@@ -39,7 +43,6 @@ b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)
 
 	m_mass = 0.0f;
 	m_impulse = 0.0f;
-	m_state = e_inactiveLimit;
 	m_length = 0.0f;
 }
 
@@ -55,14 +58,14 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIB = m_bodyB->m_invI;
 
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -72,16 +75,6 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
 
 	m_length = m_u.Length();
 
-	float32 C = m_length - m_maxLength;
-	if (C > 0.0f)
-	{
-		m_state = e_atUpperLimit;
-	}
-	else
-	{
-		m_state = e_inactiveLimit;
-	}
-
 	if (m_length > b2_linearSlop)
 	{
 		m_u *= 1.0f / m_length;
@@ -95,9 +88,9 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
 	}
 
 	// Compute effective mass.
-	float32 crA = b2Cross(m_rA, m_u);
-	float32 crB = b2Cross(m_rB, m_u);
-	float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
+	float crA = b2Cross(m_rA, m_u);
+	float crB = b2Cross(m_rB, m_u);
+	float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
 
 	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
 
@@ -126,15 +119,15 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	// Cdot = dot(u, v + cross(w, r))
 	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
 	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
-	float32 C = m_length - m_maxLength;
-	float32 Cdot = b2Dot(m_u, vpB - vpA);
+	float C = m_length - m_maxLength;
+	float Cdot = b2Dot(m_u, vpB - vpA);
 
 	// Predictive constraint.
 	if (C < 0.0f)
@@ -142,8 +135,8 @@ void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data)
 		Cdot += data.step.inv_dt * C;
 	}
 
-	float32 impulse = -m_mass * Cdot;
-	float32 oldImpulse = m_impulse;
+	float impulse = -m_mass * Cdot;
+	float oldImpulse = m_impulse;
 	m_impulse = b2Min(0.0f, m_impulse + impulse);
 	impulse = m_impulse - oldImpulse;
 
@@ -162,9 +155,9 @@ void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data)
 bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
 {
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -172,12 +165,12 @@ bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
 	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 	b2Vec2 u = cB + rB - cA - rA;
 
-	float32 length = u.Normalize();
-	float32 C = length - m_maxLength;
+	m_length = u.Normalize();
+	float C = m_length - m_maxLength;
 
 	C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);
 
-	float32 impulse = -m_mass * C;
+	float impulse = -m_mass * C;
 	b2Vec2 P = impulse * u;
 
 	cA -= m_invMassA * P;
@@ -190,7 +183,7 @@ bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
 	data.positions[m_indexB].c = cB;
 	data.positions[m_indexB].a = aB;
 
-	return length - m_maxLength < b2_linearSlop;
+	return m_length - m_maxLength < b2_linearSlop;
 }
 
 b2Vec2 b2RopeJoint::GetAnchorA() const
@@ -203,26 +196,26 @@ b2Vec2 b2RopeJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2RopeJoint::GetReactionForce(float inv_dt) const
 {
 	b2Vec2 F = (inv_dt * m_impulse) * m_u;
 	return F;
 }
 
-float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const
+float b2RopeJoint::GetReactionTorque(float inv_dt) const
 {
 	B2_NOT_USED(inv_dt);
 	return 0.0f;
 }
 
-float32 b2RopeJoint::GetMaxLength() const
+float b2RopeJoint::GetMaxLength() const
 {
 	return m_maxLength;
 }
 
-b2LimitState b2RopeJoint::GetLimitState() const
+float b2RopeJoint::GetLength() const
 {
-	return m_state;
+	return m_length;
 }
 
 void b2RopeJoint::Dump()
@@ -230,12 +223,12 @@ void b2RopeJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2RopeJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.maxLength = %.15lef;\n", m_maxLength);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2RopeJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.maxLength = %.9g;\n", m_maxLength);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp → src/libraries/Box2D/Dynamics/b2_weld_joint.cpp

@@ -1,24 +1,28 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_time_step.h"
+#include "box2d/b2_weld_joint.h"
 
 // Point-to-point constraint
 // C = p2 - p1
@@ -49,8 +53,8 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
 	m_localAnchorA = def->localAnchorA;
 	m_localAnchorB = def->localAnchorB;
 	m_referenceAngle = def->referenceAngle;
-	m_frequencyHz = def->frequencyHz;
-	m_dampingRatio = def->dampingRatio;
+	m_stiffness = def->stiffness;
+	m_damping = def->damping;
 
 	m_impulse.SetZero();
 }
@@ -66,13 +70,13 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIA = m_bodyA->m_invI;
 	m_invIB = m_bodyB->m_invI;
 
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
 	b2Rot qA(aA), qB(aB);
 
@@ -88,8 +92,8 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
 	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
 	b2Mat33 K;
 	K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
@@ -102,26 +106,22 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 	K.ey.z = K.ez.y;
 	K.ez.z = iA + iB;
 
-	if (m_frequencyHz > 0.0f)
+	if (m_stiffness > 0.0f)
 	{
 		K.GetInverse22(&m_mass);
 
-		float32 invM = iA + iB;
-		float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
-
-		float32 C = aB - aA - m_referenceAngle;
+		float invM = iA + iB;
 
-		// Frequency
-		float32 omega = 2.0f * b2_pi * m_frequencyHz;
+		float C = aB - aA - m_referenceAngle;
 
 		// Damping coefficient
-		float32 d = 2.0f * m * m_dampingRatio * omega;
+		float d = m_damping;
 
 		// Spring stiffness
-		float32 k = m * omega * omega;
+		float k = m_stiffness;
 
 		// magic formulas
-		float32 h = data.step.dt;
+		float h = data.step.dt;
 		m_gamma = h * (d + h * k);
 		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
 		m_bias = C * h * k * m_gamma;
@@ -169,18 +169,18 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
 	b2Vec2 vA = data.velocities[m_indexA].v;
-	float32 wA = data.velocities[m_indexA].w;
+	float wA = data.velocities[m_indexA].w;
 	b2Vec2 vB = data.velocities[m_indexB].v;
-	float32 wB = data.velocities[m_indexB].w;
+	float wB = data.velocities[m_indexB].w;
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
-	if (m_frequencyHz > 0.0f)
+	if (m_stiffness > 0.0f)
 	{
-		float32 Cdot2 = wB - wA;
+		float Cdot2 = wB - wA;
 
-		float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
+		float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
 		m_impulse.z += impulse2;
 
 		wA -= iA * impulse2;
@@ -203,7 +203,7 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
 	else
 	{
 		b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
-		float32 Cdot2 = wB - wA;
+		float Cdot2 = wB - wA;
 		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
 
 		b2Vec3 impulse = -b2Mul(m_mass, Cdot);
@@ -227,19 +227,19 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
 bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
 {
 	b2Vec2 cA = data.positions[m_indexA].c;
-	float32 aA = data.positions[m_indexA].a;
+	float aA = data.positions[m_indexA].a;
 	b2Vec2 cB = data.positions[m_indexB].c;
-	float32 aB = data.positions[m_indexB].a;
+	float aB = data.positions[m_indexB].a;
 
 	b2Rot qA(aA), qB(aB);
 
-	float32 mA = m_invMassA, mB = m_invMassB;
-	float32 iA = m_invIA, iB = m_invIB;
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
 
 	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
 	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
-	float32 positionError, angularError;
+	float positionError, angularError;
 
 	b2Mat33 K;
 	K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
@@ -252,7 +252,7 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
 	K.ey.z = K.ez.y;
 	K.ez.z = iA + iB;
 
-	if (m_frequencyHz > 0.0f)
+	if (m_stiffness > 0.0f)
 	{
 		b2Vec2 C1 =  cB + rB - cA - rA;
 
@@ -270,7 +270,7 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
 	else
 	{
 		b2Vec2 C1 =  cB + rB - cA - rA;
-		float32 C2 = aB - aA - m_referenceAngle;
+		float C2 = aB - aA - m_referenceAngle;
 
 		positionError = C1.Length();
 		angularError = b2Abs(C2);
@@ -315,13 +315,13 @@ b2Vec2 b2WeldJoint::GetAnchorB() const
 	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
-b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2WeldJoint::GetReactionForce(float inv_dt) const
 {
 	b2Vec2 P(m_impulse.x, m_impulse.y);
 	return inv_dt * P;
 }
 
-float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
+float b2WeldJoint::GetReactionTorque(float inv_dt) const
 {
 	return inv_dt * m_impulse.z;
 }
@@ -331,14 +331,14 @@ void b2WeldJoint::Dump()
 	int32 indexA = m_bodyA->m_islandIndex;
 	int32 indexB = m_bodyB->m_islandIndex;
 
-	b2Log("  b2WeldJointDef jd;\n");
-	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
-	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
-	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
-	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
-	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
-	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
-	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
-	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
-	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+	b2Dump("  b2WeldJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }

src/libraries/Box2D/Dynamics/b2_wheel_joint.cpp

@@ -0,0 +1,672 @@
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_draw.h"
+#include "box2d/b2_wheel_joint.h"
+#include "box2d/b2_time_step.h"
+
+// Linear constraint (point-to-line)
+// d = pB - pA = xB + rB - xA - rA
+// C = dot(ay, d)
+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
+//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+// Spring linear constraint
+// C = dot(ax, d)
+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+// Motor rotational constraint
+// Cdot = wB - wA
+// J = [0 0 -1 0 0 1]
+
+void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	localAxisA = bodyA->GetLocalVector(axis);
+}
+
+b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_localXAxisA = def->localAxisA;
+	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
+
+	m_mass = 0.0f;
+	m_impulse = 0.0f;
+	m_motorMass = 0.0f;
+	m_motorImpulse = 0.0f;
+	m_springMass = 0.0f;
+	m_springImpulse = 0.0f;
+
+	m_axialMass = 0.0f;
+	m_lowerImpulse = 0.0f;
+	m_upperImpulse = 0.0f;
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+	m_enableLimit = def->enableLimit;
+
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableMotor = def->enableMotor;
+
+	m_bias = 0.0f;
+	m_gamma = 0.0f;
+
+	m_ax.SetZero();
+	m_ay.SetZero();
+
+	m_stiffness = def->stiffness;
+	m_damping = def->damping;
+}
+
+void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+	m_indexA = m_bodyA->m_islandIndex;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	m_invMassA = m_bodyA->m_invMass;
+	m_invMassB = m_bodyB->m_invMass;
+	m_invIA = m_bodyA->m_invI;
+	m_invIB = m_bodyB->m_invI;
+
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float aB = data.positions[m_indexB].a;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	b2Rot qA(aA), qB(aB);
+
+	// Compute the effective masses.
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = cB + rB - cA - rA;
+
+	// Point to line constraint
+	{
+		m_ay = b2Mul(qA, m_localYAxisA);
+		m_sAy = b2Cross(d + rA, m_ay);
+		m_sBy = b2Cross(rB, m_ay);
+
+		m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
+
+		if (m_mass > 0.0f)
+		{
+			m_mass = 1.0f / m_mass;
+		}
+	}
+
+	// Spring constraint
+	m_ax = b2Mul(qA, m_localXAxisA);
+	m_sAx = b2Cross(d + rA, m_ax);
+	m_sBx = b2Cross(rB, m_ax);
+
+	const float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
+	if (invMass > 0.0f)
+	{
+		m_axialMass = 1.0f / invMass;
+	}
+	else
+	{
+		m_axialMass = 0.0f;
+	}
+
+	m_springMass = 0.0f;
+	m_bias = 0.0f;
+	m_gamma = 0.0f;
+
+	if (m_stiffness > 0.0f && invMass > 0.0f)
+	{
+		m_springMass = 1.0f / invMass;
+
+		float C = b2Dot(d, m_ax);
+
+		// magic formulas
+		float h = data.step.dt;
+		m_gamma = h * (m_damping + h * m_stiffness);
+		if (m_gamma > 0.0f)
+		{
+			m_gamma = 1.0f / m_gamma;
+		}
+
+		m_bias = C * h * m_stiffness * m_gamma;
+
+		m_springMass = invMass + m_gamma;
+		if (m_springMass > 0.0f)
+		{
+			m_springMass = 1.0f / m_springMass;
+		}
+	}
+	else
+	{
+		m_springImpulse = 0.0f;
+	}
+
+	if (m_enableLimit)
+	{
+		m_translation = b2Dot(m_ax, d);
+	}
+	else
+	{
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	if (m_enableMotor)
+	{
+		m_motorMass = iA + iB;
+		if (m_motorMass > 0.0f)
+		{
+			m_motorMass = 1.0f / m_motorMass;
+		}
+	}
+	else
+	{
+		m_motorMass = 0.0f;
+		m_motorImpulse = 0.0f;
+	}
+
+	if (data.step.warmStarting)
+	{
+		// Account for variable time step.
+		m_impulse *= data.step.dtRatio;
+		m_springImpulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
+
+		float axialImpulse = m_springImpulse + m_lowerImpulse - m_upperImpulse;
+		b2Vec2 P = m_impulse * m_ay + axialImpulse * m_ax;
+		float LA = m_impulse * m_sAy + axialImpulse * m_sAx + m_motorImpulse;
+		float LB = m_impulse * m_sBy + axialImpulse * m_sBx + m_motorImpulse;
+
+		vA -= m_invMassA * P;
+		wA -= m_invIA * LA;
+
+		vB += m_invMassB * P;
+		wB += m_invIB * LB;
+	}
+	else
+	{
+		m_impulse = 0.0f;
+		m_springImpulse = 0.0f;
+		m_motorImpulse = 0.0f;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+	float mA = m_invMassA, mB = m_invMassB;
+	float iA = m_invIA, iB = m_invIB;
+
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float wA = data.velocities[m_indexA].w;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float wB = data.velocities[m_indexB].w;
+
+	// Solve spring constraint
+	{
+		float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
+		float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
+		m_springImpulse += impulse;
+
+		b2Vec2 P = impulse * m_ax;
+		float LA = impulse * m_sAx;
+		float LB = impulse * m_sBx;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	// Solve rotational motor constraint
+	{
+		float Cdot = wB - wA - m_motorSpeed;
+		float impulse = -m_motorMass * Cdot;
+
+		float oldImpulse = m_motorImpulse;
+		float maxImpulse = data.step.dt * m_maxMotorTorque;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	if (m_enableLimit)
+	{
+		// Lower limit
+		{
+			float C = m_translation - m_lowerTranslation;
+			float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_lowerImpulse;
+			m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f);
+			impulse = m_lowerImpulse - oldImpulse;
+
+			b2Vec2 P = impulse * m_ax;
+			float LA = impulse * m_sAx;
+			float LB = impulse * m_sBx;
+
+			vA -= mA * P;
+			wA -= iA * LA;
+			vB += mB * P;
+			wB += iB * LB;
+		}
+
+		// Upper limit
+		// Note: signs are flipped to keep C positive when the constraint is satisfied.
+		// This also keeps the impulse positive when the limit is active.
+		{
+			float C = m_upperTranslation - m_translation;
+			float Cdot = b2Dot(m_ax, vA - vB) + m_sAx * wA - m_sBx * wB;
+			float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt);
+			float oldImpulse = m_upperImpulse;
+			m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f);
+			impulse = m_upperImpulse - oldImpulse;
+
+			b2Vec2 P = impulse * m_ax;
+			float LA = impulse * m_sAx;
+			float LB = impulse * m_sBx;
+
+			vA += mA * P;
+			wA += iA * LA;
+			vB -= mB * P;
+			wB -= iB * LB;
+		}
+	}
+
+	// Solve point to line constraint
+	{
+		float Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
+		float impulse = -m_mass * Cdot;
+		m_impulse += impulse;
+
+		b2Vec2 P = impulse * m_ay;
+		float LA = impulse * m_sAy;
+		float LB = impulse * m_sBy;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float aB = data.positions[m_indexB].a;
+
+	float linearError = 0.0f;
+
+	if (m_enableLimit)
+	{
+		b2Rot qA(aA), qB(aB);
+
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+		b2Vec2 d = (cB - cA) + rB - rA;
+
+		b2Vec2 ax = b2Mul(qA, m_localXAxisA);
+		float sAx = b2Cross(d + rA, m_ax);
+		float sBx = b2Cross(rB, m_ax);
+
+		float C = 0.0f;
+		float translation = b2Dot(ax, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			C = translation;
+		}
+		else if (translation <= m_lowerTranslation)
+		{
+			C = b2Min(translation - m_lowerTranslation, 0.0f);
+		}
+		else if (translation >= m_upperTranslation)
+		{
+			C = b2Max(translation - m_upperTranslation, 0.0f);
+		}
+
+		if (C != 0.0f)
+		{
+
+			float invMass = m_invMassA + m_invMassB + m_invIA * sAx * sAx + m_invIB * sBx * sBx;
+			float impulse = 0.0f;
+			if (invMass != 0.0f)
+			{
+				impulse = -C / invMass;
+			}
+
+			b2Vec2 P = impulse * ax;
+			float LA = impulse * sAx;
+			float LB = impulse * sBx;
+
+			cA -= m_invMassA * P;
+			aA -= m_invIA * LA;
+			cB += m_invMassB * P;
+			aB += m_invIB * LB;
+
+			linearError = b2Abs(C);
+		}
+	}
+
+	// Solve perpendicular constraint
+	{
+		b2Rot qA(aA), qB(aB);
+
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+		b2Vec2 d = (cB - cA) + rB - rA;
+
+		b2Vec2 ay = b2Mul(qA, m_localYAxisA);
+
+		float sAy = b2Cross(d + rA, ay);
+		float sBy = b2Cross(rB, ay);
+
+		float C = b2Dot(d, ay);
+
+		float invMass = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
+
+		float impulse = 0.0f;
+		if (invMass != 0.0f)
+		{
+			impulse = - C / invMass;
+		}
+
+		b2Vec2 P = impulse * ay;
+		float LA = impulse * sAy;
+		float LB = impulse * sBy;
+
+		cA -= m_invMassA * P;
+		aA -= m_invIA * LA;
+		cB += m_invMassB * P;
+		aB += m_invIB * LB;
+
+		linearError = b2Max(linearError, b2Abs(C));
+	}
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
+	return linearError <= b2_linearSlop;
+}
+
+b2Vec2 b2WheelJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2WheelJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2WheelJoint::GetReactionForce(float inv_dt) const
+{
+	return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
+}
+
+float b2WheelJoint::GetReactionTorque(float inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+float b2WheelJoint::GetJointTranslation() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
+	b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
+	b2Vec2 d = pB - pA;
+	b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
+
+	float translation = b2Dot(d, axis);
+	return translation;
+}
+
+float b2WheelJoint::GetJointLinearSpeed() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
+	b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
+	b2Vec2 p1 = bA->m_sweep.c + rA;
+	b2Vec2 p2 = bB->m_sweep.c + rB;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float wA = bA->m_angularVelocity;
+	float wB = bB->m_angularVelocity;
+
+	float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
+	return speed;
+}
+
+float b2WheelJoint::GetJointAngle() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+	return bB->m_sweep.a - bA->m_sweep.a;
+}
+
+float b2WheelJoint::GetJointAngularSpeed() const
+{
+	float wA = m_bodyA->m_angularVelocity;
+	float wB = m_bodyB->m_angularVelocity;
+	return wB - wA;
+}
+
+bool b2WheelJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2WheelJoint::EnableLimit(bool flag)
+{
+	if (flag != m_enableLimit)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableLimit = flag;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+}
+
+float b2WheelJoint::GetLowerLimit() const
+{
+	return m_lowerTranslation;
+}
+
+float b2WheelJoint::GetUpperLimit() const
+{
+	return m_upperTranslation;
+}
+
+void b2WheelJoint::SetLimits(float lower, float upper)
+{
+	b2Assert(lower <= upper);
+	if (lower != m_lowerTranslation || upper != m_upperTranslation)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_lowerTranslation = lower;
+		m_upperTranslation = upper;
+		m_lowerImpulse = 0.0f;
+		m_upperImpulse = 0.0f;
+	}
+}
+
+bool b2WheelJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2WheelJoint::EnableMotor(bool flag)
+{
+	if (flag != m_enableMotor)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableMotor = flag;
+	}
+}
+
+void b2WheelJoint::SetMotorSpeed(float speed)
+{
+	if (speed != m_motorSpeed)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_motorSpeed = speed;
+	}
+}
+
+void b2WheelJoint::SetMaxMotorTorque(float torque)
+{
+	if (torque != m_maxMotorTorque)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_maxMotorTorque = torque;
+	}
+}
+
+float b2WheelJoint::GetMotorTorque(float inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+void b2WheelJoint::SetStiffness(float stiffness)
+{
+	m_stiffness = stiffness;
+}
+
+float b2WheelJoint::GetStiffness() const
+{
+	return m_stiffness;
+}
+
+void b2WheelJoint::SetDamping(float damping)
+{
+	m_damping = damping;
+}
+
+float b2WheelJoint::GetDamping() const
+{
+	return m_damping;
+}
+
+void b2WheelJoint::Dump()
+{
+	// FLT_DECIMAL_DIG == 9
+
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2WheelJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y);
+	b2Dump("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Dump("  jd.motorSpeed = %.9g;\n", m_motorSpeed);
+	b2Dump("  jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+
+///
+void b2WheelJoint::Draw(b2Draw* draw) const
+{
+	const b2Transform& xfA = m_bodyA->GetTransform();
+	const b2Transform& xfB = m_bodyB->GetTransform();
+	b2Vec2 pA = b2Mul(xfA, m_localAnchorA);
+	b2Vec2 pB = b2Mul(xfB, m_localAnchorB);
+
+	b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA);
+
+	b2Color c1(0.7f, 0.7f, 0.7f);
+	b2Color c2(0.3f, 0.9f, 0.3f);
+	b2Color c3(0.9f, 0.3f, 0.3f);
+	b2Color c4(0.3f, 0.3f, 0.9f);
+	b2Color c5(0.4f, 0.4f, 0.4f);
+
+	draw->DrawSegment(pA, pB, c5);
+
+	if (m_enableLimit)
+	{
+		b2Vec2 lower = pA + m_lowerTranslation * axis;
+		b2Vec2 upper = pA + m_upperTranslation * axis;
+		b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA);
+		draw->DrawSegment(lower, upper, c1);
+		draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2);
+		draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3);
+	}
+	else
+	{
+		draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1);
+	}
+
+	draw->DrawPoint(pA, 5.0f, c1);
+	draw->DrawPoint(pB, 5.0f, c4);
+}

src/libraries/Box2D/Dynamics/b2World.cpp → src/libraries/Box2D/Dynamics/b2_world.cpp

@@ -1,46 +1,52 @@
-/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2Island.h>
-#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/b2BroadPhase.h>
-#include <Box2D/Collision/Shapes/b2CircleShape.h>
-#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
-#include <Box2D/Collision/Shapes/b2PolygonShape.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Common/b2Draw.h>
-#include <Box2D/Common/b2Timer.h>
+// MIT License
+
+// Copyright (c) 2019 Erin Catto
+
+// 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.
+
+#include "b2_contact_solver.h"
+#include "b2_island.h"
+
+#include "box2d/b2_body.h"
+#include "box2d/b2_broad_phase.h"
+#include "box2d/b2_chain_shape.h"
+#include "box2d/b2_circle_shape.h"
+#include "box2d/b2_collision.h"
+#include "box2d/b2_contact.h"
+#include "box2d/b2_draw.h"
+#include "box2d/b2_edge_shape.h"
+#include "box2d/b2_fixture.h"
+#include "box2d/b2_polygon_shape.h"
+#include "box2d/b2_pulley_joint.h"
+#include "box2d/b2_time_of_impact.h"
+#include "box2d/b2_timer.h"
+#include "box2d/b2_world.h"
+
 #include <new>
 
 b2World::b2World(const b2Vec2& gravity)
 {
-	m_destructionListener = NULL;
-	g_debugDraw = NULL;
+	m_destructionListener = nullptr;
+	m_debugDraw = nullptr;
 
-	m_bodyList = NULL;
-	m_jointList = NULL;
+	m_bodyList = nullptr;
+	m_jointList = nullptr;
 
 	m_bodyCount = 0;
 	m_jointCount = 0;
@@ -54,7 +60,9 @@ b2World::b2World(const b2Vec2& gravity)
 	m_allowSleep = true;
 	m_gravity = gravity;
 
-	m_flags = e_clearForces;
+	m_newContacts = false;
+	m_locked = false;
+	m_clearForces = true;
 
 	m_inv_dt0 = 0.0f;
 
@@ -101,7 +109,7 @@ void b2World::SetContactListener(b2ContactListener* listener)
 
 void b2World::SetDebugDraw(b2Draw* debugDraw)
 {
-	g_debugDraw = debugDraw;
+	m_debugDraw = debugDraw;
 }
 
 b2Body* b2World::CreateBody(const b2BodyDef* def)
@@ -109,14 +117,14 @@ b2Body* b2World::CreateBody(const b2BodyDef* def)
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
-		return NULL;
+		return nullptr;
 	}
 
 	void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
 	b2Body* b = new (mem) b2Body(def, this);
 
 	// Add to world doubly linked list.
-	b->m_prev = NULL;
+	b->m_prev = nullptr;
 	b->m_next = m_bodyList;
 	if (m_bodyList)
 	{
@@ -153,7 +161,7 @@ void b2World::DestroyBody(b2Body* b)
 
 		b->m_jointList = je;
 	}
-	b->m_jointList = NULL;
+	b->m_jointList = nullptr;
 
 	// Delete the attached contacts.
 	b2ContactEdge* ce = b->m_contactList;
@@ -163,7 +171,7 @@ void b2World::DestroyBody(b2Body* b)
 		ce = ce->next;
 		m_contactManager.Destroy(ce0->contact);
 	}
-	b->m_contactList = NULL;
+	b->m_contactList = nullptr;
 
 	// Delete the attached fixtures. This destroys broad-phase proxies.
 	b2Fixture* f = b->m_fixtureList;
@@ -185,7 +193,7 @@ void b2World::DestroyBody(b2Body* b)
 		b->m_fixtureList = f;
 		b->m_fixtureCount -= 1;
 	}
-	b->m_fixtureList = NULL;
+	b->m_fixtureList = nullptr;
 	b->m_fixtureCount = 0;
 
 	// Remove world body list.
@@ -214,13 +222,13 @@ b2Joint* b2World::CreateJoint(const b2JointDef* def)
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
-		return NULL;
+		return nullptr;
 	}
 
 	b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
 
 	// Connect to the world list.
-	j->m_prev = NULL;
+	j->m_prev = nullptr;
 	j->m_next = m_jointList;
 	if (m_jointList)
 	{
@@ -232,14 +240,14 @@ b2Joint* b2World::CreateJoint(const b2JointDef* def)
 	// Connect to the bodies' doubly linked lists.
 	j->m_edgeA.joint = j;
 	j->m_edgeA.other = j->m_bodyB;
-	j->m_edgeA.prev = NULL;
+	j->m_edgeA.prev = nullptr;
 	j->m_edgeA.next = j->m_bodyA->m_jointList;
 	if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
 	j->m_bodyA->m_jointList = &j->m_edgeA;
 
 	j->m_edgeB.joint = j;
 	j->m_edgeB.other = j->m_bodyA;
-	j->m_edgeB.prev = NULL;
+	j->m_edgeB.prev = nullptr;
 	j->m_edgeB.next = j->m_bodyB->m_jointList;
 	if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
 	j->m_bodyB->m_jointList = &j->m_edgeB;
@@ -319,8 +327,8 @@ void b2World::DestroyJoint(b2Joint* j)
 		bodyA->m_jointList = j->m_edgeA.next;
 	}
 
-	j->m_edgeA.prev = NULL;
-	j->m_edgeA.next = NULL;
+	j->m_edgeA.prev = nullptr;
+	j->m_edgeA.next = nullptr;
 
 	// Remove from body 2
 	if (j->m_edgeB.prev)
@@ -338,8 +346,8 @@ void b2World::DestroyJoint(b2Joint* j)
 		bodyB->m_jointList = j->m_edgeB.next;
 	}
 
-	j->m_edgeB.prev = NULL;
-	j->m_edgeB.next = NULL;
+	j->m_edgeB.prev = nullptr;
+	j->m_edgeB.next = nullptr;
 
 	b2Joint::Destroy(j, &m_blockAllocator);
 
@@ -420,7 +428,7 @@ void b2World::Solve(const b2TimeStep& step)
 			continue;
 		}
 
-		if (seed->IsAwake() == false || seed->IsActive() == false)
+		if (seed->IsAwake() == false || seed->IsEnabled() == false)
 		{
 			continue;
 		}
@@ -442,12 +450,9 @@ void b2World::Solve(const b2TimeStep& step)
 		{
 			// Grab the next body off the stack and add it to the island.
 			b2Body* b = stack[--stackCount];
-			b2Assert(b->IsActive() == true);
+			b2Assert(b->IsEnabled() == true);
 			island.Add(b);
 
-			// Make sure the body is awake.
-			b->SetAwake(true);
-
 			// To keep islands as small as possible, we don't
 			// propagate islands across static bodies.
 			if (b->GetType() == b2_staticBody)
@@ -455,6 +460,9 @@ void b2World::Solve(const b2TimeStep& step)
 				continue;
 			}
 
+			// Make sure the body is awake (without resetting sleep timer).
+			b->m_flags |= b2Body::e_awakeFlag;
+
 			// Search all contacts connected to this body.
 			for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
 			{
@@ -507,8 +515,8 @@ void b2World::Solve(const b2TimeStep& step)
 
 				b2Body* other = je->other;
 
-				// Don't simulate joints connected to inactive bodies.
-				if (other->IsActive() == false)
+				// Don't simulate joints connected to diabled bodies.
+				if (other->IsEnabled() == false)
 				{
 					continue;
 				}
@@ -599,8 +607,8 @@ void b2World::SolveTOI(const b2TimeStep& step)
 	for (;;)
 	{
 		// Find the first TOI.
-		b2Contact* minContact = NULL;
-		float32 minAlpha = 1.0f;
+		b2Contact* minContact = nullptr;
+		float minAlpha = 1.0f;
 
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
 		{
@@ -616,7 +624,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
 				continue;
 			}
 
-			float32 alpha = 1.0f;
+			float alpha = 1.0f;
 			if (c->m_flags & b2Contact::e_toiFlag)
 			{
 				// This contact has a valid cached TOI.
@@ -660,7 +668,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
 
 				// Compute the TOI for this contact.
 				// Put the sweeps onto the same time interval.
-				float32 alpha0 = bA->m_sweep.alpha0;
+				float alpha0 = bA->m_sweep.alpha0;
 
 				if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
 				{
@@ -690,7 +698,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
 				b2TimeOfImpact(&output, &input);
 
 				// Beta is the fraction of the remaining portion of the .
-				float32 beta = output.t;
+				float beta = output.t;
 				if (output.state == b2TOIOutput::e_touching)
 				{
 					alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
@@ -712,7 +720,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
 			}
 		}
 
-		if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
+		if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha)
 		{
 			// No more TOI events. Done!
 			m_stepComplete = true;
@@ -894,18 +902,18 @@ void b2World::SolveTOI(const b2TimeStep& step)
 	}
 }
 
-void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
+void b2World::Step(float dt, int32 velocityIterations, int32 positionIterations)
 {
 	b2Timer stepTimer;
 
 	// If new fixtures were added, we need to find the new contacts.
-	if (m_flags & e_newFixture)
+	if (m_newContacts)
 	{
 		m_contactManager.FindNewContacts();
-		m_flags &= ~e_newFixture;
+		m_newContacts = false;
 	}
 
-	m_flags |= e_locked;
+	m_locked = true;
 
 	b2TimeStep step;
 	step.dt = dt;
@@ -952,12 +960,12 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration
 		m_inv_dt0 = step.inv_dt;
 	}
 
-	if (m_flags & e_clearForces)
+	if (m_clearForces)
 	{
 		ClearForces();
 	}
 
-	m_flags &= ~e_locked;
+	m_locked = false;
 
 	m_profile.step = stepTimer.GetMilliseconds();
 }
@@ -993,7 +1001,7 @@ void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
 
 struct b2WorldRayCastWrapper
 {
-	float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
+	float RayCastCallback(const b2RayCastInput& input, int32 proxyId)
 	{
 		void* userData = broadPhase->GetUserData(proxyId);
 		b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
@@ -1004,7 +1012,7 @@ struct b2WorldRayCastWrapper
 
 		if (hit)
 		{
-			float32 fraction = output.fraction;
+			float fraction = output.fraction;
 			b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
 			return callback->ReportFixture(fixture, point, output.normal, fraction);
 		}
@@ -1037,10 +1045,10 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 			b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
 
 			b2Vec2 center = b2Mul(xf, circle->m_p);
-			float32 radius = circle->m_radius;
+			float radius = circle->m_radius;
 			b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
 
-			g_debugDraw->DrawSolidCircle(center, radius, axis, color);
+			m_debugDraw->DrawSolidCircle(center, radius, axis, color);
 		}
 		break;
 
@@ -1049,7 +1057,13 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 			b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
 			b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
 			b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
-			g_debugDraw->DrawSegment(v1, v2, color);
+			m_debugDraw->DrawSegment(v1, v2, color);
+
+			if (edge->m_oneSided == false)
+			{
+				m_debugDraw->DrawPoint(v1, 4.0f, color);
+				m_debugDraw->DrawPoint(v2, 4.0f, color);
+			}
 		}
 		break;
 
@@ -1063,8 +1077,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 			for (int32 i = 1; i < count; ++i)
 			{
 				b2Vec2 v2 = b2Mul(xf, vertices[i]);
-				g_debugDraw->DrawSegment(v1, v2, color);
-				g_debugDraw->DrawCircle(v1, 0.05f, color);
+				m_debugDraw->DrawSegment(v1, v2, color);
 				v1 = v2;
 			}
 		}
@@ -1082,64 +1095,23 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 				vertices[i] = b2Mul(xf, poly->m_vertices[i]);
 			}
 
-			g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
-		}
-		break;
-            
-    default:
-        break;
-	}
-}
-
-void b2World::DrawJoint(b2Joint* joint)
-{
-	b2Body* bodyA = joint->GetBodyA();
-	b2Body* bodyB = joint->GetBodyB();
-	const b2Transform& xf1 = bodyA->GetTransform();
-	const b2Transform& xf2 = bodyB->GetTransform();
-	b2Vec2 x1 = xf1.p;
-	b2Vec2 x2 = xf2.p;
-	b2Vec2 p1 = joint->GetAnchorA();
-	b2Vec2 p2 = joint->GetAnchorB();
-
-	b2Color color(0.5f, 0.8f, 0.8f);
-
-	switch (joint->GetType())
-	{
-	case e_distanceJoint:
-		g_debugDraw->DrawSegment(p1, p2, color);
-		break;
-
-	case e_pulleyJoint:
-		{
-			b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
-			b2Vec2 s1 = pulley->GetGroundAnchorA();
-			b2Vec2 s2 = pulley->GetGroundAnchorB();
-			g_debugDraw->DrawSegment(s1, p1, color);
-			g_debugDraw->DrawSegment(s2, p2, color);
-			g_debugDraw->DrawSegment(s1, s2, color);
+			m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
 		}
 		break;
 
-	case e_mouseJoint:
-		// don't draw this
-		break;
-
 	default:
-		g_debugDraw->DrawSegment(x1, p1, color);
-		g_debugDraw->DrawSegment(p1, p2, color);
-		g_debugDraw->DrawSegment(x2, p2, color);
+	break;
 	}
 }
 
-void b2World::DrawDebugData()
+void b2World::DebugDraw()
 {
-	if (g_debugDraw == NULL)
+	if (m_debugDraw == nullptr)
 	{
 		return;
 	}
 
-	uint32 flags = g_debugDraw->GetFlags();
+	uint32 flags = m_debugDraw->GetFlags();
 
 	if (flags & b2Draw::e_shapeBit)
 	{
@@ -1148,7 +1120,12 @@ void b2World::DrawDebugData()
 			const b2Transform& xf = b->GetTransform();
 			for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
 			{
-				if (b->IsActive() == false)
+				if (b->GetType() == b2_dynamicBody && b->m_mass == 0.0f)
+				{
+					// Bad body
+					DrawShape(f, xf, b2Color(1.0f, 0.0f, 0.0f));
+				}
+				else if (b->IsEnabled() == false)
 				{
 					DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
 				}
@@ -1176,7 +1153,7 @@ void b2World::DrawDebugData()
 	{
 		for (b2Joint* j = m_jointList; j; j = j->GetNext())
 		{
-			DrawJoint(j);
+			j->Draw(m_debugDraw);
 		}
 	}
 
@@ -1185,13 +1162,14 @@ void b2World::DrawDebugData()
 		b2Color color(0.3f, 0.9f, 0.9f);
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
 		{
-			//b2Fixture* fixtureA = c->GetFixtureA();
-			//b2Fixture* fixtureB = c->GetFixtureB();
-
-			//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
-			//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
-
-			//g_debugDraw->DrawSegment(cA, cB, color);
+			b2Fixture* fixtureA = c->GetFixtureA();
+			b2Fixture* fixtureB = c->GetFixtureB();
+			int32 indexA = c->GetChildIndexA();
+			int32 indexB = c->GetChildIndexB();
+			b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter();
+			b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter();
+
+			m_debugDraw->DrawSegment(cA, cB, color);
 		}
 	}
 
@@ -1202,7 +1180,7 @@ void b2World::DrawDebugData()
 
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
-			if (b->IsActive() == false)
+			if (b->IsEnabled() == false)
 			{
 				continue;
 			}
@@ -1219,7 +1197,7 @@ void b2World::DrawDebugData()
 					vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
 					vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
 
-					g_debugDraw->DrawPolygon(vs, 4, color);
+					m_debugDraw->DrawPolygon(vs, 4, color);
 				}
 			}
 		}
@@ -1231,7 +1209,7 @@ void b2World::DrawDebugData()
 		{
 			b2Transform xf = b->GetTransform();
 			xf.p = b->GetWorldCenter();
-			g_debugDraw->DrawTransform(xf);
+			m_debugDraw->DrawTransform(xf);
 		}
 	}
 }
@@ -1251,15 +1229,15 @@ int32 b2World::GetTreeBalance() const
 	return m_contactManager.m_broadPhase.GetTreeBalance();
 }
 
-float32 b2World::GetTreeQuality() const
+float b2World::GetTreeQuality() const
 {
 	return m_contactManager.m_broadPhase.GetTreeQuality();
 }
 
 void b2World::ShiftOrigin(const b2Vec2& newOrigin)
 {
-	b2Assert((m_flags & e_locked) == 0);
-	if ((m_flags & e_locked) == e_locked)
+	b2Assert(m_locked == false);
+	if (m_locked)
 	{
 		return;
 	}
@@ -1281,16 +1259,19 @@ void b2World::ShiftOrigin(const b2Vec2& newOrigin)
 
 void b2World::Dump()
 {
-	if ((m_flags & e_locked) == e_locked)
+	if (m_locked)
 	{
 		return;
 	}
 
-	b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);
-	b2Log("m_world->SetGravity(g);\n");
+	b2OpenDump("box2d_dump.inl");
+
+	b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y);
+	b2Dump("m_world->SetGravity(g);\n");
+
+	b2Dump("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
+	b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
 
-	b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
-	b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
 	int32 i = 0;
 	for (b2Body* b = m_bodyList; b; b = b->m_next)
 	{
@@ -1314,9 +1295,9 @@ void b2World::Dump()
 			continue;
 		}
 
-		b2Log("{\n");
+		b2Dump("{\n");
 		j->Dump();
-		b2Log("}\n");
+		b2Dump("}\n");
 	}
 
 	// Second pass on joints, only gear joints.
@@ -1327,13 +1308,15 @@ void b2World::Dump()
 			continue;
 		}
 
-		b2Log("{\n");
+		b2Dump("{\n");
 		j->Dump();
-		b2Log("}\n");
+		b2Dump("}\n");
 	}
 
-	b2Log("b2Free(joints);\n");
-	b2Log("b2Free(bodies);\n");
-	b2Log("joints = NULL;\n");
-	b2Log("bodies = NULL;\n");
+	b2Dump("b2Free(joints);\n");
+	b2Dump("b2Free(bodies);\n");
+	b2Dump("joints = nullptr;\n");
+	b2Dump("bodies = nullptr;\n");
+
+	b2CloseDump();
 }