Browse files

Added the chipmunk source for hacking.

  • Loading branch information...
1 parent 850d72c commit 6b0843785a6a4fe3b12db240dc1b356d611f8904 luqui committed Aug 6, 2008
View
58 soylent/GoD/chipmunk/CMakeLists.txt
@@ -0,0 +1,58 @@
+#INCLUDE_DIRECTORIES(${CHIPMUNK_SOURCE_DIR}/include)
+
+SET(chipmunk_includes
+ chipmunk.h
+ cpArbiter.h
+ cpArray.h
+ cpBB.h
+ cpBody.h
+ cpCollision.h
+ cpHashSet.h
+ cpJoint.h
+ cpPolyShape.h
+ cpShape.h
+ cpSpace.h
+ cpSpaceHash.h
+ cpVect.h
+)
+
+
+ADD_LIBRARY(chipmunk SHARED
+ chipmunk.c
+ cpArbiter.c
+ cpArray.c
+ cpBB.c
+ cpBody.c
+ cpCollision.c
+ cpHashSet.c
+ cpJoint.c
+ cpPolyShape.c
+ cpShape.c
+ cpSpace.c
+ cpSpaceHash.c
+ cpVect.c
+)
+
+ADD_LIBRARY(chipmunk_static STATIC
+ chipmunk.c
+ cpArbiter.c
+ cpArray.c
+ cpBB.c
+ cpBody.c
+ cpCollision.c
+ cpHashSet.c
+ cpJoint.c
+ cpPolyShape.c
+ cpShape.c
+ cpSpace.c
+ cpSpaceHash.c
+ cpVect.c
+)
+
+INSTALL(FILES ${chipmunk_includes} DESTINATION include/chipmunk)
+SET_TARGET_PROPERTIES(chipmunk_static PROPERTIES OUTPUT_NAME chipmunk) #Sets chipmunk_static to output "libchipmunk.a" not "libchipmunk_static.a"
+INSTALL(TARGETS chipmunk chipmunk_static
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
+SET_TARGET_PROPERTIES(chipmunk PROPERTIES VERSION 4)
+
View
69 soylent/GoD/chipmunk/chipmunk.c
@@ -0,0 +1,69 @@
+/* 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.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void cpInitCollisionFuncs(void);
+#ifdef __cplusplus
+}
+#endif
+
+
+void
+cpInitChipmunk(void)
+{
+ cpInitCollisionFuncs();
+}
+
+cpFloat
+cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
+{
+ return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset);
+}
+
+cpFloat
+cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
+{
+ cpVect *tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect));
+ for(int i=0; i<numVerts; i++)
+ tVerts[i] = cpvadd(verts[i], offset);
+
+ cpFloat sum1 = 0.0f;
+ cpFloat sum2 = 0.0f;
+ for(int i=0; i<numVerts; i++){
+ cpVect v1 = tVerts[i];
+ cpVect v2 = tVerts[(i+1)%numVerts];
+
+ cpFloat a = cpvcross(v2, v1);
+ cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
+
+ sum1 += a*b;
+ sum2 += a;
+ }
+
+ free(tVerts);
+ return (m*sum1)/(6.0f*sum2);
+}
View
86 soylent/GoD/chipmunk/chipmunk.h
@@ -0,0 +1,86 @@
+/* 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.
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef float cpFloat;
+
+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
+cpfclamp(cpFloat f, cpFloat min, cpFloat max){
+ return cpfmin(cpfmax(f, min), max);
+}
+
+#ifndef 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)
+ #else
+ #define INFINITY (1e1000)
+ #endif
+#endif
+
+#include "cpVect.h"
+#include "cpBB.h"
+#include "cpBody.h"
+#include "cpArray.h"
+#include "cpHashSet.h"
+#include "cpSpaceHash.h"
+
+#include "cpShape.h"
+#include "cpPolyShape.h"
+
+#include "cpArbiter.h"
+#include "cpCollision.h"
+
+#include "cpJoint.h"
+
+#include "cpSpace.h"
+
+#define CP_HASH_COEF (3344921057ul)
+#define CP_HASH_PAIR(A, B) ((unsigned int)(A)*CP_HASH_COEF ^ (unsigned int)(B)*CP_HASH_COEF)
+
+void cpInitChipmunk(void);
+
+cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset);
+cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset);
+
+#ifdef __cplusplus
+}
+#endif
View
263 soylent/GoD/chipmunk/cpArbiter.c
@@ -0,0 +1,263 @@
+/* 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.h"
+
+cpFloat cp_bias_coef = 0.1f;
+cpFloat cp_collision_slop = 0.1f;
+
+cpContact*
+cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int 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
+cpContactsSumImpulses(cpContact *contacts, int numContacts)
+{
+ cpVect sum = cpvzero;
+
+ for(int i=0; i<numContacts; i++){
+ cpContact *con = &contacts[i];
+ cpVect j = cpvmult(con->n, con->jnAcc);
+ sum = cpvadd(sum, j);
+ }
+
+ return sum;
+}
+
+cpVect
+cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts)
+{
+ cpVect sum = cpvzero;
+
+ for(int i=0; i<numContacts; i++){
+ cpContact *con = &contacts[i];
+ cpVect t = cpvperp(con->n);
+ cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
+ sum = cpvadd(sum, j);
+ }
+
+ return sum;
+}
+
+cpArbiter*
+cpArbiterAlloc(void)
+{
+ return (cpArbiter *)calloc(1, sizeof(cpArbiter));
+}
+
+cpArbiter*
+cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp)
+{
+ arb->numContacts = 0;
+ arb->contacts = NULL;
+
+ arb->a = a;
+ arb->b = b;
+
+ arb->stamp = stamp;
+
+ return arb;
+}
+
+cpArbiter*
+cpArbiterNew(cpShape *a, cpShape *b, int stamp)
+{
+ return cpArbiterInit(cpArbiterAlloc(), a, b, stamp);
+}
+
+void
+cpArbiterDestroy(cpArbiter *arb)
+{
+ free(arb->contacts);
+}
+
+void
+cpArbiterFree(cpArbiter *arb)
+{
+ if(arb) cpArbiterDestroy(arb);
+ free(arb);
+}
+
+void
+cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts)
+{
+ // 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 possitives.
+ if(new_contact->hash == old->hash){
+ // Copy the persistant contact information.
+ new_contact->jnAcc = old->jnAcc;
+ new_contact->jtAcc = old->jtAcc;
+ }
+ }
+ }
+
+ free(arb->contacts);
+
+ arb->contacts = contacts;
+ arb->numContacts = numContacts;
+}
+
+void
+cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
+{
+ cpShape *shapea = arb->a;
+ cpShape *shapeb = arb->b;
+
+ cpFloat e = shapea->e * shapeb->e;
+ arb->u = shapea->u * shapeb->u;
+ arb->target_v = 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];
+
+ // Calculate the offsets.
+ con->r1 = cpvsub(con->p, a->p);
+ con->r2 = cpvsub(con->p, b->p);
+
+ // Calculate the mass normal.
+ cpFloat mass_sum = a->m_inv + b->m_inv;
+
+ cpFloat r1cn = cpvcross(con->r1, con->n);
+ cpFloat r2cn = cpvcross(con->r2, con->n);
+ cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
+ con->nMass = 1.0f/kn;
+
+ // Calculate the mass tangent.
+ cpVect t = cpvperp(con->n);
+ cpFloat r1ct = cpvcross(con->r1, t);
+ cpFloat r2ct = cpvcross(con->r2, t);
+ cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct;
+ con->tMass = 1.0f/kt;
+
+ // 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.
+ cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w));
+ cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w));
+ con->bounce = 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->target_v = 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];
+
+ cpVect t = cpvperp(con->n);
+ cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
+ cpBodyApplyImpulse(a, cpvneg(j), con->r1);
+ cpBodyApplyImpulse(b, j, con->r2);
+ }
+}
+
+void
+cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
+{
+ 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.
+ cpVect jb = cpvmult(n, jbn);
+ cpBodyApplyBiasImpulse(a, cpvneg(jb), r1);
+ cpBodyApplyBiasImpulse(b, jb, r2);
+
+ // Calculate the relative velocity.
+ cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
+ cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
+ cpVect vr = cpvsub(v2, v1);
+ cpFloat vrn = cpvdot(vr, n);
+
+ // Calculate and clamp the normal impulse.
+ cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass;
+ cpFloat jnOld = con->jnAcc;
+ con->jnAcc = cpfmax(jnOld + jn, 0.0f);
+ jn = con->jnAcc - jnOld;
+
+ // Calculate the relative tangent velocity.
+ cpVect t = cpvperp(n);
+ cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), t);
+
+ // 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.
+ cpVect j = cpvadd(cpvmult(n, jn), cpvmult(t, jt));
+ cpBodyApplyImpulse(a, cpvneg(j), r1);
+ cpBodyApplyImpulse(b, j, r2);
+ }
+}
View
85 soylent/GoD/chipmunk/cpArbiter.h
@@ -0,0 +1,85 @@
+/* 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.
+extern cpFloat cp_bias_coef;
+// Amount of allowed penetration. Used to reduce vibrating contacts.
+extern cpFloat cp_collision_slop;
+
+// Data structure for contact points.
+typedef struct cpContact{
+ // Contact point and normal.
+ cpVect p, n;
+ // Penetration distance.
+ cpFloat dist;
+
+ // Calculated by cpArbiterPreStep().
+ cpVect r1, r2;
+ cpFloat nMass, tMass, bounce;
+
+ // Persistant contact information.
+ cpFloat jnAcc, jtAcc, jBias;
+ cpFloat bias;
+
+ // Hash value used to (mostly) uniquely identify a contact.
+ unsigned int hash;
+} cpContact;
+
+// Contacts are always allocated in groups.
+cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int hash);
+
+// Sum the contact impulses. (Can be used after cpSpaceStep() returns)
+cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
+cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
+
+// Data structure for tracking collisions between shapes.
+typedef struct cpArbiter{
+ // Information on the contact points between the objects.
+ int numContacts;
+ cpContact *contacts;
+
+ // The two shapes involved in the collision.
+ cpShape *a, *b;
+
+ // Calculated by cpArbiterPreStep().
+ cpFloat u;
+ cpVect target_v;
+
+ // Time stamp of the arbiter. (from cpSpace)
+ int stamp;
+} cpArbiter;
+
+// Basic allocation/destruction functions.
+cpArbiter* cpArbiterAlloc(void);
+cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp);
+cpArbiter* cpArbiterNew(cpShape *a, cpShape *b, int stamp);
+
+void cpArbiterDestroy(cpArbiter *arb);
+void cpArbiterFree(cpArbiter *arb);
+
+// These functions are all intended to be used internally.
+// Inject new contact points into the arbiter while preserving contact history.
+void cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts);
+// Precalculate values used by the solver.
+void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
+void cpArbiterApplyCachedImpulse(cpArbiter *arb);
+// Run an iteration of the solver on the arbiter.
+void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
View
114 soylent/GoD/chipmunk/cpArray.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 <string.h>
+
+#include "chipmunk.h"
+
+
+//#define CP_ARRAY_INCREMENT 10
+
+// NOTE: cpArray is rarely used and will probably go away.
+
+cpArray*
+cpArrayAlloc(void)
+{
+ return (cpArray *)calloc(1, sizeof(cpArray));
+}
+
+cpArray*
+cpArrayInit(cpArray *arr, int size)
+{
+ arr->num = 0;
+
+ size = (size ? size : 4);
+ arr->max = size;
+ arr->arr = (void **)malloc(size*sizeof(void**));
+
+ return arr;
+}
+
+cpArray*
+cpArrayNew(int size)
+{
+ return cpArrayInit(cpArrayAlloc(), size);
+}
+
+void
+cpArrayDestroy(cpArray *arr)
+{
+ free(arr->arr);
+}
+
+void
+cpArrayFree(cpArray *arr)
+{
+ if(!arr) return;
+ cpArrayDestroy(arr);
+ free(arr);
+}
+
+void
+cpArrayPush(cpArray *arr, void *object)
+{
+ if(arr->num == arr->max){
+ arr->max *= 2;
+ arr->arr = (void **)realloc(arr->arr, arr->max*sizeof(void**));
+ }
+
+ arr->arr[arr->num] = object;
+ arr->num++;
+}
+
+void
+cpArrayDeleteIndex(cpArray *arr, int index)
+{
+ int last = --arr->num;
+ arr->arr[index] = arr->arr[last];
+}
+
+void
+cpArrayDeleteObj(cpArray *arr, void *obj)
+{
+ for(int i=0; i<arr->num; i++){
+ if(arr->arr[i] == obj){
+ cpArrayDeleteIndex(arr, i);
+ return;
+ }
+ }
+}
+
+void
+cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
+{
+ for(int i=0; i<arr->num; i++)
+ iterFunc(arr->arr[i], data);
+}
+
+int
+cpArrayContains(cpArray *arr, void *ptr)
+{
+ for(int i=0; i<arr->num; i++)
+ if(arr->arr[i] == ptr) return 1;
+
+ return 0;
+}
View
45 soylent/GoD/chipmunk/cpArray.h
@@ -0,0 +1,45 @@
+/* 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.
+ */
+
+// NOTE: cpArray is rarely used and will probably go away.
+
+typedef struct cpArray{
+ int num, max;
+ void **arr;
+} cpArray;
+
+typedef void (*cpArrayIter)(void *ptr, void *data);
+
+cpArray *cpArrayAlloc(void);
+cpArray *cpArrayInit(cpArray *arr, int size);
+cpArray *cpArrayNew(int size);
+
+void cpArrayDestroy(cpArray *arr);
+void cpArrayFree(cpArray *arr);
+
+void cpArrayClear(cpArray *arr);
+
+void cpArrayPush(cpArray *arr, void *object);
+void cpArrayDeleteIndex(cpArray *arr, int index);
+void cpArrayDeleteObj(cpArray *arr, void *obj);
+
+void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data);
+int cpArrayContains(cpArray *arr, void *ptr);
View
46 soylent/GoD/chipmunk/cpBB.c
@@ -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.
+ */
+
+#include <math.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 = fabsf(bb.r - bb.l);
+ cpFloat modx = fmodf(v.x - bb.l, ix);
+ cpFloat x = (modx > 0.0f) ? modx : modx + ix;
+
+ cpFloat iy = fabsf(bb.t - bb.b);
+ cpFloat mody = fmodf(v.y - bb.b, iy);
+ cpFloat y = (mody > 0.0f) ? mody : mody + iy;
+
+ return cpv(x + bb.l, y + bb.b);
+}
View
53 soylent/GoD/chipmunk/cpBB.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 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 int
+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 int
+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 int
+cpBBcontainsVect(const cpBB bb, const cpVect v)
+{
+ return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y);
+}
+
+cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox
+cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox
View
180 soylent/GoD/chipmunk/cpBody.c
@@ -0,0 +1,180 @@
+/* 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 <float.h>
+
+#include "chipmunk.h"
+
+cpBody*
+cpBodyAlloc(void)
+{
+ return (cpBody *)malloc(sizeof(cpBody));
+}
+
+cpBody*
+cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
+{
+ body->velocity_func = cpBodyUpdateVelocity;
+ body->position_func = cpBodyUpdatePosition;
+
+ 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->active = 1;
+
+ return body;
+}
+
+cpBody*
+cpBodyNew(cpFloat m, cpFloat i)
+{
+ return cpBodyInit(cpBodyAlloc(), m, i);
+}
+
+void cpBodyDestroy(cpBody *body){}
+
+void
+cpBodyFree(cpBody *body)
+{
+ if(body) cpBodyDestroy(body);
+ free(body);
+}
+
+void
+cpBodySetMass(cpBody *body, cpFloat m)
+{
+ body->m = m;
+ body->m_inv = 1.0f/m;
+}
+
+void
+cpBodySetMoment(cpBody *body, cpFloat i)
+{
+ body->i = i;
+ body->i_inv = 1.0f/i;
+}
+
+void
+cpBodySetAngle(cpBody *body, cpFloat a)
+{
+ body->a = fmod(a, (cpFloat)M_PI*2.0f);
+ body->rot = cpvforangle(a);
+}
+
+void
+cpBodySlew(cpBody *body, cpVect pos, cpFloat dt)
+{
+ cpVect delta = cpvsub(pos, body->p);
+ body->v = cpvmult(delta, 1.0/dt);
+}
+
+void
+cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
+{
+ body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
+ body->w = body->w*damping + body->t*body->i_inv*dt;
+}
+
+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 f, cpVect r)
+{
+ body->f = cpvadd(body->f, f);
+ body->t += cpvcross(r, f);
+}
+
+void
+cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
+{
+ // Calculate the world space anchor coordinates.
+ cpVect r1 = cpvrotate(anchr1, a->rot);
+ cpVect r2 = cpvrotate(anchr2, b->rot);
+
+ cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1));
+ cpFloat dist = cpvlength(delta);
+ cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
+
+ cpFloat f_spring = (dist - rlen)*k;
+
+ // Calculate the world relative velocities of the anchor points.
+ cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
+ cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
+
+ // Calculate the damping force.
+ // This really should be in the impulse solver and can produce problems when using large damping values.
+ cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
+ cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv)));
+
+ // Apply!
+ cpVect f = cpvmult(n, f_spring + f_damp);
+ cpBodyApplyForce(a, f, r1);
+ cpBodyApplyForce(b, cpvneg(f), r2);
+}
+
+//int
+//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max)
+//{
+// cpFloat ke = body->m*cpvdot(body->v, body->v);
+// cpFloat re = body->i*body->w*body->w;
+//
+// if(ke + re > body->m*dvsq)
+// body->active = 1;
+// else if(body->active)
+// body->active = (body->active + 1)%(max + 1);
+// else {
+// body->v = cpvzero;
+// body->v_bias = cpvzero;
+// body->w = 0.0f;
+// body->w_bias = 0.0f;
+// }
+//
+// return body->active;
+//}
View
110 soylent/GoD/chipmunk/cpBody.h
@@ -0,0 +1,110 @@
+/* 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.
+ */
+
+struct cpBody;
+typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
+typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt);
+
+
+typedef struct cpBody{
+ // 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 and it's inverse.
+ cpFloat m, m_inv;
+ // Moment of inertia and it's inverse.
+ cpFloat i, i_inv;
+
+ // NOTE: v_bias and w_bias are used internally for penetration/joint correction.
+ // Linear components of motion (position, velocity, and force)
+ cpVect p, v, f, v_bias;
+ // Angular components of motion (angle, angular velocity, and torque)
+ cpFloat a, w, t, w_bias;
+ // Unit length
+ cpVect rot;
+
+ // User defined data pointer.
+ void *data;
+// int active;
+} cpBody;
+
+// Basic allocation/destruction functions
+cpBody *cpBodyAlloc(void);
+cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
+cpBody *cpBodyNew(cpFloat m, cpFloat i);
+
+void cpBodyDestroy(cpBody *body);
+void cpBodyFree(cpBody *body);
+
+// Setters for some of the special properties (mandatory!)
+void cpBodySetMass(cpBody *body, cpFloat m);
+void cpBodySetMoment(cpBody *body, cpFloat i);
+void cpBodySetAngle(cpBody *body, cpFloat a);
+
+// Modify the velocity of an object so that it will
+void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);
+
+// 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(cpBody *body, cpVect v)
+{
+ return cpvadd(body->p, cpvrotate(v, body->rot));
+}
+
+// Convert world to body local coordinates
+static inline cpVect
+cpBodyWorld2Local(cpBody *body, cpVect v)
+{
+ return cpvunrotate(cpvsub(v, body->p), body->rot);
+}
+
+// Apply an impulse (in world coordinates) to the body.
+static inline void
+cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r)
+{
+ body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
+ body->w += body->i_inv*cpvcross(r, j);
+}
+
+// Not intended for external use. Used by cpArbiter.c and cpJoint.c.
+static inline void
+cpBodyApplyBiasImpulse(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);
+}
+
+// Zero the forces on a body.
+void cpBodyResetForces(cpBody *body);
+// Apply a force (in world coordinates) to a body.
+void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r);
+
+// Apply a damped spring force between two bodies.
+void cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt);
+
+//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max);
View
372 soylent/GoD/chipmunk/cpCollision.c
@@ -0,0 +1,372 @@
+/* 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 <assert.h>
+
+#include "chipmunk.h"
+
+typedef int (*collisionFunc)(cpShape*, cpShape*, cpContact**);
+
+static collisionFunc *colfuncs = NULL;
+
+// Add contact points for circle to circle collisions.
+// Used by several collision tests.
+static int
+circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, 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 = sqrtf(distsq);
+ // To avoid singularities, do nothing in the case of dist = 0.
+ cpFloat non_zero_dist = (dist ? dist : INFINITY);
+
+ // Allocate and initialize the contact.
+ (*con) = (cpContact *)malloc(sizeof(cpContact));
+ cpContactInit(
+ (*con),
+ cpvadd(p1, cpvmult(delta, 0.5 + (r1 - 0.5*mindist)/non_zero_dist)),
+ cpvmult(delta, 1.0/non_zero_dist),
+ dist - mindist,
+ 0
+ );
+
+ return 1;
+}
+
+// Collide circle shapes.
+static int
+circle2circle(cpShape *shape1, 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(cpShape *circleShape, 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 = fabs(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);
+ (*con) = (cpContact *)malloc(sizeof(cpContact));
+ 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 allocating contact point lists.
+static cpContact *
+addContactPoint(cpContact **arr, int *max, int *num)
+{
+ if(*arr == NULL){
+ // Allocate the array if it hasn't been done.
+ (*max) = 2;
+ (*num) = 0;
+ (*arr) = (cpContact *)malloc((*max)*sizeof(cpContact));
+ } else if(*num == *max){
+ // Extend it if necessary.
+ (*max) *= 2;
+ (*arr) = (cpContact *)realloc(*arr, (*max)*sizeof(cpContact));
+ }
+
+ cpContact *con = &(*arr)[*num];
+ (*num)++;
+
+ return con;
+}
+
+// Find the minimum separating axis for the give poly and axis list.
+static inline int
+findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out)
+{
+ int min_index = 0;
+ cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d);
+ if(min > 0.0) return -1;
+
+ for(int i=1; i<num; i++){
+ cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d);
+ if(dist > 0.0) {
+ return -1;
+ } else if(dist > min){
+ min = dist;
+ min_index = i;
+ }
+ }
+
+ (*min_out) = min;
+ return min_index;
+}
+
+// Add contacts for penetrating vertexes.
+static inline int
+findVerts(cpContact **arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist)
+{
+ int max = 0;
+ int num = 0;
+
+ for(int i=0; i<poly1->numVerts; i++){
+ cpVect v = poly1->tVerts[i];
+ if(cpPolyShapeContainsVert(poly2, v))
+ cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i));
+ }
+
+ for(int i=0; i<poly2->numVerts; i++){
+ cpVect v = poly2->tVerts[i];
+ if(cpPolyShapeContainsVert(poly1, v))
+ cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly2, i));
+ }
+
+ // if(!num)
+ // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0));
+
+ return num;
+}
+
+// Collide poly shapes together.
+static int
+poly2poly(cpShape *shape1, 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 float
+segValueOnAxis(cpSegmentShape *seg, cpVect n, 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 *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, 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(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly, i));
+ }
+ }
+ }
+}
+
+// This one is complicated and gross. Just don't go there...
+// TODO: Comment me!
+static int
+seg2poly(cpShape *shape1, 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 max = 0;
+ 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(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0));
+ if(cpPolyShapeContainsVert(poly, vb))
+ cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 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, &max, &num, seg, poly, minNorm, 1.0f);
+ else
+ findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f);
+ }
+
+ return num;
+}
+
+// This one is less gross, but still gross.
+// TODO: Comment me!
+static int
+circle2poly(cpShape *shape1, 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.0){
+ 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) {
+ (*con) = (cpContact *)malloc(sizeof(cpContact));
+ 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 void
+addColFunc(cpShapeType a, cpShapeType b, collisionFunc func)
+{
+ colfuncs[a + b*CP_NUM_SHAPES] = func;
+}
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ // Initializes the array of collision functions.
+ // Called by cpInitChipmunk().
+ void
+ cpInitCollisionFuncs(void)
+ {
+ if(!colfuncs)
+ colfuncs = (collisionFunc *)calloc(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(cpShape *a, cpShape *b, cpContact **arr)
+{
+ // Their shape types must be in order.
+ assert(a->klass->type <= b->klass->type);
+
+ collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES];
+ return (cfunc) ? cfunc(a, b, arr) : 0;
+}
View
23 soylent/GoD/chipmunk/cpCollision.h
@@ -0,0 +1,23 @@
+/* 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.
+ */
+
+// Collides two cpShape structures. (this function is lonely :( )
+int cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr);
View
219 soylent/GoD/chipmunk/cpHashSet.c
@@ -0,0 +1,219 @@
+/* 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.h"
+#include "prime.h"
+
+void
+cpHashSetDestroy(cpHashSet *set)
+{
+ // Free the chains.
+ for(int i=0; i<set->size; i++){
+ // Free the bins in the chain.
+ cpHashSetBin *bin = set->table[i];
+ while(bin){
+ cpHashSetBin *next = bin->next;
+ free(bin);
+ bin = next;
+ }
+ }
+
+ // Free the table.
+ free(set->table);
+}
+
+void
+cpHashSetFree(cpHashSet *set)
+{
+ if(set) cpHashSetDestroy(set);
+ free(set);
+}
+
+cpHashSet *
+cpHashSetAlloc(void)
+{
+ return (cpHashSet *)calloc(1, sizeof(cpHashSet));
+}
+
+cpHashSet *
+cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans)
+{
+ set->size = next_prime(size);
+ set->entries = 0;
+
+ set->eql = eqlFunc;
+ set->trans = trans;
+
+ set->default_value = NULL;
+
+ set->table = (cpHashSetBin **)calloc(set->size, sizeof(cpHashSetBin *));
+
+ return set;
+}
+
+cpHashSet *
+cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans)
+{
+ return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, trans);
+}
+
+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 **)calloc(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 index = bin->hash%newSize;
+ bin->next = newTable[index];
+ newTable[index] = bin;
+
+ bin = next;
+ }
+ }
+
+ free(set->table);
+
+ set->table = newTable;
+ set->size = newSize;
+}
+
+void *
+cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data)
+{
+ int index = hash%set->size;
+
+ // Find the bin with the matching element.
+ cpHashSetBin *bin = set->table[index];
+ while(bin && !set->eql(ptr, bin->elt))
+ bin = bin->next;
+
+ // Create it necessary.
+ if(!bin){
+ bin = (cpHashSetBin *)malloc(sizeof(cpHashSetBin));
+ bin->hash = hash;
+ bin->elt = set->trans(ptr, data); // Transform the pointer.
+
+ bin->next = set->table[index];
+ set->table[index] = bin;
+
+ set->entries++;
+
+ // Resize the set if it's full.
+ if(setIsFull(set))
+ cpHashSetResize(set);
+ }
+
+ return bin->elt;
+}
+
+void *
+cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr)
+{
+ int index = hash%set->size;
+
+ // Pointer to the previous bin pointer.
+ cpHashSetBin **prev_ptr = &set->table[index];
+ // Pointer the the current bin.
+ cpHashSetBin *bin = set->table[index];
+
+ // 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 previos bin pointer to point to the next bin.
+ (*prev_ptr) = bin->next;
+ set->entries--;
+
+ void *return_value = bin->elt;
+ free(bin);
+ return return_value;
+ }
+
+ return NULL;
+}
+
+void *
+cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr)
+{
+ int index = hash%set->size;
+ cpHashSetBin *bin = set->table[index];
+ 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;
+ for(bin = set->table[i]; bin; bin = bin->next)
+ func(bin->elt, data);
+ }
+}
+
+void
+cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data)
+{
+ // Iterate over all the chains.
+ 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--;
+ free(bin);
+ }
+
+ bin = next;
+ }
+ }
+}
View
79 soylent/GoD/chipmunk/cpHashSet.h
@@ -0,0 +1,79 @@
+/* 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.
+ */
+
+// cpHashSet uses a chained hashtable implementation.
+// Other than the transformation functions, there is nothing fancy going on.
+
+// cpHashSetBin's form the linked lists in the chained hash table.
+typedef struct cpHashSetBin {
+ // Pointer to the element.
+ void *elt;
+ // Hash value of the element.
+ unsigned int hash;
+ // Next element in the chain.
+ struct cpHashSetBin *next;
+} cpHashSetBin;
+
+// Equality function. Returns true if ptr is equal to elt.
+typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt);
+// Used by cpHashSetInsert(). Called to transform the ptr into an element.
+typedef void *(*cpHashSetTransFunc)(void *ptr, void *data);
+// Iterator function for a hashset.
+typedef void (*cpHashSetIterFunc)(void *elt, void *data);
+// Reject function. Returns true if elt should be dropped.
+typedef int (*cpHashSetRejectFunc)(void *elt, void *data);
+
+typedef struct cpHashSet {
+ // Number of elements stored in the table.
+ int entries;
+ // Number of cells in the table.
+ int size;
+
+ cpHashSetEqlFunc eql;
+ cpHashSetTransFunc trans;
+
+ // Default value returned by cpHashSetFind() when no element is found.
+ // Defaults to NULL.
+ void *default_value;
+
+ cpHashSetBin **table;
+} cpHashSet;
+
+// Basic allocation/destruction functions.
+void cpHashSetDestroy(cpHashSet *set);
+void cpHashSetFree(cpHashSet *set);
+
+cpHashSet *cpHashSetAlloc(void);
+cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans);
+cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans);
+
+// Insert an element into the set, returns the element.
+// If it doesn't already exist, the transformation function is applied.
+void *cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data);
+// Remove and return an element from the set.
+void *cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr);
+// Find an element in the set. Returns the default value if the element isn't found.
+void *cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr);
+
+// Iterate over a hashset.
+void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data);
+// Iterate over a hashset while rejecting certain elements.
+void cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data);
View
553 soylent/GoD/chipmunk/cpJoint.c
@@ -0,0 +1,553 @@
+/* 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.h"
+
+// TODO: Comment me!
+
+cpFloat cp_joint_bias_coef = 0.1f;
+
+void cpJointDestroy(cpJoint *joint){}
+
+void
+cpJointFree(cpJoint *joint)
+{
+ if(joint) cpJointDestroy(joint);
+ free(joint);
+}
+
+static void
+cpJointInit(cpJoint *joint, const cpJointClass *klass, cpBody *a, cpBody *b)
+{
+ joint->klass = klass;
+ joint->a = a;
+ joint->b = b;
+}
+
+
+static inline cpVect
+relative_velocity(cpVect r1, cpVect v1, cpFloat w1, cpVect r2, cpVect v2, cpFloat w2){
+ cpVect v1_sum = cpvadd(v1, cpvmult(cpvperp(r1), w1));
+ cpVect v2_sum = cpvadd(v2, cpvmult(cpvperp(r2), w2));
+
+ return cpvsub(v2_sum, v1_sum);
+}
+
+static inline cpFloat
+scalar_k(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);
+
+ return mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
+}
+
+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_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
+{
+ cpBodyApplyBiasImpulse(a, cpvneg(j), r1);
+ cpBodyApplyBiasImpulse(b, j, r2);
+}
+
+
+static void
+pinJointPreStep(cpJoint *joint, cpFloat dt_inv)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+ cpPinJoint *jnt = (cpPinJoint *)joint;
+
+ jnt->r1 = cpvrotate(jnt->anchr1, a->rot);
+ jnt->r2 = cpvrotate(jnt->anchr2, b->rot);
+
+ cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1));
+ cpFloat dist = cpvlength(delta);
+ jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
+
+ // calculate mass normal
+ jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n);
+
+ // calculate bias velocity
+ jnt->bias = -cp_joint_bias_coef*dt_inv*(dist - jnt->dist);
+ jnt->jBias = 0.0f;
+
+ // apply accumulated impulse
+ cpVect j = cpvmult(jnt->n, jnt->jnAcc);
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+}
+
+static void
+pinJointApplyImpulse(cpJoint *joint)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+
+ cpPinJoint *jnt = (cpPinJoint *)joint;
+ cpVect n = jnt->n;
+ cpVect r1 = jnt->r1;
+ cpVect r2 = jnt->r2;
+
+ //calculate bias impulse
+ cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias);
+ cpFloat vbn = cpvdot(vbr, n);
+
+ cpFloat jbn = (jnt->bias - vbn)*jnt->nMass;
+ jnt->jBias += jbn;
+
+ cpVect jb = cpvmult(n, jbn);
+ apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb);
+
+ // compute relative velocity
+ cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w);
+ cpFloat vrn = cpvdot(vr, n);
+
+ // compute normal impulse
+ cpFloat jn = -vrn*jnt->nMass;
+ jnt->jnAcc =+ jn;
+
+ // apply impulse
+ cpVect j = cpvmult(n, jn);
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+}
+
+static const cpJointClass pinJointClass = {
+ CP_PIN_JOINT,
+ pinJointPreStep,
+ pinJointApplyImpulse,
+};
+
+cpPinJoint *
+cpPinJointAlloc(void)
+{
+ return (cpPinJoint *)malloc(sizeof(cpPinJoint));
+}
+
+cpPinJoint *
+cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+ cpJointInit((cpJoint *)joint, &pinJointClass, a, b);
+
+ joint->anchr1 = anchr1;
+ joint->anchr2 = anchr2;
+
+ cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot));
+ cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot));
+ joint->dist = cpvlength(cpvsub(p2, p1));
+
+ joint->jnAcc = 0.0;
+
+ return joint;
+}
+
+cpJoint *
+cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
+{
+ return (cpJoint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
+}
+
+
+
+
+static void
+slideJointPreStep(cpJoint *joint, cpFloat dt_inv)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+ cpSlideJoint *jnt = (cpSlideJoint *)joint;
+
+ jnt->r1 = cpvrotate(jnt->anchr1, a->rot);
+ jnt->r2 = cpvrotate(jnt->anchr2, b->rot);
+
+ cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1));
+ cpFloat dist = cpvlength(delta);
+ cpFloat pdist = 0.0;
+ if(dist > jnt->max) {
+ pdist = dist - jnt->max;
+ } else if(dist < jnt->min) {
+ pdist = jnt->min - dist;
+ dist = -dist;
+ }
+ jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
+
+ // calculate mass normal
+ jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n);
+
+ // calculate bias velocity
+ jnt->bias = -cp_joint_bias_coef*dt_inv*(pdist);
+ jnt->jBias = 0.0f;
+
+ // apply accumulated impulse
+ if(!jnt->bias) //{
+ // if bias is 0, then the joint is not at a limit.
+ jnt->jnAcc = 0.0f;
+// } else {
+ cpVect j = cpvmult(jnt->n, jnt->jnAcc);
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+// }
+}
+
+static void
+slideJointApplyImpulse(cpJoint *joint)
+{
+ cpSlideJoint *jnt = (cpSlideJoint *)joint;
+ if(!jnt->bias) return; // early exit
+
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+
+ cpVect n = jnt->n;
+ cpVect r1 = jnt->r1;
+ cpVect r2 = jnt->r2;
+
+ //calculate bias impulse
+ cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias);
+ cpFloat vbn = cpvdot(vbr, n);
+
+ cpFloat jbn = (jnt->bias - vbn)*jnt->nMass;
+ cpFloat jbnOld = jnt->jBias;
+ jnt->jBias = cpfmin(jbnOld + jbn, 0.0f);
+ jbn = jnt->jBias - jbnOld;
+
+ cpVect jb = cpvmult(n, jbn);
+ apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb);
+
+ // compute relative velocity
+ cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w);
+ cpFloat vrn = cpvdot(vr, n);
+
+ // compute normal impulse
+ cpFloat jn = -vrn*jnt->nMass;
+ cpFloat jnOld = jnt->jnAcc;
+ jnt->jnAcc = cpfmin(jnOld + jn, 0.0f);
+ jn = jnt->jnAcc - jnOld;
+
+ // apply impulse
+ cpVect j = cpvmult(n, jn);
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+}
+
+static const cpJointClass slideJointClass = {
+ CP_SLIDE_JOINT,
+ slideJointPreStep,
+ slideJointApplyImpulse,
+};
+
+cpSlideJoint *
+cpSlideJointAlloc(void)
+{
+ return (cpSlideJoint *)malloc(sizeof(cpSlideJoint));
+}
+
+cpSlideJoint *
+cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+ cpJointInit((cpJoint *)joint, &slideJointClass, a, b);
+
+ joint->anchr1 = anchr1;
+ joint->anchr2 = anchr2;
+ joint->min = min;
+ joint->max = max;
+
+ joint->jnAcc = 0.0;
+
+ return joint;
+}
+
+cpJoint *
+cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
+{
+ return (cpJoint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
+}
+
+
+
+
+static void
+pivotJointPreStep(cpJoint *joint, cpFloat dt_inv)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+ cpPivotJoint *jnt = (cpPivotJoint *)joint;
+
+ jnt->r1 = cpvrotate(jnt->anchr1, a->rot);
+ jnt->r2 = cpvrotate(jnt->anchr2, b->rot);
+
+ // calculate mass matrix
+ // If I wasn't lazy, this wouldn't be so gross...
+ cpFloat k11, k12, k21, k22;
+
+ cpFloat m_sum = a->m_inv + b->m_inv;
+ k11 = m_sum; k12 = 0.0f;
+ k21 = 0.0f; k22 = m_sum;
+
+ cpFloat r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv;
+ cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv;
+ cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv;
+ k11 += r1ysq; k12 += r1nxy;
+ k21 += r1nxy; k22 += r1xsq;
+
+ cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv;
+ cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv;
+ cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv;
+ k11 += r2ysq; k12 += r2nxy;
+ k21 += r2nxy; k22 += r2xsq;
+
+ cpFloat det_inv = 1.0f/(k11*k22 - k12*k21);
+ jnt->k1 = cpv( k22*det_inv, -k12*det_inv);
+ jnt->k2 = cpv(-k21*det_inv, k11*det_inv);
+
+
+ // calculate bias velocity
+ cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1));
+ jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv);
+ jnt->jBias = cpvzero;
+
+ // apply accumulated impulse
+ apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc);
+}
+
+static void
+pivotJointApplyImpulse(cpJoint *joint)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+
+ cpPivotJoint *jnt = (cpPivotJoint *)joint;
+ cpVect r1 = jnt->r1;
+ cpVect r2 = jnt->r2;
+ cpVect k1 = jnt->k1;
+ cpVect k2 = jnt->k2;
+
+ //calculate bias impulse
+ cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias);
+ vbr = cpvsub(jnt->bias, vbr);
+
+ cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2));
+ jnt->jBias = cpvadd(jnt->jBias, jb);
+
+ apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb);
+
+ // compute relative velocity
+ cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w);
+
+ // compute normal impulse
+ cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2));
+ jnt->jAcc = cpvadd(jnt->jAcc, j);
+
+ // apply impulse
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+}
+
+static const cpJointClass pivotJointClass = {
+ CP_PIVOT_JOINT,
+ pivotJointPreStep,
+ pivotJointApplyImpulse,
+};
+
+cpPivotJoint *
+cpPivotJointAlloc(void)
+{
+ return (cpPivotJoint *)malloc(sizeof(cpPivotJoint));
+}
+
+cpPivotJoint *
+cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot)
+{
+ cpJointInit((cpJoint *)joint, &pivotJointClass, a, b);
+
+ joint->anchr1 = cpvunrotate(cpvsub(pivot, a->p), a->rot);
+ joint->anchr2 = cpvunrotate(cpvsub(pivot, b->p), b->rot);
+
+ joint->jAcc = cpvzero;
+
+ return joint;
+}
+
+cpJoint *
+cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
+{
+ return (cpJoint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, pivot);
+}
+
+
+
+
+static void
+grooveJointPreStep(cpJoint *joint, cpFloat dt_inv)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+ cpGrooveJoint *jnt = (cpGrooveJoint *)joint;
+
+ // calculate endpoints in worldspace
+ cpVect ta = cpBodyLocal2World(a, jnt->grv_a);
+ cpVect tb = cpBodyLocal2World(a, jnt->grv_b);
+
+ // calculate axis
+ cpVect n = cpvrotate(jnt->grv_n, a->rot);
+ cpFloat d = cpvdot(ta, n);
+
+ jnt->grv_tn = n;
+ jnt->r2 = cpvrotate(jnt->anchr2, b->rot);
+
+ // calculate tangential distance along the axis of r2
+ cpFloat td = cpvcross(cpvadd(b->p, jnt->r2), n);
+ // calculate clamping factor and r2
+ if(td <= cpvcross(ta, n)){
+ jnt->clamp = 1.0f;
+ jnt->r1 = cpvsub(ta, a->p);
+ } else if(td >= cpvcross(tb, n)){
+ jnt->clamp = -1.0f;
+ jnt->r1 = cpvsub(tb, a->p);
+ } else {
+ jnt->clamp = 0.0f;
+ jnt->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
+ }
+
+ // 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 r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv;
+ cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv;
+ cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv;
+ k11 += r1ysq; k12 += r1nxy;
+ k21 += r1nxy; k22 += r1xsq;
+
+ // add the influnce from r2
+ cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv;
+ cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv;
+ cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv;
+ k11 += r2ysq; k12 += r2nxy;
+ k21 += r2nxy; k22 += r2xsq;
+
+ // invert
+ cpFloat det_inv = 1.0f/(k11*k22 - k12*k21);
+ jnt->k1 = cpv( k22*det_inv, -k12*det_inv);
+ jnt->k2 = cpv(-k21*det_inv, k11*det_inv);
+
+
+ // calculate bias velocity
+ cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1));
+ jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv);
+ jnt->jBias = cpvzero;
+
+ // apply accumulated impulse
+ apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc);
+}
+
+static inline cpVect
+grooveConstrain(cpGrooveJoint *jnt, cpVect j){
+ cpVect n = jnt->grv_tn;
+ cpVect jn = cpvmult(n, cpvdot(j, n));
+
+ cpVect t = cpvperp(n);
+ cpFloat coef = (jnt->clamp*cpvcross(j, n) > 0.0f) ? 1.0f : 0.0f;
+ cpVect jt = cpvmult(t, cpvdot(j, t)*coef);
+
+ return cpvadd(jn, jt);
+}
+
+static void
+grooveJointApplyImpulse(cpJoint *joint)
+{
+ cpBody *a = joint->a;
+ cpBody *b = joint->b;
+
+ cpGrooveJoint *jnt = (cpGrooveJoint *)joint;
+ cpVect r1 = jnt->r1;
+ cpVect r2 = jnt->r2;
+ cpVect k1 = jnt->k1;
+ cpVect k2 = jnt->k2;
+
+ //calculate bias impulse
+ cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias);
+ vbr = cpvsub(jnt->bias, vbr);
+
+ cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2));
+ cpVect jbOld = jnt->jBias;
+ jnt->jBias = grooveConstrain(jnt, cpvadd(jbOld, jb));
+ jb = cpvsub(jnt->jBias, jbOld);
+
+ apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb);
+
+ // compute impulse
+ cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w);
+
+ cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2));
+ cpVect jOld = jnt->jAcc;
+ jnt->jAcc = grooveConstrain(jnt, cpvadd(jOld, j));
+ j = cpvsub(jnt->jAcc, jOld);
+
+ // apply impulse
+ apply_impulses(a, b, jnt->r1, jnt->r2, j);
+}
+
+static const cpJointClass grooveJointClass = {
+ CP_GROOVE_JOINT,
+ grooveJointPreStep,
+ grooveJointApplyImpulse,
+};
+
+cpGrooveJoint *
+cpGrooveJointAlloc(void)
+{
+ return (cpGrooveJoint *)malloc(sizeof(cpGrooveJoint));
+}
+
+cpGrooveJoint *
+cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+ cpJointInit((cpJoint *)joint, &grooveJointClass, 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;
+}
+
+cpJoint *
+cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
+{
+ return (cpJoint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
+}
+
View
122 soylent/GoD/chipmunk/cpJoint.h
@@ -0,0 +1,122 @@
+/* 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: Comment me!
+
+extern cpFloat cp_joint_bias_coef;
+
+typedef enum cpJointType {
+ CP_PIN_JOINT,
+ CP_PIVOT_JOINT,
+ CP_SLIDE_JOINT,
+ CP_GROOVE_JOINT,
+ CP_CUSTOM_JOINT, // For user definable joint types.
+} cpJointType;
+
+struct cpJoint;
+struct cpJointClass;
+
+typedef struct cpJointClass {
+ cpJointType type;
+
+ void (*preStep)(struct cpJoint *joint, cpFloat dt_inv);
+ void (*applyImpulse)(struct cpJoint *joint);
+} cpJointClass;
+
+typedef struct cpJoint {
+ const cpJointClass *klass;
+
+ cpBody *a, *b;
+} cpJoint;
+
+void cpJointDestroy(cpJoint *joint);
+void cpJointFree(cpJoint *joint);
+
+
+typedef struct cpPinJoint {
+ cpJoint joint;
+ cpVect anchr1, anchr2;
+ cpFloat dist;
+
+ cpVect r1, r2;
+ cpVect n;
+ cpFloat nMass;
+
+ cpFloat jnAcc, jBias;
+ cpFloat bias;
+} cpPinJoint;
+
+cpPinJoint *cpPinJointAlloc(void);
+cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+cpJoint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
+
+
+typedef struct cpSlideJoint {
+ cpJoint joint;
+ cpVect anchr1, anchr2;
+ cpFloat min, max;
+
+ cpVect r1, r2;
+ cpVect n;
+ cpFloat nMass;
+
+ cpFloat jnAcc, jBias;
+ cpFloat bias;
+} cpSlideJoint;
+
+cpSlideJoint *cpSlideJointAlloc(void);
+cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+cpJoint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+
+
+typedef struct cpPivotJoint {
+ cpJoint joint;
+ cpVect anchr1, anchr2;
+
+ cpVect r1, r2;
+ cpVect k1, k2;
+
+ cpVect jAcc, jBias;
+ cpVect bias;
+} cpPivotJoint;
+
+cpPivotJoint *cpPivotJointAlloc(void);
+cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot);
+cpJoint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot);
+
+
+typedef struct cpGrooveJoint {
+ cpJoint joint;
+ cpVect grv_n, grv_a, grv_b;
+ cpVect anchr2;
+
+ cpVect grv_tn;
+ cpFloat clamp;
+ cpVect r1, r2;
+ cpVect k1, k2;
+
+ cpVect jAcc, jBias;
+ cpVect bias;
+} cpGrooveJoint;
+
+cpGrooveJoint *cpGrooveJointAlloc(void);
+cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
+cpJoint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
View
139 soylent/GoD/chipmunk/cpPolyShape.c
@@ -0,0 +1,139 @@
+/* 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"
+
+cpPolyShape *
+cpPolyShapeAlloc(void)
+{
+ return (cpPolyShape *)calloc(1, sizeof(cpPolyShape));
+}
+
+static void
+cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
+{
+ cpVect *src = poly->verts;
+ cpVect *dst = poly->tVerts;
+
+ for(int i=0; i<poly->numVerts; i++)
+ dst[i] = cpvadd(p, cpvrotate(src[i], rot));
+}
+
+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;
+
+ cpFloat l, b, r, t;
+
+ cpPolyShapeTransformAxes(poly, p, rot);
+ cpPolyShapeTransformVerts(poly, p, rot);
+
+ cpVect *verts = poly->tVerts;
+ l = r = verts[0].x;
+ b = t = verts[0].y;
+
+ // TODO do as part of cpPolyShapeTransformVerts?
+ for(int i=1; i<poly->numVerts; i++){