chipmunk 4.1.0-x86-mswin32

data/ext/chipmunk/cpBody.h

@@ -0,0 +1,132 @@
+/* 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{
+	// *** 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 cpBodySetMass() whenever changing the mass 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.
+	void *data;
+	
+	// *** Internally Used Fields
+	
+	// Velocity bias values used when solving penetrations and correcting joints.
+	cpVect v_bias;
+	cpFloat w_bias;
+	
+//	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 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(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);

data/ext/chipmunk/cpCollision.c

@@ -0,0 +1,390 @@
+/* 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(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
+			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(cpPolyShapeContainsVertPartial(poly1, v, n))
+			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);
+	}
+	
+	// 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(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;
+}