chipmunk 5.3.4.5 → 6.1.3.0.rc1

data/ext/chipmunk/cpPolyShape.h

@@ -19,85 +19,49 @@
  * SOFTWARE.
  */
 
-// Axis structure used by cpPolyShape.
-typedef struct cpPolyShapeAxis{
-	// normal
+/// @defgroup cpPolyShape cpPolyShape
+/// @{
+
+/// @private
+typedef struct cpSplittingPlane {
 	cpVect n;
-	// distance from origin
 	cpFloat d;
-} cpPolyShapeAxis;
+} cpSplittingPlane;
 
-// Convex polygon shape structure.
-typedef struct cpPolyShape{
-	CP_PRIVATE(cpShape shape);
+/// @private
+typedef struct cpPolyShape {
+	cpShape shape;
 	
-	// Vertex and axis lists.
-	CP_PRIVATE(int numVerts);
-	CP_PRIVATE(cpVect *verts);
-	CP_PRIVATE(cpPolyShapeAxis *axes);
-
-	// Transformed vertex and axis lists.
-	CP_PRIVATE(cpVect *tVerts);
-	CP_PRIVATE(cpPolyShapeAxis *tAxes);
+	int numVerts;
+	cpVect *verts, *tVerts;
+	cpSplittingPlane *planes, *tPlanes;
 } cpPolyShape;
 
-// Basic allocation functions.
-cpPolyShape *cpPolyShapeAlloc(void);
-cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset);
-cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset);
+/// Allocate a polygon shape.
+cpPolyShape* cpPolyShapeAlloc(void);
+/// Initialize a polygon shape.
+/// A convex hull will be created from the vertexes.
+cpPolyShape* cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset);
+/// Allocate and initialize a polygon shape.
+/// A convex hull will be created from the vertexes.
+cpShape* cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset);
 
-cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height);
-cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
+/// Initialize a box shaped polygon shape.
+cpPolyShape* cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height);
+/// Initialize an offset box shaped polygon shape.
+cpPolyShape* cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box);
+/// Allocate and initialize a box shaped polygon shape.
+cpShape* cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
+/// Allocate and initialize an offset box shaped polygon shape.
+cpShape* cpBoxShapeNew2(cpBody *body, cpBB box);
 
-// Check that a set of vertexes has a correct winding and that they are convex
+/// Check that a set of vertexes is convex and has a clockwise winding.
+/// NOTE: Due to floating point precision issues, hulls created with cpQuickHull() are not guaranteed to validate!
 cpBool cpPolyValidate(const cpVect *verts, const int numVerts);
 
+/// Get the number of verts in a polygon shape.
 int cpPolyShapeGetNumVerts(cpShape *shape);
+/// Get the @c ith vertex of a polygon shape.
 cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
 
-// *** inlined utility functions
-
-// Returns the minimum distance of the polygon to the axis.
-static inline cpFloat
-cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
-{
-	cpVect *verts = poly->CP_PRIVATE(tVerts);
-	cpFloat min = cpvdot(n, verts[0]);
-	
-	int i;
-	for(i=1; i<poly->CP_PRIVATE(numVerts); i++)
-		min = cpfmin(min, cpvdot(n, verts[i]));
-	
-	return min - d;
-}
-
-// Returns true if the polygon contains the vertex.
-static inline cpBool
-cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
-{
-	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
-	
-	int i;
-	for(i=0; i<poly->CP_PRIVATE(numVerts); i++){
-		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
-		if(dist > 0.0f) return cpFalse;
-	}
-	
-	return cpTrue;
-}
-
-// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal.
-static inline cpBool
-cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
-{
-	cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
-	
-	int i;
-	for(i=0; i<poly->CP_PRIVATE(numVerts); i++){
-		if(cpvdot(axes[i].n, n) < 0.0f) continue;
-		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
-		if(dist > 0.0f) return cpFalse;
-	}
-	
-	return cpTrue;
-}
+/// @}

data/ext/chipmunk/cpRatchetJoint.c

@@ -19,16 +19,14 @@
  * SOFTWARE.
  */
 
-#include <stdlib.h>
-#include <math.h>
-
 #include "chipmunk_private.h"
 #include "constraints/util.h"
 
 static void
-preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
+preStep(cpRatchetJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	cpFloat angle = joint->angle;
 	cpFloat phase = joint->phase;
@@ -49,35 +47,41 @@ preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
 	
 	// calculate bias velocity
 	cpFloat maxBias = joint->constraint.maxBias;
-	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
-	
-	// compute max impulse
-	joint->jMax = J_MAX(joint, dt);
+	joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
 
 	// If the bias is 0, the joint is not at a limit. Reset the impulse.
-	if(!joint->bias)
-		joint->jAcc = 0.0f;
+	if(!joint->bias) joint->jAcc = 0.0f;
+}
 
-	// apply joint torque
-	a->w -= joint->jAcc*a->i_inv;
-	b->w += joint->jAcc*b->i_inv;
+static void
+applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef)
+{
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
+	
+	cpFloat j = joint->jAcc*dt_coef;
+	a->w -= j*a->i_inv;
+	b->w += j*b->i_inv;
 }
 
 static void
-applyImpulse(cpRatchetJoint *joint)
+applyImpulse(cpRatchetJoint *joint, cpFloat dt)
 {
 	if(!joint->bias) return; // early exit
 
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w - a->w;
 	cpFloat ratchet = joint->ratchet;
 	
+	cpFloat jMax = joint->constraint.maxForce*dt;
+	
 	// compute normal impulse	
 	cpFloat j = -(joint->bias + wr)*joint->iSum;
 	cpFloat jOld = joint->jAcc;
-	joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet;
+	joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, jMax*cpfabs(ratchet))/ratchet;
 	j = joint->jAcc - jOld;
 	
 	// apply impulse
@@ -92,16 +96,17 @@ getImpulse(cpRatchetJoint *joint)
 }
 
 static const cpConstraintClass klass = {
-	(cpConstraintPreStepFunction)preStep,
-	(cpConstraintApplyImpulseFunction)applyImpulse,
-	(cpConstraintGetImpulseFunction)getImpulse,
+	(cpConstraintPreStepImpl)preStep,
+	(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
+	(cpConstraintApplyImpulseImpl)applyImpulse,
+	(cpConstraintGetImpulseImpl)getImpulse,
 };
 CP_DefineClassGetter(cpRatchetJoint)
 
 cpRatchetJoint *
 cpRatchetJointAlloc(void)
 {
-	return (cpRatchetJoint *)cpmalloc(sizeof(cpRatchetJoint));
+	return (cpRatchetJoint *)cpcalloc(1, sizeof(cpRatchetJoint));
 }
 
 cpRatchetJoint *

data/ext/chipmunk/cpRotaryLimitJoint.c

@@ -19,15 +19,14 @@
  * SOFTWARE.
  */
 
-#include <stdlib.h>
-
 #include "chipmunk_private.h"
 #include "constraints/util.h"
 
 static void
-preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
+preStep(cpRotaryLimitJoint *joint, cpFloat dt)
 {
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	cpFloat dist = b->a - a->a;
 	cpFloat pdist = 0.0f;
@@ -38,41 +37,47 @@ preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
 	}
 	
 	// calculate moment of inertia coefficient.
-	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
+	joint->iSum = 1.0f/(1.0f/a->i + 1.0f/b->i);
 	
 	// calculate bias velocity
 	cpFloat maxBias = joint->constraint.maxBias;
-	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
-	
-	// compute max impulse
-	joint->jMax = J_MAX(joint, dt);
+	joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
 
 	// If the bias is 0, the joint is not at a limit. Reset the impulse.
-	if(!joint->bias)
-		joint->jAcc = 0.0f;
+	if(!joint->bias) joint->jAcc = 0.0f;
+}
 
-	// apply joint torque
-	a->w -= joint->jAcc*a->i_inv;
-	b->w += joint->jAcc*b->i_inv;
+static void
+applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef)
+{
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
+	
+	cpFloat j = joint->jAcc*dt_coef;
+	a->w -= j*a->i_inv;
+	b->w += j*b->i_inv;
 }
 
 static void
-applyImpulse(cpRotaryLimitJoint *joint)
+applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt)
 {
 	if(!joint->bias) return; // early exit
 
-	CONSTRAINT_BEGIN(joint, a, b);
+	cpBody *a = joint->constraint.a;
+	cpBody *b = joint->constraint.b;
 	
 	// compute relative rotational velocity
 	cpFloat wr = b->w - a->w;
 	
+	cpFloat jMax = joint->constraint.maxForce*dt;
+	
 	// compute normal impulse	
 	cpFloat j = -(joint->bias + wr)*joint->iSum;
 	cpFloat jOld = joint->jAcc;
 	if(joint->bias < 0.0f){
-		joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax);
+		joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax);
 	} else {
-		joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f);
+		joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f);
 	}
 	j = joint->jAcc - jOld;
 	
@@ -88,16 +93,17 @@ getImpulse(cpRotaryLimitJoint *joint)
 }
 
 static const cpConstraintClass klass = {
-	(cpConstraintPreStepFunction)preStep,
-	(cpConstraintApplyImpulseFunction)applyImpulse,
-	(cpConstraintGetImpulseFunction)getImpulse,
+	(cpConstraintPreStepImpl)preStep,
+	(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
+	(cpConstraintApplyImpulseImpl)applyImpulse,
+	(cpConstraintGetImpulseImpl)getImpulse,
 };
 CP_DefineClassGetter(cpRotaryLimitJoint)
 
 cpRotaryLimitJoint *
 cpRotaryLimitJointAlloc(void)
 {
-	return (cpRotaryLimitJoint *)cpmalloc(sizeof(cpRotaryLimitJoint));
+	return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint));
 }
 
 cpRotaryLimitJoint *

data/ext/chipmunk/cpShape.c

@@ -19,24 +19,21 @@
  * SOFTWARE.
  */
  
-#include <stdlib.h>
-#include <stdio.h>
-#include <math.h>
-
 #include "chipmunk_private.h"
 #include "chipmunk_unsafe.h"
 
 #define CP_DefineShapeGetter(struct, type, member, name) \
 CP_DeclareShapeGetter(struct, type, name){ \
-	cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \
+	cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \
 	return ((struct *)shape)->member; \
 }
-cpHashValue SHAPE_ID_COUNTER = 0;
+
+static cpHashValue cpShapeIDCounter = 0;
 
 void
 cpResetShapeIdCounter(void)
 {
-	SHAPE_ID_COUNTER = 0;
+	cpShapeIDCounter = 0;
 }
 
 
@@ -45,8 +42,8 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
 {
 	shape->klass = klass;
 	
-	shape->hashid = SHAPE_ID_COUNTER;
-	SHAPE_ID_COUNTER++;
+	shape->hashid = cpShapeIDCounter;
+	cpShapeIDCounter++;
 	
 	shape->body = body;
 	shape->sensor = 0;
@@ -60,9 +57,11 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
 	shape->layers = CP_ALL_LAYERS;
 	
 	shape->data = NULL;
-	shape->next = NULL;
 	
-//	cpShapeCacheBB(shape);
+	shape->space = NULL;
+	
+	shape->next = NULL;
+	shape->prev = NULL;
 	
 	return shape;
 }
@@ -70,9 +69,7 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
 void
 cpShapeDestroy(cpShape *shape)
 {
-	if(shape->klass && shape->klass->destroy) { 
-    shape->klass->destroy(shape);
-  } 
+	if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape);
 }
 
 void
@@ -84,68 +81,85 @@ cpShapeFree(cpShape *shape)
 	}
 }
 
-// TODO this function should really take a position and rotation explicitly and be renamed
+void
+cpShapeSetBody(cpShape *shape, cpBody *body)
+{
+	cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body.");
+	shape->body = body;
+}
+
 cpBB
 cpShapeCacheBB(cpShape *shape)
 {
 	cpBody *body = shape->body;
-	
-	shape->bb = shape->klass->cacheData(shape, body->p, body->rot);
-	return shape->bb;
+	return cpShapeUpdate(shape, body->p, body->rot);
+}
+
+cpBB
+cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot)
+{
+	return (shape->bb = shape->klass->cacheData(shape, pos, rot));
 }
 
 cpBool
 cpShapePointQuery(cpShape *shape, cpVect p){
-	return shape->klass->pointQuery(shape, p);
+	cpNearestPointQueryInfo info = {NULL, cpvzero, INFINITY};
+	cpShapeNearestPointQuery(shape, p, &info);
+	
+	return (info.d < 0.0f);
+}
+
+cpFloat
+cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpNearestPointQueryInfo *info)
+{
+	cpNearestPointQueryInfo blank = {NULL, cpvzero, INFINITY};
+	if(info){
+		(*info) = blank;
+	} else {
+		info = &blank;
+	}
+	
+	shape->klass->nearestPointQuery(shape, p, info);
+	return info->d;
 }
 
+
 cpBool
 cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
 	cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero};
-	(*info) = blank;
+	if(info){
+		(*info) = blank;
+	} else {
+		info = &blank;
+	}
 	
 	shape->klass->segmentQuery(shape, a, b, info);
 	return (info->shape != NULL);
 }
 
-void
-cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
-{
-	printf("Segment Query:\n");
-	printf("\tt: %f\n", info->t);
-//	printf("\tdist: %f\n", info->dist);
-//	printf("\tpoint: %s\n", cpvstr(info->point));
-	printf("\tn: %s\n", cpvstr(info->n));
-}
-
-
-
-
 cpCircleShape *
 cpCircleShapeAlloc(void)
 {
 	return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
 }
 
-static inline cpBB
-bbFromCircle(const cpVect c, const cpFloat r)
+static cpBB
+cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot)
 {
-	return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
+	cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
+	return cpBBNewForCircle(c, circle->r);
 }
 
-static cpBB
-cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+static void
+cpCicleShapeNearestPointQuery(cpCircleShape *circle, cpVect p, cpNearestPointQueryInfo *info)
 {
-	cpCircleShape *circle = (cpCircleShape *)shape;
+	cpVect delta = cpvsub(p, circle->tc);
+	cpFloat d = cpvlength(delta);
+	cpFloat r = circle->r;
 	
-	circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
-	return bbFromCircle(circle->tc, circle->r);
-}
-
-static cpBool
-cpCircleShapePointQuery(cpShape *shape, cpVect p){
-	cpCircleShape *circle = (cpCircleShape *)shape;
-	return cpvnear(circle->tc, p, circle->r);
+	info->shape = (cpShape *)circle;
+	info->p = cpvadd(circle->tc, cpvmult(delta, r/d)); // TODO div/0
+	info->d = d - r;
 }
 
 static void
@@ -172,18 +186,17 @@ circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b,
 }
 
 static void
-cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpSegmentQueryInfo *info)
 {
-	cpCircleShape *circle = (cpCircleShape *)shape;
-	circleSegmentQuery(shape, circle->tc, circle->r, a, b, info);
+	circleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, info);
 }
 
 static const cpShapeClass cpCircleShapeClass = {
 	CP_CIRCLE_SHAPE,
-	cpCircleShapeCacheData,
+	(cpShapeCacheDataImpl)cpCircleShapeCacheData,
 	NULL,
-	cpCircleShapePointQuery,
-	cpCircleShapeSegmentQuery,
+	(cpShapeNearestPointQueryImpl)cpCicleShapeNearestPointQuery,
+	(cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery,
 };
 
 cpCircleShape *
@@ -213,15 +226,13 @@ cpSegmentShapeAlloc(void)
 }
 
 static cpBB
-cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot)
 {
-	cpSegmentShape *seg = (cpSegmentShape *)shape;
-	
 	seg->ta = cpvadd(p, cpvrotate(seg->a, rot));
 	seg->tb = cpvadd(p, cpvrotate(seg->b, rot));
 	seg->tn = cpvrotate(seg->n, rot);
 	
-	cpFloat l,r,s,t;
+	cpFloat l,r,b,t;
 	
 	if(seg->ta.x < seg->tb.x){
 		l = seg->ta.x;
@@ -232,96 +243,61 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
 	}
 	
 	if(seg->ta.y < seg->tb.y){
-		s = seg->ta.y;
+		b = seg->ta.y;
 		t = seg->tb.y;
 	} else {
-		s = seg->tb.y;
+		b = seg->tb.y;
 		t = seg->ta.y;
 	}
 	
 	cpFloat rad = seg->r;
-	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
+	return cpBBNew(l - rad, b - rad, r + rad, t + rad);
 }
 
-static cpBool
-cpSegmentShapePointQuery(cpShape *shape, cpVect p){
-	if(!cpBBcontainsVect(shape->bb, p)) return cpFalse;
-	
-	cpSegmentShape *seg = (cpSegmentShape *)shape;
-	
-	// Calculate normal distance from segment.
-	cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
-	cpFloat dist = cpfabs(dn) - seg->r;
-	if(dist > 0.0f) return cpFalse;
+static void
+cpSegmentShapeNearestPointQuery(cpSegmentShape *seg, cpVect p, cpNearestPointQueryInfo *info)
+{
+	cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb);
 	
-	// Calculate tangential distance along segment.
-	cpFloat dt = -cpvcross(seg->tn, p);
-	cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
-	cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
+	cpVect delta = cpvsub(p, closest);
+	cpFloat d = cpvlength(delta);
+	cpFloat r = seg->r;
 	
-	// Decision tree to decide which feature of the segment to collide with.
-	if(dt <= dtMin){
-		if(dt < (dtMin - seg->r)){
-			return cpFalse;
-		} else {
-			return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
-		}
-	} else {
-		if(dt < dtMax){
-			return cpTrue;
-		} else {
-			if(dt < (dtMax + seg->r)) {
-				return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
-			} else {
-				return cpFalse;
-			}
-		}
-	}
-	
-	return cpTrue;	
+	info->shape = (cpShape *)seg;
+	info->p = (d ? cpvadd(closest, cpvmult(delta, r/d)) : closest);
+	info->d = d - r;
 }
 
-static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
-
 static void
-cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpSegmentQueryInfo *info)
 {
-	// TODO this function could be optimized better.
-	
-	cpSegmentShape *seg = (cpSegmentShape *)shape;
 	cpVect n = seg->tn;
-	// flip n if a is behind the axis
-	if(cpvdot(a, n) < cpvdot(seg->ta, n))
-		n = cpvneg(n);
+	cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
+	cpFloat r = seg->r;
 	
-	cpFloat an = cpvdot(a, n);
-	cpFloat bn = cpvdot(b, n);
+	cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
+	cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
 	
-	if(an != bn){
-		cpFloat d = cpvdot(seg->ta, n) + seg->r;
-		cpFloat t = (d - an)/(bn - an);
+	// Make the endpoints relative to 'a' and move them by the thickness of the segment.
+	cpVect seg_a = cpvadd(seg->ta, seg_offset);
+	cpVect seg_b = cpvadd(seg->tb, seg_offset);
+	cpVect delta = cpvsub(b, a);
+	
+	if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
+		cpFloat d_offset = d + (d > 0.0f ? -r : r);
+		cpFloat ad = -d_offset;
+		cpFloat bd = cpvdot(delta, n) - d_offset;
 		
-		if(0.0f < t && t < 1.0f){
-			cpVect point = cpvlerp(a, b, t);
-			cpFloat dt = -cpvcross(seg->tn, point);
-			cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
-			cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
-			
-			if(dtMin < dt && dt < dtMax){
-				info->shape = shape;
-				info->t = t;
-				info->n = n;
-				
-				return; // don't continue on and check endcaps
-			}
+		if(ad*bd < 0.0f){
+			info->shape = (cpShape *)seg;
+			info->t = ad/(ad - bd);
+			info->n = flipped_n;
 		}
-	}
-	
-	if(seg->r) {
+	} else if(r != 0.0f){
 		cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero};
 		cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero};
-		circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
-		circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);
+		circleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, &info1);
+		circleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, &info2);
 		
 		if(info1.t < info2.t){
 			(*info) = info1;
@@ -333,10 +309,10 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf
 
 static const cpShapeClass cpSegmentShapeClass = {
 	CP_SEGMENT_SHAPE,
-	cpSegmentShapeCacheData,
+	(cpShapeCacheDataImpl)cpSegmentShapeCacheData,
 	NULL,
-	cpSegmentShapePointQuery,
-	cpSegmentShapeSegmentQuery,
+	(cpShapeNearestPointQueryImpl)cpSegmentShapeNearestPointQuery,
+	(cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery,
 };
 
 cpSegmentShape *
@@ -348,6 +324,9 @@ cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloa
 	
 	seg->r = r;
 	
+	seg->a_tangent = cpvzero;
+	seg->b_tangent = cpvzero;
+	
 	cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body);
 	
 	return seg;
@@ -364,12 +343,22 @@ CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B)
 CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal)
 CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius)
 
+void
+cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next)
+{
+	cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = (cpSegmentShape *)shape;
+	
+	seg->a_tangent = cpvsub(prev, seg->a);
+	seg->b_tangent = cpvsub(next, seg->b);
+}
+
 // Unsafe API (chipmunk_unsafe.h)
 
 void
 cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
 {
-	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 	cpCircleShape *circle = (cpCircleShape *)shape;
 	
 	circle->r = radius;
@@ -378,7 +367,7 @@ cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
 void
 cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
 {
-	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 	cpCircleShape *circle = (cpCircleShape *)shape;
 	
 	circle->c = offset;
@@ -387,7 +376,7 @@ cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
 void
 cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
 {
-	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 	cpSegmentShape *seg = (cpSegmentShape *)shape;
 	
 	seg->a = a;
@@ -398,7 +387,7 @@ cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
 void
 cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
 {
-	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 	cpSegmentShape *seg = (cpSegmentShape *)shape;
 	
 	seg->r = radius;