# HG changeset patch # User Extrawurst # Date 1291252286 -3600 # Node ID 7ebbd4d05553abe7022ba55f0d4f8ca8e6409eaf # Parent 81145a61c2fe48ba8286b7945c16caa0511184a3 initial commit diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/chipmunk.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/chipmunk.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,174 @@ + +// written in the D programming language + +module chipmunkd.chipmunk; +//#ifndef CHIPMUNK_HEADER +//#define CHIPMUNK_HEADER +// +//#ifdef __cplusplus +//extern "C" { +//#endif +// +//void cpMessage(const char *message, const char *condition, const char *file, int line, int isError); +//#ifdef NDEBUG +// #define cpAssertWarn(condition, message) +//#else +// #define cpAssertWarn(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 0) +//#endif + +debug +{ + //TODO: + //void cpAssertWarn(bool condition, string message){if(condition) cpMessage(message,condition,__FILE__,__LINE__, 0);} + void cpAssertWarn(bool condition, string message){} +} +else +{ + void cpAssertWarn(bool condition, string message){} +} + +// +//#ifdef NDEBUG +// #define cpAssert(condition, message) +//#else +// #define cpAssert(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 1) +//#endif +// +import chipmunkd.chipmunk_types_h; +import core.stdc.stdlib; +// +//#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) +// #endif +// +// #ifdef __GNUC__ +// #define INFINITY (__builtin_inf()) +// #endif +// +// #ifndef INFINITY +// #define INFINITY (1e1000) +// #endif +//#endif + +enum INFINITY = cpFloat.infinity; + +// Maximum allocated size for various Chipmunk buffer sizes +enum CP_BUFFER_BYTES = (32*1024); + +alias core.stdc.stdlib.malloc cpmalloc; +alias core.stdc.stdlib.calloc cpcalloc; +alias core.stdc.stdlib.realloc cprealloc; +alias core.stdc.stdlib.free cpfree; + +public import chipmunkd.cpVect_h,chipmunkd.cpVect; +public import chipmunkd.cpBB; +public import chipmunkd.cpArray; +public import chipmunkd.cpHashSet; +public import chipmunkd.cpSpaceHash; +// +public import chipmunkd.cpBody; +public import chipmunkd.cpShape; +public import chipmunkd.cpPolyShape; +// +public import chipmunkd.cpArbiter; +public import chipmunkd.cpCollision; +// +public import chipmunkd.constraints.cpConstraint; +// +public import chipmunkd.cpSpace; +public import chipmunkd.cpSpaceComponent; +public import chipmunkd.cpSpaceQuery; +public import chipmunkd.cpSpaceStep; + +public import chipmunkd.chipmunk_types_h; + +enum cpHashValue CP_HASH_COEF = cast(cpHashValue)(3344921057uL); // ulong to uint ?? +static cpHashValue CP_HASH_PAIR(T)(T A, T B) {return (cast(cpHashValue)(A)*CP_HASH_COEF ^ cast(cpHashValue)(B)*CP_HASH_COEF);} + +void +cpMessage(string message, string condition, string file, int line, int isError) +{ + //TODO: + //fprintf(stderr, (isError ? "Aborting due to Chipmunk error: %s\n" : "Chipmunk warning: %s\n"), message); + //fprintf(stderr, "\tFailed condition: %s\n", condition); + //fprintf(stderr, "\tSource:%s:%d\n", file, line); + + if(isError) abort(); +} + +extern const char *cpVersionString; +void cpInitChipmunk() +{ + cpInitCollisionFuncs(); +} + +/** + Calculate the moment of inertia for a circle. + r1 and r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. +*/ +cpFloat +cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) +{ + return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset); +} +/** + Calculate the moment of inertia for a line segment. + Beveling radius is not supported. +*/ +cpFloat +cpMomentForSegment(cpFloat m, cpVect a, cpVect b) +{ + cpFloat length = cpvlength(cpvsub(b, a)); + cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f); + + return m*length*length/12.0f + m*cpvdot(offset, offset); +} + +/** + Calculate the moment of inertia for a solid polygon shape. +*/ +cpFloat +cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset) +{ + cpVect *tVerts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof); + for(int i=0; i b) ? a : b; +} + +static cpFloat +cpfmin(cpFloat a, cpFloat b) +{ + return (a < b) ? a : b; +} + +static cpFloat +cpfabs(cpFloat n) +{ + return (n < 0) ? -n : n; +} + +static cpFloat +cpfclamp(cpFloat f, cpFloat min, cpFloat max) +{ + return cpfmin(cpfmax(f, min), max); +} + +static cpFloat +cpflerp(cpFloat f1, cpFloat f2, cpFloat t) +{ + return f1*(1.0f - t) + f2*t; +} + +static cpFloat +cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) +{ + return f1 + cpfclamp(f2 - f1, -d, d); +} + +//#if TARGET_OS_IPHONE +// // CGPoints are structurally the same, and allow +// // easy interoperability with other iPhone libraries +// #import +// typedef CGPoint cpVect; +//#else + struct cpVect{cpFloat x = 0; cpFloat y=0;} +//#endif + +alias uint cpHashValue; + +// Oh C, how we love to define our own boolean types to get compiler compatibility +//#ifdef CP_BOOL_TYPE +// typedef CP_BOOL_TYPE cpBool; +//#else + alias bool cpBool; +//#endif + +//#ifndef cpTrue + enum cpTrue = true; +//#endif + +//#ifndef cpFalse + enum cpFalse = false; +//#endif + +//#ifdef CP_DATA_POINTER_TYPE +// typedef CP_DATA_POINTER_TYPE cpDataPointer; +//#else + alias void * cpDataPointer; +//#endif + +//#ifdef CP_COLLISION_TYPE_TYPE +// typedef CP_COLLISION_TYPE_TYPE cpCollisionType; +//#else + alias uint cpCollisionType; +//#endif + +//#ifdef CP_GROUP_TYPE +// typedef CP_GROUP_TYPE cpGroup; +//#else + alias uint cpGroup; +//#endif + +//#ifdef CP_LAYERS_TYPE +// typedef CP_GROUP_TYPE cpLayers; +//#else + alias uint cpLayers; +//#endif + +//#ifdef CP_TIMESTAMP_TYPE +// typedef CP_TIMESTAMP_TYPE cpTimestamp; +//#else + alias uint cpTimestamp; +//#endif + +//#ifndef CP_NO_GROUP + enum CP_NO_GROUP = 0; +//#endif + +//#ifndef CP_ALL_LAYERS + enum CP_ALL_LAYERS = ~0; +//#endif diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/constraints/cpConstraint.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/constraints/cpConstraint.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,146 @@ + +// written in the D programming language + +module chipmunkd.constraints.cpConstraint; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; +import chipmunkd.cpBody; + +alias void function(cpConstraint *constraint, cpFloat dt, cpFloat dt_inv) cpConstraintPreStepFunction; +alias void function(cpConstraint *constraint) cpConstraintApplyImpulseFunction; +alias cpFloat function(cpConstraint *constraint) cpConstraintGetImpulseFunction; + +struct cpConstraintClass { + cpConstraintPreStepFunction preStep; + cpConstraintApplyImpulseFunction applyImpulse; + cpConstraintGetImpulseFunction getImpulse; +} + + + +struct cpConstraint { + cpConstraintClass *klass; + + cpBody *a; + cpBody *b; + cpFloat maxForce; + cpFloat biasCoef; + cpFloat maxBias; + + cpDataPointer data; +} + +//#ifdef CP_USE_DEPRECATED_API_4 +//typedef cpConstraint cpJoint; +//#endif + +//void cpConstraintDestroy(cpConstraint *constraint); +//void cpConstraintFree(cpConstraint *constraint); + +static void +cpConstraintActivateBodies(cpConstraint *constraint) +{ + cpBody *a = constraint.a; if(a) cpBodyActivate(a); + cpBody *b = constraint.b; if(b) cpBodyActivate(b); +} + +static cpFloat +cpConstraintGetImpulse(cpConstraint *constraint) +{ + return constraint.klass.getImpulse(constraint); +} + +template cpConstraintCheckCast(string constraint, string _struct) +{ + enum cpConstraintCheckCast = + "assert("~constraint~".klass == "~_struct~"GetClass(), \"Constraint is not a "~_struct~"\");"; +} + +//#define cpConstraintCheckCast(constraint, struct) \ +// cpAssert(constraint->klass == struct##GetClass(), "Constraint is not a "#struct); + +template CP_DefineConstraintGetter(string _struct,string type,string member,string name) +{ + enum CP_DefineConstraintGetter = + "static "~type~" "~_struct~"Get"~name~"(cpConstraint *constraint){"~ + cpConstraintCheckCast!("constraint",_struct)~ + "cpConstraintActivateBodies(constraint);"~ + "return (cast("~_struct~"*)constraint)."~member~";"~ + "}"; +} + +//#define CP_DefineConstraintGetter(struct, type, member, name) \ +//static inline type \ +//struct##Get##name(const cpConstraint *constraint){ \ +// cpConstraintCheckCast(constraint, struct); \ +// return ((struct *)constraint)->member; \ +//} \ + +template CP_DefineConstraintSetter(string _struct,string type,string member,string name) +{ + enum CP_DefineConstraintSetter = + "static void "~_struct~"Set"~name~"(cpConstraint *constraint,"~type~" value){"~ + cpConstraintCheckCast!("constraint",_struct)~ + "cpConstraintActivateBodies(constraint);"~ + "(cast("~_struct~"*)constraint)."~member~" = value;"~ + "}"; +} + +//#define CP_DefineConstraintSetter(struct, type, member, name) \ +//static inline void \ +//struct##Set##name(cpConstraint *constraint, type value){ \ +// cpConstraintCheckCast(constraint, struct); \ +// cpConstraintActivateBodies(constraint); \ +// ((struct *)constraint)->member = value; \ +//} \ + +template CP_DefineConstraintProperty(string _struct,string type,string member,string name) +{ + enum CP_DefineConstraintProperty = + CP_DefineConstraintGetter!(_struct,type,member,name)~CP_DefineConstraintSetter!(_struct,type,member,name); + +} + +//#define CP_DefineConstraintProperty(struct, type, member, name) \ +//CP_DefineConstraintGetter(struct, type, member, name) \ +//CP_DefineConstraintSetter(struct, type, member, name) +//TODO: +//// Built in Joint types +public import chipmunkd.constraints.cpPinJoint; +//#include "cpSlideJoint.h" +public import chipmunkd.constraints.cpPivotJoint; +//#include "cpGrooveJoint.h" +public import chipmunkd.constraints.cpDampedSpring; +//#include "cpDampedRotarySpring.h" +//#include "cpRotaryLimitJoint.h" +//#include "cpRatchetJoint.h" +//#include "cpGearJoint.h" +//#include "cpSimpleMotor.h" + + +cpFloat cp_constraint_bias_coef = 0.1f; + +void cpConstraintDestroy(cpConstraint *constraint){} + +void +cpConstraintFree(cpConstraint *constraint) +{ + if(constraint){ + cpConstraintDestroy(constraint); + cpfree(constraint); + } +} + +void +cpConstraintInit(cpConstraint *constraint, /+const+/ cpConstraintClass *klass, cpBody *a, cpBody *b) +{ + constraint.klass = klass; + constraint.a = a; + constraint.b = b; + + constraint.maxForce = cast(cpFloat)INFINITY; + constraint.biasCoef = cp_constraint_bias_coef; + constraint.maxBias = cast(cpFloat)INFINITY; +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/constraints/cpDampedSpring.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/constraints/cpDampedSpring.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,127 @@ + +// written in the D programming language + +module chipmunkd.constraints.cpDampedSpring; + +import chipmunkd.chipmunk; +import chipmunkd.constraints.util; + +alias cpFloat function(cpConstraint *spring, cpFloat dist) cpDampedSpringForceFunc; + +//const cpConstraintClass *cpDampedSpringGetClass(); + +struct cpDampedSpring { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat restLength; + cpFloat stiffness; + cpFloat damping; + cpDampedSpringForceFunc springForceFunc; + + cpFloat target_vrn; + cpFloat v_coef; + + cpVect r1, r2; + cpFloat nMass; + cpVect n; +} + +//cpDampedSpring *cpDampedSpringAlloc(void); +//cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); +//cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); + +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr1", "Anchr1")); +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr2", "Anchr2")); +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "restLength", "RestLength")); +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "stiffness", "Stiffness")); +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "damping", "Damping")); +mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpDampedSpringForceFunc", "springForceFunc", "SpringForceFunc")); + +static cpFloat +defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ + return (spring.restLength - dist)*spring.stiffness; +} + +static void +preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) +{ + mixin(CONSTRAINT_BEGIN!("spring", "a", "b")); + + spring.r1 = cpvrotate(spring.anchr1, a.rot); + spring.r2 = cpvrotate(spring.anchr2, b.rot); + + cpVect delta = cpvsub(cpvadd(b.p, spring.r2), cpvadd(a.p, spring.r1)); + cpFloat dist = cpvlength(delta); + spring.n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + cpFloat k = k_scalar(a, b, spring.r1, spring.r2, spring.n); + spring.nMass = 1.0f/k; + + spring.target_vrn = 0.0f; + spring.v_coef = 1.0f - cpfexp(-spring.damping*dt*k); + + // apply spring force + cpFloat f_spring = spring.springForceFunc(cast(cpConstraint *)spring, dist); + apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, f_spring*dt)); +} + +static void +applyImpulse(cpDampedSpring *spring) +{ + mixin(CONSTRAINT_BEGIN!("spring", "a", "b")); + + cpVect n = spring.n; + cpVect r1 = spring.r1; + cpVect r2 = spring.r2; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring.target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat v_damp = -vrn*spring.v_coef; + spring.target_vrn = vrn + v_damp; + + apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass)); +} + +static cpFloat +getImpulse(cpConstraint *constraint) +{ + return 0.0f; +} + +static /+const+/ cpConstraintClass klass = { + cast(cpConstraintPreStepFunction)&preStep, + cast(cpConstraintApplyImpulseFunction)&applyImpulse, + cast(cpConstraintGetImpulseFunction)&getImpulse, +}; +mixin(CP_DefineClassGetter!("cpDampedSpring")); + +cpDampedSpring * +cpDampedSpringAlloc() +{ + return cast(cpDampedSpring *)cpmalloc(cpDampedSpring.sizeof); +} + +cpDampedSpring * +cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit(cast(cpConstraint *)spring, cpDampedSpringGetClass(), a, b); + + spring.anchr1 = anchr1; + spring.anchr2 = anchr2; + + spring.restLength = restLength; + spring.stiffness = stiffness; + spring.damping = damping; + spring.springForceFunc = cast(cpDampedSpringForceFunc)&defaultSpringForce; + + return spring; +} + +cpConstraint * +cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + return cast(cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping); +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/constraints/cpPinJoint.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/constraints/cpPinJoint.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,124 @@ + +// written in the D programming language + +module chipmunkd.constraints.cpPinJoint; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; +import chipmunkd.cpBody; +import chipmunkd.constraints.util; + +//const cpConstraintClass *cpPinJointGetClass(); + +struct cpPinJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat dist; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc, jnMax; + cpFloat bias; +} + +//cpPinJoint *cpPinJointAlloc(void); +//cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +//cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +// +//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1); +//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2); +//CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist); + +// cpPinJoint.c ------------------------- + +static void +preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); + + joint.r1 = cpvrotate(joint.anchr1, a.rot); + joint.r2 = cpvrotate(joint.anchr2, b.rot); + + cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1)); + cpFloat dist = cpvlength(delta); + joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY)); + + // calculate mass normal + joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n); + + // 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 = mixin(J_MAX!("joint", "dt")); + + // apply accumulated impulse + cpVect j = cpvmult(joint.n, joint.jnAcc); + apply_impulses(a, b, joint.r1, joint.r2, j); +} + +static void +applyImpulse(cpPinJoint *joint) +{ + mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); + cpVect n = joint.n; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n); + + // compute normal impulse + cpFloat jn = (joint.bias - vrn)*joint.nMass; + cpFloat jnOld = joint.jnAcc; + joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax); + jn = joint.jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpPinJoint *joint) +{ + return cpfabs(joint.jnAcc); +} + +static /+const+/ cpConstraintClass klass = { + cast(cpConstraintPreStepFunction)&preStep, + cast(cpConstraintApplyImpulseFunction)&applyImpulse, + cast(cpConstraintGetImpulseFunction)&getImpulse, +}; +mixin(CP_DefineClassGetter!("cpPinJoint")); + +cpPinJoint * +cpPinJointAlloc() +{ + return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof); +} + +cpPinJoint * +cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); + + joint.anchr1 = anchr1; + joint.anchr2 = anchr2; + + // STATIC_BODY_CHECK + 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)); + + joint.jnAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/constraints/cpPivotJoint.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/constraints/cpPivotJoint.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,118 @@ + +// written in the D programming language + +module chipmunkd.constraints.cpPivotJoint; + +import chipmunkd.chipmunk; +import chipmunkd.constraints.util; + +//const cpConstraintClass *cpPivotJointGetClass(); + +struct cpPivotJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + + cpVect r1, r2; + cpVect k1, k2; + + cpVect jAcc; + cpFloat jMaxLen; + cpVect bias; +} + +//cpPivotJoint *cpPivotJointAlloc(void); +//cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +//cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); +//cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +// +//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1); +//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2); + +static void +preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + mixin(CONSTRAINT_BEGIN!("joint", "a", "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 = mixin(J_MAX!("joint", "dt")); + + // 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); + + // apply accumulated impulse + apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc); +} + +static void +applyImpulse(cpPivotJoint *joint) +{ + mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); + + cpVect r1 = joint.r1; + cpVect r2 = joint.r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + + // compute normal impulse + cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2); + cpVect jOld = joint.jAcc; + joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.jMaxLen); + j = cpvsub(joint.jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint.r1, joint.r2, j); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpvlength((cast(cpPivotJoint *)joint).jAcc); +} + +static /+const+/ cpConstraintClass klass = { + cast(cpConstraintPreStepFunction)&preStep, + cast(cpConstraintApplyImpulseFunction)&applyImpulse, + cast(cpConstraintGetImpulseFunction)&getImpulse, +}; +mixin(CP_DefineClassGetter!("cpPivotJoint")); + +cpPivotJoint * +cpPivotJointAlloc() +{ + return cast(cpPivotJoint *)cpmalloc(cpPivotJoint.sizeof); +} + +cpPivotJoint * +cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); + + joint.anchr1 = anchr1; + joint.anchr2 = anchr2; + + joint.jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return cast(cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2); +} + +cpConstraint * +cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) +{ + cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot); + cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot); + return cpPivotJointNew2(a, b, anchr1, anchr2); +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/constraints/util.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/constraints/util.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,128 @@ + +// written in the D programming language + +module chipmunkd.constraints.util; + +import chipmunkd.cpBody; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; + +template CP_DefineClassGetter(string t) +{ + enum CP_DefineClassGetter = + "cpConstraintClass* "~t~"GetClass(){return cast(cpConstraintClass *)&klass;}"; +} + +template J_MAX(string constraint,string dt) +{ + enum J_MAX = "((cast(cpConstraint *)"~constraint~").maxForce*("~dt~"))"; +} + +template CONSTRAINT_BEGIN(string constraint,string a_var,string b_var) +{ + enum CONSTRAINT_BEGIN = "cpBody *"~a_var~"; cpBody *"~b_var~"; + "~a_var~" = (cast(cpConstraint *)"~constraint~")."~a_var~"; + "~b_var~" = (cast(cpConstraint *)"~constraint~")."~b_var~"; + if( + (cpBodyIsSleeping("~a_var~") || cpBodyIsStatic("~a_var~")) && + (cpBodyIsSleeping("~b_var~") || cpBodyIsStatic("~b_var~")) + ) return; + "; +} + +static cpVect +relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ + cpVect v1_sum = cpvadd(a.v, cpvmult(cpvperp(r1), a.w)); + cpVect v2_sum = cpvadd(b.v, cpvmult(cpvperp(r2), b.w)); + + return cpvsub(v2_sum, v1_sum); +} + +static cpFloat +normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ + return cpvdot(relative_velocity(a, b, r1, r2), n); +} + +static void +apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyImpulse(a, cpvneg(j), r1); + cpBodyApplyImpulse(b, j, r2); +} + +static void +apply_bias_impulse(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); +} + +static void +apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + apply_bias_impulse(a, cpvneg(j), r1); + apply_bias_impulse(b, j, r2); +} + +static cpVect +clamp_vect(cpVect v, cpFloat len) +{ + return cpvclamp(v, len); +// return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +static cpFloat +k_scalar(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); + + cpFloat value = mass_sum + a.i_inv*r1cn*r1cn + b.i_inv*r2cn*r2cn; + assert(value != 0.0, "Unsolvable collision or constraint."); + + return value; +} + +static void +k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2) +{ + // 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 a_i_inv = a.i_inv; + cpFloat r1xsq = r1.x * r1.x * a_i_inv; + cpFloat r1ysq = r1.y * r1.y * a_i_inv; + cpFloat r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + cpFloat b_i_inv = b.i_inv; + cpFloat r2xsq = r2.x * r2.x * b_i_inv; + cpFloat r2ysq = r2.y * r2.y * b_i_inv; + cpFloat r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + cpFloat determinant = k11*k22 - k12*k21; + assert(determinant != 0.0, "Unsolvable constraint."); + + cpFloat det_inv = 1.0f/determinant; + *k1 = cpv( k22*det_inv, -k12*det_inv); + *k2 = cpv(-k21*det_inv, k11*det_inv); +} + +static cpVect +mult_k(cpVect vr, cpVect k1, cpVect k2) +{ + return cpv(cpvdot(vr, k1), cpvdot(vr, k2)); +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpArbiter.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpArbiter.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,394 @@ + +// written in the D programming language + +module chipmunkd.cpArbiter; + +import chipmunkd.cpSpace; +import chipmunkd.cpBody; +import chipmunkd.cpShape; +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; +import chipmunkd.constraints.util; + +// 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. +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. + cpHashValue hash; +} + +//// Contacts are always allocated in groups. +//cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); +// +//// Sum the contact impulses. (Can be used after cpSpaceStep() returns) +//cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts); +//cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts); + +enum cpArbiterState { + cpArbiterStateNormal, + cpArbiterStateFirstColl, + cpArbiterStateIgnore, + cpArbiterStateSleep, + cpArbiterStateCached, +} + +// Data structure for tracking collisions between shapes. +struct cpArbiter { + // Information on the contact points between the objects. + int numContacts; + cpContact *contacts; + + // The two shapes and bodies involved in the collision. + // These variables are NOT in the order defined by the collision handler. + // Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from + // many headaches + cpShape* private_a; + cpShape* private_b; + + // Calculated before calling the pre-solve collision handler + // Override them with custom values if you want specialized behavior + cpFloat e; + cpFloat u; + // Used for surface_v calculations, implementation may change + cpVect surface_vr; + + // Time stamp of the arbiter. (from cpSpace) + cpTimestamp stamp; + + cpCollisionHandler *handler; + + // Are the shapes swapped in relation to the collision handler? + cpBool swappedColl; + cpArbiterState state; +} + +//// Arbiters are allocated in large buffers by the space and don't require a destroy function +//cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); +// +//// These functions are all intended to be used internally. +//// Inject new contact points into the arbiter while preserving contact history. +//void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b); +//// 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); +// +//// Arbiter Helper Functions +//cpVect cpArbiterTotalImpulse(cpArbiter *arb); +//cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb); +//void cpArbiterIgnore(cpArbiter *arb); + + +static void +cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b) +{ + if(arb.swappedColl){ + (*a) = arb.private_b, (*b) = arb.private_a; + } else { + (*a) = arb.private_a, (*b) = arb.private_b; + } +} +//#define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b); +template CP_ARBITER_GET_SHAPES(string arb,string a,string b) +{ + enum CP_ARBITER_GET_SHAPES = "cpShape* "~a~", "~b~";"~ + "cpArbiterGetShapes("~arb~", &"~a~", &"~b~");"; +} + +static void +cpArbiterGetBodies(/+const+/ cpArbiter *arb, cpBody **a, cpBody **b) +{ + //CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); + cpShape *shape_a, shape_b; cpArbiterGetShapes(arb, &shape_a, &shape_b); + (*a) = shape_a._body; + (*b) = shape_b._body; +} +//#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b); + +static cpBool +cpArbiterIsFirstContact(const cpArbiter *arb) +{ + return arb.state == cpArbiterState.cpArbiterStateFirstColl; +} + +static cpVect +cpArbiterGetNormal(const cpArbiter *arb, int i) +{ + cpVect n = arb.contacts[i].n; + return arb.swappedColl ? cpvneg(n) : n; +} + +static cpVect +cpArbiterGetPoint(const cpArbiter *arb, int i) +{ + return arb.contacts[i].p; +} + +cpFloat cp_bias_coef = 0.1f; +cpFloat cp_collision_slop = 0.1f; + +cpContact* +cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue 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 +cpArbiterTotalImpulse(cpArbiter *arb) +{ + cpContact *contacts = arb.contacts; + cpVect sum = cpvzero; + + for(int i=0, count=arb.numContacts; i= arr.max){ + arr.max = arr.num; + arr.arr = cast(void **)cprealloc(arr.arr, arr.max*(void**).sizeof); + } + + memcpy(tail, other.arr, other.num*(void**).sizeof); +} + +void +cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) +{ + for(int i=0; i other.r && bb.b < other.b && bb.t > other.t); +} + +static cpBool +cpBBcontainsVect(const cpBB bb, const cpVect v) +{ + return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y); +} + +static cpBB +cpBBmerge(const cpBB a, const cpBB b){ + return cpBBNew( + cpfmin(a.l, b.l), + cpfmin(a.b, b.b), + cpfmax(a.r, b.r), + cpfmax(a.t, b.t) + ); +} + +static cpBB +cpBBexpand(const cpBB bb, const cpVect v){ + return cpBBNew( + cpfmin(bb.l, v.x), + cpfmin(bb.b, v.y), + cpfmax(bb.r, v.x), + cpfmax(bb.t, v.y) + ); +} + +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 = cpfabs(bb.r - bb.l); + cpFloat modx = cpfmod(v.x - bb.l, ix); + cpFloat x = (modx > 0.0f) ? modx : modx + ix; + + cpFloat iy = cpfabs(bb.t - bb.b); + cpFloat mody = cpfmod(v.y - bb.b, iy); + cpFloat y = (mody > 0.0f) ? mody : mody + iy; + + return cpv(x + bb.l, y + bb.b); +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpBody.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpBody.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,367 @@ + +// written in the D programming language + +module chipmunkd.cpBody; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; +import chipmunkd.cpShape; + +alias void function(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)cpBodyVelocityFunc; +alias void function(cpBody *_body, cpFloat dt)cpBodyPositionFunc; + +//extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault; +//extern cpBodyPositionFunc cpBodyUpdatePositionDefault; + +// Structure to hold information about the contact graph components +// when putting groups of objects to sleep. +// No interesting user accessible fields. +struct cpComponentNode { + cpBody *parent; + cpBody *next; + int rank; + cpFloat idleTime; +} + +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 cpBodySetMoment() whenever changing the moment 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. + cpDataPointer data; + + // *** Other Fields + + // Maximum velocities this body can move at after integrating velocity + cpFloat v_limit, w_limit; + + // *** Internally Used Fields + + // Velocity bias values used when solving penetrations and correcting constraints. + cpVect v_bias; + cpFloat w_bias; + + // Space this body has been added to + cpSpace* space; + + // Pointer to the shape list. + // Shapes form a linked list using cpShape.next when added to a space. + cpShape *shapesList; + + // Used by cpSpaceStep() to store contact graph information. + cpComponentNode node; +} + +//// 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); +// +//// Wake up a sleeping or idle body. (defined in cpSpace.c) +//void cpBodyActivate(cpBody *body); +// +//// Force a body to sleep; +//void cpBodySleep(cpBody *body); +////void cpBodySleepGroup(cpBody *body, ...); + +static cpBool +cpBodyIsSleeping(const cpBody *_body) +{ + return (_body.node.next !is null); +} + +//cpBool cpBodyIsStatic(const cpBody *body); + +static cpBool +cpBodyIsRogue(const cpBody* _body) +{ + return (_body.space is null); +} + + +//#define CP_DefineBodyGetter(type, member, name) \ +//static inline type cpBodyGet##name(const cpBody *body){return body.member;} +// +//#define CP_DefineBodySetter(type, member, name) \ +//static inline void \ +//cpBodySet##name(cpBody *body, const type value){ \ +// cpBodyActivate(body); \ +// body.member = value; \ +//} \ + +//#define CP_DefineBodyProperty(type, member, name) \ +//CP_DefineBodyGetter(type, member, name) \ +//CP_DefineBodySetter(type, member, name) + + +//// Accessors for cpBody struct members +//CP_DefineBodyGetter(cpFloat, m, Mass); +//void cpBodySetMass(cpBody *body, cpFloat m); +// +//CP_DefineBodyGetter(cpFloat, i, Moment); +//void cpBodySetMoment(cpBody *body, cpFloat i); +// +// +//CP_DefineBodyProperty(cpVect, p, Pos); +//CP_DefineBodyProperty(cpVect, v, Vel); +//CP_DefineBodyProperty(cpVect, f, Force); +static cpFloat cpBodyGetAngle(const cpBody *_body){return _body.a;} +static cpFloat cpBodySetAngle(cpBody *_body, const cpFloat value){cpBodyActivate(_body); return _body.a = value;} +//CP_DefineBodyProperty(cpFloat, w, AngVel); +//CP_DefineBodyProperty(cpFloat, t, Torque); +//CP_DefineBodyGetter(cpVect, rot, Rot); +//CP_DefineBodyProperty(cpFloat, v_limit, VelLimit); +//CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit); + +//// 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 cpVect +cpBodyLocal2World(const cpBody *_body, const cpVect v) +{ + return cpvadd(_body.p, cpvrotate(v, _body.rot)); +} + +// Convert world to body local coordinates +static cpVect +cpBodyWorld2Local(const cpBody *_body, const cpVect v) +{ + return cpvunrotate(cpvsub(v, _body.p), _body.rot); +} + +// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). +static void +cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r) +{ + _body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv)); + _body.w += _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 at a point relative to the center of gravity (also in world coordinates). +//void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); + +static cpFloat +cpBodyKineticEnergy(const cpBody *_body) +{ + // Need to do some fudging to avoid NaNs + cpFloat vsq = cpvdot(_body.v, _body.v); + cpFloat wsq = _body.w*_body.w; + return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f); +} + +//// Apply a damped spring force between two bodies. +//// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead. +//void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); + +// initialized in cpInitChipmunk() +cpBody cpStaticBodySingleton; + +cpBody* +cpBodyAlloc() +{ + return cast(cpBody *)cpmalloc(cpBody.sizeof); +} + +cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity; +cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition; + +cpBody* +cpBodyInit(cpBody *_body, cpFloat m, cpFloat i) +{ + _body.velocity_func = cpBodyUpdateVelocityDefault; + _body.position_func = cpBodyUpdatePositionDefault; + + 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.v_limit = INFINITY; + _body.w_limit = INFINITY; + + _body.space = null; + _body.shapesList = null; + + cpComponentNode node = {null, null, 0, 0.0f}; + _body.node = node; + + 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); + cpfree(_body); + } +} + +void +cpBodySetMass(cpBody *_body, cpFloat mass) +{ + _body.m = mass; + _body.m_inv = 1.0f/mass; +} + +void +cpBodySetMoment(cpBody *_body, cpFloat moment) +{ + _body.i = moment; + _body.i_inv = 1.0f/moment; +} + +void +cpBodySetAngle(cpBody *_body, cpFloat angle) +{ + _body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f); + _body.rot = cpvforangle(angle); +} + +void +cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt) +{ + cpVect delta = cpvsub(pos, _body.p); + _body.v = cpvmult(delta, 1.0f/dt); +} + +void +cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt) +{ + _body.v = cpvclamp( + cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)), + _body.v_limit); + + cpFloat w_limit = _body.w_limit; + _body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit); +} + +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 force, cpVect r) +{ + _body.f = cpvadd(_body.f, force); + _body.t += cpvcross(r, force); +} + +void +cpApplyDampedSpring(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); +} + +cpBool +cpBodyIsStatic(/+const+/ cpBody *_body) +{ + cpSpace *space = _body.space; + return ( (space !is null) && (_body is &space.staticBody) ); +} + +//void cpSpaceSleepBody(cpSpace *space, cpBody *_body); + +void +cpBodySleep(cpBody *_body) +{ + if(cpBodyIsSleeping(_body)) return; + + assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep."); + cpSpaceSleepBody(_body.space, _body); //TODO +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpCollision.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpCollision.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,380 @@ + +// written in the D programming language + +module chipmunkd.cpCollision; + +import chipmunkd.chipmunk_types_h; +import chipmunkd.chipmunk; +import chipmunkd.cpShape; + +alias int function(const cpShape *, const cpShape *, cpContact *) collisionFunc; + + +// Add contact points for circle to circle collisions. +// Used by several collision tests. +static int +circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const 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 = cpfsqrt(distsq); + + // Allocate and initialize the contact. + cpContactInit( + con, + cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))), + (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)), + dist - mindist, + 0 + ); + + return 1; +} + +// Collide circle shapes. +static int +circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) +{ + cpCircleShape *circ1 = cast(cpCircleShape*)shape1; + cpCircleShape *circ2 = cast(cpCircleShape*)shape2; + + return circle2circleQuery(circ1.tc, circ2.tc, circ1.r, circ2.r, arr); +} + +// Collide circles to segment shapes. +static int +circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con) +{ + cpCircleShape *circ = cast(cpCircleShape *)circleShape; + cpSegmentShape *seg = cast(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 = cpfabs(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); + 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 working with contact buffers +// This used to malloc/realloc memory on the fly but was repurposed. +static cpContact * +nextContactPoint(cpContact *arr, int *numPtr) +{ + int num = *numPtr; + + if(num < CP_MAX_CONTACTS_PER_ARBITER) + (*numPtr) = num + 1; + + return &arr[num]; +} + +// Find the minimum separating axis for the give poly and axis list. +static int +findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out) +{ + int min_index = 0; + cpFloat min = cpPolyShapeValueOnAxis(poly, axes.n, axes.d); + if(min > 0.0f) return -1; + + for(int i=1; i 0.0f) { + return -1; + } else if(dist > min){ + min = dist; + min_index = i; + } + } + + (*min_out) = min; + return min_index; +} + +// Add contacts for probably penetrating vertexes. +// This handles the degenerate case where an overlap was detected, but no vertexes fall inside +// the opposing polygon. (like a star of david) +static int +findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) +{ + int num = 0; + + for(int i=0; i 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 cpFloat +segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const 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 void +findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const 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= dt && dt >= dtb){ + cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly.shape.hashid, cast(const cpHashValue)i)); + } + } + } +} + +// This one is complicated and gross. Just don't go there... +// TODO: Comment me! +static int +seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) +{ + cpSegmentShape *seg = cast(cpSegmentShape *)shape1; + cpPolyShape *poly = cast(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 0.0f){ + return 0; + } else if(dist > poly_min){ + poly_min = dist; + mini = i; + } + } + + 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(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)0)); + if(cpPolyShapeContainsVert(poly, vb)) + cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)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, &num, seg, poly, minNorm, 1.0f); + else + findPointsBehindSeg(arr, &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(const cpShape *shape1, const cpShape *shape2, cpContact *con) +{ + cpCircleShape *circ = cast(cpCircleShape *)shape1; + cpPolyShape *poly = cast(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 0.0f){ + 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) { + 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 const collisionFunc builtinCollisionFuncs[9] = [ + &circle2circle, + null, + null, + &circle2segment, + null, + null, + &circle2poly, + &seg2poly, + &poly2poly, +]; + +static collisionFunc[cpShapeType.CP_NUM_SHAPES * cpShapeType.CP_NUM_SHAPES] colfuncs = builtinCollisionFuncs; + +static void +addColFunc(const cpShapeType a, const cpShapeType b, collisionFunc func) +{ + colfuncs[a + b*cpShapeType.CP_NUM_SHAPES] = func; +} + +// Initializes the array of collision functions. +// Called by cpInitChipmunk(). +void cpInitCollisionFuncs() +{ + addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_CIRCLE_SHAPE, &circle2circle); + addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_SEGMENT_SHAPE, &circle2segment); + addColFunc(cpShapeType.CP_SEGMENT_SHAPE, cpShapeType.CP_POLY_SHAPE, &seg2poly); + addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_POLY_SHAPE, &circle2poly); + addColFunc(cpShapeType.CP_POLY_SHAPE, cpShapeType.CP_POLY_SHAPE, &poly2poly); +} + + +int +cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr) +{ + // Their shape types must be in order. + assert(a.klass.type <= b.klass.type, "Collision shapes passed to cpCollideShapes() are not sorted."); + + collisionFunc cfunc = colfuncs[a.klass.type + b.klass.type*cpShapeType.CP_NUM_SHAPES]; + return (cfunc) ? cfunc(a, b, arr) : 0; +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpHashSet.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpHashSet.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,277 @@ + +// written in the D programming language + +module chipmunkd.cpHashSet; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpArray; +import chipmunkd.prime; + +// 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. +struct cpHashSetBin { + // Pointer to the element. + void *elt; + // Hash value of the element. + cpHashValue hash; + // Next element in the chain. + cpHashSetBin *next; +} + +// Equality function. Returns true if ptr is equal to elt. +alias cpBool function(void *ptr, void *elt) cpHashSetEqlFunc; +// Used by cpHashSetInsert(). Called to transform the ptr into an element. +alias void* function(void *ptr, void *data) cpHashSetTransFunc; + +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; + + // The table and recycled bins + cpHashSetBin **table; + cpHashSetBin *pooledBins; + + cpArray *allocatedBuffers; +} + +alias void function(void *elt, void *data) cpHashSetIterFunc; +alias cpBool function(void *elt, void *data) cpHashSetFilterFunc; + +static void freeWrap(void *ptr, void *unused){cpfree(ptr);} + +void +cpHashSetDestroy(cpHashSet *set) +{ + // Free the table. + cpfree(set.table); + + cpArrayEach(set.allocatedBuffers, &freeWrap, null); + cpArrayFree(set.allocatedBuffers); +} + +void +cpHashSetFree(cpHashSet *set) +{ + if(set){ + cpHashSetDestroy(set); + cpfree(set); + } +} + +cpHashSet * +cpHashSetAlloc() +{ + return cast(cpHashSet *)cpcalloc(1, cpHashSet.sizeof); +} + +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 = cast(cpHashSetBin **)cpcalloc(set.size, (cpHashSetBin *).sizeof); + set.pooledBins = null; + + set.allocatedBuffers = cpArrayNew(0); + + 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 = cast(cpHashSetBin **)cpcalloc(newSize, (cpHashSetBin *).sizeof); + + // Iterate over the chains. + for(int i=0; i 0.0f) return cpFalse; + } + + return cpTrue; +} + +// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal. +static cpBool +cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n) +{ + const cpPolyShapeAxis *axes = poly.tAxes; + + int i; + for(i=0; i 0.0f) return cpFalse; + } + + return cpTrue; +} + +// cpPolyShape.c --------------------------------- + + +cpPolyShape * +cpPolyShapeAlloc() +{ + return cast(cpPolyShape *)cpcalloc(1, cpPolyShape.sizeof); +} + +static void +cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpVect *src = poly.verts; + cpVect *dst = poly.tVerts; + + for(int i=0; i an) continue; + + cpFloat bn = cpvdot(b, n); + cpFloat t = (axes[i].d - an)/(bn - an); + if(t < 0.0f || 1.0f < t) continue; + + cpVect point = cpvlerp(a, b, t); + cpFloat dt = -cpvcross(n, point); + cpFloat dtMin = -cpvcross(n, verts[i]); + cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]); + + if(dtMin <= dt && dt <= dtMax){ + info.shape = shape; + info.t = t; + info.n = n; + } + } +} + +static /+const+/ cpShapeClass polyClass = { + cpShapeType.CP_POLY_SHAPE, + &cpPolyShapeCacheData, + &cpPolyShapeDestroy, + &cpPolyShapePointQuery, + &cpPolyShapeSegmentQuery, +}; + +cpBool +cpPolyValidate(cpVect *verts, int numVerts) +{ + for(int i=0; i 0.0f) + return cpFalse; + } + + return cpTrue; +} + +int +cpPolyShapeGetNumVerts(cpShape *shape) +{ + assert(shape.klass == &polyClass, "Shape is not a poly shape."); + return (cast(cpPolyShape *)shape).numVerts; +} + +cpVect +cpPolyShapeGetVert(cpShape *shape, int idx) +{ + assert(shape.klass == &polyClass, "Shape is not a poly shape."); + assert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range."); + + return (cast(cpPolyShape *)shape).verts[idx]; +} + + +static void +setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset) +{ + poly.numVerts = numVerts; + + poly.verts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof); + poly.tVerts = cast(cpVect *)cpcalloc(numVerts, cpVect.sizeof); + poly.axes = cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof); + poly.tAxes = cast(cpPolyShapeAxis *)cpcalloc(numVerts, (cpPolyShapeAxis).sizeof); + + for(int i=0; i= 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; +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpSpace.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpSpace.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,642 @@ + +// written in the D programming language + +module chipmunkd.cpSpace; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; +import chipmunkd.cpVect,chipmunkd.cpVect_h; +import chipmunkd.cpHashSet; +import chipmunkd.cpCollision; +import chipmunkd.cpBody; +import chipmunkd.cpArray; +import chipmunkd.cpShape; +import chipmunkd.cpBB; +import chipmunkd.cpArbiter; +import chipmunkd.cpSpaceHash; +import chipmunkd.constraints.cpConstraint; +import chipmunkd.cpSpaceQuery; + +// Number of frames that contact information should persist. +//extern cpTimestamp cp_contact_persistence; + +// User collision handler function types. +alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionBeginFunc; +alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPreSolveFunc; +alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPostSolveFunc; +alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionSeparateFunc; + +// Structure for holding collision pair function information. +// Used internally. +struct cpCollisionHandler { + cpCollisionType a; + cpCollisionType b; + cpCollisionBeginFunc begin; + cpCollisionPreSolveFunc preSolve; + cpCollisionPostSolveFunc postSolve; + cpCollisionSeparateFunc separate; + void *data; +} + +enum CP_MAX_CONTACTS_PER_ARBITER = 6; +struct cpContactBufferHeader { + cpTimestamp stamp; + cpContactBufferHeader *next; + uint numContacts; +} + +struct cpSpace{ + // *** User definable fields + + // Number of iterations to use in the impulse solver to solve contacts. + int iterations; + + // Number of iterations to use in the impulse solver to solve elastic collisions. + int elasticIterations; + + // Default gravity to supply when integrating rigid body motions. + cpVect gravity; + + // Default damping to supply when integrating rigid body motions. + cpFloat damping; + + // Speed threshold for a body to be considered idle. + // The default value of 0 means to let the space guess a good threshold based on gravity. + cpFloat idleSpeedThreshold; + + // Time a group of bodies must remain idle in order to fall asleep + // The default value of INFINITY disables the sleeping algorithm. + cpFloat sleepTimeThreshold; + + // *** Internally Used Fields + + // When the space is locked, you should not add or remove objects; + cpBool locked; + + // Time stamp. Is incremented on every call to cpSpaceStep(). + cpTimestamp stamp; + + // The static and active shape spatial hashes. + cpSpaceHash* staticShapes; + cpSpaceHash* activeShapes; + + // List of bodies in the system. + cpArray *bodies; + + // List of groups of sleeping bodies. + cpArray *sleepingComponents; + + // List of active arbiters for the impulse solver. + cpArray* arbiters, pooledArbiters; + + // Linked list ring of contact buffers. + // Head is the newest buffer, and each buffer points to a newer buffer. + // Head wraps around and points to the oldest (tail) buffer. + cpContactBufferHeader* contactBuffersHead, _contactBuffersTail; + + // List of buffers to be free()ed when destroying the space. + cpArray *allocatedBuffers; + + // Persistant contact set. + cpHashSet *contactSet; + + // List of constraints in the system. + cpArray *constraints; + + // Set of collisionpair functions. + cpHashSet *collFuncSet; + // Default collision handler. + cpCollisionHandler defaultHandler; + + cpHashSet *postStepCallbacks; + + cpBody staticBody; +} + +//// Basic allocation/destruction functions. +//cpSpace* cpSpaceAlloc(void); +//cpSpace* cpSpaceInit(cpSpace *space); +//cpSpace* cpSpaceNew(void); +// +//void cpSpaceDestroy(cpSpace *space); +//void cpSpaceFree(cpSpace *space); +// +//// Convenience function. Frees all referenced entities. (bodies, shapes and constraints) +//void cpSpaceFreeChildren(cpSpace *space); +// +//// Collision handler management functions. +//void cpSpaceSetDefaultCollisionHandler( +// cpSpace *space, +// cpCollisionBeginFunc begin, +// cpCollisionPreSolveFunc preSolve, +// cpCollisionPostSolveFunc postSolve, +// cpCollisionSeparateFunc separate, +// void *data +//); +//void cpSpaceAddCollisionHandler( +// cpSpace *space, +// cpCollisionType a, cpCollisionType b, +// cpCollisionBeginFunc begin, +// cpCollisionPreSolveFunc preSolve, +// cpCollisionPostSolveFunc postSolve, +// cpCollisionSeparateFunc separate, +// void *data +//); +//void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); +// +//// Add and remove entities from the system. +//cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape); +//cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); +//cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body); +//cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); +// +//void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); +//void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); +//void cpSpaceRemoveBody(cpSpace *space, cpBody *body); +//void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); +// +//// Post Step function definition +alias void function(cpSpace *space, void *obj, void *data) cpPostStepFunc; +//// Register a post step function to be called after cpSpaceStep() has finished. +//// obj is used a key, you can only register one callback per unique value for obj +//void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data); +// +//// Point query callback function +alias void function(cpShape *shape, void *data) cpSpacePointQueryFunc; +//void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); +//cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); +// +//// Segment query callback function +alias void function(cpShape *shape, cpFloat t, cpVect n, void *data)cpSpaceSegmentQueryFunc; +//void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data); +//cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out); +// +//// BB query callback function +alias void function(cpShape *shape, void *data)cpSpaceBBQueryFunc; +//void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); +// +// +//// Iterator function for iterating the bodies in a space. +alias void function(cpBody *_body, void *data)cpSpaceBodyIterator; +//void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data); +// +//// Spatial hash management functions. +//void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count); +//void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count); +//void cpSpaceRehashStatic(cpSpace *space); +// +//void cpSpaceRehashShape(cpSpace *space, cpShape *shape); +// +//// Update the space. +//void cpSpaceStep(cpSpace *space, cpFloat dt); + + +cpTimestamp cp_contact_persistence = 3; + +// Equal function for contactSet. +static cpBool +contactSetEql(cpShape **shapes, cpArbiter *arb) +{ + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + return ((a == arb.private_a && b == arb.private_b) || (b == arb.private_a && a == arb.private_b)); +} + +// Transformation function for contactSet. +static void * +contactSetTrans(cpShape **shapes, cpSpace *space) +{ + if(space.pooledArbiters.num == 0){ + // arbiter pool is exhausted, make more + int count = CP_BUFFER_BYTES/cpArbiter.sizeof; + assert(count, "Buffer size too small."); + + cpArbiter *buffer = cast(cpArbiter *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(space.allocatedBuffers, buffer); + + for(int i=0; i b_root.node.rank){ + b_root.node.parent = a_root; + } else if(a_root != b_root){ + b_root.node.parent = a_root; + a_root.node.rank++; + } +} + +static void +componentActivate(cpBody *root) +{ + if(!cpBodyIsSleeping(root)) return; + + cpSpace *space = root.space; + assert(space, "Trying to activate a body that was never added to a space."); + + cpBody* _body = root; + cpBody* next; + do { + next = _body.node.next; + + cpComponentNode node = {null, null, 0, 0.0f}; + _body.node = node; + cpArrayPush(space.bodies, _body); + + for(cpShape *shape=_body.shapesList; shape; shape=shape.next){ + cpSpaceHashRemove(space.staticShapes, shape, shape.hashid); + cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb); + } + } while((_body = next) != root); + + cpArrayDeleteObj(space.sleepingComponents, root); +} + +void +cpBodyActivate(cpBody *_body) +{ + // Reset the idle time even if it's not in a currently sleeping component + // Like a body resting on or jointed to a rogue body. + _body.node.idleTime = 0.0f; + componentActivate(componentNodeRoot(_body)); +} + +static void +mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b) +{ + // Don't merge with the static body + if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return; + + cpBody *a_root = componentNodeRoot(a); + cpBody *b_root = componentNodeRoot(b); + + cpBool a_sleep = cpBodyIsSleeping(a_root); + cpBool b_sleep = cpBodyIsSleeping(b_root); + + if(a_sleep && b_sleep){ + return; + } else if(a_sleep || b_sleep){ + componentActivate(a_root); + componentActivate(b_root); + } + + // Add any rogue bodies (bodies not added to the space) + if(!a.space) cpArrayPush(rogueBodies, a); + if(!b.space) cpArrayPush(rogueBodies, b); + + componentNodeMerge(a_root, b_root); +} + +static cpBool +componentActive(cpBody *root, cpFloat threshold) +{ + cpBody *_body = root; + cpBody *next; + do { + next = _body.node.next; + if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue; + } while((_body = next) != root); + + return cpFalse; +} + +static void +addToComponent(cpBody *_body, cpArray *components) +{ + // Check that the body is not already added to the component list + if(_body.node.next) return; + cpBody *root = componentNodeRoot(_body); + + cpBody *next = root.node.next; + if(!next){ + // If the root isn't part of a list yet, then it hasn't been + // added to the components list. Do that now. + cpArrayPush(components, root); + // Start the list + _body.node.next = root; + root.node.next = _body; + } else if(root != _body) { + // Splice in body after the root. + _body.node.next = next; + root.node.next = _body; + } +} + +// TODO this function needs more commenting. +void +cpSpaceProcessComponents(cpSpace *space, cpFloat dt) +{ + cpArray *bodies = space.bodies; + cpArray *newBodies = cpArrayNew(bodies.num); + cpArray *rogueBodies = cpArrayNew(16); + cpArray *arbiters = space.arbiters; + cpArray *constraints = space.constraints; + cpArray *components = cpArrayNew(bodies.num/8); + + cpFloat dv = space.idleSpeedThreshold; + cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt); + // update idling + for(int i=0; i thresh ? 0.0f : _body.node.idleTime + dt); + } + + // iterate graph edges and build forests + for(int i=0; i a.x){ + x_inc = 1; + temp_h = (cpffloor(a.x + 1.0f) - a.x); + } else { + x_inc = -1; + temp_h = (a.x - cpffloor(a.x)); + } + + if (b.y > a.y){ + y_inc = 1; + temp_v = (cpffloor(a.y + 1.0f) - a.y); + } else { + y_inc = -1; + temp_v = (a.y - cpffloor(a.y)); + } + + // Division by zero is *very* slow on ARM + cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); + cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); + + // fix NANs in horizontal directions + cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); + cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); + + cpSpaceHashBin **table = hash.table; + + int n = hash.numcells; + while(t < t_exit){ + int idx = hash_func(cell_x, cell_y, n); + t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data)); + + if (next_v < next_h){ + cell_y += y_inc; + t = next_v; + next_v += dt_dy; + } else { + cell_x += x_inc; + t = next_h; + next_h += dt_dx; + } + } + + hash.stamp++; +} diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/cpSpaceQuery.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/cpSpaceQuery.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,167 @@ + +// written in the D programming language + +module chipmunkd.cpSpaceQuery; + +import chipmunkd.chipmunk; +import chipmunkd.chipmunk_types_h; + +struct pointQueryContext { + cpLayers layers; + cpGroup group; + cpSpacePointQueryFunc func; + void *data; +} + +static void +pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context) +{ + if( + !(shape.group && context.group == shape.group) && (context.layers&shape.layers) && + cpShapePointQuery(shape, *point) + ){ + context.func(shape, context.data); + } +} + +void +cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data) +{ + pointQueryContext context = {layers, group, func, data}; + cpSpaceHashPointQuery(space.activeShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context); + cpSpaceHashPointQuery(space.staticShapes, point, cast(cpSpaceHashQueryFunc)&pointQueryHelper, &context); +} + +static void +rememberLastPointQuery(cpShape *shape, cpShape **outShape) +{ + if(!shape.sensor) *outShape = shape; +} + +cpShape * +cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) +{ + cpShape *shape = null; + cpSpacePointQuery(space, point, layers, group, cast(cpSpacePointQueryFunc)&rememberLastPointQuery, &shape); + + return shape; +} + +void +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) +{ + cpArray *bodies = space.bodies; + + for(int i=0; i cp_contact_persistence){ + // The tail buffer is available, rotate the ring + cpContactBufferHeader *tail = head.next; + space.contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); + } else { + // Allocate a new buffer and push it into the ring + cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); + space.contactBuffersHead = head.next = buffer; + } +} + + +static cpContact * +cpContactBufferGetArray(cpSpace *space) +{ + if(space.contactBuffersHead.numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){ + // contact buffer could overflow on the next collision, push a fresh one. + cpSpacePushFreshContactBuffer(space); + } + + cpContactBufferHeader *head = space.contactBuffersHead; + return &(cast(cpContactBuffer *)head).contacts[head.numContacts]; //TODO: check if this works +} + +static void +cpSpacePushContacts(cpSpace *space, int count){ + assert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error, too many contact point overflow!"); + space.contactBuffersHead.numContacts += count; +} + +static void +cpSpacePopContacts(cpSpace *space, int count){ + space.contactBuffersHead.numContacts -= count; +} + +//#pragma mark Collision Detection Functions + +static cpBool +queryReject(cpShape *a, cpShape *b) +{ + return + // BBoxes must overlap + !cpBBintersects(a.bb, b.bb) + // Don't collide shapes attached to the same body. + || a._body == b._body + // Don't collide objects in the same non-zero group + || (a.group && a.group == b.group) + // Don't collide objects that don't share at least on layer. + || !(a.layers & b.layers); +} + +// Callback from the spatial hash. +static void +queryFunc(cpShape *a, cpShape *b, cpSpace *space) +{ + // Reject any of the simple cases + if(queryReject(a,b)) return; + + // Find the collision pair function for the shapes. + struct tmp{cpCollisionType a, b;} tmp ids = {a.collision_type, b.collision_type}; + cpHashValue collHashID = CP_HASH_PAIR(a.collision_type, b.collision_type); + cpCollisionHandler *handler = cast(cpCollisionHandler *)cpHashSetFind(space.collFuncSet, collHashID, &ids); + + cpBool sensor = a.sensor || b.sensor; + if(sensor && handler == &space.defaultHandler) return; + + // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) + if(a.klass.type > b.klass.type){ + cpShape *temp = a; + a = b; + b = temp; + } + + // Narrow-phase collision detection. + cpContact *contacts = cpContactBufferGetArray(space); + int numContacts = cpCollideShapes(a, b, contacts); + if(!numContacts) return; // Shapes are not colliding. + cpSpacePushContacts(space, numContacts); + + // Get an arbiter from space.contactSet for the two shapes. + // This is where the persistant contact magic comes from. + cpShape *shape_pair[] = [a, b]; + cpHashValue arbHashID = CP_HASH_PAIR(cast(size_t)a, cast(size_t)b); + cpArbiter *arb = cast(cpArbiter *)cpHashSetInsert(space.contactSet, arbHashID, shape_pair.ptr, space); + cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array + + // Call the begin function first if it's the first step + if(arb.state == cpArbiterState.cpArbiterStateFirstColl && !handler.begin(arb, space, handler.data)){ + cpArbiterIgnore(arb); // permanently ignore the collision until separation + } + + if( + // Ignore the arbiter if it has been flagged + (arb.state != cpArbiterState.cpArbiterStateIgnore) && + // Call preSolve + handler.preSolve(arb, space, handler.data) && + // Process, but don't add collisions for sensors. + !sensor + ){ + cpArrayPush(space.arbiters, arb); + } else { + cpSpacePopContacts(space, numContacts); + + arb.contacts = null; + arb.numContacts = 0; + + // Normally arbiters are set as used after calling the post-step callback. + // However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve. + if(arb.state != cpArbiterState.cpArbiterStateIgnore) arb.state = cpArbiterState.cpArbiterStateNormal; + } + + // Time stamp the arbiter so we know it was used recently. + arb.stamp = space.stamp; +} + +// Iterator for active/static hash collisions. +static void +active2staticIter(cpShape *shape, cpSpace *space) +{ + cpSpaceHashQuery(space.staticShapes, shape, shape.bb, cast(cpSpaceHashQueryFunc)&queryFunc, space); +} + +// Hashset filter func to throw away old arbiters. +static cpBool +contactSetFilter(cpArbiter *arb, cpSpace *space) +{ + if(space.sleepTimeThreshold != INFINITY){ + cpBody *a = arb.private_a._body; + cpBody *b = arb.private_b._body; + + // both bodies are either static or sleeping + cpBool sleepingNow = + (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && + (cpBodyIsStatic(b) || cpBodyIsSleeping(b)); + + if(sleepingNow){ + arb.state = cpArbiterState.cpArbiterStateSleep; + return cpTrue; + } else if(arb.state == cpArbiterState.cpArbiterStateSleep){ + // wake up the arbiter and continue as normal + arb.state = cpArbiterState.cpArbiterStateNormal; + // TODO is it possible that cpArbiterStateIgnore should be set here instead? + } + } + + cpTimestamp ticks = space.stamp - arb.stamp; + + // was used last frame, but not this one + if(ticks >= 1 && arb.state != cpArbiterState.cpArbiterStateCached){ + arb.handler.separate(arb, space, arb.handler.data); + arb.state = cpArbiterState.cpArbiterStateCached; + } + + if(ticks >= cp_contact_persistence){ + arb.contacts = null; + arb.numContacts = 0; + + cpArrayPush(space.pooledArbiters, arb); + return cpFalse; + } + + return cpTrue; +} + +// Hashset filter func to call and throw away post step callbacks. +static void +postStepCallbackSetIter(postStepCallback *callback, cpSpace *space) +{ + callback.func(space, callback.obj, callback.data); + cpfree(callback); +} + +//#pragma mark All Important cpSpaceStep() Function + +//void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); + +static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);} + +void +cpSpaceStep(cpSpace *space, cpFloat dt) +{ + if(!dt) return; // don't step if the timestep is 0! + + cpFloat dt_inv = 1.0f/dt; + + cpArray *bodies = space.bodies; + cpArray *constraints = space.constraints; + + space.locked = cpTrue; + + // Empty the arbiter list. + space.arbiters.num = 0; + + // Integrate positions. + for(int i=0; i len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +/// Linearly interpolate between v1 towards v2 by distance d. +static cpVect +cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) +{ + return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); +} + +/// Returns the distance between v1 and v2. +static cpFloat +cpvdist(const cpVect v1, const cpVect v2) +{ + return cpvlength(cpvsub(v1, v2)); +} + +/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances. +static cpFloat +cpvdistsq(const cpVect v1, const cpVect v2) +{ + return cpvlengthsq(cpvsub(v1, v2)); +} + +/// Returns true if the distance between v1 and v2 is less than dist. +static cpBool +cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) +{ + return cpvdistsq(v1, v2) < dist*dist; +} \ No newline at end of file diff -r 81145a61c2fe -r 7ebbd4d05553 trunk/chipmunkd/prime.d --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trunk/chipmunkd/prime.d Thu Dec 02 02:11:26 2010 +0100 @@ -0,0 +1,52 @@ + +// written in the D programming language + +module chipmunkd.prime; + +// Used for resizing hash tables. +// Values approximately double. +// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html +enum int primes[] = [ + 5, + 13, + 23, + 47, + 97, + 193, + 389, + 769, + 1543, + 3079, + 6151, + 12289, + 24593, + 49157, + 98317, + 196613, + 393241, + 786433, + 1572869, + 3145739, + 6291469, + 12582917, + 25165843, + 50331653, + 100663319, + 201326611, + 402653189, + 805306457, + 1610612741, + 0, +]; + +static int +next_prime(int n) +{ + int i = 0; + while(n > primes[i]){ + i++; + assert(primes[i]!=0, "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen + } + + return primes[i]; +} \ No newline at end of file