Started working on the Physics Engine, a Chipmunk OOP wrapper.

Makefile

@@ -12,7 +12,7 @@ ifeq ($(DEBUGBUILD), yes)
     DEBUGFLAGS = -g -DDEBUG -DEE_DEBUG -DEE_MEMORY_MANAGER
     RELEASETYPE = debug
 else
-    DEBUGFLAGS = -fno-strict-aliasing -O3 -s -DNDEBUG
+    DEBUGFLAGS = -fno-strict-aliasing -O3 -s -DNDEBUG -ffast-math
     RELEASETYPE = release
 endif
 
@@ -32,7 +32,7 @@ export CC         	= gcc
 export CPP        	= g++
 endif
 
-export CFLAGS     	= -Wall $(DEBUGFLAGS) $(BUILDFLAGS)
+export CFLAGS     	= -Wall -Wno-unknown-pragmas $(DEBUGFLAGS) $(BUILDFLAGS)
 export CFLAGSEXT  	= $(DEBUGFLAGS) $(BUILDFLAGS)
 export LDFLAGS    	= $(LINKFLAGS)
 export LIBPATH    	= ./
@@ -61,6 +61,8 @@ OTHERINC	= -I/usr/include/freetype2
 endif
 endif
 
+HELPERSINC	= -I./src/helper/chipmunk
+
 EXE     			= eetest-$(RELEASETYPE)
 EXEIV				= eeiv-$(RELEASETYPE)
 EXEFLUID			= eefluid-$(RELEASETYPE)
@@ -70,6 +72,7 @@ SRCSOIL 			= $(wildcard ./src/helper/SOIL/*.c)
 SRCSTBVORBIS 		= $(wildcard ./src/helper/stb_vorbis/*.c)
 SRCZLIB				= $(wildcard ./src/helper/zlib/*.c)
 SRCLIBZIP			= $(wildcard ./src/helper/libzip/*.c)
+SRCCHIPMUNK			= $(wildcard ./src/helper/chipmunk/*.c) $(wildcard ./src/helper/chipmunk/constraints/*.c)
 
 SRCHAIKUTTF 		= $(wildcard ./src/helper/haikuttf/*.cpp)
 SRCBASE				= $(wildcard ./src/base/*.cpp)
@@ -81,13 +84,14 @@ SRCSYSTEM			= $(wildcard ./src/system/*.cpp)
 SRCUI				= $(wildcard ./src/ui/*.cpp)
 SRCUTILS     		= $(wildcard ./src/utils/*.cpp)
 SRCWINDOW     		= $(wildcard ./src/window/*.cpp)
+SRCPHYSICS			= $(wildcard ./src/physics/*.cpp) $(wildcard ./src/physics/constraints/*.cpp)
 
 SRCTEST     		= $(wildcard ./src/test/*.cpp)
 SRCEEIV     		= $(wildcard ./src/eeiv/*.cpp)
 SRCFLUID     		= $(wildcard ./src/fluid/*.cpp)
 
-SRCHELPERS			= $(SRCGLEW) $(SRCSOIL) $(SRCSTBVORBIS) $(SRCZLIB) $(SRCLIBZIP)
-SRCMODULES			= $(SRCHAIKUTTF) $(SRCBASE) $(SRCAUDIO) $(SRCGAMING) $(SRCGRAPHICS) $(SRCMATH) $(SRCSYSTEM) $(SRCUI) $(SRCUTILS) $(SRCWINDOW)
+SRCHELPERS			= $(SRCGLEW) $(SRCSOIL) $(SRCSTBVORBIS) $(SRCZLIB) $(SRCLIBZIP) $(SRCCHIPMUNK)
+SRCMODULES			= $(SRCHAIKUTTF) $(SRCBASE) $(SRCAUDIO) $(SRCGAMING) $(SRCGRAPHICS) $(SRCMATH) $(SRCSYSTEM) $(SRCUI) $(SRCUTILS) $(SRCWINDOW) $(SRCPHYSICS)
 SRCALL				= $(SRCMODULES) $(SRCHELPERS) $(SRCTEST) $(SRCEEIV)
 SRCHPPALL			= $(SRCALL:.cpp=.hpp)
 SRCHALL				= $(SRCALL:.c=.h)
@@ -97,6 +101,7 @@ OBJSOIL 			= $(SRCSOIL:.c=.o)
 OBJSTBVORBIS 		= $(SRCSTBVORBIS:.c=.o) 
 OBJZLIB 			= $(SRCZLIB:.c=.o) 
 OBJLIBZIP 			= $(SRCLIBZIP:.c=.o) 
+OBJCHIPMUNK			= $(SRCCHIPMUNK:.c=.o)
 
 OBJHAIKUTTF 		= $(SRCHAIKUTTF:.cpp=.o)
 OBJBASE 			= $(SRCBASE:.cpp=.o)
@@ -108,9 +113,10 @@ OBJSYSTEM 			= $(SRCSYSTEM:.cpp=.o)
 OBJUI 				= $(SRCUI:.cpp=.o)
 OBJUTILS			= $(SRCUTILS:.cpp=.o)
 OBJWINDOW			= $(SRCWINDOW:.cpp=.o)
+OBJPHYSICS			= $(SRCPHYSICS:.cpp=.o)
 
-OBJHELPERS			= $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP)
-OBJMODULES			= $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI)
+OBJHELPERS			= $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) $(OBJCHIPMUNK)
+OBJMODULES			= $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) $(OBJPHYSICS)
 
 OBJTEST     		= $(SRCTEST:.cpp=.o)
 OBJEEIV     		= $(SRCEEIV:.cpp=.o)
@@ -118,8 +124,8 @@ OBJFLUID     		= $(SRCFLUID:.cpp=.o)
 
 OBJDIR				= obj/$(OS)/$(RELEASETYPE)/
 
-FOBJHELPERS			= $(patsubst ./%, $(OBJDIR)%, $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) )
-FOBJMODULES			= $(patsubst ./%, $(OBJDIR)%, $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) )
+FOBJHELPERS			= $(patsubst ./%, $(OBJDIR)%, $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) $(OBJCHIPMUNK) )
+FOBJMODULES			= $(patsubst ./%, $(OBJDIR)%, $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) $(OBJPHYSICS) )
 
 FOBJTEST     		= $(patsubst ./%, $(OBJDIR)%, $(SRCTEST:.cpp=.o) )
 FOBJEEIV     		= $(patsubst ./%, $(OBJDIR)%, $(SRCEEIV:.cpp=.o) )
@@ -140,6 +146,8 @@ dirs:
 	@mkdir -p $(OBJDIR)/src/helper/stb_vorbis
 	@mkdir -p $(OBJDIR)/src/helper/zlib
 	@mkdir -p $(OBJDIR)/src/helper/libzip
+	@mkdir -p $(OBJDIR)/src/helper/chipmunk
+	@mkdir -p $(OBJDIR)/src/helper/chipmunk/constraints
 	@mkdir -p $(OBJDIR)/src/helper/haikuttf
 	@mkdir -p $(OBJDIR)/src/base
 	@mkdir -p $(OBJDIR)/src/audio
@@ -150,6 +158,8 @@ dirs:
 	@mkdir -p $(OBJDIR)/src/ui
 	@mkdir -p $(OBJDIR)/src/utils
 	@mkdir -p $(OBJDIR)/src/window
+	@mkdir -p $(OBJDIR)/src/physics
+	@mkdir -p $(OBJDIR)/src/physics/constraints
 	@mkdir -p $(OBJDIR)/src/test
 	@mkdir -p $(OBJDIR)/src/eeiv
 	@mkdir -p $(OBJDIR)/src/fluid
@@ -161,8 +171,8 @@ $(FOBJMODULES):
 	@$(CPP) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.cpp,$@) $(OTHERINC) > $(patsubst %.o,%.d,$@)
 
 $(FOBJHELPERS):
-	$(CC) -o $@ -c $(patsubst $(OBJDIR)%.o,%.c,$@) $(CFLAGSEXT) -DSTBI_FAILURE_USERMSG
-	@$(CC) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.c,$@) -DSTBI_FAILURE_USERMSG > $(patsubst %.o,%.d,$@)
+	$(CC) -o $@ -c $(patsubst $(OBJDIR)%.o,%.c,$@) $(CFLAGSEXT) -DSTBI_FAILURE_USERMSG -std=gnu99 $(HELPERSINC)
+	@$(CC) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.c,$@) -DSTBI_FAILURE_USERMSG > $(patsubst %.o,%.d,$@) $(HELPERSINC)
 
 $(FOBJTEST):
 	$(CPP) -o $@ -c $(patsubst $(OBJDIR)%.o,%.cpp,$@) $(CFLAGS) $(OTHERINC)

src/ee.h

@@ -173,4 +173,24 @@
 	#include "ui/cuigenericgrid.hpp"
 	#include "ui/cuiwindow.hpp"
 	using namespace EE::UI;
+
+	#include "physics/cphysicsmanager.hpp"
+	#include "physics/cshape.hpp"
+	#include "physics/cshapecircle.hpp"
+	#include "physics/cshapesegment.hpp"
+	#include "physics/cshapepoly.hpp"
+	#include "physics/cspace.hpp"
+	#include "physics/cbody.hpp"
+	#include "physics/constraints/cconstraint.hpp"
+	#include "physics/constraints/cdampedrotaryspring.hpp"
+	#include "physics/constraints/cdampedspring.hpp"
+	#include "physics/constraints/cgearjoint.hpp"
+	#include "physics/constraints/cgroovejoint.hpp"
+	#include "physics/constraints/cpinjoint.hpp"
+	#include "physics/constraints/cpivotjoint.hpp"
+	#include "physics/constraints/cratchetjoint.hpp"
+	#include "physics/constraints/crotarylimitjoint.hpp"
+	#include "physics/constraints/csimplemotor.hpp"
+	#include "physics/constraints/cslidejoint.hpp"
+	using namespace EE::Physics;
 #endif

src/helper/chipmunk/chipmunk.c

@@ -0,0 +1,150 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "chipmunk.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void cpInitCollisionFuncs(void);
+#ifdef __cplusplus
+}
+#endif
+
+void
+cpMessage(const char *message, const char *condition, const char *file, int line, int isError)
+{
+	fprintf(stderr, (isError ? "Aborting due to Chipmunk error: %s\n" : "Chipmunk warning: %s\n"), message);
+	fprintf(stderr, "\tFailed condition: %s\n", condition);
+	fprintf(stderr, "\tSource:%s:%d\n", file, line);
+	
+	if(isError) abort();
+}
+
+
+const char *cpVersionString = "5.3.4";
+
+void
+cpInitChipmunk(void)
+{
+#ifndef NDEBUG
+	printf("Initializing Chipmunk v%s (Debug Enabled)\n", cpVersionString);
+	printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n");
+#endif
+	
+	cpInitCollisionFuncs();
+}
+
+cpFloat
+cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
+{
+	return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset));
+}
+
+cpFloat
+cpAreaForCircle(cpFloat r1, cpFloat r2)
+{
+	return 2.0f*(cpFloat)M_PI*cpfabs(r1*r1 - r2*r2);
+}
+
+cpFloat
+cpMomentForSegment(cpFloat m, cpVect a, cpVect b)
+{
+	cpFloat length = cpvlength(cpvsub(b, a));
+	cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f);
+	
+	return m*(length*length/12.0f + cpvlengthsq(offset));
+}
+
+cpFloat
+cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
+{
+	return 2.0f*r*((cpFloat)M_PI*r + cpvdist(a, b));
+}
+
+cpFloat
+cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset)
+{
+	cpFloat sum1 = 0.0f;
+	cpFloat sum2 = 0.0f;
+	for(int i=0; i<numVerts; i++){
+		cpVect v1 = cpvadd(verts[i], offset);
+		cpVect v2 = cpvadd(verts[(i+1)%numVerts], offset);
+		
+		cpFloat a = cpvcross(v2, v1);
+		cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
+		
+		sum1 += a*b;
+		sum2 += a;
+	}
+	
+	return (m*sum1)/(6.0f*sum2);
+}
+
+cpFloat
+cpAreaForPoly(const int numVerts, const cpVect *verts)
+{
+	cpFloat area = 0.0f;
+	for(int i=0; i<numVerts; i++){
+		area += cpvcross(verts[i], verts[(i+1)%numVerts]);
+	}
+	
+	return area/2.0f;
+}
+
+cpVect
+cpCentroidForPoly(const int numVerts, const cpVect *verts)
+{
+	cpFloat sum = 0.0f;
+	cpVect vsum = cpvzero;
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect v1 = verts[i];
+		cpVect v2 = verts[(i+1)%numVerts];
+		cpFloat cross = cpvcross(v1, v2);
+		
+		sum += cross;
+		vsum = cpvadd(vsum, cpvmult(cpvadd(v1, v2), cross));
+	}
+	
+	return cpvmult(vsum, 1.0f/(3.0f*sum));
+}
+
+void
+cpRecenterPoly(const int numVerts, cpVect *verts){
+	cpVect centroid = cpCentroidForPoly(numVerts, verts);
+	
+	for(int i=0; i<numVerts; i++){
+		verts[i] = cpvsub(verts[i], centroid);
+	}
+}
+
+cpFloat
+cpMomentForBox(cpFloat m, cpFloat width, cpFloat height)
+{
+	return m*(width*width + height*height)/12.0f;
+}
+
+#include "chipmunk_ffi.h"

src/helper/chipmunk/chipmunk.h

@@ -0,0 +1,171 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 CHIPMUNK_HEADER
+#define CHIPMUNK_HEADER
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CP_ALLOW_PRIVATE_ACCESS
+	#define CP_ALLOW_PRIVATE_ACCESS 0
+#endif
+
+#if CP_ALLOW_PRIVATE_ACCESS == 1
+	#define CP_PRIVATE(symbol) symbol
+#else
+	#define CP_PRIVATE(symbol) symbol##_private
+#endif
+
+void cpMessage(const char *message, const char *condition, const char *file, int line, int isError);
+#ifdef NDEBUG
+	#define	cpAssertWarn(condition, message)
+#else
+	#define cpAssertWarn(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 0)
+#endif
+
+#ifdef NDEBUG
+	#define	cpAssert(condition, message)
+#else
+	#define cpAssert(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 1)
+#endif
+
+#include "chipmunk_types.h"
+	
+#ifndef INFINITY
+	//TODO use C++ infinity
+	#ifdef _MSC_VER
+		union MSVC_EVIL_FLOAT_HACK
+		{
+			unsigned __int8 Bytes[4];
+			float Value;
+		};
+		static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}};
+		#define INFINITY (INFINITY_HACK.Value)
+	#endif
+	
+	#ifdef __GNUC__
+		#define INFINITY (__builtin_inf())
+	#endif
+	
+	#ifndef INFINITY
+		#define INFINITY (1e1000)
+	#endif
+#endif
+
+// Maximum allocated size for various Chipmunk buffers
+#define CP_BUFFER_BYTES (32*1024)
+
+#define cpmalloc malloc
+#define cpcalloc calloc
+#define cprealloc realloc
+#define cpfree free
+
+typedef struct cpArray cpArray;
+typedef struct cpHashSet cpHashSet;
+
+typedef struct cpBody cpBody;
+typedef struct cpShape cpShape;
+typedef struct cpConstraint cpConstraint;
+
+typedef struct cpArbiter cpArbiter;
+
+typedef struct cpSpace cpSpace;
+
+#include "cpVect.h"
+#include "cpBB.h"
+#include "cpSpatialIndex.h"
+
+#include "cpBody.h"
+#include "cpShape.h"
+#include "cpPolyShape.h"
+
+#include "cpArbiter.h"	
+#include "constraints/cpConstraint.h"
+
+#include "cpSpace.h"
+
+#define CP_HASH_COEF (3344921057ul)
+#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF)
+
+extern const char *cpVersionString;
+void cpInitChipmunk(void);
+
+/**
+	Calculate the moment of inertia for a circle.
+	r1 and r2 are the inner and outer diameters. A solid circle has an inner diameter of 0.
+*/
+cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset);
+
+/**
+	Calculate area of a hollow circle.
+*/
+cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2);
+
+/**
+	Calculate the moment of inertia for a line segment.
+	Beveling radius is not supported.
+*/
+cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b);
+
+/**
+	Calculate the area of a fattened (capsule shaped) line segment.
+*/
+cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r);
+
+/**
+	Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex.
+*/
+cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset);
+
+/**
+	Calculate the signed area of a polygon.
+*/
+cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts);
+
+/**
+	Calculate the natural centroid of a polygon.
+*/
+cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts);
+
+/**
+	Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex)
+*/
+void cpRecenterPoly(const int numVerts, cpVect *verts);
+
+/**
+	Calculate the moment of inertia for a solid box.
+*/
+cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height);
+
+#ifdef __cplusplus
+}
+
+static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);}
+static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);}
+static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);}
+static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);}
+static inline cpVect operator -(const cpVect v){return cpvneg(v);}
+
+#endif
+
+#endif

src/helper/chipmunk/chipmunk_ffi.h

@@ -0,0 +1,59 @@
+// Create non static inlined copies of Chipmunk functions, useful for working with dynamic FFIs
+// This file should only be included in chipmunk.c
+
+#ifdef _MSC_VER
+ #if _MSC_VER >= 1600
+  #define MAKE_REF(name) decltype(name) *_##name = name
+ #else
+  #define MAKE_REF(name)
+ #endif
+#else
+ #define MAKE_REF(name) __typeof__(name) *_##name = name
+#endif
+
+MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv()
+MAKE_REF(cpveql);
+MAKE_REF(cpvadd);
+MAKE_REF(cpvneg);
+MAKE_REF(cpvsub);
+MAKE_REF(cpvmult);
+MAKE_REF(cpvdot);
+MAKE_REF(cpvcross);
+MAKE_REF(cpvperp);
+MAKE_REF(cpvrperp);
+MAKE_REF(cpvproject);
+MAKE_REF(cpvrotate);
+MAKE_REF(cpvunrotate);
+MAKE_REF(cpvlengthsq);
+MAKE_REF(cpvlerp);
+MAKE_REF(cpvnormalize);
+MAKE_REF(cpvnormalize_safe);
+MAKE_REF(cpvclamp);
+MAKE_REF(cpvlerpconst);
+MAKE_REF(cpvdist);
+MAKE_REF(cpvdistsq);
+MAKE_REF(cpvnear);
+
+MAKE_REF(cpBBNew);
+MAKE_REF(cpBBintersects);
+MAKE_REF(cpBBcontainsBB);
+MAKE_REF(cpBBcontainsVect);
+MAKE_REF(cpBBmerge);
+MAKE_REF(cpBBexpand);
+
+MAKE_REF(cpBodyWorld2Local);
+MAKE_REF(cpBodyLocal2World);
+MAKE_REF(cpBodyApplyImpulse);
+MAKE_REF(cpBodyIsSleeping);
+MAKE_REF(cpBodyIsRogue);
+MAKE_REF(cpBodyKineticEnergy);
+
+MAKE_REF(cpArbiterIsFirstContact);
+MAKE_REF(cpArbiterGetShapes);
+MAKE_REF(cpArbiterGetNormal);
+MAKE_REF(cpArbiterGetPoint);
+
+MAKE_REF(cpConstraintGetImpulse);
+
+MAKE_REF(cpSegmentQueryHitPoint);
+MAKE_REF(cpSegmentQueryHitDist);

src/helper/chipmunk/chipmunk_private.h

@@ -0,0 +1,150 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 CP_ALLOW_PRIVATE_ACCESS 1
+#include "chipmunk.h"
+
+#pragma mark cpArray
+
+struct cpArray {
+	int num, max;
+	void **arr;
+};
+
+// TODO get rid of reference versions?
+cpArray *cpArrayAlloc(void);
+cpArray *cpArrayInit(cpArray *arr, int size);
+cpArray *cpArrayNew(int size);
+
+void cpArrayDestroy(cpArray *arr);
+void cpArrayFree(cpArray *arr);
+
+void cpArrayPush(cpArray *arr, void *object);
+void *cpArrayPop(cpArray *arr);
+void cpArrayDeleteObj(cpArray *arr, void *obj);
+cpBool cpArrayContains(cpArray *arr, void *ptr);
+
+void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*));
+
+#pragma mark Foreach loops
+
+#define CP_BODY_FOREACH_CONSTRAINT(body, var)\
+	for(cpConstraint *var = body->constraintList; var; var = (var->a == body ? var->nextA : var->nextB))
+
+#define CP_BODY_FOREACH_ARBITER(body, var)\
+	for(cpArbiter *var = body->arbiterList; var; var = (var->a->body == body ? var->nextA : var->nextB))
+
+#define CP_BODY_FOREACH_SHAPE(body, var)\
+	for(cpShape *var = body->shapeList; var; var = var->next)
+
+#define CP_BODY_FOREACH_GROUP(root, var)\
+	for(cpBody *var = root; var; var = var->node.next)
+
+#pragma mark cpHashSet
+
+typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt);
+typedef void *(*cpHashSetTransFunc)(void *ptr, void *data);
+
+// TODO get rid of reference versions?
+cpHashSet *cpHashSetAlloc(void);
+cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *defaultValue);
+cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *defaultValue);
+
+void cpHashSetDestroy(cpHashSet *set);
+void cpHashSetFree(cpHashSet *set);
+
+int cpHashSetCount(cpHashSet *set);
+void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans);
+void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr);
+void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr);
+
+typedef void (*cpHashSetIterFunc)(void *elt, void *data);
+void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data);
+
+typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data);
+void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data);
+
+#pragma mark Arbiters
+
+cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash);
+cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
+
+void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
+void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
+void cpArbiterApplyCachedImpulse(cpArbiter *arb);
+void cpArbiterApplyImpulse(cpArbiter *arb);
+
+#pragma mark Collision Functions
+
+int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr);
+
+static inline cpFloat
+cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
+{
+	cpVect *verts = poly->CP_PRIVATE(tVerts);
+	cpFloat min = cpvdot(n, verts[0]);
+	
+	for(int i=1; i<poly->CP_PRIVATE(numVerts); i++){
+		min = cpfmin(min, cpvdot(n, verts[i]));
+	}
+	
+	return min - d;
+}
+
+static inline cpBool
+cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
+{
+	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
+	
+	for(int i=0; i<poly->CP_PRIVATE(numVerts); i++){
+		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
+		if(dist > 0.0f) return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+static inline cpBool
+cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
+{
+	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
+	
+	for(int i=0; i<poly->CP_PRIVATE(numVerts); i++){
+		if(cpvdot(axes[i].n, n) < 0.0f) continue;
+		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
+		if(dist > 0.0f) return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+#pragma mark Spatial Index Functions
+
+cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
+
+#pragma mark Space Functions
+
+cpContact *cpContactBufferGetArray(cpSpace *space);
+void cpSpacePushContacts(cpSpace *space, int count);
+void cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space);
+void cpSpaceActivateBody(cpSpace *space, cpBody *body);
+void cpSpaceLock(cpSpace *space);
+void cpSpaceUnlock(cpSpace *space);

src/helper/chipmunk/chipmunk_types.h

@@ -0,0 +1,151 @@
+#ifdef __APPLE__
+   #import "TargetConditionals.h"
+#endif
+
+#if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS)
+	#define CP_USE_CGPOINTS
+#endif
+
+#ifdef CP_USE_CGPOINTS
+	#if TARGET_OS_IPHONE
+		#import <CoreGraphics/CGGeometry.h>
+	#elif TARGET_OS_MAC
+		#import <ApplicationServices/ApplicationServices.h>
+	#endif
+	
+	#if defined(__LP64__) && __LP64__
+		#define CP_USE_DOUBLES 1
+	#else
+		#define CP_USE_DOUBLES 0
+	#endif
+#endif
+
+#ifndef CP_USE_DOUBLES
+	// use doubles by default for higher precision
+	#define CP_USE_DOUBLES 1
+#endif
+
+#if CP_USE_DOUBLES
+	typedef double cpFloat;
+	#define cpfsqrt sqrt
+	#define cpfsin sin
+	#define cpfcos cos
+	#define cpfacos acos
+	#define cpfatan2 atan2
+	#define cpfmod fmod
+	#define cpfexp exp
+	#define cpfpow pow
+	#define cpffloor floor
+	#define cpfceil ceil
+#else
+	typedef float cpFloat;
+	#define cpfsqrt sqrtf
+	#define cpfsin sinf
+	#define cpfcos cosf
+	#define cpfacos acosf
+	#define cpfatan2 atan2f
+	#define cpfmod fmodf
+	#define cpfexp expf
+	#define cpfpow powf
+	#define cpffloor floorf
+	#define cpfceil ceilf
+#endif
+
+static inline cpFloat
+cpfmax(cpFloat a, cpFloat b)
+{
+	return (a > b) ? a : b;
+}
+
+static inline cpFloat
+cpfmin(cpFloat a, cpFloat b)
+{
+	return (a < b) ? a : b;
+}
+
+static inline cpFloat
+cpfabs(cpFloat n)
+{
+	return (n < 0) ? -n : n;
+}
+
+static inline cpFloat
+cpfclamp(cpFloat f, cpFloat min, cpFloat max)
+{
+	return cpfmin(cpfmax(f, min), max);
+}
+
+static inline cpFloat
+cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
+{
+	return f1*(1.0f - t) + f2*t;
+}
+
+static inline cpFloat
+cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
+{
+	return f1 + cpfclamp(f2 - f1, -d, d);
+}
+
+// CGPoints are structurally the same, and allow
+// easy interoperability with other Cocoa libraries
+#ifdef CP_USE_CGPOINTS
+	typedef CGPoint cpVect;
+#else
+	typedef struct cpVect{cpFloat x,y;} cpVect;
+#endif
+
+typedef unsigned int cpHashValue;
+
+// Oh C, how we love to define our own boolean types to get compiler compatibility
+#ifdef CP_BOOL_TYPE
+	typedef CP_BOOL_TYPE cpBool;
+#else
+	typedef int cpBool;
+#endif
+
+#ifndef cpTrue
+	#define cpTrue 1
+#endif
+
+#ifndef cpFalse
+	#define cpFalse 0
+#endif
+
+#ifdef CP_DATA_POINTER_TYPE
+	typedef CP_DATA_POINTER_TYPE cpDataPointer;
+#else
+	typedef void * cpDataPointer;
+#endif
+
+#ifdef CP_COLLISION_TYPE_TYPE
+	typedef CP_COLLISION_TYPE_TYPE cpCollisionType;
+#else
+	typedef unsigned int cpCollisionType;
+#endif
+
+#ifdef CP_GROUP_TYPE
+	typedef CP_GROUP_TYPE cpGroup;
+#else
+	typedef unsigned int cpGroup;
+#endif
+
+#ifdef CP_LAYERS_TYPE
+	typedef CP_GROUP_TYPE cpLayers;
+#else
+	typedef unsigned int cpLayers;
+#endif
+
+#ifdef CP_TIMESTAMP_TYPE
+	typedef CP_TIMESTAMP_TYPE cpTimestamp;
+#else
+	typedef unsigned int cpTimestamp;
+#endif
+
+#ifndef CP_NO_GROUP
+	#define CP_NO_GROUP ((cpGroup)0)
+#endif
+
+#ifndef CP_ALL_LAYERS
+	#define CP_ALL_LAYERS (~(cpLayers)0)
+#endif

src/helper/chipmunk/chipmunk_unsafe.h

@@ -0,0 +1,52 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+/* This header defines a number of "unsafe" operations on Chipmunk objects.
+ * In this case "unsafe" is referring to operations which may reduce the
+ * physical accuracy or numerical stability of the simulation, but will not
+ * cause crashes.
+ *
+ * The prime example is mutating collision shapes. Chipmunk does not support
+ * this directly. Mutating shapes using this API will caused objects in contact
+ * to be pushed apart using Chipmunk's overlap solver, but not using real
+ * persistent velocities. Probably not what you meant, but perhaps close enough.
+ */
+
+#ifndef CHIPMUNK_UNSAFE_HEADER
+#define CHIPMUNK_UNSAFE_HEADER
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius);
+void cpCircleShapeSetOffset(cpShape *shape, cpVect offset);
+
+void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b);
+void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius);
+
+void cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/helper/chipmunk/constraints/cpConstraint.c

@@ -0,0 +1,58 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+// TODO: Comment me!
+
+cpFloat cp_constraint_bias_coef = 0.1f;
+
+void cpConstraintDestroy(cpConstraint *constraint){}
+
+void
+cpConstraintFree(cpConstraint *constraint)
+{
+	if(constraint){
+		cpConstraintDestroy(constraint);
+		cpfree(constraint);
+	}
+}
+
+// *** defined in util.h
+
+void
+cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b)
+{
+	constraint->klass = klass;
+	
+	constraint->a = a;
+	constraint->b = b;
+	
+	constraint->nextA = NULL;
+	constraint->nextB = NULL;
+	
+	constraint->maxForce = (cpFloat)INFINITY;
+	constraint->biasCoef = cp_constraint_bias_coef;
+	constraint->maxBias = (cpFloat)INFINITY;
+}

src/helper/chipmunk/constraints/cpConstraint.h

@@ -0,0 +1,102 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+// TODO: refactoring needed here
+	
+extern cpFloat cp_constraint_bias_coef;
+
+typedef struct cpConstraintClass cpConstraintClass;
+
+typedef void (*cpConstraintPreStepFunction)(cpConstraint *constraint, cpFloat dt, cpFloat dt_inv);
+typedef void (*cpConstraintApplyImpulseFunction)(cpConstraint *constraint);
+typedef cpFloat (*cpConstraintGetImpulseFunction)(cpConstraint *constraint);
+
+struct cpConstraintClass {
+	cpConstraintPreStepFunction preStep;
+	cpConstraintApplyImpulseFunction applyImpulse;
+	cpConstraintGetImpulseFunction getImpulse;
+};
+
+
+
+struct cpConstraint {
+	CP_PRIVATE(const cpConstraintClass *klass);
+	
+	cpBody *a, *b;
+	cpConstraint CP_PRIVATE(*nextA), CP_PRIVATE(*nextB);
+	
+	cpFloat maxForce;
+	cpFloat biasCoef;
+	cpFloat maxBias;
+	
+	cpDataPointer data;
+};
+
+void cpConstraintDestroy(cpConstraint *constraint);
+void cpConstraintFree(cpConstraint *constraint);
+
+static inline void
+cpConstraintActivateBodies(cpConstraint *constraint)
+{
+	cpBody *a = constraint->a; if(a) cpBodyActivate(a);
+	cpBody *b = constraint->b; if(b) cpBodyActivate(b);
+}
+
+static inline cpFloat
+cpConstraintGetImpulse(cpConstraint *constraint)
+{
+	return constraint->CP_PRIVATE(klass)->getImpulse(constraint);
+}
+
+#define cpConstraintCheckCast(constraint, struct) \
+	cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct);
+
+
+#define CP_DefineConstraintGetter(struct, type, member, name) \
+static inline type \
+struct##Get##name(const cpConstraint *constraint){ \
+	cpConstraintCheckCast(constraint, struct); \
+	return ((struct *)constraint)->member; \
+} \
+
+#define CP_DefineConstraintSetter(struct, type, member, name) \
+static inline void \
+struct##Set##name(cpConstraint *constraint, type value){ \
+	cpConstraintCheckCast(constraint, struct); \
+	cpConstraintActivateBodies(constraint); \
+	((struct *)constraint)->member = value; \
+} \
+
+#define CP_DefineConstraintProperty(struct, type, member, name) \
+CP_DefineConstraintGetter(struct, type, member, name) \
+CP_DefineConstraintSetter(struct, type, member, name)
+
+// Built in Joint types
+#include "cpPinJoint.h"
+#include "cpSlideJoint.h"
+#include "cpPivotJoint.h"
+#include "cpGrooveJoint.h"
+#include "cpDampedSpring.h"
+#include "cpDampedRotarySpring.h"
+#include "cpRotaryLimitJoint.h"
+#include "cpRatchetJoint.h"
+#include "cpGearJoint.h"
+#include "cpSimpleMotor.h"

src/helper/chipmunk/constraints/cpDampedRotarySpring.c

@@ -0,0 +1,105 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static cpFloat
+defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
+	return (relativeAngle - spring->restAngle)*spring->stiffness;
+}
+
+static void
+preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(spring, a, b);
+	
+	cpFloat moment = a->i_inv + b->i_inv;
+	spring->iSum = 1.0f/moment;
+
+	spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
+	spring->target_wrn = 0.0f;
+
+	// apply spring torque
+	cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
+	a->w -= j_spring*a->i_inv;
+	b->w += j_spring*b->i_inv;
+}
+
+static void
+applyImpulse(cpDampedRotarySpring *spring)
+{
+	CONSTRAINT_BEGIN(spring, a, b);
+	
+	// compute relative velocity
+	cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
+	
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	cpFloat w_damp = wrn*spring->w_coef;
+	spring->target_wrn = wrn - w_damp;
+	
+	//apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
+	cpFloat j_damp = w_damp*spring->iSum;
+	a->w -= j_damp*a->i_inv;
+	b->w += j_damp*b->i_inv;
+}
+
+static cpFloat
+getImpulse(cpConstraint *constraint)
+{
+	return 0.0f;
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpDampedRotarySpring)
+
+cpDampedRotarySpring *
+cpDampedRotarySpringAlloc(void)
+{
+	return (cpDampedRotarySpring *)cpmalloc(sizeof(cpDampedRotarySpring));
+}
+
+cpDampedRotarySpring *
+cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
+{
+	cpConstraintInit((cpConstraint *)spring, &klass, a, b);
+	
+	spring->restAngle = restAngle;
+	spring->stiffness = stiffness;
+	spring->damping = damping;
+	spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque;
+	
+	return spring;
+}
+
+cpConstraint *
+cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
+{
+	return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
+}

src/helper/chipmunk/constraints/cpDampedRotarySpring.h

@@ -0,0 +1,46 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle);
+
+const cpConstraintClass *cpDampedRotarySpringGetClass();
+
+typedef struct cpDampedRotarySpring {
+	cpConstraint constraint;
+	cpFloat restAngle;
+	cpFloat stiffness;
+	cpFloat damping;
+	cpDampedRotarySpringTorqueFunc springTorqueFunc;
+	
+	cpFloat target_wrn;
+	cpFloat w_coef;
+	
+	cpFloat iSum;
+} cpDampedRotarySpring;
+
+cpDampedRotarySpring *cpDampedRotarySpringAlloc(void);
+cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
+cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
+
+CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle);
+CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness);
+CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping);
+CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc);

src/helper/chipmunk/constraints/cpDampedSpring.c

@@ -0,0 +1,115 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static cpFloat
+defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
+	return (spring->restLength - dist)*spring->stiffness;
+}
+
+static void
+preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(spring, a, b);
+	
+	spring->r1 = cpvrotate(spring->anchr1, a->rot);
+	spring->r2 = cpvrotate(spring->anchr2, b->rot);
+	
+	cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
+	cpFloat dist = cpvlength(delta);
+	spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
+	
+	cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
+	spring->nMass = 1.0f/k;
+	
+	spring->target_vrn = 0.0f;
+	spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
+
+	// apply spring force
+	cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
+	apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
+}
+
+static void
+applyImpulse(cpDampedSpring *spring)
+{
+	CONSTRAINT_BEGIN(spring, a, b);
+	
+	cpVect n = spring->n;
+	cpVect r1 = spring->r1;
+	cpVect r2 = spring->r2;
+
+	// compute relative velocity
+	cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
+	
+	// compute velocity loss from drag
+	// not 100% certain this is derived correctly, though it makes sense
+	cpFloat v_damp = -vrn*spring->v_coef;
+	spring->target_vrn = vrn + v_damp;
+	
+	apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
+}
+
+static cpFloat
+getImpulse(cpConstraint *constraint)
+{
+	return 0.0f;
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpDampedSpring)
+
+cpDampedSpring *
+cpDampedSpringAlloc(void)
+{
+	return (cpDampedSpring *)cpmalloc(sizeof(cpDampedSpring));
+}
+
+cpDampedSpring *
+cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
+{
+	cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
+	
+	spring->anchr1 = anchr1;
+	spring->anchr2 = anchr2;
+	
+	spring->restLength = restLength;
+	spring->stiffness = stiffness;
+	spring->damping = damping;
+	spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce;
+	
+	return spring;
+}
+
+cpConstraint *
+cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
+{
+	return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping);
+}

src/helper/chipmunk/constraints/cpDampedSpring.h

@@ -0,0 +1,53 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+typedef struct cpDampedSpring cpDampedSpring;
+
+typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist);
+
+const cpConstraintClass *cpDampedSpringGetClass();
+
+struct cpDampedSpring {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat restLength;
+	cpFloat stiffness;
+	cpFloat damping;
+	cpDampedSpringForceFunc springForceFunc;
+	
+	cpFloat target_vrn;
+	cpFloat v_coef;
+	
+	cpVect r1, r2;
+	cpFloat nMass;
+	cpVect n;
+};
+
+cpDampedSpring *cpDampedSpringAlloc(void);
+cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
+cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
+
+CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1);
+CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2);
+CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength);
+CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness);
+CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping);
+CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc);

src/helper/chipmunk/constraints/cpGearJoint.c

@@ -0,0 +1,113 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// calculate moment of inertia coefficient.
+	joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint->constraint.maxBias;
+	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint->jMax = J_MAX(joint, dt);
+
+	// apply joint torque
+	cpFloat j = joint->jAcc;
+	a->w -= j*a->i_inv*joint->ratio_inv;
+	b->w += j*b->i_inv;
+}
+
+static void
+applyImpulse(cpGearJoint *joint)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// compute relative rotational velocity
+	cpFloat wr = b->w*joint->ratio - a->w;
+	
+	// compute normal impulse	
+	cpFloat j = (joint->bias - wr)*joint->iSum;
+	cpFloat jOld = joint->jAcc;
+	joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
+	j = joint->jAcc - jOld;
+	
+	// apply impulse
+	a->w -= j*a->i_inv*joint->ratio_inv;
+	b->w += j*b->i_inv;
+}
+
+static cpFloat
+getImpulse(cpGearJoint *joint)
+{
+	return cpfabs(joint->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpGearJoint)
+
+cpGearJoint *
+cpGearJointAlloc(void)
+{
+	return (cpGearJoint *)cpmalloc(sizeof(cpGearJoint));
+}
+
+cpGearJoint *
+cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->phase = phase;
+	joint->ratio = ratio;
+	joint->ratio_inv = 1.0f/ratio;
+	
+	joint->jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
+{
+	return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
+}
+
+void
+cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
+{
+	cpConstraintCheckCast(constraint, cpGearJoint);
+	((cpGearJoint *)constraint)->ratio = value;
+	((cpGearJoint *)constraint)->ratio_inv = 1.0f/value;
+	cpConstraintActivateBodies(constraint);
+}

src/helper/chipmunk/constraints/cpGearJoint.h

@@ -0,0 +1,41 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpGearJointGetClass();
+
+typedef struct cpGearJoint {
+	cpConstraint constraint;
+	cpFloat phase, ratio;
+	cpFloat ratio_inv;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+} cpGearJoint;
+
+cpGearJoint *cpGearJointAlloc(void);
+cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
+cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
+
+CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase);
+CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio);
+void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value);

src/helper/chipmunk/constraints/cpGrooveJoint.c

@@ -0,0 +1,161 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// calculate endpoints in worldspace
+	cpVect ta = cpBodyLocal2World(a, joint->grv_a);
+	cpVect tb = cpBodyLocal2World(a, joint->grv_b);
+
+	// calculate axis
+	cpVect n = cpvrotate(joint->grv_n, a->rot);
+	cpFloat d = cpvdot(ta, n);
+	
+	joint->grv_tn = n;
+	joint->r2 = cpvrotate(joint->anchr2, b->rot);
+	
+	// calculate tangential distance along the axis of r2
+	cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
+	// calculate clamping factor and r2
+	if(td <= cpvcross(ta, n)){
+		joint->clamp = 1.0f;
+		joint->r1 = cpvsub(ta, a->p);
+	} else if(td >= cpvcross(tb, n)){
+		joint->clamp = -1.0f;
+		joint->r1 = cpvsub(tb, a->p);
+	} else {
+		joint->clamp = 0.0f;
+		joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+	}
+	
+	// Calculate mass tensor
+	k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);	
+	
+	// compute max impulse
+	joint->jMaxLen = J_MAX(joint, dt);
+	
+	// calculate bias velocity
+	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+	
+	// apply accumulated impulse
+	apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
+}
+
+static inline cpVect
+grooveConstrain(cpGrooveJoint *joint, cpVect j){
+	cpVect n = joint->grv_tn;
+	cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
+	return cpvclamp(jClamp, joint->jMaxLen);
+}
+
+static void
+applyImpulse(cpGrooveJoint *joint)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	cpVect r1 = joint->r1;
+	cpVect r2 = joint->r2;
+	
+	// compute impulse
+	cpVect vr = relative_velocity(a, b, r1, r2);
+
+	cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
+	cpVect jOld = joint->jAcc;
+	joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j));
+	j = cpvsub(joint->jAcc, jOld);
+	
+	// apply impulse
+	apply_impulses(a, b, joint->r1, joint->r2, j);
+}
+
+static cpFloat
+getImpulse(cpGrooveJoint *joint)
+{
+	return cpvlength(joint->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpGrooveJoint)
+
+cpGrooveJoint *
+cpGrooveJointAlloc(void)
+{
+	return (cpGrooveJoint *)cpmalloc(sizeof(cpGrooveJoint));
+}
+
+cpGrooveJoint *
+cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->grv_a = groove_a;
+	joint->grv_b = groove_b;
+	joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
+	joint->anchr2 = anchr2;
+	
+	joint->jAcc = cpvzero;
+	
+	return joint;
+}
+
+cpConstraint *
+cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+	return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
+}
+
+void
+cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
+	cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g->grv_a = value;
+	g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+
+void
+cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
+{
+	cpGrooveJoint *g = (cpGrooveJoint *)constraint;
+	cpConstraintCheckCast(constraint, cpGrooveJoint);
+	
+	g->grv_b = value;
+	g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
+	
+	cpConstraintActivateBodies(constraint);
+}
+

src/helper/chipmunk/constraints/cpGrooveJoint.h

@@ -0,0 +1,48 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpGrooveJointGetClass();
+
+typedef struct cpGrooveJoint {
+	cpConstraint constraint;
+	cpVect grv_n, grv_a, grv_b;
+	cpVect  anchr2;
+	
+	cpVect grv_tn;
+	cpFloat clamp;
+	cpVect r1, r2;
+	cpVect k1, k2;
+	
+	cpVect jAcc;
+	cpFloat jMaxLen;
+	cpVect bias;
+} cpGrooveJoint;
+
+cpGrooveJoint *cpGrooveJointAlloc(void);
+cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
+cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
+
+
+CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
+void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
+CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
+void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
+CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);

src/helper/chipmunk/constraints/cpPinJoint.c

@@ -0,0 +1,116 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+//#include <math.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	joint->r1 = cpvrotate(joint->anchr1, a->rot);
+	joint->r2 = cpvrotate(joint->anchr2, b->rot);
+	
+	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	cpFloat dist = cpvlength(delta);
+	joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
+	
+	// calculate mass normal
+	joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint->constraint.maxBias;
+	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint->jnMax = J_MAX(joint, dt);
+	
+	// apply accumulated impulse
+	cpVect j = cpvmult(joint->n, joint->jnAcc);
+	apply_impulses(a, b, joint->r1, joint->r2, j);
+}
+
+static void
+applyImpulse(cpPinJoint *joint)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	cpVect n = joint->n;
+
+	// compute relative velocity
+	cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
+	
+	// compute normal impulse
+	cpFloat jn = (joint->bias - vrn)*joint->nMass;
+	cpFloat jnOld = joint->jnAcc;
+	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	jn = joint->jnAcc - jnOld;
+	
+	// apply impulse
+	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
+}
+
+static cpFloat
+getImpulse(cpPinJoint *joint)
+{
+	return cpfabs(joint->jnAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpPinJoint);
+
+
+cpPinJoint *
+cpPinJointAlloc(void)
+{
+	return (cpPinJoint *)cpmalloc(sizeof(cpPinJoint));
+}
+
+cpPinJoint *
+cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->anchr1 = anchr1;
+	joint->anchr2 = anchr2;
+	
+	// STATIC_BODY_CHECK
+	cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1);
+	cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2);
+	joint->dist = cpvlength(cpvsub(p2, p1));
+
+	joint->jnAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
+}

src/helper/chipmunk/constraints/cpPinJoint.h

@@ -0,0 +1,43 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+ 
+const cpConstraintClass *cpPinJointGetClass();
+
+typedef struct cpPinJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat dist;
+	
+	cpVect r1, r2;
+	cpVect n;
+	cpFloat nMass;
+	
+	cpFloat jnAcc, jnMax;
+	cpFloat bias;
+} cpPinJoint;
+
+cpPinJoint *cpPinJointAlloc(void);
+cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+
+CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1);
+CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2);
+CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist);

src/helper/chipmunk/constraints/cpPivotJoint.c

@@ -0,0 +1,114 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	joint->r1 = cpvrotate(joint->anchr1, a->rot);
+	joint->r2 = cpvrotate(joint->anchr2, b->rot);
+	
+	// Calculate mass tensor
+	k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
+	
+	// compute max impulse
+	joint->jMaxLen = J_MAX(joint, dt);
+	
+	// calculate bias velocity
+	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
+	
+	// apply accumulated impulse
+	apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
+}
+
+static void
+applyImpulse(cpPivotJoint *joint)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	cpVect r1 = joint->r1;
+	cpVect r2 = joint->r2;
+		
+	// compute relative velocity
+	cpVect vr = relative_velocity(a, b, r1, r2);
+	
+	// compute normal impulse
+	cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
+	cpVect jOld = joint->jAcc;
+	joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
+	j = cpvsub(joint->jAcc, jOld);
+	
+	// apply impulse
+	apply_impulses(a, b, joint->r1, joint->r2, j);
+}
+
+static cpFloat
+getImpulse(cpConstraint *joint)
+{
+	return cpvlength(((cpPivotJoint *)joint)->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpPivotJoint)
+
+cpPivotJoint *
+cpPivotJointAlloc(void)
+{
+	return (cpPivotJoint *)cpmalloc(sizeof(cpPivotJoint));
+}
+
+cpPivotJoint *
+cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->anchr1 = anchr1;
+	joint->anchr2 = anchr2;
+	
+	joint->jAcc = cpvzero;
+	
+	return joint;
+}
+
+cpConstraint *
+cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+	return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
+}
+
+cpConstraint *
+cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
+{
+	cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
+	cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
+	return cpPivotJointNew2(a, b, anchr1, anchr2);
+}

src/helper/chipmunk/constraints/cpPivotJoint.h

@@ -0,0 +1,42 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpPivotJointGetClass();
+
+typedef struct cpPivotJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	
+	cpVect r1, r2;
+	cpVect k1, k2;
+	
+	cpVect jAcc;
+	cpFloat jMaxLen;
+	cpVect bias;
+} cpPivotJoint;
+
+cpPivotJoint *cpPivotJointAlloc(void);
+cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot);
+cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+
+CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1);
+CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2);

src/helper/chipmunk/constraints/cpRatchetJoint.c

@@ -0,0 +1,126 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	cpFloat angle = joint->angle;
+	cpFloat phase = joint->phase;
+	cpFloat ratchet = joint->ratchet;
+	
+	cpFloat delta = b->a - a->a;
+	cpFloat diff = angle - delta;
+	cpFloat pdist = 0.0f;
+	
+	if(diff*ratchet > 0.0f){
+		pdist = diff;
+	} else {
+		joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
+	}
+	
+	// calculate moment of inertia coefficient.
+	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint->constraint.maxBias;
+	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
+	
+	// compute max impulse
+	joint->jMax = J_MAX(joint, dt);
+
+	// If the bias is 0, the joint is not at a limit. Reset the impulse.
+	if(!joint->bias)
+		joint->jAcc = 0.0f;
+
+	// apply joint torque
+	a->w -= joint->jAcc*a->i_inv;
+	b->w += joint->jAcc*b->i_inv;
+}
+
+static void
+applyImpulse(cpRatchetJoint *joint)
+{
+	if(!joint->bias) return; // early exit
+
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// compute relative rotational velocity
+	cpFloat wr = b->w - a->w;
+	cpFloat ratchet = joint->ratchet;
+	
+	// compute normal impulse	
+	cpFloat j = -(joint->bias + wr)*joint->iSum;
+	cpFloat jOld = joint->jAcc;
+	joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet;
+	j = joint->jAcc - jOld;
+	
+	// apply impulse
+	a->w -= j*a->i_inv;
+	b->w += j*b->i_inv;
+}
+
+static cpFloat
+getImpulse(cpRatchetJoint *joint)
+{
+	return cpfabs(joint->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpRatchetJoint)
+
+cpRatchetJoint *
+cpRatchetJointAlloc(void)
+{
+	return (cpRatchetJoint *)cpmalloc(sizeof(cpRatchetJoint));
+}
+
+cpRatchetJoint *
+cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->angle = 0.0f;
+	joint->phase = phase;
+	joint->ratchet = ratchet;
+	
+	// STATIC_BODY_CHECK
+	joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f);
+	
+	return joint;
+}
+
+cpConstraint *
+cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
+{
+	return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet);
+}

src/helper/chipmunk/constraints/cpRatchetJoint.h

@@ -0,0 +1,40 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpRatchetJointGetClass();
+
+typedef struct cpRatchetJoint {
+	cpConstraint constraint;
+	cpFloat angle, phase, ratchet;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+} cpRatchetJoint;
+
+cpRatchetJoint *cpRatchetJointAlloc(void);
+cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle);
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase);
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet);

src/helper/chipmunk/constraints/cpRotaryLimitJoint.c

@@ -0,0 +1,120 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	cpFloat dist = b->a - a->a;
+	cpFloat pdist = 0.0f;
+	if(dist > joint->max) {
+		pdist = joint->max - dist;
+	} else if(dist < joint->min) {
+		pdist = joint->min - dist;
+	}
+	
+	// calculate moment of inertia coefficient.
+	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint->constraint.maxBias;
+	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint->jMax = J_MAX(joint, dt);
+
+	// If the bias is 0, the joint is not at a limit. Reset the impulse.
+	if(!joint->bias)
+		joint->jAcc = 0.0f;
+
+	// apply joint torque
+	a->w -= joint->jAcc*a->i_inv;
+	b->w += joint->jAcc*b->i_inv;
+}
+
+static void
+applyImpulse(cpRotaryLimitJoint *joint)
+{
+	if(!joint->bias) return; // early exit
+
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// compute relative rotational velocity
+	cpFloat wr = b->w - a->w;
+	
+	// compute normal impulse	
+	cpFloat j = -(joint->bias + wr)*joint->iSum;
+	cpFloat jOld = joint->jAcc;
+	if(joint->bias < 0.0f){
+		joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax);
+	} else {
+		joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f);
+	}
+	j = joint->jAcc - jOld;
+	
+	// apply impulse
+	a->w -= j*a->i_inv;
+	b->w += j*b->i_inv;
+}
+
+static cpFloat
+getImpulse(cpRotaryLimitJoint *joint)
+{
+	return cpfabs(joint->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpRotaryLimitJoint)
+
+cpRotaryLimitJoint *
+cpRotaryLimitJointAlloc(void)
+{
+	return (cpRotaryLimitJoint *)cpmalloc(sizeof(cpRotaryLimitJoint));
+}
+
+cpRotaryLimitJoint *
+cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->min = min;
+	joint->max  = max;
+	
+	joint->jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
+{
+	return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
+}

src/helper/chipmunk/constraints/cpRotaryLimitJoint.h

@@ -0,0 +1,39 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpRotaryLimitJointGetClass();
+
+typedef struct cpRotaryLimitJoint {
+	cpConstraint constraint;
+	cpFloat min, max;
+	
+	cpFloat iSum;
+		
+	cpFloat bias;
+	cpFloat jAcc, jMax;
+} cpRotaryLimitJoint;
+
+cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void);
+cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+
+CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min);
+CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max);

src/helper/chipmunk/constraints/cpSimpleMotor.c

@@ -0,0 +1,97 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// calculate moment of inertia coefficient.
+	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
+	
+	// compute max impulse
+	joint->jMax = J_MAX(joint, dt);
+
+	// apply joint torque
+	a->w -= joint->jAcc*a->i_inv;
+	b->w += joint->jAcc*b->i_inv;
+}
+
+static void
+applyImpulse(cpSimpleMotor *joint)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	// compute relative rotational velocity
+	cpFloat wr = b->w - a->w + joint->rate;
+	
+	// compute normal impulse	
+	cpFloat j = -wr*joint->iSum;
+	cpFloat jOld = joint->jAcc;
+	joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
+	j = joint->jAcc - jOld;
+	
+	// apply impulse
+	a->w -= j*a->i_inv;
+	b->w += j*b->i_inv;
+}
+
+static cpFloat
+getImpulse(cpSimpleMotor *joint)
+{
+	return cpfabs(joint->jAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpSimpleMotor)
+
+cpSimpleMotor *
+cpSimpleMotorAlloc(void)
+{
+	return (cpSimpleMotor *)cpmalloc(sizeof(cpSimpleMotor));
+}
+
+cpSimpleMotor *
+cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->rate = rate;
+	
+	joint->jAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
+{
+	return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
+}

src/helper/chipmunk/constraints/cpSimpleMotor.h

@@ -0,0 +1,37 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpSimpleMotorGetClass();
+
+typedef struct cpSimpleMotor {
+	cpConstraint constraint;
+	cpFloat rate;
+	
+	cpFloat iSum;
+		
+	cpFloat jAcc, jMax;
+} cpSimpleMotor;
+
+cpSimpleMotor *cpSimpleMotorAlloc(void);
+cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
+cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
+
+CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate);

src/helper/chipmunk/constraints/cpSlideJoint.c

@@ -0,0 +1,129 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+static void
+preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
+{
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	joint->r1 = cpvrotate(joint->anchr1, a->rot);
+	joint->r2 = cpvrotate(joint->anchr2, b->rot);
+	
+	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	cpFloat dist = cpvlength(delta);
+	cpFloat pdist = 0.0f;
+	if(dist > joint->max) {
+		pdist = dist - joint->max;
+	} else if(dist < joint->min) {
+		pdist = joint->min - dist;
+		dist = -dist;
+	}
+	joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
+	
+	// calculate mass normal
+	joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
+	
+	// calculate bias velocity
+	cpFloat maxBias = joint->constraint.maxBias;
+	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
+	
+	// compute max impulse
+	joint->jnMax = J_MAX(joint, dt);
+
+	// apply accumulated impulse
+	if(!joint->bias) //{
+		// if bias is 0, then the joint is not at a limit.
+		joint->jnAcc = 0.0f;
+//	} else {
+		cpVect j = cpvmult(joint->n, joint->jnAcc);
+		apply_impulses(a, b, joint->r1, joint->r2, j);
+//	}
+}
+
+static void
+applyImpulse(cpSlideJoint *joint)
+{
+	if(!joint->bias) return;  // early exit
+
+	CONSTRAINT_BEGIN(joint, a, b);
+	
+	cpVect n = joint->n;
+	cpVect r1 = joint->r1;
+	cpVect r2 = joint->r2;
+		
+	// compute relative velocity
+	cpVect vr = relative_velocity(a, b, r1, r2);
+	cpFloat vrn = cpvdot(vr, n);
+	
+	// compute normal impulse
+	cpFloat jn = (joint->bias - vrn)*joint->nMass;
+	cpFloat jnOld = joint->jnAcc;
+	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
+	jn = joint->jnAcc - jnOld;
+	
+	// apply impulse
+	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
+}
+
+static cpFloat
+getImpulse(cpConstraint *joint)
+{
+	return cpfabs(((cpSlideJoint *)joint)->jnAcc);
+}
+
+static const cpConstraintClass klass = {
+	(cpConstraintPreStepFunction)preStep,
+	(cpConstraintApplyImpulseFunction)applyImpulse,
+	(cpConstraintGetImpulseFunction)getImpulse,
+};
+CP_DefineClassGetter(cpSlideJoint)
+
+cpSlideJoint *
+cpSlideJointAlloc(void)
+{
+	return (cpSlideJoint *)cpmalloc(sizeof(cpSlideJoint));
+}
+
+cpSlideJoint *
+cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
+	
+	joint->anchr1 = anchr1;
+	joint->anchr2 = anchr2;
+	joint->min = min;
+	joint->max = max;
+	
+	joint->jnAcc = 0.0f;
+	
+	return joint;
+}
+
+cpConstraint *
+cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+	return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
+}

src/helper/chipmunk/constraints/cpSlideJoint.h

@@ -0,0 +1,44 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+const cpConstraintClass *cpSlideJointGetClass();
+
+typedef struct cpSlideJoint {
+	cpConstraint constraint;
+	cpVect anchr1, anchr2;
+	cpFloat min, max;
+	
+	cpVect r1, r2;
+	cpVect n;
+	cpFloat nMass;
+	
+	cpFloat jnAcc, jnMax;
+	cpFloat bias;
+} cpSlideJoint;
+
+cpSlideJoint *cpSlideJointAlloc(void);
+cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+
+CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1);
+CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2);
+CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min);
+CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max);

src/helper/chipmunk/constraints/util.h

@@ -0,0 +1,130 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(){return (cpConstraintClass *)&klass;}
+
+void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b);
+
+#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt))
+
+// Get valid body pointers and exit early if the bodies are idle
+#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \
+cpBody *a_var, *b_var; { \
+	a_var = ((cpConstraint *)constraint)->a; \
+	b_var = ((cpConstraint *)constraint)->b; \
+}
+
+static inline cpVect
+relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
+	cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
+	cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
+	
+	return cpvsub(v2_sum, v1_sum);
+}
+
+static inline cpFloat
+normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
+	return cpvdot(relative_velocity(a, b, r1, r2), n);
+}
+
+static inline void
+apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
+{
+	cpBodyApplyImpulse(a, cpvneg(j), r1);
+	cpBodyApplyImpulse(b, j, r2);
+}
+
+static inline void
+apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
+{
+	body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
+	body->w_bias += body->i_inv*cpvcross(r, j);
+}
+
+static inline void
+apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
+{
+	apply_bias_impulse(a, cpvneg(j), r1);
+	apply_bias_impulse(b, j, r2);
+}
+
+static inline cpVect
+clamp_vect(cpVect v, cpFloat len)
+{
+	return cpvclamp(v, len);
+//	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
+}
+
+static inline cpFloat
+k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
+{
+	cpFloat mass_sum = a->m_inv + b->m_inv;
+	cpFloat r1cn = cpvcross(r1, n);
+	cpFloat r2cn = cpvcross(r2, n);
+	
+	cpFloat value = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
+	cpAssert(value != 0.0, "Unsolvable collision or constraint.");
+	
+	return value;
+}
+
+static inline void
+k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
+{
+	// calculate mass matrix
+	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
+	cpFloat k11, k12, k21, k22;
+	cpFloat m_sum = a->m_inv + b->m_inv;
+	
+	// start with I*m_sum
+	k11 = m_sum; k12 = 0.0f;
+	k21 = 0.0f;  k22 = m_sum;
+	
+	// add the influence from r1
+	cpFloat a_i_inv = a->i_inv;
+	cpFloat r1xsq =  r1.x * r1.x * a_i_inv;
+	cpFloat r1ysq =  r1.y * r1.y * a_i_inv;
+	cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
+	k11 += r1ysq; k12 += r1nxy;
+	k21 += r1nxy; k22 += r1xsq;
+	
+	// add the influnce from r2
+	cpFloat b_i_inv = b->i_inv;
+	cpFloat r2xsq =  r2.x * r2.x * b_i_inv;
+	cpFloat r2ysq =  r2.y * r2.y * b_i_inv;
+	cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
+	k11 += r2ysq; k12 += r2nxy;
+	k21 += r2nxy; k22 += r2xsq;
+	
+	// invert
+	cpFloat determinant = k11*k22 - k12*k21;
+	cpAssert(determinant != 0.0, "Unsolvable constraint.");
+	
+	cpFloat det_inv = 1.0f/determinant;
+	*k1 = cpv( k22*det_inv, -k12*det_inv);
+	*k2 = cpv(-k21*det_inv,  k11*det_inv);
+}
+
+static inline cpVect
+mult_k(cpVect vr, cpVect k1, cpVect k2)
+{
+	return cpv(cpvdot(vr, k1), cpvdot(vr, k2));
+}

src/helper/chipmunk/cpArbiter.c

@@ -0,0 +1,256 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "constraints/util.h"
+
+cpFloat cp_bias_coef = 0.1f;
+cpFloat cp_collision_slop = 0.1f;
+
+cpContact*
+cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
+{
+	con->p = p;
+	con->n = n;
+	con->dist = dist;
+	
+	con->jnAcc = 0.0f;
+	con->jtAcc = 0.0f;
+	con->jBias = 0.0f;
+	
+	con->hash = hash;
+		
+	return con;
+}
+
+cpVect
+cpArbiterTotalImpulse(cpArbiter *arb)
+{
+	cpContact *contacts = arb->contacts;
+	cpVect sum = cpvzero;
+	
+	for(int i=0, count=arb->numContacts; i<count; i++){
+		cpContact *con = &contacts[i];
+		sum = cpvadd(sum, cpvmult(con->n, con->jnAcc));
+	}
+		
+	return sum;
+}
+
+cpVect
+cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
+{
+	cpContact *contacts = arb->contacts;
+	cpVect sum = cpvzero;
+	
+	for(int i=0, count=arb->numContacts; i<count; i++){
+		cpContact *con = &contacts[i];
+		sum = cpvadd(sum, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
+	}
+		
+	return sum;
+}
+
+cpFloat
+cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
+{
+	cpFloat fsum = 0.0f;
+	cpVect vsum = cpvzero;
+	
+	for(int i=0; i<numContacts; i++){
+		cpContact *con = &contacts[i];
+		cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
+		
+		fsum += cpvlength(j);
+		vsum = cpvadd(vsum, j);
+	}
+	
+	cpFloat vmag = cpvlength(vsum);
+	return (1.0f - vmag/fsum);
+}
+
+void
+cpArbiterIgnore(cpArbiter *arb)
+{
+	arb->state = cpArbiterStateIgnore;
+}
+
+cpArbiter*
+cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
+{
+	arb->handler = NULL;
+	arb->swappedColl = cpFalse;
+	
+	arb->e = 0.0f;
+	arb->u = 0.0f;
+	arb->surface_vr = cpvzero;
+	
+	arb->numContacts = 0;
+	arb->contacts = NULL;
+	
+	arb->a = a;
+	arb->b = b;
+	
+	arb->nextA = NULL;
+	arb->nextB = NULL;
+	
+	arb->stamp = 0;
+	arb->state = cpArbiterStateFirstColl;
+	
+	return arb;
+}
+
+void
+cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
+{
+	// Arbiters without contact data may exist if a collision function rejected the collision.
+	if(arb->contacts){
+		// Iterate over the possible pairs to look for hash value matches.
+		for(int i=0; i<arb->numContacts; i++){
+			cpContact *old = &arb->contacts[i];
+			
+			for(int j=0; j<numContacts; j++){
+				cpContact *new_contact = &contacts[j];
+				
+				// This could trigger false positives, but is fairly unlikely nor serious if it does.
+				if(new_contact->hash == old->hash){
+					// Copy the persistant contact information.
+					new_contact->jnAcc = old->jnAcc;
+					new_contact->jtAcc = old->jtAcc;
+				}
+			}
+		}
+	}
+	
+	arb->contacts = contacts;
+	arb->numContacts = numContacts;
+	
+	arb->handler = handler;
+	arb->swappedColl = (a->collision_type != handler->a);
+	
+	arb->e = a->e * b->e;
+	arb->u = a->u * b->u;
+	arb->surface_vr = cpvsub(a->surface_v, b->surface_v);
+	
+	// For collisions between two similar primitive types, the order could have been swapped.
+	arb->a = a;
+	arb->b = b;
+	
+	// mark it as new if it's been cached
+	if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
+}
+
+void
+cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
+{
+	cpBody *a = arb->a->body;
+	cpBody *b = arb->b->body;
+	
+	for(int i=0; i<arb->numContacts; i++){
+		cpContact *con = &arb->contacts[i];
+		
+		// Calculate the offsets.
+		con->r1 = cpvsub(con->p, a->p);
+		con->r2 = cpvsub(con->p, b->p);
+		
+		// Calculate the mass normal and mass tangent.
+		con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
+		con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
+				
+		// Calculate the target bias velocity.
+		con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
+		con->jBias = 0.0f;
+		
+		// Calculate the target bounce velocity.
+		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e;
+	}
+}
+
+void
+cpArbiterApplyCachedImpulse(cpArbiter *arb)
+{
+	cpShape *shapea = arb->a;
+	cpShape *shapeb = arb->b;
+		
+	arb->u = shapea->u * shapeb->u;
+	arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v);
+
+	cpBody *a = shapea->body;
+	cpBody *b = shapeb->body;
+	
+	for(int i=0; i<arb->numContacts; i++){
+		cpContact *con = &arb->contacts[i];
+		apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
+	}
+}
+
+void
+cpArbiterApplyImpulse(cpArbiter *arb)
+{
+	cpBody *a = arb->a->body;
+	cpBody *b = arb->b->body;
+
+	for(int i=0; i<arb->numContacts; i++){
+		cpContact *con = &arb->contacts[i];
+		cpVect n = con->n;
+		cpVect r1 = con->r1;
+		cpVect r2 = con->r2;
+		
+		// Calculate the relative bias velocities.
+		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
+		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
+		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
+		
+		// Calculate and clamp the bias impulse.
+		cpFloat jbn = (con->bias - vbn)*con->nMass;
+		cpFloat jbnOld = con->jBias;
+		con->jBias = cpfmax(jbnOld + jbn, 0.0f);
+		jbn = con->jBias - jbnOld;
+		
+		// Apply the bias impulse.
+		apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));
+
+		// Calculate the relative velocity.
+		cpVect vr = relative_velocity(a, b, r1, r2);
+		cpFloat vrn = cpvdot(vr, n);
+		
+		// Calculate and clamp the normal impulse.
+		cpFloat jn = -(con->bounce + vrn)*con->nMass;
+		cpFloat jnOld = con->jnAcc;
+		con->jnAcc = cpfmax(jnOld + jn, 0.0f);
+		jn = con->jnAcc - jnOld;
+		
+		// Calculate the relative tangent velocity.
+		cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n));
+		
+		// Calculate and clamp the friction impulse.
+		cpFloat jtMax = arb->u*con->jnAcc;
+		cpFloat jt = -vrt*con->tMass;
+		cpFloat jtOld = con->jtAcc;
+		con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
+		jt = con->jtAcc - jtOld;
+		
+		// Apply the final impulse.
+		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
+	}
+}

src/helper/chipmunk/cpArbiter.h

@@ -0,0 +1,181 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+ 
+// Determines how fast penetrations resolve themselves expressed as a percentage per step. Defaults to 0.1.
+extern cpFloat cp_bias_coef;
+
+// Amount of allowed penetration. Used to reduce oscillating contacts and keep the collision cache warm. Defaults to 0.1.
+extern cpFloat cp_collision_slop;
+
+// User collision handler function types.
+typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, void *data);
+typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, void *data);
+typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, void *data);
+typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, void *data);
+
+// Structure for holding collision handler function callback information.
+typedef struct cpCollisionHandler {
+	cpCollisionType a;
+	cpCollisionType b;
+	cpCollisionBeginFunc begin;
+	cpCollisionPreSolveFunc preSolve;
+	cpCollisionPostSolveFunc postSolve;
+	cpCollisionSeparateFunc separate;
+	void *data;
+} cpCollisionHandler;
+
+// Data structure for contact points.
+typedef struct cpContact {
+	// Contact point and normal.
+	cpVect CP_PRIVATE(p), CP_PRIVATE(n);
+	// Penetration distance.
+	CP_PRIVATE(cpFloat dist);
+	
+	// Calculated by cpArbiterPreStep().
+	cpVect CP_PRIVATE(r1), CP_PRIVATE(r2);
+	cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce);
+
+	// Persistant contact information.
+	cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias);
+	CP_PRIVATE(cpFloat bias);
+	
+	// Hash value used to (mostly) uniquely identify a contact.
+	CP_PRIVATE(cpHashValue hash);
+} cpContact;
+
+#define CP_MAX_CONTACTS_PER_ARBITER 4
+
+typedef enum cpArbiterState {
+	cpArbiterStateNormal,
+	cpArbiterStateFirstColl,
+	cpArbiterStateIgnore,
+	cpArbiterStateCached,
+} cpArbiterState;
+
+// Data structure for tracking collisions between shapes.
+struct cpArbiter {
+	// Information on the contact points between the objects.
+	CP_PRIVATE(int numContacts);
+	CP_PRIVATE(cpContact *contacts);
+	
+	// The two shapes and bodies involved in the collision.
+	// These variables are NOT in the order defined by the collision handler.
+	cpShape CP_PRIVATE(*a), CP_PRIVATE(*b);
+	cpArbiter CP_PRIVATE(*nextA), CP_PRIVATE(*nextB);
+	
+	// Calculated before calling the pre-solve collision handler
+	// Override them with custom values if you want specialized behavior
+	CP_PRIVATE(cpFloat e);
+	CP_PRIVATE(cpFloat u);
+	 // Used for surface_v calculations, implementation may change
+	CP_PRIVATE(cpVect surface_vr);
+	
+	// Time stamp of the arbiter. (from cpSpace)
+	CP_PRIVATE(cpTimestamp stamp);
+	
+	CP_PRIVATE(cpCollisionHandler *handler);
+	
+	// Are the shapes swapped in relation to the collision handler?
+	CP_PRIVATE(cpBool swappedColl);
+	CP_PRIVATE(cpArbiterState state);
+};
+
+// Arbiter Helper Functions
+cpVect cpArbiterTotalImpulse(cpArbiter *arb);
+cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb);
+void cpArbiterIgnore(cpArbiter *arb);
+
+
+static inline void
+cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
+{
+	if(arb->CP_PRIVATE(swappedColl)){
+		(*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a);
+	} else {
+		(*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b);
+	}
+}
+#define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b);
+
+static inline void
+cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
+{
+	CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
+	(*a) = shape_a->body;
+	(*b) = shape_b->body;
+}
+#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b);
+
+static inline cpBool
+cpArbiterIsFirstContact(const cpArbiter *arb)
+{
+	return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl;
+}
+
+static inline int
+cpArbiterGetCount(const cpArbiter *arb)
+{
+	return arb->CP_PRIVATE(numContacts);
+}
+
+static inline cpVect
+cpArbiterGetNormal(const cpArbiter *arb, int i)
+{
+	cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
+	return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n;
+}
+
+static inline cpVect
+cpArbiterGetPoint(const cpArbiter *arb, int i)
+{
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+}
+
+static inline cpFloat
+cpArbiteGetDepth(const cpArbiter *arb, int i)
+{
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+}
+
+typedef struct cpContactPointSet {
+	int count;
+	
+	struct {
+		cpVect point, normal;
+		cpFloat dist;
+	} points[CP_MAX_CONTACTS_PER_ARBITER];
+} cpContactPointSet;
+
+static inline cpContactPointSet
+cpArbiterGetContactPointSet(const cpArbiter *arb)
+{
+	cpContactPointSet set;
+	set.count = cpArbiterGetCount(arb);
+	
+	int i;
+	for(i=0; i<set.count; i++){
+		set.points[i].point = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+		set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
+		set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+	}
+	
+	return set;
+}

src/helper/chipmunk/cpArray.c

@@ -0,0 +1,140 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <string.h>
+
+#include "chipmunk_private.h"
+
+
+cpArray*
+cpArrayAlloc(void)
+{
+	return (cpArray *)cpcalloc(1, sizeof(cpArray));
+}
+
+cpArray*
+cpArrayInit(cpArray *arr, int size)
+{
+	arr->num = 0;
+	arr->max = (size ? size : 4);
+	arr->arr = (void **)cpmalloc(arr->max*sizeof(void**));
+	
+	return arr;
+}
+
+cpArray*
+cpArrayNew(int size)
+{
+	return cpArrayInit(cpArrayAlloc(), size);
+}
+
+void
+cpArrayDestroy(cpArray *arr)
+{
+	cpfree(arr->arr);
+	arr->arr = NULL;
+}
+
+void
+cpArrayFree(cpArray *arr)
+{
+	if(arr){
+		cpArrayDestroy(arr);
+		cpfree(arr);
+	}
+}
+
+void
+cpArrayPush(cpArray *arr, void *object)
+{
+	if(arr->num == arr->max){
+		arr->max *= 2;
+		arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**));
+	}
+	
+	arr->arr[arr->num] = object;
+	arr->num++;
+}
+
+void *
+cpArrayPop(cpArray *arr)
+{
+	arr->num--;
+	
+	void *value = arr->arr[arr->num];
+	arr->arr[arr->num] = NULL;
+	
+	return value;
+}
+
+//static void
+//cpArrayDeleteIndex(cpArray *arr, int idx)
+//{
+//	arr->num--;
+//	
+//	arr->arr[idx] = arr->arr[arr->num];
+//	arr->arr[arr->num] = NULL;
+//}
+
+void
+cpArrayDeleteObj(cpArray *arr, void *obj)
+{
+	for(int i=0; i<arr->num; i++){
+		if(arr->arr[i] == obj){
+			arr->num--;
+			
+			arr->arr[i] = arr->arr[arr->num];
+			arr->arr[arr->num] = NULL;
+			
+			return;
+		}
+	}
+}
+
+//void
+//cpArrayAppend(cpArray *arr, cpArray *other)
+//{
+//	void *tail = &arr->arr[arr->num];
+//	
+//	arr->num += other->num;
+//	if(arr->num >= arr->max){
+//		arr->max = arr->num;
+//		arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**));
+//	}
+//	
+//	memcpy(tail, other->arr, other->num*sizeof(void**));
+//}
+
+void
+cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*))
+{
+	for(int i=0; i<arr->num; i++) freeFunc(arr->arr[i]);
+}
+
+cpBool
+cpArrayContains(cpArray *arr, void *ptr)
+{
+	for(int i=0; i<arr->num; i++)
+		if(arr->arr[i] == ptr) return cpTrue;
+	
+	return cpFalse;
+}

src/helper/chipmunk/cpBB.c

@@ -0,0 +1,47 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <math.h>
+#include <stdlib.h>
+
+#include "chipmunk.h"
+
+cpVect
+cpBBClampVect(const cpBB bb, const cpVect v)
+{
+	cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r);
+	cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t);
+	return cpv(x, y);
+}
+
+cpVect
+cpBBWrapVect(const cpBB bb, const cpVect v)
+{
+	cpFloat ix = cpfabs(bb.r - bb.l);
+	cpFloat modx = cpfmod(v.x - bb.l, ix);
+	cpFloat x = (modx > 0.0f) ? modx : modx + ix;
+	
+	cpFloat iy = cpfabs(bb.t - bb.b);
+	cpFloat mody = cpfmod(v.y - bb.b, iy);
+	cpFloat y = (mody > 0.0f) ? mody : mody + iy;
+	
+	return cpv(x + bb.l, y + bb.b);
+}

src/helper/chipmunk/cpBB.h

@@ -0,0 +1,102 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+// TODO fix naming
+typedef struct cpBB{
+	cpFloat l, b, r ,t;
+} cpBB;
+
+static inline cpBB
+cpBBNew(const cpFloat l, const cpFloat b,
+		const cpFloat r, const cpFloat t)
+{
+	cpBB bb = {l, b, r, t};
+	return bb;
+}
+
+static inline cpBool
+cpBBintersects(const cpBB a, const cpBB b)
+{
+	return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t);
+}
+
+static inline cpBool
+cpBBcontainsBB(const cpBB bb, const cpBB other)
+{
+	return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t);
+}
+
+static inline cpBool
+cpBBcontainsVect(const cpBB bb, const cpVect v)
+{
+	return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y);
+}
+
+static inline cpBB
+cpBBmerge(const cpBB a, const cpBB b){
+	return cpBBNew(
+		cpfmin(a.l, b.l),
+		cpfmin(a.b, b.b),
+		cpfmax(a.r, b.r),
+		cpfmax(a.t, b.t)
+	);
+}
+
+static inline cpBB
+cpBBexpand(const cpBB bb, const cpVect v){
+	return cpBBNew(
+		cpfmin(bb.l, v.x),
+		cpfmin(bb.b, v.y),
+		cpfmax(bb.r, v.x),
+		cpfmax(bb.t, v.y)
+	);
+}
+
+static inline cpFloat
+cpBBArea(cpBB bb)
+{
+	return (bb.r - bb.l)*(bb.t - bb.b);
+}
+
+static inline cpFloat
+cpBBMergedArea(cpBB a, cpBB b)
+{
+	return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b));
+}
+
+static inline cpBool
+cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b)
+{
+	cpBB seg_bb = cpBBNew(cpfmin(a.x, b.x), cpfmin(a.y, b.y), cpfmax(a.x, b.x), cpfmax(a.y, b.y));
+	if(cpBBintersects(bb, seg_bb)){
+		cpVect axis = cpv(b.y - a.y, a.x - b.x);
+		cpVect offset = cpv((a.x + b.x - bb.r - bb.l), (a.y + b.y - bb.t - bb.b));
+		cpVect extents = cpv(bb.r - bb.l, bb.t - bb.b);
+		
+		return (cpfabs(cpvdot(axis, offset)) < cpfabs(axis.x*extents.x) + cpfabs(axis.y*extents.y));
+	}
+	
+	return cpFalse;
+}
+
+cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox
+// TODO edge case issue
+cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox

src/helper/chipmunk/cpBBTree.c

@@ -0,0 +1,847 @@
+#include "stdlib.h"
+#include "stdio.h"
+
+#include "chipmunk_private.h"
+
+static cpSpatialIndexClass klass;
+
+typedef struct Node Node;
+typedef struct Pair Pair;
+
+struct cpBBTree {
+	cpSpatialIndex spatialIndex;
+	cpBBTreeVelocityFunc velocityFunc;
+	
+	cpHashSet *leaves;
+	Node *root;
+	
+	Node *pooledNodes;
+	Pair *pooledPairs;
+	cpArray *allocatedBuffers;
+	
+	cpTimestamp stamp;
+};
+
+struct Node {
+	void *obj;
+	cpBB bb;
+	Node *parent;
+	
+	union {
+		// Internal nodes
+		struct { Node *a, *b; };
+		
+		// Leaves
+		struct {
+			cpTimestamp stamp;
+			Pair *pairs;
+		};
+	};
+};
+
+typedef struct Thread {
+	Pair *prev;
+	Node *leaf;
+	Pair *next;
+} Thread;
+
+struct Pair { Thread a, b; };
+
+#pragma mark Misc Functions
+
+//static inline cpFloat
+//cpBBProximity(cpBB a, cpBB b)
+//{
+//	return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + b.t - b.b - b.t);
+//}
+
+static inline cpBB
+GetBB(cpBBTree *tree, void *obj)
+{
+	cpBB bb = tree->spatialIndex.bbfunc(obj);
+	
+	cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc;
+	if(velocityFunc){
+		cpFloat coef = 0.1f;
+		cpFloat x = (bb.r - bb.l)*coef;
+		cpFloat y = (bb.t - bb.b)*coef;
+		
+		cpVect v = cpvmult(velocityFunc(obj), 0.1f);
+		return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y));
+	} else {
+		return bb;
+	}
+}
+
+static inline cpBBTree *
+GetTree(cpSpatialIndex *index)
+{
+	return (index && index->klass == &klass ? (cpBBTree *)index : NULL);
+}
+
+static inline Node *
+GetRootIfTree(cpSpatialIndex *index){
+	return (index && index->klass == &klass ? ((cpBBTree *)index)->root : NULL);
+}
+
+static inline cpTimestamp
+GetStamp(cpBBTree *tree)
+{
+	cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
+	return (dynamicTree ? dynamicTree->stamp : tree->stamp);
+}
+
+static inline void
+IncrementStamp(cpBBTree *tree)
+{
+	cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
+	if(dynamicTree){
+		dynamicTree->stamp++;
+	} else {
+		tree->stamp++;
+	}
+}
+
+#pragma mark Pair/Thread Functions
+
+static void
+PairRecycle(cpBBTree *tree, Pair *pair)
+{
+	pair->a.next = tree->pooledPairs;
+	tree->pooledPairs = pair;
+}
+
+static Pair *
+PairFromPool(cpBBTree *tree)
+{
+	Pair *pair = tree->pooledPairs;
+	
+	if(pair){
+		tree->pooledPairs = pair->a.next;
+		return pair;
+	} else {
+		// Pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(Pair);
+		cpAssert(count, "Buffer size is too small.");
+		
+		Pair *buffer = (Pair *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(tree->allocatedBuffers, buffer);
+		
+		// push all but the first one, return the first instead
+		for(int i=1; i<count; i++) PairRecycle(tree, buffer + i);
+		return buffer;
+	}
+}
+
+static inline void
+ThreadUnlink(Thread thread)
+{
+	Pair *next = thread.next;
+	Pair *prev = thread.prev;
+	
+	if(next){
+		if(next->a.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev;
+	}
+	
+	if(prev){
+		if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next;
+	} else {
+		thread.leaf->pairs = next;
+	}
+}
+
+static void
+PairsClear(Node *leaf, cpBBTree *tree)
+{
+	Pair *pair = leaf->pairs;
+	leaf->pairs = NULL;
+	
+	while(pair){
+		if(pair->a.leaf == leaf){
+			Pair *next = pair->a.next;
+			ThreadUnlink(pair->b);
+			PairRecycle(tree, pair);
+			pair = next;
+		} else {
+			Pair *next = pair->b.next;
+			ThreadUnlink(pair->a);
+			PairRecycle(tree, pair);
+			pair = next;
+		}
+	}
+}
+
+static void
+PairInsert(Node *a, Node *b, cpBBTree *tree)
+{
+	Pair *pair = PairFromPool(tree);
+	Pair *nextA = a->pairs, *nextB = b->pairs;
+	
+	if(nextA){
+		if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair;
+	}
+	
+	if(nextB){
+		if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair;
+	}
+	
+	(*pair) = (Pair){{NULL, a, nextA},{NULL, b, nextB}};
+	a->pairs = b->pairs = pair;
+}
+
+
+#pragma mark Node Functions
+
+static void
+NodeRecycle(cpBBTree *tree, Node *node)
+{
+	node->parent = tree->pooledNodes;
+	tree->pooledNodes = node;
+}
+
+static Node *
+NodeFromPool(cpBBTree *tree)
+{
+	Node *node = tree->pooledNodes;
+	
+	if(node){
+		tree->pooledNodes = node->parent;
+		return node;
+	} else {
+		// Pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(Node);
+		cpAssert(count, "Buffer size is too small.");
+		
+		Node *buffer = (Node *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(tree->allocatedBuffers, buffer);
+		
+		// push all but the first one, return the first instead
+		for(int i=1; i<count; i++) NodeRecycle(tree, buffer + i);
+		return buffer;
+	}
+}
+
+static inline void
+NodeSetA(Node *node, Node *value)
+{
+	node->a = value;
+	value->parent = node;
+}
+
+static inline void
+NodeSetB(Node *node, Node *value)
+{
+	node->b = value;
+	value->parent = node;
+}
+
+static Node *
+NodeNew(cpBBTree *tree, Node *a, Node *b)
+{
+	Node *node = NodeFromPool(tree);
+	
+	node->obj = NULL;
+	node->bb = cpBBmerge(a->bb, b->bb);
+	node->parent = NULL;
+	
+	NodeSetA(node, a);
+	NodeSetB(node, b);
+	
+	return node;
+}
+
+static inline cpBool
+NodeIsLeaf(Node *node)
+{
+	return (node->obj != NULL);
+}
+
+static inline Node *
+NodeOther(Node *node, Node *child)
+{
+	return (node->a == child ? node->b : node->a);
+}
+
+static inline void
+NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree)
+{
+	cpAssert(!NodeIsLeaf(parent), "Cannot replace child of a leaf.");
+	cpAssert(child == parent->a || child == parent->b, "Node is not a child of parent.");
+	
+	if(parent->a == child){
+		NodeRecycle(tree, parent->a);
+		NodeSetA(parent, value);
+	} else {
+		NodeRecycle(tree, parent->b);
+		NodeSetB(parent, value);
+	}
+	
+	for(Node *node=parent; node; node = node->parent){
+		node->bb = cpBBmerge(node->a->bb, node->b->bb);
+	}
+}
+
+#pragma mark Subtree Functions
+
+static Node *
+SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree)
+{
+	if(subtree == NULL){
+		return leaf;
+	} else if(NodeIsLeaf(subtree)){
+		return NodeNew(tree, leaf, subtree);
+	} else {
+		cpFloat cost_a = cpBBArea(subtree->b->bb) + cpBBMergedArea(subtree->a->bb, leaf->bb);
+		cpFloat cost_b = cpBBArea(subtree->a->bb) + cpBBMergedArea(subtree->b->bb, leaf->bb);
+		
+//		cpFloat cost_a = cpBBProximity(subtree->a->bb, leaf->bb);
+//		cpFloat cost_b = cpBBProximity(subtree->b->bb, leaf->bb);
+		
+		if(cost_b < cost_a){
+			NodeSetB(subtree, SubtreeInsert(subtree->b, leaf, tree));
+		} else {
+			NodeSetA(subtree, SubtreeInsert(subtree->a, leaf, tree));
+		}
+		
+		subtree->bb = cpBBmerge(subtree->bb, leaf->bb);
+		return subtree;
+	}
+}
+
+static void
+SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data)
+{
+	if(cpBBintersects(subtree->bb, bb)){
+		if(NodeIsLeaf(subtree)){
+			func(obj, subtree->obj, data);
+		} else {
+			SubtreeQuery(subtree->a, obj, bb, func, data);
+			SubtreeQuery(subtree->b, obj, bb, func, data);
+		}
+	}
+}
+
+static void
+SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpSpatialIndexSegmentQueryCallback func, void *data)
+{
+	if(cpBBIntersectsSegment(subtree->bb, a, b)){
+		if(NodeIsLeaf(subtree)){
+			func(obj, subtree->obj, data);
+		} else {
+			SubtreeSegmentQuery(subtree->a, obj, a, b, func, data);
+			SubtreeSegmentQuery(subtree->b, obj, a, b, func, data);
+		}
+	}
+}
+
+static void
+SubtreeRecycle(cpBBTree *tree, Node *node)
+{
+	if(!NodeIsLeaf(node)){
+		SubtreeRecycle(tree, node->a);
+		SubtreeRecycle(tree, node->b);
+		NodeRecycle(tree, node);
+	}
+}
+
+static inline Node *
+SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree)
+{
+	if(leaf == subtree){
+		return NULL;
+	} else {
+		Node *parent = leaf->parent;
+		if(parent == subtree){
+			Node *other = NodeOther(subtree, leaf);
+			other->parent = subtree->parent;
+			NodeRecycle(tree, subtree);
+			return other;
+		} else {
+			NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree);
+			return subtree;
+		}
+	}
+}
+
+#pragma mark Marking Functions
+
+typedef struct MarkContext {
+	cpBBTree *tree;
+	Node *staticRoot;
+	cpSpatialIndexQueryCallback func;
+	void *data;
+} MarkContext;
+
+static void
+MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context)
+{
+	if(cpBBintersects(leaf->bb, subtree->bb)){
+		if(NodeIsLeaf(subtree)){
+			if(left){
+				PairInsert(leaf, subtree, context->tree);
+			} else {
+				if(subtree->stamp < leaf->stamp) PairInsert(subtree, leaf, context->tree);
+				context->func(leaf->obj, subtree->obj, context->data);
+			}
+		} else {
+			MarkLeafQuery(subtree->a, leaf, left, context);
+			MarkLeafQuery(subtree->b, leaf, left, context);
+		}
+	}
+}
+
+static void
+MarkLeaf(Node *leaf, MarkContext *context)
+{
+	cpBBTree *tree = context->tree;
+	if(leaf->stamp == GetStamp(tree)){
+		Node *staticRoot = context->staticRoot;
+		if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context);
+		
+		for(Node *node = leaf; node->parent; node = node->parent){
+			if(node == node->parent->a){
+				MarkLeafQuery(node->parent->b, leaf, cpTrue, context);
+			} else {
+				MarkLeafQuery(node->parent->a, leaf, cpFalse, context);
+			}
+		}
+	} else {
+		Pair *pair = leaf->pairs;
+		while(pair){
+			if(leaf == pair->b.leaf){
+				context->func(pair->a.leaf->obj, leaf->obj, context->data);
+				pair = pair->b.next;
+			} else {
+				pair = pair->a.next;
+			}
+		}
+	}
+}
+
+static void
+MarkSubtree(Node *subtree, MarkContext *context)
+{
+	if(NodeIsLeaf(subtree)){
+		MarkLeaf(subtree, context);
+	} else {
+		MarkSubtree(subtree->a, context);
+		MarkSubtree(subtree->b, context);
+	}
+}
+
+#pragma mark Leaf Functions
+
+static Node *
+LeafNew(cpBBTree *tree, void *obj, cpBB bb)
+{
+	Node *node = NodeFromPool(tree);
+	node->obj = obj;
+	node->bb = GetBB(tree, obj);
+	
+	node->parent = NULL;
+	node->stamp = 0;
+	node->pairs = NULL;
+	
+	return node;
+}
+
+static cpBool
+LeafUpdate(Node *leaf, cpBBTree *tree)
+{
+	Node *root = tree->root;
+	cpBB bb = tree->spatialIndex.bbfunc(leaf->obj);
+	
+	if(!cpBBcontainsBB(leaf->bb, bb)){
+		leaf->bb = GetBB(tree, leaf->obj);
+		
+		root = SubtreeRemove(root, leaf, tree);
+		tree->root = SubtreeInsert(root, leaf, tree);
+		
+		PairsClear(leaf, tree);
+		leaf->stamp = GetStamp(tree);
+		
+		return cpTrue;
+	}
+	
+	return cpFalse;
+}
+
+static void VoidQueryCallback(void *obj1, void *obj2, void *data){}
+
+static void
+LeafAddPairs(Node *leaf, cpBBTree *tree)
+{
+	cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex;
+	if(dynamicIndex){
+		cpBBTree *dynamicTree = GetTree(dynamicIndex);
+		Node *dynamicRoot = dynamicTree->root;
+		if(dynamicRoot){
+			MarkContext context = {dynamicTree, NULL, NULL, NULL};
+			MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context);
+		}
+	} else {
+		Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex);
+		MarkContext context = {tree, staticRoot, VoidQueryCallback, NULL};
+		MarkLeaf(leaf, &context);
+	}
+}
+
+#pragma mark Memory Management Functions
+
+cpBBTree *
+cpBBTreeAlloc(void)
+{
+	return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree));
+}
+
+static int
+leafSetEql(void *obj, Node *node)
+{
+	return (obj == node->obj);
+}
+
+static void *
+leafSetTrans(void *obj, cpBBTree *tree)
+{
+	return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj));
+}
+
+cpSpatialIndex *
+cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
+{
+	cpSpatialIndexInit((cpSpatialIndex *)tree, &klass, bbfunc, staticIndex);
+	
+	tree->velocityFunc = NULL;
+	
+	tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql, NULL);
+	tree->root = NULL;
+	
+	tree->pooledNodes = NULL;
+	tree->allocatedBuffers = cpArrayNew(0);
+	
+	tree->stamp = 0;
+	
+	return (cpSpatialIndex *)tree;
+}
+
+void
+cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func)
+{
+	if(index->klass != &klass){
+		cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index.");
+		return;
+	}
+	
+	((cpBBTree *)index)->velocityFunc = func;
+}
+
+cpSpatialIndex *
+cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
+{
+	return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex);
+}
+
+static void
+cpBBTreeDestroy(cpBBTree *tree)
+{
+	cpHashSetFree(tree->leaves);
+	
+	cpArrayFreeEach(tree->allocatedBuffers, cpfree);
+	cpArrayFree(tree->allocatedBuffers);
+}
+
+#pragma mark Insert/Remove
+
+static void
+cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid)
+{
+	Node *leaf = cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans);
+	
+	Node *root = tree->root;
+	tree->root = SubtreeInsert(root, leaf, tree);
+	
+	leaf->stamp = GetStamp(tree);
+	LeafAddPairs(leaf, tree);
+	IncrementStamp(tree);
+}
+
+static void
+cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid)
+{
+	Node *leaf = cpHashSetRemove(tree->leaves, hashid, obj);
+	
+	tree->root = SubtreeRemove(tree->root, leaf, tree);
+	PairsClear(leaf, tree);
+	NodeRecycle(tree, leaf);
+}
+
+static cpBool
+cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid)
+{
+	return (cpHashSetFind(tree->leaves, hashid, obj) != NULL);
+}
+
+#pragma mark Reindex
+
+static void
+cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryCallback func, void *data)
+{
+	if(!tree->root) return;
+	
+	// LeafUpdate() may modify tree->root. Don't cache it.
+	cpHashSetEach(tree->leaves, (cpHashSetIterFunc)LeafUpdate, tree);
+	
+	cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex;
+	Node *staticRoot = (staticIndex && staticIndex->klass == &klass ? ((cpBBTree *)staticIndex)->root : NULL);
+	
+	MarkContext context = {tree, staticRoot, func, data};
+	MarkSubtree(tree->root, &context);
+	if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data);
+	
+	IncrementStamp(tree);
+}
+
+static void
+cpBBTreeReindex(cpBBTree *tree)
+{
+	cpBBTreeReindexQuery(tree, VoidQueryCallback, NULL);
+}
+
+static void
+cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid)
+{
+	Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj);
+	if(leaf){
+		if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree);
+		IncrementStamp(tree);
+	}
+}
+
+#pragma mark Query
+
+static void
+cpBBTreePointQuery(cpBBTree *tree, cpVect point, cpSpatialIndexQueryCallback func, void *data)
+{
+	Node *root = tree->root;
+	if(root) SubtreeQuery(root, &point, cpBBNew(point.x, point.y, point.x, point.y), func, data);
+}
+
+static void
+cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data)
+{
+	Node *root = tree->root;
+	if(root) SubtreeSegmentQuery(root, obj, a, b, func, data);
+}
+
+static void
+cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data)
+{
+	if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data);
+}
+
+#pragma mark Misc
+
+static int
+cpBBTreeCount(cpBBTree *tree)
+{
+	return cpHashSetCount(tree->leaves);
+}
+
+typedef struct eachContext {
+	cpSpatialIndexIterator func;
+	void *data;
+} eachContext;
+
+static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);}
+
+static void
+cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIterator func, void *data)
+{
+	eachContext context = {func, data};
+	cpHashSetEach(tree->leaves, (cpHashSetIterFunc)each_helper, &context);
+}
+
+static cpSpatialIndexClass klass = {
+	(cpSpatialIndexDestroyFunc)cpBBTreeDestroy,
+	
+	(cpSpatialIndexCountFunc)cpBBTreeCount,
+	(cpSpatialIndexEachFunc)cpBBTreeEach,
+	
+	(cpSpatialIndexContainsFunc)cpBBTreeContains,
+	(cpSpatialIndexInsertFunc)cpBBTreeInsert,
+	(cpSpatialIndexRemoveFunc)cpBBTreeRemove,
+	
+	(cpSpatialIndexReindexFunc)cpBBTreeReindex,
+	(cpSpatialIndexReindexObjectFunc)cpBBTreeReindexObject,
+	(cpSpatialIndexReindexQueryFunc)cpBBTreeReindexQuery,
+	
+	(cpSpatialIndexPointQueryFunc)cpBBTreePointQuery,
+	(cpSpatialIndexSegmentQueryFunc)cpBBTreeSegmentQuery,
+	(cpSpatialIndexQueryFunc)cpBBTreeQuery,
+};
+
+#pragma mark Tree Optimization
+
+static int
+cpfcompare(const cpFloat *a, const cpFloat *b){
+	return (*a < *b ? -1 : (*b < *a ? 1 : 0));
+}
+
+static void
+fillNodeArray(Node *node, Node ***cursor){
+	(**cursor) = node;
+	(*cursor)++;
+}
+
+static Node *
+partitionNodes(cpBBTree *tree, Node **nodes, int count)
+{
+	if(count == 1){
+		return nodes[0];
+	} else if(count == 2) {
+		return NodeNew(tree, nodes[0], nodes[1]);
+	}
+	
+	// Find the AABB for these nodes
+	cpBB bb = nodes[0]->bb;
+	for(int i=1; i<count; i++) bb = cpBBmerge(bb, nodes[i]->bb);
+	
+	// Split it on it's longest axis
+	cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b);
+	
+	// Sort the bounds and use the median as the splitting point
+	cpFloat bounds[count*2];
+	if(splitWidth){
+		for(int i=0; i<count; i++){
+			bounds[2*i + 0] = nodes[i]->bb.l;
+			bounds[2*i + 1] = nodes[i]->bb.r;
+		}
+	} else {
+		for(int i=0; i<count; i++){
+			bounds[2*i + 0] = nodes[i]->bb.b;
+			bounds[2*i + 1] = nodes[i]->bb.t;
+		}
+	}
+	
+	qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare);
+	cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split
+	
+	// Generate the child BBs
+	cpBB a = bb, b = bb;
+	if(splitWidth) a.r = b.l = split; else a.t = b.b = split;
+	
+	// Partition the nodes
+	int right = count;
+	for(int left=0; left < right;){
+		Node *node = nodes[left];
+		if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){
+//		if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){
+			right--;
+			nodes[left] = nodes[right];
+			nodes[right] = node;
+		} else {
+			left++;
+		}
+	}
+	
+	if(right == count){
+		Node *node = NULL;
+		for(int i=0; i<count; i++) node = SubtreeInsert(node, nodes[i], tree);
+		return node;
+	}
+	
+	// Recurse and build the node!
+	return NodeNew(tree,
+		partitionNodes(tree, nodes, right),
+		partitionNodes(tree, nodes + right, count - right)
+	);
+}
+
+//static void
+//cpBBTreeOptimizeIncremental(cpBBTree *tree, int passes)
+//{
+//	for(int i=0; i<passes; i++){
+//		Node *root = tree->root;
+//		Node *node = root;
+//		int bit = 0;
+//		unsigned int path = tree->opath;
+//		
+//		while(!NodeIsLeaf(node)){
+//			node = (path&(1<<bit) ? node->a : node->b);
+//			bit = (bit + 1)&(sizeof(unsigned int)*8 - 1);
+//		}
+//		
+//		root = subtreeRemove(root, node, tree);
+//		tree->root = subtreeInsert(root, node, tree);
+//	}
+//}
+
+void
+cpBBTreeOptimize(cpSpatialIndex *index)
+{
+	if(index->klass != &klass){
+		cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
+		return;
+	}
+	
+	cpBBTree *tree = (cpBBTree *)index;
+	Node *root = tree->root;
+	if(!root) return;
+	
+	int count = cpBBTreeCount(tree);
+	Node *nodes[count];
+	Node **cursor = nodes;
+	
+	cpHashSetEach(tree->leaves, (cpHashSetIterFunc)fillNodeArray, &cursor);
+	
+	SubtreeRecycle(tree, root);
+	tree->root = partitionNodes(tree, nodes, count);
+}
+
+#pragma mark Debug Draw
+
+#define CP_BBTREE_DEBUG_DRAW
+#ifdef CP_BBTREE_DEBUG_DRAW
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include <GL/glut.h>
+
+static void
+NodeRender(Node *node, int depth)
+{
+	if(!NodeIsLeaf(node) && depth <= 10){
+		NodeRender(node->a, depth + 1);
+		NodeRender(node->b, depth + 1);
+	}
+	
+	cpBB bb = node->bb;
+	
+//	GLfloat v = depth/2.0f;	
+//	glColor3f(1.0f - v, v, 0.0f);
+	glLineWidth(cpfmax(5.0f - depth, 1.0f));
+	glBegin(GL_LINES); {
+		glVertex2f(bb.l, bb.b);
+		glVertex2f(bb.l, bb.t);
+		
+		glVertex2f(bb.l, bb.t);
+		glVertex2f(bb.r, bb.t);
+		
+		glVertex2f(bb.r, bb.t);
+		glVertex2f(bb.r, bb.b);
+		
+		glVertex2f(bb.r, bb.b);
+		glVertex2f(bb.l, bb.b);
+	}; glEnd();
+}
+
+void
+cpBBTreeRenderDebug(cpSpatialIndex *index){
+	if(index->klass != &klass){
+		cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index.");
+		return;
+	}
+	
+	cpBBTree *tree = (cpBBTree *)index;
+	if(tree->root) NodeRender(tree->root, 0);
+}
+#endif

src/helper/chipmunk/cpBody.c

@@ -0,0 +1,166 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <float.h>
+#include <stdarg.h>
+
+#include "chipmunk_private.h"
+
+// initialized in cpInitChipmunk()
+cpBody cpStaticBodySingleton;
+
+cpBody*
+cpBodyAlloc(void)
+{
+	return (cpBody *)cpmalloc(sizeof(cpBody));
+}
+
+cpBodyVelocityFunc cpBodyUpdateVelocityDefault = cpBodyUpdateVelocity;
+cpBodyPositionFunc cpBodyUpdatePositionDefault = cpBodyUpdatePosition;
+
+cpBody*
+cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
+{
+	body->velocity_func = cpBodyUpdateVelocityDefault;
+	body->position_func = cpBodyUpdatePositionDefault;
+	
+	cpBodySetMass(body, m);
+	cpBodySetMoment(body, i);
+
+	body->p = cpvzero;
+	body->v = cpvzero;
+	body->f = cpvzero;
+	
+	cpBodySetAngle(body, 0.0f);
+	body->w = 0.0f;
+	body->t = 0.0f;
+	
+	body->v_bias = cpvzero;
+	body->w_bias = 0.0f;
+	
+	body->data = NULL;
+	body->v_limit = (cpFloat)INFINITY;
+	body->w_limit = (cpFloat)INFINITY;
+	
+	body->space = NULL;
+	body->shapeList = NULL;
+	body->arbiterList = NULL;
+	body->constraintList = NULL;
+	
+	cpComponentNode node = {NULL, NULL, 0.0f};
+	body->node = node;
+	
+	return body;
+}
+
+cpBody*
+cpBodyNew(cpFloat m, cpFloat i)
+{
+	return cpBodyInit(cpBodyAlloc(), m, i);
+}
+
+cpBody *
+cpBodyInitStatic(cpBody *body)
+{
+	cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY);
+	body->node.idleTime = (cpFloat)INFINITY;
+	
+	return body;
+}
+
+cpBody *
+cpBodyNewStatic()
+{
+	return cpBodyInitStatic(cpBodyAlloc());
+}
+
+void cpBodyDestroy(cpBody *body){}
+
+void
+cpBodyFree(cpBody *body)
+{
+	if(body){
+		cpBodyDestroy(body);
+		cpfree(body);
+	}
+}
+
+void
+cpBodySetMass(cpBody *body, cpFloat mass)
+{
+	body->m = mass;
+	body->m_inv = 1.0f/mass;
+}
+
+void
+cpBodySetMoment(cpBody *body, cpFloat moment)
+{
+	body->i = moment;
+	body->i_inv = 1.0f/moment;
+}
+
+void
+cpBodySetAngle(cpBody *body, cpFloat angle)
+{
+	body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
+	body->rot = cpvforangle(angle);
+}
+
+void
+cpBodySlew(cpBody *body, cpVect pos, cpFloat dt)
+{
+	cpVect delta = cpvsub(pos, body->p);
+	body->v = cpvmult(delta, 1.0f/dt);
+}
+
+void
+cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
+{
+	body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), body->v_limit);
+	
+	cpFloat w_limit = body->w_limit;
+	body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit);
+}
+
+void
+cpBodyUpdatePosition(cpBody *body, cpFloat dt)
+{
+	body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
+	cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt);
+	
+	body->v_bias = cpvzero;
+	body->w_bias = 0.0f;
+}
+
+void
+cpBodyResetForces(cpBody *body)
+{
+	body->f = cpvzero;
+	body->t = 0.0f;
+}
+
+void
+cpBodyApplyForce(cpBody *body, cpVect force, cpVect r)
+{
+	body->f = cpvadd(body->f, force);
+	body->t += cpvcross(r, force);
+}

src/helper/chipmunk/cpBody.h

@@ -0,0 +1,212 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
+typedef void (*cpBodyPositionFunc)(cpBody *body, cpFloat dt);
+
+extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault;
+extern cpBodyPositionFunc cpBodyUpdatePositionDefault;
+
+// Structure to hold information about the contact graph components
+// when putting groups of objects to sleep.
+// No interesting user accessible fields.
+typedef struct cpComponentNode {
+	cpBody *root;
+	cpBody *next;
+	cpFloat idleTime;
+} cpComponentNode;
+
+struct cpBody {
+	// *** Integration Functions.
+
+	// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
+	cpBodyVelocityFunc velocity_func;
+	
+	// Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition)
+	cpBodyPositionFunc position_func;
+	
+	// *** Mass Properties
+	
+	// Mass and it's inverse.
+	// Always use cpBodySetMass() whenever changing the mass as these values must agree.
+	cpFloat m, m_inv;
+	
+	// Moment of inertia and it's inverse.
+	// Always use cpBodySetMoment() whenever changing the moment as these values must agree.
+	cpFloat i, i_inv;
+	
+	// *** Positional Properties
+	
+	// Linear components of motion (position, velocity, and force)
+	cpVect p, v, f;
+	
+	// Angular components of motion (angle, angular velocity, and torque)
+	// Always use cpBodySetAngle() to set the angle of the body as a and rot must agree.
+	cpFloat a, w, t;
+	
+	// Cached unit length vector representing the angle of the body.
+	// Used for fast vector rotation using cpvrotate().
+	cpVect rot;
+	
+	// *** User Definable Fields
+	
+	// User defined data pointer.
+	cpDataPointer data;
+	
+	// *** Other Fields
+	
+	// Maximum velocities this body can move at after integrating velocity
+	cpFloat v_limit, w_limit;
+	
+	// *** Internally Used Fields
+	
+	// Velocity bias values used when solving penetrations and correcting constraints.
+	CP_PRIVATE(cpVect v_bias);
+	CP_PRIVATE(cpFloat w_bias);
+	
+	// Space this body has been added to
+	CP_PRIVATE(cpSpace *space);
+	
+	// Pointer to the shape list.
+	// Shapes form a linked list using cpShape.next when added to a space.
+	CP_PRIVATE(cpShape *shapeList);
+	CP_PRIVATE(cpArbiter *arbiterList);
+	CP_PRIVATE(cpConstraint *constraintList);
+	
+	// Used by cpSpaceStep() to store contact graph information.
+	CP_PRIVATE(cpComponentNode node);
+};
+
+// Basic allocation/destruction functions
+cpBody *cpBodyAlloc(void);
+cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
+cpBody *cpBodyNew(cpFloat m, cpFloat i);
+
+cpBody *cpBodyInitStatic(cpBody *body);
+cpBody *cpBodyNewStatic();
+
+void cpBodyDestroy(cpBody *body);
+void cpBodyFree(cpBody *body);
+
+// Wake up a sleeping or idle body. (defined in cpSpace.c)
+void cpBodyActivate(cpBody *body);
+
+// Force a body to sleep;
+// defined in cpSpaceComponent.c
+void cpBodySleep(cpBody *body);
+void cpBodySleepWithGroup(cpBody *body, cpBody *group);
+
+static inline cpBool
+cpBodyIsSleeping(const cpBody *body)
+{
+	return (CP_PRIVATE(body->node).root != ((cpBody*)0));
+}
+
+static inline cpBool
+cpBodyIsStatic(const cpBody *body)
+{
+	return CP_PRIVATE(body->node).idleTime == INFINITY;
+}
+
+static inline cpBool
+cpBodyIsRogue(const cpBody *body)
+{
+	return (body->CP_PRIVATE(space) == ((cpSpace*)0));
+}
+
+
+#define CP_DefineBodyGetter(type, member, name) \
+static inline type cpBodyGet##name(const cpBody *body){return body->member;}
+
+#define CP_DefineBodySetter(type, member, name) \
+static inline void \
+cpBodySet##name(cpBody *body, const type value){ \
+	cpBodyActivate(body); \
+	body->member = value; \
+} \
+
+#define CP_DefineBodyProperty(type, member, name) \
+CP_DefineBodyGetter(type, member, name) \
+CP_DefineBodySetter(type, member, name)
+
+
+// Accessors for cpBody struct members
+CP_DefineBodyGetter(cpFloat, m, Mass);
+void cpBodySetMass(cpBody *body, cpFloat m);
+
+CP_DefineBodyGetter(cpFloat, i, Moment);
+void cpBodySetMoment(cpBody *body, cpFloat i);
+
+
+CP_DefineBodyProperty(cpVect, p, Pos);
+CP_DefineBodyProperty(cpVect, v, Vel);
+CP_DefineBodyProperty(cpVect, f, Force);
+CP_DefineBodyGetter(cpFloat, a, Angle);
+void cpBodySetAngle(cpBody *body, cpFloat a);
+CP_DefineBodyProperty(cpFloat, w, AngVel);
+CP_DefineBodyProperty(cpFloat, t, Torque);
+CP_DefineBodyGetter(cpVect, rot, Rot);
+CP_DefineBodyProperty(cpFloat, v_limit, VelLimit);
+CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit);
+
+//  Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep.
+// Intended for objects that are moved manually with a custom velocity integration function.
+void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);
+
+// Default Integration functions.
+void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
+void cpBodyUpdatePosition(cpBody *body, cpFloat dt);
+
+// Convert body local to world coordinates
+static inline cpVect
+cpBodyLocal2World(const cpBody *body, const cpVect v)
+{
+	return cpvadd(body->p, cpvrotate(v, body->rot));
+}
+
+// Convert world to body local coordinates
+static inline cpVect
+cpBodyWorld2Local(const cpBody *body, const cpVect v)
+{
+	return cpvunrotate(cpvsub(v, body->p), body->rot);
+}
+
+// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates).
+static inline void
+cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r)
+{
+	body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
+	body->w += body->i_inv*cpvcross(r, j);
+}
+
+// Zero the forces on a body.
+void cpBodyResetForces(cpBody *body);
+// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates).
+void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);
+
+static inline cpFloat
+cpBodyKineticEnergy(const cpBody *body)
+{
+	// Need to do some fudging to avoid NaNs
+	cpFloat vsq = cpvdot(body->v, body->v);
+	cpFloat wsq = body->w*body->w;
+	return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f);
+}

src/helper/chipmunk/cpCollision.c

@@ -0,0 +1,411 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <math.h>
+//#include <stdio.h>
+
+#include "chipmunk_private.h"
+
+typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *);
+
+// Add contact points for circle to circle collisions.
+// Used by several collision tests.
+static int
+circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
+{
+	cpFloat mindist = r1 + r2;
+	cpVect delta = cpvsub(p2, p1);
+	cpFloat distsq = cpvlengthsq(delta);
+	if(distsq >= mindist*mindist) return 0;
+	
+	cpFloat dist = cpfsqrt(distsq);
+
+	// Allocate and initialize the contact.
+	cpContactInit(
+		con,
+		cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
+		(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
+		dist - mindist,
+		0
+	);
+	
+	return 1;
+}
+
+// Collide circle shapes.
+static int
+circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpCircleShape *circ1 = (cpCircleShape *)shape1;
+	cpCircleShape *circ2 = (cpCircleShape *)shape2;
+	
+	return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr);
+}
+
+// Collide circles to segment shapes.
+static int
+circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con)
+{
+	cpCircleShape *circ = (cpCircleShape *)circleShape;
+	cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
+	
+	// Radius sum
+	cpFloat rsum = circ->r + seg->r;
+	
+	// Calculate normal distance from segment.
+	cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn);
+	cpFloat dist = cpfabs(dn) - rsum;
+	if(dist > 0.0f) return 0;
+	
+	// Calculate tangential distance along segment.
+	cpFloat dt = -cpvcross(seg->tn, circ->tc);
+	cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
+	cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
+	
+	// Decision tree to decide which feature of the segment to collide with.
+	if(dt < dtMin){
+		if(dt < (dtMin - rsum)){
+			return 0;
+		} else {
+			return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con);
+		}
+	} else {
+		if(dt < dtMax){
+			cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn);
+			cpContactInit(
+				con,
+				cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)),
+				n,
+				dist,
+				0				 
+			);
+			return 1;
+		} else {
+			if(dt < (dtMax + rsum)) {
+				return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con);
+			} else {
+				return 0;
+			}
+		}
+	}
+	
+	return 1;
+}
+
+// Helper function for working with contact buffers
+// This used to malloc/realloc memory on the fly but was repurposed.
+static cpContact *
+nextContactPoint(cpContact *arr, int *numPtr)
+{
+	int index = *numPtr;
+	
+	if(index < CP_MAX_CONTACTS_PER_ARBITER){
+		(*numPtr) = index + 1;
+		return &arr[index];
+	} else {
+		return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1];
+	}
+}
+
+// Find the minimum separating axis for the give poly and axis list.
+static inline int
+findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out)
+{
+	int min_index = 0;
+	cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d);
+	if(min > 0.0f) return -1;
+	
+	for(int i=1; i<num; i++){
+		cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d);
+		if(dist > 0.0f) {
+			return -1;
+		} else if(dist > min){
+			min = dist;
+			min_index = i;
+		}
+	}
+	
+	(*min_out) = min;
+	return min_index;
+}
+
+// Add contacts for probably penetrating vertexes.
+// This handles the degenerate case where an overlap was detected, but no vertexes fall inside
+// the opposing polygon. (like a star of david)
+static inline int
+findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
+{
+	int num = 0;
+	
+	for(int i=0; i<poly1->numVerts; i++){
+		cpVect v = poly1->tVerts[i];
+		if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i));
+	}
+	
+	for(int i=0; i<poly2->numVerts; i++){
+		cpVect v = poly2->tVerts[i];
+		if(cpPolyShapeContainsVertPartial(poly1, v, n))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
+	}
+	
+	return num;
+}
+
+// Add contacts for penetrating vertexes.
+static inline int
+findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
+{
+	int num = 0;
+	
+	for(int i=0; i<poly1->numVerts; i++){
+		cpVect v = poly1->tVerts[i];
+		if(cpPolyShapeContainsVert(poly2, v))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i));
+	}
+	
+	for(int i=0; i<poly2->numVerts; i++){
+		cpVect v = poly2->tVerts[i];
+		if(cpPolyShapeContainsVert(poly1, v))
+			cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
+	}
+	
+	return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
+}
+
+// Collide poly shapes together.
+static int
+poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpPolyShape *poly1 = (cpPolyShape *)shape1;
+	cpPolyShape *poly2 = (cpPolyShape *)shape2;
+	
+	cpFloat min1;
+	int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1);
+	if(mini1 == -1) return 0;
+	
+	cpFloat min2;
+	int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2);
+	if(mini2 == -1) return 0;
+	
+	// There is overlap, find the penetrating verts
+	if(min1 > min2)
+		return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1);
+	else
+		return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2);
+}
+
+// Like cpPolyValueOnAxis(), but for segments.
+static inline cpFloat
+segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d)
+{
+	cpFloat a = cpvdot(n, seg->ta) - seg->r;
+	cpFloat b = cpvdot(n, seg->tb) - seg->r;
+	return cpfmin(a, b) - d;
+}
+
+// Identify vertexes that have penetrated the segment.
+static inline void
+findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef) 
+{
+	cpFloat dta = cpvcross(seg->tn, seg->ta);
+	cpFloat dtb = cpvcross(seg->tn, seg->tb);
+	cpVect n = cpvmult(seg->tn, coef);
+	
+	for(int i=0; i<poly->numVerts; i++){
+		cpVect v = poly->tVerts[i];
+		if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){
+			cpFloat dt = cpvcross(seg->tn, v);
+			if(dta >= dt && dt >= dtb){
+				cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i));
+			}
+		}
+	}
+}
+
+// This one is complicated and gross. Just don't go there...
+// TODO: Comment me!
+static int
+seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
+{
+	cpSegmentShape *seg = (cpSegmentShape *)shape1;
+	cpPolyShape *poly = (cpPolyShape *)shape2;
+	cpPolyShapeAxis *axes = poly->tAxes;
+	
+	cpFloat segD = cpvdot(seg->tn, seg->ta);
+	cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r;
+	cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r;
+	if(minNeg > 0.0f || minNorm > 0.0f) return 0;
+	
+	int mini = 0;
+	cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d);
+	if(poly_min > 0.0f) return 0;
+	for(int i=0; i<poly->numVerts; i++){
+		cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d);
+		if(dist > 0.0f){
+			return 0;
+		} else if(dist > poly_min){
+			poly_min = dist;
+			mini = i;
+		}
+	}
+	
+	int num = 0;
+	
+	cpVect poly_n = cpvneg(axes[mini].n);
+	
+	cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r));
+	cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r));
+	if(cpPolyShapeContainsVert(poly, va))
+		cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0));
+	if(cpPolyShapeContainsVert(poly, vb))
+		cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1));
+
+	// Floating point precision problems here.
+	// This will have to do for now.
+	poly_min -= cp_collision_slop;
+	if(minNorm >= poly_min || minNeg >= poly_min) {
+		if(minNorm > minNeg)
+			findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f);
+		else
+			findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f);
+	}
+	
+	// If no other collision points are found, try colliding endpoints.
+	if(num == 0){
+		cpVect poly_a = poly->tVerts[mini];
+		cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts];
+		
+		if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr))
+			return 1;
+			
+		if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr))
+			return 1;
+	}
+
+	return num;
+}
+
+// This one is less gross, but still gross.
+// TODO: Comment me!
+static int
+circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
+{
+	cpCircleShape *circ = (cpCircleShape *)shape1;
+	cpPolyShape *poly = (cpPolyShape *)shape2;
+	cpPolyShapeAxis *axes = poly->tAxes;
+	
+	int mini = 0;
+	cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r;
+	for(int i=0; i<poly->numVerts; i++){
+		cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r;
+		if(dist > 0.0f){
+			return 0;
+		} else if(dist > min) {
+			min = dist;
+			mini = i;
+		}
+	}
+	
+	cpVect n = axes[mini].n;
+	cpVect a = poly->tVerts[mini];
+	cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
+	cpFloat dta = cpvcross(n, a);
+	cpFloat dtb = cpvcross(n, b);
+	cpFloat dt = cpvcross(n, circ->tc);
+		
+	if(dt < dtb){
+		return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
+	} else if(dt < dta) {
+		cpContactInit(
+			con,
+			cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
+			cpvneg(n),
+			min,
+			0				 
+		);
+	
+		return 1;
+	} else {
+		return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
+	}
+}
+
+//static const collisionFunc builtinCollisionFuncs[9] = {
+//	circle2circle,
+//	NULL,
+//	NULL,
+//	circle2segment,
+//	NULL,
+//	NULL,
+//	circle2poly,
+//	seg2poly,
+//	poly2poly,
+//};
+//static const collisionFunc *colfuncs = builtinCollisionFuncs;
+
+static collisionFunc *colfuncs = NULL;
+
+static void
+addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func)
+{
+	colfuncs[a + b*CP_NUM_SHAPES] = func;
+}
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void cpInitCollisionFuncs(void);
+	
+	// Initializes the array of collision functions.
+	// Called by cpInitChipmunk().
+	void
+	cpInitCollisionFuncs(void)
+	{
+		if(!colfuncs)
+			colfuncs = (collisionFunc *)cpcalloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc));
+		
+		addColFunc(CP_CIRCLE_SHAPE,  CP_CIRCLE_SHAPE,  circle2circle);
+		addColFunc(CP_CIRCLE_SHAPE,  CP_SEGMENT_SHAPE, circle2segment);
+		addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE,    seg2poly);
+		addColFunc(CP_CIRCLE_SHAPE,  CP_POLY_SHAPE,    circle2poly);
+		addColFunc(CP_POLY_SHAPE,    CP_POLY_SHAPE,    poly2poly);
+	}	
+#ifdef __cplusplus
+}
+#endif
+
+int
+cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
+{
+	// Their shape types must be in order.
+	cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted.");
+	
+	collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES];
+	return (cfunc) ? cfunc(a, b, arr) : 0;
+}

src/helper/chipmunk/cpHashSet.c

@@ -0,0 +1,265 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <assert.h>
+
+#include "chipmunk_private.h"
+#include "prime.h"
+
+typedef struct cpHashSetBin {
+	void *elt;
+	cpHashValue hash;
+	struct cpHashSetBin *next;
+} cpHashSetBin;
+
+struct cpHashSet {
+	int entries, size;
+	
+	cpHashSetEqlFunc eql;
+	void *default_value;
+	
+	cpHashSetBin **table;
+	cpHashSetBin *pooledBins;
+	
+	cpArray *allocatedBuffers;
+};
+
+void
+cpHashSetDestroy(cpHashSet *set)
+{
+	cpfree(set->table);
+	
+	cpArrayFreeEach(set->allocatedBuffers, cpfree);
+	cpArrayFree(set->allocatedBuffers);
+}
+
+void
+cpHashSetFree(cpHashSet *set)
+{
+	if(set){
+		cpHashSetDestroy(set);
+		cpfree(set);
+	}
+}
+
+cpHashSet *
+cpHashSetAlloc(void)
+{
+	return (cpHashSet *)cpcalloc(1, sizeof(cpHashSet));
+}
+
+cpHashSet *
+cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *default_value)
+{
+	set->size = next_prime(size);
+	set->entries = 0;
+	
+	set->eql = eqlFunc;
+	set->default_value = default_value;
+	
+	set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *));
+	set->pooledBins = NULL;
+	
+	set->allocatedBuffers = cpArrayNew(0);
+	
+	return set;
+}
+
+cpHashSet *
+cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *default_value)
+{
+	return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, default_value);
+}
+
+static int
+setIsFull(cpHashSet *set)
+{
+	return (set->entries >= set->size);
+}
+
+static void
+cpHashSetResize(cpHashSet *set)
+{
+	// Get the next approximate doubled prime.
+	int newSize = next_prime(set->size + 1);
+	// Allocate a new table.
+	cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *));
+	
+	// Iterate over the chains.
+	for(int i=0; i<set->size; i++){
+		// Rehash the bins into the new table.
+		cpHashSetBin *bin = set->table[i];
+		while(bin){
+			cpHashSetBin *next = bin->next;
+			
+			int idx = bin->hash%newSize;
+			bin->next = newTable[idx];
+			newTable[idx] = bin;
+			
+			bin = next;
+		}
+	}
+	
+	cpfree(set->table);
+	
+	set->table = newTable;
+	set->size = newSize;
+}
+
+static inline void
+recycleBin(cpHashSet *set, cpHashSetBin *bin)
+{
+	bin->next = set->pooledBins;
+	set->pooledBins = bin;
+	bin->elt = NULL;
+}
+
+static cpHashSetBin *
+getUnusedBin(cpHashSet *set)
+{
+	cpHashSetBin *bin = set->pooledBins;
+	
+	if(bin){
+		set->pooledBins = bin->next;
+		return bin;
+	} else {
+		// Pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
+		cpAssert(count, "Buffer size is too small.");
+		
+		cpHashSetBin *buffer = (cpHashSetBin *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(set->allocatedBuffers, buffer);
+		
+		// push all but the first one, return it instead
+		for(int i=1; i<count; i++) recycleBin(set, buffer + i);
+		return buffer;
+	}
+}
+
+int
+cpHashSetCount(cpHashSet *set)
+{
+	return set->entries;
+}
+
+void *
+cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans)
+{
+	int idx = hash%set->size;
+	
+	// Find the bin with the matching element.
+	cpHashSetBin *bin = set->table[idx];
+	while(bin && !set->eql(ptr, bin->elt))
+		bin = bin->next;
+	
+	// Create it if necessary.
+	if(!bin){
+		bin = getUnusedBin(set);
+		bin->hash = hash;
+		bin->elt = (trans ? trans(ptr, data) : data);
+		
+		bin->next = set->table[idx];
+		set->table[idx] = bin;
+		
+		set->entries++;
+		if(setIsFull(set)) cpHashSetResize(set);
+	}
+	
+	return bin->elt;
+}
+
+void *
+cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr)
+{
+	int idx = hash%set->size;
+	
+	cpHashSetBin **prev_ptr = &set->table[idx];
+	cpHashSetBin *bin = set->table[idx];
+	
+	// Find the bin
+	while(bin && !set->eql(ptr, bin->elt)){
+		prev_ptr = &bin->next;
+		bin = bin->next;
+	}
+	
+	// Remove it if it exists.
+	if(bin){
+		// Update the previous linked list pointer
+		(*prev_ptr) = bin->next;
+		set->entries--;
+		
+		void *elt = bin->elt;
+		recycleBin(set, bin);
+		
+		return elt;
+	}
+	
+	return NULL;
+}
+
+void *
+cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr)
+{	
+	int idx = hash%set->size;
+	cpHashSetBin *bin = set->table[idx];
+	while(bin && !set->eql(ptr, bin->elt))
+		bin = bin->next;
+		
+	return (bin ? bin->elt : set->default_value);
+}
+
+void
+cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data)
+{
+	for(int i=0; i<set->size; i++){
+		cpHashSetBin *bin = set->table[i];
+		while(bin){
+			cpHashSetBin *next = bin->next;
+			func(bin->elt, data);
+			bin = next;
+		}
+	}
+}
+
+void
+cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data)
+{
+	for(int i=0; i<set->size; i++){
+		// The rest works similarly to cpHashSetRemove() above.
+		cpHashSetBin **prev_ptr = &set->table[i];
+		cpHashSetBin *bin = set->table[i];
+		while(bin){
+			cpHashSetBin *next = bin->next;
+			
+			if(func(bin->elt, data)){
+				prev_ptr = &bin->next;
+			} else {
+				(*prev_ptr) = next;
+
+				set->entries--;
+				recycleBin(set, bin);
+			}
+			
+			bin = next;
+		}
+	}
+}

src/helper/chipmunk/cpPolyShape.c

@@ -0,0 +1,235 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "chipmunk_unsafe.h"
+
+cpPolyShape *
+cpPolyShapeAlloc(void)
+{
+	return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
+}
+
+static cpBB
+cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
+{
+	cpVect *src = poly->verts;
+	cpVect *dst = poly->tVerts;
+	
+	cpFloat l = INFINITY, r = -INFINITY;
+	cpFloat b = INFINITY, t = -INFINITY;
+	
+	for(int i=0; i<poly->numVerts; i++){
+		cpVect v = cpvadd(p, cpvrotate(src[i], rot));
+		
+		dst[i] = v;
+		l = cpfmin(l, v.x);
+		r = cpfmax(r, v.x);
+		b = cpfmin(b, v.y);
+		t = cpfmax(t, v.y);
+	}
+	
+	return cpBBNew(l, b, r, t);
+}
+
+static void
+cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot)
+{
+	cpPolyShapeAxis *src = poly->axes;
+	cpPolyShapeAxis *dst = poly->tAxes;
+	
+	for(int i=0; i<poly->numVerts; i++){
+		cpVect n = cpvrotate(src[i].n, rot);
+		dst[i].n = n;
+		dst[i].d = cpvdot(p, n) + src[i].d;
+	}
+}
+
+static cpBB
+cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpPolyShape *poly = (cpPolyShape *)shape;
+	
+	cpPolyShapeTransformAxes(poly, p, rot);
+	cpBB bb = shape->bb = cpPolyShapeTransformVerts(poly, p, rot);
+	
+	return bb;
+}
+
+static void
+cpPolyShapeDestroy(cpShape *shape)
+{
+	cpPolyShape *poly = (cpPolyShape *)shape;
+	
+	cpfree(poly->verts);
+	cpfree(poly->tVerts);
+	
+	cpfree(poly->axes);
+	cpfree(poly->tAxes);
+}
+
+static cpBool
+cpPolyShapePointQuery(cpShape *shape, cpVect p){
+	return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
+}
+
+static void
+cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	cpPolyShape *poly = (cpPolyShape *)shape;
+	cpPolyShapeAxis *axes = poly->tAxes;
+	cpVect *verts = poly->tVerts;
+	int numVerts = poly->numVerts;
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect n = axes[i].n;
+		cpFloat an = cpvdot(a, n);
+		if(axes[i].d > an) continue;
+		
+		cpFloat bn = cpvdot(b, n);
+		cpFloat t = (axes[i].d - an)/(bn - an);
+		if(t < 0.0f || 1.0f < t) continue;
+		
+		cpVect point = cpvlerp(a, b, t);
+		cpFloat dt = -cpvcross(n, point);
+		cpFloat dtMin = -cpvcross(n, verts[i]);
+		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
+		
+		if(dtMin <= dt && dt <= dtMax){
+			info->shape = shape;
+			info->t = t;
+			info->n = n;
+		}
+	}
+}
+
+static const cpShapeClass polyClass = {
+	CP_POLY_SHAPE,
+	cpPolyShapeCacheData,
+	cpPolyShapeDestroy,
+	cpPolyShapePointQuery,
+	cpPolyShapeSegmentQuery,
+};
+
+cpBool
+cpPolyValidate(const cpVect *verts, const int numVerts)
+{
+	for(int i=0; i<numVerts; i++){
+		cpVect a = verts[i];
+		cpVect b = verts[(i+1)%numVerts];
+		cpVect c = verts[(i+2)%numVerts];
+		
+		if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
+			return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+int
+cpPolyShapeGetNumVerts(cpShape *shape)
+{
+	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
+	return ((cpPolyShape *)shape)->numVerts;
+}
+
+cpVect
+cpPolyShapeGetVert(cpShape *shape, int idx)
+{
+	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
+	cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
+	
+	return ((cpPolyShape *)shape)->verts[idx];
+}
+
+
+static void
+setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
+{
+	poly->numVerts = numVerts;
+
+	poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
+	poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
+	poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
+	poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
+	
+	for(int i=0; i<numVerts; i++){
+		cpVect a = cpvadd(offset, verts[i]);
+		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
+		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
+
+		poly->verts[i] = a;
+		poly->axes[i].n = n;
+		poly->axes[i].d = cpvdot(n, a);
+	}
+}
+
+cpPolyShape *
+cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
+{
+	// Fail if the user attempts to pass a concave poly, or a bad winding.
+	cpAssert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");
+	
+	setUpVerts(poly, numVerts, verts, offset);
+	cpShapeInit((cpShape *)poly, &polyClass, body);
+
+	return poly;
+}
+
+cpShape *
+cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset)
+{
+	return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset);
+}
+
+cpPolyShape *
+cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height)
+{
+	cpFloat hw = width/2.0f;
+	cpFloat hh = height/2.0f;
+	
+	cpVect verts[] = {
+		cpv(-hw,-hh),
+		cpv(-hw, hh),
+		cpv( hw, hh),
+		cpv( hw,-hh),
+	};
+	
+	return cpPolyShapeInit(poly, body, 4, verts, cpvzero);
+}
+
+cpShape *
+cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height)
+{
+	return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height);
+}
+
+// Unsafe API (chipmunk_unsafe.h)
+
+void
+cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
+{
+	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
+	cpPolyShapeDestroy(shape);
+	setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
+}

src/helper/chipmunk/cpPolyShape.h

@@ -0,0 +1,56 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+// Axis structure used by cpPolyShape.
+typedef struct cpPolyShapeAxis {
+	// normal
+	cpVect n;
+	// distance from origin
+	cpFloat d;
+} cpPolyShapeAxis;
+
+// Convex polygon shape structure.
+typedef struct cpPolyShape {
+	CP_PRIVATE(cpShape shape);
+	
+	// Vertex and axis lists.
+	CP_PRIVATE(int numVerts);
+	CP_PRIVATE(cpVect *verts);
+	CP_PRIVATE(cpPolyShapeAxis *axes);
+
+	// Transformed vertex and axis lists.
+	CP_PRIVATE(cpVect *tVerts);
+	CP_PRIVATE(cpPolyShapeAxis *tAxes);
+} cpPolyShape;
+
+// Basic allocation functions.
+cpPolyShape *cpPolyShapeAlloc(void);
+cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset);
+cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset);
+
+cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height);
+cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
+
+// Check that a set of vertexes has a correct winding and that they are convex
+cpBool cpPolyValidate(const cpVect *verts, const int numVerts);
+
+int cpPolyShapeGetNumVerts(cpShape *shape);
+cpVect cpPolyShapeGetVert(cpShape *shape, int idx);

src/helper/chipmunk/cpShape.c

@@ -0,0 +1,393 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+#include "chipmunk_unsafe.h"
+
+#define CP_DefineShapeGetter(struct, type, member, name) \
+CP_DeclareShapeGetter(struct, type, name){ \
+	cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \
+	return ((struct *)shape)->member; \
+}
+cpHashValue SHAPE_ID_COUNTER = 0;
+
+void
+cpResetShapeIdCounter(void)
+{
+	SHAPE_ID_COUNTER = 0;
+}
+
+
+cpShape*
+cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
+{
+	shape->klass = klass;
+	
+	shape->hashid = SHAPE_ID_COUNTER;
+	SHAPE_ID_COUNTER++;
+	
+	shape->body = body;
+	shape->sensor = 0;
+	
+	shape->e = 0.0f;
+	shape->u = 0.0f;
+	shape->surface_v = cpvzero;
+	
+	shape->collision_type = 0;
+	shape->group = CP_NO_GROUP;
+	shape->layers = CP_ALL_LAYERS;
+	
+	shape->data = NULL;
+	shape->next = NULL;
+	
+//	cpShapeCacheBB(shape);
+	
+	return shape;
+}
+
+void
+cpShapeDestroy(cpShape *shape)
+{
+	if(shape->klass->destroy) shape->klass->destroy(shape);
+}
+
+void
+cpShapeFree(cpShape *shape)
+{
+	if(shape){
+		cpShapeDestroy(shape);
+		cpfree(shape);
+	}
+}
+
+cpBB
+cpShapeCacheBB(cpShape *shape)
+{
+	cpBody *body = shape->body;
+	return cpShapeUpdate(shape, body->p, body->rot);
+}
+
+cpBB
+cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot)
+{
+	return (shape->bb = shape->klass->cacheData(shape, pos, rot));
+}
+
+cpBool
+cpShapePointQuery(cpShape *shape, cpVect p){
+	return shape->klass->pointQuery(shape, p);
+}
+
+cpBool
+cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
+	cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero};
+	(*info) = blank;
+	
+	shape->klass->segmentQuery(shape, a, b, info);
+	return (info->shape != NULL);
+}
+
+cpCircleShape *
+cpCircleShapeAlloc(void)
+{
+	return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
+}
+
+static inline cpBB
+bbFromCircle(const cpVect c, const cpFloat r)
+{
+	return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
+}
+
+static cpBB
+cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	
+	circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
+	return bbFromCircle(circle->tc, circle->r);
+}
+
+static cpBool
+cpCircleShapePointQuery(cpShape *shape, cpVect p){
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	return cpvnear(circle->tc, p, circle->r);
+}
+
+static void
+circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// offset the line to be relative to the circle
+	a = cpvsub(a, center);
+	b = cpvsub(b, center);
+	
+	cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
+	cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
+	cpFloat qc = cpvdot(a, a) - r*r;
+	
+	cpFloat det = qb*qb - 4.0f*qa*qc;
+	
+	if(det >= 0.0f){
+		cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
+		if(0.0f<= t && t <= 1.0f){
+			info->shape = shape;
+			info->t = t;
+			info->n = cpvnormalize(cpvlerp(a, b, t));
+		}
+	}
+}
+
+static void
+cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	circleSegmentQuery(shape, circle->tc, circle->r, a, b, info);
+}
+
+static const cpShapeClass cpCircleShapeClass = {
+	CP_CIRCLE_SHAPE,
+	cpCircleShapeCacheData,
+	NULL,
+	cpCircleShapePointQuery,
+	cpCircleShapeSegmentQuery,
+};
+
+cpCircleShape *
+cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset)
+{
+	circle->c = offset;
+	circle->r = radius;
+	
+	cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body);
+	
+	return circle;
+}
+
+cpShape *
+cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset)
+{
+	return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset);
+}
+
+CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset)
+CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius)
+
+cpSegmentShape *
+cpSegmentShapeAlloc(void)
+{
+	return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape));
+}
+
+static cpBB
+cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->ta = cpvadd(p, cpvrotate(seg->a, rot));
+	seg->tb = cpvadd(p, cpvrotate(seg->b, rot));
+	seg->tn = cpvrotate(seg->n, rot);
+	
+	cpFloat l,r,s,t;
+	
+	if(seg->ta.x < seg->tb.x){
+		l = seg->ta.x;
+		r = seg->tb.x;
+	} else {
+		l = seg->tb.x;
+		r = seg->ta.x;
+	}
+	
+	if(seg->ta.y < seg->tb.y){
+		s = seg->ta.y;
+		t = seg->tb.y;
+	} else {
+		s = seg->tb.y;
+		t = seg->ta.y;
+	}
+	
+	cpFloat rad = seg->r;
+	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
+}
+
+static cpBool
+cpSegmentShapePointQuery(cpShape *shape, cpVect p){
+	if(!cpBBcontainsVect(shape->bb, p)) return cpFalse;
+	
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	// Calculate normal distance from segment.
+	cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
+	cpFloat dist = cpfabs(dn) - seg->r;
+	if(dist > 0.0f) return cpFalse;
+	
+	// Calculate tangential distance along segment.
+	cpFloat dt = -cpvcross(seg->tn, p);
+	cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
+	cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
+	
+	// Decision tree to decide which feature of the segment to collide with.
+	if(dt <= dtMin){
+		if(dt < (dtMin - seg->r)){
+			return cpFalse;
+		} else {
+			return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
+		}
+	} else {
+		if(dt < dtMax){
+			return cpTrue;
+		} else {
+			if(dt < (dtMax + seg->r)) {
+				return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
+			} else {
+				return cpFalse;
+			}
+		}
+	}
+	
+	return cpTrue;	
+}
+
+static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
+
+static void
+cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// TODO this function could be optimized better.
+	
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	cpVect n = seg->tn;
+	// flip n if a is behind the axis
+	if(cpvdot(a, n) < cpvdot(seg->ta, n))
+		n = cpvneg(n);
+	
+	cpFloat an = cpvdot(a, n);
+	cpFloat bn = cpvdot(b, n);
+	
+	if(an != bn){
+		cpFloat d = cpvdot(seg->ta, n) + seg->r;
+		cpFloat t = (d - an)/(bn - an);
+		
+		if(0.0f < t && t < 1.0f){
+			cpVect point = cpvlerp(a, b, t);
+			cpFloat dt = -cpvcross(seg->tn, point);
+			cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
+			cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
+			
+			if(dtMin < dt && dt < dtMax){
+				info->shape = shape;
+				info->t = t;
+				info->n = n;
+				
+				return; // don't continue on and check endcaps
+			}
+		}
+	}
+	
+	if(seg->r) {
+		cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero};
+		cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero};
+		circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
+		circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);
+		
+		if(info1.t < info2.t){
+			(*info) = info1;
+		} else {
+			(*info) = info2;
+		}
+	}
+}
+
+static const cpShapeClass cpSegmentShapeClass = {
+	CP_SEGMENT_SHAPE,
+	cpSegmentShapeCacheData,
+	NULL,
+	cpSegmentShapePointQuery,
+	cpSegmentShapeSegmentQuery,
+};
+
+cpSegmentShape *
+cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r)
+{
+	seg->a = a;
+	seg->b = b;
+	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
+	
+	seg->r = r;
+	
+	cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body);
+	
+	return seg;
+}
+
+cpShape*
+cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r)
+{
+	return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r);
+}
+
+CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A)
+CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B)
+CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal)
+CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius)
+
+// Unsafe API (chipmunk_unsafe.h)
+
+void
+cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	
+	circle->r = radius;
+}
+
+void
+cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
+{
+	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = (cpCircleShape *)shape;
+	
+	circle->c = offset;
+}
+
+void
+cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
+{
+	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->a = a;
+	seg->b = b;
+	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
+}
+
+void
+cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->r = radius;
+}

src/helper/chipmunk/cpShape.h

@@ -0,0 +1,174 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+ 
+// Forward declarations required for defining other structs.
+typedef struct cpShapeClass cpShapeClass;
+
+typedef struct cpSegmentQueryInfo {
+	cpShape *shape; // shape that was hit, NULL if no collision
+	cpFloat t; // Distance along query segment, will always be in the range [0, 1].
+	cpVect n; // normal of hit surface
+} cpSegmentQueryInfo;
+
+// Enumeration of shape types.
+typedef enum cpShapeType{
+	CP_CIRCLE_SHAPE,
+	CP_SEGMENT_SHAPE,
+	CP_POLY_SHAPE,
+	CP_NUM_SHAPES
+} cpShapeType;
+
+// Shape class. Holds function pointers and type data.
+struct cpShapeClass {
+	cpShapeType type;
+	
+	// Called by cpShapeCacheBB().
+	cpBB (*cacheData)(cpShape *shape, cpVect p, cpVect rot);
+	// Called to by cpShapeDestroy().
+	void (*destroy)(cpShape *shape);
+	
+	// called by cpShapePointQuery().
+	cpBool (*pointQuery)(cpShape *shape, cpVect p);
+	
+	// called by cpShapeSegmentQuery()
+	 void (*segmentQuery)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
+};
+
+// Basic shape struct that the others inherit from.
+struct cpShape {
+	// The "class" of a shape as defined above 
+	CP_PRIVATE(const cpShapeClass *klass);
+	
+	// cpBody that the shape is attached to.
+	cpBody *body;
+
+	// Cached BBox for the shape.
+	cpBB bb;
+	
+	// Sensors invoke callbacks, but do not generate collisions
+	cpBool sensor;
+	
+	// *** Surface properties.
+	
+	// Coefficient of restitution. (elasticity)
+	cpFloat e;
+	// Coefficient of friction.
+	cpFloat u;
+	// Surface velocity used when solving for friction.
+	cpVect surface_v;
+
+	// *** User Definable Fields
+
+	// User defined data pointer for the shape.
+	cpDataPointer data;
+	
+	// User defined collision type for the shape.
+	cpCollisionType collision_type;
+	// User defined collision group for the shape.
+	cpGroup group;
+	// User defined layer bitmask for the shape.
+	cpLayers layers;
+	
+	// *** Internally Used Fields
+	
+	// Shapes form a linked list when added to space on a non-NULL body
+	CP_PRIVATE(cpShape *next);
+	
+	// Unique id used as the hash value.
+	CP_PRIVATE(cpHashValue hashid);
+};
+
+// Low level shape initialization func.
+cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body);
+
+// Basic destructor functions. (allocation functions are not shared)
+void cpShapeDestroy(cpShape *shape);
+void cpShapeFree(cpShape *shape);
+
+// Cache the BBox of the shape.
+cpBB cpShapeCacheBB(cpShape *shape);
+cpBB cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot);
+
+// Test if a point lies within a shape.
+cpBool cpShapePointQuery(cpShape *shape, cpVect p);
+
+#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape)
+
+// Circle shape structure.
+typedef struct cpCircleShape {
+	CP_PRIVATE(cpShape shape);
+	
+	// Center in body space coordinates
+	CP_PRIVATE(cpVect c);
+	// Radius.
+	CP_PRIVATE(cpFloat r);
+	
+	// Transformed center. (world space coordinates)
+	CP_PRIVATE(cpVect tc);
+} cpCircleShape;
+
+// Basic allocation functions for cpCircleShape.
+cpCircleShape *cpCircleShapeAlloc(void);
+cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset);
+cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset);
+
+CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset);
+CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius);
+
+// Segment shape structure.
+typedef struct cpSegmentShape {
+	CP_PRIVATE(cpShape shape);
+	
+	// Endpoints and normal of the segment. (body space coordinates)
+	cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n);
+	// Radius of the segment. (Thickness)
+	cpFloat CP_PRIVATE(r);
+
+	// Transformed endpoints and normal. (world space coordinates)
+	cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn);
+} cpSegmentShape;
+
+// Basic allocation functions for cpSegmentShape.
+cpSegmentShape* cpSegmentShapeAlloc(void);
+cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius);
+cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius);
+
+CP_DeclareShapeGetter(cpSegmentShape, cpVect, A);
+CP_DeclareShapeGetter(cpSegmentShape, cpVect, B);
+CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal);
+CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius);
+
+// For determinism, you can reset the shape id counter.
+void cpResetShapeIdCounter(void);
+
+cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
+
+static inline cpVect
+cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvlerp(start, end, info.t);
+}
+
+static inline cpFloat
+cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvdist(start, end)*info.t;
+}

src/helper/chipmunk/cpSpace.c

@@ -0,0 +1,522 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+
+cpTimestamp cp_contact_persistence = 3;
+
+#pragma mark Contact Set Helpers
+
+// Equal function for contactSet.
+static cpBool
+contactSetEql(cpShape **shapes, cpArbiter *arb)
+{
+	cpShape *a = shapes[0];
+	cpShape *b = shapes[1];
+	
+	return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
+}
+
+#pragma mark Collision Pair Function Helpers
+
+// Equals function for collFuncSet.
+static cpBool
+collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
+{
+	return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b));
+}
+
+// Transformation function for collFuncSet.
+static void *
+collFuncSetTrans(cpCollisionHandler *handler, void *unused)
+{
+	cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
+	(*copy) = (*handler);
+	
+	return copy;
+}
+
+#pragma mark Misc Helper Funcs
+
+// Default collision functions.
+static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
+static void nothing(cpArbiter *arb, cpSpace *space, void *data){}
+
+// BBfunc callback for the spatial hash.
+static cpBB shapeBBFunc(cpShape *shape){return shape->bb;}
+static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;}
+
+// Iterator functions for destructors.
+static void             freeWrap(void         *ptr, void *unused){          cpfree(ptr);}
+static void        shapeFreeWrap(cpShape      *ptr, void *unused){     cpShapeFree(ptr);}
+
+void
+cpSpaceLock(cpSpace *space)
+{
+	space->locked++;
+}
+
+void
+cpSpaceUnlock(cpSpace *space)
+{
+	space->locked--;
+	cpAssert(space->locked >= 0, "Internal error:Space lock underflow.");
+	
+	if(!space->locked){
+		cpArray *waking = space->rousedBodies;
+		for(int i=0, count=waking->num; i<count; i++){
+			cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
+		}
+		
+		waking->num = 0;
+	}
+}
+
+
+#pragma mark Memory Management Functions
+
+cpSpace *
+cpSpaceAlloc(void)
+{
+	return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
+}
+
+#define DEFAULT_DIM_SIZE 100.0f
+#define DEFAULT_COUNT 1000
+#define DEFAULT_ITERATIONS 10
+#define DEFAULT_ELASTIC_ITERATIONS 0
+
+cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};
+
+cpSpace*
+cpSpaceInit(cpSpace *space)
+{
+	space->iterations = DEFAULT_ITERATIONS;
+	
+	space->gravity = cpvzero;
+	space->damping = 1.0f;
+	
+	space->locked = 0;
+	space->stamp = 0;
+
+	if(0){
+		space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, NULL);
+		space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes);
+	} else {
+		space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, NULL);
+		space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes);
+		cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc);
+	}
+	
+	space->allocatedBuffers = cpArrayNew(0);
+	
+	space->bodies = cpArrayNew(0);
+	space->sleepingComponents = cpArrayNew(0);
+	space->rousedBodies = cpArrayNew(0);
+	
+	space->sleepTimeThreshold = INFINITY;
+	space->idleSpeedThreshold = 0.0f;
+	
+	space->arbiters = cpArrayNew(0);
+	space->pooledArbiters = cpArrayNew(0);
+	
+	space->contactBuffersHead = NULL;
+	space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, NULL);
+	
+	space->constraints = cpArrayNew(0);
+	
+	space->defaultHandler = defaultHandler;
+	space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, &space->defaultHandler);
+	
+	space->postStepCallbacks = NULL;
+	
+	cpBodyInitStatic(&space->_staticBody);
+	space->staticBody = &space->_staticBody;
+	
+	return space;
+}
+
+cpSpace*
+cpSpaceNew(void)
+{
+	return cpSpaceInit(cpSpaceAlloc());
+}
+
+void
+cpSpaceDestroy(cpSpace *space)
+{
+	cpSpatialIndexFree(space->staticShapes);
+	cpSpatialIndexFree(space->activeShapes);
+	
+	cpArrayFree(space->bodies);
+	cpArrayFree(space->sleepingComponents);
+	cpArrayFree(space->rousedBodies);
+	
+	cpArrayFree(space->constraints);
+	
+	cpHashSetFree(space->contactSet);
+	
+	cpArrayFree(space->arbiters);
+	cpArrayFree(space->pooledArbiters);
+	
+	if(space->allocatedBuffers){
+		cpArrayFreeEach(space->allocatedBuffers, cpfree);
+		cpArrayFree(space->allocatedBuffers);
+	}
+	
+	if(space->postStepCallbacks){
+		cpHashSetEach(space->postStepCallbacks, freeWrap, NULL);
+		cpHashSetFree(space->postStepCallbacks);
+	}
+	
+	if(space->collFuncSet){
+		cpHashSetEach(space->collFuncSet, freeWrap, NULL);
+		cpHashSetFree(space->collFuncSet);
+	}
+}
+
+void
+cpSpaceFree(cpSpace *space)
+{
+	if(space){
+		cpSpaceDestroy(space);
+		cpfree(space);
+	}
+}
+
+void
+cpSpaceFreeChildren(cpSpace *space)
+{
+	cpArray *components = space->sleepingComponents;
+	while(components->num) cpBodyActivate((cpBody *)components->arr[0]);
+	
+	cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL);
+	cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL);
+	
+	cpArrayFreeEach(space->bodies, (void (*)(void*))cpBodyFree);
+	cpArrayFreeEach(space->constraints, (void (*)(void*))cpConstraintFree);
+}
+
+#pragma mark Collision Handler Function Management
+
+void
+cpSpaceAddCollisionHandler(
+	cpSpace *space,
+	cpCollisionType a, cpCollisionType b,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+){
+	// Remove any old function so the new one will get added.
+	cpSpaceRemoveCollisionHandler(space, a, b);
+	
+	cpCollisionHandler handler = {
+		a, b,
+		begin ? begin : alwaysCollide,
+		preSolve ? preSolve : alwaysCollide,
+		postSolve ? postSolve : nothing,
+		separate ? separate : nothing,
+		data
+	};
+	
+	cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)collFuncSetTrans);
+}
+
+void
+cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
+{
+	struct { cpCollisionType a, b; } ids = {a, b};
+	cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids);
+	cpfree(old_handler);
+}
+
+void
+cpSpaceSetDefaultCollisionHandler(
+	cpSpace *space,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+){
+	cpCollisionHandler handler = {
+		0, 0,
+		begin ? begin : alwaysCollide,
+		preSolve ? preSolve : alwaysCollide,
+		postSolve ? postSolve : nothing,
+		separate ? separate : nothing,
+		data
+	};
+	
+	space->defaultHandler = handler;
+}
+
+#pragma mark Body, Shape, and Joint Management
+
+#define cpAssertSpaceUnlocked(space) \
+	cpAssert(!space->locked, \
+		"This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \
+		"Put these calls into a post-step callback." \
+	);
+
+static void
+cpBodyRemoveShape(cpBody *body, cpShape *shape)
+{
+	cpShape **prev_ptr = &body->shapeList;
+	cpShape *node = body->shapeList;
+	
+	while(node && node != shape){
+		prev_ptr = &node->next;
+		node = node->next;
+	}
+	
+	cpAssert(node, "Attempted to remove a shape from a body it was never attached to.");
+	(*prev_ptr) = node->next;
+}
+
+cpShape *
+cpSpaceAddShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *body = shape->body;
+	if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
+	
+	cpAssert(!cpSpatialIndexContains(space->activeShapes, shape, shape->hashid),
+		"Cannot add the same shape more than once.");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(body);
+	
+	// Push onto the head of the body's shape list
+	shape->next = body->shapeList; body->shapeList = shape;
+	
+	cpShapeUpdate(shape, body->p, body->rot);
+	cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
+		
+	return shape;
+}
+
+cpShape *
+cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
+{
+	cpAssert(!cpSpatialIndexContains(space->staticShapes, shape, shape->hashid),
+		"Cannot add the same static shape more than once.");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
+	cpSpaceActivateShapesTouchingShape(space, shape);
+	cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
+	
+	return shape;
+}
+
+cpBody *
+cpSpaceAddBody(cpSpace *space, cpBody *body)
+{
+	cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
+	cpAssert(!body->space, "Cannot add a body to a more than one space or to the same space twice.");
+//	cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
+	
+	cpArrayPush(space->bodies, body);
+	body->space = space;
+	
+	return body;
+}
+
+static void
+cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint)
+{
+	cpConstraint **prev_ptr = &body->constraintList;
+	cpConstraint *node = body->constraintList;
+	
+	while(node && node != constraint){
+		prev_ptr = (node->a == body ? &node->nextA : &node->nextB);
+		node = *prev_ptr;
+	}
+	
+	cpAssert(node, "Attempted to remove a constraint from a body it was never attached to.");
+	(*prev_ptr) = (node->a == body ? node->nextA : node->nextB);
+}
+
+
+
+cpConstraint *
+cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
+{
+	cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once.");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(constraint->a);
+	cpBodyActivate(constraint->b);
+	cpArrayPush(space->constraints, constraint);
+	
+	// Push onto the heads of the bodies' constraint lists
+	cpBody *a = constraint->a, *b = constraint->b;
+	constraint->nextA = a->constraintList; a->constraintList = constraint;
+	constraint->nextB = b->constraintList; b->constraintList = constraint;
+	
+	return constraint;
+}
+
+typedef struct removalContext {
+	cpSpace *space;
+	cpShape *shape;
+} removalContext;
+
+// Hashset filter func to throw away old arbiters.
+static cpBool
+contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
+{
+	if(context->shape == arb->a || context->shape == arb->b){
+		if(arb->state != cpArbiterStateCached){
+			arb->handler->separate(arb, context->space, arb->handler->data);
+		}
+		
+		cpArrayPush(context->space->pooledArbiters, arb);
+		return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+void
+cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *body = shape->body;
+	if(cpBodyIsStatic(body)){
+		cpSpaceRemoveStaticShape(space, shape);
+		return;
+	}
+
+	cpBodyActivate(body);
+	
+	cpAssert(cpSpatialIndexContains(space->activeShapes, shape, shape->hashid),
+		"Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyRemoveShape(body, shape);
+	
+	removalContext context = {space, shape};
+	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
+	cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
+}
+
+void
+cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
+{
+	cpAssert(cpSpatialIndexContains(space->staticShapes, shape, shape->hashid),
+		"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	removalContext context = {space, shape};
+	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
+	cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
+	
+	cpSpaceActivateShapesTouchingShape(space, shape);
+}
+
+void
+cpSpaceRemoveBody(cpSpace *space, cpBody *body)
+{
+	cpAssertWarn(body->space == space,
+		"Cannot remove a body that was not added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(body);
+	cpArrayDeleteObj(space->bodies, body);
+	body->space = NULL;
+}
+
+void
+cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
+{
+	cpAssertWarn(cpArrayContains(space->constraints, constraint),
+		"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
+	cpAssertSpaceUnlocked(space);
+	
+	cpBodyActivate(constraint->a);
+	cpBodyActivate(constraint->b);
+	cpArrayDeleteObj(space->constraints, constraint);
+	
+	cpBodyRemoveConstraint(constraint->a, constraint);
+	cpBodyRemoveConstraint(constraint->b, constraint);
+}
+
+#pragma mark Iteration
+
+// TODO copy fix from 5.x
+void
+cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
+{
+	cpArray *bodies = space->bodies;
+	
+	for(int i=0; i<bodies->num; i++)
+		func((cpBody *)bodies->arr[i], data);
+}
+
+#pragma mark Spatial Hash Management
+
+static void
+updateBBCache(cpShape *shape, void *unused)
+{
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
+}
+
+void
+cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
+{
+// TODO spatial hash specific
+	cpSpaceHashResize((cpSpaceHash *)space->staticShapes, dim, count);
+	cpSpatialIndexReindex(space->staticShapes);
+}
+
+void
+cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
+{
+// TODO spatial hash specific
+	cpSpaceHashResize((cpSpaceHash *)space->activeShapes, dim, count);
+}
+
+void 
+cpSpaceRehashStatic(cpSpace *space)
+{
+	cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&updateBBCache, NULL);
+	cpSpatialIndexReindex(space->staticShapes);
+}
+
+void
+cpSpaceRehashShape(cpSpace *space, cpShape *shape)
+{
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
+	
+	// attempt to rehash the shape in both hashes
+	cpSpatialIndexReindexObject(space->activeShapes, shape, shape->hashid);
+	cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid);
+}

src/helper/chipmunk/cpSpace.h

@@ -0,0 +1,181 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+// Number of frames that contact information should persist.
+extern cpTimestamp cp_contact_persistence;
+
+typedef struct cpContactBufferHeader cpContactBufferHeader;
+
+struct cpSpace {
+	// *** User definable fields
+	
+	// Number of iterations to use in the impulse solver to solve contacts.
+	int iterations;
+	
+	// Default gravity to supply when integrating rigid body motions.
+	cpVect gravity;
+	
+	// Default damping to supply when integrating rigid body motions.
+	cpFloat damping;
+	
+	// Speed threshold for a body to be considered idle.
+	// The default value of 0 means to let the space guess a good threshold based on gravity.
+	cpFloat idleSpeedThreshold;
+	
+	// Time a group of bodies must remain idle in order to fall asleep
+	// The default value of INFINITY disables the sleeping algorithm.
+	cpFloat sleepTimeThreshold;
+	
+	// *** Internally Used Fields
+	
+	// When the space lock count is non zero you cannot add or remove objects
+	CP_PRIVATE(int locked);
+	
+	// Time stamp. Is incremented on every call to cpSpaceStep().
+	CP_PRIVATE(cpTimestamp stamp);
+
+	// The static and active shape spatial hashes.
+	CP_PRIVATE(cpSpatialIndex *staticShapes);
+	CP_PRIVATE(cpSpatialIndex *activeShapes);
+	
+	// List of bodies in the system.
+	CP_PRIVATE(cpArray *bodies);
+	
+	// List of groups of sleeping bodies.
+	CP_PRIVATE(cpArray *sleepingComponents);
+	
+	// List of bodies that have been flagged to be awoken.
+	CP_PRIVATE(cpArray *rousedBodies);
+	
+	// List of active arbiters for the impulse solver.
+	CP_PRIVATE(cpArray *arbiters);
+	CP_PRIVATE(cpArray *pooledArbiters);
+	
+	// Linked list ring of contact buffers.
+	// Head is the newest buffer, and each buffer points to a newer buffer.
+	// Head wraps around and points to the oldest (tail) buffer.
+	CP_PRIVATE(cpContactBufferHeader *contactBuffersHead);
+	
+	// List of buffers to be free()ed when destroying the space.
+	CP_PRIVATE(cpArray *allocatedBuffers);
+	
+	// Persistant contact set.
+	CP_PRIVATE(cpHashSet *contactSet);
+	
+	// List of constraints in the system.
+	CP_PRIVATE(cpArray *constraints);
+	
+	// Set of collisionpair functions.
+	CP_PRIVATE(cpHashSet *collFuncSet);
+	// Default collision handler.
+	CP_PRIVATE(cpCollisionHandler defaultHandler);
+	
+	CP_PRIVATE(cpHashSet *postStepCallbacks);
+	
+	CP_PRIVATE(cpBody _staticBody);
+	
+	cpDataPointer data;
+	cpBody *staticBody;
+};
+
+// Basic allocation/destruction functions.
+cpSpace* cpSpaceAlloc(void);
+cpSpace* cpSpaceInit(cpSpace *space);
+cpSpace* cpSpaceNew(void);
+
+void cpSpaceDestroy(cpSpace *space);
+void cpSpaceFree(cpSpace *space);
+
+// Convenience function. Frees all referenced entities. (bodies, shapes and constraints)
+void cpSpaceFreeChildren(cpSpace *space);
+
+// Collision handler management functions.
+void cpSpaceSetDefaultCollisionHandler(
+	cpSpace *space,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+);
+void cpSpaceAddCollisionHandler(
+	cpSpace *space,
+	cpCollisionType a, cpCollisionType b,
+	cpCollisionBeginFunc begin,
+	cpCollisionPreSolveFunc preSolve,
+	cpCollisionPostSolveFunc postSolve,
+	cpCollisionSeparateFunc separate,
+	void *data
+);
+void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b);
+
+// Add and remove entities from the system.
+cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape);
+cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
+cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body);
+cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
+
+void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
+void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
+void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
+void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint);
+
+// Post Step function definition
+typedef void (*cpPostStepFunc)(cpSpace *space, void *obj, void *data);
+// Register a post step function to be called after cpSpaceStep() has finished.
+// obj is used a key, you can only register one callback per unique value for obj
+void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data);
+
+// Point query callback function
+typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data);
+void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data);
+cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group);
+
+// Segment query callback function
+typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data);
+void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
+cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
+
+// BB query callback function
+typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data);
+void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);
+
+// Shape query callback function
+typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data);
+cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data);
+
+
+void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape);
+
+
+// Iterator function for iterating the bodies in a space.
+typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data);
+void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
+
+// Spatial hash management functions.
+void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
+void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
+void cpSpaceRehashStatic(cpSpace *space);
+
+void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
+
+// Update the space.
+void cpSpaceStep(cpSpace *space, cpFloat dt);

src/helper/chipmunk/cpSpaceComponent.c

@@ -0,0 +1,290 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "chipmunk_private.h"
+
+#pragma mark Sleeping Functions
+
+void
+cpSpaceActivateBody(cpSpace *space, cpBody *body)
+{
+	if(space->locked){
+		// cpSpaceActivateBody() is called again once the space is unlocked
+		cpArrayPush(space->rousedBodies, body);
+	} else {
+		cpArrayPush(space->bodies, body);
+
+		CP_BODY_FOREACH_SHAPE(body, shape){
+			cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
+			cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
+		}
+		
+		CP_BODY_FOREACH_ARBITER(body, arb){
+			cpBody *bodyA = arb->a->body;
+			if(body == bodyA || cpBodyIsStatic(bodyA)){
+				int numContacts = arb->numContacts;
+				cpContact *contacts = arb->contacts;
+				
+				// Restore contact values back to the space's contact buffer memory
+				arb->contacts = cpContactBufferGetArray(space);
+				memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact));
+				cpSpacePushContacts(space, numContacts);
+				
+				cpShape *a = arb->a, *b = arb->b;
+				cpShape *shape_pair[] = {a, b};
+				cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
+				cpHashSetInsert(space->contactSet, arbHashID, shape_pair, arb, NULL);
+				cpArrayPush(space->arbiters, arb);
+				arb->stamp = space->stamp;
+				
+				arb->stamp = space->stamp;
+				cpArrayPush(space->arbiters, arb);
+				
+				cpfree(contacts);
+			}
+		}
+		
+		CP_BODY_FOREACH_CONSTRAINT(body, constraint){
+			cpBody *bodyA = constraint->a;
+			if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayPush(space->constraints, constraint);
+		}
+	}
+}
+
+static void
+cpSpaceDeactivateBody(cpSpace *space, cpBody *body)
+{
+	cpArrayDeleteObj(space->bodies, body);
+	
+	CP_BODY_FOREACH_SHAPE(body, shape){
+		cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid);
+		cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
+	}
+	
+	CP_BODY_FOREACH_ARBITER(body, arb){
+		cpBody *bodyA = arb->a->body;
+		if(body == bodyA || cpBodyIsStatic(bodyA)){
+			cpShape *a = arb->a, *b = arb->b;
+			cpShape *shape_pair[] = {a, b};
+			cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
+			cpHashSetRemove(space->contactSet, arbHashID, shape_pair);
+			cpArrayDeleteObj(space->arbiters, arb);
+			
+			// Save contact values to a new block of memory so they won't time out
+			size_t bytes = arb->numContacts*sizeof(cpContact);
+			cpContact *contacts = (cpContact *)cpmalloc(bytes);
+			memcpy(contacts, arb->contacts, bytes);
+			arb->contacts = contacts;
+		}
+	}
+		
+	CP_BODY_FOREACH_CONSTRAINT(body, constraint){
+		cpBody *bodyA = constraint->a;
+		if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayDeleteObj(space->constraints, constraint);
+	}
+}
+
+static inline void
+ComponentActivate(cpBody *root)
+{
+	if(!root || !cpBodyIsSleeping(root)) return;
+	cpAssert(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body.");
+	
+	cpSpace *space = root->space;
+	CP_BODY_FOREACH_GROUP(root, body){
+		body->node.root = NULL;
+		cpSpaceActivateBody(space, body);
+	}
+	
+	cpArrayDeleteObj(space->sleepingComponents, root);
+}
+
+static inline void
+ComponentAdd(cpBody *root, cpBody *body){
+	body->node.root = root;
+
+	if(body != root){
+		body->node.next = root->node.next;
+		root->node.next = body;
+	}
+}
+
+void
+cpBodyActivate(cpBody *body)
+{
+	if(!cpBodyIsStatic(body)){
+		body->node.idleTime = 0.0f;
+		ComponentActivate(body->node.root);
+	}
+}
+
+static inline cpBool
+ComponentActive(cpBody *root, cpFloat threshold)
+{
+	CP_BODY_FOREACH_GROUP(root, body){
+		if(body->node.idleTime < threshold) return cpTrue;
+	}
+	
+	return cpFalse;
+}
+
+static inline void
+DepthFirstSearch(cpBody *root, cpBody *body)
+{
+	if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){
+		cpBody *other_root = body->node.root;
+		if(other_root == NULL){
+			ComponentAdd(root, body);
+			CP_BODY_FOREACH_ARBITER(body, arb) DepthFirstSearch(root, (body == arb->a->body ? arb->b->body : arb->a->body));
+			CP_BODY_FOREACH_CONSTRAINT(body, constraint) DepthFirstSearch(root, (body == constraint->a ? constraint->b : constraint->a));
+		} else if(other_root != root){
+			cpAssert(cpFalse, "Uh oh. This shouldn't happen?");
+		}
+	}
+}
+
+static inline void
+cpBodyPushArbiter(cpBody *body, cpArbiter *arb)
+{
+	if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){
+		if(body == arb->a->body){
+			arb->nextA = body->arbiterList;
+		} else {
+			arb->nextB = body->arbiterList;
+		}
+		
+		body->arbiterList = arb;
+	}
+}
+
+// TODO this function needs more commenting.
+void
+cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
+{
+	cpFloat dv = space->idleSpeedThreshold;
+	cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt);
+	
+	// update idling and reset arbiter list and component nodes
+	cpArray *bodies = space->bodies;
+	for(int i=0; i<bodies->num; i++){
+		cpBody *body = (cpBody*)bodies->arr[i];
+		
+		// Need to deal with infinite mass objects
+		cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f);
+		body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt);
+		
+		body->arbiterList = NULL;
+		body->node.next = NULL;
+	}
+	
+	// Add arbiters to body lists and awaken any sleeping bodies found
+	cpArray *arbiters = space->arbiters;
+	for(int i=0, count=arbiters->num; i<count; i++){
+		cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
+		cpBody *a = arb->a->body, *b = arb->b->body;
+		
+		if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a);
+		if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b);
+		
+		cpBodyPushArbiter(a, arb);
+		cpBodyPushArbiter(b, arb);
+	}
+	
+	cpArray *constraints = space->constraints;
+	for(int i=0; i<constraints->num; i++){
+		cpConstraint *constraint = constraints->arr[i];
+		cpBody *a = constraint->a, *b = constraint->b;
+		
+		if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a);
+		if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b);
+	}
+	
+	for(int i=0; i<bodies->num;){
+		cpBody *body = (cpBody*)bodies->arr[i];
+		
+		if(body->node.root == NULL){
+			DepthFirstSearch(body, body);
+			
+			if(!ComponentActive(body, space->sleepTimeThreshold)){
+				cpArrayPush(space->sleepingComponents, body);
+				CP_BODY_FOREACH_GROUP(body, other) cpSpaceDeactivateBody(space, other);
+				
+				// cpSpaceDeactivateBody() removed the current body from the list.
+				// Skip the index increment and don't reset the root pointer.
+				continue;
+			}
+		}
+		
+		i++;
+		body->node.root = NULL;
+	}
+}
+
+void
+cpBodySleep(cpBody *body)
+{
+	cpBodySleepWithGroup(body, NULL);
+}
+
+void
+cpBodySleepWithGroup(cpBody *body, cpBody *group){
+	cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep.");
+	
+	cpSpace *space = body->space;
+	cpAssert(space, "Cannot put a rogue body to sleep.");
+	cpAssert(!space->locked, "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback.");
+	cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
+	cpAssert(!group || cpBodyIsSleeping(body), "Cannot add a body that is already sleeping to a group.");
+		
+	CP_BODY_FOREACH_SHAPE(body, shape) cpShapeUpdate(shape, body->p, body->rot);
+	cpSpaceDeactivateBody(space, body);
+	
+	if(group){
+		cpBody *root = group->node.root;
+		
+		cpComponentNode node = {root, root->node.next, 0.0f};
+		body->node = node;
+		
+		root->node.next = body;
+	} else {
+		cpComponentNode node = {body, NULL, 0.0f};
+		body->node = node;
+		
+		cpArrayPush(space->sleepingComponents, body);
+	}
+	
+	cpArrayDeleteObj(space->bodies, body);
+}
+
+static void
+activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){
+	cpBodyActivate(shape->body);
+}
+
+void
+cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){
+	cpArray *bodies = NULL;
+	cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, &bodies);
+}

src/helper/chipmunk/cpSpaceHash.c

@@ -0,0 +1,646 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <math.h>
+#include <stdlib.h>
+
+#include "chipmunk_private.h"
+#include "prime.h"
+
+typedef struct cpSpaceHashBin cpSpaceHashBin;
+typedef struct cpHandle cpHandle;
+
+struct cpSpaceHash {
+	cpSpatialIndex spatialIndex;
+	
+	int numcells;
+	cpFloat celldim;
+	
+	cpSpaceHashBin **table;
+	cpHashSet *handleSet;
+	
+	cpSpaceHashBin *pooledBins;
+	cpArray *pooledHandles;
+	cpArray *allocatedBuffers;
+	
+	cpTimestamp stamp;
+};
+
+
+#pragma mark Handle Functions
+
+struct cpHandle {
+	void *obj;
+	int retain;
+	cpTimestamp stamp;
+};
+
+static cpHandle*
+cpHandleInit(cpHandle *hand, void *obj)
+{
+	hand->obj = obj;
+	hand->retain = 0;
+	hand->stamp = 0;
+	
+	return hand;
+}
+
+static inline void cpHandleRetain(cpHandle *hand){hand->retain++;}
+
+static inline void
+cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
+{
+	hand->retain--;
+	if(hand->retain == 0) cpArrayPush(pooledHandles, hand);
+}
+
+static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);}
+
+static void *
+handleSetTrans(void *obj, cpSpaceHash *hash)
+{
+	if(hash->pooledHandles->num == 0){
+		// handle pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(cpHandle);
+		cpAssert(count, "Buffer size is too small.");
+		
+		cpHandle *buffer = (cpHandle *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(hash->allocatedBuffers, buffer);
+		
+		for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
+	}
+	
+	cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj);
+	cpHandleRetain(hand);
+	
+	return hand;
+}
+
+#pragma mark Bin Functions
+
+struct cpSpaceHashBin {
+	cpHandle *handle;
+	cpSpaceHashBin *next;
+};
+
+static inline void
+recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin)
+{
+	bin->next = hash->pooledBins;
+	hash->pooledBins = bin;
+}
+
+static inline void
+clearTableCell(cpSpaceHash *hash, int idx)
+{
+	cpSpaceHashBin *bin = hash->table[idx];
+	while(bin){
+		cpSpaceHashBin *next = bin->next;
+		
+		cpHandleRelease(bin->handle, hash->pooledHandles);
+		recycleBin(hash, bin);
+		
+		bin = next;
+	}
+	
+	hash->table[idx] = NULL;
+}
+
+static void
+clearTable(cpSpaceHash *hash)
+{
+	for(int i=0; i<hash->numcells; i++) clearTableCell(hash, i);
+}
+
+// Get a recycled or new bin.
+static inline cpSpaceHashBin *
+getEmptyBin(cpSpaceHash *hash)
+{
+	cpSpaceHashBin *bin = hash->pooledBins;
+	
+	if(bin){
+		hash->pooledBins = bin->next;
+		return bin;
+	} else {
+		// Pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
+		cpAssert(count, "Buffer size is too small.");
+		
+		cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(hash->allocatedBuffers, buffer);
+		
+		// push all but the first one, return the first instead
+		for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
+		return buffer;
+	}
+}
+
+#pragma mark Memory Management Functions
+
+cpSpaceHash *
+cpSpaceHashAlloc(void)
+{
+	return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash));
+}
+
+// Frees the old table, and allocate a new one.
+static void
+cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
+{
+	cpfree(hash->table);
+	
+	hash->numcells = numcells;
+	hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *));
+}
+
+static cpSpatialIndexClass klass;
+
+cpSpatialIndex *
+cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
+{
+	cpSpatialIndexInit((cpSpatialIndex *)hash, &klass, bbfunc, staticIndex);
+	
+	cpSpaceHashAllocTable(hash, next_prime(numcells));
+	hash->celldim = celldim;
+	
+	hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, NULL);
+	hash->pooledHandles = cpArrayNew(0);
+	
+	hash->pooledBins = NULL;
+	hash->allocatedBuffers = cpArrayNew(0);
+	
+	hash->stamp = 1;
+	
+	return (cpSpatialIndex *)hash;
+}
+
+cpSpatialIndex *
+cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
+{
+	return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex);
+}
+
+static void
+cpSpaceHashDestroy(cpSpaceHash *hash)
+{
+	clearTable(hash);
+	
+	cpHashSetFree(hash->handleSet);
+	
+	cpArrayFreeEach(hash->allocatedBuffers, cpfree);
+	cpArrayFree(hash->allocatedBuffers);
+	cpArrayFree(hash->pooledHandles);
+	
+	cpfree(hash->table);
+}
+
+#pragma mark Helper Functions
+
+static inline cpBool
+containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
+{
+	while(bin){
+		if(bin->handle == hand) return cpTrue;
+		bin = bin->next;
+	}
+	
+	return cpFalse;
+}
+
+// The hash function itself.
+static inline cpHashValue
+hash_func(cpHashValue x, cpHashValue y, cpHashValue n)
+{
+	return (x*1640531513ul ^ y*2654435789ul) % n;
+}
+
+// Much faster than (int)floor(f)
+// Profiling showed floor() to be a sizable performance hog
+static inline int
+floor_int(cpFloat f)
+{
+	int i = (int)f;
+	return (f < 0.0f && f != i ? i - 1 : i);
+}
+
+static inline void
+hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
+{
+	// Find the dimensions in cell coordinates.
+	cpFloat dim = hash->celldim;
+	int l = floor_int(bb.l/dim); // Fix by ShiftZ
+	int r = floor_int(bb.r/dim);
+	int b = floor_int(bb.b/dim);
+	int t = floor_int(bb.t/dim);
+	
+	int n = hash->numcells;
+	for(int i=l; i<=r; i++){
+		for(int j=b; j<=t; j++){
+			int idx = hash_func(i,j,n);
+			cpSpaceHashBin *bin = hash->table[idx];
+			
+			// Don't add an object twice to the same cell.
+			if(containsHandle(bin, hand)) continue;
+
+			cpHandleRetain(hand);
+			// Insert a new bin for the handle in this cell.
+			cpSpaceHashBin *newBin = getEmptyBin(hash);
+			newBin->handle = hand;
+			newBin->next = bin;
+			hash->table[idx] = newBin;
+		}
+	}
+}
+
+#pragma mark Basic Operations
+
+static void
+cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid)
+{
+	cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash, (cpHashSetTransFunc)handleSetTrans);
+	hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj));
+}
+
+static void
+cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
+{
+	cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
+	
+	if(hand){
+		hand->obj = NULL;
+		cpHandleRelease(hand, hash->pooledHandles);
+		
+		cpSpaceHashInsert(hash, obj, hashid);
+	}
+}
+
+static void
+rehash_helper(cpHandle *hand, cpSpaceHash *hash)
+{
+	hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj));
+}
+
+static void
+cpSpaceHashRehash(cpSpaceHash *hash)
+{
+	clearTable(hash);
+	cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)rehash_helper, hash);
+}
+
+static void
+cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
+{
+	cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
+	
+	if(hand){
+		hand->obj = NULL;
+		cpHandleRelease(hand, hash->pooledHandles);
+	}
+}
+
+typedef struct eachContext {
+	cpSpatialIndexIterator func;
+	void *data;
+} eachContext;
+
+static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);}
+
+static void
+cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIterator func, void *data)
+{
+	eachContext context = {func, data};
+	cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &context);
+}
+
+static void
+remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
+{
+	cpSpaceHashBin *bin = *bin_ptr;
+	while(bin){
+		cpHandle *hand = bin->handle;
+		cpSpaceHashBin *next = bin->next;
+		
+		if(!hand->obj){
+			// orphaned handle, unlink and recycle the bin
+			(*bin_ptr) = bin->next;
+			recycleBin(hash, bin);
+			
+			cpHandleRelease(hand, hash->pooledHandles);
+		} else {
+			bin_ptr = &bin->next;
+		}
+		
+		bin = next;
+	}
+}
+
+#pragma mark Query Functions
+
+static inline void
+query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryCallback func, void *data)
+{
+	restart:
+	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
+		cpHandle *hand = bin->handle;
+		void *other = hand->obj;
+		
+		if(hand->stamp == hash->stamp || obj == other){
+			continue;
+		} else if(other){
+			func(obj, other, data);
+			hand->stamp = hash->stamp;
+		} else {
+			// The object for this handle has been removed
+			// cleanup this cell and restart the query
+			remove_orphaned_handles(hash, bin_ptr);
+			goto restart; // GCC not smart enough/able to tail call an inlined function.
+		}
+	}
+}
+
+static void
+cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpatialIndexQueryCallback func, void *data)
+{
+	cpFloat dim = hash->celldim;
+	int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells);  // Fix by ShiftZ
+	
+	query_helper(hash, &hash->table[idx], &point, func, data);
+	hash->stamp++;
+}
+
+static void
+cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data)
+{
+	// Get the dimensions in cell coordinates.
+	cpFloat dim = hash->celldim;
+	int l = floor_int(bb.l/dim);  // Fix by ShiftZ
+	int r = floor_int(bb.r/dim);
+	int b = floor_int(bb.b/dim);
+	int t = floor_int(bb.t/dim);
+	
+	int n = hash->numcells;
+	cpSpaceHashBin **table = hash->table;
+	
+	// Iterate over the cells and query them.
+	for(int i=l; i<=r; i++){
+		for(int j=b; j<=t; j++){
+			query_helper(hash, &table[hash_func(i,j,n)], obj, func, data);
+		}
+	}
+	
+	hash->stamp++;
+}
+
+// Similar to struct eachPair above.
+typedef struct queryRehashContext {
+	cpSpaceHash *hash;
+	cpSpatialIndexQueryCallback func;
+	void *data;
+} queryRehashContext;
+
+// Hashset iterator func used with cpSpaceHashQueryRehash().
+static void
+queryRehash_helper(cpHandle *hand, queryRehashContext *context)
+{
+	cpSpaceHash *hash = context->hash;
+	cpSpatialIndexQueryCallback func = context->func;
+	void *data = context->data;
+
+	cpFloat dim = hash->celldim;
+	int n = hash->numcells;
+
+	void *obj = hand->obj;
+	cpBB bb = hash->spatialIndex.bbfunc(obj);
+
+	int l = floor_int(bb.l/dim);
+	int r = floor_int(bb.r/dim);
+	int b = floor_int(bb.b/dim);
+	int t = floor_int(bb.t/dim);
+	
+	cpSpaceHashBin **table = hash->table;
+
+	for(int i=l; i<=r; i++){
+		for(int j=b; j<=t; j++){
+			int idx = hash_func(i,j,n);
+			cpSpaceHashBin *bin = table[idx];
+			
+			if(containsHandle(bin, hand)) continue;
+			
+			cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
+			query_helper(hash, &bin, obj, func, data);
+			
+			cpSpaceHashBin *newBin = getEmptyBin(hash);
+			newBin->handle = hand;
+			newBin->next = bin;
+			table[idx] = newBin;
+		}
+	}
+	
+	// Increment the stamp for each object hashed.
+	hash->stamp++;
+}
+
+static void
+cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryCallback func, void *data)
+{
+	clearTable(hash);
+	
+	queryRehashContext context = {hash, func, data};
+	cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)queryRehash_helper, &context);
+	
+	cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data);
+}
+
+static inline cpFloat
+segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryCallback func, void *data)
+{
+	cpFloat t = 1.0f;
+	 
+	restart:
+	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
+		cpHandle *hand = bin->handle;
+		void *other = hand->obj;
+		
+		// Skip over certain conditions
+		if(hand->stamp == hash->stamp){
+			continue;
+		} else if(other){
+			t = cpfmin(t, func(obj, other, data));
+			hand->stamp = hash->stamp;
+		} else {
+			// The object for this handle has been removed
+			// cleanup this cell and restart the query
+			remove_orphaned_handles(hash, bin_ptr);
+			goto restart; // GCC not smart enough/able to tail call an inlined function.
+		}
+	}
+	
+	return t;
+}
+
+// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
+void
+cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data)
+{
+	a = cpvmult(a, 1.0f/hash->celldim);
+	b = cpvmult(b, 1.0f/hash->celldim);
+	
+	int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
+
+	cpFloat t = 0;
+
+	int x_inc, y_inc;
+	cpFloat temp_v, temp_h;
+
+	if (b.x > a.x){
+		x_inc = 1;
+		temp_h = (cpffloor(a.x + 1.0f) - a.x);
+	} else {
+		x_inc = -1;
+		temp_h = (a.x - cpffloor(a.x));
+	}
+
+	if (b.y > a.y){
+		y_inc = 1;
+		temp_v = (cpffloor(a.y + 1.0f) - a.y);
+	} else {
+		y_inc = -1;
+		temp_v = (a.y - cpffloor(a.y));
+	}
+	
+	// Division by zero is *very* slow on ARM
+	cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
+	cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
+	
+	// fix NANs in horizontal directions
+	cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
+	cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
+	
+	int n = hash->numcells;
+	cpSpaceHashBin **table = hash->table;
+
+	while(t < t_exit){
+		int idx = hash_func(cell_x, cell_y, n);
+		t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data));
+
+		if (next_v < next_h){
+			cell_y += y_inc;
+			t = next_v;
+			next_v += dt_dy;
+		} else {
+			cell_x += x_inc;
+			t = next_h;
+			next_h += dt_dx;
+		}
+	}
+	
+	hash->stamp++;
+}
+
+#pragma mark Misc
+
+void
+cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
+{
+	if(hash->spatialIndex.klass != &klass){
+		cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index.");
+		return;
+	}
+	
+	clearTable(hash);
+	
+	hash->celldim = celldim;
+	cpSpaceHashAllocTable(hash, next_prime(numcells));
+}
+
+static int
+cpSpaceHashCount(cpSpaceHash *hash)
+{
+	return cpHashSetCount(hash->handleSet);
+}
+
+static int
+cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid)
+{
+	return cpHashSetFind(hash->handleSet, hashid, obj) != NULL;
+}
+
+static cpSpatialIndexClass klass = {
+	(cpSpatialIndexDestroyFunc)cpSpaceHashDestroy,
+	
+	(cpSpatialIndexCountFunc)cpSpaceHashCount,
+	(cpSpatialIndexEachFunc)cpSpaceHashEach,
+	(cpSpatialIndexContainsFunc)cpSpaceHashContains,
+	
+	(cpSpatialIndexInsertFunc)cpSpaceHashInsert,
+	(cpSpatialIndexRemoveFunc)cpSpaceHashRemove,
+	
+	(cpSpatialIndexReindexFunc)cpSpaceHashRehash,
+	(cpSpatialIndexReindexObjectFunc)cpSpaceHashRehashObject,
+	(cpSpatialIndexReindexQueryFunc)cpSpaceHashReindexQuery,
+	
+	(cpSpatialIndexPointQueryFunc)cpSpaceHashPointQuery,
+	(cpSpatialIndexSegmentQueryFunc)cpSpaceHashSegmentQuery,
+	(cpSpatialIndexQueryFunc)cpSpaceHashQuery,
+};
+
+#pragma mark Debug Drawing
+
+#define CP_BBTREE_DEBUG_DRAW
+#ifdef CP_BBTREE_DEBUG_DRAW
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include <GL/glut.h>
+
+void
+cpSpaceHashRenderDebug(cpSpatialIndex *index)
+{
+	if(index->klass != &klass){
+		cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index.");
+		return;
+	}
+	
+	cpSpaceHash *hash = (cpSpaceHash *)index;
+	cpBB bb = cpBBNew(-320, -240, 320, 240);
+	
+	cpFloat dim = hash->celldim;
+	int n = hash->numcells;
+	
+	int l = (int)floor(bb.l/dim);
+	int r = (int)floor(bb.r/dim);
+	int b = (int)floor(bb.b/dim);
+	int t = (int)floor(bb.t/dim);
+	
+	for(int i=l; i<=r; i++){
+		for(int j=b; j<=t; j++){
+			int cell_count = 0;
+			
+			int index = hash_func(i,j,n);
+			for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next)
+				cell_count++;
+			
+			GLfloat v = 1.0f - (GLfloat)cell_count/10.0f;
+			glColor3f(v,v,v);
+			glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim);
+		}
+	}
+}
+#endif

src/helper/chipmunk/cpSpaceQuery.c

@@ -0,0 +1,246 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+
+#include "chipmunk_private.h"
+
+#pragma mark Point Query Functions
+
+typedef struct pointQueryContext {
+	cpLayers layers;
+	cpGroup group;
+	cpSpacePointQueryFunc func;
+	void *data;
+} pointQueryContext;
+
+static void 
+pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
+{
+	if(
+		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
+		cpShapePointQuery(shape, *point)
+	){
+		context->func(shape, context->data);
+	}
+}
+
+void
+cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
+{
+	pointQueryContext context = {layers, group, func, data};
+	
+	cpSpaceLock(space); {
+    cpSpatialIndexPointQuery(space->activeShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context);
+    cpSpatialIndexPointQuery(space->staticShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context);
+	} cpSpaceUnlock(space);
+}
+
+static void
+rememberLastPointQuery(cpShape *shape, cpShape **outShape)
+{
+	if(!shape->sensor) *outShape = shape;
+}
+
+cpShape *
+cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
+{
+	cpShape *shape = NULL;
+	cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape);
+	
+	return shape;
+}
+
+
+#pragma mark Segment Query Functions
+
+typedef struct segQueryContext {
+	cpVect start, end;
+	cpLayers layers;
+	cpGroup group;
+	cpSpaceSegmentQueryFunc func;
+} segQueryContext;
+
+static cpFloat
+segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
+{
+	cpSegmentQueryInfo info;
+	
+	if(
+		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
+		cpShapeSegmentQuery(shape, context->start, context->end, &info)
+	){
+		context->func(shape, info.t, info.n, data);
+	}
+	
+	return 1.0f;
+}
+
+void
+cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
+{
+	segQueryContext context = {
+		start, end,
+		layers, group,
+		func,
+	};
+	
+	cpSpaceLock(space); {
+    cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data);
+    cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data);
+	} cpSpaceUnlock(space);
+}
+
+typedef struct segQueryFirstContext {
+	cpVect start, end;
+	cpLayers layers;
+	cpGroup group;
+} segQueryFirstContext;
+
+static cpFloat
+segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out)
+{
+	cpSegmentQueryInfo info;
+	
+	if(
+		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
+		!shape->sensor &&
+		cpShapeSegmentQuery(shape, context->start, context->end, &info) &&
+		info.t < out->t
+	){
+		*out = info;
+	}
+	
+	return out->t;
+}
+
+cpShape *
+cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
+{
+	cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
+	if(out){
+		(*out) = info;
+  } else {
+		out = &info;
+	}
+	
+	segQueryFirstContext context = {
+		start, end,
+		layers, group
+	};
+	
+	cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out);
+	cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out);
+	
+	return out->shape;
+}
+
+#pragma mark BB Query Functions
+
+typedef struct bbQueryContext {
+	cpLayers layers;
+	cpGroup group;
+	cpSpaceBBQueryFunc func;
+	void *data;
+} bbQueryContext;
+
+static void 
+bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
+{
+	if(
+		!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
+		cpBBintersects(*bb, shape->bb)
+	){
+		context->func(shape, context->data);
+	}
+}
+
+void
+cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
+{
+	bbQueryContext context = {layers, group, func, data};
+	
+	cpSpaceLock(space); {
+    cpSpatialIndexQuery(space->activeShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context);
+    cpSpatialIndexQuery(space->staticShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context);
+	} cpSpaceUnlock(space);
+}
+
+#pragma mark Shape Query Functions
+
+typedef struct shapeQueryContext {
+	cpSpaceShapeQueryFunc func;
+	void *data;
+	cpBool anyCollision;
+} shapeQueryContext;
+
+// Callback from the spatial hash.
+static void
+shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context)
+{
+	// Reject any of the simple cases
+	if(
+		(a->group && a->group == b->group) ||
+		!(a->layers & b->layers) ||
+		a->sensor || b->sensor
+	) return;
+	
+	cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
+	int numContacts = 0;
+	
+	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
+	if(a->klass->type <= b->klass->type){
+		numContacts = cpCollideShapes(a, b, contacts);
+	} else {
+		numContacts = cpCollideShapes(b, a, contacts);
+		for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
+	}
+	
+	if(numContacts){
+		context->anyCollision = cpTrue;
+		
+		if(context->func){
+			cpContactPointSet set = {numContacts, {}};
+			for(int i=0; i<set.count; i++){
+				set.points[i].point = contacts[i].p;
+				set.points[i].normal = contacts[i].p;
+				set.points[i].dist = contacts[i].dist;
+			}
+			
+			context->func(b, &set, context->data);
+		}
+	}
+}
+
+cpBool
+cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
+{
+	cpBody *body = shape->body;
+	cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb);
+	shapeQueryContext context = {func, data, cpFalse};
+	
+	cpSpaceLock(space); {
+    cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context);
+    cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context);
+	} cpSpaceUnlock(space);
+	
+	return context.anyCollision;
+}

src/helper/chipmunk/cpSpaceStep.c

@@ -0,0 +1,373 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "chipmunk_private.h"
+
+#pragma mark Post Step Callback Functions
+
+typedef struct postStepCallback {
+	cpPostStepFunc func;
+	void *obj;
+	void *data;
+} postStepCallback;
+
+static cpBool
+postStepFuncSetEql(postStepCallback *a, postStepCallback *b){
+	return a->obj == b->obj;
+}
+
+static void *
+postStepFuncSetTrans(postStepCallback *callback, void *ignored)
+{
+	postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback));
+	(*value) = (*callback);
+	
+	return value;
+}
+
+void
+cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
+{
+	if(!space->postStepCallbacks){
+		space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, NULL);
+	}
+	
+	postStepCallback callback = {func, obj, data};
+	cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL, (cpHashSetTransFunc)postStepFuncSetTrans);
+}
+
+#pragma mark Contact Buffer Functions
+
+struct cpContactBufferHeader {
+	cpTimestamp stamp;
+	cpContactBufferHeader *next;
+	unsigned int numContacts;
+};
+
+#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
+typedef struct cpContactBuffer {
+	cpContactBufferHeader header;
+	cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
+} cpContactBuffer;
+
+static cpContactBufferHeader *
+cpSpaceAllocContactBuffer(cpSpace *space)
+{
+	cpContactBuffer *buffer = (cpContactBuffer *)cpmalloc(sizeof(cpContactBuffer));
+	cpArrayPush(space->allocatedBuffers, buffer);
+	return (cpContactBufferHeader *)buffer;
+}
+
+static cpContactBufferHeader *
+cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
+{
+	header->stamp = stamp;
+	header->next = (splice ? splice->next : header);
+	header->numContacts = 0;
+	
+	return header;
+}
+
+static void
+cpSpacePushFreshContactBuffer(cpSpace *space)
+{
+	cpTimestamp stamp = space->stamp;
+	
+	cpContactBufferHeader *head = space->contactBuffersHead;
+	
+	if(!head){
+		// No buffers have been allocated, make one
+		space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL);
+	} else if(stamp - head->next->stamp > cp_contact_persistence){
+		// The tail buffer is available, rotate the ring
+	cpContactBufferHeader *tail = head->next;
+		space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
+	} else {
+		// Allocate a new buffer and push it into the ring
+		cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
+		space->contactBuffersHead = head->next = buffer;
+	}
+}
+
+
+cpContact *
+cpContactBufferGetArray(cpSpace *space)
+{
+	if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
+		// contact buffer could overflow on the next collision, push a fresh one.
+		cpSpacePushFreshContactBuffer(space);
+	}
+	
+	cpContactBufferHeader *head = space->contactBuffersHead;
+	return ((cpContactBuffer *)head)->contacts + head->numContacts;
+}
+
+void
+cpSpacePushContacts(cpSpace *space, int count)
+{
+	cpAssert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error:contact buffer overflow!");
+	space->contactBuffersHead->numContacts += count;
+}
+
+static inline void
+cpSpacePopContacts(cpSpace *space, int count){
+	space->contactBuffersHead->numContacts -= count;
+}
+
+#pragma mark Collision Detection Functions
+
+static void *
+contactSetTrans(cpShape **shapes, cpSpace *space)
+{
+	if(space->pooledArbiters->num == 0){
+		// arbiter pool is exhausted, make more
+		int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
+		cpAssert(count, "Buffer size too small.");
+		
+		cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
+		cpArrayPush(space->allocatedBuffers, buffer);
+		
+		for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
+	}
+	
+	return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
+}
+
+static inline cpBool
+queryReject(cpShape *a, cpShape *b)
+{
+	return (
+		// BBoxes must overlap
+		!cpBBintersects(a->bb, b->bb)
+		// Don't collide shapes attached to the same body.
+		|| a->body == b->body
+		// Don't collide objects in the same non-zero group
+		|| (a->group && a->group == b->group)
+		// Don't collide objects that don't share at least on layer.
+		|| !(a->layers & b->layers)
+	);
+}
+
+// Callback from the spatial hash.
+void
+cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space)
+{
+	// Reject any of the simple cases
+	if(queryReject(a,b)) return;
+	
+	// Find the collision pair function for the shapes.
+	cpCollisionType types[] = {a->collision_type, b->collision_type};
+	cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
+	cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, types);
+	
+	cpBool sensor = a->sensor || b->sensor;
+	if(sensor && handler == &space->defaultHandler) return;
+	
+	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
+	if(a->klass->type > b->klass->type){
+		cpShape *temp = a;
+		a = b;
+		b = temp;
+	}
+	
+	// Narrow-phase collision detection.
+	cpContact *contacts = cpContactBufferGetArray(space);
+	int numContacts = cpCollideShapes(a, b, contacts);
+	if(!numContacts) return; // Shapes are not colliding.
+	cpSpacePushContacts(space, numContacts);
+	
+	// Get an arbiter from space->contactSet for the two shapes.
+	// This is where the persistant contact magic comes from.
+	cpShape *shape_pair[] = {a, b};
+	cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
+	cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space, (cpHashSetTransFunc)contactSetTrans);
+	cpArbiterUpdate(arb, contacts, numContacts, handler, a, b);
+	
+	// Call the begin function first if it's the first step
+	if(arb->state == cpArbiterStateFirstColl && !handler->begin(arb, space, handler->data)){
+		cpArbiterIgnore(arb); // permanently ignore the collision until separation
+	}
+	
+	if(
+		// Ignore the arbiter if it has been flagged
+		(arb->state != cpArbiterStateIgnore) && 
+		// Call preSolve
+		handler->preSolve(arb, space, handler->data) &&
+		// Process, but don't add collisions for sensors.
+		!sensor
+	){
+		cpArrayPush(space->arbiters, arb);
+	} else {
+		cpSpacePopContacts(space, numContacts);
+		
+		arb->contacts = NULL;
+		arb->numContacts = 0;
+		
+		// Normally arbiters are set as used after calling the post-step callback.
+		// However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve.
+		if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal;
+	}
+	
+	// Time stamp the arbiter so we know it was used recently.
+	arb->stamp = space->stamp;
+}
+
+// Hashset filter func to throw away old arbiters.
+static cpBool
+contactSetFilter(cpArbiter *arb, cpSpace *space)
+{
+	cpTimestamp ticks = space->stamp - arb->stamp;
+	
+	// was used last frame, but not this one
+	if(ticks >= 1 && arb->state != cpArbiterStateCached){
+		arb->handler->separate(arb, space, arb->handler->data);
+		arb->state = cpArbiterStateCached;
+	}
+	
+	if(ticks >= cp_contact_persistence){
+		arb->contacts = NULL;
+		arb->numContacts = 0;
+		
+		cpArrayPush(space->pooledArbiters, arb);
+		return cpFalse;
+	}
+	
+	return cpTrue;
+}
+
+// Hashset filter func to call and throw away post step callbacks.
+static void
+postStepCallbackSetIter(postStepCallback *callback, cpSpace *space)
+{
+	callback->func(space, callback->obj, callback->data);
+	cpfree(callback);
+}
+
+#pragma mark All Important cpSpaceStep() Function
+
+void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);
+
+static void
+updateBBCache(cpShape *shape, void *unused)
+{
+	cpBody *body = shape->body;
+	cpShapeUpdate(shape, body->p, body->rot);
+}
+
+void
+cpSpaceStep(cpSpace *space, cpFloat dt)
+{
+	if(!dt) return; // don't step if the timestep is 0!
+	cpFloat dt_inv = 1.0f/dt;
+
+	cpArray *bodies = space->bodies;
+	cpArray *constraints = space->constraints;
+	
+	// Empty the arbiter list.
+	space->arbiters->num = 0;
+
+	// Integrate positions
+	for(int i=0; i<bodies->num; i++){
+		cpBody *body = (cpBody *)bodies->arr[i];
+		body->position_func(body, dt);
+	}
+	
+	// Pre-cache BBoxes and shape data.
+	cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)updateBBCache, NULL);
+	
+	
+	// Collide!
+	cpSpaceLock(space);
+	cpSpacePushFreshContactBuffer(space);
+	cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryCallback)cpSpaceCollideShapes, space);
+	cpSpaceUnlock(space);
+	
+	// If body sleeping is enabled, do that now.
+	if(space->sleepTimeThreshold != INFINITY){
+		cpSpaceProcessComponents(space, dt);
+//		bodies = space->bodies; // rebuilt by processContactComponents()
+	}
+	
+	// Clear out old cached arbiters and call separate callbacks
+	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
+
+	// Prestep the arbiters.
+	cpArray *arbiters = space->arbiters;
+	for(int i=0; i<arbiters->num; i++)
+		cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
+
+	// Prestep the constraints.
+	for(int i=0; i<constraints->num; i++){
+		cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
+		constraint->klass->preStep(constraint, dt, dt_inv);
+	}
+
+	// Integrate velocities.
+	cpFloat damping = cpfpow(space->damping, dt);
+	for(int i=0; i<bodies->num; i++){
+		cpBody *body = (cpBody *)bodies->arr[i];
+		body->velocity_func(body, space->gravity, damping, dt);
+	}
+
+	for(int i=0; i<arbiters->num; i++)
+		cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
+	
+	// Run the impulse solver.
+	for(int i=0; i<space->iterations; i++){
+		for(int j=0; j<arbiters->num; j++)
+			cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
+			
+		for(int j=0; j<constraints->num; j++){
+			cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
+			constraint->klass->applyImpulse(constraint);
+		}
+	}
+	
+	
+	// run the post solve callbacks
+	cpSpaceLock(space);
+	for(int i=0; i<arbiters->num; i++){
+		cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
+		
+		cpCollisionHandler *handler = arb->handler;
+		handler->postSolve(arb, space, handler->data);
+		
+		arb->state = cpArbiterStateNormal;
+	}
+	cpSpaceUnlock(space);
+	
+	// Run the post step callbacks
+	// Loop because post step callbacks may create more post step callbacks
+	while(space->postStepCallbacks){
+		cpHashSet *callbacks = space->postStepCallbacks;
+		space->postStepCallbacks = NULL;
+		
+		cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
+		cpHashSetFree(callbacks);
+	}
+	
+	// Increment the stamp.
+	space->stamp++;
+}

src/helper/chipmunk/cpSpatialIndex.c

@@ -0,0 +1,50 @@
+#include <stdlib.h>
+
+#include "chipmunk.h"
+
+void
+cpSpatialIndexFree(cpSpatialIndex *index)
+{
+	if(index){
+		cpSpatialIndexDestroy(index);
+		cpfree(index);
+	}
+}
+
+typedef struct dynamicToStaticContext {
+	cpSpatialIndexBBFunc bbfunc;
+	cpSpatialIndex *staticIndex;
+	cpSpatialIndexQueryCallback queryFunc;
+	void *data;
+} dynamicToStaticContext;
+
+cpSpatialIndex *
+cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
+{
+	index->klass = klass;
+	index->bbfunc = bbfunc;
+	index->staticIndex = staticIndex;
+	
+	if(staticIndex){
+		cpAssert(!staticIndex->dynamicIndex, "This static index is already is already associated with a dynamic index.");
+		staticIndex->dynamicIndex = index;
+	}
+	
+	return index;
+}
+
+static void
+dynamicToStaticIter(void *obj, dynamicToStaticContext *context)
+{
+	cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data);
+}
+
+void
+cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data)
+{
+	if(cpSpatialIndexCount(staticIndex) > 0){
+		dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data};
+		cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIterator)dynamicToStaticIter, &context);
+	}
+}
+

src/helper/chipmunk/cpSpatialIndex.h

@@ -0,0 +1,159 @@
+#pragma mark Spatial Index
+
+typedef cpBB (*cpSpatialIndexBBFunc)(void *obj);
+typedef void (*cpSpatialIndexIterator)(void *obj, void *data);
+typedef void (*cpSpatialIndexQueryCallback)(void *obj1, void *obj2, void *data);
+typedef cpFloat (*cpSpatialIndexSegmentQueryCallback)(void *obj1, void *obj2, void *data);
+
+
+typedef struct cpSpatialIndexClass cpSpatialIndexClass;
+typedef struct cpSpatialIndex cpSpatialIndex;
+
+struct cpSpatialIndex {
+	cpSpatialIndexClass *klass;
+	
+	cpSpatialIndexBBFunc bbfunc;
+	
+	cpSpatialIndex *staticIndex, *dynamicIndex;
+};
+
+
+#pragma mark Spatial Hash
+
+typedef struct cpSpaceHash cpSpaceHash;
+
+cpSpaceHash *cpSpaceHashAlloc(void);
+cpSpatialIndex *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
+cpSpatialIndex *cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
+
+void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells);
+
+#pragma mark AABB Tree
+
+typedef struct cpBBTree cpBBTree;
+
+cpBBTree *cpBBTreeAlloc(void);
+cpSpatialIndex *cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
+cpSpatialIndex *cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
+
+void cpBBTreeOptimize(cpSpatialIndex *index);
+
+typedef cpVect (*cpBBTreeVelocityFunc)(void *obj);
+void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func);
+
+#pragma mark Spatial Index Implementation
+
+typedef void (*cpSpatialIndexDestroyFunc)(cpSpatialIndex *index);
+
+typedef int (*cpSpatialIndexCountFunc)(cpSpatialIndex *index);
+typedef void (*cpSpatialIndexEachFunc)(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data);
+
+typedef cpBool (*cpSpatialIndexContainsFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid);
+typedef void (*cpSpatialIndexInsertFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid);
+typedef void (*cpSpatialIndexRemoveFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid);
+
+typedef void (*cpSpatialIndexReindexFunc)(cpSpatialIndex *index);
+typedef void (*cpSpatialIndexReindexObjectFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid);
+typedef void (*cpSpatialIndexReindexQueryFunc)(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data);
+
+typedef void (*cpSpatialIndexPointQueryFunc)(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data);
+typedef void (*cpSpatialIndexSegmentQueryFunc)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data);
+typedef void (*cpSpatialIndexQueryFunc)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data);
+
+struct cpSpatialIndexClass {
+	cpSpatialIndexDestroyFunc destroy;
+	
+	cpSpatialIndexCountFunc count;
+	cpSpatialIndexEachFunc each;
+	
+	cpSpatialIndexContainsFunc contains;
+	cpSpatialIndexInsertFunc insert;
+	cpSpatialIndexRemoveFunc remove;
+	
+	cpSpatialIndexReindexFunc reindex;
+	cpSpatialIndexReindexObjectFunc reindexObject;
+	cpSpatialIndexReindexQueryFunc reindexQuery;
+	
+	cpSpatialIndexPointQueryFunc pointQuery;
+	cpSpatialIndexSegmentQueryFunc segmentQuery;
+	cpSpatialIndexQueryFunc query;
+};
+
+
+void cpSpatialIndexFree(cpSpatialIndex *index);
+void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data);
+
+static inline void
+cpSpatialIndexDestroy(cpSpatialIndex *index)
+{
+	index->klass->destroy(index);
+}
+
+static inline int
+cpSpatialIndexCount(cpSpatialIndex *index)
+{
+	return index->klass->count(index);
+}
+
+static inline void
+cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data)
+{
+	index->klass->each(index, func, data);
+}
+
+static inline cpBool
+cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid)
+{
+	return index->klass->contains(index, obj, hashid);
+}
+
+static inline void
+cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid)
+{
+	index->klass->insert(index, obj, hashid);
+}
+
+static inline void
+cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid)
+{
+	index->klass->remove(index, obj, hashid);
+}
+
+static inline void
+cpSpatialIndexReindex(cpSpatialIndex *index)
+{
+	index->klass->reindex(index);
+}
+
+static inline void
+cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid)
+{
+	index->klass->reindexObject(index, obj, hashid);
+}
+
+// TODO make sure to doc the callback type, a pointer to the point is passed as obj1
+static inline	void
+cpSpatialIndexPointQuery(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data)
+{
+	index->klass->pointQuery(index, point, func, data);
+}
+
+// TODO make sure to doc the callback type
+static inline void
+cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data)
+{
+	index->klass->segmentQuery(index, obj, a, b, t_exit, func, data);
+}
+
+
+static inline void
+cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data)
+{
+	index->klass->query(index, obj, bb, func, data);
+}
+
+static inline void
+cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data)
+{
+	index->klass->reindexQuery(index, func, data);
+}

src/helper/chipmunk/cpVect.c

@@ -0,0 +1,71 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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 <stdio.h>
+#include <math.h>
+
+#include "chipmunk.h"
+
+cpFloat
+cpvlength(const cpVect v)
+{
+	return cpfsqrt( cpvdot(v, v) );
+}
+
+inline cpVect
+cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t)
+{
+	cpFloat omega = cpfacos(cpvdot(v1, v2));
+	
+	if(omega){
+		cpFloat denom = 1.0f/cpfsin(omega);
+		return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom));
+	} else {
+		return v1;
+	}
+}
+
+cpVect
+cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a)
+{
+	cpFloat angle = cpfacos(cpvdot(v1, v2));
+	return cpvslerp(v1, v2, cpfmin(a, angle)/angle);
+}
+
+cpVect
+cpvforangle(const cpFloat a)
+{
+	return cpv(cpfcos(a), cpfsin(a));
+}
+
+cpFloat
+cpvtoangle(const cpVect v)
+{
+	return cpfatan2(v.y, v.x);
+}
+
+char*
+cpvstr(const cpVect v)
+{
+	static char str[256];
+	sprintf(str, "(% .3f, % .3f)", v.x, v.y);
+	return str;
+}

src/helper/chipmunk/cpVect.h

@@ -0,0 +1,207 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+
+/// Constant for the zero vector.
+static const cpVect cpvzero = {0.0f,0.0f};
+
+/// Convenience constructor for cpVect structs.
+static inline cpVect
+cpv(const cpFloat x, const cpFloat y)
+{
+	cpVect v = {x, y};
+	return v;
+}
+
+// non-inlined functions
+
+/// Returns the length of v.
+cpFloat cpvlength(const cpVect v);
+
+/// Spherical linearly interpolate between v1 and v2.
+cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t);
+
+/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians
+cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a);
+
+/// Returns the unit length vector for the given angle (in radians).
+cpVect cpvforangle(const cpFloat a);
+
+/// Returns the angular direction v is pointing in (in radians).
+cpFloat cpvtoangle(const cpVect v);
+
+/**
+	Returns a string representation of v. Intended mostly for debugging purposes and not production use.
+	
+	@attention The string points to a static local and is reset every time the function is called.
+	If you want to print more than one vector you will have to split up your printing onto separate lines.
+*/
+char *cpvstr(const cpVect v);
+
+/// Check if two vectors are equal. (Be careful when comparing floating point numbers!)
+static inline cpBool
+cpveql(const cpVect v1, const cpVect v2)
+{
+	return (v1.x == v2.x && v1.y == v2.y);
+}
+
+/// Add two vectors
+static inline cpVect
+cpvadd(const cpVect v1, const cpVect v2)
+{
+	return cpv(v1.x + v2.x, v1.y + v2.y);
+}
+
+/// Negate a vector.
+static inline cpVect
+cpvneg(const cpVect v)
+{
+	return cpv(-v.x, -v.y);
+}
+
+/// Subtract two vectors.
+static inline cpVect
+cpvsub(const cpVect v1, const cpVect v2)
+{
+	return cpv(v1.x - v2.x, v1.y - v2.y);
+}
+
+/// Scalar multiplication.
+static inline cpVect
+cpvmult(const cpVect v, const cpFloat s)
+{
+	return cpv(v.x*s, v.y*s);
+}
+
+/// Vector dot product.
+static inline cpFloat
+cpvdot(const cpVect v1, const cpVect v2)
+{
+	return v1.x*v2.x + v1.y*v2.y;
+}
+
+/**
+	2D vector cross product analog.
+	The cross product of 2D vectors results in a 3D vector with only a z component.
+	This function returns the magnitude of the z value.
+*/
+static inline cpFloat
+cpvcross(const cpVect v1, const cpVect v2)
+{
+	return v1.x*v2.y - v1.y*v2.x;
+}
+
+/// Returns a perpendicular vector. (90 degree rotation)
+static inline cpVect
+cpvperp(const cpVect v)
+{
+	return cpv(-v.y, v.x);
+}
+
+/// Returns a perpendicular vector. (-90 degree rotation)
+static inline cpVect
+cpvrperp(const cpVect v)
+{
+	return cpv(v.y, -v.x);
+}
+
+/// Returns the vector projection of v1 onto v2.
+static inline cpVect
+cpvproject(const cpVect v1, const cpVect v2)
+{
+	return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2));
+}
+
+/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector.
+static inline cpVect
+cpvrotate(const cpVect v1, const cpVect v2)
+{
+	return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x);
+}
+
+/// Inverse of cpvrotate().
+static inline cpVect
+cpvunrotate(const cpVect v1, const cpVect v2)
+{
+	return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y);
+}
+
+/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths.
+static inline cpFloat
+cpvlengthsq(const cpVect v)
+{
+	return cpvdot(v, v);
+}
+
+/// Linearly interpolate between v1 and v2.
+static inline cpVect
+cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t)
+{
+	return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t));
+}
+
+/// Returns a normalized copy of v.
+static inline cpVect
+cpvnormalize(const cpVect v)
+{
+	return cpvmult(v, 1.0f/cpvlength(v));
+}
+
+/// Returns a normalized copy of v or cpvzero if v was already cpvzero. Protects against divide by zero errors.
+static inline cpVect
+cpvnormalize_safe(const cpVect v)
+{
+	return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v));
+}
+
+/// Clamp v to length len.
+static inline cpVect
+cpvclamp(const cpVect v, const cpFloat len)
+{
+	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
+}
+
+/// Linearly interpolate between v1 towards v2 by distance d.
+static inline cpVect
+cpvlerpconst(cpVect v1, cpVect v2, cpFloat d)
+{
+	return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d));
+}
+
+/// Returns the distance between v1 and v2.
+static inline cpFloat
+cpvdist(const cpVect v1, const cpVect v2)
+{
+	return cpvlength(cpvsub(v1, v2));
+}
+
+/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances.
+static inline cpFloat
+cpvdistsq(const cpVect v1, const cpVect v2)
+{
+	return cpvlengthsq(cpvsub(v1, v2));
+}
+
+/// Returns true if the distance between v1 and v2 is less than dist.
+static inline cpBool
+cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist)
+{
+	return cpvdistsq(v1, v2) < dist*dist;
+}

src/helper/chipmunk/prime.h

@@ -0,0 +1,68 @@
+/* Copyright (c) 2007 Scott Lembcke
+ * 
+ * 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.
+ */
+ 
+// Used for resizing hash tables.
+// Values approximately double.
+// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html
+static int primes[] = {
+	5,
+	13,
+	23,
+	47,
+	97,
+	193,
+	389,
+	769,
+	1543,
+	3079,
+	6151,
+	12289,
+	24593,
+	49157,
+	98317,
+	196613,
+	393241,
+	786433,
+	1572869,
+	3145739,
+	6291469,
+	12582917,
+	25165843,
+	50331653,
+	100663319,
+	201326611,
+	402653189,
+	805306457,
+	1610612741,
+	0,
+};
+
+static inline int
+next_prime(int n)
+{
+	int i = 0;
+	while(n > primes[i]){
+		i++;
+		cpAssert(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen
+	}
+	
+	return primes[i];
+}

src/physics/base.hpp

@@ -0,0 +1,24 @@
+#ifndef EE_PHYSICS_BASE
+#define EE_PHYSICS_BASE
+
+#include "../base.hpp"
+
+#include "../math/math.hpp"
+using namespace EE::Math;
+
+#include "../window/cengine.hpp"
+using namespace EE::Window;
+
+#include "../system/tsingleton.hpp"
+using namespace EE::System;
+
+#include "../graphics/cprimitives.hpp"
+
+#define CP_ALLOW_PRIVATE_ACCESS 1
+#include "../helper/chipmunk/chipmunk_private.h"
+#include "../helper/chipmunk/chipmunk_unsafe.h"
+#include "../helper/chipmunk/chipmunk.h"
+
+#include "physicshelper.hpp"
+
+#endif

src/physics/cbody.cpp

@@ -0,0 +1,170 @@
+#include "cbody.hpp"
+
+namespace EE { namespace Physics {
+
+cBody::cBody( cpBody * body ) {
+	mBody = body;
+	mBody->data = (void*)this;
+}
+
+cBody::cBody( cpFloat m, cpFloat i ) {
+	mBody = cpBodyNew( m, i );
+	mBody->data = (void*)this;
+}
+
+cBody::cBody() {
+	mBody = cpBodyNewStatic();
+}
+
+cBody::~cBody() {
+}
+
+void cBody::Activate() {
+	cpBodyActivate( mBody );
+}
+
+void cBody::Sleep() {
+	cpBodySleep( mBody );
+}
+
+void cBody::SleepWithGroup( cBody * Group ) {
+	cpBodySleepWithGroup( mBody, Group->Body() );
+}
+
+bool cBody::IsSleeping() {
+	return cpFalse != cpBodyIsSleeping( mBody );
+}
+
+bool cBody::IsStatic() {
+	return cpFalse != cpBodyIsStatic( mBody );
+}
+
+bool cBody::IsRogue() {
+	return cpFalse != cpBodyIsRogue( mBody );
+}
+
+cpBody * cBody::Body() const {
+	return mBody;
+}
+
+cpFloat cBody::Mass() const {
+	return cpBodyGetMass( mBody );
+}
+
+void cBody::Mass( const cpFloat& mass ) {
+	cpBodySetMass( mBody, mass );
+}
+
+cpFloat cBody::Moment() const {
+	return cpBodyGetMoment( mBody );
+}
+
+void cBody::Moment( const cpFloat& i ) {
+	cpBodySetMoment( mBody, i );
+}
+
+cpVect cBody::Pos() const {
+	return cpBodyGetPos( mBody );
+}
+
+void cBody::Pos( const cpVect& pos ) {
+	cpBodySetPos( mBody, pos );
+}
+
+cpVect cBody::Vel() const {
+	return cpBodyGetVel( mBody );
+}
+
+void cBody::Vel( const cpVect& vel ) {
+	cpBodySetVel( mBody, vel );
+}
+
+cpVect cBody::Force() const {
+	return cpBodyGetForce( mBody );
+}
+
+void cBody::Force( const cpVect& force ) {
+	cpBodySetForce( mBody, force );
+}
+
+cpFloat cBody::Angle() const {
+	return cpBodyGetAngle( mBody );
+}
+
+void cBody::Angle( const cpFloat& rads ) {
+	cpBodySetAngle( mBody, rads );
+}
+
+cpFloat cBody::AngleDeg() {
+	return Degrees( mBody->a );
+}
+
+void cBody::AngleDeg( const cpFloat& angle ) {
+	Angle( Radians( angle ) );
+}
+
+cpFloat cBody::AngVel() const {
+	return cpBodyGetAngVel( mBody );
+}
+
+void cBody::AngVel( const cpFloat& rotVel ) {
+	cpBodySetAngVel( mBody, rotVel );
+}
+
+cpVect cBody::Rot() const {
+	return cpBodyGetRot( mBody );
+}
+
+cpFloat cBody::VelLimit() const {
+	return cpBodyGetVelLimit( mBody );
+}
+
+void cBody::VelLimit( const cpFloat& speed ) {
+	cpBodySetVelLimit( mBody, speed );
+}
+
+cpFloat cBody::AngVelLimit() const {
+	return cpBodyGetAngVelLimit( mBody );
+}
+
+void cBody::AngVelLimit( const cpFloat& speed ) {
+	cpBodySetAngVelLimit( mBody, speed );
+}
+
+void cBody::Slew( cpVect pos, cpFloat dt ) {
+	cpBodySlew( mBody, pos, dt );
+}
+
+void cBody::UpdateVelocity( cpVect gravity, cpFloat damping, cpFloat dt ) {
+	cpBodyUpdateVelocity( mBody, gravity, damping, dt );
+}
+
+void cBody::UpdatePosition( cpFloat dt ) {
+	cpBodyUpdatePosition( mBody, dt );
+}
+
+cpVect cBody::Local2World( const cpVect v ) {
+	return cpBodyLocal2World( mBody, v );
+}
+
+cpVect cBody::World2Local( const cpVect v ) {
+	return cpBodyWorld2Local( mBody, v );
+}
+
+void cBody::ApplyImpulse( const cpVect j, const cpVect r ) {
+	cpBodyApplyImpulse( mBody, j, r );
+}
+
+void cBody::ResetForces() {
+	cpBodyResetForces( mBody );
+}
+
+void cBody::ApplyForce( const cpVect f, const cpVect r ) {
+	cpBodyApplyForce( mBody, f, r );
+}
+
+cpFloat cBody::KineticEnergy() {
+	return cpBodyKineticEnergy( mBody );
+}
+
+}}

src/physics/cbody.hpp

@@ -0,0 +1,97 @@
+#ifndef EE_PHYSICS_CBODY_HPP
+#define EE_PHYSICS_CBODY_HPP
+
+#include "base.hpp"
+
+namespace EE { namespace Physics {
+
+class cBody {
+	public:
+		cBody( cpBody * body );
+
+		cBody( cpFloat m, cpFloat i );
+
+		cBody();
+
+		~cBody();
+
+		void Activate();
+
+		void Sleep();
+
+		void SleepWithGroup( cBody * Group );
+
+		bool IsSleeping();
+
+		bool IsStatic();
+
+		bool IsRogue();
+
+		cpBody * Body() const;
+
+		cpFloat Mass() const;
+
+		void Mass( const cpFloat& mass );
+
+		cpFloat Moment() const;
+
+		void Moment( const cpFloat& i );
+
+		cpVect Pos() const;
+
+		void Pos( const cpVect& pos );
+
+		cpVect Vel() const;
+
+		void Vel( const cpVect& vel );
+
+		cpVect Force() const;
+
+		void Force( const cpVect& force );
+
+		cpFloat Angle() const;
+
+		void Angle( const cpFloat& rads );
+
+		cpFloat AngleDeg();
+
+		void AngleDeg( const cpFloat& angle );
+
+		cpFloat AngVel() const;
+
+		void AngVel( const cpFloat& angVel );
+
+		cpVect Rot() const;
+
+		cpFloat VelLimit() const;
+
+		void VelLimit( const cpFloat& speed );
+
+		cpFloat AngVelLimit() const;
+
+		void AngVelLimit( const cpFloat& speed );
+
+		void Slew( cpVect pos, cpFloat dt );
+
+		void UpdateVelocity( cpVect gravity, cpFloat damping, cpFloat dt );
+
+		void UpdatePosition( cpFloat dt );
+
+		cpVect Local2World( const cpVect v );
+
+		cpVect World2Local( const cpVect v );
+
+		void ApplyImpulse( const cpVect j, const cpVect r );
+
+		void ResetForces();
+
+		void ApplyForce( const cpVect f, const cpVect r );
+
+		cpFloat KineticEnergy();
+	protected:
+		cpBody *		mBody;
+};
+
+}}
+
+#endif

src/physics/constraints/cconstraint.cpp

@@ -0,0 +1,59 @@
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+cConstraint::cConstraint( cpConstraint * Constraint ) {
+	mConstraint = Constraint;
+	SetData();
+}
+
+cConstraint::cConstraint() {
+}
+
+cConstraint::~cConstraint() {
+}
+
+void cConstraint::SetData() {
+	mConstraint->data = (void*)this;
+}
+
+cpConstraint * cConstraint::Constraint() const {
+	return mConstraint;
+}
+
+cBody * cConstraint::A() {
+	return reinterpret_cast<cBody*>( mConstraint->a->data );
+}
+
+cBody * cConstraint::B() {
+	return reinterpret_cast<cBody*>( mConstraint->b->data );
+}
+
+cpFloat cConstraint::MaxForce() {
+	return mConstraint->maxForce;
+}
+
+void cConstraint::MaxForce( const cpFloat& maxforce ) {
+	mConstraint->maxForce = maxforce;
+}
+
+cpFloat cConstraint::BiasCoef() {
+	return mConstraint->biasCoef;
+}
+
+void cConstraint::BiasCoef( const cpFloat& biascoef ) {
+	mConstraint->biasCoef = biascoef;
+}
+
+cpFloat cConstraint::MaxBias() {
+	return mConstraint->maxBias;
+}
+
+void cConstraint::MaxBias( const cpFloat& maxbias ) {
+	mConstraint->maxBias = maxbias;
+}
+
+void cConstraint::Draw() {
+}
+
+}}

src/physics/constraints/cconstraint.hpp

@@ -0,0 +1,44 @@
+#ifndef EE_PHYSICS_CCONSTRAINT_HPP
+#define EE_PHYSICS_CCONSTRAINT_HPP
+
+#include "../base.hpp"
+#include "../cbody.hpp"
+
+namespace EE { namespace Physics {
+
+class cConstraint {
+	public:
+		cConstraint( cpConstraint * Constraint );
+
+		cConstraint();
+
+		~cConstraint();
+
+		cpConstraint * Constraint() const;
+
+		cBody * A();
+
+		cBody * B();
+
+		cpFloat MaxForce();
+
+		void MaxForce( const cpFloat& maxforce );
+
+		cpFloat BiasCoef();
+
+		void BiasCoef( const cpFloat& biascoef );
+
+		cpFloat MaxBias();
+
+		void MaxBias( const cpFloat& maxbias );
+
+		virtual void Draw();
+	protected:
+		cpConstraint *		mConstraint;
+
+		void SetData();
+};
+
+}}
+
+#endif

src/physics/constraints/cdampedrotaryspring.cpp

@@ -0,0 +1,38 @@
+#include "cdampedrotaryspring.hpp"
+
+namespace EE { namespace Physics {
+
+cDampedRotarySpring::cDampedRotarySpring( cBody * a, cBody * b, cpFloat restAngle, cpFloat stiffness, cpFloat damping ) {
+	mConstraint = cpDampedRotarySpringNew( a->Body(), b->Body(), restAngle, stiffness, damping );
+	SetData();
+}
+
+cpFloat cDampedRotarySpring::RestAngle() {
+	return cpDampedRotarySpringGetRestAngle( mConstraint );
+}
+
+void cDampedRotarySpring::RestAngle( const cpFloat& restangle ) {
+	cpDampedRotarySpringSetRestAngle( mConstraint, restangle );
+}
+
+cpFloat cDampedRotarySpring::Stiffness() {
+	return cpDampedRotarySpringGetStiffness( mConstraint );
+}
+
+void cDampedRotarySpring::Stiffness( const cpFloat& stiffness ) {
+	cpDampedRotarySpringSetStiffness( mConstraint, stiffness );
+}
+
+cpFloat cDampedRotarySpring::Damping() {
+	return cpDampedRotarySpringGetDamping( mConstraint );
+}
+
+void cDampedRotarySpring::Damping( const cpFloat& damping ) {
+	cpDampedRotarySpringSetDamping( mConstraint, damping );
+}
+
+void cDampedRotarySpring::Draw() {
+	// Not implemented
+}
+
+}}

src/physics/constraints/cdampedrotaryspring.hpp

@@ -0,0 +1,29 @@
+#ifndef EE_PHYSICS_CDAMPEDROTARYSPRING_HPP
+#define EE_PHYSICS_CDAMPEDROTARYSPRING_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cDampedRotarySpring : public cConstraint {
+	public:
+		cDampedRotarySpring( cBody * a, cBody * b, cpFloat restAngle, cpFloat stiffness, cpFloat damping );
+
+		cpFloat RestAngle();
+
+		void RestAngle( const cpFloat& restangle );
+
+		cpFloat Stiffness();
+
+		void Stiffness( const cpFloat& stiffness );
+
+		cpFloat Damping();
+
+		void Damping( const cpFloat& damping );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cdampedspring.cpp

@@ -0,0 +1,86 @@
+#include "cdampedspring.hpp"
+
+namespace EE { namespace Physics {
+
+cDampedSpring::cDampedSpring( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping ) {
+	mConstraint = cpDampedSpringNew( a->Body(), b->Body(), anchr1, anchr2, restLength, stiffness, damping );
+	SetData();
+}
+
+cpVect cDampedSpring::Anchr1() {
+	return cpDampedSpringGetAnchr1( mConstraint );
+}
+
+void cDampedSpring::Anchr1( const cpVect& anchr1 ) {
+	cpDampedSpringSetAnchr1( mConstraint, anchr1 );
+}
+
+cpVect cDampedSpring::Anchr2() {
+	return cpDampedSpringGetAnchr2( mConstraint );
+}
+
+void cDampedSpring::Anchr2( const cpVect& anchr2 ) {
+	cpDampedSpringSetAnchr2( mConstraint, anchr2 );
+}
+
+cpFloat cDampedSpring::RestLength() {
+	return cpDampedSpringGetRestLength( mConstraint );
+}
+
+void cDampedSpring::RestLength( const cpFloat& restlength ) {
+	cpDampedSpringSetRestLength( mConstraint, restlength );
+}
+
+cpFloat cDampedSpring::Stiffness() {
+	return cpDampedSpringGetStiffness( mConstraint );
+}
+
+void cDampedSpring::Stiffness( const cpFloat& stiffness ) {
+	cpDampedSpringSetStiffness( mConstraint, stiffness );
+}
+
+cpFloat cDampedSpring::Damping() {
+	return cpDampedSpringGetDamping( mConstraint );
+}
+
+void cDampedSpring::Damping( const cpFloat& damping ) {
+	cpDampedSpringSetDamping( mConstraint, damping );
+}
+
+void cDampedSpring::Draw() {
+	cpDampedSpring * spring = (cpDampedSpring*)mConstraint;
+	cpBody * body_a = mConstraint->a;
+	cpBody * body_b = mConstraint->b;
+
+	cpVect a = cpvadd(body_a->p, cpvrotate(spring->anchr1, body_a->rot));
+	cpVect b = cpvadd(body_b->p, cpvrotate(spring->anchr2, body_b->rot));
+
+	glPointSize(5.0f);
+	glBegin(GL_POINTS); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+
+	cpVect delta = cpvsub(b, a);
+
+	glVertexPointer(2, GL_FLOAT, 0, springVAR);
+	glPushMatrix(); {
+		GLfloat x = a.x;
+		GLfloat y = a.y;
+		GLfloat cos = delta.x;
+		GLfloat sin = delta.y;
+		GLfloat s = 1.0f/cpvlength(delta);
+
+		const GLfloat matrix[] = {
+				 cos,    sin, 0.0f, 0.0f,
+			-sin*s,  cos*s, 0.0f, 0.0f,
+				0.0f,   0.0f, 1.0f, 0.0f,
+					 x,      y, 0.0f, 1.0f,
+		};
+
+		glMultMatrixf(matrix);
+		glDrawArrays(GL_LINE_STRIP, 0, springVAR_count);
+	} glPopMatrix();
+}
+
+}}

src/physics/constraints/cdampedspring.hpp

@@ -0,0 +1,37 @@
+#ifndef EE_PHYSICS_CDAMPEDSPRING_HPP
+#define EE_PHYSICS_CDAMPEDSPRING_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cDampedSpring : public cConstraint {
+	public:
+		cDampedSpring( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping );
+
+		cpVect Anchr1();
+
+		void Anchr1( const cpVect& anchr1 );
+
+		cpVect Anchr2();
+
+		void Anchr2( const cpVect& anchr2 );
+
+		cpFloat RestLength();
+
+		void RestLength( const cpFloat& restlength );
+
+		cpFloat Stiffness();
+
+		void Stiffness( const cpFloat& stiffness );
+
+		cpFloat Damping();
+
+		void Damping( const cpFloat& damping );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cgearjoint.cpp

@@ -0,0 +1,30 @@
+#include "cgearjoint.hpp"
+
+namespace EE { namespace Physics {
+
+cGearJoint::cGearJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratio ) {
+	mConstraint = cpGearJointNew( a->Body(), b->Body(), phase, ratio );
+	SetData();
+}
+
+cpFloat cGearJoint::Phase() {
+	return cpGearJointGetPhase( mConstraint );
+}
+
+void cGearJoint::Phase( const cpFloat& phase ) {
+	cpGearJointSetPhase( mConstraint, phase );
+}
+
+cpFloat cGearJoint::Ratio() {
+	return cpGearJointGetRatio( mConstraint );
+}
+
+void cGearJoint::Ratio( const cpFloat& ratio ) {
+	cpGearJointSetRatio( mConstraint, ratio );
+}
+
+void cGearJoint::Draw() {
+	// Not implemented
+}
+
+}}

src/physics/constraints/cgearjoint.hpp

@@ -0,0 +1,25 @@
+#ifndef EE_PHYSICS_CGEARJOINT_HPP
+#define EE_PHYSICS_CGEARJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cGearJoint : public cConstraint {
+	public:
+		cGearJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratio );
+
+		cpFloat Phase();
+
+		void Phase( const cpFloat& phase );
+
+		cpFloat Ratio();
+
+		void Ratio( const cpFloat& ratio );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cgroovejoint.cpp

@@ -0,0 +1,54 @@
+#include "cgroovejoint.hpp"
+
+namespace EE { namespace Physics {
+
+cGrooveJoint::cGrooveJoint( cBody * a, cBody * b, cpVect groove_a, cpVect groove_b, cpVect anchr2 ) {
+	mConstraint = cpGrooveJointNew( a->Body(), b->Body(), groove_a, groove_b, anchr2 );
+	SetData();
+}
+
+cpVect cGrooveJoint::Anchr2() {
+	return cpGrooveJointGetAnchr2( mConstraint );
+}
+
+void cGrooveJoint::Anchr2( const cpVect& anchr2 ) {
+	cpGrooveJointSetAnchr2( mConstraint, anchr2 );
+}
+
+cpVect cGrooveJoint::GrooveA() {
+	return cpGrooveJointGetGrooveA( mConstraint );
+}
+
+void cGrooveJoint::GrooveA( const cpVect& groove_a ) {
+	cpGrooveJointSetGrooveA( mConstraint, groove_a );
+}
+
+cpVect cGrooveJoint::GrooveB() {
+	return cpGrooveJointGetGrooveB( mConstraint );
+}
+
+void cGrooveJoint::GrooveB( const cpVect& groove_b ) {
+	cpGrooveJointSetGrooveB( mConstraint, groove_b );
+}
+
+void cGrooveJoint::Draw() {
+	cpGrooveJoint *joint = (cpGrooveJoint *)mConstraint;
+	cpBody * body_a = mConstraint->a;
+	cpBody * body_b = mConstraint->b;
+
+	cpVect a = cpvadd(body_a->p, cpvrotate(joint->grv_a, body_a->rot));
+	cpVect b = cpvadd(body_a->p, cpvrotate(joint->grv_b, body_a->rot));
+	cpVect c = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot));
+
+	glPointSize(5.0f);
+	glBegin(GL_POINTS); {
+		glVertex2f(c.x, c.y);
+	} glEnd();
+
+	glBegin(GL_LINES); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+}
+
+}}

src/physics/constraints/cgroovejoint.hpp

@@ -0,0 +1,29 @@
+#ifndef EE_PHYSICS_CGROOVEJOINT_HPP
+#define EE_PHYSICS_CGROOVEJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cGrooveJoint : public cConstraint {
+	public:
+		cGrooveJoint( cBody * a, cBody * b, cpVect groove_a, cpVect groove_b, cpVect anchr2 );
+
+		cpVect Anchr2();
+
+		void Anchr2( const cpVect& anchr2 );
+
+		cpVect GrooveA();
+
+		void GrooveA( const cpVect& groove_a );
+
+		cpVect GrooveB();
+
+		void GrooveB( const cpVect& groove_b );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cpinjoint.cpp

@@ -0,0 +1,54 @@
+#include "cpinjoint.hpp"
+
+namespace EE { namespace Physics {
+
+cPinJoint::cPinJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ) {
+	mConstraint = cpPinJointNew( a->Body(), b->Body(), anchr1, anchr2 );
+	SetData();
+}
+
+cpVect cPinJoint::Anchr1() {
+	return cpPinJointGetAnchr1( mConstraint );
+}
+
+void cPinJoint::Anchr1( const cpVect& anchr1 ) {
+	cpPinJointSetAnchr1( mConstraint, anchr1 );
+}
+
+cpVect cPinJoint::Anchr2() {
+	return cpPinJointGetAnchr2( mConstraint );
+}
+
+void cPinJoint::Anchr2( const cpVect& anchr2 ) {
+	cpPinJointSetAnchr2( mConstraint, anchr2 );
+}
+
+cpFloat cPinJoint::Dist() {
+	return cpPinJointGetDist( mConstraint );
+}
+
+void cPinJoint::Dist( const cpFloat& dist ) {
+	cpPinJointSetDist( mConstraint, dist );
+}
+
+void cPinJoint::Draw() {
+	cpPinJoint *joint = (cpPinJoint *)mConstraint;
+	cpBody * body_a = mConstraint->a;
+	cpBody * body_b = mConstraint->b;
+
+	cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot));
+	cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot));
+
+	glPointSize(5.0f);
+	glBegin(GL_POINTS); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+
+	glBegin(GL_LINES); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+}
+
+}}

src/physics/constraints/cpinjoint.hpp

@@ -0,0 +1,29 @@
+#ifndef EE_PHYSICS_CPINJOINT_HPP
+#define EE_PHYSICS_CPINJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cPinJoint : public cConstraint {
+	public:
+		cPinJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 );
+
+		cpVect Anchr1();
+
+		void Anchr1( const cpVect& anchr1 );
+
+		cpVect Anchr2();
+
+		void Anchr2( const cpVect& anchr2 );
+
+		cpFloat Dist();
+
+		void Dist( const cpFloat& dist );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cpivotjoint.cpp

@@ -0,0 +1,46 @@
+#include "cpivotjoint.hpp"
+
+namespace EE { namespace Physics {
+
+cPivotJoint::cPivotJoint( cBody * a, cBody * b, cpVect pivot ) {
+	mConstraint = cpPivotJointNew( a->Body(), b->Body(), pivot );
+	SetData();
+}
+
+cPivotJoint::cPivotJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ) {
+	mConstraint = cpPivotJointNew2( a->Body(), b->Body(), anchr1, anchr2 );
+	SetData();
+}
+
+cpVect cPivotJoint::Anchr1() {
+	return cpPivotJointGetAnchr1( mConstraint );
+}
+
+void cPivotJoint::Anchr1( const cpVect& anchr1 ) {
+	cpPivotJointSetAnchr1( mConstraint, anchr1 );
+}
+
+cpVect cPivotJoint::Anchr2() {
+	return cpPivotJointGetAnchr2( mConstraint );
+}
+
+void cPivotJoint::Anchr2( const cpVect& anchr2 ) {
+	cpPivotJointSetAnchr2( mConstraint, anchr2 );
+}
+
+void cPivotJoint::Draw() {
+	cpBody * body_a = mConstraint->a;
+	cpBody * body_b = mConstraint->b;
+	cpPivotJoint *joint = (cpPivotJoint *)mConstraint;
+
+	cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot));
+	cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot));
+
+	glPointSize(10.0f);
+	glBegin(GL_POINTS); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+}
+
+}}

src/physics/constraints/cpivotjoint.hpp

@@ -0,0 +1,27 @@
+#ifndef EE_PHYSICS_CPIVOTJOINT_HPP
+#define EE_PHYSICS_CPIVOTJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cPivotJoint : public cConstraint {
+	public:
+		cPivotJoint( cBody * a, cBody * b, cpVect pivot );
+
+		cPivotJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 );
+
+		cpVect Anchr1();
+
+		void Anchr1( const cpVect& anchr1 );
+
+		cpVect Anchr2();
+
+		void Anchr2( const cpVect& anchr2 );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cratchetjoint.cpp

@@ -0,0 +1,38 @@
+#include "cratchetjoint.hpp"
+
+namespace EE { namespace Physics {
+
+cRatchetJoint::cRatchetJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratchet ) {
+	mConstraint = cpRatchetJointNew( a->Body(), b->Body(), phase, ratchet );
+	SetData();
+}
+
+cpFloat cRatchetJoint::Angle() {
+	return cpRatchetJointGetAngle( mConstraint );
+}
+
+void cRatchetJoint::Angle( const cpFloat& angle ) {
+	cpRatchetJointSetAngle( mConstraint, angle );
+}
+
+cpFloat cRatchetJoint::Phase() {
+	return cpRatchetJointGetPhase( mConstraint );
+}
+
+void cRatchetJoint::Phase( const cpFloat& phase ) {
+	cpRatchetJointSetPhase( mConstraint, phase );
+}
+
+cpFloat cRatchetJoint::Ratchet() {
+	return cpRatchetJointGetRatchet( mConstraint );
+}
+
+void cRatchetJoint::Ratchet( const cpFloat& ratchet ) {
+	cpRatchetJointSetRatchet( mConstraint, ratchet );
+}
+
+void cRatchetJoint::Draw() {
+	// Not implemented
+}
+
+}}

src/physics/constraints/cratchetjoint.hpp

@@ -0,0 +1,29 @@
+#ifndef EE_PHYSICS_CRATCHETJOINT_HPP
+#define EE_PHYSICS_CRATCHETJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cRatchetJoint : public cConstraint {
+	public:
+		cRatchetJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratchet );
+
+		cpFloat Angle();
+
+		void Angle( const cpFloat& angle );
+
+		cpFloat Phase();
+
+		void Phase( const cpFloat& phase );
+
+		cpFloat Ratchet();
+
+		void Ratchet( const cpFloat& ratchet );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/crotarylimitjoint.cpp

@@ -0,0 +1,30 @@
+#include "crotarylimitjoint.hpp"
+
+namespace EE { namespace Physics {
+
+cRotaryLimitJoint::cRotaryLimitJoint( cBody * a, cBody * b, cpFloat min, cpFloat max ) {
+	mConstraint = cpRotaryLimitJointNew( a->Body(), b->Body(), min, max );
+	SetData();
+}
+
+cpFloat cRotaryLimitJoint::Min() {
+	return cpRotaryLimitJointGetMin( mConstraint );
+}
+
+void cRotaryLimitJoint::Min( const cpFloat& min ) {
+	cpRotaryLimitJointSetMin( mConstraint, min );
+}
+
+cpFloat cRotaryLimitJoint::Max() {
+	return cpRotaryLimitJointGetMax( mConstraint );
+}
+
+void cRotaryLimitJoint::Max( const cpFloat& max ) {
+	cpRotaryLimitJointSetMax( mConstraint, max );
+}
+
+void cRotaryLimitJoint::Draw() {
+	// Not implemented
+}
+
+}}

src/physics/constraints/crotarylimitjoint.hpp

@@ -0,0 +1,25 @@
+#ifndef EE_PHYSICS_CROTARYLIMITJOINT_HPP
+#define EE_PHYSICS_CROTARYLIMITJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cRotaryLimitJoint : public cConstraint {
+	public:
+		cRotaryLimitJoint( cBody * a, cBody * b, cpFloat min, cpFloat max );
+
+		cpFloat Min();
+
+		void Min( const cpFloat& min );
+
+		cpFloat Max();
+
+		void Max( const cpFloat& max );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/csimplemotor.cpp

@@ -0,0 +1,22 @@
+#include "csimplemotor.hpp"
+
+namespace EE { namespace Physics {
+
+cSimpleMotor::cSimpleMotor( cBody * a, cBody * b, cpFloat rate ) {
+	mConstraint = cpSimpleMotorNew( a->Body(), b->Body(), rate );
+	SetData();
+}
+
+cpFloat cSimpleMotor::Rate() {
+	return cpSimpleMotorGetRate( mConstraint );
+}
+
+void cSimpleMotor::Rate( const cpFloat& rate ) {
+	cpSimpleMotorSetRate( mConstraint, rate );
+}
+
+void cSimpleMotor::Draw() {
+	// Not implemented
+}
+
+}}

src/physics/constraints/csimplemotor.hpp

@@ -0,0 +1,21 @@
+#ifndef EE_PHYSICS_CSIMPLEMOTOR_HPP
+#define EE_PHYSICS_CSIMPLEMOTOR_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cSimpleMotor : public cConstraint {
+	public:
+		cSimpleMotor( cBody * a, cBody * b, cpFloat rate );
+
+		cpFloat Rate();
+
+		void Rate( const cpFloat& rate );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/constraints/cslidejoint.cpp

@@ -0,0 +1,62 @@
+#include "cslidejoint.hpp"
+
+namespace EE { namespace Physics {
+
+cSlideJoint::cSlideJoint( cBody * a, cBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max ) {
+	mConstraint = cpSlideJointNew( a->Body(), b->Body(), anchr1, anchr2, min, max );
+	SetData();
+}
+
+cpVect cSlideJoint::Anchr1() {
+	return cpSlideJointGetAnchr1( mConstraint );
+}
+
+void cSlideJoint::Anchr1( const cpVect& anchr1 ) {
+	cpSlideJointSetAnchr1( mConstraint, anchr1 );
+}
+
+cpVect cSlideJoint::Anchr2() {
+	return cpSlideJointGetAnchr2( mConstraint );
+}
+
+void cSlideJoint::Anchr2( const cpVect& anchr2 ) {
+	cpSlideJointSetAnchr2( mConstraint, anchr2 );
+}
+
+cpFloat cSlideJoint::Min() {
+	return cpSlideJointGetMin( mConstraint );
+}
+
+void cSlideJoint::Min( const cpFloat& min ) {
+	cpSlideJointSetMin( mConstraint, min );
+}
+
+cpFloat cSlideJoint::Max() {
+	return cpSlideJointGetMax( mConstraint );
+}
+
+void cSlideJoint::Max( const cpFloat& max ) {
+	cpSlideJointSetMax( mConstraint, max );
+}
+
+void cSlideJoint::Draw() {
+	cpBody * body_a = mConstraint->a;
+	cpBody * body_b = mConstraint->b;
+	cpSlideJoint *joint = (cpSlideJoint *)mConstraint;
+
+	cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot));
+	cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot));
+
+	glPointSize(5.0f);
+	glBegin(GL_POINTS); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+
+	glBegin(GL_LINES); {
+		glVertex2f(a.x, a.y);
+		glVertex2f(b.x, b.y);
+	} glEnd();
+}
+
+}}

src/physics/constraints/cslidejoint.hpp

@@ -0,0 +1,33 @@
+#ifndef EE_PHYSICS_CSLIDEJOINT_HPP
+#define EE_PHYSICS_CSLIDEJOINT_HPP
+
+#include "cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cSlideJoint : public cConstraint {
+	public:
+		cSlideJoint( cBody * a, cBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max );
+
+		cpVect Anchr1();
+
+		void Anchr1( const cpVect& anchr1 );
+
+		cpVect Anchr2();
+
+		void Anchr2( const cpVect& anchr2 );
+
+		cpFloat Min();
+
+		void Min( const cpFloat& min );
+
+		cpFloat Max();
+
+		void Max( const cpFloat& max );
+
+		virtual void Draw();
+};
+
+}}
+
+#endif

src/physics/cphysicsmanager.cpp

@@ -0,0 +1,48 @@
+#include "cphysicsmanager.hpp"
+
+namespace EE { namespace Physics {
+
+cPhysicsManager::cPhysicsManager() {
+	cpInitChipmunk();
+}
+
+cPhysicsManager::~cPhysicsManager() {
+}
+
+const cpFloat& cPhysicsManager::BiasCoef() const {
+	return cp_bias_coef;
+}
+
+void cPhysicsManager::BiasCoef( const cpFloat& biasCoef ) {
+	cp_bias_coef = biasCoef;
+}
+
+const cpFloat& cPhysicsManager::ConstraintBiasCoef() const {
+	return cp_constraint_bias_coef;
+}
+
+void cPhysicsManager::ConstraintBiasCoef( const cpFloat& constraintBiasCoef ) {
+	cp_constraint_bias_coef = constraintBiasCoef;
+}
+
+const cpTimestamp& cPhysicsManager::ContactPersistence() const {
+	return cp_contact_persistence;
+}
+
+void cPhysicsManager::ContactPersistence( const cpTimestamp& timestamp ) {
+	cp_contact_persistence = timestamp;
+}
+
+const cpFloat& cPhysicsManager::CollisionSlop() const {
+	return cp_collision_slop;
+}
+
+void cPhysicsManager::CollisionSlop( const cpFloat& slop ) {
+	cp_collision_slop = slop;
+}
+
+cPhysicsManager::cDrawSpaceOptions * cPhysicsManager::GetDrawOptions() {
+	return &mOptions;
+}
+
+}}

src/physics/cphysicsmanager.hpp

@@ -0,0 +1,57 @@
+#ifndef EE_PHYSICS_PHYSICSMANAGER_HPP
+#define EE_PHYSICS__PHYSICSMANAGER_HPP
+
+#include "base.hpp"
+
+namespace EE { namespace Physics {
+
+class EE_API cPhysicsManager : public tSingleton<cPhysicsManager> {
+	friend class tSingleton<cPhysicsManager>;
+	public:
+		class cDrawSpaceOptions {
+			public:
+				cDrawSpaceOptions() :
+					DrawHash( false ),
+					DrawBBs( false ),
+					DrawShapes( true ),
+					CollisionPointSize( 4.0f ),
+					BodyPointSize( 0.0f ),
+					LineThickness( 1.5f )
+				{}
+
+				int		DrawHash;
+				int		DrawBBs;
+				int		DrawShapes;
+				cpFloat CollisionPointSize;
+				cpFloat BodyPointSize;
+				cpFloat LineThickness;
+		};
+
+		cPhysicsManager();
+
+		~cPhysicsManager();
+
+		const cpFloat& BiasCoef() const;
+
+		void BiasCoef( const cpFloat& biasCoef );
+
+		const cpFloat& ConstraintBiasCoef() const;
+
+		void ConstraintBiasCoef( const cpFloat& constraintBiasCoef );
+
+		const cpTimestamp& ContactPersistence() const;
+
+		void ContactPersistence( const cpTimestamp& timestamp );
+
+		const cpFloat& CollisionSlop() const;
+
+		void CollisionSlop( const cpFloat& slop );
+
+		cPhysicsManager::cDrawSpaceOptions * GetDrawOptions();
+	protected:
+		cDrawSpaceOptions	mOptions;
+};
+
+}}
+
+#endif

src/physics/cshape.cpp

@@ -0,0 +1,143 @@
+#include "cshape.hpp"
+#include "cshapecircle.hpp"
+#include "cshapesegment.hpp"
+#include "cshapepoly.hpp"
+
+namespace EE { namespace Physics {
+
+void cShape::ResetShapeIdCounter() {
+	cpResetShapeIdCounter();
+}
+
+cShape::cShape() {
+}
+
+cShape::~cShape() {
+}
+
+void cShape::SetData() {
+	mShape->data	= (void*)this;
+}
+
+cpShape * cShape::Shape() const {
+	return mShape;
+}
+
+cpBB cShape::CacheBB() {
+	return cpShapeCacheBB( mShape );
+}
+
+cpBB cShape::Update( cpVect pos, cpVect rot ) {
+	return cpShapeUpdate( mShape, pos, rot );
+}
+
+bool cShape::PointQuery( cpVect p ) {
+	return 0 != cpShapePointQuery( mShape, p );
+}
+
+cBody * cShape::Body() const {
+	return reinterpret_cast<cBody*>( mShape->body->data );
+}
+
+void cShape::Body( cBody * body ) {
+	mShape->body = body->Body();
+}
+
+cpBB cShape::BB() const {
+	return mShape->bb;
+}
+
+void cShape::BB( const cpBB& bb ) {
+	mShape->bb = bb;
+}
+
+bool cShape::Sensor() {
+	return 0 != mShape->sensor;
+}
+
+void cShape::Sensor( const bool& sensor ) {
+	mShape->sensor = sensor ? 1 : 0;
+}
+
+cpFloat cShape::e() const {
+	return mShape->e;
+}
+
+void cShape::e( const cpFloat& e ) {
+	mShape->e = e;
+}
+
+cpFloat cShape::u() const {
+	return mShape->u;
+}
+
+void cShape::u( const cpFloat& u ) {
+	mShape->u = u;
+}
+
+cpVect cShape::SurfaceVel() const {
+	return mShape->surface_v;
+}
+
+void cShape::SurfaceVel( const cpVect& vel ) {
+	mShape->surface_v = vel;
+}
+
+cpCollisionType cShape::CollisionType()	 const {
+	return mShape->collision_type;
+}
+
+void cShape::CollisionType( const cpCollisionType& type ) {
+	mShape->collision_type = type;
+}
+
+cpGroup cShape::Group() const {
+	return mShape->group;
+}
+
+void cShape::Group( const cpGroup& group ) {
+	mShape->group = group;
+}
+
+cpLayers cShape::Layers() const {
+	return mShape->layers;
+}
+
+void cShape::Layers( const cpLayers& layers ) {
+	mShape->layers = layers;
+}
+
+cpShapeType cShape::Type() const {
+	return mShape->klass->type;
+}
+
+cShapePoly * cShape::GetAsPoly() {
+	eeASSERT( Type() == CP_POLY_SHAPE );
+
+	return reinterpret_cast<cShapePoly*>( this );
+}
+
+cShapeCircle * cShape::GetAsCircle() {
+	eeASSERT( Type() == CP_CIRCLE_SHAPE );
+
+	return reinterpret_cast<cShapeCircle*>( this );
+}
+
+cShapeSegment * cShape::GetAsSegment() {
+	eeASSERT( Type() == CP_SEGMENT_SHAPE );
+
+	return reinterpret_cast<cShapeSegment*>( this );
+}
+
+void cShape::DrawBB() {
+	glBegin(GL_LINE_LOOP);
+
+	glVertex2f(mShape->bb.l, mShape->bb.b);
+	glVertex2f(mShape->bb.l, mShape->bb.t);
+	glVertex2f(mShape->bb.r, mShape->bb.t);
+	glVertex2f(mShape->bb.r, mShape->bb.b);
+
+	glEnd();
+}
+
+}}

src/physics/cshape.hpp

@@ -0,0 +1,86 @@
+#ifndef EE_PHYSICS_CSHAPE_HPP
+#define EE_PHYSICS_CSHAPE_HPP
+
+#include "base.hpp"
+#include "cbody.hpp"
+
+namespace EE { namespace Physics {
+
+class cShapeCircle;
+class cShapeSegment;
+class cShapePoly;
+class cSpace;
+
+class cShape {
+	public:
+		static void ResetShapeIdCounter();
+
+		cpShape * Shape() const;
+
+		~cShape();
+
+		cBody * Body() const;
+
+		void Body( cBody * body );
+
+		cpBB BB() const;
+
+		void BB( const cpBB& bb );
+
+		bool Sensor();
+
+		void Sensor( const bool& sensor );
+
+		cpFloat e() const;
+
+		void e( const cpFloat& e );
+
+		cpFloat u() const;
+
+		void u( const cpFloat& u );
+
+		cpVect SurfaceVel() const;
+
+		void SurfaceVel( const cpVect& vel );
+
+		cpCollisionType CollisionType()	 const;
+
+		void CollisionType( const cpCollisionType& type );
+
+		cpGroup Group() const;
+
+		void Group( const cpGroup& group );
+
+		cpLayers Layers() const;
+
+		void Layers( const cpLayers& layers );
+
+		cpBB CacheBB();
+
+		cpBB Update( cpVect pos, cpVect rot );
+
+		bool PointQuery( cpVect p );
+
+		cpShapeType Type() const;
+
+		cShapePoly * GetAsPoly();
+
+		cShapeCircle * GetAsCircle();
+
+		cShapeSegment * GetAsSegment();
+
+		virtual void Draw( cSpace * space ) = 0;
+
+		virtual void DrawBB();
+	protected:
+		cShape();
+
+		cpShape *		mShape;
+		cBody *			mBody;
+
+		void SetData();
+};
+
+}}
+
+#endif

src/physics/cshapecircle.cpp

@@ -0,0 +1,57 @@
+#include "cshapecircle.hpp"
+#include "cspace.hpp"
+
+namespace EE { namespace Physics {
+
+cShapeCircle::cShapeCircle( cBody * body, cpFloat radius, cpVect offset ) {
+	mShape	= cpCircleShapeNew( body->Body(), radius, offset );
+	SetData();
+}
+
+cpVect cShapeCircle::Offset() {
+	return cpCircleShapeGetOffset( mShape );
+}
+
+void cShapeCircle::Offset( const cpVect &offset ) {
+	cpCircleShapeSetOffset( mShape, offset );
+}
+
+cpFloat cShapeCircle::Radius() {
+	return cpCircleShapeGetRadius( mShape );
+}
+
+void cShapeCircle::Radius( const cpFloat& radius ) {
+	cpCircleShapeSetRadius( mShape, radius );
+}
+
+void cShapeCircle::Draw( cSpace * space ) {
+	cPrimitives p;
+
+	cpCircleShape * cs = (cpCircleShape*)mShape;
+
+	p.DrawCircle( cs->tc.x, cs->tc.y, cs->r );
+
+	/*glVertexPointer( 2, GL_FLOAT, 0, circleVAR );
+
+	cpCircleShape * cs = (cpCircleShape*)mShape;
+
+	glPushMatrix();
+
+	cpVect center = cs->tc;
+
+	glTranslatef( center.x, center.y, 0.0f );
+	glRotatef( space->StaticBody()->Body()->a * 180.0f / M_PI, 0.0f, 0.0f, 1.0f);
+
+	glScalef( cs->r, cs->r, 1.0f);
+
+	if(!cs->shape.sensor){
+		glColor_for_shape( mShape, space->Space() );
+		glDrawArrays(GL_TRIANGLE_FAN, 0, circleVAR_count - 1);
+	}
+
+	glColor3f(LINE_COLOR);
+	glDrawArrays(GL_LINE_STRIP, 0, circleVAR_count);
+	glPopMatrix();*/
+}
+
+}}

src/physics/cshapecircle.hpp

@@ -0,0 +1,25 @@
+#ifndef EE_PHYSICS_CSHAPECIRCLE_HPP
+#define EE_PHYSICS_CSHAPECIRCLE_HPP
+
+#include "cshape.hpp"
+
+namespace EE { namespace Physics {
+
+class cShapeCircle : public cShape {
+	public:
+		cShapeCircle( cBody * body, cpFloat radius, cpVect offset );
+
+		cpVect Offset();
+
+		void Offset( const cpVect& offset );
+
+		cpFloat Radius();
+
+		void Radius( const cpFloat& radius );
+
+		virtual void Draw( cSpace * space );
+};
+
+}}
+
+#endif

src/physics/cshapepoly.cpp

@@ -0,0 +1,52 @@
+#include "cshapepoly.hpp"
+#include "cspace.hpp"
+
+namespace EE { namespace Physics {
+
+cShapePoly::cShapePoly( cBody * body, int numVerts, cpVect *verts, cpVect offset ) {
+	mShape = cpPolyShapeNew( body->Body(), numVerts, verts, offset );
+	SetData();
+}
+
+cShapePoly::cShapePoly( cBody * body, cpFloat width, cpFloat height ) :
+	cShape()
+{
+	mShape = cpBoxShapeNew( body->Body(), width, height );
+}
+
+bool cShapePoly::Validate( const cpVect * verts, const int numVerts ) {
+	return 0 != cpPolyValidate( verts,  numVerts );
+}
+
+int cShapePoly::GetNumVerts() {
+	return cpPolyShapeGetNumVerts( mShape );
+}
+
+cpVect cShapePoly::GetVert( int idx ) {
+	return cpPolyShapeGetVert( mShape, idx );
+}
+
+void cShapePoly::SetVerts( int numVerts, cpVect *verts, cpVect offset ) {
+	cpPolyShapeSetVerts( mShape, numVerts, verts, offset );
+}
+
+void cShapePoly::Draw( cSpace * space ) {
+	cpPolyShape *poly = (cpPolyShape*)mShape;
+
+	int count = poly->numVerts;
+#if CP_USE_DOUBLES
+	glVertexPointer(2, GL_DOUBLE, 0, poly->tVerts);
+#else
+	glVertexPointer(2, GL_FLOAT, 0, poly->tVerts);
+#endif
+
+	if(!poly->shape.sensor){
+		glColor_for_shape((cpShape *)poly, space->Space());
+		glDrawArrays(GL_TRIANGLE_FAN, 0, count);
+	}
+
+	glColor3f(LINE_COLOR);
+	glDrawArrays(GL_LINE_LOOP, 0, count);
+}
+
+}}

src/physics/cshapepoly.hpp

@@ -0,0 +1,27 @@
+#ifndef EE_PHYSICS_CSHAPEPOLY_HPP
+#define EE_PHYSICS_CSHAPEPOLY_HPP
+
+#include "cshape.hpp"
+
+namespace EE { namespace Physics {
+
+class cShapePoly : public cShape {
+	public:
+		cShapePoly( cBody * body, int numVerts, cpVect *verts, cpVect offset );
+
+		cShapePoly( cBody * body, cpFloat width, cpFloat height );
+
+		static bool Validate( const cpVect * verts, const int numVerts );
+
+		int GetNumVerts();
+
+		cpVect GetVert( int idx );
+
+		void SetVerts( int numVerts, cpVect *verts, cpVect offset );
+
+		virtual void Draw( cSpace * space );
+};
+
+}}
+
+#endif

src/physics/cshapesegment.cpp

@@ -0,0 +1,91 @@
+#include "cshapesegment.hpp"
+#include "cspace.hpp"
+
+namespace EE { namespace Physics {
+
+cShapeSegment::cShapeSegment( cBody * body, cpVect a, cpVect b, cpFloat radius ) {
+	mShape = cpSegmentShapeNew( body->Body(), a, b, radius );
+	SetData();
+}
+
+cpVect cShapeSegment::A() {
+	return cpSegmentShapeGetA( mShape );
+}
+
+cpVect cShapeSegment::B() {
+	return cpSegmentShapeGetB( mShape );
+}
+
+cpVect cShapeSegment::Normal() {
+	return cpSegmentShapeGetNormal( mShape );
+}
+
+cpFloat cShapeSegment::Radius() {
+	return cpSegmentShapeGetRadius( mShape );
+}
+
+void cShapeSegment::Radius( const cpFloat& radius ) {
+	cpSegmentShapeSetRadius( mShape, radius );
+}
+
+void cShapeSegment::Endpoints( const cpVect& a, const cpVect& b ) {
+	cpSegmentShapeSetEndpoints( mShape, a, b );
+}
+
+bool cShapeSegment::Query( cpVect a, cpVect b, cpSegmentQueryInfo * info ) {
+	return 0 != cpShapeSegmentQuery( mShape, a, b, info );
+}
+
+cpVect cShapeSegment::HitPoint( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ) {
+	return cpSegmentQueryHitPoint( start, end, info );
+}
+
+cpFloat cShapeSegment::HitDist( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ) {
+	return cpSegmentQueryHitDist( start, end, info );
+}
+
+void cShapeSegment::Draw( cSpace * space ) {
+	cPrimitives p;
+
+	cpSegmentShape * seg = (cpSegmentShape *)mShape;
+	cpVect a = seg->ta;
+	cpVect b = seg->tb;
+
+	p.DrawLine( eeVector2f( a.x, a.y ), eeVector2f( b.x, b.y ) );
+	/*cpSegmentShape * seg = (cpSegmentShape *)mShape;
+
+	cpVect a = seg->ta;
+	cpVect b = seg->tb;
+
+	if(seg->r){
+		glVertexPointer(3, GL_FLOAT, 0, pillVAR);
+		glPushMatrix(); {
+			cpVect d = cpvsub(b, a);
+			cpVect r = cpvmult(d, seg->r/cpvlength(d));
+
+			const GLfloat matrix[] = {
+				 r.x, r.y, 0.0f, 0.0f,
+				-r.y, r.x, 0.0f, 0.0f,
+				 d.x, d.y, 0.0f, 0.0f,
+				 a.x, a.y, 0.0f, 1.0f,
+			};
+			glMultMatrixf(matrix);
+
+			if(!seg->shape.sensor){
+				glColor_for_shape((cpShape *)seg, space->Space());
+				glDrawArrays(GL_TRIANGLE_FAN, 0, pillVAR_count);
+			}
+
+			glColor3f(LINE_COLOR);
+			glDrawArrays(GL_LINE_LOOP, 0, pillVAR_count);
+		} glPopMatrix();
+	} else {
+		glColor3f(LINE_COLOR);
+		glBegin(GL_LINES); {
+			glVertex2f(a.x, a.y);
+			glVertex2f(b.x, b.y);
+		} glEnd();
+	}*/
+}
+
+}}

src/physics/cshapesegment.hpp

@@ -0,0 +1,35 @@
+#ifndef EE_PHYSICS_CSHAPESEGMENT_HPP
+#define EE_PHYSICS_CSHAPESEGMENT_HPP
+
+#include "cshape.hpp"
+
+namespace EE { namespace Physics {
+
+class cShapeSegment : public cShape {
+	public:
+		cShapeSegment( cBody * body, cpVect a, cpVect b, cpFloat radius );
+
+		cpVect A();
+
+		cpVect B();
+
+		cpVect Normal();
+
+		cpFloat Radius();
+
+		void Radius( const cpFloat& radius );
+
+		void Endpoints( const cpVect& a, const cpVect& b );
+
+		bool Query( cpVect a, cpVect b, cpSegmentQueryInfo * info );
+
+		static cpVect HitPoint( const cpVect start, const cpVect end, const cpSegmentQueryInfo info );
+
+		static cpFloat HitDist( const cpVect start, const cpVect end, const cpSegmentQueryInfo info );
+
+		virtual void Draw( cSpace * space );
+};
+
+}}
+
+#endif

src/physics/cspace.cpp

@@ -0,0 +1,238 @@
+#include "cspace.hpp"
+#include "cphysicsmanager.hpp"
+
+namespace EE { namespace Physics {
+
+cSpace::cSpace() {
+	mSpace = cpSpaceNew();
+	mSpace->data = (void*)this;
+	mStaticBody = eeNew( cBody, ( mSpace->staticBody ) );
+}
+
+cSpace::~cSpace() {
+	cpSpaceFreeChildren( mSpace );
+	cpSpaceFree( mSpace );
+
+	std::list<cConstraint*>::iterator itc = mConstraints.begin();
+	for ( ; itc != mConstraints.end(); itc++ )
+		eeSAFE_DELETE( *itc );
+
+	std::list<cShape*>::iterator its = mShapes.begin();
+	for ( ; its != mShapes.end(); its++ )
+		eeSAFE_DELETE( *its );
+
+	std::list<cBody*>::iterator itb = mBodys.begin();
+	for ( ; itb != mBodys.end(); itb++ )
+		eeSAFE_DELETE( *itb );
+
+	eeSAFE_DELETE( mStaticBody );
+}
+
+void cSpace::Update( const cpFloat& dt ) {
+	cpSpaceStep( mSpace, dt );
+}
+
+void cSpace::Update() {
+	Update( cEngine::instance()->Elapsed() / 1000 );
+}
+
+void cSpace::ResizeStaticHash( cpFloat dim, int count ) {
+	cpSpaceResizeStaticHash( mSpace, dim, count );
+}
+
+void cSpace::ResizeActiveHash( cpFloat dim, int count ) {
+	cpSpaceResizeActiveHash( mSpace, dim, count );
+}
+
+void cSpace::RehashStatic() {
+	cpSpaceRehashStatic( mSpace );
+}
+
+const int& cSpace::Iterations() const {
+	return mSpace->iterations;
+}
+
+void cSpace::Iterations( const int& iterations ) {
+	mSpace->iterations = iterations;
+}
+
+const cpVect& cSpace::Gravity() const {
+	return mSpace->gravity;
+}
+
+void cSpace::Gravity( const cpVect& gravity ) {
+	mSpace->gravity = gravity;
+}
+
+const cpFloat& cSpace::Damping() const {
+	return mSpace->damping;
+}
+
+void cSpace::Damping( const cpFloat& damping ) {
+	mSpace->damping = damping;
+}
+
+const cpFloat& cSpace::IdleSpeedThreshold() const {
+	return mSpace->idleSpeedThreshold;
+}
+
+void cSpace::IdleSpeedThreshold( const cpFloat& idleSpeedThreshold ) {
+	mSpace->idleSpeedThreshold = idleSpeedThreshold;
+}
+
+const cpFloat& cSpace::SleepTimeThreshold() const {
+	return mSpace->sleepTimeThreshold;
+}
+
+void cSpace::SleepTimeThreshold( const cpFloat& sleepTimeThreshold ) {
+	mSpace->sleepTimeThreshold = sleepTimeThreshold;
+}
+
+cBody * cSpace::StaticBody() const {
+	return mStaticBody;
+}
+
+cShape * cSpace::AddShape( cShape * shape ) {
+	cpSpaceAddShape( mSpace, shape->Shape() );
+	mShapes.push_back( shape );
+	return shape;
+}
+
+cShape * cSpace::AddStaticShape( cShape * shape ) {
+	cpSpaceAddStaticShape( mSpace, shape->Shape() );
+	mShapes.push_back( shape );
+	return shape;
+}
+
+cBody * cSpace::AddBody( cBody * body ) {
+	cpSpaceAddBody( mSpace, body->Body() );
+	mBodys.push_back( body );
+	return body;
+}
+
+cConstraint * cSpace::AddConstraint( cConstraint * constraint ) {
+	cpSpaceAddConstraint( mSpace, constraint->Constraint() );
+	mConstraints.push_back( constraint );
+	return constraint;
+}
+
+void cSpace::RemoveShape( cShape * shape ) {
+	cpSpaceRemoveShape( mSpace, shape->Shape() );
+	mShapes.remove( shape );
+}
+
+void cSpace::RemoveStaticShape( cShape * shape ) {
+	cpSpaceRemoveStaticShape( mSpace, shape->Shape() );
+	mShapes.remove( shape );
+}
+
+void cSpace::RemoveBody( cBody * body ) {
+	cpSpaceRemoveBody( mSpace, body->Body() );
+	mBodys.remove( body );
+}
+
+void cSpace::RemoveConstraint( cConstraint * constraint ) {
+	cpSpaceRemoveConstraint( mSpace, constraint->Constraint() );
+	mConstraints.remove( constraint );
+}
+
+cpSpace * cSpace::Space() const {
+	return mSpace;
+}
+
+void cSpace::ActivateShapesTouchingShape( cShape * shape ) {
+	cpSpaceActivateShapesTouchingShape( mSpace, shape->Shape() );
+}
+
+void cSpace::RehashShape( cShape * shape ) {
+	cpSpaceRehashShape( mSpace, shape->Shape() );
+}
+
+static void drawObject( cpShape * shape, cpSpace * space ) {
+	reinterpret_cast<cShape*> ( shape->data )->Draw( reinterpret_cast<cSpace*>( space->data ) );
+}
+
+static void drawBB( cpShape *shape, void * unused ) {
+	reinterpret_cast<cShape*> ( shape->data )->DrawBB();
+}
+
+static void drawConstraint( cpConstraint *constraint ) {
+	reinterpret_cast<cConstraint*> ( constraint->data )->Draw();
+}
+
+void cSpace::Draw() {
+	cPhysicsManager::cDrawSpaceOptions * options = cPhysicsManager::instance()->GetDrawOptions();
+
+	if( options->DrawHash ) {
+		glColor3f(0.5, 0.5, 0.5);
+		//cpBBTreeRenderDebug( mSpace->staticShapes );
+
+		glColor3f(0, 1, 0);
+		//cpBBTreeRenderDebug( mSpace->activeShapes );
+	}
+
+	glLineWidth( options->LineThickness );
+
+	if( options->DrawShapes ) {
+		cpSpatialIndexEach( mSpace->activeShapes, (cpSpatialIndexIterator)drawObject, mSpace );
+		cpSpatialIndexEach( mSpace->staticShapes, (cpSpatialIndexIterator)drawObject, mSpace );
+	}
+
+	glLineWidth( 1.0f );
+
+	if( options->DrawBBs ){
+		glColor3f( 0.3f, 0.5f, 0.3f );
+
+		cpSpatialIndexEach(mSpace->activeShapes, (cpSpatialIndexIterator)drawBB, NULL);
+		cpSpatialIndexEach(mSpace->staticShapes, (cpSpatialIndexIterator)drawBB, NULL);
+	}
+
+	cpArray * constraints = mSpace->constraints;
+
+	glColor3f(0.5f, 1.0f, 0.5f);
+
+	for( int i=0, count = constraints->num; i<count; i++ ) {
+		drawConstraint( (cpConstraint *)constraints->arr[i] );
+	}
+
+	if( options->BodyPointSize ){
+		glPointSize( options->BodyPointSize );
+
+		glBegin(GL_POINTS);
+
+		glColor3f(LINE_COLOR);
+
+		cpArray * bodies = mSpace->bodies;
+
+		for( int i=0, count = bodies->num; i<count; i++ ) {
+			cpBody * body = (cpBody *)bodies->arr[i];
+
+			glVertex2f( body->p.x, body->p.y );
+		}
+
+		 glEnd();
+	}
+
+	if( options->CollisionPointSize ){
+		glPointSize( options->CollisionPointSize );
+
+		glBegin( GL_POINTS );
+
+		cpArray * arbiters = mSpace->arbiters;
+
+		for( int i = 0; i < arbiters->num; i++ ){
+			cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
+
+			glColor3f( COLLISION_COLOR );
+
+			for(int i=0; i<arb->numContacts; i++){
+				cpVect v = arb->contacts[i].p;
+				glVertex2f( v.x, v.y );
+			}
+		}
+
+		glEnd();
+	}
+}
+
+}}

src/physics/cspace.hpp

@@ -0,0 +1,82 @@
+#ifndef EE_PHYSICS_CSPACE_HPP
+#define EE_PHYSICS_CSPACE_HPP
+
+#include "base.hpp"
+#include "cbody.hpp"
+#include "cshape.hpp"
+#include "constraints/cconstraint.hpp"
+
+namespace EE { namespace Physics {
+
+class cSpace {
+	public:
+		cSpace();
+
+		~cSpace();
+
+		void Update( const cpFloat& dt );
+
+		void Update();
+
+		cBody * StaticBody() const;
+
+		const int& Iterations() const;
+
+		void Iterations( const int& iterations );
+
+		const cpVect& Gravity() const;
+
+		void Gravity( const cpVect& gravity );
+
+		const cpFloat& Damping() const;
+
+		void Damping( const cpFloat& damping );
+
+		const cpFloat& IdleSpeedThreshold() const;
+
+		void IdleSpeedThreshold( const cpFloat& idleSpeedThreshold );
+
+		const cpFloat& SleepTimeThreshold() const;
+
+		void SleepTimeThreshold( const cpFloat& sleepTimeThreshold );
+
+		cShape * AddShape( cShape *shape );
+
+		cShape * AddStaticShape( cShape *shape );
+
+		cBody * AddBody( cBody * body );
+
+		cConstraint * AddConstraint( cConstraint * constraint );
+
+		void RemoveShape( cShape * shape);
+
+		void RemoveStaticShape( cShape * shape );
+
+		void RemoveBody( cBody * body );
+
+		void RemoveConstraint( cConstraint * constraint );
+
+		void ResizeStaticHash( cpFloat dim, int count );
+
+		void ResizeActiveHash( cpFloat dim, int count );
+
+		void RehashStatic();
+
+		void RehashShape( cShape * shape );
+
+		cpSpace * Space() const;
+
+		void ActivateShapesTouchingShape( cShape * shape );
+
+		virtual void Draw();
+	protected:
+		cpSpace *					mSpace;
+		cBody *						mStaticBody;
+		std::list<cBody*>			mBodys;
+		std::list<cShape*>			mShapes;
+		std::list<cConstraint*>		mConstraints;
+};
+
+}}
+
+#endif

src/physics/physicshelper.hpp

@@ -0,0 +1,132 @@
+#ifndef EE_PHYSICS_HELPER
+#define EE_PHYSICS_HELPER
+
+#define LINE_COLOR 1.0f, 1.0f, 1.0f
+#define COLLISION_COLOR 1.0f, 0.0f, 0.0f
+#define BODY_COLOR 0.0f, 0.0f, 1.0f
+
+static const GLfloat circleVAR[] = {
+	 0.0000f,  1.0000f,
+	 0.2588f,  0.9659f,
+	 0.5000f,  0.8660f,
+	 0.7071f,  0.7071f,
+	 0.8660f,  0.5000f,
+	 0.9659f,  0.2588f,
+	 1.0000f,  0.0000f,
+	 0.9659f, -0.2588f,
+	 0.8660f, -0.5000f,
+	 0.7071f, -0.7071f,
+	 0.5000f, -0.8660f,
+	 0.2588f, -0.9659f,
+	 0.0000f, -1.0000f,
+	-0.2588f, -0.9659f,
+	-0.5000f, -0.8660f,
+	-0.7071f, -0.7071f,
+	-0.8660f, -0.5000f,
+	-0.9659f, -0.2588f,
+	-1.0000f, -0.0000f,
+	-0.9659f,  0.2588f,
+	-0.8660f,  0.5000f,
+	-0.7071f,  0.7071f,
+	-0.5000f,  0.8660f,
+	-0.2588f,  0.9659f,
+	 0.0000f,  1.0000f,
+	 0.0f, 0.0f, // For an extra line to see the rotation.
+};
+
+static const int circleVAR_count = sizeof(circleVAR)/sizeof(GLfloat)/2;
+
+static const GLfloat pillVAR[] = {
+	 0.0000f,  1.0000f, 1.0f,
+	 0.2588f,  0.9659f, 1.0f,
+	 0.5000f,  0.8660f, 1.0f,
+	 0.7071f,  0.7071f, 1.0f,
+	 0.8660f,  0.5000f, 1.0f,
+	 0.9659f,  0.2588f, 1.0f,
+	 1.0000f,  0.0000f, 1.0f,
+	 0.9659f, -0.2588f, 1.0f,
+	 0.8660f, -0.5000f, 1.0f,
+	 0.7071f, -0.7071f, 1.0f,
+	 0.5000f, -0.8660f, 1.0f,
+	 0.2588f, -0.9659f, 1.0f,
+	 0.0000f, -1.0000f, 1.0f,
+
+	 0.0000f, -1.0000f, 0.0f,
+	-0.2588f, -0.9659f, 0.0f,
+	-0.5000f, -0.8660f, 0.0f,
+	-0.7071f, -0.7071f, 0.0f,
+	-0.8660f, -0.5000f, 0.0f,
+	-0.9659f, -0.2588f, 0.0f,
+	-1.0000f, -0.0000f, 0.0f,
+	-0.9659f,  0.2588f, 0.0f,
+	-0.8660f,  0.5000f, 0.0f,
+	-0.7071f,  0.7071f, 0.0f,
+	-0.5000f,  0.8660f, 0.0f,
+	-0.2588f,  0.9659f, 0.0f,
+	 0.0000f,  1.0000f, 0.0f,
+};
+static const int pillVAR_count = sizeof(pillVAR)/sizeof(GLfloat)/3;
+
+inline void glColor_from_pointer(void *ptr) {
+	unsigned long val = (long)ptr;
+
+	// hash the pointer up nicely
+	val = (val+0x7ed55d16) + (val<<12);
+	val = (val^0xc761c23c) ^ (val>>19);
+	val = (val+0x165667b1) + (val<<5);
+	val = (val+0xd3a2646c) ^ (val<<9);
+	val = (val+0xfd7046c5) + (val<<3);
+	val = (val^0xb55a4f09) ^ (val>>16);
+
+	GLubyte r = (val>>0) & 0xFF;
+	GLubyte g = (val>>8) & 0xFF;
+	GLubyte b = (val>>16) & 0xFF;
+
+	GLubyte max = r>g ? (r>b ? r : b) : (g>b ? g : b);
+
+	const int mult = 127;
+	const int add = 63;
+	r = (r*mult)/max + add;
+	g = (g*mult)/max + add;
+	b = (b*mult)/max + add;
+
+	glColor3ub(r, g, b);
+}
+
+inline void glColor_for_shape( cpShape *shape, cpSpace *space ) {
+	cpBody *body = shape->body;
+	if(body){
+		if(cpBodyIsSleeping(body)){
+			GLfloat v = 0.25f;
+			glColor3f(v,v,v);
+			return;
+		} else if(body->node.idleTime > space->sleepTimeThreshold) {
+			GLfloat v = 0.9f;
+			glColor3f(v,v,v);
+			return;
+		}
+	}
+
+	glColor_from_pointer(shape);
+}
+
+static const GLfloat springVAR[] = {
+	0.00f, 0.0f,
+	0.20f, 0.0f,
+	0.25f, 3.0f,
+	0.30f,-6.0f,
+	0.35f, 6.0f,
+	0.40f,-6.0f,
+	0.45f, 6.0f,
+	0.50f,-6.0f,
+	0.55f, 6.0f,
+	0.60f,-6.0f,
+	0.65f, 6.0f,
+	0.70f,-3.0f,
+	0.75f, 6.0f,
+	0.80f, 0.0f,
+	1.00f, 0.0f,
+};
+static const int springVAR_count = sizeof(springVAR)/sizeof(GLfloat)/2;
+
+#endif

src/test/ee.cpp

@@ -96,7 +96,7 @@ class cEETest : private cThread {
 		std::vector<Uint32> TN;
 		std::vector<cTexture *> TNP;
 
-		std::vector<cShape*> Tiles;
+		std::vector<Graphics::cShape*> Tiles;
 
 		eeVector2i Mouse;
 		eeVector2f Mousef;
@@ -218,6 +218,15 @@ class cEETest : private cThread {
 		cTextCache mEEText;
 		cTextCache mFBOText;
 		cTextCache mInfoText;
+
+		cSpace * mSpace;
+		cBody * mMouseBody;
+		cpVect mMousePoint;
+		cpVect mMousePoint_last;
+		cConstraint * mMouseJoint;
+		void PhysicsCreate();
+		void PhysicsUpdate();
+		void PhysicsDestroy();
 };
 
 void cEETest::CreateAquaTextureAtlas() {
@@ -344,6 +353,8 @@ void cEETest::Init() {
 
 		mVBO->Compile();
 
+		PhysicsCreate();
+
 		Launch();
 	} else {
 		std::cout << "Failed to start EE++" << std::endl;
@@ -722,6 +733,7 @@ void cEETest::CreateUI() {
 
 	cUIWindow::CreateParams WinParams;
 	WinParams.Flags = UI_HALIGN_CENTER;
+	WinParams.WinFlags |= cUIWindow::UI_WIN_MAXIMIZE_BUTTON;
 	WinParams.PosSet( 200, 50 );
 	WinParams.Size = eeSize( 530, 380 );
 	WinParams.ButtonsPositionFixer.x = -4;
@@ -936,7 +948,7 @@ void cEETest::LoadTextures() {
 	TreeTilingCreated = false;
 	CreateTiling(Wireframe);
 
-	cGlobalShapeGroup::instance()->Add( eeNew( cShape, ( TF->Load( MyPath + "data/aqua/aqua_button_ok.png" ), "aqua_button_ok" ) ) );
+	cGlobalShapeGroup::instance()->Add( eeNew( Graphics::cShape, ( TF->Load( MyPath + "data/aqua/aqua_button_ok.png" ), "aqua_button_ok" ) ) );
 }
 
 void cEETest::RandomizeHeights() {
@@ -1287,6 +1299,8 @@ void cEETest::Render() {
 		}
 	}
 
+	PhysicsUpdate();
+
 	cUIManager::instance()->Update();
 	cUIManager::instance()->Draw();
 
@@ -1532,6 +1546,8 @@ void cEETest::Process() {
 void cEETest::End() {
 	Wait();
 
+	PhysicsDestroy();
+
 	Mus->Stop();
 	eeSAFE_DELETE( Mus );
 	eeSAFE_DELETE( mTGL );
@@ -1567,6 +1583,134 @@ void cEETest::Particles() {
 		PS[i].Draw();
 }
 
+#define GRABABLE_MASK_BIT (1<<31)
+#define NOT_GRABABLE_MASK (~GRABABLE_MASK_BIT)
+
+void cEETest::PhysicsCreate() {
+	cPhysicsManager::CreateSingleton();
+	cPhysicsManager::instance()->CollisionSlop( 0.2 );
+
+	mMouseJoint = NULL;
+
+	mMouseBody = eeNew( cBody, ( INFINITY, INFINITY ) );
+
+	// Create a space, a space is a simulation world. It simulates the motions of rigid bodies,
+	// handles collisions between them, and simulates the joints between them.
+	mSpace = eeNew( cSpace, () );
+
+	// Lets set some parameters of the space:
+	// More iterations make the simulation more accurate but slower
+	mSpace->Iterations( 10 );
+
+	// These parameters tune the efficiency of the collision detection.
+	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpSpace
+	mSpace->ResizeStaticHash( 30.0f, 1000 );
+	mSpace->ResizeActiveHash( 30.0f, 1000 );
+
+	// Give it some gravity
+	mSpace->Gravity( cpv(0, 1000) );
+	mSpace->SleepTimeThreshold( 0.5f );
+
+	// Create A ground segment along the bottom of the screen
+	// By attaching it to &space->staticBody instead of a body, we make it a static shape.
+	Physics::cShapeSegment * ground = eeNew( cShapeSegment, ( mSpace->StaticBody(), cpv(0,640), cpv(640, 640), 0.0f ) );
+
+	// Set some parameters of the shape.
+	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpShape
+	ground->e( 1.0f );
+	ground->u( 1.0f );
+	ground->Layers( NOT_GRABABLE_MASK ); // Used by the Demo mouse grabbing code
+
+	// Add the shape to the space as a static shape
+	// If a shape never changes position, add it as static so Chipmunk knows it only needs to
+	// calculate collision information for it once when it is added.
+	// Do not change the postion of a static shape after adding it.
+	mSpace->AddShape( ground );
+
+	// Add a moving circle object.
+	cpFloat radius = 15.0f;
+	cpFloat mass = 10.0f;
+
+	// This time we need to give a mass and moment of inertia when creating the circle.
+	cBody * ballBody = eeNew( cBody, ( mass, cpMomentForCircle(mass, 0.0f, radius, cpvzero) ) );
+
+	// Set some parameters of the body:
+	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpBody
+	ballBody->Pos( cpv(320, 240 + radius+50) );
+	ballBody->Vel( cpv(0, 20) );
+
+	// Add the body to the space so it will be simulated and move around.
+	mSpace->AddBody( ballBody );
+
+	// Add a circle shape for the ball.
+	// Shapes are always defined relative to the center of gravity of the body they are attached to.
+	// When the body moves or rotates, the shape will move with it.
+	// Additionally, all of the cpSpaceAdd*() functions return the thing they added so you can create and add in one go.
+	Physics::cShapeCircle * ballShape = eeNew( Physics::cShapeCircle, ( ballBody, radius, cpvzero ) );
+	mSpace->AddShape( ballShape );
+	ballShape->e( 0.0f );
+	ballShape->u( 0.9f );
+
+	ballBody = eeNew( cBody, ( mass, cpMomentForCircle( mass, 0.0f, radius, cpvzero ) ) );
+
+	// Set some parameters of the body:
+	// For more info: http://code.google.com/p/chipmunk-physics/wiki/cpBody
+	ballBody->Pos( cpv(320, 240 + radius + 50 + 200) );
+	ballBody->Vel( cpv(0, 20) );
+
+	// Add the body to the space so it will be simulated and move around.
+	mSpace->AddBody( ballBody );
+
+	// Add a circle shape for the ball.
+	// Shapes are always defined relative to the center of gravity of the body they are attached to.
+	// When the body moves or rotates, the shape will move with it.
+	// Additionally, all of the cpSpaceAdd*() functions return the thing they added so you can create and add in one go.
+	ballShape = eeNew( Physics::cShapeCircle, ( ballBody, radius, cpvzero ) );
+	ballShape->e( 0.0f );
+	ballShape->u( 0.9f );
+	mSpace->AddShape( ballShape );
+}
+
+void cEETest::PhysicsUpdate() {
+	cGlobalBatchRenderer::instance()->Draw();
+
+	mMousePoint = cpv( KM->GetMousePosf().x, KM->GetMousePosf().y );
+	cpVect newPoint = cpvlerp(mMousePoint_last, mMousePoint, 0.25f);
+	mMouseBody->Pos( newPoint );
+	mMouseBody->Vel( cpvmult( cpvsub( newPoint, mMousePoint_last ), 60.0f ) );
+	mMousePoint_last = newPoint;
+
+	if ( KM->MouseLeftPressed() ) {
+		if ( NULL == mMouseJoint ) {
+			cpVect point = cpv( KM->GetMousePosf().x, KM->GetMousePosf().y );
+
+			cpShape * shape = cpSpacePointQueryFirst( mSpace->Space(), point, GRABABLE_MASK_BIT, CP_NO_GROUP );
+
+			if( shape ){
+				cpBody * body = shape->body;
+				mMouseJoint = eeNew( cConstraint, ( cpPivotJointNew2( mMouseBody->Body(), body, cpvzero, cpBodyWorld2Local( body, point ) ) ) );
+				mMouseJoint->MaxForce( 50000.0f );
+				mMouseJoint->BiasCoef( 0.15f );
+				mSpace->AddConstraint( mMouseJoint );
+			}
+		}
+	} else if ( NULL != mMouseJoint ) {
+		mSpace->RemoveConstraint( mMouseJoint );
+		cpConstraintFree( mMouseJoint->Constraint() );
+		eeSAFE_DELETE( mMouseJoint );
+	}
+
+	mSpace->Update();
+	mSpace->Draw();
+}
+
+void cEETest::PhysicsDestroy() {
+	eeSAFE_DELETE( mSpace );
+	eeSAFE_DELETE( mMouseJoint );
+	eeSAFE_DELETE( mMouseBody );
+	cPhysicsManager::DestroySingleton();
+}
+
 int main (int argc, char * argv []) {
 	cEETest * Test = eeNew( cEETest, () );
 

src/ui/cuimanager.hpp

@@ -12,6 +12,7 @@ class EE_API cUIManager : public tSingleton<cUIManager> {
 	friend class tSingleton<cUIManager>;
 	public:
 		cUIManager();
+
 		~cUIManager();
 
 		cUIControlAnim * MainControl() const;

src/ui/cuiwindow.cpp

@@ -110,15 +110,15 @@ void cUIWindow::ButtonCloseClick( const cUIEvent * Event ) {
 void cUIWindow::ButtonMaximizeClick( const cUIEvent * Event ) {
 	cUIControl * Ctrl = cUIManager::instance()->MainControl();
 
-	if ( Ctrl->Size() == cUIControl::Size() ) {
+	if ( Ctrl->Size() == mSize ) {
 		Pos( mNonMaxPos );
-		Size( mNonMaxSize );
+		InternalSize( mNonMaxSize );
 	} else {
 		mNonMaxPos	= mPos;
 		mNonMaxSize = mSize;
 
 		Pos( 0, 0 );
-		Size( cUIManager::instance()->MainControl()->Size() );
+		InternalSize( cUIManager::instance()->MainControl()->Size() );
 	}
 }
 
@@ -622,4 +622,12 @@ cUITextBox * cUIWindow::TitleTextBox() const {
 	return mTitle;
 }
 
+Uint32 cUIWindow::OnMouseDoubleClick( const eeVector2i &Pos, Uint32 Flags ) {
+	if ( ( mWinFlags & UI_WIN_RESIZEABLE ) && ( NULL != mButtonMaximize ) && ( Flags & EE_BUTTON_LMASK ) ) {
+		ButtonMaximizeClick( NULL );
+	}
+
+	return 1;
+}
+
 }}

src/ui/cuiwindow.hpp

@@ -158,6 +158,8 @@ class cUIWindow : public cUIComplexControl {
 		void GetMinWinSize();
 
 		void FixTitleSize();
+
+		Uint32 OnMouseDoubleClick( const eeVector2i &Pos, Uint32 Flags );
 };
 
 }}

src/utils/vector2.hpp

@@ -208,6 +208,7 @@ T Vector2<T>::Distance( const Vector2<T>& Vec ) {
 typedef Vector2<Int32> eeVector2if;
 typedef Vector2<eeInt> eeVector2i;
 typedef Vector2<eeFloat> eeVector2f;
+typedef Vector2<eeDouble> eeVector2d;
 
 }}
 

src/utils/vector3.hpp

@@ -89,6 +89,7 @@ bool operator !=(const Vector3<T>& V1, const Vector3<T>& V2) {
 // Define the most common types
 typedef Vector3<eeInt>   eeVector3i;
 typedef Vector3<eeFloat> eeVector3f;
+typedef Vector3<eeDouble> eeVector3d;
 
 }}