From 948c6d750eb81216b904ea2f4d0fee4bf5c418bb Mon Sep 17 00:00:00 2001 From: Walzer Date: Sat, 21 May 2011 14:58:22 +0800 Subject: [PATCH] issue #503, chippmunk library is upgraded from 5.1 to 5.3.4, which is usd in cocos2d-iphone 0.99.5 & 1.0.0-rc --- chipmunk/Android.mk | 3 + chipmunk/include/chipmunk/chipmunk.h | 107 +-- chipmunk/include/chipmunk/chipmunk_ffi.h | 11 +- chipmunk/include/chipmunk/chipmunk_types.h | 97 ++- .../chipmunk/constraints/cpConstraint.h | 19 +- .../constraints/cpDampedRotarySpring.h | 2 +- .../chipmunk/constraints/cpDampedSpring.h | 2 +- .../chipmunk/constraints/cpGrooveJoint.h | 6 +- chipmunk/include/chipmunk/constraints/util.h | 22 +- chipmunk/include/chipmunk/cpArbiter.h | 124 +++- chipmunk/include/chipmunk/cpArray.h | 9 +- chipmunk/include/chipmunk/cpBB.h | 6 +- chipmunk/include/chipmunk/cpBody.h | 101 ++- chipmunk/include/chipmunk/cpCollision.h | 9 +- chipmunk/include/chipmunk/cpHashSet.h | 32 +- chipmunk/include/chipmunk/cpPolyShape.h | 42 +- chipmunk/include/chipmunk/cpShape.h | 35 +- chipmunk/include/chipmunk/cpSpace.h | 70 +- chipmunk/include/chipmunk/cpSpaceHash.h | 21 +- chipmunk/include/chipmunk/cpVect.h | 66 +- chipmunk/proj.wophone/Makefile.ARM | 16 +- chipmunk/src/CMakeLists.txt | 2 +- chipmunk/src/chipmunk.c | 74 +- chipmunk/src/constraints/cpConstraint.c | 2 +- .../src/constraints/cpDampedRotarySpring.c | 15 +- chipmunk/src/constraints/cpDampedSpring.c | 18 +- chipmunk/src/constraints/cpGearJoint.c | 9 +- chipmunk/src/constraints/cpGrooveJoint.c | 33 +- chipmunk/src/constraints/cpPinJoint.c | 13 +- chipmunk/src/constraints/cpPivotJoint.c | 12 +- chipmunk/src/constraints/cpRatchetJoint.c | 12 +- chipmunk/src/constraints/cpRotaryLimitJoint.c | 8 +- chipmunk/src/constraints/cpSimpleMotor.c | 8 +- chipmunk/src/constraints/cpSlideJoint.c | 8 +- chipmunk/src/cpArbiter.c | 38 +- chipmunk/src/cpArray.c | 23 +- chipmunk/src/cpBody.c | 50 +- chipmunk/src/cpCollision.c | 76 ++- chipmunk/src/cpHashSet.c | 2 +- chipmunk/src/cpPolyShape.c | 17 +- chipmunk/src/cpShape.c | 73 +- chipmunk/src/cpSpace.c | 632 ++++-------------- chipmunk/src/cpSpaceHash.c | 183 +++-- 43 files changed, 1074 insertions(+), 1034 deletions(-) diff --git a/chipmunk/Android.mk b/chipmunk/Android.mk index a14266a562..e84dee18ee 100644 --- a/chipmunk/Android.mk +++ b/chipmunk/Android.mk @@ -26,7 +26,10 @@ src/cpHashSet.c \ src/cpPolyShape.c \ src/cpShape.c \ src/cpSpace.c \ +src/cpSpaceComponent.c \ src/cpSpaceHash.c \ +src/cpSpaceQuery.c \ +src/cpSpaceStep.c \ src/cpVect.c LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/chipmunk diff --git a/chipmunk/include/chipmunk/chipmunk.h b/chipmunk/include/chipmunk/chipmunk.h index c9c8870cc8..03581b0616 100644 --- a/chipmunk/include/chipmunk/chipmunk.h +++ b/chipmunk/include/chipmunk/chipmunk.h @@ -26,6 +26,16 @@ extern "C" { #endif +#ifndef CP_ALLOW_PRIVATE_ACCESS + #define CP_ALLOW_PRIVATE_ACCESS 1 +#endif + +#if CP_ALLOW_PRIVATE_ACCESS == 1 + #define CP_PRIVATE(symbol) symbol +#else + #define CP_PRIVATE(symbol) symbol##_private +#endif + void cpMessage(const char *message, const char *condition, const char *file, int line, int isError); #ifdef NDEBUG #define cpAssertWarn(condition, message) @@ -40,43 +50,7 @@ void cpMessage(const char *message, const char *condition, const char *file, int #endif #include "chipmunk_types.h" - -static inline cpFloat -cpfmax(cpFloat a, cpFloat b) -{ - return (a > b) ? a : b; -} - -static inline cpFloat -cpfmin(cpFloat a, cpFloat b) -{ - return (a < b) ? a : b; -} - -static inline cpFloat -cpfabs(cpFloat n) -{ - return (n < 0) ? -n : n; -} - -static inline cpFloat -cpfclamp(cpFloat f, cpFloat min, cpFloat max) -{ - return cpfmin(cpfmax(f, min), max); -} - -static inline cpFloat -cpflerp(cpFloat f1, cpFloat f2, cpFloat t) -{ - return f1*(1.0f - t) + f2*t; -} - -static inline cpFloat -cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) -{ - return f1 + cpfclamp(f2 - f1, -d, d); -} - + #ifndef INFINITY #ifdef _MSC_VER union MSVC_EVIL_FLOAT_HACK @@ -97,7 +71,7 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) #endif #endif -// Maximum allocated size for various Chipmunk buffer sizes +// Maximum allocated size for various Chipmunk buffers #define CP_BUFFER_BYTES (32*1024) #define cpmalloc malloc @@ -107,11 +81,11 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) #include "cpVect.h" #include "cpBB.h" -#include "cpBody.h" #include "cpArray.h" #include "cpHashSet.h" #include "cpSpaceHash.h" +#include "cpBody.h" #include "cpShape.h" #include "cpPolyShape.h" @@ -125,24 +99,65 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) #define CP_HASH_COEF (3344921057ul) #define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) -extern char *cpVersionString; +extern const char *cpVersionString; void cpInitChipmunk(void); -// 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) +/** + 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); -// Calculate the moment of inertia for a line segment. (beveling radius not supported) +/** + Calculate area of a hollow circle. +*/ +cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2); + +/** + Calculate the moment of inertia for a line segment. + Beveling radius is not supported. +*/ cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b); -// Calculate the moment of inertia for a solid polygon shape. -cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset); +/** + Calculate the area of a fattened (capsule shaped) line segment. +*/ +cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r); -// Calculate the moment of inertia for a solid box. +/** + Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex. +*/ +cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset); + +/** + Calculate the signed area of a polygon. +*/ +cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts); + +/** + Calculate the natural centroid of a polygon. +*/ +cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts); + +/** + Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex) +*/ +void cpRecenterPoly(const int numVerts, cpVect *verts); + +/** + Calculate the moment of inertia for a solid box. +*/ cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); #ifdef __cplusplus } + +static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);} +static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);} +static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);} +static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);} +static inline cpVect operator -(const cpVect v){return cpvneg(v);} + #endif #endif diff --git a/chipmunk/include/chipmunk/chipmunk_ffi.h b/chipmunk/include/chipmunk/chipmunk_ffi.h index a5aa6e6f27..95dc4f65b1 100644 --- a/chipmunk/include/chipmunk/chipmunk_ffi.h +++ b/chipmunk/include/chipmunk/chipmunk_ffi.h @@ -15,6 +15,7 @@ #endif MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() +MAKE_REF(cpveql); MAKE_REF(cpvadd); MAKE_REF(cpvneg); MAKE_REF(cpvsub); @@ -33,8 +34,8 @@ MAKE_REF(cpvnormalize_safe); MAKE_REF(cpvclamp); MAKE_REF(cpvlerpconst); MAKE_REF(cpvdist); -MAKE_REF(cpvnear); MAKE_REF(cpvdistsq); +MAKE_REF(cpvnear); MAKE_REF(cpBBNew); MAKE_REF(cpBBintersects); @@ -46,10 +47,18 @@ MAKE_REF(cpBBexpand); MAKE_REF(cpBodyWorld2Local); MAKE_REF(cpBodyLocal2World); MAKE_REF(cpBodyApplyImpulse); +MAKE_REF(cpBodyIsSleeping); +MAKE_REF(cpBodyIsRogue); +MAKE_REF(cpBodyKineticEnergy); MAKE_REF(cpArbiterIsFirstContact); MAKE_REF(cpArbiterGetShapes); MAKE_REF(cpArbiterGetNormal); MAKE_REF(cpArbiterGetPoint); +MAKE_REF(cpConstraintGetImpulse); + +MAKE_REF(cpSegmentQueryHitPoint); +MAKE_REF(cpSegmentQueryHitDist); + #endif // _CHIPMUNK_FFI_H_ \ No newline at end of file diff --git a/chipmunk/include/chipmunk/chipmunk_types.h b/chipmunk/include/chipmunk/chipmunk_types.h index 9268b78f92..7d38472ef1 100644 --- a/chipmunk/include/chipmunk/chipmunk_types.h +++ b/chipmunk/include/chipmunk/chipmunk_types.h @@ -3,14 +3,29 @@ #ifdef __APPLE__ #ifndef AIRPLAYUSECHIPMUNK - #import "TargetConditionals.h" -#endif + #import "TargetConditionals.h" #endif -// Use single precision floats on the iPhone. -#if TARGET_OS_IPHONE==1 - #define CP_USE_DOUBLES 0 -#else +// dont' use CGPoints in cocos2d-x to make your code multi-platform +// #if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS) +// #define CP_USE_CGPOINTS +// #endif + +#ifdef CP_USE_CGPOINTS + #if TARGET_OS_IPHONE + #import + #elif TARGET_OS_MAC + #import + #endif + + #if defined(__LP64__) && __LP64__ + #define CP_USE_DOUBLES 1 + #else + #define CP_USE_DOUBLES 0 + #endif +#endif + +#ifndef CP_USE_DOUBLES // use doubles by default for higher precision #define CP_USE_DOUBLES 1 #endif @@ -41,17 +56,67 @@ #define cpfceil ceilf #endif -//#if TARGET_OS_IPHONE - // CCPoints are structurally the same, and allow - // easy interoperability with other iPhone libraries - //#import - //typedef CCPoint cpVect; -//#else +static inline cpFloat +cpfmax(cpFloat a, cpFloat b) +{ + return (a > b) ? a : b; +} + +static inline cpFloat +cpfmin(cpFloat a, cpFloat b) +{ + return (a < b) ? a : b; +} + +static inline cpFloat +cpfabs(cpFloat n) +{ + return (n < 0) ? -n : n; +} + +static inline cpFloat +cpfclamp(cpFloat f, cpFloat min, cpFloat max) +{ + return cpfmin(cpfmax(f, min), max); +} + +static inline cpFloat +cpflerp(cpFloat f1, cpFloat f2, cpFloat t) +{ + return f1*(1.0f - t) + f2*t; +} + +static inline cpFloat +cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) +{ + return f1 + cpfclamp(f2 - f1, -d, d); +} + +// CGPoints are structurally the same, and allow +// easy interoperability with other Cocoa libraries +#ifdef CP_USE_CGPOINTS + typedef CGPoint cpVect; +#else typedef struct cpVect{cpFloat x,y;} cpVect; -//#endif +#endif typedef unsigned int 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 + typedef int cpBool; +#endif + +#ifndef cpTrue + #define cpTrue 1 +#endif + +#ifndef cpFalse + #define cpFalse 0 +#endif + #ifdef CP_DATA_POINTER_TYPE typedef CP_DATA_POINTER_TYPE cpDataPointer; #else @@ -76,6 +141,12 @@ typedef unsigned int cpHashValue; typedef unsigned int cpLayers; #endif +#ifdef CP_TIMESTAMP_TYPE + typedef CP_TIMESTAMP_TYPE cpTimestamp; +#else + typedef unsigned int cpTimestamp; +#endif + #ifndef CP_NO_GROUP #define CP_NO_GROUP ((cpGroup)0) #endif diff --git a/chipmunk/include/chipmunk/constraints/cpConstraint.h b/chipmunk/include/chipmunk/constraints/cpConstraint.h index a361d7aab2..fe11e65196 100644 --- a/chipmunk/include/chipmunk/constraints/cpConstraint.h +++ b/chipmunk/include/chipmunk/constraints/cpConstraint.h @@ -39,7 +39,7 @@ typedef struct cpConstraintClass { typedef struct cpConstraint { - const cpConstraintClass *klass; + CP_PRIVATE(const cpConstraintClass *klass); cpBody *a, *b; cpFloat maxForce; @@ -56,14 +56,26 @@ typedef cpConstraint cpJoint; void cpConstraintDestroy(cpConstraint *constraint); void cpConstraintFree(cpConstraint *constraint); +static inline void +cpConstraintActivateBodies(cpConstraint *constraint) +{ + cpBody *a = constraint->a; if(a) cpBodyActivate(a); + cpBody *b = constraint->b; if(b) cpBodyActivate(b); +} + +static inline cpFloat +cpConstraintGetImpulse(cpConstraint *constraint) +{ + return constraint->CP_PRIVATE(klass)->getImpulse(constraint); +} #define cpConstraintCheckCast(constraint, struct) \ - cpAssert(constraint->klass == struct##GetClass(), "Constraint is not a "#struct); + cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct); #define CP_DefineConstraintGetter(struct, type, member, name) \ static inline type \ -struct##Get##name(cpConstraint *constraint){ \ +struct##Get##name(const cpConstraint *constraint){ \ cpConstraintCheckCast(constraint, struct); \ return ((struct *)constraint)->member; \ } \ @@ -72,6 +84,7 @@ struct##Get##name(cpConstraint *constraint){ \ static inline void \ struct##Set##name(cpConstraint *constraint, type value){ \ cpConstraintCheckCast(constraint, struct); \ + cpConstraintActivateBodies(constraint); \ ((struct *)constraint)->member = value; \ } \ diff --git a/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h b/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h index 7ea65f83dd..b446a3a513 100644 --- a/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h +++ b/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h @@ -30,8 +30,8 @@ typedef struct cpDampedRotarySpring { cpFloat damping; cpDampedRotarySpringTorqueFunc springTorqueFunc; - cpFloat dt; cpFloat target_wrn; + cpFloat w_coef; cpFloat iSum; } cpDampedRotarySpring; diff --git a/chipmunk/include/chipmunk/constraints/cpDampedSpring.h b/chipmunk/include/chipmunk/constraints/cpDampedSpring.h index 55780cbc91..732812892c 100644 --- a/chipmunk/include/chipmunk/constraints/cpDampedSpring.h +++ b/chipmunk/include/chipmunk/constraints/cpDampedSpring.h @@ -33,8 +33,8 @@ typedef struct cpDampedSpring { cpFloat damping; cpDampedSpringForceFunc springForceFunc; - cpFloat dt; cpFloat target_vrn; + cpFloat v_coef; cpVect r1, r2; cpFloat nMass; diff --git a/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h b/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h index dc54da9a5a..831e3b68c8 100644 --- a/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h +++ b/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h @@ -40,5 +40,9 @@ cpGrooveJoint *cpGrooveJointAlloc(void); cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); -// TODO setters for the groove. + +CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA); +void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value); +CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB); +void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value); CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2); diff --git a/chipmunk/include/chipmunk/constraints/util.h b/chipmunk/include/chipmunk/constraints/util.h index b7242c777c..e8c0e0b2a0 100644 --- a/chipmunk/include/chipmunk/constraints/util.h +++ b/chipmunk/include/chipmunk/constraints/util.h @@ -25,6 +25,17 @@ void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, #define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt)) +// Get valid body pointers and exit early if the bodies are idle +#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \ +cpBody *a_var, *b_var; { \ + a_var = ((cpConstraint *)constraint)->a; \ + b_var = ((cpConstraint *)constraint)->b; \ + if( \ + (cpBodyIsSleeping(a_var) || cpBodyIsStatic(a_var)) && \ + (cpBodyIsSleeping(b_var) || cpBodyIsStatic(b_var)) \ + ) return; \ +} + static inline cpVect relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); @@ -45,11 +56,18 @@ apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) cpBodyApplyImpulse(b, j, r2); } +static inline 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 inline void apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) { - cpBodyApplyBiasImpulse(a, cpvneg(j), r1); - cpBodyApplyBiasImpulse(b, j, r2); + apply_bias_impulse(a, cpvneg(j), r1); + apply_bias_impulse(b, j, r2); } static inline cpVect diff --git a/chipmunk/include/chipmunk/cpArbiter.h b/chipmunk/include/chipmunk/cpArbiter.h index 6db9b29e86..31bb929ff3 100644 --- a/chipmunk/include/chipmunk/cpArbiter.h +++ b/chipmunk/include/chipmunk/cpArbiter.h @@ -31,73 +31,79 @@ extern cpFloat cp_collision_slop; // Data structure for contact points. typedef struct cpContact { // Contact point and normal. - cpVect p, n; + cpVect CP_PRIVATE(p), CP_PRIVATE(n); // Penetration distance. - cpFloat dist; + CP_PRIVATE(cpFloat dist); // Calculated by cpArbiterPreStep(). - cpVect r1, r2; - cpFloat nMass, tMass, bounce; + cpVect CP_PRIVATE(r1), CP_PRIVATE(r2); + cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce); // Persistant contact information. - cpFloat jnAcc, jtAcc, jBias; - cpFloat bias; + cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias); + CP_PRIVATE(cpFloat bias); // Hash value used to (mostly) uniquely identify a contact. - cpHashValue hash; + CP_PRIVATE(cpHashValue hash); } cpContact; // 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); +cpVect CP_PRIVATE(cpContactsSumImpulses)(cpContact *contacts, int numContacts); +cpVect CP_PRIVATE(cpContactsSumImpulsesWithFriction)(cpContact *contacts, int numContacts); + +#define CP_MAX_CONTACTS_PER_ARBITER 6 typedef enum cpArbiterState { cpArbiterStateNormal, cpArbiterStateFirstColl, cpArbiterStateIgnore, + cpArbiterStateSleep, + cpArbiterStateCached, } cpArbiterState; // Data structure for tracking collisions between shapes. typedef struct cpArbiter { // Information on the contact points between the objects. - int numContacts; - cpContact *contacts; + CP_PRIVATE(int numContacts); + CP_PRIVATE(cpContact *contacts); - // The two shapes involved in the collision. + // The two shapes and bodies involved in the collision. // These variables are NOT in the order defined by the collision handler. - cpShape *private_a, *private_b; + // Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from + // many headaches + cpShape CP_PRIVATE(*a), CP_PRIVATE(*b); // Calculated before calling the pre-solve collision handler // Override them with custom values if you want specialized behavior - cpFloat e; - cpFloat u; + CP_PRIVATE(cpFloat e); + CP_PRIVATE(cpFloat u); // Used for surface_v calculations, implementation may change - cpVect surface_vr; + CP_PRIVATE(cpVect surface_vr); // Time stamp of the arbiter. (from cpSpace) - int stamp; + CP_PRIVATE(cpTimestamp stamp); - struct cpCollisionHandler *handler; + CP_PRIVATE(struct cpCollisionHandler *handler); // Are the shapes swapped in relation to the collision handler? - char swappedColl; - char state; + CP_PRIVATE(cpBool swappedColl); + CP_PRIVATE(cpArbiterState state); } cpArbiter; // Arbiters are allocated in large buffers by the space and don't require a destroy function -cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); +cpArbiter* CP_PRIVATE(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); +void CP_PRIVATE(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); +void CP_PRIVATE(cpArbiterPreStep)(cpArbiter *arb, cpFloat dt_inv); +void CP_PRIVATE(cpArbiterApplyCachedImpulse)(cpArbiter *arb); // Run an iteration of the solver on the arbiter. -void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef); +void CP_PRIVATE(cpArbiterApplyImpulse)(cpArbiter *arb, cpFloat eCoef); // Arbiter Helper Functions cpVect cpArbiterTotalImpulse(cpArbiter *arb); @@ -106,31 +112,77 @@ void cpArbiterIgnore(cpArbiter *arb); static inline void -cpArbiterGetShapes(cpArbiter *arb, cpShape **a, cpShape **b) +cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) { - if(arb->swappedColl){ - (*a) = arb->private_b, (*b) = arb->private_a; + if(arb->CP_PRIVATE(swappedColl)){ + (*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a); } else { - (*a) = arb->private_a, (*b) = arb->private_b; + (*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b); } } #define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b); +static inline void +cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) +{ + CP_ARBITER_GET_SHAPES(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 inline cpBool +cpArbiterIsFirstContact(const cpArbiter *arb) +{ + return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl; +} + static inline int -cpArbiterIsFirstContact(cpArbiter *arb) +cpArbiterGetCount(const cpArbiter *arb) { - return arb->state == cpArbiterStateFirstColl; + return arb->CP_PRIVATE(numContacts); } static inline cpVect -cpArbiterGetNormal(cpArbiter *arb, int i) +cpArbiterGetNormal(const cpArbiter *arb, int i) { - cpVect n = arb->contacts[i].n; - return arb->swappedColl ? cpvneg(n) : n; + cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); + return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n; } static inline cpVect -cpArbiterGetPoint(cpArbiter *arb, int i) +cpArbiterGetPoint(const cpArbiter *arb, int i) { - return arb->contacts[i].p; + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); +} + +static inline cpFloat +cpArbiteGetDepth(const cpArbiter *arb, int i) +{ + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); +} + +typedef struct cpContactPointSet { + int count; + + struct { + cpVect point, normal; + cpFloat dist; + } points[CP_MAX_CONTACTS_PER_ARBITER]; +} cpContactPointSet; + +static inline cpContactPointSet +cpArbiterGetContactPointSet(const cpArbiter *arb) +{ + cpContactPointSet set; + set.count = cpArbiterGetCount(arb); + + int i; + for(i=0; iCP_PRIVATE(contacts)[i].CP_PRIVATE(p); + set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); + set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); + } + + return set; } diff --git a/chipmunk/include/chipmunk/cpArray.h b/chipmunk/include/chipmunk/cpArray.h index 9417c3637c..742c391293 100644 --- a/chipmunk/include/chipmunk/cpArray.h +++ b/chipmunk/include/chipmunk/cpArray.h @@ -22,8 +22,9 @@ // NOTE: cpArray is rarely used and will probably go away. typedef struct cpArray{ - int num, max; - void **arr; + CP_PRIVATE(int num); + CP_PRIVATE(int max); + CP_PRIVATE(void **arr); } cpArray; typedef void (*cpArrayIter)(void *ptr, void *data); @@ -42,5 +43,7 @@ void *cpArrayPop(cpArray *arr); void cpArrayDeleteIndex(cpArray *arr, int idx); void cpArrayDeleteObj(cpArray *arr, void *obj); +void cpArrayAppend(cpArray *arr, cpArray *other); + void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data); -int cpArrayContains(cpArray *arr, void *ptr); +cpBool cpArrayContains(cpArray *arr, void *ptr); diff --git a/chipmunk/include/chipmunk/cpBB.h b/chipmunk/include/chipmunk/cpBB.h index 3d467dbe75..e2563590ca 100644 --- a/chipmunk/include/chipmunk/cpBB.h +++ b/chipmunk/include/chipmunk/cpBB.h @@ -31,19 +31,19 @@ cpBBNew(const cpFloat l, const cpFloat b, return bb; } -static inline int +static inline cpBool cpBBintersects(const cpBB a, const cpBB b) { return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t); } -static inline int +static inline cpBool cpBBcontainsBB(const cpBB bb, const cpBB other) { return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t); } -static inline int +static inline 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); diff --git a/chipmunk/include/chipmunk/cpBody.h b/chipmunk/include/chipmunk/cpBody.h index 163c29c17d..ba1a3372b5 100644 --- a/chipmunk/include/chipmunk/cpBody.h +++ b/chipmunk/include/chipmunk/cpBody.h @@ -20,14 +20,27 @@ */ struct cpBody; +struct cpShape; +struct cpSpace; + typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt); 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. +typedef struct cpComponentNode { + struct cpBody *parent; + struct cpBody *next; + int rank; + cpFloat idleTime; +} cpComponentNode; + typedef struct cpBody{ - // *** Integration Functions.ntoehu + // *** Integration Functions. // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) cpBodyVelocityFunc velocity_func; @@ -63,16 +76,26 @@ typedef struct cpBody{ // 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; + CP_PRIVATE(cpVect v_bias); + CP_PRIVATE(cpFloat w_bias); -// int active; + // Space this body has been added to + CP_PRIVATE(struct cpSpace *space); + + // Pointer to the shape list. + // Shapes form a linked list using cpShape.next when added to a space. + CP_PRIVATE(struct cpShape *shapesList); + + // Used by cpSpaceStep() to store contact graph information. + CP_PRIVATE(cpComponentNode node); } cpBody; // Basic allocation/destruction functions @@ -80,11 +103,48 @@ cpBody *cpBodyAlloc(void); cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); cpBody *cpBodyNew(cpFloat m, cpFloat i); +cpBody *cpBodyInitStatic(cpBody *body); +cpBody *cpBodyNewStatic(); + void cpBodyDestroy(cpBody *body); void cpBodyFree(cpBody *body); -#define CP_DefineBodyGetter(type, member, name) static inline type cpBodyGet##name(cpBody *body){return body->member;} -#define CP_DefineBodySetter(type, member, name) static inline void cpBodySet##name(cpBody *body, type value){body->member = value;} +// Wake up a sleeping or idle body. (defined in cpSpace.c) +void cpBodyActivate(cpBody *body); + +// Force a body to sleep; +// defined in cpSpaceComponent.c +void cpBodySleep(cpBody *body); +void cpBodySleepWithGroup(cpBody *body, cpBody *group); + +static inline cpBool +cpBodyIsSleeping(const cpBody *body) +{ + return (CP_PRIVATE(body->node).next != ((cpBody*)0)); +} + +static inline cpBool +cpBodyIsStatic(const cpBody *body) +{ + return CP_PRIVATE(body->node).idleTime == INFINITY; +} + +static inline cpBool +cpBodyIsRogue(const cpBody *body) +{ + return (body->CP_PRIVATE(space) == ((struct cpSpace*)0)); +} + + +#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) \ @@ -120,41 +180,40 @@ void cpBodyUpdatePosition(cpBody *body, cpFloat dt); // Convert body local to world coordinates static inline cpVect -cpBodyLocal2World(cpBody *body, cpVect v) +cpBodyLocal2World(const cpBody *body, const cpVect v) { return cpvadd(body->p, cpvrotate(v, body->rot)); } // Convert world to body local coordinates static inline cpVect -cpBodyWorld2Local(cpBody *body, cpVect v) +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 inline void -cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r) +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); } -// Not intended for external use. Used by cpArbiter.c and cpConstraint.c. -static inline void -cpBodyApplyBiasImpulse(cpBody *body, cpVect j, cpVect r) -{ - body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); - body->w_bias += body->i_inv*cpvcross(r, j); -} - // Zero the forces on a body. void cpBodyResetForces(cpBody *body); // Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates). -void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r); +void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); + +static inline 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); - -//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max); diff --git a/chipmunk/include/chipmunk/cpCollision.h b/chipmunk/include/chipmunk/cpCollision.h index 374273c71e..1c886f713b 100644 --- a/chipmunk/include/chipmunk/cpCollision.h +++ b/chipmunk/include/chipmunk/cpCollision.h @@ -19,5 +19,10 @@ * SOFTWARE. */ -// Collides two cpShape structures. (this function is lonely :( ) -int cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr); +//TODO delete this header? + +// Collides two cpShape structures. +// Returns the number of contact points added to arr +// which should be at least CP_MAX_CONTACTS_PER_ARBITER in length. +// This function is very lonely in this header :( +int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr); diff --git a/chipmunk/include/chipmunk/cpHashSet.h b/chipmunk/include/chipmunk/cpHashSet.h index 2b0f404857..cafdbf5129 100644 --- a/chipmunk/include/chipmunk/cpHashSet.h +++ b/chipmunk/include/chipmunk/cpHashSet.h @@ -25,39 +25,36 @@ // cpHashSetBin's form the linked lists in the chained hash table. typedef struct cpHashSetBin { // Pointer to the element. - void *elt; + CP_PRIVATE(void *elt); // Hash value of the element. - cpHashValue hash; + CP_PRIVATE(cpHashValue hash); // Next element in the chain. - struct cpHashSetBin *next; + CP_PRIVATE(struct cpHashSetBin *next); } cpHashSetBin; // Equality function. Returns true if ptr is equal to elt. -typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt); +typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); // Used by cpHashSetInsert(). Called to transform the ptr into an element. typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); -// Iterator function for a hashset. -typedef void (*cpHashSetIterFunc)(void *elt, void *data); -// Filter function. Returns false if elt should be dropped. -typedef int (*cpHashSetFilterFunc)(void *elt, void *data); typedef struct cpHashSet { // Number of elements stored in the table. - int entries; + CP_PRIVATE(int entries); // Number of cells in the table. - int size; + CP_PRIVATE(int size); - cpHashSetEqlFunc eql; - cpHashSetTransFunc trans; + CP_PRIVATE(cpHashSetEqlFunc eql); + CP_PRIVATE(cpHashSetTransFunc trans); // Default value returned by cpHashSetFind() when no element is found. // Defaults to NULL. - void *default_value; + CP_PRIVATE(void *default_value); // The table and recycled bins - cpHashSetBin **table, *pooledBins; + CP_PRIVATE(cpHashSetBin **table); + CP_PRIVATE(cpHashSetBin *pooledBins); - cpArray *allocatedBuffers; + CP_PRIVATE(cpArray *allocatedBuffers); } cpHashSet; // Basic allocation/destruction functions. @@ -77,6 +74,9 @@ void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); // Iterate over a hashset. +typedef void (*cpHashSetIterFunc)(void *elt, void *data); void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); -// Iterate over a hashset, retain . + +// Iterate over a hashset, drop the element if the func returns false. +typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); diff --git a/chipmunk/include/chipmunk/cpPolyShape.h b/chipmunk/include/chipmunk/cpPolyShape.h index 11d822c8c3..12816ce56b 100644 --- a/chipmunk/include/chipmunk/cpPolyShape.h +++ b/chipmunk/include/chipmunk/cpPolyShape.h @@ -29,16 +29,16 @@ typedef struct cpPolyShapeAxis{ // Convex polygon shape structure. typedef struct cpPolyShape{ - cpShape shape; + CP_PRIVATE(cpShape shape); // Vertex and axis lists. - int numVerts; - cpVect *verts; - cpPolyShapeAxis *axes; + CP_PRIVATE(int numVerts); + CP_PRIVATE(cpVect *verts); + CP_PRIVATE(cpPolyShapeAxis *axes); // Transformed vertex and axis lists. - cpVect *tVerts; - cpPolyShapeAxis *tAxes; + CP_PRIVATE(cpVect *tVerts); + CP_PRIVATE(cpPolyShapeAxis *tAxes); } cpPolyShape; // Basic allocation functions. @@ -50,7 +50,7 @@ cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFl cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); // Check that a set of vertexes has a correct winding and that they are convex -int cpPolyValidate(cpVect *verts, int numVerts); +cpBool cpPolyValidate(const cpVect *verts, const int numVerts); int cpPolyShapeGetNumVerts(cpShape *shape); cpVect cpPolyShapeGetVert(cpShape *shape, int idx); @@ -61,43 +61,43 @@ cpVect cpPolyShapeGetVert(cpShape *shape, int idx); static inline cpFloat cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) { - cpVect *verts = poly->tVerts; + cpVect *verts = poly->CP_PRIVATE(tVerts); cpFloat min = cpvdot(n, verts[0]); int i; - for(i=1; inumVerts; i++) + for(i=1; iCP_PRIVATE(numVerts); i++) min = cpfmin(min, cpvdot(n, verts[i])); return min - d; } // Returns true if the polygon contains the vertex. -static inline int -cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v) +static inline cpBool +cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) { - cpPolyShapeAxis *axes = poly->tAxes; + cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); int i; - for(i=0; inumVerts; i++){ + for(i=0; iCP_PRIVATE(numVerts); i++){ cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; - if(dist > 0.0f) return 0; + if(dist > 0.0f) return cpFalse; } - return 1; + return cpTrue; } // Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal. -static inline int -cpPolyShapeContainsVertPartial(cpPolyShape *poly, cpVect v, cpVect n) +static inline cpBool +cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n) { - cpPolyShapeAxis *axes = poly->tAxes; + cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); int i; - for(i=0; inumVerts; i++){ + for(i=0; iCP_PRIVATE(numVerts); i++){ if(cpvdot(axes[i].n, n) < 0.0f) continue; cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; - if(dist > 0.0f) return 0; + if(dist > 0.0f) return cpFalse; } - return 1; + return cpTrue; } diff --git a/chipmunk/include/chipmunk/cpShape.h b/chipmunk/include/chipmunk/cpShape.h index f402185ebe..5335642aad 100644 --- a/chipmunk/include/chipmunk/cpShape.h +++ b/chipmunk/include/chipmunk/cpShape.h @@ -47,7 +47,7 @@ typedef struct cpShapeClass { void (*destroy)(struct cpShape *shape); // called by cpShapePointQuery(). - int (*pointQuery)(struct cpShape *shape, cpVect p); + cpBool (*pointQuery)(struct cpShape *shape, cpVect p); // called by cpShapeSegmentQuery() void (*segmentQuery)(struct cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); @@ -56,7 +56,7 @@ typedef struct cpShapeClass { // Basic shape struct that the others inherit from. typedef struct cpShape{ // The "class" of a shape as defined above - const cpShapeClass *klass; + CP_PRIVATE(const cpShapeClass *klass); // cpBody that the shape is attached to. cpBody *body; @@ -65,7 +65,7 @@ typedef struct cpShape{ cpBB bb; // Sensors invoke callbacks, but do not generate collisions - int sensor; + cpBool sensor; // *** Surface properties. @@ -90,8 +90,11 @@ typedef struct cpShape{ // *** Internally Used Fields + // Shapes form a linked list when added to space on a non-NULL body + CP_PRIVATE(struct cpShape *next); + // Unique id used as the hash value. - cpHashValue hashid; + CP_PRIVATE(cpHashValue hashid); } cpShape; // Low level shape initialization func. @@ -105,21 +108,21 @@ void cpShapeFree(cpShape *shape); cpBB cpShapeCacheBB(cpShape *shape); // Test if a point lies within a shape. -int cpShapePointQuery(cpShape *shape, cpVect p); +cpBool cpShapePointQuery(cpShape *shape, cpVect p); #define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape) // Circle shape structure. typedef struct cpCircleShape{ - cpShape shape; + CP_PRIVATE(cpShape shape); // Center in body space coordinates - cpVect c; + CP_PRIVATE(cpVect c); // Radius. - cpFloat r; + CP_PRIVATE(cpFloat r); // Transformed center. (world space coordinates) - cpVect tc; + CP_PRIVATE(cpVect tc); } cpCircleShape; // Basic allocation functions for cpCircleShape. @@ -132,15 +135,15 @@ CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); // Segment shape structure. typedef struct cpSegmentShape{ - cpShape shape; + CP_PRIVATE(cpShape shape); // Endpoints and normal of the segment. (body space coordinates) - cpVect a, b, n; + cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n); // Radius of the segment. (Thickness) - cpFloat r; + cpFloat CP_PRIVATE(r); // Transformed endpoints and normal. (world space coordinates) - cpVect ta, tb, tn; + cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn); } cpSegmentShape; // Basic allocation functions for cpSegmentShape. @@ -159,16 +162,16 @@ void cpResetShapeIdCounter(void); // Directed segment queries against individual shapes. void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info); -int cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); +cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); static inline cpVect -cpSegmentQueryHitPoint(cpVect start, cpVect end, cpSegmentQueryInfo info) +cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) { return cpvlerp(start, end, info.t); } static inline cpFloat -cpSegmentQueryHitDist(cpVect start, cpVect end, cpSegmentQueryInfo info) +cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) { return cpvdist(start, end)*info.t; } diff --git a/chipmunk/include/chipmunk/cpSpace.h b/chipmunk/include/chipmunk/cpSpace.h index 06d4c70010..2f6832a082 100644 --- a/chipmunk/include/chipmunk/cpSpace.h +++ b/chipmunk/include/chipmunk/cpSpace.h @@ -22,11 +22,11 @@ struct cpSpace; // Number of frames that contact information should persist. -extern int cp_contact_persistence; +extern cpTimestamp cp_contact_persistence; // User collision handler function types. -typedef int (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data); -typedef int (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data); +typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data); +typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data); typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data); typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, struct cpSpace *space, void *data); @@ -42,9 +42,8 @@ typedef struct cpCollisionHandler { void *data; } cpCollisionHandler; -#define CP_MAX_CONTACTS_PER_ARBITER 6 typedef struct cpContactBufferHeader { - int stamp; + cpTimestamp stamp; struct cpContactBufferHeader *next; unsigned int numContacts; } cpContactBufferHeader; @@ -64,44 +63,62 @@ typedef struct cpSpace{ // 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; - int locked; + // When the space lock count is non zero you cannot add or remove objects + CP_PRIVATE(int locked); // Time stamp. Is incremented on every call to cpSpaceStep(). - int stamp; + CP_PRIVATE(cpTimestamp stamp); // The static and active shape spatial hashes. - cpSpaceHash *staticShapes; - cpSpaceHash *activeShapes; + CP_PRIVATE(cpSpaceHash *staticShapes); + CP_PRIVATE(cpSpaceHash *activeShapes); // List of bodies in the system. - cpArray *bodies; + CP_PRIVATE(cpArray *bodies); + + // List of groups of sleeping bodies. + CP_PRIVATE(cpArray *sleepingComponents); + + // List of bodies that have been flagged to be awoken. + CP_PRIVATE(cpArray *rousedBodies); // List of active arbiters for the impulse solver. - cpArray *arbiters, *pooledArbiters; + CP_PRIVATE(cpArray *arbiters); + CP_PRIVATE(cpArray *pooledArbiters); // Linked list ring of contact buffers. - // Head is the current buffer. Tail is the oldest buffer. - // The list points in the direction of tail->head. - cpContactBufferHeader *contactBuffersHead, *contactBuffersTail; + // Head is the newest buffer, and each buffer points to a newer buffer. + // Head wraps around and points to the oldest (tail) buffer. + CP_PRIVATE(cpContactBufferHeader *contactBuffersHead); + CP_PRIVATE(cpContactBufferHeader *_contactBuffersTail_Deprecated); // List of buffers to be free()ed when destroying the space. - cpArray *allocatedBuffers; + CP_PRIVATE(cpArray *allocatedBuffers); // Persistant contact set. - cpHashSet *contactSet; + CP_PRIVATE(cpHashSet *contactSet); // List of constraints in the system. - cpArray *constraints; + CP_PRIVATE(cpArray *constraints); // Set of collisionpair functions. - cpHashSet *collFuncSet; + CP_PRIVATE(cpHashSet *collFuncSet); // Default collision handler. - cpCollisionHandler defaultHandler; + CP_PRIVATE(cpCollisionHandler defaultHandler); - cpHashSet *postStepCallbacks; + CP_PRIVATE(cpHashSet *postStepCallbacks); + + cpBody staticBody; } cpSpace; // Basic allocation/destruction functions. @@ -159,13 +176,20 @@ cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, c // Segment query callback function typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data); -int cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data); +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 typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); +// Shape query callback function +typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); +cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); + + +void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape); + // Iterator function for iterating the bodies in a space. typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data); @@ -176,5 +200,7 @@ 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); diff --git a/chipmunk/include/chipmunk/cpSpaceHash.h b/chipmunk/include/chipmunk/cpSpaceHash.h index 85766b1b80..dac38a371f 100644 --- a/chipmunk/include/chipmunk/cpSpaceHash.h +++ b/chipmunk/include/chipmunk/cpSpaceHash.h @@ -30,7 +30,7 @@ typedef struct cpHandle{ int retain; // Query stamp. Used to make sure two objects // aren't identified twice in the same query. - int stamp; + cpTimestamp stamp; } cpHandle; // Linked list element for in the chains. @@ -44,25 +44,26 @@ typedef cpBB (*cpSpaceHashBBFunc)(void *obj); typedef struct cpSpaceHash{ // Number of cells in the table. - int numcells; + CP_PRIVATE(int numcells); // Dimentions of the cells. - cpFloat celldim; + CP_PRIVATE(cpFloat celldim); // BBox callback. - cpSpaceHashBBFunc bbfunc; + CP_PRIVATE(cpSpaceHashBBFunc bbfunc); // Hashset of the handles and the recycled ones. - cpHashSet *handleSet; - cpArray *pooledHandles; + CP_PRIVATE(cpHashSet *handleSet); + CP_PRIVATE(cpArray *pooledHandles); // The table and the recycled bins. - cpSpaceHashBin **table, *pooledBins; + CP_PRIVATE(cpSpaceHashBin **table); + CP_PRIVATE(cpSpaceHashBin *pooledBins); // list of buffers to free on destruction. - cpArray *allocatedBuffers; + CP_PRIVATE(cpArray *allocatedBuffers); // Incremented on each query. See cpHandle.stamp. - int stamp; + CP_PRIVATE(cpTimestamp stamp); } cpSpaceHash; //Basic allocation/destruction functions. @@ -77,7 +78,7 @@ void cpSpaceHashFree(cpSpaceHash *hash); void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); // Add an object to the hash. -void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB bb); +void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB _deprecated_ignored); // Remove an object from the hash. void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id); diff --git a/chipmunk/include/chipmunk/cpVect.h b/chipmunk/include/chipmunk/cpVect.h index 995205f4f1..261cec522c 100644 --- a/chipmunk/include/chipmunk/cpVect.h +++ b/chipmunk/include/chipmunk/cpVect.h @@ -19,8 +19,10 @@ * SOFTWARE. */ -static const cpVect cpvzero={0.0f,0.0f}; +/// Constant for the zero vector. +static const cpVect cpvzero = {0.0f,0.0f}; +/// Convenience constructor for cpVect structs. static inline cpVect cpv(const cpFloat x, const cpFloat y) { @@ -29,128 +31,176 @@ cpv(const cpFloat x, const cpFloat y) } // non-inlined functions -cpFloat cpvlength(const cpVect v); -cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t); -cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a); -cpVect cpvforangle(const cpFloat a); // convert radians to a normalized vector -cpFloat cpvtoangle(const cpVect v); // convert a vector to radians -char *cpvstr(const cpVect v); // get a string representation of a vector +/// Returns the length of v. +cpFloat cpvlength(const cpVect v); + +/// Spherical linearly interpolate between v1 and v2. +cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t); + +/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians +cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a); + +/// Returns the unit length vector for the given angle (in radians). +cpVect cpvforangle(const cpFloat a); + +/// Returns the angular direction v is pointing in (in radians). +cpFloat cpvtoangle(const cpVect v); + +/** + Returns a string representation of v. Intended mostly for debugging purposes and not production use. + + @attention The string points to a static local and is reset every time the function is called. + If you want to print more than one vector you will have to split up your printing onto separate lines. +*/ +char *cpvstr(const cpVect v); + +/// Check if two vectors are equal. (Be careful when comparing floating point numbers!) +static inline cpBool +cpveql(const cpVect v1, const cpVect v2) +{ + return (v1.x == v2.x && v1.y == v2.y); +} + +/// Add two vectors static inline cpVect cpvadd(const cpVect v1, const cpVect v2) { return cpv(v1.x + v2.x, v1.y + v2.y); } +/// Negate a vector. static inline cpVect cpvneg(const cpVect v) { return cpv(-v.x, -v.y); } +/// Subtract two vectors. static inline cpVect cpvsub(const cpVect v1, const cpVect v2) { return cpv(v1.x - v2.x, v1.y - v2.y); } +/// Scalar multiplication. static inline cpVect cpvmult(const cpVect v, const cpFloat s) { return cpv(v.x*s, v.y*s); } +/// Vector dot product. static inline cpFloat cpvdot(const cpVect v1, const cpVect v2) { return v1.x*v2.x + v1.y*v2.y; } +/** + 2D vector cross product analog. + The cross product of 2D vectors results in a 3D vector with only a z component. + This function returns the magnitude of the z value. +*/ static inline cpFloat cpvcross(const cpVect v1, const cpVect v2) { return v1.x*v2.y - v1.y*v2.x; } +/// Returns a perpendicular vector. (90 degree rotation) static inline cpVect cpvperp(const cpVect v) { return cpv(-v.y, v.x); } +/// Returns a perpendicular vector. (-90 degree rotation) static inline cpVect cpvrperp(const cpVect v) { return cpv(v.y, -v.x); } +/// Returns the vector projection of v1 onto v2. static inline cpVect cpvproject(const cpVect v1, const cpVect v2) { return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); } +/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector. static inline cpVect cpvrotate(const cpVect v1, const cpVect v2) { return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); } +/// Inverse of cpvrotate(). static inline cpVect cpvunrotate(const cpVect v1, const cpVect v2) { return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); } +/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths. static inline cpFloat cpvlengthsq(const cpVect v) { return cpvdot(v, v); } +/// Linearly interpolate between v1 and v2. static inline cpVect cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) { return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); } +/// Returns a normalized copy of v. static inline cpVect cpvnormalize(const cpVect v) { return cpvmult(v, 1.0f/cpvlength(v)); } +/// Returns a normalized copy of v or cpvzero if v was already cpvzero. Protects against divide by zero errors. static inline cpVect cpvnormalize_safe(const cpVect v) { return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v)); } +/// Clamp v to length len. static inline cpVect cpvclamp(const cpVect v, const cpFloat len) { return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; } +/// Linearly interpolate between v1 towards v2 by distance d. static inline cpVect cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) { return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); } +/// Returns the distance between v1 and v2. static inline 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 inline cpFloat cpvdistsq(const cpVect v1, const cpVect v2) { return cpvlengthsq(cpvsub(v1, v2)); } -static inline int +/// Returns true if the distance between v1 and v2 is less than dist. +static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) { return cpvdistsq(v1, v2) < dist*dist; diff --git a/chipmunk/proj.wophone/Makefile.ARM b/chipmunk/proj.wophone/Makefile.ARM index 38f4fb6cbd..68ec5cf6d8 100644 --- a/chipmunk/proj.wophone/Makefile.ARM +++ b/chipmunk/proj.wophone/Makefile.ARM @@ -38,8 +38,11 @@ OBJECTS = \ $(OBJECTS_DIR)/cpHashSet.o \ $(OBJECTS_DIR)/cpPolyShape.o \ $(OBJECTS_DIR)/cpShape.o \ - $(OBJECTS_DIR)/cpSpace.o \ - $(OBJECTS_DIR)/cpSpaceHash.o \ + $(OBJECTS_DIR)/cpSpace.o \ + $(OBJECTS_DIR)/cpSpaceComponent.o \ + $(OBJECTS_DIR)/cpSpaceHash.o \ + $(OBJECTS_DIR)/cpSpaceQuery.o \ + $(OBJECTS_DIR)/cpSpaceStep.o \ $(OBJECTS_DIR)/cpVect.o \ $(OBJECTS_DIR)/cpConstraint.o \ $(OBJECTS_DIR)/cpDampedRotarySpring.o \ @@ -99,9 +102,18 @@ $(OBJECTS_DIR)/cpShape.o : ../src/cpShape.c $(OBJECTS_DIR)/cpSpace.o : ../src/cpSpace.c $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpace.c + +$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceComponent.c + $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceComponent.c $(OBJECTS_DIR)/cpSpaceHash.o : ../src/cpSpaceHash.c $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpaceHash.o ../src/cpSpaceHash.c + +$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceQuery.c + $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceQuery.c + +$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceStep.c + $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceStep.c $(OBJECTS_DIR)/cpVect.o : ../src/cpVect.c $(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpVect.o ../src/cpVect.c diff --git a/chipmunk/src/CMakeLists.txt b/chipmunk/src/CMakeLists.txt index 26753fdd06..2b3cbc8ba0 100644 --- a/chipmunk/src/CMakeLists.txt +++ b/chipmunk/src/CMakeLists.txt @@ -9,7 +9,7 @@ if(BUILD_SHARED) ${chipmunk_source_files} ) # set the lib's version number - set_target_properties(chipmunk PROPERTIES VERSION 5.1) + set_target_properties(chipmunk PROPERTIES VERSION 5.3.4) install(TARGETS chipmunk RUNTIME DESTINATION lib LIBRARY DESTINATION lib) endif(BUILD_SHARED) diff --git a/chipmunk/src/chipmunk.c b/chipmunk/src/chipmunk.c index bc07b9d930..1862e78288 100644 --- a/chipmunk/src/chipmunk.c +++ b/chipmunk/src/chipmunk.c @@ -18,9 +18,11 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ - + #include #include +#define _USE_MATH_DEFINES +#include #include "chipmunk.h" @@ -43,7 +45,7 @@ cpMessage(const char *message, const char *condition, const char *file, int line } -char *cpVersionString = "5.x.x"; +const char *cpVersionString = "5.3.4"; void cpInitChipmunk(void) @@ -59,7 +61,13 @@ cpInitChipmunk(void) cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) { - return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset); + return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset)); +} + +cpFloat +cpAreaForCircle(cpFloat r1, cpFloat r2) +{ + return 2.0f*(cpFloat)M_PI*cpfabs(r1*r1 - r2*r2); } cpFloat @@ -68,21 +76,23 @@ 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); + return m*(length*length/12.0f + cpvlengthsq(offset)); } cpFloat -cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset) +cpAreaForSegment(cpVect a, cpVect b, cpFloat r) +{ + return 2.0f*r*((cpFloat)M_PI*r + cpvdist(a, b)); +} + +cpFloat +cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset) { - cpVect *tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect)); - for(int i=0; i -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" // TODO: Comment me! diff --git a/chipmunk/src/constraints/cpDampedRotarySpring.c b/chipmunk/src/constraints/cpDampedRotarySpring.c index 637a6ce2d6..499103dbaf 100644 --- a/chipmunk/src/constraints/cpDampedRotarySpring.c +++ b/chipmunk/src/constraints/cpDampedRotarySpring.c @@ -22,7 +22,7 @@ #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static cpFloat @@ -33,12 +33,12 @@ defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ static void preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) { - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; + CONSTRAINT_BEGIN(spring, a, b); - spring->iSum = 1.0f/(a->i_inv + b->i_inv); + cpFloat moment = a->i_inv + b->i_inv; + spring->iSum = 1.0f/moment; - spring->dt = dt; + spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); spring->target_wrn = 0.0f; // apply spring torque @@ -50,15 +50,14 @@ preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpDampedRotarySpring *spring) { - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; + CONSTRAINT_BEGIN(spring, a, b); // compute relative velocity cpFloat wrn = a->w - b->w;//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 w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum)); + cpFloat w_damp = wrn*spring->w_coef; spring->target_wrn = wrn - w_damp; //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); diff --git a/chipmunk/src/constraints/cpDampedSpring.c b/chipmunk/src/constraints/cpDampedSpring.c index b873d9f675..406af3af63 100644 --- a/chipmunk/src/constraints/cpDampedSpring.c +++ b/chipmunk/src/constraints/cpDampedSpring.c @@ -22,7 +22,7 @@ #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static cpFloat @@ -33,8 +33,7 @@ defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ static void preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) { - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; + CONSTRAINT_BEGIN(spring, a, b); spring->r1 = cpvrotate(spring->anchr1, a->rot); spring->r2 = cpvrotate(spring->anchr2, b->rot); @@ -43,11 +42,11 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) cpFloat dist = cpvlength(delta); spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); - // calculate mass normal - spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n); - - spring->dt = dt; + 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((cpConstraint *)spring, dist); @@ -57,8 +56,7 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpDampedSpring *spring) { - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; + CONSTRAINT_BEGIN(spring, a, b); cpVect n = spring->n; cpVect r1 = spring->r1; @@ -69,7 +67,7 @@ applyImpulse(cpDampedSpring *spring) // compute velocity loss from drag // not 100% certain this is derived correctly, though it makes sense - cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass)); + 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)); diff --git a/chipmunk/src/constraints/cpGearJoint.c b/chipmunk/src/constraints/cpGearJoint.c index f9235e2ff0..dd694d35b9 100644 --- a/chipmunk/src/constraints/cpGearJoint.c +++ b/chipmunk/src/constraints/cpGearJoint.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); @@ -49,8 +48,7 @@ preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpGearJoint *joint) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // compute relative rotational velocity cpFloat wr = b->w*joint->ratio - a->w; @@ -111,4 +109,5 @@ cpGearJointSetRatio(cpConstraint *constraint, cpFloat value) cpConstraintCheckCast(constraint, cpGearJoint); ((cpGearJoint *)constraint)->ratio = value; ((cpGearJoint *)constraint)->ratio_inv = 1.0f/value; + cpConstraintActivateBodies(constraint); } diff --git a/chipmunk/src/constraints/cpGrooveJoint.c b/chipmunk/src/constraints/cpGrooveJoint.c index 71d1309d41..4a067ef929 100644 --- a/chipmunk/src/constraints/cpGrooveJoint.c +++ b/chipmunk/src/constraints/cpGrooveJoint.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // calculate endpoints in worldspace cpVect ta = cpBodyLocal2World(a, joint->grv_a); @@ -79,8 +78,7 @@ grooveConstrain(cpGrooveJoint *joint, cpVect j){ static void applyImpulse(cpGrooveJoint *joint) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpVect r1 = joint->r1; cpVect r2 = joint->r2; @@ -136,3 +134,28 @@ cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect { return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); } + +void +cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) +{ + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + cpConstraintCheckCast(constraint, cpGrooveJoint); + + g->grv_a = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value))); + + cpConstraintActivateBodies(constraint); +} + +void +cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) +{ + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + cpConstraintCheckCast(constraint, cpGrooveJoint); + + g->grv_b = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a))); + + cpConstraintActivateBodies(constraint); +} + diff --git a/chipmunk/src/constraints/cpPinJoint.c b/chipmunk/src/constraints/cpPinJoint.c index 96c6b8c279..3955143ff5 100644 --- a/chipmunk/src/constraints/cpPinJoint.c +++ b/chipmunk/src/constraints/cpPinJoint.c @@ -22,14 +22,13 @@ #include //#include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -56,8 +55,7 @@ preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpPinJoint *joint) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpVect n = joint->n; // compute relative velocity @@ -101,8 +99,9 @@ cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect an joint->anchr1 = anchr1; joint->anchr2 = anchr2; - cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot)); - cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot)); + // 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; diff --git a/chipmunk/src/constraints/cpPivotJoint.c b/chipmunk/src/constraints/cpPivotJoint.c index 60db726ba9..634bd02336 100644 --- a/chipmunk/src/constraints/cpPivotJoint.c +++ b/chipmunk/src/constraints/cpPivotJoint.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -50,8 +49,7 @@ preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpPivotJoint *joint) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpVect r1 = joint->r1; cpVect r2 = joint->r2; @@ -110,5 +108,7 @@ cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) cpConstraint * cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) { - return cpPivotJointNew2(a, b, cpBodyWorld2Local(a, pivot), cpBodyWorld2Local(b, pivot)); + cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot); + cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot); + return cpPivotJointNew2(a, b, anchr1, anchr2); } diff --git a/chipmunk/src/constraints/cpRatchetJoint.c b/chipmunk/src/constraints/cpRatchetJoint.c index b49e44ea4b..a7103c7f01 100644 --- a/chipmunk/src/constraints/cpRatchetJoint.c +++ b/chipmunk/src/constraints/cpRatchetJoint.c @@ -19,18 +19,16 @@ * SOFTWARE. */ -#include #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpFloat angle = joint->angle; cpFloat phase = joint->phase; @@ -70,8 +68,7 @@ applyImpulse(cpRatchetJoint *joint) { if(!joint->bias) return; // early exit - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // compute relative rotational velocity cpFloat wr = b->w - a->w; @@ -116,7 +113,8 @@ cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, c joint->phase = phase; joint->ratchet = ratchet; - joint->angle = b->a - a->a; + // STATIC_BODY_CHECK + joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f); return joint; } diff --git a/chipmunk/src/constraints/cpRotaryLimitJoint.c b/chipmunk/src/constraints/cpRotaryLimitJoint.c index 731335112c..9bfa1ea708 100644 --- a/chipmunk/src/constraints/cpRotaryLimitJoint.c +++ b/chipmunk/src/constraints/cpRotaryLimitJoint.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpFloat dist = b->a - a->a; cpFloat pdist = 0.0f; @@ -62,8 +61,7 @@ applyImpulse(cpRotaryLimitJoint *joint) { if(!joint->bias) return; // early exit - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // compute relative rotational velocity cpFloat wr = b->w - a->w; diff --git a/chipmunk/src/constraints/cpSimpleMotor.c b/chipmunk/src/constraints/cpSimpleMotor.c index 55e2b0d974..c4f771e441 100644 --- a/chipmunk/src/constraints/cpSimpleMotor.c +++ b/chipmunk/src/constraints/cpSimpleMotor.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); @@ -44,8 +43,7 @@ preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpSimpleMotor *joint) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); // compute relative rotational velocity cpFloat wr = b->w - a->w + joint->rate; diff --git a/chipmunk/src/constraints/cpSlideJoint.c b/chipmunk/src/constraints/cpSlideJoint.c index 27e4aeec1e..79c627c7f7 100644 --- a/chipmunk/src/constraints/cpSlideJoint.c +++ b/chipmunk/src/constraints/cpSlideJoint.c @@ -21,14 +21,13 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" static void preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) { - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -69,8 +68,7 @@ applyImpulse(cpSlideJoint *joint) { if(!joint->bias) return; // early exit - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; + CONSTRAINT_BEGIN(joint, a, b); cpVect n = joint->n; cpVect r1 = joint->r1; diff --git a/chipmunk/src/cpArbiter.c b/chipmunk/src/cpArbiter.c index 6279eeee62..9e023a7a96 100644 --- a/chipmunk/src/cpArbiter.c +++ b/chipmunk/src/cpArbiter.c @@ -21,7 +21,7 @@ #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "constraints/util.h" cpFloat cp_bias_coef = 0.1f; @@ -104,13 +104,20 @@ cpArbiterAlloc(void) cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) { + arb->handler = NULL; + arb->swappedColl = cpFalse; + + arb->e = 0.0f; + arb->u = 0.0f; + arb->surface_vr = cpvzero; + arb->numContacts = 0; arb->contacts = NULL; - arb->private_a = a; - arb->private_b = b; + arb->a = a; + arb->b = b; - arb->stamp = -1; + arb->stamp = 0; arb->state = cpArbiterStateFirstColl; return arb; @@ -157,8 +164,6 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio } } } - -// cpfree(arb->contacts); } arb->contacts = contacts; @@ -172,17 +177,18 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio arb->surface_vr = cpvsub(a->surface_v, b->surface_v); // For collisions between two similar primitive types, the order could have been swapped. - arb->private_a = a; arb->private_b = b; + arb->a = a; + arb->b = b; + + // mark it as new if it's been cached + if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl; } void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) { - cpShape *shapea = arb->private_a; - cpShape *shapeb = arb->private_b; - - cpBody *a = shapea->body; - cpBody *b = shapeb->body; + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; for(int i=0; inumContacts; i++){ cpContact *con = &arb->contacts[i]; @@ -207,8 +213,8 @@ cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) void cpArbiterApplyCachedImpulse(cpArbiter *arb) { - cpShape *shapea = arb->private_a; - cpShape *shapeb = arb->private_b; + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; arb->u = shapea->u * shapeb->u; arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v); @@ -225,8 +231,8 @@ cpArbiterApplyCachedImpulse(cpArbiter *arb) void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef) { - cpBody *a = arb->private_a->body; - cpBody *b = arb->private_b->body; + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; for(int i=0; inumContacts; i++){ cpContact *con = &arb->contacts[i]; diff --git a/chipmunk/src/cpArray.c b/chipmunk/src/cpArray.c index 6cc73efaa2..98dbb5e4fd 100644 --- a/chipmunk/src/cpArray.c +++ b/chipmunk/src/cpArray.c @@ -22,7 +22,7 @@ #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" //#define CP_ARRAY_INCREMENT 10 @@ -57,6 +57,7 @@ void cpArrayDestroy(cpArray *arr) { cpfree(arr->arr); + arr->arr = NULL; } void @@ -111,6 +112,20 @@ cpArrayDeleteObj(cpArray *arr, void *obj) } } +void +cpArrayAppend(cpArray *arr, cpArray *other) +{ + void *tail = &arr->arr[arr->num]; + + arr->num += other->num; + if(arr->num >= arr->max){ + arr->max = arr->num; + arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**)); + } + + memcpy(tail, other->arr, other->num*sizeof(void**)); +} + void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) { @@ -118,11 +133,11 @@ cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) iterFunc(arr->arr[i], data); } -int +cpBool cpArrayContains(cpArray *arr, void *ptr) { for(int i=0; inum; i++) - if(arr->arr[i] == ptr) return 1; + if(arr->arr[i] == ptr) return cpTrue; - return 0; + return cpFalse; } diff --git a/chipmunk/src/cpBody.c b/chipmunk/src/cpBody.c index 0347f55b58..a242181125 100644 --- a/chipmunk/src/cpBody.c +++ b/chipmunk/src/cpBody.c @@ -21,8 +21,12 @@ #include #include +#include -#include "chipmunk.h" +#include "chipmunk_private.h" + +// initialized in cpInitChipmunk() +cpBody cpStaticBodySingleton; cpBody* cpBodyAlloc(void) @@ -56,8 +60,13 @@ cpBodyInit(cpBody *body, cpFloat m, cpFloat i) body->data = NULL; body->v_limit = (cpFloat)INFINITY; body->w_limit = (cpFloat)INFINITY; -// body->active = 1; - + + body->space = NULL; + body->shapesList = NULL; + + cpComponentNode node = {NULL, NULL, 0, 0.0f}; + body->node = node; + return body; } @@ -67,6 +76,21 @@ cpBodyNew(cpFloat m, cpFloat i) return cpBodyInit(cpBodyAlloc(), m, i); } +cpBody * +cpBodyInitStatic(cpBody *body) +{ + cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY); + body->node.idleTime = (cpFloat)INFINITY; + + return body; +} + +cpBody * +cpBodyNewStatic() +{ + return cpBodyInitStatic(cpBodyAlloc()); +} + void cpBodyDestroy(cpBody *body){} void @@ -166,23 +190,3 @@ cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat cpBodyApplyForce(a, f, r1); cpBodyApplyForce(b, cpvneg(f), r2); } - -//int -//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max) -//{ -// cpFloat ke = body->m*cpvdot(body->v, body->v); -// cpFloat re = body->i*body->w*body->w; -// -// if(ke + re > body->m*dvsq) -// body->active = 1; -// else if(body->active) -// body->active = (body->active + 1)%(max + 1); -// else { -// body->v = cpvzero; -// body->v_bias = cpvzero; -// body->w = 0.0f; -// body->w_bias = 0.0f; -// } -// -// return body->active; -//} diff --git a/chipmunk/src/cpCollision.c b/chipmunk/src/cpCollision.c index b1c1343e0c..a67ba25e42 100644 --- a/chipmunk/src/cpCollision.c +++ b/chipmunk/src/cpCollision.c @@ -21,16 +21,16 @@ #include #include -#include +//#include -#include "chipmunk.h" +#include "chipmunk_private.h" -typedef int (*collisionFunc)(cpShape *, cpShape *, cpContact *); +typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *); // Add contact points for circle to circle collisions. // Used by several collision tests. static int -circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con) +circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con) { cpFloat mindist = r1 + r2; cpVect delta = cpvsub(p2, p1); @@ -38,14 +38,12 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con) if(distsq >= mindist*mindist) return 0; cpFloat dist = cpfsqrt(distsq); - // To avoid singularities, do nothing in the case of dist = 0. - cpFloat non_zero_dist = (dist ? dist : INFINITY); // Allocate and initialize the contact. cpContactInit( con, - cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)), - cpvmult(delta, 1.0f/non_zero_dist), + 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 ); @@ -55,7 +53,7 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con) // Collide circle shapes. static int -circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr) +circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { cpCircleShape *circ1 = (cpCircleShape *)shape1; cpCircleShape *circ2 = (cpCircleShape *)shape2; @@ -65,7 +63,7 @@ circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr) // Collide circles to segment shapes. static int -circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con) +circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con) { cpCircleShape *circ = (cpCircleShape *)circleShape; cpSegmentShape *seg = (cpSegmentShape *)segmentShape; @@ -118,17 +116,19 @@ circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con) static cpContact * nextContactPoint(cpContact *arr, int *numPtr) { - int num = *numPtr; + int index = *numPtr; - if(num <= CP_MAX_CONTACTS_PER_ARBITER) - (*numPtr) = num + 1; - - return &arr[num]; + if(index < CP_MAX_CONTACTS_PER_ARBITER){ + (*numPtr) = index + 1; + return &arr[index]; + } else { + return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1]; + } } // Find the minimum separating axis for the give poly and axis list. static inline int -findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out) +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); @@ -148,9 +148,11 @@ findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out) return min_index; } -// Add contacts for penetrating vertexes. +// 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 inline int -findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) +findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) { int num = 0; @@ -166,15 +168,33 @@ findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFl cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); } - // if(!num) - // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0)); - return num; } +// Add contacts for penetrating vertexes. +static inline int +findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) +{ + int num = 0; + + for(int i=0; inumVerts; i++){ + cpVect v = poly1->tVerts[i]; + if(cpPolyShapeContainsVert(poly2, v)) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); + } + + for(int i=0; inumVerts; i++){ + cpVect v = poly2->tVerts[i]; + if(cpPolyShapeContainsVert(poly1, v)) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); + } + + return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); +} + // Collide poly shapes together. static int -poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr) +poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { cpPolyShape *poly1 = (cpPolyShape *)shape1; cpPolyShape *poly2 = (cpPolyShape *)shape2; @@ -196,7 +216,7 @@ poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr) // Like cpPolyValueOnAxis(), but for segments. static inline cpFloat -segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d) +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; @@ -205,7 +225,7 @@ segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d) // Identify vertexes that have penetrated the segment. static inline void -findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) +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); @@ -225,7 +245,7 @@ findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape * // This one is complicated and gross. Just don't go there... // TODO: Comment me! static int -seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr) +seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { cpSegmentShape *seg = (cpSegmentShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; @@ -294,7 +314,7 @@ seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr) // This one is less gross, but still gross. // TODO: Comment me! static int -circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con) +circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) { cpCircleShape *circ = (cpCircleShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; @@ -352,7 +372,7 @@ circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con) static collisionFunc *colfuncs = NULL; static void -addColFunc(cpShapeType a, cpShapeType b, collisionFunc func) +addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func) { colfuncs[a + b*CP_NUM_SHAPES] = func; } @@ -381,7 +401,7 @@ extern "C" { #endif int -cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr) +cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr) { // Their shape types must be in order. cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted."); diff --git a/chipmunk/src/cpHashSet.c b/chipmunk/src/cpHashSet.c index fd600fb3e8..c7f3cba605 100644 --- a/chipmunk/src/cpHashSet.c +++ b/chipmunk/src/cpHashSet.c @@ -22,7 +22,7 @@ #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "prime.h" static void freeWrap(void *ptr, void *unused){cpfree(ptr);} diff --git a/chipmunk/src/cpPolyShape.c b/chipmunk/src/cpPolyShape.c index 80b4af4c1c..aa9ea245e6 100644 --- a/chipmunk/src/cpPolyShape.c +++ b/chipmunk/src/cpPolyShape.c @@ -20,9 +20,8 @@ */ #include -#include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "chipmunk_unsafe.h" cpPolyShape * @@ -94,7 +93,7 @@ cpPolyShapeDestroy(cpShape *shape) cpfree(poly->tAxes); } -static int +static cpBool cpPolyShapePointQuery(cpShape *shape, cpVect p){ return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p); } @@ -137,8 +136,8 @@ static const cpShapeClass polyClass = { cpPolyShapeSegmentQuery, }; -int -cpPolyValidate(cpVect *verts, int numVerts) +cpBool +cpPolyValidate(const cpVect *verts, const int numVerts) { for(int i=0; i 0.0f) - return 0; + return cpFalse; } - return 1; + return cpTrue; } int @@ -211,8 +210,8 @@ cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset) cpPolyShape * cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height) { - cpFloat hw = width/2; - cpFloat hh = height/2; + cpFloat hw = width/2.0f; + cpFloat hh = height/2.0f; cpVect verts[] = { cpv(-hw,-hh), diff --git a/chipmunk/src/cpShape.c b/chipmunk/src/cpShape.c index 5353c6f66b..a190891920 100644 --- a/chipmunk/src/cpShape.c +++ b/chipmunk/src/cpShape.c @@ -23,7 +23,7 @@ #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "chipmunk_unsafe.h" #define CP_DefineShapeGetter(struct, type, member, name) \ @@ -60,8 +60,9 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) shape->layers = CP_ALL_LAYERS; shape->data = NULL; + shape->next = NULL; - cpShapeCacheBB(shape); +// cpShapeCacheBB(shape); return shape; } @@ -81,6 +82,7 @@ cpShapeFree(cpShape *shape) } } +// TODO this function should really take a position and rotation explicitly and be renamed cpBB cpShapeCacheBB(cpShape *shape) { @@ -90,12 +92,12 @@ cpShapeCacheBB(cpShape *shape) return shape->bb; } -int +cpBool cpShapePointQuery(cpShape *shape, cpVect p){ return shape->klass->pointQuery(shape, p); } -int +cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){ cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero}; (*info) = blank; @@ -138,7 +140,7 @@ cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) return bbFromCircle(circle->tc, circle->r); } -static int +static cpBool cpCircleShapePointQuery(cpShape *shape, cpVect p){ cpCircleShape *circle = (cpCircleShape *)shape; return cpvnear(circle->tc, p, circle->r); @@ -147,7 +149,7 @@ cpCircleShapePointQuery(cpShape *shape, cpVect p){ static void circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info) { - // umm... gross I normally frown upon such things + // offset the line to be relative to the circle a = cpvsub(a, center); b = cpvsub(b, center); @@ -239,16 +241,16 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) return cpBBNew(l - rad, s - rad, r + rad, t + rad); } -static int +static cpBool cpSegmentShapePointQuery(cpShape *shape, cpVect p){ - if(!cpBBcontainsVect(shape->bb, p)) return 0; + if(!cpBBcontainsVect(shape->bb, p)) return cpFalse; cpSegmentShape *seg = (cpSegmentShape *)shape; // Calculate normal distance from segment. cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); cpFloat dist = cpfabs(dn) - seg->r; - if(dist > 0.0f) return 0; + if(dist > 0.0f) return cpFalse; // Calculate tangential distance along segment. cpFloat dt = -cpvcross(seg->tn, p); @@ -258,28 +260,32 @@ cpSegmentShapePointQuery(cpShape *shape, cpVect p){ // Decision tree to decide which feature of the segment to collide with. if(dt <= dtMin){ if(dt < (dtMin - seg->r)){ - return 0; + return cpFalse; } else { return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); } } else { if(dt < dtMax){ - return 1; + return cpTrue; } else { if(dt < (dtMax + seg->r)) { return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); } else { - return 0; + return cpFalse; } } } - return 1; + return cpTrue; } +static inline 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 = (cpSegmentShape *)shape; cpVect n = seg->tn; // flip n if a is behind the axis @@ -288,40 +294,37 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf cpFloat an = cpvdot(a, n); cpFloat bn = cpvdot(b, n); - 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(an != bn){ + cpFloat d = cpvdot(seg->ta, n) + seg->r; + cpFloat t = (d - an)/(bn - an); - if(dtMin < dt && dt < dtMax){ - info->shape = shape; - info->t = t; - info->n = n; + 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); - return; // don't continue on and check endcaps + 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; info1.shape = NULL; - cpSegmentQueryInfo info2; info2.shape = NULL; + 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.shape && !info2.shape){ + if(info1.t < info2.t){ (*info) = info1; - } else if(info2.shape && !info1.shape){ + } else { (*info) = info2; - } else if(info1.shape && info2.shape){ - if(info1.t < info2.t){ - (*info) = info1; - } else { - (*info) = info2; - } } } } diff --git a/chipmunk/src/cpSpace.c b/chipmunk/src/cpSpace.c index 88461a1106..8dc3b60ec4 100644 --- a/chipmunk/src/cpSpace.c +++ b/chipmunk/src/cpSpace.c @@ -20,24 +20,24 @@ */ #include -#include +//#include #include #include -#include "chipmunk.h" +#include "chipmunk_private.h" -int cp_contact_persistence = 1; +cpTimestamp cp_contact_persistence = 3; #pragma mark Contact Set Helpers // Equal function for contactSet. -static int +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)); + return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); } // Transformation function for contactSet. @@ -55,13 +55,13 @@ contactSetTrans(cpShape **shapes, cpSpace *space) for(int i=0; ipooledArbiters, buffer + i); } - return cpArbiterInit( (cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); + return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); } #pragma mark Collision Pair Function Helpers // Equals function for collFuncSet. -static int +static cpBool collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) { return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b)); @@ -77,32 +77,10 @@ collFuncSetTrans(cpCollisionHandler *handler, void *unused) return copy; } -#pragma mark Post Step Function Helpers - -typedef struct postStepCallback { - cpPostStepFunc func; - void *obj; - void *data; -} postStepCallback; - -static int -postStepFuncSetEql(postStepCallback *a, postStepCallback *b){ - return a->obj == b->obj; -} - -static void * -postStepFuncSetTrans(postStepCallback *callback, void *ignored) -{ - postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback)); - (*value) = (*callback); - - return value; -} - #pragma mark Misc Helper Funcs // Default collision functions. -static int alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} +static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} static void nothing(cpArbiter *arb, cpSpace *space, void *data){} // BBfunc callback for the spatial hash. @@ -116,30 +94,6 @@ static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFr #pragma mark Memory Management Functions -#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact)) -typedef struct cpContactBuffer { - cpContactBufferHeader header; - cpContact contacts[CP_CONTACTS_BUFFER_SIZE]; -} cpContactBuffer; - -static cpContactBufferHeader * -cpSpaceAllocContactBuffer(cpSpace *space) -{ - cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer)); - cpArrayPush(space->allocatedBuffers, buffer); - return (cpContactBufferHeader *)buffer; -} - -static cpContactBufferHeader * -cpContactBufferHeaderInit(cpContactBufferHeader *header, cpSpace *space) -{ - header->stamp = space->stamp; - header->next = space->contactBuffersTail; - header->numContacts = 0; - - return header; -} - cpSpace * cpSpaceAlloc(void) { @@ -151,8 +105,6 @@ cpSpaceAlloc(void) #define DEFAULT_ITERATIONS 10 #define DEFAULT_ELASTIC_ITERATIONS 0 -#define MAX_CONTACTS 10000 - cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL}; cpSpace* @@ -174,14 +126,16 @@ cpSpaceInit(cpSpace *space) space->allocatedBuffers = cpArrayNew(0); space->bodies = cpArrayNew(0); + space->sleepingComponents = cpArrayNew(0); + space->rousedBodies = cpArrayNew(0); + + space->sleepTimeThreshold = INFINITY; + space->idleSpeedThreshold = 0.0f; + space->arbiters = cpArrayNew(0); space->pooledArbiters = cpArrayNew(0); - cpContactBufferHeader *header = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), space); - space->contactBuffersHead = header; - space->contactBuffersTail = header; - header->next = header; // Buffers will form a ring, start the ring explicitly - + space->contactBuffersHead = NULL; space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans); space->constraints = cpArrayNew(0); @@ -190,7 +144,9 @@ cpSpaceInit(cpSpace *space) space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans); space->collFuncSet->default_value = &space->defaultHandler; - space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans); + space->postStepCallbacks = NULL; + + cpBodyInitStatic(&space->staticBody); return space; } @@ -208,6 +164,8 @@ cpSpaceDestroy(cpSpace *space) cpSpaceHashFree(space->activeShapes); cpArrayFree(space->bodies); + cpArrayFree(space->sleepingComponents); + cpArrayFree(space->rousedBodies); cpArrayFree(space->constraints); @@ -244,6 +202,9 @@ cpSpaceFree(cpSpace *space) void cpSpaceFreeChildren(cpSpace *space) { + cpArray *components = space->sleepingComponents; + while(components->num) cpBodyActivate((cpBody *)components->arr[0]); + cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL); cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL); cpArrayEach(space->bodies, (cpArrayIter)&bodyFreeWrap, NULL); @@ -310,31 +271,62 @@ cpSpaceSetDefaultCollisionHandler( #define cpAssertSpaceUnlocked(space) \ cpAssert(!space->locked, \ - "This addition/removal cannot be done safely during a call to cpSpaceStep(). " \ - "Put these calls into a Post Step Callback." \ + "This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \ + "Put these calls into a post-step callback." \ ); +static void +cpBodyAddShape(cpBody *body, cpShape *shape) +{ + shape->next = shape->body->shapesList; + shape->body->shapesList = shape; +} + +static void +cpBodyRemoveShape(cpBody *body, cpShape *shape) +{ + cpShape **prev_ptr = &body->shapesList; + cpShape *node = body->shapesList; + + while(node && node != shape){ + prev_ptr = &node->next; + node = node->next; + } + + cpAssert(node, "Attempted to remove a shape from a body it was never attached to."); + (*prev_ptr) = node->next; +} + cpShape * cpSpaceAddShape(cpSpace *space, cpShape *shape) { - cpAssert(shape->body, "Cannot add a shape with a NULL body."); + cpBody *body = shape->body; + if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); + cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), "Cannot add the same shape more than once."); cpAssertSpaceUnlocked(space); + cpBodyActivate(body); + cpBodyAddShape(body, shape); + + cpShapeCacheBB(shape); cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb); + return shape; } cpShape * cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) { - cpAssert(shape->body, "Cannot add a static shape with a NULL body."); cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), "Cannot add the same static shape more than once."); cpAssertSpaceUnlocked(space); + if(!shape->body) shape->body = &space->staticBody; + cpShapeCacheBB(shape); + cpSpaceActivateShapesTouchingShape(space, shape); cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb); return shape; @@ -343,10 +335,12 @@ cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { - cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once."); + cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); + cpAssert(!body->space, "Cannot add a body to a more than one space or to the same space twice."); // cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback cpArrayPush(space->bodies, body); + body->space = space; return body; } @@ -357,6 +351,11 @@ cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once."); // cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback. + if(!constraint->a) constraint->a = &space->staticBody; + if(!constraint->b) constraint->b = &space->staticBody; + + cpBodyActivate(constraint->a); + cpBodyActivate(constraint->b); cpArrayPush(space->constraints, constraint); return constraint; @@ -368,24 +367,37 @@ typedef struct removalContext { } removalContext; // Hashset filter func to throw away old arbiters. -static int +static cpBool contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context) { - if(context->shape == arb->private_a || context->shape == arb->private_b){ - arb->handler->separate(arb, context->space, arb->handler->data); + if(context->shape == arb->a || context->shape == arb->b){ + if(arb->state != cpArbiterStateCached){ + arb->handler->separate(arb, context->space, arb->handler->data); + } + cpArrayPush(context->space->pooledArbiters, arb); - return 0; + return cpFalse; } - return 1; + return cpTrue; } void cpSpaceRemoveShape(cpSpace *space, cpShape *shape) { - cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), - "Cannot remove a shape that was never added to the space. (Removed twice maybe?)"); + cpBody *body = shape->body; + if(cpBodyIsStatic(body)){ + cpSpaceRemoveStaticShape(space, shape); + return; + } + + cpBodyActivate(body); + cpAssertSpaceUnlocked(space); + cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), + "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); + + cpBodyRemoveShape(body, shape); removalContext context = {space, shape}; cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context); @@ -396,238 +408,43 @@ void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) { cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), - "Cannot remove a static shape that was never added to the space. (Removed twice maybe?)"); + "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); removalContext context = {space, shape}; cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context); cpSpaceHashRemove(space->staticShapes, shape, shape->hashid); + + cpSpaceActivateShapesTouchingShape(space, shape); } void cpSpaceRemoveBody(cpSpace *space, cpBody *body) { - cpAssertWarn(cpArrayContains(space->bodies, body), - "Cannot remove a body that was never added to the space. (Removed twice maybe?)"); + cpAssertWarn(body->space == space, + "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); + cpBodyActivate(body); cpArrayDeleteObj(space->bodies, body); + body->space = NULL; } void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) { cpAssertWarn(cpArrayContains(space->constraints, constraint), - "Cannot remove a constraint that was never added to the space. (Removed twice maybe?)"); + "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); // cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback. + cpBodyActivate(constraint->a); + cpBodyActivate(constraint->b); cpArrayDeleteObj(space->constraints, constraint); } -#pragma mark Post Step Functions - -void -cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data) -{ - postStepCallback callback = {func, obj, data}; - cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL); -} - -static void -removeAndFreeShapeAndBody(cpShape *shape, cpSpace *space) -{ - cpSpaceRemoveShape(space, shape); - cpShapeFree(shape); -} - -void -cpSpacePostStepRemoveAndFreeShapeAndBody(cpSpace *space, cpShape *shape) -{ - cpSpaceAddPostStepCallback(space, (cpPostStepFunc)removeAndFreeShapeAndBody, shape, space); -} - -#pragma mark Point Query Functions - -typedef struct pointQueryContext { - cpLayers layers; - cpGroup group; - cpSpacePointQueryFunc func; - void *data; -} pointQueryContext; - -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, (cpSpaceHashQueryFunc)pointQueryHelper, &context); - cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context); -} - -static void -rememberLastPointQuery(cpShape *shape, cpShape **outShape) -{ - (*outShape) = shape; -} - -cpShape * -cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) -{ - cpShape *shape = NULL; - cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape); - - return shape; -} - -void -cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) -{ - cpArray *bodies = space->bodies; - - for(int i=0; inum; i++) - func((cpBody *)bodies->arr[i], data); -} - -#pragma mark Segment Query Functions - -typedef struct segQueryContext { - cpVect start, end; - cpLayers layers; - cpGroup group; - cpSpaceSegmentQueryFunc func; - int anyCollision; -} segQueryContext; - -static cpFloat -segQueryFunc(segQueryContext *context, cpShape *shape, void *data) -{ - cpSegmentQueryInfo info; - - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpShapeSegmentQuery(shape, context->start, context->end, &info) - ){ - if(context->func){ - context->func(shape, info.t, info.n, data); - } - - context->anyCollision = 1; - } - - return 1.0f; -} - -int -cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data) -{ - segQueryContext context = { - start, end, - layers, group, - func, - 0, - }; - - cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data); - cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data); - - return context.anyCollision; -} - -typedef struct segQueryFirstContext { - cpVect start, end; - cpLayers layers; - cpGroup group; -} segQueryFirstContext; - -static cpFloat -segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info;// = {NULL, 1.0f, cpvzero}; - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpShapeSegmentQuery(shape, context->start, context->end, &info) - ){ - if(info.t < out->t){ - out->shape = info.shape; - out->t = info.t; - out->n = info.n; - } - - return info.t; - } - - return 1.0f; -} - -cpShape * -cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero}; - if(out){ - (*out) = info; - } else { - out = &info; - } - - out->t = 1.0f; - - segQueryFirstContext context = { - start, end, - layers, group - }; - - cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out); - cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out); - - return out->shape; -} - -#pragma mark BB Query functions - -typedef struct bbQueryContext { - cpLayers layers; - cpGroup group; - cpSpaceBBQueryFunc func; - void *data; -} bbQueryContext; - -static void -bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context) -{ - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpBBintersects(*bb, shape->bb) - ){ - context->func(shape, context->data); - } -} - -void -cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data) -{ - bbQueryContext context = {layers, group, func, data}; - cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context); - cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context); -} - #pragma mark Spatial Hash Management -// Iterator function used for updating shape BBoxes. -static void -updateBBCache(cpShape *shape, void *unused) -{ - cpShapeCacheBB(shape); -} +static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);} void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) @@ -649,251 +466,34 @@ cpSpaceRehashStatic(cpSpace *space) cpSpaceHashRehash(space->staticShapes); } -#pragma mark Collision Detection Functions - -static cpContactBufferHeader * -cpSpaceGetFreeContactBuffer(cpSpace *space) +void +cpSpaceRehashShape(cpSpace *space, cpShape *shape) { - if(space->stamp - space->contactBuffersTail->stamp > cp_contact_persistence){ - cpContactBufferHeader *header = space->contactBuffersTail; - space->contactBuffersTail = header->next; - - return cpContactBufferHeaderInit(header, space); - } else { - cpContactBufferHeader *header = cpSpaceAllocContactBuffer(space); - return cpContactBufferHeaderInit(header, space); - } + cpShapeCacheBB(shape); + + // attempt to rehash the shape in both hashes + cpSpaceHashRehashObject(space->activeShapes, shape, shape->hashid); + cpSpaceHashRehashObject(space->staticShapes, shape, shape->hashid); } -static void -cpSpacePushNewContactBuffer(cpSpace *space) -{ -// for(cpContactBuffer *buffer = space->contactBuffersTail; buffer != space->contactBuffersHead; buffer = buffer->next){ -// printf("%p -> ", buffer); -// } -// printf("%p (head)\n", space->contactBuffersHead); - - cpContactBufferHeader *buffer = cpSpaceGetFreeContactBuffer(space); - space->contactBuffersHead->next = buffer; - space->contactBuffersHead = buffer; -} - -static inline int -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 && b->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{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type}; - cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type); - cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids); - - int 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; - } - - 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. - cpSpacePushNewContactBuffer(space); - } - - // Narrow-phase collision detection. - cpContact *contacts = ((cpContactBuffer *)(space->contactBuffersHead))->contacts + space->contactBuffersHead->numContacts; - int numContacts = cpCollideShapes(a, b, contacts); - if(!numContacts) return; // Shapes are not colliding. - space->contactBuffersHead->numContacts += 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((size_t)a, (size_t)b); - cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, 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->stamp == -1 && !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 != cpArbiterStateIgnore) && - // Call preSolve - handler->preSolve(arb, space, handler->data) && - // Process, but don't add collisions for sensors. - !sensor - ){ - cpArrayPush(space->arbiters, arb); - } else { -// cpfree(arb->contacts); - space->contactBuffersHead->numContacts -= numContacts; - arb->contacts = NULL; - arb->numContacts = 0; - } - - // 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, (cpSpaceHashQueryFunc)queryFunc, space); -} - -// Hashset filter func to throw away old arbiters. -static int -contactSetFilter(cpArbiter *arb, cpSpace *space) -{ - int ticks = space->stamp - arb->stamp; - - // was used last frame, but not this one - if(ticks == 1){ - arb->handler->separate(arb, space, arb->handler->data); - arb->stamp = -1; // mark it as a new pair again. - } - - if(ticks >= cp_contact_persistence){ - cpArrayPush(space->pooledArbiters, arb); - return 0; - } - - return 1; -} - -// Hashset filter func to call and throw away post step callbacks. -static int -postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space) -{ - callback->func(space, callback->obj, callback->data); - cpfree(callback); - - return 0; -} - -#pragma mark All Important cpSpaceStep() Function - void -cpSpaceStep(cpSpace *space, cpFloat dt) +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) { - 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 = 1; - - // Empty the arbiter list. - space->arbiters->num = 0; - - // Integrate positions. for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->position_func(body, dt); + func((cpBody *)bodies->arr[i], data); } - // Pre-cache BBoxes and shape data. - cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL); - - // Collide! - cpSpacePushNewContactBuffer(space); - cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space); - cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space); - - // Clear out old cached arbiters and dispatch untouch functions - cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space); - - // Prestep the arbiters. - cpArray *arbiters = space->arbiters; - for(int i=0; inum; i++) - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); - - // Prestep the constraints. - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - constraint->klass->preStep(constraint, dt, dt_inv); + cpArray *components = space->sleepingComponents; + for(int i=0; inum; i++){ + cpBody *root = (cpBody *)components->arr[i]; + cpBody *body = root, *next; + do { + next = body->node.next; + func(body, data); + } while((body = next) != root); } - - for(int i=0; ielasticIterations; i++){ - for(int j=0; jnum; j++) - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f); - - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint); - } - } - - // Integrate velocities. - cpFloat damping = cpfpow(1.0f/space->damping, -dt); - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, space->gravity, damping, dt); - } - - for(int i=0; inum; i++) - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]); - - // run the old-style elastic solver if elastic iterations are disabled - cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f); - - // Run the impulse solver. - for(int i=0; iiterations; i++){ - for(int j=0; jnum; j++) - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef); - - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint); - } - } - - space->locked = 0; - - // run the post solve callbacks - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; - - cpCollisionHandler *handler = arb->handler; - handler->postSolve(arb, space, handler->data); - - arb->state = cpArbiterStateNormal; - } - - // Run the post step callbacks - // Use filter as an easy way to clear out the queue as it runs - cpHashSetFilter(space->postStepCallbacks, (cpHashSetFilterFunc)postStepCallbackSetFilter, space); - -// cpFloat dvsq = cpvdot(space->gravity, space->gravity); -// dvsq *= dt*dt * space->damping*space->damping; -// for(int i=0; inum; i++) -// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks); - - // Increment the stamp. - space->stamp++; } + + diff --git a/chipmunk/src/cpSpaceHash.c b/chipmunk/src/cpSpaceHash.c index bbc34f7981..b3f7917354 100644 --- a/chipmunk/src/cpSpaceHash.c +++ b/chipmunk/src/cpSpaceHash.c @@ -21,9 +21,8 @@ #include #include -#include -#include "chipmunk.h" +#include "chipmunk_private.h" #include "prime.h" static cpHandle* @@ -36,11 +35,7 @@ cpHandleInit(cpHandle *hand, void *obj) return hand; } -static inline void -cpHandleRetain(cpHandle *hand) -{ - hand->retain++; -} +static inline void cpHandleRetain(cpHandle *hand){hand->retain++;} static inline void cpHandleRelease(cpHandle *hand, cpArray *pooledHandles) @@ -55,7 +50,7 @@ cpSpaceHashAlloc(void) return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash)); } -// Frees the old table, and allocates a new one. +// Frees the old table, and allocate a new one. static void cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) { @@ -66,12 +61,7 @@ cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) } // Equality function for the handleset. -static int -handleSetEql(void *obj, void *elt) -{ - cpHandle *hand = (cpHandle *)elt; - return (obj == hand->obj); -} +static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);} // Transformation function for the handleset. static void * @@ -101,7 +91,7 @@ cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBF hash->celldim = celldim; hash->bbfunc = bbfunc; - hash->handleSet = cpHashSetNew(0, handleSetEql, (cpHashSetTransFunc)handleSetTrans); + hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, (cpHashSetTransFunc)handleSetTrans); hash->pooledHandles = cpArrayNew(0); hash->pooledBins = NULL; @@ -132,7 +122,6 @@ clearHashCell(cpSpaceHash *hash, int idx) while(bin){ cpSpaceHashBin *next = bin->next; - // Release the lock on the handle. cpHandleRelease(bin->handle, hash->pooledHandles); recycleBin(hash, bin); @@ -186,15 +175,15 @@ cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) } // Return true if the chain contains the handle. -static inline int +static inline cpBool containsHandle(cpSpaceHashBin *bin, cpHandle *hand) { while(bin){ - if(bin->handle == hand) return 1; + if(bin->handle == hand) return cpTrue; bin = bin->next; } - return 0; + return cpFalse; } // Get a recycled or new bin. @@ -266,36 +255,32 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) } void -cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB bb) +cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB _deprecated_unused) { cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash); - hashHandle(hash, hand, bb); + hashHandle(hash, hand, hash->bbfunc(obj)); } void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) { - cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, hashid, obj); - hashHandle(hash, hand, hash->bbfunc(obj)); + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand, hash->pooledHandles); + + cpSpaceHashInsert(hash, obj, hashid, cpBBNew(0.0f, 0.0f, 0.0f, 0.0f)); + } } -// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?) -static void -handleRehashHelper(void *elt, void *data) -{ - cpHandle *hand = (cpHandle *)elt; - cpSpaceHash *hash = (cpSpaceHash *)data; - - hashHandle(hash, hand, hash->bbfunc(hand->obj)); -} +static void handleRehashHelper(cpHandle *hand, cpSpaceHash *hash){hashHandle(hash, hand, hash->bbfunc(hand->obj));} void cpSpaceHashRehash(cpSpaceHash *hash) { clearHash(hash); - - // Rehash all of the handles. - cpHashSetEach(hash->handleSet, &handleRehashHelper, hash); + cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)handleRehashHelper, hash); } void @@ -309,54 +294,63 @@ cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) } } -// Used by the cpSpaceHashEach() iterator. typedef struct eachPair { cpSpaceHashIterator func; void *data; } eachPair; -// Calls the user iterator function. (Gross I know.) -static void -eachHelper(void *elt, void *data) -{ - cpHandle *hand = (cpHandle *)elt; - eachPair *pair = (eachPair *)data; - - pair->func(hand->obj, pair->data); -} +static void eachHelper(cpHandle *hand, eachPair *pair){pair->func(hand->obj, pair->data);} // Iterate over the objects in the spatial hash. void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data) { - // Bundle the callback up to send to the hashset iterator. eachPair pair = {func, data}; - - cpHashSetEach(hash->handleSet, &eachHelper, &pair); + cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &pair); +} + +static inline void +removeOrphanedHandles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) +{ + cpSpaceHashBin *bin = *bin_ptr; + while(bin){ + cpHandle *hand = bin->handle; + cpSpaceHashBin *next = bin->next; + + if(!hand->obj){ + // orphaned handle, unlink and recycle the bin + (*bin_ptr) = bin->next; + recycleBin(hash, bin); + + cpHandleRelease(hand, hash->pooledHandles); + } else { + bin_ptr = &bin->next; + } + + bin = next; + } } // Calls the callback function for the objects in a given chain. static inline void -query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data) +query(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashQueryFunc func, void *data) { - for(; bin; bin = bin->next){ + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ cpHandle *hand = bin->handle; void *other = hand->obj; - // Skip over certain conditions - if( - // Have we already tried this pair in this query? - hand->stamp == hash->stamp - // Is obj the same as other? - || obj == other - // Has other been removed since the last rehash? - || !other - ) continue; - - func(obj, other, data); - - // Stamp that the handle was checked already against this object. - hand->stamp = hash->stamp; + if(hand->stamp == hash->stamp || obj == other){ + continue; + } else if(other){ + func(obj, other, data); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + removeOrphanedHandles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } } } @@ -366,10 +360,7 @@ cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func cpFloat dim = hash->celldim; int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells); // Fix by ShiftZ - query(hash, hash->table[idx], &point, func, data); - - // Increment the stamp. - // Only one cell is checked, but query() requires it anyway. + query(hash, &hash->table[idx], &point, func, data); hash->stamp++; } @@ -384,16 +375,15 @@ cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc fun int t = floor_int(bb.t/dim); int n = hash->numcells; + cpSpaceHashBin **table = hash->table; // Iterate over the cells and query them. for(int i=l; i<=r; i++){ for(int j=b; j<=t; j++){ - int idx = hash_func(i,j,n); - query(hash, hash->table[idx], obj, func, data); + query(hash, &table[hash_func(i,j,n)], obj, func, data); } } - // Increment the stamp. hash->stamp++; } @@ -425,29 +415,27 @@ handleQueryRehashHelper(void *elt, void *data) int r = floor_int(bb.r/dim); int b = floor_int(bb.b/dim); int t = floor_int(bb.t/dim); + + cpSpaceHashBin **table = hash->table; for(int i=l; i<=r; i++){ for(int j=b; j<=t; j++){ -// // exit the loops if the object has been deleted in func(). -// if(!hand->obj) goto break_out; - int idx = hash_func(i,j,n); - cpSpaceHashBin *bin = hash->table[idx]; + cpSpaceHashBin *bin = table[idx]; if(containsHandle(bin, hand)) continue; cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() - query(hash, bin, obj, func, pair->data); + query(hash, &bin, obj, func, pair->data); cpSpaceHashBin *newBin = getEmptyBin(hash); newBin->handle = hand; newBin->next = bin; - hash->table[idx] = newBin; + table[idx] = newBin; } } -// break_out: - // Increment the stamp for each object we hash. + // Increment the stamp for each object hashed. hash->stamp++; } @@ -461,26 +449,27 @@ cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data) } static inline cpFloat -segmentQuery(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashSegmentQueryFunc func, void *data) +segmentQuery(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashSegmentQueryFunc func, void *data) { cpFloat t = 1.0f; - for(; bin; bin = bin->next){ + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ cpHandle *hand = bin->handle; void *other = hand->obj; // Skip over certain conditions - if( - // Have we already tried this pair in this query? - hand->stamp == hash->stamp - // Has other been removed since the last rehash? - || !other - ) continue; - - // Stamp that the handle was checked already against this object. - hand->stamp = hash->stamp; - - t = cpfmin(t, func(obj, other, data)); + if(hand->stamp == hash->stamp){ + continue; + } else if(other){ + t = cpfmin(t, func(obj, other, data)); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + removeOrphanedHandles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } } return t; @@ -492,8 +481,6 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c a = cpvmult(a, 1.0f/hash->celldim); b = cpvmult(b, 1.0f/hash->celldim); - cpFloat dt_dx = 1.0f/cpfabs(b.x - a.x), dt_dy = 1.0f/cpfabs(b.y - a.y); - int cell_x = floor_int(a.x), cell_y = floor_int(a.y); cpFloat t = 0; @@ -517,14 +504,20 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c 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, hash->table[idx], obj, func, data)); + t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data)); if (next_v < next_h){ cell_y += y_inc;