chipmunk 5.3.4.5 → 6.1.3.0.rc1

data/ext/chipmunk/cpPinJoint.c

@@ -19,16 +19,14 @@
  * 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)
+preStep(cpPinJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	joint->r1 = cpvrotate(joint->anchr1, a->rot);
 	joint->r2 = cpvrotate(joint->anchr2, b->rot);
@@ -42,29 +40,35 @@ preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
 	
 	// 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);
+	joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
+}
+
+static void
+applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef)
+{
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
-	// apply accumulated impulse
-	cpVect j = cpvmult(joint->n, joint->jnAcc);
+	cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
 	apply_impulses(a, b, joint->r1, joint->r2, j);
 }
 
 static void
-applyImpulse(cpPinJoint *joint)
+applyImpulse(cpPinJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	cpVect n = joint->n;
 
 	// compute relative velocity
 	cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
 	
+	cpFloat jnMax = joint->constraint.maxForce*dt;
+	
 	// compute normal impulse
 	cpFloat jn = (joint->bias - vrn)*joint->nMass;
 	cpFloat jnOld = joint->jnAcc;
-	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
+	joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
 	jn = joint->jnAcc - jnOld;
 	
 	// apply impulse
@@ -78,17 +82,18 @@ getImpulse(cpPinJoint *joint)
 }
 
 static const cpConstraintClass klass = {
-	(cpConstraintPreStepFunction)preStep,
-	(cpConstraintApplyImpulseFunction)applyImpulse,
-	(cpConstraintGetImpulseFunction)getImpulse,
+	(cpConstraintPreStepImpl)preStep,
+	(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
+	(cpConstraintApplyImpulseImpl)applyImpulse,
+	(cpConstraintGetImpulseImpl)getImpulse,
 };
-CP_DefineClassGetter(cpPinJoint);
+CP_DefineClassGetter(cpPinJoint)
 
 
 cpPinJoint *
 cpPinJointAlloc(void)
 {
-	return (cpPinJoint *)cpmalloc(sizeof(cpPinJoint));
+	return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint));
 }
 
 cpPinJoint *
@@ -103,6 +108,8 @@ cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect an
 	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));
+	
+	cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
 
 	joint->jnAcc = 0.0f;
 	

data/ext/chipmunk/cpPivotJoint.c

@@ -19,37 +19,40 @@
  * SOFTWARE.
  */
 
-#include <stdlib.h>
-
 #include "chipmunk_private.h"
 #include "constraints/util.h"
 
 static void
-preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
+preStep(cpPivotJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.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);
+	joint-> k = k_tensor(a, b, joint->r1, joint->r2);
 	
 	// 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);
+	joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
+}
+
+static void
+applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef)
+{
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
-	// apply accumulated impulse
-	apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
+	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
 }
 
 static void
-applyImpulse(cpPivotJoint *joint)
+applyImpulse(cpPivotJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	cpVect r1 = joint->r1;
 	cpVect r2 = joint->r2;
@@ -58,9 +61,9 @@ applyImpulse(cpPivotJoint *joint)
 	cpVect vr = relative_velocity(a, b, r1, r2);
 	
 	// compute normal impulse
-	cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
+	cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
 	cpVect jOld = joint->jAcc;
-	joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
+	joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
 	j = cpvsub(joint->jAcc, jOld);
 	
 	// apply impulse
@@ -74,16 +77,17 @@ getImpulse(cpConstraint *joint)
 }
 
 static const cpConstraintClass klass = {
-	(cpConstraintPreStepFunction)preStep,
-	(cpConstraintApplyImpulseFunction)applyImpulse,
-	(cpConstraintGetImpulseFunction)getImpulse,
+	(cpConstraintPreStepImpl)preStep,
+	(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
+	(cpConstraintApplyImpulseImpl)applyImpulse,
+	(cpConstraintGetImpulseImpl)getImpulse,
 };
 CP_DefineClassGetter(cpPivotJoint)
 
 cpPivotJoint *
 cpPivotJointAlloc(void)
 {
-	return (cpPivotJoint *)cpmalloc(sizeof(cpPivotJoint));
+	return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint));
 }
 
 cpPivotJoint *

data/ext/chipmunk/cpPolyShape.c

@@ -19,8 +19,6 @@
  * SOFTWARE.
  */
  
-#include <stdlib.h>
-
 #include "chipmunk_private.h"
 #include "chipmunk_unsafe.h"
 
@@ -30,21 +28,33 @@ cpPolyShapeAlloc(void)
 	return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
 }
 
-static void
+static cpBB
 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));
+	cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
+	cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)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;
+	cpSplittingPlane *src = poly->planes;
+	cpSplittingPlane *dst = poly->tPlanes;
 	
 	for(int i=0; i<poly->numVerts; i++){
 		cpVect n = cpvrotate(src[i].n, rot);
@@ -54,55 +64,56 @@ cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot)
 }
 
 static cpBB
-cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+cpPolyShapeCacheData(cpPolyShape *poly, 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;
+	cpBB bb = poly->shape.bb = cpPolyShapeTransformVerts(poly, p, rot);
 	
-	// TODO do as part of cpPolyShapeTransformVerts?
-	for(int i=1; i<poly->numVerts; i++){
-		cpVect v = verts[i];
-		
-		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);
+	return bb;
 }
 
 static void
-cpPolyShapeDestroy(cpShape *shape)
+cpPolyShapeDestroy(cpPolyShape *poly)
 {
-	cpPolyShape *poly = (cpPolyShape *)shape;
-	
 	cpfree(poly->verts);
-	cpfree(poly->tVerts);
-	
-	cpfree(poly->axes);
-	cpfree(poly->tAxes);
+	cpfree(poly->planes);
 }
 
-static cpBool
-cpPolyShapePointQuery(cpShape *shape, cpVect p){
-	return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
+static void
+cpPolyShapeNearestPointQuery(cpPolyShape *poly, cpVect p, cpNearestPointQueryInfo *info){
+	int count = poly->numVerts;
+	cpSplittingPlane *planes = poly->tPlanes;
+	cpVect *verts = poly->tVerts;
+	
+	cpVect v0 = verts[count - 1];
+	cpFloat minDist = INFINITY;
+	cpVect closestPoint = cpvzero;
+	cpBool outside = cpFalse;
+	
+	for(int i=0; i<count; i++){
+		if(cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = cpTrue;
+		
+		cpVect v1 = verts[i];
+		cpVect closest = cpClosetPointOnSegment(p, v0, v1);
+		
+		cpFloat dist = cpvdist(p, closest);
+		if(dist < minDist){
+			minDist = dist;
+			closestPoint = closest;
+		}
+		
+		v0 = v1;
+	}
+	
+	info->shape = (cpShape *)poly;
+	info->p = closestPoint; // TODO div/0
+	info->d = (outside ? minDist : -minDist);
 }
 
 static void
-cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
 {
-	cpPolyShape *poly = (cpPolyShape *)shape;
-	cpPolyShapeAxis *axes = poly->tAxes;
+	cpSplittingPlane *axes = poly->tPlanes;
 	cpVect *verts = poly->tVerts;
 	int numVerts = poly->numVerts;
 	
@@ -121,7 +132,7 @@ cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *
 		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
 		
 		if(dtMin <= dt && dt <= dtMax){
-			info->shape = shape;
+			info->shape = (cpShape *)poly;
 			info->t = t;
 			info->n = n;
 		}
@@ -130,10 +141,10 @@ cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *
 
 static const cpShapeClass polyClass = {
 	CP_POLY_SHAPE,
-	cpPolyShapeCacheData,
-	cpPolyShapeDestroy,
-	cpPolyShapePointQuery,
-	cpPolyShapeSegmentQuery,
+	(cpShapeCacheDataImpl)cpPolyShapeCacheData,
+	(cpShapeDestroyImpl)cpPolyShapeDestroy,
+	(cpShapeNearestPointQueryImpl)cpPolyShapeNearestPointQuery,
+	(cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery,
 };
 
 cpBool
@@ -144,8 +155,9 @@ cpPolyValidate(const cpVect *verts, const int numVerts)
 		cpVect b = verts[(i+1)%numVerts];
 		cpVect c = verts[(i+2)%numVerts];
 		
-		if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
+		if(cpvcross(cpvsub(b, a), cpvsub(c, a)) > 0.0f){
 			return cpFalse;
+		}
 	}
 	
 	return cpTrue;
@@ -154,29 +166,31 @@ cpPolyValidate(const cpVect *verts, const int numVerts)
 int
 cpPolyShapeGetNumVerts(cpShape *shape)
 {
-	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
+	cpAssertHard(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.");
+	cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
+	cpAssertHard(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)
+setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset)
 {
+	// Fail if the user attempts to pass a concave poly, or a bad winding.
+	cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL().");
+	
 	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));
+	poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect));
+	poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane));
+	poly->tVerts = poly->verts + numVerts;
+	poly->tPlanes = poly->planes + numVerts;
 	
 	for(int i=0; i<numVerts; i++){
 		cpVect a = cpvadd(offset, verts[i]);
@@ -184,17 +198,15 @@ setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
 		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
 
 		poly->verts[i] = a;
-		poly->axes[i].n = n;
-		poly->axes[i].d = cpvdot(n, a);
+		poly->planes[i].n = n;
+		poly->planes[i].d = cpvdot(n, a);
 	}
+	
 }
 
 cpPolyShape *
-cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
+cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const 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);
 
@@ -213,11 +225,17 @@ cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height)
 	cpFloat hw = width/2.0f;
 	cpFloat hh = height/2.0f;
 	
+	return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh));
+}
+
+cpPolyShape *
+cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box)
+{
 	cpVect verts[] = {
-		cpv(-hw,-hh),
-		cpv(-hw, hh),
-		cpv( hw, hh),
-		cpv( hw,-hh),
+		cpv(box.l, box.b),
+		cpv(box.l, box.t),
+		cpv(box.r, box.t),
+		cpv(box.r, box.b),
 	};
 	
 	return cpPolyShapeInit(poly, body, 4, verts, cpvzero);
@@ -229,12 +247,18 @@ cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height)
 	return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height);
 }
 
+cpShape *
+cpBoxShapeNew2(cpBody *body, cpBB box)
+{
+	return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box);
+}
+
 // 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);
+	cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
+	cpPolyShapeDestroy((cpPolyShape *)shape);
 	setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
 }