--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trunk/chipmunkd/cpShape.d	Thu Dec 02 02:11:26 2010 +0100
@@ -0,0 +1,533 @@
+
+// written in the D programming language
+
+module chipmunkd.cpShape;
+
+import chipmunkd.cpBody;
+import chipmunkd.chipmunk;
+import chipmunkd.chipmunk_types_h;
+import chipmunkd.cpVect_h;
+import chipmunkd.cpBB;
+
+import std.stdio;
+
+struct cpSegmentQueryInfo {
+	cpShape *shape; // shape that was hit, null if no collision
+	cpFloat t; // Distance along query segment, will always be in the range [0, 1].
+	cpVect n; // normal of hit surface
+}
+
+// Enumeration of shape types.
+enum cpShapeType{
+	CP_CIRCLE_SHAPE,
+	CP_SEGMENT_SHAPE,
+	CP_POLY_SHAPE,
+	CP_NUM_SHAPES
+}
+
+// Shape class. Holds function pointers and type data.
+struct cpShapeClass {
+	cpShapeType type;
+	
+	// Called by cpShapeCacheBB().
+	cpBB function(cpShape *shape, cpVect p, cpVect rot) cacheData;
+	// Called to by cpShapeDestroy().
+	void function(cpShape *shape) destroy;
+	
+	// called by cpShapePointQuery().
+	cpBool function(cpShape *shape, cpVect p)pointQuery;
+	
+	// called by cpShapeSegmentQuery()
+	 void function(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) segmentQuery;
+}
+
+// Basic shape struct that the others inherit from.
+struct cpShape{
+	// The "class" of a shape as defined above 
+	/+const+/ cpShapeClass *klass;
+	
+	// cpBody that the shape is attached to.
+	cpBody* _body;
+	
+	// Cached BBox for the shape.
+	cpBB bb;
+	
+	// Sensors invoke callbacks, but do not generate collisions
+	cpBool sensor;
+	
+	// *** Surface properties.
+	
+	// Coefficient of restitution. (elasticity)
+	cpFloat e;
+	// Coefficient of friction.
+	cpFloat u;
+	// Surface velocity used when solving for friction.
+	cpVect surface_v;
+	
+	// *** User Definable Fields
+	
+	// User defined data pointer for the shape.
+	cpDataPointer data;
+	
+	// User defined collision type for the shape.
+	cpCollisionType collision_type;
+	// User defined collision group for the shape.
+	cpGroup group;
+	// User defined layer bitmask for the shape.
+	cpLayers layers;
+	
+	// *** Internally Used Fields
+	
+	// Shapes form a linked list when added to space on a non-null body
+	cpShape* next;
+	
+	// Unique id used as the hash value.
+	cpHashValue hashid;
+}
+
+//
+//// Low level shape initialization func.
+//cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body);
+//
+//// Basic destructor functions. (allocation functions are not shared)
+//void cpShapeDestroy(cpShape *shape);
+//void cpShapeFree(cpShape *shape);
+//
+//// Cache the BBox of the shape.
+//cpBB cpShapeCacheBB(cpShape *shape);
+//
+//// Test if a point lies within a shape.
+//cpBool cpShapePointQuery(cpShape *shape, cpVect p);
+//
+//#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape)
+//
+
+// Circle shape structure.
+struct cpCircleShape{
+	cpShape shape;
+	
+	// Center in body space coordinates
+	cpVect c;
+	// Radius.
+	cpFloat r;
+	
+	// Transformed center. (world space coordinates)
+	cpVect tc;
+}
+
+//// Basic allocation functions for cpCircleShape.
+//cpCircleShape *cpCircleShapeAlloc(void);
+//cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset);
+//cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset);
+//
+//CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset);
+//CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius);
+
+// Segment shape structure.
+struct cpSegmentShape{
+	cpShape shape;
+	
+	// Endpoints and normal of the segment. (body space coordinates)
+	cpVect a, b, n;
+	// Radius of the segment. (Thickness)
+	cpFloat r;
+
+	// Transformed endpoints and normal. (world space coordinates)
+	cpVect ta, tb, tn;
+}
+//
+//// Basic allocation functions for cpSegmentShape.
+//cpSegmentShape* cpSegmentShapeAlloc(void);
+//cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius);
+//cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius);
+//
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, A);
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, B);
+//CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal);
+//CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius);
+//
+//// For determinism, you can reset the shape id counter.
+//void cpResetShapeIdCounter(void);
+//
+//// Directed segment queries against individual shapes.
+//void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);
+//
+//cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
+//
+static cpVect
+cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvlerp(start, end, info.t);
+}
+
+static cpFloat
+cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
+{
+	return cpvdist(start, end)*info.t;
+}
+
+cpHashValue SHAPE_ID_COUNTER = 0;
+
+void
+cpResetShapeIdCounter()
+{
+	SHAPE_ID_COUNTER = 0;
+}
+
+cpShape*
+cpShapeInit(cpShape *shape, /+const+/ cpShapeClass *klass, cpBody *_body)
+{
+	shape.klass = klass;
+	
+	shape.hashid = SHAPE_ID_COUNTER;
+	SHAPE_ID_COUNTER++;
+	
+	shape._body = _body;
+	shape.sensor = 0;
+	
+	shape.e = 0.0f;
+	shape.u = 0.0f;
+	shape.surface_v = cpvzero;
+	
+	shape.collision_type = 0;
+	shape.group = CP_NO_GROUP;
+	shape.layers = CP_ALL_LAYERS;
+	
+	shape.data = null;
+	shape.next = null;
+	
+//	cpShapeCacheBB(shape);
+	
+	return shape;
+}
+
+void
+cpShapeDestroy(cpShape *shape)
+{
+	if(shape.klass.destroy) shape.klass.destroy(shape);
+}
+
+void
+cpShapeFree(cpShape *shape)
+{
+	if(shape){
+		cpShapeDestroy(shape);
+		cpfree(shape);
+	}
+}
+
+cpBB
+cpShapeCacheBB(cpShape *shape)
+{
+	cpBody *_body = shape._body;
+	
+	shape.bb = shape.klass.cacheData(shape, _body.p, _body.rot);
+	return shape.bb;
+}
+
+cpBool
+cpShapePointQuery(cpShape *shape, cpVect p){
+	return shape.klass.pointQuery(shape, p);
+}
+
+cpBool
+cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
+	cpSegmentQueryInfo blank = {null, 0.0f, cpvzero};
+	(*info) = blank;
+	
+	shape.klass.segmentQuery(shape, a, b, info);
+	return (info.shape !is null);
+}
+
+void
+cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
+{
+	writefln("Segment Query:\n");
+	writefln("\tt: %s\n", info.t);
+//	writefln("\tdist: %f\n", info.dist);
+//	writefln("\tpoint: %s\n", cpvstr(info.point));
+	//writefln("\tn: %s\n", cpvstr(info.n)); //TODO:
+}
+
+cpCircleShape *
+cpCircleShapeAlloc()
+{
+	return cast(cpCircleShape *)cpcalloc(1, cpCircleShape.sizeof);
+}
+
+static cpBB
+bbFromCircle(const cpVect c, const cpFloat r)
+{
+	return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
+}
+
+static cpBB
+cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.tc = cpvadd(p, cpvrotate(circle.c, rot));
+	return bbFromCircle(circle.tc, circle.r);
+}
+
+static cpBool
+cpCircleShapePointQuery(cpShape *shape, cpVect p){
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	return cpvnear(circle.tc, p, circle.r);
+}
+
+static void
+circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// offset the line to be relative to the circle
+	a = cpvsub(a, center);
+	b = cpvsub(b, center);
+	
+	cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
+	cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
+	cpFloat qc = cpvdot(a, a) - r*r;
+	
+	cpFloat det = qb*qb - 4.0f*qa*qc;
+	
+	if(det >= 0.0f){
+		cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
+		if(0.0f<= t && t <= 1.0f){
+			info.shape = shape;
+			info.t = t;
+			info.n = cpvnormalize(cpvlerp(a, b, t));
+		}
+	}
+}
+
+static void
+cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	circleSegmentQuery(shape, circle.tc, circle.r, a, b, info);
+}
+
+static /+const+/ cpShapeClass cpCircleShapeClass = {
+	cpShapeType.CP_CIRCLE_SHAPE,
+	&cpCircleShapeCacheData,
+	null,
+	&cpCircleShapePointQuery,
+	&cpCircleShapeSegmentQuery,
+};
+
+cpCircleShape *
+cpCircleShapeInit(cpCircleShape *circle, cpBody *_body, cpFloat radius, cpVect offset)
+{
+	circle.c = offset;
+	circle.r = radius;
+	
+	cpShapeInit(cast(cpShape *)circle, &cpCircleShapeClass, _body);
+	
+	return circle;
+}
+
+cpShape *
+cpCircleShapeNew(cpBody *_body, cpFloat radius, cpVect offset)
+{
+	return cast(cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), _body, radius, offset);
+}
+//TODO:
+//CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset)
+//CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius)
+
+cpSegmentShape *
+cpSegmentShapeAlloc()
+{
+	return cast(cpSegmentShape *)cpcalloc(1, cpSegmentShape.sizeof);
+}
+
+static cpBB
+cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
+{
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.ta = cpvadd(p, cpvrotate(seg.a, rot));
+	seg.tb = cpvadd(p, cpvrotate(seg.b, rot));
+	seg.tn = cpvrotate(seg.n, rot);
+	
+	cpFloat l,r,s,t;
+	
+	if(seg.ta.x < seg.tb.x){
+		l = seg.ta.x;
+		r = seg.tb.x;
+	} else {
+		l = seg.tb.x;
+		r = seg.ta.x;
+	}
+	
+	if(seg.ta.y < seg.tb.y){
+		s = seg.ta.y;
+		t = seg.tb.y;
+	} else {
+		s = seg.tb.y;
+		t = seg.ta.y;
+	}
+	
+	cpFloat rad = seg.r;
+	return cpBBNew(l - rad, s - rad, r + rad, t + rad);
+}
+
+static cpBool
+cpSegmentShapePointQuery(cpShape *shape, cpVect p){
+	if(!cpBBcontainsVect(shape.bb, p)) return cpFalse;
+	
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	// Calculate normal distance from segment.
+	cpFloat dn = cpvdot(seg.tn, p) - cpvdot(seg.ta, seg.tn);
+	cpFloat dist = cpfabs(dn) - seg.r;
+	if(dist > 0.0f) return cpFalse;
+	
+	// Calculate tangential distance along segment.
+	cpFloat dt = -cpvcross(seg.tn, p);
+	cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
+	cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
+	
+	// Decision tree to decide which feature of the segment to collide with.
+	if(dt <= dtMin){
+		if(dt < (dtMin - seg.r)){
+			return cpFalse;
+		} else {
+			return cpvlengthsq(cpvsub(seg.ta, p)) < (seg.r*seg.r);
+		}
+	} else {
+		if(dt < dtMax){
+			return cpTrue;
+		} else {
+			if(dt < (dtMax + seg.r)) {
+				return cpvlengthsq(cpvsub(seg.tb, p)) < (seg.r*seg.r);
+			} else {
+				return cpFalse;
+			}
+		}
+	}
+	
+	return cpTrue;	
+}
+
+static cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
+
+static void
+cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
+{
+	// TODO this function could be optimized better.
+	
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	cpVect n = seg.tn;
+	// flip n if a is behind the axis
+	if(cpvdot(a, n) < cpvdot(seg.ta, n))
+		n = cpvneg(n);
+	
+	cpFloat an = cpvdot(a, n);
+	cpFloat bn = cpvdot(b, n);
+	
+	if(an != bn){
+		cpFloat d = cpvdot(seg.ta, n) + seg.r;
+		cpFloat t = (d - an)/(bn - an);
+		
+		if(0.0f < t && t < 1.0f){
+			cpVect point = cpvlerp(a, b, t);
+			cpFloat dt = -cpvcross(seg.tn, point);
+			cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
+			cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
+			
+			if(dtMin < dt && dt < dtMax){
+				info.shape = shape;
+				info.t = t;
+				info.n = n;
+				
+				return; // don't continue on and check endcaps
+			}
+		}
+	}
+	
+	if(seg.r) {
+		cpSegmentQueryInfo info1 = {null, 1.0f, cpvzero};
+		cpSegmentQueryInfo info2 = {null, 1.0f, cpvzero};
+		circleSegmentQuery(shape, seg.ta, seg.r, a, b, &info1);
+		circleSegmentQuery(shape, seg.tb, seg.r, a, b, &info2);
+		
+		if(info1.t < info2.t){
+			(*info) = info1;
+		} else {
+			(*info) = info2;
+		}
+	}
+}
+
+static /+const+/ cpShapeClass cpSegmentShapeClass = {
+	cpShapeType.CP_SEGMENT_SHAPE,
+	&cpSegmentShapeCacheData,
+	null,
+	&cpSegmentShapePointQuery,
+	&cpSegmentShapeSegmentQuery,
+};
+
+cpSegmentShape *
+cpSegmentShapeInit(cpSegmentShape *seg, cpBody *_body, cpVect a, cpVect b, cpFloat r)
+{
+	seg.a = a;
+	seg.b = b;
+	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
+	
+	seg.r = r;
+	
+	cpShapeInit(cast(cpShape *)seg, &cpSegmentShapeClass, _body);
+	
+	return seg;
+}
+
+cpShape*
+cpSegmentShapeNew(cpBody *_body, cpVect a, cpVect b, cpFloat r)
+{
+	return cast(cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), _body, a, b, r);
+}
+//TODO:
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A)
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B)
+//CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal)
+//CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius)
+
+// Unsafe API (chipmunk_unsafe.h)
+
+void
+cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.r = radius;
+}
+
+void
+cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
+{
+	assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
+	cpCircleShape *circle = cast(cpCircleShape *)shape;
+	
+	circle.c = offset;
+}
+
+void
+cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
+{
+	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.a = a;
+	seg.b = b;
+	seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
+}
+
+void
+cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
+{
+	assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
+	cpSegmentShape *seg = cast(cpSegmentShape *)shape;
+	
+	seg.r = radius;
+}