From 5cd33cc4e95c43b89918f3098ec10d5bf73afee0 Mon Sep 17 00:00:00 2001 From: spartanj Date: Tue, 19 Apr 2011 04:48:02 -0300 Subject: [PATCH] Updated chipmunk 0.6 with the current beta. Removed some deprecated functions, still lacks of some new functions, i'll add them soon. --- src/helper/chipmunk/chipmunk.c | 4 +- src/helper/chipmunk/chipmunk.h | 76 ++--- src/helper/chipmunk/chipmunk_ffi.h | 12 +- src/helper/chipmunk/chipmunk_private.h | 74 +++-- src/helper/chipmunk/chipmunk_types.h | 80 +++++- src/helper/chipmunk/chipmunk_unsafe.h | 13 +- .../chipmunk/constraints/cpConstraint.c | 9 +- .../chipmunk/constraints/cpConstraint.h | 81 ++++-- .../constraints/cpDampedRotarySpring.c | 20 +- .../constraints/cpDampedRotarySpring.h | 8 + .../chipmunk/constraints/cpDampedSpring.c | 21 +- .../chipmunk/constraints/cpDampedSpring.h | 9 + src/helper/chipmunk/constraints/cpGearJoint.c | 28 +- src/helper/chipmunk/constraints/cpGearJoint.h | 10 + .../chipmunk/constraints/cpGrooveJoint.c | 30 +- .../chipmunk/constraints/cpGrooveJoint.h | 12 +- src/helper/chipmunk/constraints/cpPinJoint.c | 29 +- src/helper/chipmunk/constraints/cpPinJoint.h | 9 + .../chipmunk/constraints/cpPivotJoint.c | 28 +- .../chipmunk/constraints/cpPivotJoint.h | 10 + .../chipmunk/constraints/cpRatchetJoint.c | 33 ++- .../chipmunk/constraints/cpRatchetJoint.h | 9 + .../chipmunk/constraints/cpRotaryLimitJoint.c | 36 ++- .../chipmunk/constraints/cpRotaryLimitJoint.h | 9 + .../chipmunk/constraints/cpSimpleMotor.c | 29 +- .../chipmunk/constraints/cpSimpleMotor.h | 9 + .../chipmunk/constraints/cpSlideJoint.c | 38 ++- .../chipmunk/constraints/cpSlideJoint.h | 9 + src/helper/chipmunk/constraints/util.h | 30 +- src/helper/chipmunk/cpArbiter.c | 96 +++++-- src/helper/chipmunk/cpArbiter.h | 162 +++++------ src/helper/chipmunk/cpArray.c | 37 +-- src/helper/chipmunk/cpBB.h | 32 ++- src/helper/chipmunk/cpBBTree.c | 265 +++++++++-------- src/helper/chipmunk/cpBody.c | 122 ++++++-- src/helper/chipmunk/cpBody.h | 208 ++++++++------ src/helper/chipmunk/cpCollision.c | 3 +- src/helper/chipmunk/cpHashSet.c | 35 +-- src/helper/chipmunk/cpPolyShape.c | 23 +- src/helper/chipmunk/cpPolyShape.h | 34 ++- src/helper/chipmunk/cpShape.c | 75 ++--- src/helper/chipmunk/cpShape.h | 187 +++++++----- src/helper/chipmunk/cpSpace.c | 247 ++++++++-------- src/helper/chipmunk/cpSpace.h | 179 +++++++----- src/helper/chipmunk/cpSpaceComponent.c | 62 ++-- src/helper/chipmunk/cpSpaceHash.c | 203 ++++++------- src/helper/chipmunk/cpSpaceQuery.c | 30 +- src/helper/chipmunk/cpSpaceStep.c | 198 ++++++++----- src/helper/chipmunk/cpSpatialIndex.c | 18 +- src/helper/chipmunk/cpSpatialIndex.h | 163 ++++++++--- src/helper/chipmunk/cpSweep1D.c | 269 ++++++++++++++++++ src/helper/chipmunk/cpVect.c | 2 +- src/helper/chipmunk/cpVect.h | 24 +- src/physics/cbody.cpp | 10 +- src/physics/cbody.hpp | 5 +- src/physics/constraints/cconstraint.cpp | 8 - src/physics/constraints/cconstraint.hpp | 4 - src/physics/cphysicsmanager.cpp | 32 --- src/physics/cphysicsmanager.hpp | 16 -- src/physics/cspace.cpp | 24 +- src/physics/cspace.hpp | 8 - src/test/eetest.cpp | 2 - 62 files changed, 2192 insertions(+), 1356 deletions(-) create mode 100644 src/helper/chipmunk/cpSweep1D.c diff --git a/src/helper/chipmunk/chipmunk.c b/src/helper/chipmunk/chipmunk.c index 058930800..29ab14e2e 100644 --- a/src/helper/chipmunk/chipmunk.c +++ b/src/helper/chipmunk/chipmunk.c @@ -44,7 +44,7 @@ cpMessage(const char *message, const char *condition, const char *file, int line } -const char *cpVersionString = "5.3.4"; +const char *cpVersionString = "6.0.0b"; void cpInitChipmunk(void) @@ -111,7 +111,7 @@ cpAreaForPoly(const int numVerts, const cpVect *verts) area += cpvcross(verts[i], verts[(i+1)%numVerts]); } - return area/2.0f; + return -area/2.0f; } cpVect diff --git a/src/helper/chipmunk/chipmunk.h b/src/helper/chipmunk/chipmunk.h index 73ebb1f1d..680c89675 100644 --- a/src/helper/chipmunk/chipmunk.h +++ b/src/helper/chipmunk/chipmunk.h @@ -19,6 +19,9 @@ * SOFTWARE. */ +/// @defgroup misc Misc +/// @{ + #ifndef CHIPMUNK_HEADER #define CHIPMUNK_HEADER @@ -51,30 +54,10 @@ void cpMessage(const char *message, const char *condition, const char *file, int #include "chipmunk_types.h" -#ifndef INFINITY - //TODO use C++ infinity - #ifdef _MSC_VER - union MSVC_EVIL_FLOAT_HACK - { - unsigned __int8 Bytes[4]; - float Value; - }; - static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; - #define INFINITY (INFINITY_HACK.Value) - #endif - - #ifdef __GNUC__ - #define INFINITY (__builtin_inf()) - #endif - - #ifndef INFINITY - #define INFINITY (1e1000) - #endif -#endif - // Maximum allocated size for various Chipmunk buffers #define CP_BUFFER_BYTES (32*1024) +//TODO allow redifinition #define cpmalloc malloc #define cpcalloc calloc #define cprealloc realloc @@ -104,57 +87,42 @@ typedef struct cpSpace cpSpace; #include "cpSpace.h" -#define CP_HASH_COEF (3344921057ul) -#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) - +/// Version string. extern const char *cpVersionString; + +/// Initialize Chipmunk. +/// Must be called before anything else or your program will crash. 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. +/// @c r1 and @c 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 area of a hollow circle. -*/ +/// Calculate area of a hollow circle. +/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2); -/** - Calculate the moment of inertia for a line segment. - Beveling radius is not supported. -*/ +/// Calculate the moment of inertia for a line segment. +/// Beveling radius is not supported. cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b); -/** - Calculate the area of a fattened (capsule shaped) line segment. -*/ +/// 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 polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex. -*/ +/// 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. -*/ +/// Calculate the signed area of a polygon. A Clockwise winding gives positive area. +/// This is probably backwards from what you expect, but matches Chipmunk's the winding for poly shapes. cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts); -/** - Calculate the natural centroid of a polygon. -*/ +/// 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) -*/ +/// 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. -*/ +/// Calculate the moment of inertia for a solid box. cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); #ifdef __cplusplus @@ -167,5 +135,5 @@ static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql static inline cpVect operator -(const cpVect v){return cpvneg(v);} #endif - #endif +//@} diff --git a/src/helper/chipmunk/chipmunk_ffi.h b/src/helper/chipmunk/chipmunk_ffi.h index 7fe0d8ec9..4e2214cbc 100644 --- a/src/helper/chipmunk/chipmunk_ffi.h +++ b/src/helper/chipmunk/chipmunk_ffi.h @@ -1,3 +1,5 @@ +// TODO update me + // Create non static inlined copies of Chipmunk functions, useful for working with dynamic FFIs // This file should only be included in chipmunk.c @@ -35,11 +37,11 @@ MAKE_REF(cpvdistsq); MAKE_REF(cpvnear); MAKE_REF(cpBBNew); -MAKE_REF(cpBBintersects); -MAKE_REF(cpBBcontainsBB); -MAKE_REF(cpBBcontainsVect); -MAKE_REF(cpBBmerge); -MAKE_REF(cpBBexpand); +MAKE_REF(cpBBIntersects); +MAKE_REF(cpBBContainsBB); +MAKE_REF(cpBBContainsVect); +MAKE_REF(cpBBMerge); +MAKE_REF(cpBBExpand); MAKE_REF(cpBodyWorld2Local); MAKE_REF(cpBodyLocal2World); diff --git a/src/helper/chipmunk/chipmunk_private.h b/src/helper/chipmunk/chipmunk_private.h index 6c30fe9b0..a39695159 100644 --- a/src/helper/chipmunk/chipmunk_private.h +++ b/src/helper/chipmunk/chipmunk_private.h @@ -22,6 +22,9 @@ #define CP_ALLOW_PRIVATE_ACCESS 1 #include "chipmunk.h" +#define CP_HASH_COEF (3344921057ul) +#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) + #pragma mark cpArray struct cpArray { @@ -29,12 +32,9 @@ struct cpArray { void **arr; }; -// TODO get rid of reference versions? -cpArray *cpArrayAlloc(void); -cpArray *cpArrayInit(cpArray *arr, int size); cpArray *cpArrayNew(int size); -void cpArrayDestroy(cpArray *arr); +//void cpArrayDestroy(cpArray *arr); void cpArrayFree(cpArray *arr); void cpArrayPush(cpArray *arr, void *object); @@ -47,10 +47,10 @@ void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); #pragma mark Foreach loops #define CP_BODY_FOREACH_CONSTRAINT(body, var)\ - for(cpConstraint *var = body->constraintList; var; var = (var->a == body ? var->nextA : var->nextB)) + for(cpConstraint *var = body->constraintList; var; var = (var->a == body ? var->next_a : var->next_b)) #define CP_BODY_FOREACH_ARBITER(bdy, var)\ - for(cpArbiter *var = bdy->arbiterList; var; var = (var->a->body == bdy ? var->nextA : var->nextB)) + for(cpArbiter *var = bdy->arbiterList; var; var = (var->a->body == bdy ? var->next_a : var->next_b)) #define CP_BODY_FOREACH_SHAPE(body, var)\ for(cpShape *var = body->shapeList; var; var = var->next) @@ -63,12 +63,10 @@ void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); -// TODO get rid of reference versions? -cpHashSet *cpHashSetAlloc(void); -cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *defaultValue); -cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *defaultValue); +cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc); +void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value); -void cpHashSetDestroy(cpHashSet *set); +//void cpHashSetDestroy(cpHashSet *set); void cpHashSetFree(cpHashSet *set); int cpHashSetCount(cpHashSet *set); @@ -76,33 +74,48 @@ void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, c void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); -typedef void (*cpHashSetIterFunc)(void *elt, void *data); -void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); +typedef void (*cpHashSetIteratorFunc)(void *elt, void *data); +void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data); typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); #pragma mark Arbiters +struct cpContact { + cpVect p, n; + cpFloat dist; + + cpVect r1, r2; + cpFloat nMass, tMass, bounce; + + cpFloat jnAcc, jtAcc, jBias; + cpFloat bias; + + cpHashValue hash; +}; + cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b); -void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv, cpFloat bias, cpFloat slop); +void cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat bias, cpFloat slop); void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef); void cpArbiterApplyImpulse(cpArbiter *arb); -#pragma mark Collision Functions +#pragma mark Shape/Collision Functions + +cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body); int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr); static inline cpFloat cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) { - cpVect *verts = poly->CP_PRIVATE(tVerts); + cpVect *verts = poly->tVerts; cpFloat min = cpvdot(n, verts[0]); - for(int i=1; iCP_PRIVATE(numVerts); i++){ + for(int i=1; inumVerts; i++){ min = cpfmin(min, cpvdot(n, verts[i])); } @@ -112,9 +125,9 @@ cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) static inline cpBool cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) { - cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); + cpPolyShapeAxis *axes = poly->tAxes; - for(int i=0; iCP_PRIVATE(numVerts); i++){ + for(int i=0; inumVerts; i++){ cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; if(dist > 0.0f) return cpFalse; } @@ -125,9 +138,9 @@ cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) static inline cpBool cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n) { - cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); + cpPolyShapeAxis *axes = poly->tAxes; - for(int i=0; iCP_PRIVATE(numVerts); i++){ + for(int i=0; inumVerts; i++){ if(cpvdot(axes[i].n, n) < 0.0f) continue; cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; if(dist > 0.0f) return cpFalse; @@ -142,9 +155,24 @@ cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *k #pragma mark Space Functions +extern cpCollisionHandler cpDefaultCollisionHandler; +void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); + +void cpSpacePushFreshContactBuffer(cpSpace *space); cpContact *cpContactBufferGetArray(cpSpace *space); void cpSpacePushContacts(cpSpace *space, int count); -void cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space); +void cpSpacePopContacts(cpSpace *space, int count); + +void *cpSpaceGetPostStepData(cpSpace *space, void *obj); + +typedef struct cpPostStepCallback cpPostStepCallback; +//void cpSpacePostStepCallbackSetIter(cpPostStepCallback *callback, cpSpace *space); + +cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space); +void *cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space); +void cpShapeUpdateFunc(cpShape *shape, void *unused); + +//void cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space); void cpSpaceActivateBody(cpSpace *space, cpBody *body); void cpSpaceLock(cpSpace *space); -void cpSpaceUnlock(cpSpace *space); +void cpSpaceUnlock(cpSpace *space, cpBool runPostStep); diff --git a/src/helper/chipmunk/chipmunk_types.h b/src/helper/chipmunk/chipmunk_types.h index a9d9a4172..6c2d091d7 100644 --- a/src/helper/chipmunk/chipmunk_types.h +++ b/src/helper/chipmunk/chipmunk_types.h @@ -25,7 +25,13 @@ #define CP_USE_DOUBLES 1 #endif +/// @defgroup basicTypes Basic Types +/// Most of these types can be configured at compile time. +/// @{ + #if CP_USE_DOUBLES +/// Chipmunk's floating point type. +/// Can be reconfigured at compile time. typedef double cpFloat; #define cpfsqrt sqrt #define cpfsin sin @@ -51,53 +57,83 @@ #define cpfceil ceilf #endif +#ifndef INFINITY + //TODO use C++ infinity + #ifdef _MSC_VER + union MSVC_EVIL_FLOAT_HACK + { + unsigned __int8 Bytes[4]; + float Value; + }; + static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; + #define INFINITY (INFINITY_HACK.Value) + #endif + + #ifdef __GNUC__ + #define INFINITY (__builtin_inf()) + #endif + + #ifndef INFINITY + #define INFINITY (1e1000) + #endif +#endif + +#ifndef M_PI + #define M_PI 3.14159265358979323846264338327950288 +#endif + +#ifndef M_E + #define M_E 2.71828182845904523536028747135266250 +#endif + + +/// Return the max of two cpFloats. static inline cpFloat cpfmax(cpFloat a, cpFloat b) { return (a > b) ? a : b; } +/// Return the min of two cpFloats. static inline cpFloat cpfmin(cpFloat a, cpFloat b) { return (a < b) ? a : b; } +/// Return the absolute value of a cpFloat. static inline cpFloat -cpfabs(cpFloat n) +cpfabs(cpFloat f) { - return (n < 0) ? -n : n; + return (f < 0) ? -f : f; } +/// Clamp @c f to be between @c min and @c max. static inline cpFloat cpfclamp(cpFloat f, cpFloat min, cpFloat max) { return cpfmin(cpfmax(f, min), max); } +/// Linearly interpolate (or extrapolate) between @c f1 and @c f2 by @c t percent. static inline cpFloat cpflerp(cpFloat f1, cpFloat f2, cpFloat t) { return f1*(1.0f - t) + f2*t; } +/// Linearly interpolate from @c f1 to @c f2 by no more than @c d. 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 - +/// Hash value type. typedef unsigned int cpHashValue; -// Oh C, how we love to define our own boolean types to get compiler compatibility +/// Chipmunk's boolean type. +/// 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 @@ -105,47 +141,69 @@ typedef unsigned int cpHashValue; #endif #ifndef cpTrue +/// true value. #define cpTrue 1 #endif #ifndef cpFalse +/// false value. #define cpFalse 0 #endif #ifdef CP_DATA_POINTER_TYPE typedef CP_DATA_POINTER_TYPE cpDataPointer; #else +/// Type used for user data pointers. typedef void * cpDataPointer; #endif #ifdef CP_COLLISION_TYPE_TYPE typedef CP_COLLISION_TYPE_TYPE cpCollisionType; #else +/// Type used for cpSpace.collision_type. typedef unsigned int cpCollisionType; #endif #ifdef CP_GROUP_TYPE typedef CP_GROUP_TYPE cpGroup; #else +/// Type used for cpShape.group. typedef unsigned int cpGroup; #endif #ifdef CP_LAYERS_TYPE typedef CP_GROUP_TYPE cpLayers; #else +/// Type used for cpShape.layers. typedef unsigned int cpLayers; #endif #ifdef CP_TIMESTAMP_TYPE typedef CP_TIMESTAMP_TYPE cpTimestamp; #else +/// Type used for various timestamps in Chipmunk. typedef unsigned int cpTimestamp; #endif #ifndef CP_NO_GROUP +/// Value for cpShape.group signifying that a shape is in no group. #define CP_NO_GROUP ((cpGroup)0) #endif #ifndef CP_ALL_LAYERS +/// Value for cpShape.layers signifying that a shape is in every layer. #define CP_ALL_LAYERS (~(cpLayers)0) #endif +/// @} + +// CGPoints are structurally the same, and allow +// easy interoperability with other Cocoa libraries +#ifdef CP_USE_CGPOINTS + typedef CGPoint cpVect; +#else +/// Chipmunk's 2D vector type. +/// @addtogroup cpVect + typedef struct cpVect{cpFloat x,y;} cpVect; +#endif + + diff --git a/src/helper/chipmunk/chipmunk_unsafe.h b/src/helper/chipmunk/chipmunk_unsafe.h index d15b6712d..637e33670 100644 --- a/src/helper/chipmunk/chipmunk_unsafe.h +++ b/src/helper/chipmunk/chipmunk_unsafe.h @@ -30,6 +30,12 @@ * persistent velocities. Probably not what you meant, but perhaps close enough. */ +/// @defgroup unsafe Chipmunk Unsafe Shape Operations +/// These functions are used for mutating collision shapes. +/// Chipmunk does not have any way to get velocity information on changing shapes, +/// so the results will be unrealistic. You must explicity include the chipmunk_unsafe.h header to use them. +/// @{ + #ifndef CHIPMUNK_UNSAFE_HEADER #define CHIPMUNK_UNSAFE_HEADER @@ -37,16 +43,21 @@ extern "C" { #endif +/// Set the radius of a circle shape. void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); +/// Set the offset of a circle shape. void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); +/// Set the endpoints of a segment shape. void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); +/// Set the radius of a segment shape. void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); +/// Set the vertexes of a poly shape. void cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset); #ifdef __cplusplus } #endif - #endif +/// @} diff --git a/src/helper/chipmunk/constraints/cpConstraint.c b/src/helper/chipmunk/constraints/cpConstraint.c index 2b65deeeb..e4c5c0cd0 100644 --- a/src/helper/chipmunk/constraints/cpConstraint.c +++ b/src/helper/chipmunk/constraints/cpConstraint.c @@ -20,14 +20,13 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" // TODO: Comment me! -cpFloat cp_constraint_bias_coef = 0.1f; - void cpConstraintDestroy(cpConstraint *constraint){} void @@ -49,10 +48,10 @@ cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBod constraint->a = a; constraint->b = b; - constraint->nextA = NULL; - constraint->nextB = NULL; + constraint->next_a = NULL; + constraint->next_b = NULL; constraint->maxForce = (cpFloat)INFINITY; - constraint->biasCoef = cp_constraint_bias_coef; + constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f); constraint->maxBias = (cpFloat)INFINITY; } diff --git a/src/helper/chipmunk/constraints/cpConstraint.h b/src/helper/chipmunk/constraints/cpConstraint.h index cbc804605..a6b2c2822 100644 --- a/src/helper/chipmunk/constraints/cpConstraint.h +++ b/src/helper/chipmunk/constraints/cpConstraint.h @@ -19,38 +19,60 @@ * SOFTWARE. */ -extern cpFloat cp_constraint_bias_coef; +/// @defgroup cpConstraint cpConstraint +/// @{ typedef struct cpConstraintClass cpConstraintClass; -typedef void (*cpConstraintPreStepFunction)(cpConstraint *constraint, cpFloat dt, cpFloat dt_inv); -typedef void (*cpConstraintApplyImpulseFunction)(cpConstraint *constraint); -typedef cpFloat (*cpConstraintGetImpulseFunction)(cpConstraint *constraint); +typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); +typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); +typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint); +typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); +/// @private struct cpConstraintClass { - cpConstraintPreStepFunction preStep; - cpConstraintApplyImpulseFunction applyImpulse; - cpConstraintGetImpulseFunction getImpulse; + cpConstraintPreStepImpl preStep; + cpConstraintApplyCachedImpulseImpl applyCachedImpulse; + cpConstraintApplyImpulseImpl applyImpulse; + cpConstraintGetImpulseImpl getImpulse; }; - +/// Opaque cpConstraint struct. struct cpConstraint { CP_PRIVATE(const cpConstraintClass *klass); - cpBody *a, *b; - cpConstraint CP_PRIVATE(*nextA), CP_PRIVATE(*nextB); + /// The first body connected to this constraint. + cpBody *a; + /// The second body connected to this constraint. + cpBody *b; + CP_PRIVATE(cpConstraint *next_a); + CP_PRIVATE(cpConstraint *next_b); + + /// The maximum force that this constraint is allowed to use. + /// Defaults to infinity. cpFloat maxForce; - cpFloat biasCoef; + /// The rate at which joint error is corrected. + /// Defaults to pow(1.0 - 0.1, 60.0) meaning that it will + /// correct 10% of the error every 1/60th of a second. + cpFloat errorBias; + /// The maximum rate at which joint error is corrected. + /// Defaults to infinity. cpFloat maxBias; + /// User definable data pointer. + /// Generally this points to your the game object class so you can access it + /// when given a cpConstraint reference in a callback. cpDataPointer data; }; +/// Destroy a constraint. void cpConstraintDestroy(cpConstraint *constraint); +/// Destroy and free a constraint. void cpConstraintFree(cpConstraint *constraint); +/// @private static inline void cpConstraintActivateBodies(cpConstraint *constraint) { @@ -58,36 +80,55 @@ cpConstraintActivateBodies(cpConstraint *constraint) cpBody *b = constraint->b; if(b) cpBodyActivate(b); } +#define CP_DefineConstraintStructGetter(type, member, name) \ +static inline type cpConstraint##Get##name(const cpConstraint *constraint){return constraint->member;} + +#define CP_DefineConstraintStructSetter(type, member, name) \ +static inline void cpConstraint##Set##name(cpConstraint *constraint, type value){ \ + cpConstraintActivateBodies(constraint); \ + constraint->member = value; \ +} + +#define CP_DefineConstraintStructProperty(type, member, name) \ +CP_DefineConstraintStructGetter(type, member, name) \ +CP_DefineConstraintStructSetter(type, member, name) + +CP_DefineConstraintStructGetter(cpBody *, a, A); +CP_DefineConstraintStructGetter(cpBody *, b, B); +CP_DefineConstraintStructProperty(cpFloat, maxForce, MaxForce); +CP_DefineConstraintStructProperty(cpFloat, errorBias, ErrorBias); +CP_DefineConstraintStructProperty(cpFloat, maxBias, MaxBias); +CP_DefineConstraintStructProperty(cpDataPointer, data, Data); + +/// Get the last impulse applied by this constraint. static inline cpFloat cpConstraintGetImpulse(cpConstraint *constraint) { return constraint->CP_PRIVATE(klass)->getImpulse(constraint); } -#define cpConstraintCheckCast(constraint, struct) \ - cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct); +/// @} +#define cpConstraintCheckCast(constraint, 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(const cpConstraint *constraint){ \ +static inline type struct##Get##name(const cpConstraint *constraint){ \ cpConstraintCheckCast(constraint, struct); \ return ((struct *)constraint)->member; \ -} \ +} #define CP_DefineConstraintSetter(struct, type, member, name) \ -static inline void \ -struct##Set##name(cpConstraint *constraint, type value){ \ +static inline void struct##Set##name(cpConstraint *constraint, type value){ \ cpConstraintCheckCast(constraint, struct); \ cpConstraintActivateBodies(constraint); \ ((struct *)constraint)->member = value; \ -} \ +} #define CP_DefineConstraintProperty(struct, type, member, name) \ CP_DefineConstraintGetter(struct, type, member, name) \ CP_DefineConstraintSetter(struct, type, member, name) -// Built in Joint types #include "cpPinJoint.h" #include "cpSlideJoint.h" #include "cpPivotJoint.h" diff --git a/src/helper/chipmunk/constraints/cpDampedRotarySpring.c b/src/helper/chipmunk/constraints/cpDampedRotarySpring.c index 499103dba..81cb6bc29 100644 --- a/src/helper/chipmunk/constraints/cpDampedRotarySpring.c +++ b/src/helper/chipmunk/constraints/cpDampedRotarySpring.c @@ -31,14 +31,16 @@ defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ } static void -preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) +preStep(cpDampedRotarySpring *spring, cpFloat dt) { - CONSTRAINT_BEGIN(spring, a, b); + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; cpFloat moment = a->i_inv + b->i_inv; + cpAssert(moment != 0.0, "Unsolvable spring."); spring->iSum = 1.0f/moment; - spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); + spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment*0.25f/(cpFloat)M_E); spring->target_wrn = 0.0f; // apply spring torque @@ -47,10 +49,13 @@ preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) b->w += j_spring*b->i_inv; } +static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){} + static void applyImpulse(cpDampedRotarySpring *spring) { - CONSTRAINT_BEGIN(spring, a, b); + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; // compute relative velocity cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; @@ -73,9 +78,10 @@ getImpulse(cpConstraint *constraint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpDampedRotarySpring) diff --git a/src/helper/chipmunk/constraints/cpDampedRotarySpring.h b/src/helper/chipmunk/constraints/cpDampedRotarySpring.h index b446a3a51..9f3775a6b 100644 --- a/src/helper/chipmunk/constraints/cpDampedRotarySpring.h +++ b/src/helper/chipmunk/constraints/cpDampedRotarySpring.h @@ -19,6 +19,9 @@ * SOFTWARE. */ +/// @defgroup cpDampedRotarySpring cpDampedRotarySpring +/// @{ + typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); const cpConstraintClass *cpDampedRotarySpringGetClass(); @@ -36,11 +39,16 @@ typedef struct cpDampedRotarySpring { cpFloat iSum; } cpDampedRotarySpring; +/// Allocate a damped rotary spring. cpDampedRotarySpring *cpDampedRotarySpringAlloc(void); +/// Initialize a damped rotary spring. cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); +/// Allocate and initialize a damped rotary spring. cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle); CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness); CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping); CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpDampedSpring.c b/src/helper/chipmunk/constraints/cpDampedSpring.c index 406af3af6..ef3819251 100644 --- a/src/helper/chipmunk/constraints/cpDampedSpring.c +++ b/src/helper/chipmunk/constraints/cpDampedSpring.c @@ -31,9 +31,10 @@ defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ } static void -preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) +preStep(cpDampedSpring *spring, cpFloat dt) { - CONSTRAINT_BEGIN(spring, a, b); + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; spring->r1 = cpvrotate(spring->anchr1, a->rot); spring->r2 = cpvrotate(spring->anchr2, b->rot); @@ -43,20 +44,24 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); + cpAssert(k != 0.0, "Unsolvable spring."); spring->nMass = 1.0f/k; spring->target_vrn = 0.0f; - spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); + spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k*0.5f/(cpFloat)M_E); // apply spring force cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt)); } +static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){} + static void applyImpulse(cpDampedSpring *spring) { - CONSTRAINT_BEGIN(spring, a, b); + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; cpVect n = spring->n; cpVect r1 = spring->r1; @@ -66,7 +71,6 @@ applyImpulse(cpDampedSpring *spring) cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; // compute velocity loss from drag - // not 100% certain this is derived correctly, though it makes sense cpFloat v_damp = -vrn*spring->v_coef; spring->target_vrn = vrn + v_damp; @@ -80,9 +84,10 @@ getImpulse(cpConstraint *constraint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpDampedSpring) diff --git a/src/helper/chipmunk/constraints/cpDampedSpring.h b/src/helper/chipmunk/constraints/cpDampedSpring.h index aed871e51..b34f6639f 100644 --- a/src/helper/chipmunk/constraints/cpDampedSpring.h +++ b/src/helper/chipmunk/constraints/cpDampedSpring.h @@ -19,12 +19,16 @@ * SOFTWARE. */ +/// @defgroup cpDampedSpring cpDampedSpring +/// @{ + typedef struct cpDampedSpring cpDampedSpring; typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); const cpConstraintClass *cpDampedSpringGetClass(); +/// @private struct cpDampedSpring { cpConstraint constraint; cpVect anchr1, anchr2; @@ -41,8 +45,11 @@ struct cpDampedSpring { cpVect n; }; +/// Allocate a damped spring. cpDampedSpring *cpDampedSpringAlloc(void); +/// Initialize a damped spring. cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); +/// Allocate and initialize a damped spring. cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1); @@ -51,3 +58,5 @@ CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength); CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness); CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping); CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpGearJoint.c b/src/helper/chipmunk/constraints/cpGearJoint.c index dd694d35b..5426c4ec0 100644 --- a/src/helper/chipmunk/constraints/cpGearJoint.c +++ b/src/helper/chipmunk/constraints/cpGearJoint.c @@ -20,27 +20,35 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpGearJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias); + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias); // compute max impulse joint->jMax = J_MAX(joint, dt); +} - // apply joint torque - cpFloat j = joint->jAcc; +static void +applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; a->w -= j*a->i_inv*joint->ratio_inv; b->w += j*b->i_inv; } @@ -48,7 +56,8 @@ preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) static void applyImpulse(cpGearJoint *joint) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // compute relative rotational velocity cpFloat wr = b->w*joint->ratio - a->w; @@ -71,9 +80,10 @@ getImpulse(cpGearJoint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpGearJoint) diff --git a/src/helper/chipmunk/constraints/cpGearJoint.h b/src/helper/chipmunk/constraints/cpGearJoint.h index f0317c1d4..0b0682734 100644 --- a/src/helper/chipmunk/constraints/cpGearJoint.h +++ b/src/helper/chipmunk/constraints/cpGearJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpGearJoint cpGearJoint +/// @{ + const cpConstraintClass *cpGearJointGetClass(); +/// @private typedef struct cpGearJoint { cpConstraint constraint; cpFloat phase, ratio; @@ -32,10 +36,16 @@ typedef struct cpGearJoint { cpFloat jAcc, jMax; } cpGearJoint; +/// Allocate a gear joint. cpGearJoint *cpGearJointAlloc(void); +/// Initialize a gear joint. cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); +/// Allocate and initialize a gear joint. cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase); CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio); +/// Set the ratio of a gear joint. void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpGrooveJoint.c b/src/helper/chipmunk/constraints/cpGrooveJoint.c index 4a067ef92..5d46dded1 100644 --- a/src/helper/chipmunk/constraints/cpGrooveJoint.c +++ b/src/helper/chipmunk/constraints/cpGrooveJoint.c @@ -20,14 +20,16 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpGrooveJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // calculate endpoints in worldspace cpVect ta = cpBodyLocal2World(a, joint->grv_a); @@ -62,10 +64,16 @@ preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); - - // apply accumulated impulse - apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); + joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); +} + +static void +applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); } static inline cpVect @@ -78,7 +86,8 @@ grooveConstrain(cpGrooveJoint *joint, cpVect j){ static void applyImpulse(cpGrooveJoint *joint) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpVect r1 = joint->r1; cpVect r2 = joint->r2; @@ -102,9 +111,10 @@ getImpulse(cpGrooveJoint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpGrooveJoint) diff --git a/src/helper/chipmunk/constraints/cpGrooveJoint.h b/src/helper/chipmunk/constraints/cpGrooveJoint.h index 831e3b68c..bed2c52ff 100644 --- a/src/helper/chipmunk/constraints/cpGrooveJoint.h +++ b/src/helper/chipmunk/constraints/cpGrooveJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpGrooveJoint cpGrooveJoint +/// @{ + const cpConstraintClass *cpGrooveJointGetClass(); +/// @private typedef struct cpGrooveJoint { cpConstraint constraint; cpVect grv_n, grv_a, grv_b; @@ -36,13 +40,19 @@ typedef struct cpGrooveJoint { cpVect bias; } cpGrooveJoint; +/// Allocate a groove joint. cpGrooveJoint *cpGrooveJointAlloc(void); +/// Initialize a groove joint. cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); +/// Allocate and initialize a groove joint. cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); - CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA); +/// Set endpoint a of a groove joint's groove void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value); CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB); +/// Set endpoint b of a groove joint's groove void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value); CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpPinJoint.c b/src/helper/chipmunk/constraints/cpPinJoint.c index 3955143ff..8bd53dfa5 100644 --- a/src/helper/chipmunk/constraints/cpPinJoint.c +++ b/src/helper/chipmunk/constraints/cpPinJoint.c @@ -20,15 +20,16 @@ */ #include -//#include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpPinJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -42,20 +43,27 @@ preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias); + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias); // compute max impulse joint->jnMax = J_MAX(joint, dt); +} + +static void +applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; - // apply accumulated impulse - cpVect j = cpvmult(joint->n, joint->jnAcc); + cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); apply_impulses(a, b, joint->r1, joint->r2, j); } static void applyImpulse(cpPinJoint *joint) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpVect n = joint->n; // compute relative velocity @@ -78,9 +86,10 @@ getImpulse(cpPinJoint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpPinJoint); diff --git a/src/helper/chipmunk/constraints/cpPinJoint.h b/src/helper/chipmunk/constraints/cpPinJoint.h index 5b257a190..8b57f2664 100644 --- a/src/helper/chipmunk/constraints/cpPinJoint.h +++ b/src/helper/chipmunk/constraints/cpPinJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpPinJoint cpPinJoint +/// @{ + const cpConstraintClass *cpPinJointGetClass(); +/// @private typedef struct cpPinJoint { cpConstraint constraint; cpVect anchr1, anchr2; @@ -34,10 +38,15 @@ typedef struct cpPinJoint { cpFloat bias; } cpPinJoint; +/// Allocate a pin joint. cpPinJoint *cpPinJointAlloc(void); +/// Initialize a pin joint. cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +/// Allocate and initialize a pin joint. cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1); CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2); CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist); + +///@} diff --git a/src/helper/chipmunk/constraints/cpPivotJoint.c b/src/helper/chipmunk/constraints/cpPivotJoint.c index 634bd0233..b2c7b5795 100644 --- a/src/helper/chipmunk/constraints/cpPivotJoint.c +++ b/src/helper/chipmunk/constraints/cpPivotJoint.c @@ -20,14 +20,16 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpPivotJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -40,16 +42,23 @@ preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); +} + +static void +applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; - // apply accumulated impulse - apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); } static void applyImpulse(cpPivotJoint *joint) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpVect r1 = joint->r1; cpVect r2 = joint->r2; @@ -74,9 +83,10 @@ getImpulse(cpConstraint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpPivotJoint) diff --git a/src/helper/chipmunk/constraints/cpPivotJoint.h b/src/helper/chipmunk/constraints/cpPivotJoint.h index e71c7a015..13cf4a43a 100644 --- a/src/helper/chipmunk/constraints/cpPivotJoint.h +++ b/src/helper/chipmunk/constraints/cpPivotJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpPivotJoint cpPivotJoint +/// @{ + const cpConstraintClass *cpPivotJointGetClass(); +/// @private typedef struct cpPivotJoint { cpConstraint constraint; cpVect anchr1, anchr2; @@ -33,10 +37,16 @@ typedef struct cpPivotJoint { cpVect bias; } cpPivotJoint; +/// Allocate a pivot joint cpPivotJoint *cpPivotJointAlloc(void); +/// Initialize a pivot joint. cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +/// Allocate and initialize a pivot joint. cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); +/// Allocate and initialize a pivot joint with specific anchors. cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1); CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpRatchetJoint.c b/src/helper/chipmunk/constraints/cpRatchetJoint.c index a7103c7f0..4a1a4dd7f 100644 --- a/src/helper/chipmunk/constraints/cpRatchetJoint.c +++ b/src/helper/chipmunk/constraints/cpRatchetJoint.c @@ -26,9 +26,10 @@ #include "constraints/util.h" static void -preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpRatchetJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpFloat angle = joint->angle; cpFloat phase = joint->phase; @@ -49,18 +50,24 @@ preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias); + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); // compute max impulse joint->jMax = J_MAX(joint, dt); // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) - joint->jAcc = 0.0f; + if(!joint->bias) joint->jAcc = 0.0f; +} - // apply joint torque - a->w -= joint->jAcc*a->i_inv; - b->w += joint->jAcc*b->i_inv; +static void +applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; } static void @@ -68,7 +75,8 @@ applyImpulse(cpRatchetJoint *joint) { if(!joint->bias) return; // early exit - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // compute relative rotational velocity cpFloat wr = b->w - a->w; @@ -92,9 +100,10 @@ getImpulse(cpRatchetJoint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpRatchetJoint) diff --git a/src/helper/chipmunk/constraints/cpRatchetJoint.h b/src/helper/chipmunk/constraints/cpRatchetJoint.h index ecb026ce7..a37339c48 100644 --- a/src/helper/chipmunk/constraints/cpRatchetJoint.h +++ b/src/helper/chipmunk/constraints/cpRatchetJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpRatchetJoint cpRatchetJoint +/// @{ + const cpConstraintClass *cpRatchetJointGetClass(); +/// @private typedef struct cpRatchetJoint { cpConstraint constraint; cpFloat angle, phase, ratchet; @@ -31,10 +35,15 @@ typedef struct cpRatchetJoint { cpFloat jAcc, jMax; } cpRatchetJoint; +/// Allocate a ratchet joint. cpRatchetJoint *cpRatchetJointAlloc(void); +/// Initialize a ratched joint. cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); +/// Allocate and initialize a ratchet joint. cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle); CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase); CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c index 9bfa1ea70..f4d9f51a5 100644 --- a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c +++ b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c @@ -20,14 +20,16 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpRotaryLimitJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpFloat dist = b->a - a->a; cpFloat pdist = 0.0f; @@ -38,22 +40,28 @@ preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv) } // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); + joint->iSum = 1.0f/(1.0f/a->i + 1.0f/b->i); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); // compute max impulse joint->jMax = J_MAX(joint, dt); // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) - joint->jAcc = 0.0f; + if(!joint->bias) joint->jAcc = 0.0f; +} - // apply joint torque - a->w -= joint->jAcc*a->i_inv; - b->w += joint->jAcc*b->i_inv; +static void +applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; } static void @@ -61,7 +69,8 @@ applyImpulse(cpRotaryLimitJoint *joint) { if(!joint->bias) return; // early exit - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // compute relative rotational velocity cpFloat wr = b->w - a->w; @@ -88,9 +97,10 @@ getImpulse(cpRotaryLimitJoint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpRotaryLimitJoint) diff --git a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h index 9062f85f0..5d55e8c3f 100644 --- a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h +++ b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint +/// @{ + const cpConstraintClass *cpRotaryLimitJointGetClass(); +/// @private typedef struct cpRotaryLimitJoint { cpConstraint constraint; cpFloat min, max; @@ -31,9 +35,14 @@ typedef struct cpRotaryLimitJoint { cpFloat jAcc, jMax; } cpRotaryLimitJoint; +/// Allocate a damped rotary limit joint. cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void); +/// Initialize a damped rotary limit joint. cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); +/// Allocate and initialize a damped rotary limit joint. cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min); CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpSimpleMotor.c b/src/helper/chipmunk/constraints/cpSimpleMotor.c index c4f771e44..333c845d7 100644 --- a/src/helper/chipmunk/constraints/cpSimpleMotor.c +++ b/src/helper/chipmunk/constraints/cpSimpleMotor.c @@ -20,30 +20,40 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpSimpleMotor *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); // compute max impulse joint->jMax = J_MAX(joint, dt); +} - // apply joint torque - a->w -= joint->jAcc*a->i_inv; - b->w += joint->jAcc*b->i_inv; +static void +applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; } static void applyImpulse(cpSimpleMotor *joint) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; // compute relative rotational velocity cpFloat wr = b->w - a->w + joint->rate; @@ -66,9 +76,10 @@ getImpulse(cpSimpleMotor *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpSimpleMotor) diff --git a/src/helper/chipmunk/constraints/cpSimpleMotor.h b/src/helper/chipmunk/constraints/cpSimpleMotor.h index 1611e640d..acd6ca256 100644 --- a/src/helper/chipmunk/constraints/cpSimpleMotor.h +++ b/src/helper/chipmunk/constraints/cpSimpleMotor.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpSimpleMotor cpSimpleMotor +/// @{ + const cpConstraintClass *cpSimpleMotorGetClass(); +/// @private typedef struct cpSimpleMotor { cpConstraint constraint; cpFloat rate; @@ -30,8 +34,13 @@ typedef struct cpSimpleMotor { cpFloat jAcc, jMax; } cpSimpleMotor; +/// Allocate a simple motor. cpSimpleMotor *cpSimpleMotorAlloc(void); +/// initialize a simple motor. cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); +/// Allocate and initialize a simple motor. cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate); + +/// @} diff --git a/src/helper/chipmunk/constraints/cpSlideJoint.c b/src/helper/chipmunk/constraints/cpSlideJoint.c index 79c627c7f..698b936d6 100644 --- a/src/helper/chipmunk/constraints/cpSlideJoint.c +++ b/src/helper/chipmunk/constraints/cpSlideJoint.c @@ -20,14 +20,16 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" static void -preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) +preStep(cpSlideJoint *joint, cpFloat dt) { - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; joint->r1 = cpvrotate(joint->anchr1, a->rot); joint->r2 = cpvrotate(joint->anchr2, b->rot); @@ -48,19 +50,23 @@ preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); // compute max impulse joint->jnMax = J_MAX(joint, dt); + + // if bias is 0, then the joint is not at a limit. Reset cached impulse. + if(!joint->bias) joint->jnAcc = 0.0f; +} - // apply accumulated impulse - if(!joint->bias) //{ - // if bias is 0, then the joint is not at a limit. - joint->jnAcc = 0.0f; -// } else { - cpVect j = cpvmult(joint->n, joint->jnAcc); - apply_impulses(a, b, joint->r1, joint->r2, j); -// } +static void +applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); + apply_impulses(a, b, joint->r1, joint->r2, j); } static void @@ -68,7 +74,8 @@ applyImpulse(cpSlideJoint *joint) { if(!joint->bias) return; // early exit - CONSTRAINT_BEGIN(joint, a, b); + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; cpVect n = joint->n; cpVect r1 = joint->r1; @@ -95,9 +102,10 @@ getImpulse(cpConstraint *joint) } static const cpConstraintClass klass = { - (cpConstraintPreStepFunction)preStep, - (cpConstraintApplyImpulseFunction)applyImpulse, - (cpConstraintGetImpulseFunction)getImpulse, + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, }; CP_DefineClassGetter(cpSlideJoint) diff --git a/src/helper/chipmunk/constraints/cpSlideJoint.h b/src/helper/chipmunk/constraints/cpSlideJoint.h index c5ffa4e3d..86689538d 100644 --- a/src/helper/chipmunk/constraints/cpSlideJoint.h +++ b/src/helper/chipmunk/constraints/cpSlideJoint.h @@ -19,8 +19,12 @@ * SOFTWARE. */ +/// @defgroup cpSlideJoint cpSlideJoint +/// @{ + const cpConstraintClass *cpSlideJointGetClass(); +/// @private typedef struct cpSlideJoint { cpConstraint constraint; cpVect anchr1, anchr2; @@ -34,11 +38,16 @@ typedef struct cpSlideJoint { cpFloat bias; } cpSlideJoint; +/// Allocate a slide joint. cpSlideJoint *cpSlideJointAlloc(void); +/// Initialize a slide joint. cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); +/// Allocate and initialize a slide joint. cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1); CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2); CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min); CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max); + +/// @} diff --git a/src/helper/chipmunk/constraints/util.h b/src/helper/chipmunk/constraints/util.h index 8d402db63..eff16df61 100644 --- a/src/helper/chipmunk/constraints/util.h +++ b/src/helper/chipmunk/constraints/util.h @@ -25,13 +25,6 @@ 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; \ -} - 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 +38,17 @@ normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ return cpvdot(relative_velocity(a, b, r1, r2), n); } +static inline void +apply_impulse(cpBody *body, cpVect j, cpVect r){ + body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); + body->w += body->i_inv*cpvcross(r, j); +} + static inline void apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) { - cpBodyApplyImpulse(a, cpvneg(j), r1); - cpBodyApplyImpulse(b, j, r2); + apply_impulse(a, cpvneg(j), r1); + apply_impulse(b, j, r2); } static inline void @@ -66,13 +65,6 @@ apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) apply_bias_impulse(b, j, r2); } -static inline cpVect -clamp_vect(cpVect v, cpFloat len) -{ - return cpvclamp(v, len); -// return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; -} - static inline cpFloat k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) { @@ -128,3 +120,9 @@ mult_k(cpVect vr, cpVect k1, cpVect k2) { return cpv(cpvdot(vr, k1), cpvdot(vr, k2)); } + +static inline cpFloat +bias_coef(cpFloat errorBias, cpFloat dt) +{ + return 1.0f - cpfpow(errorBias, dt); +} diff --git a/src/helper/chipmunk/cpArbiter.c b/src/helper/chipmunk/cpArbiter.c index 3e4a52f61..ae45d9f71 100644 --- a/src/helper/chipmunk/cpArbiter.c +++ b/src/helper/chipmunk/cpArbiter.c @@ -20,13 +20,11 @@ */ #include +#include #include "chipmunk_private.h" #include "constraints/util.h" -cpFloat cp_collision_slop = 0.1; -cpFloat cp_bias_coef = 0.1; - cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash) { @@ -44,7 +42,49 @@ cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash } cpVect -cpArbiterTotalImpulse(cpArbiter *arb) +cpArbiterGetNormal(const cpArbiter *arb, int i) +{ + cpAssert(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + + cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); + return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n; +} + +cpVect +cpArbiterGetPoint(const cpArbiter *arb, int i) +{ + cpAssert(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); +} + +cpFloat +cpArbiterGetDepth(const cpArbiter *arb, int i) +{ + cpAssert(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); +} + +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; +} + + +cpVect +cpArbiterTotalImpulse(const cpArbiter *arb) { cpContact *contacts = arb->contacts; cpVect sum = cpvzero; @@ -58,7 +98,7 @@ cpArbiterTotalImpulse(cpArbiter *arb) } cpVect -cpArbiterTotalImpulseWithFriction(cpArbiter *arb) +cpArbiterTotalImpulseWithFriction(const cpArbiter *arb) { cpContact *contacts = arb->contacts; cpVect sum = cpvzero; @@ -71,23 +111,23 @@ cpArbiterTotalImpulseWithFriction(cpArbiter *arb) return sum; } -cpFloat -cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts) -{ - cpFloat fsum = 0.0f; - cpVect vsum = cpvzero; - - for(int i=0; in, cpv(con->jnAcc, con->jtAcc)); - - fsum += cpvlength(j); - vsum = cpvadd(vsum, j); - } - - cpFloat vmag = cpvlength(vsum); - return (1.0f - vmag/fsum); -} +//cpFloat +//cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts) +//{ +// cpFloat fsum = 0.0f; +// cpVect vsum = cpvzero; +// +// for(int i=0; in, cpv(con->jnAcc, con->jtAcc)); +// +// fsum += cpvlength(j); +// vsum = cpvadd(vsum, j); +// } +// +// cpFloat vmag = cpvlength(vsum); +// return (1.0f - vmag/fsum); +//} void cpArbiterIgnore(cpArbiter *arb) @@ -111,8 +151,8 @@ cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) arb->a = a; arb->body_a = a->body; arb->b = b; arb->body_b = b->body; - arb->nextA = NULL; - arb->nextB = NULL; + arb->next_a = NULL; + arb->next_b = NULL; arb->stamp = 0; arb->state = cpArbiterStateFirstColl; @@ -161,7 +201,7 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio } void -cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv, cpFloat slop, cpFloat bias) +cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias) { cpBody *a = arb->body_a; cpBody *b = arb->body_b; @@ -178,11 +218,11 @@ cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv, cpFloat slop, cpFloat bias) con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n)); // Calculate the target bias velocity. - con->bias = -bias*dt_inv*cpfmin(0.0f, con->dist + slop); + con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt; con->jBias = 0.0f; // Calculate the target bounce velocity. - con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e; + con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e; } } @@ -201,6 +241,8 @@ cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef) } } +// TODO is it worth splitting velocity/position correction? + void cpArbiterApplyImpulse(cpArbiter *arb) { diff --git a/src/helper/chipmunk/cpArbiter.h b/src/helper/chipmunk/cpArbiter.h index 45da59b23..136605b30 100644 --- a/src/helper/chipmunk/cpArbiter.h +++ b/src/helper/chipmunk/cpArbiter.h @@ -19,20 +19,25 @@ * SOFTWARE. */ -// Determines how fast penetrations resolve themselves expressed as a percentage per step. Defaults to 0.1. -extern cpFloat cp_bias_coef; +/// @defgroup cpArbiter cpArbiter +/// The cpArbiter struct controls pairs of colliding shapes. +/// They are also used in conjuction with collision handler callbacks +/// allowing you to retrieve information on the collision and control it. +/// @{ -// Amount of allowed penetration. Used to reduce oscillating contacts and keep the collision cache warm. Defaults to 0.1. -extern cpFloat cp_collision_slop; - - -// User collision handler function types. +/// Collision begin event function callback type. +/// Returning false from a begin callback causes the collision to be ignored until +/// the the separate callback is called when the objects stop colliding. typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, void *data); +/// Collision pre-solve event function callback type. +/// Returning false from a pre-step callback causes the collision to be ignored until the next step. typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); +/// Collision post-solve event function callback type. typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); +/// Collision separate event function callback type. typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, void *data); -// Structure for holding collision handler function callback information. +/// @private typedef struct cpCollisionHandler { cpCollisionType a; cpCollisionType b; @@ -43,69 +48,66 @@ typedef struct cpCollisionHandler { void *data; } cpCollisionHandler; -// Data structure for contact points. -typedef struct cpContact { - // Contact point and normal. - cpVect CP_PRIVATE(p), CP_PRIVATE(n); - // Penetration distance. - CP_PRIVATE(cpFloat dist); - - // Calculated by cpArbiterPreStep(). - cpVect CP_PRIVATE(r1), CP_PRIVATE(r2); - cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce); - - // Persistant contact information. - cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias); - CP_PRIVATE(cpFloat bias); - - // Hash value used to (mostly) uniquely identify a contact. - CP_PRIVATE(cpHashValue hash); -} cpContact; +typedef struct cpContact cpContact; #define CP_MAX_CONTACTS_PER_ARBITER 4 +/// @private typedef enum cpArbiterState { - cpArbiterStateNormal, + // Arbiter is active and its the first collision. cpArbiterStateFirstColl, + // Arbiter is active and its not the first collision. + cpArbiterStateNormal, + // Collision has been explicitly ignored. + // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). cpArbiterStateIgnore, + // Collison has separated, arbiter will be recyled soon. cpArbiterStateCached, } cpArbiterState; -// Data structure for tracking collisions between shapes. +/// A colliding pair of shapes. struct cpArbiter { - // Information on the contact points between the objects. + /// Calculated value to use for the elasticity coefficient. + /// Override in a pre-solve collision handler for custom behavior. + cpFloat e; + /// Calculated value to use for the friction coefficient. + /// Override in a pre-solve collision handler for custom behavior. + cpFloat u; + /// Calculated value to use for applying surface velocities. + /// Override in a pre-solve collision handler for custom behavior. + cpVect surface_vr; + + CP_PRIVATE(cpShape *a); + CP_PRIVATE(cpShape *b); + CP_PRIVATE(cpBody *body_a); + CP_PRIVATE(cpBody *body_b); + CP_PRIVATE(cpArbiter *next_a); + CP_PRIVATE(cpArbiter *next_b); + CP_PRIVATE(int numContacts); CP_PRIVATE(cpContact *contacts); - // The two shapes and bodies involved in the collision. - // These variables are NOT in the order defined by the collision handler. - cpShape CP_PRIVATE(*a), CP_PRIVATE(*b); - cpBody CP_PRIVATE(*body_a), CP_PRIVATE(*body_b); - cpArbiter CP_PRIVATE(*nextA), CP_PRIVATE(*nextB); - - // Calculated before calling the pre-solve collision handler - // Override them with custom values if you want specialized behavior - CP_PRIVATE(cpFloat e); - CP_PRIVATE(cpFloat u); - // Used for surface_v calculations, implementation may change - CP_PRIVATE(cpVect surface_vr); - - // Time stamp of the arbiter. (from cpSpace) CP_PRIVATE(cpTimestamp stamp); - CP_PRIVATE(cpCollisionHandler *handler); - - // Are the shapes swapped in relation to the collision handler? CP_PRIVATE(cpBool swappedColl); CP_PRIVATE(cpArbiterState state); }; -// Arbiter Helper Functions -cpVect cpArbiterTotalImpulse(cpArbiter *arb); -cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb); +/// Calculate the total impulse that was applied by this arbiter. +/// Calling this function from a begin or pre-solve callback is undefined. +cpVect cpArbiterTotalImpulse(const cpArbiter *arb); +/// Calculate the total impulse including the friction that was applied by this arbiter. +/// Calling this function from a begin or pre-solve callback is undefined. +cpVect cpArbiterTotalImpulseWithFriction(const cpArbiter *arb); + +/// Causes a collision pair to be ignored as if you returned false from a begin callback. +/// If called from a pre-step callback, you will still need to return false +/// if you want it to be ignored in the current step. void cpArbiterIgnore(cpArbiter *arb); - +/// Return the colliding shapes involved for this arbiter. +/// The order of their cpSpace.collision_type values will match +/// the order set when the collision handler was registered. static inline void cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) { @@ -115,8 +117,12 @@ cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) (*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b); } } +/// A macro shortcut for defining and retrieving the shapes from an arbiter. #define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b); +/// Return the colliding bodies involved for this arbiter. +/// The order of the cpSpace.collision_type the bodies are associated with values will match +/// the order set when the collision handler was registered. static inline void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) { @@ -124,60 +130,46 @@ cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) (*a) = shape_a->body; (*b) = shape_b->body; } +/// A macro shortcut for defining and retrieving the bodies from an arbiter. #define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b); +/// Returns true if this is the first step a pair of objects started colliding. static inline cpBool cpArbiterIsFirstContact(const cpArbiter *arb) { return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl; } +/// Get the number of contact points for this arbiter. static inline int cpArbiterGetCount(const cpArbiter *arb) { return arb->CP_PRIVATE(numContacts); } -static inline cpVect -cpArbiterGetNormal(const cpArbiter *arb, int i) -{ - cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); - return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n; -} - -static inline cpVect -cpArbiterGetPoint(const cpArbiter *arb, int i) -{ - return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); -} - -static inline cpFloat -cpArbiterGetDepth(const cpArbiter *arb, int i) -{ - return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); -} - +/// A struct that wraps up the important collision data for an arbiter. typedef struct cpContactPointSet { + /// The number of contact points in the set. int count; + /// The array of contact points. struct { - cpVect point, normal; + /// The position of the contact point. + cpVect point; + /// The normal of the contact point. + cpVect normal; + /// The depth of the contact point. cpFloat dist; } points[CP_MAX_CONTACTS_PER_ARBITER]; } cpContactPointSet; +/// Return a contact set from an arbiter. +cpContactPointSet cpArbiterGetContactPointSet(const cpArbiter *arb); -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; -} +/// Get the normal of the @c ith contact point. +cpVect cpArbiterGetNormal(const cpArbiter *arb, int i); +/// Get the position of the @c ith contact point. +cpVect cpArbiterGetPoint(const cpArbiter *arb, int i); +/// Get the depth of the @c ith contact point. +cpFloat cpArbiterGetDepth(const cpArbiter *arb, int i); + +/// @} diff --git a/src/helper/chipmunk/cpArray.c b/src/helper/chipmunk/cpArray.c index 787d62577..a4a4fd87e 100644 --- a/src/helper/chipmunk/cpArray.c +++ b/src/helper/chipmunk/cpArray.c @@ -25,40 +25,25 @@ #include "chipmunk_private.h" -cpArray* -cpArrayAlloc(void) -{ - return (cpArray *)cpcalloc(1, sizeof(cpArray)); -} - -cpArray* -cpArrayInit(cpArray *arr, int size) -{ - arr->num = 0; - arr->max = (size ? size : 4); - arr->arr = (void **)cpmalloc(arr->max*sizeof(void**)); - - return arr; -} - -cpArray* +cpArray * cpArrayNew(int size) { - return cpArrayInit(cpArrayAlloc(), size); -} - -void -cpArrayDestroy(cpArray *arr) -{ - cpfree(arr->arr); - arr->arr = NULL; + cpArray *arr = (cpArray *)cpcalloc(1, sizeof(cpArray)); + + arr->num = 0; + arr->max = (size ? size : 4); + arr->arr = (void **)cpcalloc(arr->max, sizeof(void**)); + + return arr; } void cpArrayFree(cpArray *arr) { if(arr){ - cpArrayDestroy(arr); + cpfree(arr->arr); + arr->arr = NULL; + cpfree(arr); } } diff --git a/src/helper/chipmunk/cpBB.h b/src/helper/chipmunk/cpBB.h index 300f8cd54..cb769407f 100644 --- a/src/helper/chipmunk/cpBB.h +++ b/src/helper/chipmunk/cpBB.h @@ -19,11 +19,16 @@ * SOFTWARE. */ -// TODO fix naming +/// @defgroup cpBBB cpBB +/// Chipmunk's axis-aligned 2D bounding box type along with a few handy routines. +/// @{ + +/// Chipmunk's axis-aligned 2D bounding box type. (left, bottom, right, top) typedef struct cpBB{ cpFloat l, b, r ,t; } cpBB; +/// Convenience constructor for cpBB structs. static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t) @@ -32,26 +37,30 @@ cpBBNew(const cpFloat l, const cpFloat b, return bb; } +/// Returns true if @c a and @c b intersect. static inline cpBool -cpBBintersects(const cpBB a, const cpBB b) +cpBBIntersects(const cpBB a, const cpBB b) { return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t); } +/// Returns true if @c other lies completely within @c bb. static inline cpBool -cpBBcontainsBB(const cpBB bb, const cpBB other) +cpBBContainsBB(const cpBB bb, const cpBB other) { return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t); } +/// Returns true if @c bb contains @c v. static inline cpBool -cpBBcontainsVect(const cpBB bb, const cpVect v) +cpBBContainsVect(const cpBB bb, const cpVect v) { return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y); } +/// Returns a bounding box that holds both bounding boxes. static inline cpBB -cpBBmerge(const cpBB a, const cpBB b){ +cpBBMerge(const cpBB a, const cpBB b){ return cpBBNew( cpfmin(a.l, b.l), cpfmin(a.b, b.b), @@ -60,8 +69,9 @@ cpBBmerge(const cpBB a, const cpBB b){ ); } +/// Returns a bounding box that holds both @c bb and @c v. static inline cpBB -cpBBexpand(const cpBB bb, const cpVect v){ +cpBBExpand(const cpBB bb, const cpVect v){ return cpBBNew( cpfmin(bb.l, v.x), cpfmin(bb.b, v.y), @@ -70,23 +80,26 @@ cpBBexpand(const cpBB bb, const cpVect v){ ); } +/// Returns the area of the bounding box. static inline cpFloat cpBBArea(cpBB bb) { return (bb.r - bb.l)*(bb.t - bb.b); } +/// Merges @c a and @c b and returns the area of the merged bounding box. static inline cpFloat cpBBMergedArea(cpBB a, cpBB b) { return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b)); } +/// Return true if the bounding box intersects the line segment with ends @c a and @c b. static inline cpBool cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b) { cpBB seg_bb = cpBBNew(cpfmin(a.x, b.x), cpfmin(a.y, b.y), cpfmax(a.x, b.x), cpfmax(a.y, b.y)); - if(cpBBintersects(bb, seg_bb)){ + if(cpBBIntersects(bb, seg_bb)){ cpVect axis = cpv(b.y - a.y, a.x - b.x); cpVect offset = cpv((a.x + b.x - bb.r - bb.l), (a.y + b.y - bb.t - bb.b)); cpVect extents = cpv(bb.r - bb.l, bb.t - bb.b); @@ -97,6 +110,11 @@ cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b) return cpFalse; } +/// Clamp a vector to a bounding box. cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox + // TODO edge case issue +/// Wrap a vector to a bounding box. cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox + +///@} diff --git a/src/helper/chipmunk/cpBBTree.c b/src/helper/chipmunk/cpBBTree.c index 76f642502..6ba93c656 100644 --- a/src/helper/chipmunk/cpBBTree.c +++ b/src/helper/chipmunk/cpBBTree.c @@ -1,9 +1,38 @@ +/* Copyright (c) 2009 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + #include "stdlib.h" #include "stdio.h" #include "chipmunk_private.h" -static cpSpatialIndexClass klass; +#ifdef _MSC_VER +// Are you freaking kidding me? +// Can it really not tell the difference between a declaration and a definition? +// Have I ever mentioned how much I hate MSVC? +extern +#else +static +#endif +cpSpatialIndexClass klass; typedef struct Node Node; typedef struct Pair Pair; @@ -11,14 +40,14 @@ typedef struct Pair Pair; struct cpBBTree { cpSpatialIndex spatialIndex; cpBBTreeVelocityFunc velocityFunc; - + cpHashSet *leaves; Node *root; - + Node *pooledNodes; Pair *pooledPairs; cpArray *allocatedBuffers; - + cpTimestamp stamp; }; @@ -26,11 +55,11 @@ struct Node { void *obj; cpBB bb; Node *parent; - + union { // Internal nodes struct { Node *a, *b; }; - + // Leaves struct { cpTimestamp stamp; @@ -59,13 +88,13 @@ static inline cpBB GetBB(cpBBTree *tree, void *obj) { cpBB bb = tree->spatialIndex.bbfunc(obj); - + cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc; if(velocityFunc){ cpFloat coef = 0.1f; cpFloat x = (bb.r - bb.l)*coef; cpFloat y = (bb.t - bb.b)*coef; - + cpVect v = cpvmult(velocityFunc(obj), 0.1f); return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y)); } else { @@ -115,7 +144,7 @@ static Pair * PairFromPool(cpBBTree *tree) { Pair *pair = tree->pooledPairs; - + if(pair){ tree->pooledPairs = pair->a.next; return pair; @@ -123,10 +152,10 @@ PairFromPool(cpBBTree *tree) // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(Pair); cpAssert(count, "Buffer size is too small."); - + Pair *buffer = (Pair *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(tree->allocatedBuffers, buffer); - + // push all but the first one, return the first instead for(int i=1; ia.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev; } - + if(prev){ if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; } else { @@ -155,7 +184,7 @@ PairsClear(Node *leaf, cpBBTree *tree) { Pair *pair = leaf->pairs; leaf->pairs = NULL; - + while(pair){ if(pair->a.leaf == leaf){ Pair *next = pair->a.next; @@ -177,14 +206,14 @@ PairInsert(Node *a, Node *b, cpBBTree *tree) Pair *nextA = a->pairs, *nextB = b->pairs; Pair *pair = PairFromPool(tree); Pair temp = {{NULL, a, nextA},{NULL, b, nextB}}; - + a->pairs = b->pairs = pair; *pair = temp; - + if(nextA){ if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair; } - + if(nextB){ if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair; } @@ -204,7 +233,7 @@ static Node * NodeFromPool(cpBBTree *tree) { Node *node = tree->pooledNodes; - + if(node){ tree->pooledNodes = node->parent; return node; @@ -212,10 +241,10 @@ NodeFromPool(cpBBTree *tree) // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(Node); cpAssert(count, "Buffer size is too small."); - + Node *buffer = (Node *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(tree->allocatedBuffers, buffer); - + // push all but the first one, return the first instead for(int i=1; iobj = NULL; - node->bb = cpBBmerge(a->bb, b->bb); + node->bb = cpBBMerge(a->bb, b->bb); node->parent = NULL; - + NodeSetA(node, a); NodeSetB(node, b); - + return node; } @@ -268,7 +297,7 @@ NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) { cpAssert(!NodeIsLeaf(parent), "Cannot replace child of a leaf."); cpAssert(child == parent->a || child == parent->b, "Node is not a child of parent."); - + if(parent->a == child){ NodeRecycle(tree, parent->a); NodeSetA(parent, value); @@ -276,9 +305,9 @@ NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) NodeRecycle(tree, parent->b); NodeSetB(parent, value); } - + for(Node *node=parent; node; node = node->parent){ - node->bb = cpBBmerge(node->a->bb, node->b->bb); + node->bb = cpBBMerge(node->a->bb, node->b->bb); } } @@ -294,25 +323,25 @@ SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) } else { cpFloat cost_a = cpBBArea(subtree->b->bb) + cpBBMergedArea(subtree->a->bb, leaf->bb); cpFloat cost_b = cpBBArea(subtree->a->bb) + cpBBMergedArea(subtree->b->bb, leaf->bb); - + // cpFloat cost_a = cpBBProximity(subtree->a->bb, leaf->bb); // cpFloat cost_b = cpBBProximity(subtree->b->bb, leaf->bb); - + if(cost_b < cost_a){ NodeSetB(subtree, SubtreeInsert(subtree->b, leaf, tree)); } else { NodeSetA(subtree, SubtreeInsert(subtree->a, leaf, tree)); } - - subtree->bb = cpBBmerge(subtree->bb, leaf->bb); + + subtree->bb = cpBBMerge(subtree->bb, leaf->bb); return subtree; } } static void -SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) { - if(cpBBintersects(subtree->bb, bb)){ + if(cpBBIntersects(subtree->bb, bb)){ if(NodeIsLeaf(subtree)){ func(obj, subtree->obj, data); } else { @@ -325,7 +354,7 @@ SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func // TODO Needs early exit optimization for ray queries static void -SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpSpatialIndexSegmentQueryCallback func, void *data) +SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpSpatialIndexSegmentQueryFunc func, void *data) { if(cpBBIntersectsSegment(subtree->bb, a, b)){ if(NodeIsLeaf(subtree)){ @@ -371,14 +400,14 @@ SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) typedef struct MarkContext { cpBBTree *tree; Node *staticRoot; - cpSpatialIndexQueryCallback func; + cpSpatialIndexQueryFunc func; void *data; } MarkContext; static void MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) { - if(cpBBintersects(leaf->bb, subtree->bb)){ + if(cpBBIntersects(leaf->bb, subtree->bb)){ if(NodeIsLeaf(subtree)){ if(left){ PairInsert(leaf, subtree, context->tree); @@ -400,7 +429,7 @@ MarkLeaf(Node *leaf, MarkContext *context) if(leaf->stamp == GetStamp(tree)){ Node *staticRoot = context->staticRoot; if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context); - + for(Node *node = leaf; node->parent; node = node->parent){ if(node == node->parent->a){ MarkLeafQuery(node->parent->b, leaf, cpTrue, context); @@ -440,11 +469,11 @@ LeafNew(cpBBTree *tree, void *obj, cpBB bb) Node *node = NodeFromPool(tree); node->obj = obj; node->bb = GetBB(tree, obj); - + node->parent = NULL; node->stamp = 0; node->pairs = NULL; - + return node; } @@ -453,38 +482,38 @@ LeafUpdate(Node *leaf, cpBBTree *tree) { Node *root = tree->root; cpBB bb = tree->spatialIndex.bbfunc(leaf->obj); - - if(!cpBBcontainsBB(leaf->bb, bb)){ + + if(!cpBBContainsBB(leaf->bb, bb)){ leaf->bb = GetBB(tree, leaf->obj); - + root = SubtreeRemove(root, leaf, tree); tree->root = SubtreeInsert(root, leaf, tree); - + PairsClear(leaf, tree); leaf->stamp = GetStamp(tree); - + return cpTrue; } - + return cpFalse; } -static void VoidQueryCallback(void *obj1, void *obj2, void *data){} +static void VoidQueryFunc(void *obj1, void *obj2, void *data){} static void LeafAddPairs(Node *leaf, cpBBTree *tree) { cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex; if(dynamicIndex){ - cpBBTree *dynamicTree = GetTree(dynamicIndex); - Node *dynamicRoot = dynamicTree->root; + Node *dynamicRoot = GetRootIfTree(dynamicIndex); if(dynamicRoot){ + cpBBTree *dynamicTree = GetTree(dynamicIndex); MarkContext context = {dynamicTree, NULL, NULL, NULL}; MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context); } } else { Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex); - MarkContext context = {tree, staticRoot, VoidQueryCallback, NULL}; + MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL}; MarkLeaf(leaf, &context); } } @@ -513,17 +542,17 @@ cpSpatialIndex * cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) { cpSpatialIndexInit((cpSpatialIndex *)tree, &klass, bbfunc, staticIndex); - + tree->velocityFunc = NULL; - - tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql, NULL); + + tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql); tree->root = NULL; - + tree->pooledNodes = NULL; tree->allocatedBuffers = cpArrayNew(0); - + tree->stamp = 0; - + return (cpSpatialIndex *)tree; } @@ -534,7 +563,7 @@ cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func) cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index."); return; } - + ((cpBBTree *)index)->velocityFunc = func; } @@ -548,7 +577,7 @@ static void cpBBTreeDestroy(cpBBTree *tree) { cpHashSetFree(tree->leaves); - + cpArrayFreeEach(tree->allocatedBuffers, cpfree); cpArrayFree(tree->allocatedBuffers); } @@ -558,11 +587,11 @@ cpBBTreeDestroy(cpBBTree *tree) static void cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) { - Node *leaf = cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans); - + Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans); + Node *root = tree->root; tree->root = SubtreeInsert(root, leaf, tree); - + leaf->stamp = GetStamp(tree); LeafAddPairs(leaf, tree); IncrementStamp(tree); @@ -571,8 +600,8 @@ cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) static void cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid) { - Node *leaf = cpHashSetRemove(tree->leaves, hashid, obj); - + Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj); + tree->root = SubtreeRemove(tree->root, leaf, tree); PairsClear(leaf, tree); NodeRecycle(tree, leaf); @@ -587,27 +616,27 @@ cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) #pragma mark Reindex static void -cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryCallback func, void *data) +cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data) { if(!tree->root) return; - + // LeafUpdate() may modify tree->root. Don't cache it. - cpHashSetEach(tree->leaves, (cpHashSetIterFunc)LeafUpdate, tree); - + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdate, tree); + cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex; Node *staticRoot = (staticIndex && staticIndex->klass == &klass ? ((cpBBTree *)staticIndex)->root : NULL); - + MarkContext context = {tree, staticRoot, func, data}; MarkSubtree(tree->root, &context); if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data); - + IncrementStamp(tree); } static void cpBBTreeReindex(cpBBTree *tree) { - cpBBTreeReindexQuery(tree, VoidQueryCallback, NULL); + cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL); } static void @@ -623,21 +652,21 @@ cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) #pragma mark Query static void -cpBBTreePointQuery(cpBBTree *tree, cpVect point, cpSpatialIndexQueryCallback func, void *data) +cpBBTreePointQuery(cpBBTree *tree, cpVect point, cpSpatialIndexQueryFunc func, void *data) { Node *root = tree->root; if(root) SubtreeQuery(root, &point, cpBBNew(point.x, point.y, point.x, point.y), func, data); } static void -cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { Node *root = tree->root; if(root) SubtreeSegmentQuery(root, obj, a, b, func, data); } static void -cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) { if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); } @@ -651,36 +680,36 @@ cpBBTreeCount(cpBBTree *tree) } typedef struct eachContext { - cpSpatialIndexIterator func; + cpSpatialIndexIteratorFunc func; void *data; } eachContext; static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);} static void -cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIterator func, void *data) +cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data) { eachContext context = {func, data}; - cpHashSetEach(tree->leaves, (cpHashSetIterFunc)each_helper, &context); + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context); } static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyFunc)cpBBTreeDestroy, - - (cpSpatialIndexCountFunc)cpBBTreeCount, - (cpSpatialIndexEachFunc)cpBBTreeEach, - - (cpSpatialIndexContainsFunc)cpBBTreeContains, - (cpSpatialIndexInsertFunc)cpBBTreeInsert, - (cpSpatialIndexRemoveFunc)cpBBTreeRemove, - - (cpSpatialIndexReindexFunc)cpBBTreeReindex, - (cpSpatialIndexReindexObjectFunc)cpBBTreeReindexObject, - (cpSpatialIndexReindexQueryFunc)cpBBTreeReindexQuery, - - (cpSpatialIndexPointQueryFunc)cpBBTreePointQuery, - (cpSpatialIndexSegmentQueryFunc)cpBBTreeSegmentQuery, - (cpSpatialIndexQueryFunc)cpBBTreeQuery, + (cpSpatialIndexDestroyImpl)cpBBTreeDestroy, + + (cpSpatialIndexCountImpl)cpBBTreeCount, + (cpSpatialIndexEachImpl)cpBBTreeEach, + + (cpSpatialIndexContainsImpl)cpBBTreeContains, + (cpSpatialIndexInsertImpl)cpBBTreeInsert, + (cpSpatialIndexRemoveImpl)cpBBTreeRemove, + + (cpSpatialIndexReindexImpl)cpBBTreeReindex, + (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject, + (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery, + + (cpSpatialIndexPointQueryImpl)cpBBTreePointQuery, + (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, + (cpSpatialIndexQueryImpl)cpBBTreeQuery, }; #pragma mark Tree Optimization @@ -704,16 +733,16 @@ partitionNodes(cpBBTree *tree, Node **nodes, int count) } else if(count == 2) { return NodeNew(tree, nodes[0], nodes[1]); } - + // Find the AABB for these nodes cpBB bb = nodes[0]->bb; - for(int i=1; ibb); - + for(int i=1; ibb); + // Split it on it's longest axis cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); - + // Sort the bounds and use the median as the splitting point - cpFloat bounds[count*2]; + cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat)); if(splitWidth){ for(int i=0; ibb.l; @@ -725,14 +754,15 @@ partitionNodes(cpBBTree *tree, Node **nodes, int count) bounds[2*i + 1] = nodes[i]->bb.t; } } - + qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split + cpfree(bounds); // Generate the child BBs cpBB a = bb, b = bb; if(splitWidth) a.r = b.l = split; else a.t = b.b = split; - + // Partition the nodes int right = count; for(int left=0; left < right;){ @@ -746,13 +776,13 @@ partitionNodes(cpBBTree *tree, Node **nodes, int count) left++; } } - + if(right == count){ Node *node = NULL; for(int i=0; iopath; -// +// // while(!NodeIsLeaf(node)){ // node = (path&(1<a : node->b); // bit = (bit + 1)&(sizeof(unsigned int)*8 - 1); // } -// +// // root = subtreeRemove(root, node, tree); // tree->root = subtreeInsert(root, node, tree); // } @@ -786,26 +816,29 @@ cpBBTreeOptimize(cpSpatialIndex *index) cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index."); return; } - + cpBBTree *tree = (cpBBTree *)index; Node *root = tree->root; if(!root) return; - + int count = cpBBTreeCount(tree); - Node *nodes[count]; + Node **nodes = (Node **)cpcalloc(count, sizeof(Node *)); Node **cursor = nodes; - - cpHashSetEach(tree->leaves, (cpHashSetIterFunc)fillNodeArray, &cursor); - + + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor); + SubtreeRecycle(tree, root); tree->root = partitionNodes(tree, nodes, count); + cpfree(nodes); } #pragma mark Debug Draw -#define CP_BBTREE_DEBUG_DRAW +//#define CP_BBTREE_DEBUG_DRAW #ifdef CP_BBTREE_DEBUG_DRAW #include +#include +#include static void NodeRender(Node *node, int depth) @@ -814,22 +847,22 @@ NodeRender(Node *node, int depth) NodeRender(node->a, depth + 1); NodeRender(node->b, depth + 1); } - + cpBB bb = node->bb; - -// GLfloat v = depth/2.0f; + +// GLfloat v = depth/2.0f; // glColor3f(1.0f - v, v, 0.0f); glLineWidth(cpfmax(5.0f - depth, 1.0f)); glBegin(GL_LINES); { glVertex2f(bb.l, bb.b); glVertex2f(bb.l, bb.t); - + glVertex2f(bb.l, bb.t); glVertex2f(bb.r, bb.t); - + glVertex2f(bb.r, bb.t); glVertex2f(bb.r, bb.b); - + glVertex2f(bb.r, bb.b); glVertex2f(bb.l, bb.b); }; glEnd(); @@ -841,7 +874,7 @@ cpBBTreeRenderDebug(cpSpatialIndex *index){ cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index."); return; } - + cpBBTree *tree = (cpBBTree *)index; if(tree->root) NodeRender(tree->root, 0); } diff --git a/src/helper/chipmunk/cpBody.c b/src/helper/chipmunk/cpBody.c index e18e308f7..4c9be6427 100644 --- a/src/helper/chipmunk/cpBody.c +++ b/src/helper/chipmunk/cpBody.c @@ -24,6 +24,7 @@ #include #include "chipmunk_private.h" +#include "constraints/util.h" // initialized in cpInitChipmunk() cpBody cpStaticBodySingleton; @@ -34,14 +35,18 @@ cpBodyAlloc(void) return (cpBody *)cpmalloc(sizeof(cpBody)); } -cpBodyVelocityFunc cpBodyUpdateVelocityDefault = cpBodyUpdateVelocity; -cpBodyPositionFunc cpBodyUpdatePositionDefault = cpBodyUpdatePosition; - cpBody * cpBodyInit(cpBody *body, cpFloat m, cpFloat i) { - body->velocity_func = cpBodyUpdateVelocityDefault; - body->position_func = cpBodyUpdatePositionDefault; + bzero(body, sizeof(cpBody)); + + body->space = NULL; + body->shapeList = NULL; + body->arbiterList = NULL; + body->constraintList = NULL; + + body->velocity_func = cpBodyUpdateVelocity; + body->position_func = cpBodyUpdatePosition; cpBodySetMass(body, m); cpBodySetMoment(body, i); @@ -57,18 +62,14 @@ cpBodyInit(cpBody *body, cpFloat m, cpFloat i) body->v_bias = cpvzero; body->w_bias = 0.0f; - body->data = NULL; body->v_limit = (cpFloat)INFINITY; body->w_limit = (cpFloat)INFINITY; - body->space = NULL; - body->shapeList = NULL; - body->arbiterList = NULL; - body->constraintList = NULL; - cpComponentNode node = {NULL, NULL, 0.0f}; body->node = node; + body->data = NULL; + return body; } @@ -104,9 +105,34 @@ cpBodyFree(cpBody *body) } } +static void cpv_assert_nan(cpVect v, char *message){cpAssert(v.x == v.x && v.y == v.y, message);} +static void cpv_assert_infinite(cpVect v, char *message){cpAssert(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);} +static void cpv_assert_sane(cpVect v, char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);} + +void +cpBodySanityCheck(cpBody *body) +{ + cpAssert(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is invalid."); + cpAssert(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is invalid."); + + cpv_assert_sane(body->p, "Body's position is invalid."); + cpv_assert_sane(body->v, "Body's velocity is invalid."); + cpv_assert_sane(body->f, "Body's force is invalid."); + + cpAssert(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid."); + cpAssert(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid."); + cpAssert(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid."); + + cpv_assert_sane(body->rot, "Internal error: Body's rotation vector is invalid."); + + cpAssert(body->v_limit == body->v_limit, "Body's velocity limit is invalid."); + cpAssert(body->w_limit == body->w_limit, "Body's angular velocity limit is invalid."); +} + void cpBodySetMass(cpBody *body, cpFloat mass) { + cpBodyActivate(body); body->m = mass; body->m_inv = 1.0f/mass; } @@ -114,24 +140,52 @@ cpBodySetMass(cpBody *body, cpFloat mass) void cpBodySetMoment(cpBody *body, cpFloat moment) { + cpBodyActivate(body); body->i = moment; body->i_inv = 1.0f/moment; } +static inline void +updateShapes(cpBody *body){ + cpSpace *space = body->space; + + if(space){ + CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape); + } +} + void -cpBodySetAngle(cpBody *body, cpFloat angle) +cpBodySetPos(cpBody *body, cpVect pos) +{ + cpBodyActivate(body); + cpBodyAssertSane(body); + updateShapes(body); + body->p = pos; +} + +static inline void +setAngle(cpBody *body, cpFloat angle) { body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f); body->rot = cpvforangle(angle); } void -cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) +cpBodySetAngle(cpBody *body, cpFloat angle) { - cpVect delta = cpvsub(pos, body->p); - body->v = cpvmult(delta, 1.0f/dt); + cpBodyActivate(body); + cpBodyAssertSane(body); + updateShapes(body); + setAngle(body, angle); } +//void +//cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) +//{ +// cpBodyActivate(body); +// body->v = cpvmult(cpvsub(pos, body->p), 1.0f/dt); +//} + void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) { @@ -140,16 +194,20 @@ cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) cpFloat w_limit = body->w_limit; body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit); + + cpBodySanityCheck(body); } void cpBodyUpdatePosition(cpBody *body, cpFloat dt) { body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); - cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt); + setAngle(body, body->a + (body->w + body->w_bias)*dt); body->v_bias = cpvzero; body->w_bias = 0.0f; + + cpBodySanityCheck(body); } void @@ -167,23 +225,29 @@ cpBodyApplyForce(cpBody *body, cpVect force, cpVect r) } void -cpBodyEachArbiter(cpBody *body, cpBodyArbiterIterator func, void *data) +cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r) { - CP_BODY_FOREACH_ARBITER(body, arb) func(body, arb, data); + cpBodyActivate(body); + apply_impulse(body, j, r); } -cpBool -cpBodyIsGroundedTolerance(cpBody *body, cpVect n, cpFloat tol) +void +cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data) +{ + CP_BODY_FOREACH_SHAPE(body, shape) func(body, shape, data); +} + +void +cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data) +{ + CP_BODY_FOREACH_CONSTRAINT(body,constraint) func(body, constraint, data); +} + +void +cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data) { CP_BODY_FOREACH_ARBITER(body, arb){ - if(cpvdot(arb->contacts[0].n, n) < tol) return cpTrue; + arb->swappedColl = (body == arb->body_b); + func(body, arb, data); } - - return cpFalse; -} - -cpBool -cpBodyIsGrounded(cpBody *body) -{ - return cpBodyIsGroundedTolerance(body, cpv(0.0f, 1.0f), 0.0f); } diff --git a/src/helper/chipmunk/cpBody.h b/src/helper/chipmunk/cpBody.h index 64d8c3d39..0d1ba98dd 100644 --- a/src/helper/chipmunk/cpBody.h +++ b/src/helper/chipmunk/cpBody.h @@ -19,113 +19,134 @@ * SOFTWARE. */ +/// @defgroup cpBody cpBody +/// Chipmunk's rigid body type. Rigid bodies hold the physical properties of an object like +/// it's mass, and position and velocity of it's center of gravity. They don't have an shape on their own. +/// They are given a shape by creating collision shapes (cpShape) that point to the body. +/// @{ + +/// Rigid body velocity update function type. typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +/// Rigid body position update function type. typedef void (*cpBodyPositionFunc)(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. +/// Used internally to track information on the collision graph. +/// @private typedef struct cpComponentNode { cpBody *root; cpBody *next; cpFloat idleTime; } cpComponentNode; +/// Chipmunk's rigid body struct. struct cpBody { - // *** Integration Functions. - - // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) + /// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) cpBodyVelocityFunc velocity_func; - // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) + /// Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) cpBodyPositionFunc position_func; - // *** Mass Properties + /// Mass of the body. + /// Must agree with cpBody.m_inv! Use cpBodySetMass() when changing the mass for this reason. + cpFloat m; + /// Mass inverse. + cpFloat m_inv; - // Mass and it's inverse. - // Always use cpBodySetMass() whenever changing the mass as these values must agree. - cpFloat m, m_inv; + /// Moment of inertia of the body. + /// Must agree with cpBody.i_inv! Use cpBodySetMoment() when changing the moment for this reason. + cpFloat i; + /// Moment of inertia inverse. + cpFloat i_inv; - // Moment of inertia and it's inverse. - // Always use cpBodySetMoment() whenever changing the moment as these values must agree. - cpFloat i, i_inv; + /// Position of the rigid body's center of gravity. + cpVect p; + /// Velocity of the rigid body's center of gravity. + cpVect v; + /// Force acting on the rigid body's center of gravity. + cpVect f; - // *** Positional Properties + /// Rotation of the body around it's center of gravity in radians. + /// Must agree with cpBody.rot! Use cpBodySetAngle() when changing the angle for this reason. + cpFloat a; + /// Angular velocity of the body around it's center of gravity in radians/second. + cpFloat w; + /// Torque applied to the body around it's center of gravity. + cpFloat t; - // Linear components of motion (position, velocity, and force) - cpVect p, v, f; - - // Angular components of motion (angle, angular velocity, and torque) - // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. - cpFloat a, w, t; - - // Cached unit length vector representing the angle of the body. - // Used for fast vector rotation using cpvrotate(). + /// Cached unit length vector representing the angle of the body. + /// Used for fast rotations using cpvrotate(). cpVect rot; - // *** User Definable Fields - - // User defined data pointer. + /// User definable data pointer. + /// Generally this points to your the game object class so you can access it + /// when given a cpBody reference in a callback. cpDataPointer data; - // *** Other Fields + /// Maximum velocity allowed when updating the velocity. + cpFloat v_limit; + /// Maximum rotational rate (in radians/second) allowed when updating the angular velocity. + cpFloat w_limit; - // 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. CP_PRIVATE(cpVect v_bias); CP_PRIVATE(cpFloat w_bias); - // Space this body has been added to CP_PRIVATE(cpSpace *space); - // Pointer to the shape list. - // Shapes form a linked list using cpShape.next when added to a space. CP_PRIVATE(cpShape *shapeList); CP_PRIVATE(cpArbiter *arbiterList); CP_PRIVATE(cpConstraint *constraintList); - // Used by cpSpaceStep() to store contact graph information. CP_PRIVATE(cpComponentNode node); }; -// Basic allocation/destruction functions +/// Allocate a cpBody. cpBody *cpBodyAlloc(void); +/// Initialize a cpBody. cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); +/// Allocate and initialize a cpBody. cpBody *cpBodyNew(cpFloat m, cpFloat i); +/// Initialize a static cpBody. cpBody *cpBodyInitStatic(cpBody *body); +/// Allocate and initialize a static cpBody. cpBody *cpBodyNewStatic(); +/// Destroy a cpBody. void cpBodyDestroy(cpBody *body); +/// Destroy and free a cpBody. void cpBodyFree(cpBody *body); -// Wake up a sleeping or idle body. (defined in cpSpace.c) -void cpBodyActivate(cpBody *body); +/// Check that the properties of a body is sane. (Only in debug mode) +#ifdef NDEBUG + #define cpBodyAssertSane(body) +#else + void cpBodySanityCheck(cpBody *body); + #define cpBodyAssertSane(body) cpBodySanityCheck(body) +#endif -// Force a body to sleep; -// defined in cpSpaceComponent.c +// Defined in cpSpace.c +/// Wake up a sleeping or idle body. +void cpBodyActivate(cpBody *body); +/// Force a body to fall asleep immediately. void cpBodySleep(cpBody *body); +/// Force a body to fall asleep immediately along with other bodies in a group. void cpBodySleepWithGroup(cpBody *body, cpBody *group); +/// Returns true if the body is sleeping. static inline cpBool cpBodyIsSleeping(const cpBody *body) { return (CP_PRIVATE(body->node).root != ((cpBody*)0)); } +/// Returns true if the body is static. static inline cpBool cpBodyIsStatic(const cpBody *body) { return CP_PRIVATE(body->node).idleTime == INFINITY; } +/// Returns true if the body has not been added to a space. static inline cpBool cpBodyIsRogue(const cpBody *body) { @@ -133,75 +154,75 @@ cpBodyIsRogue(const cpBody *body) } -#define CP_DefineBodyGetter(type, member, name) \ +#define CP_DefineBodyStructGetter(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){ \ +#define CP_DefineBodyStructSetter(type, member, name) \ +static inline void cpBodySet##name(cpBody *body, const type value){ \ cpBodyActivate(body); \ + cpBodyAssertSane(body); \ body->member = value; \ -} \ +} -#define CP_DefineBodyProperty(type, member, name) \ -CP_DefineBodyGetter(type, member, name) \ -CP_DefineBodySetter(type, member, name) +#define CP_DefineBodyStructProperty(type, member, name) \ +CP_DefineBodyStructGetter(type, member, name) \ +CP_DefineBodyStructSetter(type, member, name) - -// Accessors for cpBody struct members -CP_DefineBodyGetter(cpFloat, m, Mass); +/// TODO need to document these somehow. +CP_DefineBodyStructGetter(cpFloat, m, Mass); +/// Set the mass of a body. void cpBodySetMass(cpBody *body, cpFloat m); -CP_DefineBodyGetter(cpFloat, i, Moment); +CP_DefineBodyStructGetter(cpFloat, i, Moment); +/// Set the moment of a body. void cpBodySetMoment(cpBody *body, cpFloat i); - -CP_DefineBodyProperty(cpVect, p, Pos); -CP_DefineBodyProperty(cpVect, v, Vel); -CP_DefineBodyProperty(cpVect, f, Force); -CP_DefineBodyGetter(cpFloat, a, Angle); +CP_DefineBodyStructGetter(cpVect, p, Pos); +/// Set the position of a body. +void cpBodySetPos(cpBody *body, cpVect pos); +CP_DefineBodyStructProperty(cpVect, v, Vel); +CP_DefineBodyStructProperty(cpVect, f, Force); +CP_DefineBodyStructGetter(cpFloat, a, Angle); +/// Set the angle of a body. void cpBodySetAngle(cpBody *body, cpFloat a); -CP_DefineBodyProperty(cpFloat, w, AngVel); -CP_DefineBodyProperty(cpFloat, t, Torque); -CP_DefineBodyGetter(cpVect, rot, Rot); -CP_DefineBodyProperty(cpFloat, v_limit, VelLimit); -CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit); +CP_DefineBodyStructProperty(cpFloat, w, AngVel); +CP_DefineBodyStructProperty(cpFloat, t, Torque); +CP_DefineBodyStructGetter(cpVect, rot, Rot); +CP_DefineBodyStructProperty(cpFloat, v_limit, VelLimit); +CP_DefineBodyStructProperty(cpFloat, w_limit, AngVelLimit); +CP_DefineBodyStructProperty(cpDataPointer, data, Data); -// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. -// Intended for objects that are moved manually with a custom velocity integration function. -void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); +/// Return the user data pointer for a body. +static inline cpDataPointer cpBodyGetUserData(const cpBody *body){return body->data;} +/// Set the user data pointer for a body. +static inline void cpBodySetUserData(cpBody *body, cpDataPointer data){body->data = data;} -// Default Integration functions. +/// Default Integration functions. void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); void cpBodyUpdatePosition(cpBody *body, cpFloat dt); -// Convert body local to world coordinates +/// Convert body relative/local coordinates to absolute/world coordinates. static inline cpVect cpBodyLocal2World(const cpBody *body, const cpVect v) { return cpvadd(body->p, cpvrotate(v, body->rot)); } -// Convert world to body local coordinates +/// Convert body absolute/world coordinates to relative/local coordinates. static inline cpVect cpBodyWorld2Local(const cpBody *body, const cpVect v) { return cpvunrotate(cpvsub(v, body->p), body->rot); } -// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). -static inline void -cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r) -{ - body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); - body->w += body->i_inv*cpvcross(r, j); -} - -// Zero the forces on a body. +/// Set the forces and torque or a body to zero. 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). +/// Apply an force (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); +/// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). +void cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r); +/// Get the kinetic energy of a body. static inline cpFloat cpBodyKineticEnergy(const cpBody *body) { @@ -211,8 +232,19 @@ cpBodyKineticEnergy(const cpBody *body) return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f); } -typedef void (*cpBodyArbiterIterator)(cpBody *body, cpArbiter *arbiter, void *data); -void cpBodyEachArbiter(cpBody *body, cpBodyArbiterIterator func, void *data); +/// Body/shape iterator callback function type. +typedef void (*cpBodyShapeIteratorFunc)(cpBody *body, cpShape *shape, void *data); +/// Call @c func once for each shape attached to @c body and added to the space. +void cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data); -cpBool cpBodyIsGrounded(cpBody *body); -cpBool cpBodyIsGroundedTolerance(cpBody *body, cpVect normal, cpFloat tolerance); +/// Body/constraint iterator callback function type. +typedef void (*cpBodyConstraintIteratorFunc)(cpBody *body, cpConstraint *constraint, void *data); +/// Call @c func once for each constraint attached to @c body and added to the space. +void cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data); + +/// Body/arbiter iterator callback function type. +typedef void (*cpBodyArbiterIteratorFunc)(cpBody *body, cpArbiter *arbiter, void *data); +/// Call @c func once for each arbiter that is currently active on the body. +void cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data); + +///@} diff --git a/src/helper/chipmunk/cpCollision.c b/src/helper/chipmunk/cpCollision.c index 1fb6deda6..d51107311 100644 --- a/src/helper/chipmunk/cpCollision.c +++ b/src/helper/chipmunk/cpCollision.c @@ -55,7 +55,7 @@ circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpF static int circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { - cpCircleShape *circ1 = (cpCircleShape *)shape1; + cpCircleShape *circ1 = (cpCircleShape *)shape1; //TODO cpCircleShape *circ2 = (cpCircleShape *)shape2; return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); @@ -283,6 +283,7 @@ seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) // Floating point precision problems here. // This will have to do for now. // poly_min -= cp_collision_slop; // TODO is this needed anymore? + if(minNorm >= poly_min || minNeg >= poly_min) { if(minNorm > minNeg) findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f); diff --git a/src/helper/chipmunk/cpHashSet.c b/src/helper/chipmunk/cpHashSet.c index dc2d4d4c1..1fb2d10bd 100644 --- a/src/helper/chipmunk/cpHashSet.c +++ b/src/helper/chipmunk/cpHashSet.c @@ -43,38 +43,29 @@ struct cpHashSet { cpArray *allocatedBuffers; }; -void -cpHashSetDestroy(cpHashSet *set) -{ - cpfree(set->table); - - cpArrayFreeEach(set->allocatedBuffers, cpfree); - cpArrayFree(set->allocatedBuffers); -} - void cpHashSetFree(cpHashSet *set) { if(set){ - cpHashSetDestroy(set); + cpfree(set->table); + + cpArrayFreeEach(set->allocatedBuffers, cpfree); + cpArrayFree(set->allocatedBuffers); + cpfree(set); } } cpHashSet * -cpHashSetAlloc(void) -{ - return (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); -} - -cpHashSet * -cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *default_value) +cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc) { + cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); + set->size = next_prime(size); set->entries = 0; set->eql = eqlFunc; - set->default_value = default_value; + set->default_value = NULL; set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *)); set->pooledBins = NULL; @@ -84,10 +75,10 @@ cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *default_ return set; } -cpHashSet * -cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *default_value) +void +cpHashSetSetDefaultValue(cpHashSet *set, void *default_value) { - return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, default_value); + set->default_value = default_value; } static int @@ -228,7 +219,7 @@ cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) } void -cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data) +cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) { for(int i=0; isize; i++){ cpHashSetBin *bin = set->table[i]; diff --git a/src/helper/chipmunk/cpPolyShape.c b/src/helper/chipmunk/cpPolyShape.c index ac6d337f5..6b627e0de 100644 --- a/src/helper/chipmunk/cpPolyShape.c +++ b/src/helper/chipmunk/cpPolyShape.c @@ -66,12 +66,10 @@ cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) } static cpBB -cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +cpPolyShapeCacheData(cpPolyShape *poly, cpVect p, cpVect rot) { - cpPolyShape *poly = (cpPolyShape *)shape; - cpPolyShapeTransformAxes(poly, p, rot); - cpBB bb = shape->bb = cpPolyShapeTransformVerts(poly, p, rot); + cpBB bb = poly->shape.bb = cpPolyShapeTransformVerts(poly, p, rot); return bb; } @@ -89,14 +87,13 @@ cpPolyShapeDestroy(cpShape *shape) } static cpBool -cpPolyShapePointQuery(cpShape *shape, cpVect p){ - return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p); +cpPolyShapePointQuery(cpPolyShape *poly, cpVect p){ + return cpBBContainsVect(poly->shape.bb, p) && cpPolyShapeContainsVert(poly, p); } static void -cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info) { - cpPolyShape *poly = (cpPolyShape *)shape; cpPolyShapeAxis *axes = poly->tAxes; cpVect *verts = poly->tVerts; int numVerts = poly->numVerts; @@ -116,7 +113,7 @@ cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo * cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]); if(dtMin <= dt && dt <= dtMax){ - info->shape = shape; + info->shape = (cpShape *)poly; info->t = t; info->n = n; } @@ -125,10 +122,10 @@ cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo * static const cpShapeClass polyClass = { CP_POLY_SHAPE, - cpPolyShapeCacheData, - cpPolyShapeDestroy, - cpPolyShapePointQuery, - cpPolyShapeSegmentQuery, + (cpShapeCacheDataImpl)cpPolyShapeCacheData, + (cpShapeDestroyImpl)cpPolyShapeDestroy, + (cpShapePointQueryImpl)cpPolyShapePointQuery, + (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery, }; cpBool diff --git a/src/helper/chipmunk/cpPolyShape.h b/src/helper/chipmunk/cpPolyShape.h index 0bde85eeb..a59b03fd6 100644 --- a/src/helper/chipmunk/cpPolyShape.h +++ b/src/helper/chipmunk/cpPolyShape.h @@ -19,38 +19,44 @@ * SOFTWARE. */ -// Axis structure used by cpPolyShape. +/// @defgroup cpPolyShape cpPolyShape +/// @{ + +/// @private typedef struct cpPolyShapeAxis { - // normal cpVect n; - // distance from origin cpFloat d; } cpPolyShapeAxis; -// Convex polygon shape structure. +/// @private typedef struct cpPolyShape { cpShape shape; - // Vertex and axis lists. - CP_PRIVATE(int numVerts); - CP_PRIVATE(cpVect *verts); - CP_PRIVATE(cpPolyShapeAxis *axes); - - // Transformed vertex and axis lists. - CP_PRIVATE(cpVect *tVerts); - CP_PRIVATE(cpPolyShapeAxis *tAxes); + int numVerts; + cpVect *verts, *tVerts; + cpPolyShapeAxis *axes, *tAxes; } cpPolyShape; -// Basic allocation functions. +/// Allocate a polygon shape. cpPolyShape *cpPolyShapeAlloc(void); +/// Initialize a polygon shape. +/// The vertexes must be convex and have a clockwise winding. cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset); +/// Allocate and initialize a polygon shape. +/// The vertexes must be convex and have a clockwise winding. cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset); +/// Initialize a box shaped polygon shape. cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height); +/// Allocate and initialize a box shaped polygon shape. cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); -// Check that a set of vertexes has a correct winding and that they are convex +/// Check that a set of vertexes is convex and has a clockwise winding. cpBool cpPolyValidate(const cpVect *verts, const int numVerts); +/// Get the number of verts in a polygon shape. int cpPolyShapeGetNumVerts(cpShape *shape); +/// Get the @c ith vertex of a polygon shape. cpVect cpPolyShapeGetVert(cpShape *shape, int idx); + +/// @} diff --git a/src/helper/chipmunk/cpShape.c b/src/helper/chipmunk/cpShape.c index a16370a91..bb5bcb1d3 100644 --- a/src/helper/chipmunk/cpShape.c +++ b/src/helper/chipmunk/cpShape.c @@ -31,12 +31,13 @@ CP_DeclareShapeGetter(struct, type, name){ \ cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \ return ((struct *)shape)->member; \ } -cpHashValue SHAPE_ID_COUNTER = 0; + +static cpHashValue cpShapeIDCounter = 0; void cpResetShapeIdCounter(void) { - SHAPE_ID_COUNTER = 0; + cpShapeIDCounter = 0; } @@ -45,8 +46,8 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) { shape->klass = klass; - shape->hashid = SHAPE_ID_COUNTER; - SHAPE_ID_COUNTER++; + shape->hashid = cpShapeIDCounter; + cpShapeIDCounter++; shape->body = body; shape->sensor = 0; @@ -62,8 +63,6 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) shape->data = NULL; shape->next = NULL; -// cpShapeCacheBB(shape); - return shape; } @@ -115,24 +114,16 @@ cpCircleShapeAlloc(void) return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape)); } -static inline cpBB -bbFromCircle(const cpVect c, const cpFloat r) +static cpBB +cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot) { + cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); + cpFloat r = circle->r; return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r); } -static cpBB -cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) -{ - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); - return bbFromCircle(circle->tc, circle->r); -} - static cpBool -cpCircleShapePointQuery(cpShape *shape, cpVect p){ - cpCircleShape *circle = (cpCircleShape *)shape; +cpCircleShapePointQuery(cpCircleShape *circle, cpVect p){ return cpvnear(circle->tc, p, circle->r); } @@ -160,18 +151,17 @@ circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, } static void -cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpSegmentQueryInfo *info) { - cpCircleShape *circle = (cpCircleShape *)shape; - circleSegmentQuery(shape, circle->tc, circle->r, a, b, info); + circleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, info); } static const cpShapeClass cpCircleShapeClass = { CP_CIRCLE_SHAPE, - cpCircleShapeCacheData, + (cpShapeCacheDataImpl)cpCircleShapeCacheData, NULL, - cpCircleShapePointQuery, - cpCircleShapeSegmentQuery, + (cpShapePointQueryImpl)cpCircleShapePointQuery, + (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery, }; cpCircleShape * @@ -201,15 +191,13 @@ cpSegmentShapeAlloc(void) } static cpBB -cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot) { - cpSegmentShape *seg = (cpSegmentShape *)shape; - seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); seg->tn = cpvrotate(seg->n, rot); - cpFloat l,r,s,t; + cpFloat l,r,b,t; if(seg->ta.x < seg->tb.x){ l = seg->ta.x; @@ -220,22 +208,20 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) } if(seg->ta.y < seg->tb.y){ - s = seg->ta.y; + b = seg->ta.y; t = seg->tb.y; } else { - s = seg->tb.y; + b = seg->tb.y; t = seg->ta.y; } cpFloat rad = seg->r; - return cpBBNew(l - rad, s - rad, r + rad, t + rad); + return cpBBNew(l - rad, b - rad, r + rad, t + rad); } static cpBool -cpSegmentShapePointQuery(cpShape *shape, cpVect p){ - if(!cpBBcontainsVect(shape->bb, p)) return cpFalse; - - cpSegmentShape *seg = (cpSegmentShape *)shape; +cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p){ + if(!cpBBContainsVect(seg->shape.bb, p)) return cpFalse; // Calculate normal distance from segment. cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); @@ -272,11 +258,8 @@ cpSegmentShapePointQuery(cpShape *shape, cpVect p){ static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);} static void -cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpSegmentQueryInfo *info) { - // TODO this function could be optimized better. - - cpSegmentShape *seg = (cpSegmentShape *)shape; cpVect n = seg->tn; // flip n if a is behind the axis if(cpvdot(a, n) < cpvdot(seg->ta, n)) @@ -296,7 +279,7 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf cpFloat dtMax = -cpvcross(seg->tn, seg->tb); if(dtMin < dt && dt < dtMax){ - info->shape = shape; + info->shape = (cpShape *)seg; info->t = t; info->n = n; @@ -308,8 +291,8 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf if(seg->r) { cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero}; cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero}; - circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1); - circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2); + circleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, &info1); + circleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, &info2); if(info1.t < info2.t){ (*info) = info1; @@ -321,10 +304,10 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf static const cpShapeClass cpSegmentShapeClass = { CP_SEGMENT_SHAPE, - cpSegmentShapeCacheData, + (cpShapeCacheDataImpl)cpSegmentShapeCacheData, NULL, - cpSegmentShapePointQuery, - cpSegmentShapeSegmentQuery, + (cpShapePointQueryImpl)cpSegmentShapePointQuery, + (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery, }; cpSegmentShape * diff --git a/src/helper/chipmunk/cpShape.h b/src/helper/chipmunk/cpShape.h index 390472ead..691281d59 100644 --- a/src/helper/chipmunk/cpShape.h +++ b/src/helper/chipmunk/cpShape.h @@ -19,16 +19,23 @@ * SOFTWARE. */ -// Forward declarations required for defining other structs. +/// @defgroup cpShape cpShape +/// The cpShape struct defines the shape of a rigid body. +/// @{ + typedef struct cpShapeClass cpShapeClass; +/// Segment query info struct. typedef struct cpSegmentQueryInfo { - cpShape *shape; // shape that was hit, NULL if no collision - cpFloat t; // Distance along query segment, will always be in the range [0, 1]. - cpVect n; // normal of hit surface + /// The shape that was hit, NULL if no collision occured. + cpShape *shape; + /// The normalized distance along the query segment in the range [0, 1]. + cpFloat t; + /// The normal of the surface hit. + cpVect n; } cpSegmentQueryInfo; -// Enumeration of shape types. +/// @private typedef enum cpShapeType{ CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, @@ -36,119 +43,158 @@ typedef enum cpShapeType{ CP_NUM_SHAPES } cpShapeType; -// Shape class. Holds function pointers and type data. +typedef cpBB (*cpShapeCacheDataImpl)(cpShape *shape, cpVect p, cpVect rot); +typedef void (*cpShapeDestroyImpl)(cpShape *shape); +typedef cpBool (*cpShapePointQueryImpl)(cpShape *shape, cpVect p); +typedef void (*cpShapeSegmentQueryImpl)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); + +/// @private struct cpShapeClass { cpShapeType type; - // Called by cpShapeCacheBB(). - cpBB (*cacheData)(cpShape *shape, cpVect p, cpVect rot); - // Called to by cpShapeDestroy(). - void (*destroy)(cpShape *shape); - - // called by cpShapePointQuery(). - cpBool (*pointQuery)(cpShape *shape, cpVect p); - - // called by cpShapeSegmentQuery() - void (*segmentQuery)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); + cpShapeCacheDataImpl cacheData; + cpShapeDestroyImpl destroy; + cpShapePointQueryImpl pointQuery; + cpShapeSegmentQueryImpl segmentQuery; }; -// Basic shape struct that the others inherit from. +/// Opaque collision shape struct. struct cpShape { - // The "class" of a shape as defined above CP_PRIVATE(const cpShapeClass *klass); - // cpBody that the shape is attached to. + /// The rigid body this collision shape is attached to. cpBody *body; - // Cached BBox for the shape. + /// The current bounding box of the shape. cpBB bb; - // Sensors invoke callbacks, but do not generate collisions + /// Sensor flag. + /// Sensor shapes call collision callbacks but don't produce collisions. cpBool sensor; - // *** Surface properties. - - // Coefficient of restitution. (elasticity) + /// Coefficient of restitution. (elasticity) cpFloat e; - // Coefficient of friction. + /// Coefficient of friction. cpFloat u; - // Surface velocity used when solving for friction. + /// Surface velocity used when solving for friction. cpVect surface_v; - // *** User Definable Fields - - // User defined data pointer for the shape. + /// User definable data pointer. + /// Generally this points to your the game object class so you can access it + /// when given a cpShape reference in a callback. cpDataPointer data; - // User defined collision type for the shape. + /// Collision type of this shape used when picking collision handlers. cpCollisionType collision_type; - // User defined collision group for the shape. + /// Group of this shape. Shapes in the same group don't collide. cpGroup group; - // User defined layer bitmask for the shape. + // Layer bitmask for this shape. Shapes only collide if the bitwise and of their layers is non-zero. cpLayers layers; - // *** Internally Used Fields - - // Shapes form a linked list when added to space on a non-NULL body CP_PRIVATE(cpShape *next); - - // Unique id used as the hash value. CP_PRIVATE(cpHashValue hashid); }; -// Low level shape initialization func. -cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body); - -// Basic destructor functions. (allocation functions are not shared) +/// Destroy a shape. void cpShapeDestroy(cpShape *shape); +/// Destroy and Free a shape. void cpShapeFree(cpShape *shape); -// Cache the BBox of the shape. +/// Update, cache and return the bounding box of a shape based on the body it's attached to. cpBB cpShapeCacheBB(cpShape *shape); +/// Update, cache and return the bounding box of a shape with an explicit transformation. cpBB cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot); -// Test if a point lies within a shape. +/// Test if a point lies within a shape. cpBool cpShapePointQuery(cpShape *shape, cpVect p); -#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape) +#define CP_DefineShapeStructGetter(type, member, name) \ +static inline type cpShapeGet##name(const cpShape *shape){return shape->member;} -// Circle shape structure. +#define CP_DefineShapeStructSetter(type, member, name, activates) \ +static inline void cpShapeSet##name(cpShape *shape, type value){ \ + if(activates) cpBodyActivate(shape->body); \ + shape->member = value; \ +} + +#define CP_DefineShapeStructProperty(type, member, name, activates) \ +CP_DefineShapeStructGetter(type, member, name) \ +CP_DefineShapeStructSetter(type, member, name, activates) + +// TODO the properties +CP_DefineShapeStructProperty(cpBody *, body, Body, cpTrue); +CP_DefineShapeStructGetter(cpBB, bb, BB); +CP_DefineShapeStructProperty(cpBool, sensor, IsSensor, cpTrue); +CP_DefineShapeStructProperty(cpFloat, e, Elasticity, cpFalse); +CP_DefineShapeStructProperty(cpFloat, u, Friction, cpTrue); +CP_DefineShapeStructProperty(cpVect, surface_v, SurfaceVelocity, cpTrue); +CP_DefineShapeStructProperty(cpDataPointer, data, UserData, cpFalse); +CP_DefineShapeStructProperty(cpCollisionType, collision_type, CollisionType, cpTrue); +CP_DefineShapeStructProperty(cpGroup, group, Group, cpTrue); +CP_DefineShapeStructProperty(cpLayers, layers, Layers, cpTrue); + +/// When initializing a shape, it's hash value comes from a counter. +/// Because the hash value may affect iteration order, you can reset the shape ID counter +/// when recreating a space. This will make the simulation be deterministic. +void cpResetShapeIdCounter(void); + +/// Perform a segment query against a shape. @c info must be a pointer to a valid cpSegmentQueryInfo structure. +cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); + +/// Get the hit point for a segment query. +static inline cpVect +cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) +{ + return cpvlerp(start, end, info.t); +} + +/// Get the hit distance for a segment query. +static inline cpFloat +cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) +{ + return cpvdist(start, end)*info.t; +} + +#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(const cpShape *shape) + +/// @} +/// @defgroup cpCircleShape cpCircleShape + +/// @private typedef struct cpCircleShape { cpShape shape; - // Center in body space coordinates - CP_PRIVATE(cpVect c); - // Radius. - CP_PRIVATE(cpFloat r); - - // Transformed center. (world space coordinates) - CP_PRIVATE(cpVect tc); + cpVect c, tc; + cpFloat r; } cpCircleShape; -// Basic allocation functions for cpCircleShape. +/// Allocate a circle shape. cpCircleShape *cpCircleShapeAlloc(void); +/// Initialize a circle shape. cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); +/// Allocate and initialize a circle shape. cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset); CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); -// Segment shape structure. +/// @} +/// @defgroup cpSegmentShape cpSegmentShape + +/// @private typedef struct cpSegmentShape { cpShape shape; - // Endpoints and normal of the segment. (body space coordinates) - cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n); - // Radius of the segment. (Thickness) - cpFloat CP_PRIVATE(r); - - // Transformed endpoints and normal. (world space coordinates) - cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn); + cpVect a, b, n; + cpVect ta, tb, tn; + cpFloat r; } cpSegmentShape; -// Basic allocation functions for cpSegmentShape. +/// Allocate a segment shape. cpSegmentShape* cpSegmentShapeAlloc(void); +/// Initialize a segment shape. cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); +/// Allocate and initialize a segment shape. cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); CP_DeclareShapeGetter(cpSegmentShape, cpVect, A); @@ -156,19 +202,4 @@ CP_DeclareShapeGetter(cpSegmentShape, cpVect, B); CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal); CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius); -// For determinism, you can reset the shape id counter. -void cpResetShapeIdCounter(void); - -cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); - -static inline cpVect -cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) -{ - return cpvlerp(start, end, info.t); -} - -static inline cpFloat -cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) -{ - return cpvdist(start, end)*info.t; -} +/// @} diff --git a/src/helper/chipmunk/cpSpace.c b/src/helper/chipmunk/cpSpace.c index 61f3c1210..bf8024907 100644 --- a/src/helper/chipmunk/cpSpace.c +++ b/src/helper/chipmunk/cpSpace.c @@ -26,13 +26,11 @@ #include "chipmunk_private.h" -cpTimestamp cp_contact_persistence = 3; - #pragma mark Contact Set Helpers -// Equal function for contactSet. +// Equal function for arbiterSet. static cpBool -contactSetEql(cpShape **shapes, cpArbiter *arb) +arbiterSetEql(cpShape **shapes, cpArbiter *arb) { cpShape *a = shapes[0]; cpShape *b = shapes[1]; @@ -40,18 +38,18 @@ contactSetEql(cpShape **shapes, cpArbiter *arb) return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); } -#pragma mark Collision Pair Function Helpers +#pragma mark Collision Handler Set HelperFunctions -// Equals function for collFuncSet. +// Equals function for collisionHandlers. static cpBool -collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) +handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) { return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b)); } -// Transformation function for collFuncSet. +// Transformation function for collisionHandlers. static void * -collFuncSetTrans(cpCollisionHandler *handler, void *unused) +handlerSetTrans(cpCollisionHandler *handler, void *unused) { cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler)); (*copy) = (*handler); @@ -65,36 +63,10 @@ collFuncSetTrans(cpCollisionHandler *handler, void *unused) 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. -static cpBB shapeBBFunc(cpShape *shape){return shape->bb;} +// function to get the estimated velocity of a shape for the cpBBTree. static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;} -// Iterator functions for destructors. -static void freeWrap(void *ptr, void *unused){ cpfree(ptr);} -static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);} - -void -cpSpaceLock(cpSpace *space) -{ - space->locked++; -} - -void -cpSpaceUnlock(cpSpace *space) -{ - space->locked--; - cpAssert(space->locked >= 0, "Internal error:Space lock underflow."); - - if(!space->locked){ - cpArray *waking = space->rousedBodies; - for(int i=0, count=waking->num; iarr[i]); - } - - waking->num = 0; - } -} - +static void freeWrap(void *ptr, void *unused){cpfree(ptr);} #pragma mark Memory Management Functions @@ -104,35 +76,26 @@ cpSpaceAlloc(void) return (cpSpace *)cpcalloc(1, sizeof(cpSpace)); } -#define DEFAULT_DIM_SIZE 100.0f -#define DEFAULT_COUNT 1000 -#define DEFAULT_ITERATIONS 10 -#define DEFAULT_ELASTIC_ITERATIONS 0 - -cpCollisionHandler cpSpaceDefaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL}; +cpCollisionHandler cpDefaultCollisionHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL}; cpSpace* cpSpaceInit(cpSpace *space) { - space->iterations = DEFAULT_ITERATIONS; + space->iterations = 10; space->gravity = cpvzero; space->damping = 1.0f; - space->collisionSlop = cp_collision_slop; - space->collisionBias = cp_bias_coef; + space->collisionSlop = 0.1f; + space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f); + space->collisionPersistence = 3; space->locked = 0; space->stamp = 0; - if(0){ - space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, NULL); - space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes); - } else { - space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, NULL); - space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes); - cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc); - } + space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL); + space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes); + cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc); space->allocatedBuffers = cpArrayNew(0); @@ -142,18 +105,19 @@ cpSpaceInit(cpSpace *space) space->sleepTimeThreshold = INFINITY; space->idleSpeedThreshold = 0.0f; + space->enableContactGraph = cpFalse; space->arbiters = cpArrayNew(0); space->pooledArbiters = cpArrayNew(0); space->contactBuffersHead = NULL; - space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, NULL); + space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql); space->constraints = cpArrayNew(0); - space->defaultHandler = cpSpaceDefaultHandler; - space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, &space->defaultHandler); - // TODO space->collFuncSet->default_value = &cpSpaceDefaultHandler; + space->defaultHandler = cpDefaultCollisionHandler; + space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql); + cpHashSetSetDefaultValue(space->collisionHandlers, &cpDefaultCollisionHandler); space->postStepCallbacks = NULL; @@ -181,7 +145,7 @@ cpSpaceDestroy(cpSpace *space) cpArrayFree(space->constraints); - cpHashSetFree(space->contactSet); + cpHashSetFree(space->cachedArbiters); cpArrayFree(space->arbiters); cpArrayFree(space->pooledArbiters); @@ -196,9 +160,9 @@ cpSpaceDestroy(cpSpace *space) cpHashSetFree(space->postStepCallbacks); } - if(space->collFuncSet){ - cpHashSetEach(space->collFuncSet, freeWrap, NULL); - cpHashSetFree(space->collFuncSet); + if(space->collisionHandlers){ + cpHashSetEach(space->collisionHandlers, freeWrap, NULL); + cpHashSetFree(space->collisionHandlers); } } @@ -211,19 +175,6 @@ cpSpaceFree(cpSpace *space) } } -void -cpSpaceFreeChildren(cpSpace *space) -{ - cpArray *components = space->sleepingComponents; - while(components->num) cpBodyActivate((cpBody *)components->arr[0]); - - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL); - cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL); - - cpArrayFreeEach(space->bodies, (void (*)(void*))cpBodyFree); - cpArrayFreeEach(space->constraints, (void (*)(void*))cpConstraintFree); -} - #pragma mark Collision Handler Function Management void @@ -248,14 +199,14 @@ cpSpaceAddCollisionHandler( data }; - cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)collFuncSetTrans); + cpHashSetInsert(space->collisionHandlers, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)handlerSetTrans); } void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) { struct { cpCollisionType a, b; } ids = {a, b}; - cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids); + cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collisionHandlers, CP_HASH_PAIR(a, b), &ids); cpfree(old_handler); } @@ -278,7 +229,7 @@ cpSpaceSetDefaultCollisionHandler( }; space->defaultHandler = handler; - // TODO space->collFuncSet->default_value = &space->defaultHandler; + cpHashSetSetDefaultValue(space->collisionHandlers, &space->defaultHandler); } #pragma mark Body, Shape, and Joint Management @@ -346,7 +297,7 @@ cpSpaceAddBody(cpSpace *space, cpBody *body) cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); cpAssert(!cpSpaceContainsBody(space, body), "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 + cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); body->space = space; @@ -361,16 +312,14 @@ cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint) cpConstraint *node = body->constraintList; while(node && node != constraint){ - prev_ptr = (node->a == body ? &node->nextA : &node->nextB); + prev_ptr = (node->a == body ? &node->next_a : &node->next_b); node = *prev_ptr; } cpAssert(node, "Attempted to remove a constraint from a body it was never attached to."); - (*prev_ptr) = (node->a == body ? node->nextA : node->nextB); + (*prev_ptr) = (node->a == body ? node->next_a : node->next_b); } - - cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { @@ -384,26 +333,52 @@ cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) // Push onto the heads of the bodies' constraint lists cpBody *a = constraint->a, *b = constraint->b; - constraint->nextA = a->constraintList; a->constraintList = constraint; - constraint->nextB = b->constraintList; b->constraintList = constraint; + constraint->next_a = a->constraintList; a->constraintList = constraint; + constraint->next_b = b->constraintList; b->constraintList = constraint; return constraint; } -typedef struct removalContext { +static void +removeArbiterFromBodyList(cpBody *body, cpArbiter *arb) +{ + cpArbiter **prev_ptr = &body->arbiterList; + cpArbiter *node = body->arbiterList; + + while(node && node != arb){ + prev_ptr = (node->body_a == body ? &node->next_a : &node->next_b); + node = *prev_ptr; + } + + // Need to double check, but there is probably quite a few situations where an arbiter isn't in the body list. + //cpAssert(node, "Internal Error: Attempted to remove an arbiter from a body it was never attached to."); + + if(node){ + (*prev_ptr) = (node->body_a == body ? node->next_a : node->next_b); + } +} + +typedef struct arbiterRemovalContext { cpSpace *space; cpShape *shape; -} removalContext; +} arbiterRemovalContext; // Hashset filter func to throw away old arbiters. static cpBool -contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context) +arbiterSetFilterRemovedShape(cpArbiter *arb, arbiterRemovalContext *context) { - if(context->shape == arb->a || context->shape == arb->b){ + cpShape *shape = context->shape; + + if(shape == arb->a || shape == arb->b){ + // If the Arbiter is active (non-cached) call it's separate function immediately. if(arb->state != cpArbiterStateCached){ arb->handler->separate(arb, context->space, arb->handler->data); } + removeArbiterFromBodyList(arb->body_a, arb); + removeArbiterFromBodyList(arb->body_b, arb); + + // Add the arbiter back to the pool cpArrayPush(context->space->pooledArbiters, arb); return cpFalse; } @@ -428,8 +403,8 @@ cpSpaceRemoveShape(cpSpace *space, cpShape *shape) cpBodyRemoveShape(body, shape); - removalContext context = {space, shape}; - cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context); + arbiterRemovalContext context = {space, shape}; + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)arbiterSetFilterRemovedShape, &context); cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); } @@ -440,8 +415,8 @@ cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) "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); + arbiterRemovalContext context = {space, shape}; + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)arbiterSetFilterRemovedShape, &context); cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); cpSpaceActivateShapesTouchingShape(space, shape); @@ -494,17 +469,46 @@ cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint) #pragma mark Iteration -// TODO copy fix from 5.x void -cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data) { - cpArray *bodies = space->bodies; - - for(int i=0; inum; i++) - func((cpBody *)bodies->arr[i], data); + cpSpaceLock(space); { + cpArray *bodies = space->bodies; + + for(int i=0; inum; i++){ + func((cpBody *)bodies->arr[i], data); + } + + cpArray *components = space->sleepingComponents; + for(int i=0; inum; i++){ + cpBody *root = (cpBody *)components->arr[i]; + CP_BODY_FOREACH_COMPONENT(root, body) func(body, data); + } + } cpSpaceUnlock(space, cpTrue); } -#pragma mark Spatial Hash Management +typedef struct spaceShapeContext { + cpSpaceShapeIteratorFunc func; + void *data; +} spaceShapeContext; + +static void +spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context) +{ + context->func(shape, context->data); +} + +void +cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data) +{ + cpSpaceLock(space); { + spaceShapeContext context = {func, data}; + cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); + } cpSpaceUnlock(space, cpTrue); +} + +#pragma mark Spatial Index Management static void updateBBCache(cpShape *shape, void *unused) @@ -513,30 +517,15 @@ updateBBCache(cpShape *shape, void *unused) cpShapeUpdate(shape, body->p, body->rot); } -void -cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) -{ -// TODO spatial hash specific - cpSpaceHashResize((cpSpaceHash *)space->staticShapes, dim, count); - cpSpatialIndexReindex(space->staticShapes); -} - -void -cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) -{ -// TODO spatial hash specific - cpSpaceHashResize((cpSpaceHash *)space->activeShapes, dim, count); -} - void -cpSpaceRehashStatic(cpSpace *space) +cpSpaceReindexStatic(cpSpace *space) { - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&updateBBCache, NULL); + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&updateBBCache, NULL); cpSpatialIndexReindex(space->staticShapes); } void -cpSpaceRehashShape(cpSpace *space, cpShape *shape) +cpSpaceReindexShape(cpSpace *space, cpShape *shape) { cpBody *body = shape->body; cpShapeUpdate(shape, body->p, body->rot); @@ -545,3 +534,25 @@ cpSpaceRehashShape(cpSpace *space, cpShape *shape) cpSpatialIndexReindexObject(space->activeShapes, shape, shape->hashid); cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid); } + +static void +copyShapes(cpShape *shape, cpSpatialIndex *index) +{ + cpSpatialIndexInsert(index, shape, shape->hashid); +} + +void +cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL); + cpSpatialIndex *activeShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes); + + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes); + cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)copyShapes, activeShapes); + + cpSpatialIndexFree(space->staticShapes); + cpSpatialIndexFree(space->activeShapes); + + space->staticShapes = staticShapes; + space->activeShapes = activeShapes; +} diff --git a/src/helper/chipmunk/cpSpace.h b/src/helper/chipmunk/cpSpace.h index eb612ff9d..27ee6664e 100644 --- a/src/helper/chipmunk/cpSpace.h +++ b/src/helper/chipmunk/cpSpace.h @@ -19,101 +19,105 @@ * SOFTWARE. */ -// Number of frames that contact information should persist. -extern cpTimestamp cp_contact_persistence; - -extern cpCollisionHandler cpSpaceDefaultHandler; +/// @defgroup cpSpace cpSpace +/// @{ typedef struct cpContactBufferHeader cpContactBufferHeader; +/// Basic Unit of Simulation in Chipmunk struct cpSpace { - // *** User definable fields - - // Number of iterations to use in the impulse solver to solve contacts. + /// Number of iterations to use in the impulse solver to solve contacts. int iterations; - // Default gravity to supply when integrating rigid body motions. + /// Gravity to pass to rigid bodies when integrating velocity. cpVect gravity; - // Default damping to supply when integrating rigid body motions. + /// Damping rate expressed as the fraction of velocity bodies retain each second. + /// A value of 0.9 would mean that each body's velocity will drop 10% per second. + /// The default value is 1.0, meaning no damping is applied. + /// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring. 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. + /// 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. + /// Time a group of bodies must remain idle in order to fall asleep. + /// Enabling sleeping also implicitly enables the the contact graph. + /// The default value of INFINITY disables the sleeping algorithm. cpFloat sleepTimeThreshold; + /// Amount of allowed penetration. + /// Used to reduce oscillating contacts and keep the collision cache warm. + /// Defaults to 0.1. If you have poor simulation quality, + /// increase this number as much as possible without allowing visible amounts of overlap. cpFloat collisionSlop; + + /// Determines how fast overlapping shapes are pushed apart. + /// Expressed as a fraction of the error remaining after each second. + /// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz. cpFloat collisionBias; - // *** Internally Used Fields + /// Number of frames that contact information should persist. + /// Defaults to 3. There is probably never a reason to change this value. + cpTimestamp collisionPersistence; - // When the space lock count is non zero you cannot add or remove objects - CP_PRIVATE(int locked); + /// Rebuild the contact graph during each step. Must be enabled to use the cpBodyEachArbiter() function. + /// Disabled by default for a small performance boost. + cpBool enableContactGraph; + + /// User definable data pointer. + /// Generally this points to your game's controller or game state + /// class so you can access it when given a cpSpace reference in a callback. + cpDataPointer data; + + /// The designated static body for this space. + /// You can modify this body, or replace it with your own static body. + /// By default it points to a statically allocated cpBody in the cpSpace struct. + cpBody *staticBody; - // Time stamp. Is incremented on every call to cpSpaceStep(). CP_PRIVATE(cpTimestamp stamp); CP_PRIVATE(cpFloat prev_dt); - // The static and active shape spatial hashes. + CP_PRIVATE(cpArray *bodies); + CP_PRIVATE(cpArray *rousedBodies); + CP_PRIVATE(cpArray *sleepingComponents); + CP_PRIVATE(cpSpatialIndex *staticShapes); CP_PRIVATE(cpSpatialIndex *activeShapes); - // List of bodies in the system. - 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. CP_PRIVATE(cpArray *arbiters); - CP_PRIVATE(cpArray *pooledArbiters); - - // Linked list ring of contact buffers. - // Head is the newest buffer, and each buffer points to a newer buffer. - // Head wraps around and points to the oldest (tail) buffer. CP_PRIVATE(cpContactBufferHeader *contactBuffersHead); - - // List of buffers to be free()ed when destroying the space. - CP_PRIVATE(cpArray *allocatedBuffers); - - // Persistant contact set. - CP_PRIVATE(cpHashSet *contactSet); - - // List of constraints in the system. + CP_PRIVATE(cpHashSet *cachedArbiters); + CP_PRIVATE(cpArray *pooledArbiters); CP_PRIVATE(cpArray *constraints); - // Set of collisionpair functions. - CP_PRIVATE(cpHashSet *collFuncSet); - // Default collision handler. - CP_PRIVATE(cpCollisionHandler defaultHandler); + CP_PRIVATE(cpArray *allocatedBuffers); + CP_PRIVATE(int locked); + CP_PRIVATE(cpHashSet *collisionHandlers); + CP_PRIVATE(cpCollisionHandler defaultHandler); CP_PRIVATE(cpHashSet *postStepCallbacks); CP_PRIVATE(cpBody _staticBody); - - cpDataPointer data; - cpBody *staticBody; }; -// Basic allocation/destruction functions. +/// Allocate a cpSpace. cpSpace* cpSpaceAlloc(void); +/// Initialize a cpSpace. cpSpace* cpSpaceInit(cpSpace *space); +/// Allocate and initialize a cpSpace. cpSpace* cpSpaceNew(void); +/// Destroy a cpSpace. void cpSpaceDestroy(cpSpace *space); +/// Destroy and free a cpSpace. void cpSpaceFree(cpSpace *space); -// Convenience function. Frees all referenced entities. (bodies, shapes and constraints) -void cpSpaceFreeChildren(cpSpace *space); - -// Collision handler management functions. +/// Set a default collision handler for this space. +/// The default collision handler is invoked for each colliding pair of shapes +/// that isn't explicitly handled by a specific collision handler. +/// You can pass NULL for any function you don't want to implement. void cpSpaceSetDefaultCollisionHandler( cpSpace *space, cpCollisionBeginFunc begin, @@ -122,6 +126,9 @@ void cpSpaceSetDefaultCollisionHandler( cpCollisionSeparateFunc separate, void *data ); + +/// Set a collision handler to be used whenever the two shapes with the given collision types collide. +/// You can pass NULL for any function you don't want to implement. void cpSpaceAddCollisionHandler( cpSpace *space, cpCollisionType a, cpCollisionType b, @@ -131,62 +138,90 @@ void cpSpaceAddCollisionHandler( cpCollisionSeparateFunc separate, void *data ); + +/// Unset a collision handler. void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); -// Add and remove objects from the system. +/// Add a collision shape to the simulation. +/// If the shape is attached to a static body, it will be added as a static shape. cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape); +/// Explicity add a shape as a static shape to the simulation. cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); +/// Add a rigid body to the simulation. cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body); +/// Add a constraint to the simulation. cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); +/// Remove a collision shape from the simulation. void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); +/// Remove a collision shape added using cpSpaceAddStaticShape() from the simulation. void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); +/// Remove a rigid body from the simulation. void cpSpaceRemoveBody(cpSpace *space, cpBody *body); +/// Remove a constraint from the simulation. void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); -// Test if an object is in the space +/// Test if a collision shape has been added to the space. cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape); +/// Test if a rigid body has been added to the space. cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body); +/// Test if a constraint has been added to the space. cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint); -// Post Step function definition +/// Post Step callback function type. typedef void (*cpPostStepFunc)(cpSpace *space, void *obj, void *data); -// Register a post step function to be called after cpSpaceStep() has finished. -// obj is used a key, you can only register one callback per unique value for obj +/// Schedule a post-step callback to be called when cpSpaceStep() finishes. +/// @c obj is used a key, you can only register one callback per unique value for @c obj void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data); -// Point query callback function +/// Point query callback function type. typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); +/// Query the space at a point and call @c func for each shape found. void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); +/// Query the space at a point and return the first shape found. Returns NULL if no shapes were found. cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); -// Segment query callback function +/// Segment query callback function type. typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data); +/// Perform a directed line segment query (like a raycast) against the space calling @c func for each shape intersected. void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data); +/// Perform a directed line segment query (like a raycast) against the space and return the first shape hit. Returns NULL if no shapes were hit. cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out); -// BB query callback function +/// Rectangle Query callback function type. typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); +/// Perform a fast rectangle query on the space calling @c func for each shape found. +/// Only the shape's bounding boxes are checked for overlap, not their full shape. void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); -// Shape query callback function +/// Shape query callback function type. typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); +/// Query a space for any shapes overlapping the given shape and call @c func for each shape found. cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); - +/// Call cpBodyActivate() for any shape that is overlaps the given shape. void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape); -// Iterator function for iterating the bodies in a space. -typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data); -void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data); +/// Space/body iterator callback function type. +typedef void (*cpSpaceBodyIteratorFunc)(cpBody *body, void *data); +/// Call @c func for each body in the space. +void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data); -// Spatial hash management functions. -void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count); -void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count); -void cpSpaceRehashStatic(cpSpace *space); +/// Space/body iterator callback function type. +typedef void (*cpSpaceShapeIteratorFunc)(cpShape *shape, void *data); +/// Call @c func for each shape in the space. +void cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data); -void cpSpaceRehashShape(cpSpace *space, cpShape *shape); +/// Update the collision detection info for the static shapes in the space. +void cpSpaceReindexStatic(cpSpace *space); +/// Update the collision detection data for a specific shape in the space. +void cpSpaceReindexShape(cpSpace *space, cpShape *shape); -// Update the space. +/// Switch the space to use a spatial has as it's spatial index. +void cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count); + +/// Step the space forward in time by @c dt. void cpSpaceStep(cpSpace *space, cpFloat dt); + +/// @} diff --git a/src/helper/chipmunk/cpSpaceComponent.c b/src/helper/chipmunk/cpSpaceComponent.c index 036f7082a..49ab7411d 100644 --- a/src/helper/chipmunk/cpSpaceComponent.c +++ b/src/helper/chipmunk/cpSpaceComponent.c @@ -55,7 +55,7 @@ cpSpaceActivateBody(cpSpace *space, cpBody *body) cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); - cpHashSetInsert(space->contactSet, arbHashID, shape_pair, arb, NULL); + cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); cpArrayPush(space->arbiters, arb); arb->stamp = space->stamp; @@ -86,7 +86,7 @@ cpSpaceDeactivateBody(cpSpace *space, cpBody *body) cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); - cpHashSetRemove(space->contactSet, arbHashID, shape_pair); + cpHashSetRemove(space->cachedArbiters, arbHashID, shape_pair); cpArrayDeleteObj(space->arbiters, arb); // Save contact values to a new block of memory so they won't time out @@ -103,6 +103,12 @@ cpSpaceDeactivateBody(cpSpace *space, cpBody *body) } } +static inline cpBody * +ComponentRoot(cpBody *body) +{ + return (body ? body->node.root : NULL); +} + static inline void ComponentActivate(cpBody *root) { @@ -131,9 +137,9 @@ ComponentAdd(cpBody *root, cpBody *body){ void cpBodyActivate(cpBody *body) { - if(!cpBodyIsStatic(body)){ + if(!cpBodyIsRogue(body)){ body->node.idleTime = 0.0f; - ComponentActivate(body->node.root); + ComponentActivate(ComponentRoot(body)); } } @@ -151,14 +157,13 @@ static inline void FloodFillComponent(cpBody *root, cpBody *body) { if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){ - cpBody *other_root = body->node.root; + cpBody *other_root = ComponentRoot(body); if(other_root == NULL){ ComponentAdd(root, body); CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a)); CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a)); } else { - // TODO can probably remove this at some point. - cpAssert(other_root == root, "Uh oh. This shouldn't happen?"); + cpAssert(other_root == root, "Internal Error: Inconsistency dectected in the contact graph."); } } } @@ -168,9 +173,9 @@ cpBodyPushArbiter(cpBody *body, cpArbiter *arb) { if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){ if(body == arb->body_a){ - arb->nextA = body->arbiterList; + arb->next_a = body->arbiterList; } else { - arb->nextB = body->arbiterList; + arb->next_b = body->arbiterList; } body->arbiterList = arb; @@ -181,7 +186,7 @@ void cpSpaceProcessComponents(cpSpace *space, cpFloat dt) { cpFloat dv = space->idleSpeedThreshold; - cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); // is dt^2 a bug? + cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); // update idling and reset arbiter list and component nodes cpArray *bodies = space->bodies; @@ -189,45 +194,46 @@ cpSpaceProcessComponents(cpSpace *space, cpFloat dt) cpBody *body = (cpBody*)bodies->arr[i]; // Need to deal with infinite mass objects - cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f); - body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt); + cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); + body->node.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->node.idleTime + dt); body->arbiterList = NULL; body->node.next = NULL; } - // Add arbiters to body lists and awaken any sleeping bodies found + // Awaken any sleeping bodies found and then push arbiters to the bodies' lists. cpArray *arbiters = space->arbiters; for(int i=0, count=arbiters->num; iarr[i]; cpBody *a = arb->body_a, *b = arb->body_b; - if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a); - if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b); + if((cpBodyIsRogue(b) && !cpBodyIsStatic(b)) || cpBodyIsSleeping(a)) cpBodyActivate(a); + if((cpBodyIsRogue(a) && !cpBodyIsStatic(a)) || cpBodyIsSleeping(b)) cpBodyActivate(b); cpBodyPushArbiter(a, arb); cpBodyPushArbiter(b, arb); } - // Bodies are awoken when adding constraints, is this redundant? + // Bodies should be held active if connected by a joint to a non-static rouge body. cpArray *constraints = space->constraints; for(int i=0; inum; i++){ - cpConstraint *constraint = constraints->arr[i]; + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; cpBody *a = constraint->a, *b = constraint->b; - if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a); - if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b); + if(cpBodyIsRogue(b) && !cpBodyIsStatic(b)) cpBodyActivate(a); + if(cpBodyIsRogue(a) && !cpBodyIsStatic(a)) cpBodyActivate(b); } // Generate components and deactivate sleeping ones for(int i=0; inum;){ cpBody *body = (cpBody*)bodies->arr[i]; - if(body->node.root == NULL){ - // Body not in a component. - // Perform a flood fill to find and mark the component in the contact graph. + if(ComponentRoot(body) == NULL){ + // Body not in a component yet. Perform a DFS to flood fill mark + // the component in the contact graph using this body as the root. FloodFillComponent(body, body); + // Check if the component should be put to sleep. if(!ComponentActive(body, space->sleepTimeThreshold)){ cpArrayPush(space->sleepingComponents, body); CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other); @@ -259,14 +265,18 @@ cpBodySleepWithGroup(cpBody *body, cpBody *group){ cpSpace *space = body->space; cpAssert(space, "Cannot put a rogue body to sleep."); cpAssert(!space->locked, "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback."); - cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); - cpAssert(!group || cpBodyIsSleeping(body), "Cannot add a body that is already sleeping to a group."); - + cpAssert(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); + + if(cpBodyIsSleeping(body)){ + cpAssert(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned."); + return; + } + CP_BODY_FOREACH_SHAPE(body, shape) cpShapeUpdate(shape, body->p, body->rot); cpSpaceDeactivateBody(space, body); if(group){ - cpBody *root = group->node.root; + cpBody *root = ComponentRoot(group); cpComponentNode node = {root, root->node.next, 0.0f}; body->node = node; diff --git a/src/helper/chipmunk/cpSpaceHash.c b/src/helper/chipmunk/cpSpaceHash.c index 3efde92e5..74e7b0109 100644 --- a/src/helper/chipmunk/cpSpaceHash.c +++ b/src/helper/chipmunk/cpSpaceHash.c @@ -1,15 +1,15 @@ /* Copyright (c) 2007 Scott Lembcke - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -30,17 +30,17 @@ typedef struct cpHandle cpHandle; struct cpSpaceHash { cpSpatialIndex spatialIndex; - + int numcells; cpFloat celldim; - + cpSpaceHashBin **table; cpHashSet *handleSet; - + cpSpaceHashBin *pooledBins; cpArray *pooledHandles; cpArray *allocatedBuffers; - + cpTimestamp stamp; }; @@ -59,7 +59,7 @@ cpHandleInit(cpHandle *hand, void *obj) hand->obj = obj; hand->retain = 0; hand->stamp = 0; - + return hand; } @@ -81,16 +81,16 @@ handleSetTrans(void *obj, cpSpaceHash *hash) // handle pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpHandle); cpAssert(count, "Buffer size is too small."); - + cpHandle *buffer = (cpHandle *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(hash->allocatedBuffers, buffer); - + for(int i=0; ipooledHandles, buffer + i); } - + cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); cpHandleRetain(hand); - + return hand; } @@ -114,13 +114,13 @@ clearTableCell(cpSpaceHash *hash, int idx) cpSpaceHashBin *bin = hash->table[idx]; while(bin){ cpSpaceHashBin *next = bin->next; - + cpHandleRelease(bin->handle, hash->pooledHandles); recycleBin(hash, bin); - + bin = next; } - + hash->table[idx] = NULL; } @@ -135,7 +135,7 @@ static inline cpSpaceHashBin * getEmptyBin(cpSpaceHash *hash) { cpSpaceHashBin *bin = hash->pooledBins; - + if(bin){ hash->pooledBins = bin->next; return bin; @@ -143,10 +143,10 @@ getEmptyBin(cpSpaceHash *hash) // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); cpAssert(count, "Buffer size is too small."); - + cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES); cpArrayPush(hash->allocatedBuffers, buffer); - + // push all but the first one, return the first instead for(int i=1; itable); - + hash->numcells = numcells; hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); } -static cpSpatialIndexClass klass; +#ifdef _MSC_VER +// Are you freaking kidding me? +// Can it really not tell the difference between a declaration and a definition? +// Have I ever mentioned how much I hate MSVC? +extern +#else +static +#endif +cpSpatialIndexClass klass; cpSpatialIndex * cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) { cpSpatialIndexInit((cpSpatialIndex *)hash, &klass, bbfunc, staticIndex); - + cpSpaceHashAllocTable(hash, next_prime(numcells)); hash->celldim = celldim; - - hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, NULL); + + hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql); + hash->pooledHandles = cpArrayNew(0); - + hash->pooledBins = NULL; hash->allocatedBuffers = cpArrayNew(0); - + hash->stamp = 1; - + return (cpSpatialIndex *)hash; } @@ -202,13 +211,13 @@ static void cpSpaceHashDestroy(cpSpaceHash *hash) { clearTable(hash); - + cpHashSetFree(hash->handleSet); - + cpArrayFreeEach(hash->allocatedBuffers, cpfree); cpArrayFree(hash->allocatedBuffers); cpArrayFree(hash->pooledHandles); - + cpfree(hash->table); } @@ -221,7 +230,7 @@ containsHandle(cpSpaceHashBin *bin, cpHandle *hand) if(bin->handle == hand) return cpTrue; bin = bin->next; } - + return cpFalse; } @@ -250,13 +259,13 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) int r = floor_int(bb.r/dim); int b = floor_int(bb.b/dim); int t = floor_int(bb.t/dim); - + int n = hash->numcells; for(int i=l; i<=r; i++){ for(int j=b; j<=t; j++){ int idx = hash_func(i,j,n); cpSpaceHashBin *bin = hash->table[idx]; - + // Don't add an object twice to the same cell. if(containsHandle(bin, hand)) continue; @@ -283,11 +292,11 @@ static void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) { cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - + if(hand){ hand->obj = NULL; cpHandleRelease(hand, hash->pooledHandles); - + cpSpaceHashInsert(hash, obj, hashid); } } @@ -302,14 +311,14 @@ static void cpSpaceHashRehash(cpSpaceHash *hash) { clearTable(hash); - cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)rehash_helper, hash); + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash); } static void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) { cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - + if(hand){ hand->obj = NULL; cpHandleRelease(hand, hash->pooledHandles); @@ -317,17 +326,17 @@ cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) } typedef struct eachContext { - cpSpatialIndexIterator func; + cpSpatialIndexIteratorFunc func; void *data; } eachContext; static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);} static void -cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIterator func, void *data) +cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data) { eachContext context = {func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &context); + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context); } static void @@ -337,17 +346,17 @@ remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **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; } } @@ -355,13 +364,13 @@ remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) #pragma mark Query Functions static inline void -query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryCallback func, void *data) +query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data) { restart: for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ cpHandle *hand = bin->handle; void *other = hand->obj; - + if(hand->stamp == hash->stamp || obj == other){ continue; } else if(other){ @@ -377,17 +386,17 @@ query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIn } static void -cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpatialIndexQueryCallback func, void *data) +cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpatialIndexQueryFunc func, void *data) { cpFloat dim = hash->celldim; int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells); // Fix by ShiftZ - + query_helper(hash, &hash->table[idx], &point, func, data); hash->stamp++; } static void -cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) { // Get the dimensions in cell coordinates. cpFloat dim = hash->celldim; @@ -395,24 +404,24 @@ cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryCallb int r = floor_int(bb.r/dim); int b = floor_int(bb.b/dim); 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++){ query_helper(hash, &table[hash_func(i,j,n)], obj, func, data); } } - + hash->stamp++; } // Similar to struct eachPair above. typedef struct queryRehashContext { cpSpaceHash *hash; - cpSpatialIndexQueryCallback func; + cpSpatialIndexQueryFunc func; void *data; } queryRehashContext; @@ -421,7 +430,7 @@ static void queryRehash_helper(cpHandle *hand, queryRehashContext *context) { cpSpaceHash *hash = context->hash; - cpSpatialIndexQueryCallback func = context->func; + cpSpatialIndexQueryFunc func = context->func; void *data = context->data; cpFloat dim = hash->celldim; @@ -434,51 +443,51 @@ queryRehash_helper(cpHandle *hand, queryRehashContext *context) 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++){ int idx = hash_func(i,j,n); 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_helper(hash, &bin, obj, func, data); - + cpSpaceHashBin *newBin = getEmptyBin(hash); newBin->handle = hand; newBin->next = bin; table[idx] = newBin; } } - + // Increment the stamp for each object hashed. hash->stamp++; } static void -cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryCallback func, void *data) +cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data) { clearTable(hash); - + queryRehashContext context = {hash, func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)queryRehash_helper, &context); - + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context); + cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data); } static inline cpFloat -segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryCallback func, void *data) +segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data) { cpFloat t = 1.0f; - + restart: for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ cpHandle *hand = bin->handle; void *other = hand->obj; - + // Skip over certain conditions if(hand->stamp == hash->stamp){ continue; @@ -492,17 +501,17 @@ segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSp goto restart; // GCC not smart enough/able to tail call an inlined function. } } - + return t; } // modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html void -cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { a = cpvmult(a, 1.0f/hash->celldim); b = cpvmult(b, 1.0f/hash->celldim); - + int cell_x = floor_int(a.x), cell_y = floor_int(a.y); cpFloat t = 0; @@ -525,15 +534,15 @@ cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloa y_inc = -1; temp_v = (a.y - cpffloor(a.y)); } - + // Division by zero is *very* slow on ARM cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); - + // fix NANs in horizontal directions cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); - + int n = hash->numcells; cpSpaceHashBin **table = hash->table; @@ -551,7 +560,7 @@ cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloa next_h += dt_dx; } } - + hash->stamp++; } @@ -564,9 +573,9 @@ cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index."); return; } - + clearTable(hash); - + hash->celldim = celldim; cpSpaceHashAllocTable(hash, next_prime(numcells)); } @@ -584,29 +593,31 @@ cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid) } static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyFunc)cpSpaceHashDestroy, - - (cpSpatialIndexCountFunc)cpSpaceHashCount, - (cpSpatialIndexEachFunc)cpSpaceHashEach, - (cpSpatialIndexContainsFunc)cpSpaceHashContains, - - (cpSpatialIndexInsertFunc)cpSpaceHashInsert, - (cpSpatialIndexRemoveFunc)cpSpaceHashRemove, - - (cpSpatialIndexReindexFunc)cpSpaceHashRehash, - (cpSpatialIndexReindexObjectFunc)cpSpaceHashRehashObject, - (cpSpatialIndexReindexQueryFunc)cpSpaceHashReindexQuery, - - (cpSpatialIndexPointQueryFunc)cpSpaceHashPointQuery, - (cpSpatialIndexSegmentQueryFunc)cpSpaceHashSegmentQuery, - (cpSpatialIndexQueryFunc)cpSpaceHashQuery, + (cpSpatialIndexDestroyImpl)cpSpaceHashDestroy, + + (cpSpatialIndexCountImpl)cpSpaceHashCount, + (cpSpatialIndexEachImpl)cpSpaceHashEach, + (cpSpatialIndexContainsImpl)cpSpaceHashContains, + + (cpSpatialIndexInsertImpl)cpSpaceHashInsert, + (cpSpatialIndexRemoveImpl)cpSpaceHashRemove, + + (cpSpatialIndexReindexImpl)cpSpaceHashRehash, + (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject, + (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery, + + (cpSpatialIndexPointQueryImpl)cpSpaceHashPointQuery, + (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, + (cpSpatialIndexQueryImpl)cpSpaceHashQuery, }; #pragma mark Debug Drawing -#define CP_BBTREE_DEBUG_DRAW +//#define CP_BBTREE_DEBUG_DRAW #ifdef CP_BBTREE_DEBUG_DRAW #include +#include +#include void cpSpaceHashRenderDebug(cpSpatialIndex *index) @@ -615,26 +626,26 @@ cpSpaceHashRenderDebug(cpSpatialIndex *index) cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index."); return; } - + cpSpaceHash *hash = (cpSpaceHash *)index; cpBB bb = cpBBNew(-320, -240, 320, 240); - + cpFloat dim = hash->celldim; int n = hash->numcells; - + int l = (int)floor(bb.l/dim); int r = (int)floor(bb.r/dim); int b = (int)floor(bb.b/dim); int t = (int)floor(bb.t/dim); - + for(int i=l; i<=r; i++){ for(int j=b; j<=t; j++){ int cell_count = 0; - + int index = hash_func(i,j,n); for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next) cell_count++; - + GLfloat v = 1.0f - (GLfloat)cell_count/10.0f; glColor3f(v,v,v); glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim); diff --git a/src/helper/chipmunk/cpSpaceQuery.c b/src/helper/chipmunk/cpSpaceQuery.c index eaa4a132c..359e00409 100644 --- a/src/helper/chipmunk/cpSpaceQuery.c +++ b/src/helper/chipmunk/cpSpaceQuery.c @@ -49,9 +49,9 @@ cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, pointQueryContext context = {layers, group, func, data}; cpSpaceLock(space); { - cpSpatialIndexPointQuery(space->activeShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context); - cpSpatialIndexPointQuery(space->staticShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context); - } cpSpaceUnlock(space); + cpSpatialIndexPointQuery(space->activeShapes, point, (cpSpatialIndexQueryFunc)pointQueryHelper, &context); + cpSpatialIndexPointQuery(space->staticShapes, point, (cpSpatialIndexQueryFunc)pointQueryHelper, &context); + } cpSpaceUnlock(space, cpTrue); } static void @@ -104,9 +104,9 @@ cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, c }; cpSpaceLock(space); { - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data); - } cpSpaceUnlock(space); + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFunc, data); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFunc, data); + } cpSpaceUnlock(space, cpTrue); } typedef struct segQueryFirstContext { @@ -147,8 +147,8 @@ cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers laye layers, group }; - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out); + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFirst, out); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryFunc)segQueryFirst, out); return out->shape; } @@ -167,7 +167,7 @@ bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context) { if( !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpBBintersects(*bb, shape->bb) + cpBBIntersects(*bb, shape->bb) ){ context->func(shape, context->data); } @@ -179,9 +179,9 @@ cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceB bbQueryContext context = {layers, group, func, data}; cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context); - cpSpatialIndexQuery(space->staticShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context); - } cpSpaceUnlock(space); + cpSpatialIndexQuery(space->activeShapes, &bb, bb, (cpSpatialIndexQueryFunc)bbQueryHelper, &context); + cpSpatialIndexQuery(space->staticShapes, &bb, bb, (cpSpatialIndexQueryFunc)bbQueryHelper, &context); + } cpSpaceUnlock(space, cpTrue); } #pragma mark Shape Query Functions @@ -238,9 +238,9 @@ cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, vo shapeQueryContext context = {func, data, cpFalse}; cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context); - cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context); - } cpSpaceUnlock(space); + cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)shapeQueryHelper, &context); + cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)shapeQueryHelper, &context); + } cpSpaceUnlock(space, cpTrue); return context.anyCollision; } diff --git a/src/helper/chipmunk/cpSpaceStep.c b/src/helper/chipmunk/cpSpaceStep.c index 4a6440240..6a9afff37 100644 --- a/src/helper/chipmunk/cpSpaceStep.c +++ b/src/helper/chipmunk/cpSpaceStep.c @@ -27,21 +27,21 @@ #pragma mark Post Step Callback Functions -typedef struct postStepCallback { +struct cpPostStepCallback { cpPostStepFunc func; void *obj; void *data; -} postStepCallback; +}; static cpBool -postStepFuncSetEql(postStepCallback *a, postStepCallback *b){ +postStepFuncSetEql(cpPostStepCallback *a, cpPostStepCallback *b){ return a->obj == b->obj; } static void * -postStepFuncSetTrans(postStepCallback *callback, void *ignored) +postStepFuncSetTrans(cpPostStepCallback *callback, void *ignored) { - postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback)); + cpPostStepCallback *value = (cpPostStepCallback *)cpmalloc(sizeof(cpPostStepCallback)); (*value) = (*callback); return value; @@ -50,14 +50,76 @@ postStepFuncSetTrans(postStepCallback *callback, void *ignored) void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data) { + cpAssertWarn(space->locked, + "Adding a post-step callback when the space is not locked is unnecessary. " + "Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query."); + if(!space->postStepCallbacks){ - space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, NULL); + space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql); } - postStepCallback callback = {func, obj, data}; + cpPostStepCallback callback = {func, obj, data}; cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL, (cpHashSetTransFunc)postStepFuncSetTrans); } +void * +cpSpaceGetPostStepData(cpSpace *space, void *obj) +{ + if(space->postStepCallbacks){ + cpPostStepCallback query = {NULL, obj, NULL}; + cpPostStepCallback *callback = (cpPostStepCallback *)cpHashSetFind(space->postStepCallbacks, (cpHashValue)(size_t)obj, &query); + return (callback ? callback->data : NULL); + } else { + return NULL; + } +} + +static void +cpSpacePostStepCallbackSetIter(cpPostStepCallback *callback, cpSpace *space) +{ + callback->func(space, callback->obj, callback->data); + cpfree(callback); +} + +static void +cpSpaceRunPostStepCallbacks(cpSpace *space) +{ + // Loop because post step callbacks may add more post step callbacks directly or indirectly. + while(space->postStepCallbacks){ + cpHashSet *callbacks = space->postStepCallbacks; + space->postStepCallbacks = NULL; + + cpHashSetEach(callbacks, (cpHashSetIteratorFunc)cpSpacePostStepCallbackSetIter, space); + cpHashSetFree(callbacks); + } +} + +#pragma mark Locking Functions + +void +cpSpaceLock(cpSpace *space) +{ + space->locked++; +} + +void +cpSpaceUnlock(cpSpace *space, cpBool runPostStep) +{ + space->locked--; + cpAssert(space->locked >= 0, "Internal Error: Space lock underflow."); + + if(!space->locked){ + cpArray *waking = space->rousedBodies; + for(int i=0, count=waking->num; iarr[i]); + } + + waking->num = 0; + + cpSpaceRunPostStepCallbacks(space); + } +} + #pragma mark Contact Buffer Functions struct cpContactBufferHeader { @@ -90,7 +152,7 @@ cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpCo return header; } -static void +void cpSpacePushFreshContactBuffer(cpSpace *space) { cpTimestamp stamp = space->stamp; @@ -100,7 +162,7 @@ cpSpacePushFreshContactBuffer(cpSpace *space) if(!head){ // No buffers have been allocated, make one space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL); - } else if(stamp - head->next->stamp > cp_contact_persistence){ + } else if(stamp - head->next->stamp > space->collisionPersistence){ // The tail buffer is available, rotate the ring cpContactBufferHeader *tail = head->next; space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); @@ -131,15 +193,15 @@ cpSpacePushContacts(cpSpace *space, int count) space->contactBuffersHead->numContacts += count; } -static inline void +void cpSpacePopContacts(cpSpace *space, int count){ space->contactBuffersHead->numContacts -= count; } #pragma mark Collision Detection Functions -static void * -contactSetTrans(cpShape **shapes, cpSpace *space) +void * +cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) { if(space->pooledArbiters->num == 0){ // arbiter pool is exhausted, make more @@ -160,7 +222,7 @@ queryReject(cpShape *a, cpShape *b) { return ( // BBoxes must overlap - !cpBBintersects(a->bb, b->bb) + !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 @@ -171,8 +233,8 @@ queryReject(cpShape *a, cpShape *b) } // Callback from the spatial hash. -void -cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) +static void +collideShapes(cpShape *a, cpShape *b, cpSpace *space) { // Reject any of the simple cases if(queryReject(a,b)) return; @@ -180,10 +242,10 @@ cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) // Find the collision pair function for the shapes. cpCollisionType types[] = {a->collision_type, b->collision_type}; cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type); - cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, types); + cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, collHashID, types); cpBool sensor = a->sensor || b->sensor; - if(sensor && handler == &cpSpaceDefaultHandler) return; + if(sensor && handler == &cpDefaultCollisionHandler) return; // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) if(a->klass->type > b->klass->type){ @@ -198,11 +260,11 @@ cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) if(!numContacts) return; // Shapes are not colliding. cpSpacePushContacts(space, numContacts); - // Get an arbiter from space->contactSet for the two shapes. + // Get an arbiter from space->arbiterSet 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, (cpHashSetTransFunc)contactSetTrans); + cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, space, (cpHashSetTransFunc)cpSpaceArbiterSetTrans); cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // Call the begin function first if it's the first step @@ -225,8 +287,8 @@ cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) arb->contacts = NULL; arb->numContacts = 0; - // Normally arbiters are set as used after calling the post-step callback. - // However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve. + // Normally arbiters are set as used after calling the post-solve callback. + // However, post-solve callbacks are not called for sensors or arbiters rejected from pre-solve. if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal; } @@ -235,18 +297,28 @@ cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) } // Hashset filter func to throw away old arbiters. -static cpBool -contactSetFilter(cpArbiter *arb, cpSpace *space) +cpBool +cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) { cpTimestamp ticks = space->stamp - arb->stamp; - // was used last frame, but not this one + cpBody *a = arb->body_a, *b = arb->body_b; + + // Preserve arbiters on sensors and rejected arbiters for sleeping objects. + if( + (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && + (cpBodyIsStatic(b) || cpBodyIsSleeping(b)) + ){ + return cpTrue; + } + + // Arbiter was used last frame, but not this one if(ticks >= 1 && arb->state != cpArbiterStateCached){ arb->handler->separate(arb, space, arb->handler->data); arb->state = cpArbiterStateCached; } - if(ticks >= cp_contact_persistence){ + if(ticks >= space->collisionPersistence){ arb->contacts = NULL; arb->numContacts = 0; @@ -257,20 +329,10 @@ contactSetFilter(cpArbiter *arb, cpSpace *space) return cpTrue; } -// Hashset filter func to call and throw away post step callbacks. -static void -postStepCallbackSetIter(postStepCallback *callback, cpSpace *space) -{ - callback->func(space, callback->obj, callback->data); - cpfree(callback); -} - #pragma mark All Important cpSpaceStep() Function -void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); - -static void -updateBBCache(cpShape *shape, void *unused) +void +cpShapeUpdateFunc(cpShape *shape, void *unused) { cpBody *body = shape->body; cpShapeUpdate(shape, body->p, body->rot); @@ -279,12 +341,8 @@ updateBBCache(cpShape *shape, void *unused) void cpSpaceStep(cpSpace *space, cpFloat dt) { - if(!dt) return; // don't step if the timestep is 0! - cpFloat dt_inv = 1.0f/dt; - - cpArray *bodies = space->bodies; - cpArray *constraints = space->constraints; - + if(dt == 0.0f) return; // don't step if the timestep is 0! + // Reset and empty the arbiter list. cpArray *arbiters = space->arbiters; for(int i=0; inum; i++){ @@ -294,38 +352,38 @@ cpSpaceStep(cpSpace *space, cpFloat dt) arbiters->num = 0; // Integrate positions + cpArray *bodies = space->bodies; for(int i=0; inum; i++){ cpBody *body = (cpBody *)bodies->arr[i]; body->position_func(body, dt); } // Find colliding pairs. - cpSpaceLock(space); - cpSpacePushFreshContactBuffer(space); - cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)updateBBCache, NULL); - cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryCallback)cpSpaceCollideShapes, space); - cpSpaceUnlock(space); + cpSpaceLock(space); { + cpSpacePushFreshContactBuffer(space); + cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); + cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)collideShapes, space); + } cpSpaceUnlock(space, cpFalse); // If body sleeping is enabled, do that now. - if(space->sleepTimeThreshold != INFINITY){ + if(space->sleepTimeThreshold != INFINITY || space->enableContactGraph){ cpSpaceProcessComponents(space, dt); } // Clear out old cached arbiters and call separate callbacks - cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space); + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); // Prestep the arbiters and constraints. cpFloat slop = space->collisionSlop; - // TODO should be dt independent? - // 1.0f - cpfpow(error after 1s, dt); - cpFloat bias = space->collisionBias; + cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); for(int i=0; inum; i++){ - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv, slop, bias); + cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef); } + cpArray *constraints = space->constraints; for(int i=0; inum; i++){ cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - constraint->klass->preStep(constraint, dt, dt_inv); + constraint->klass->preStep(constraint, dt); } // Integrate velocities. @@ -336,13 +394,15 @@ cpSpaceStep(cpSpace *space, cpFloat dt) body->velocity_func(body, gravity, damping, dt); } - if(space->stamp){ - cpFloat dt_coef = dt/space->prev_dt; - for(int i=0; inum; i++){ - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef); - } - - // TODO Separate constraint cached impulses + // Apply cached impulses + cpFloat dt_coef = (space->stamp ? dt/space->prev_dt : 0.0f); + for(int i=0; inum; i++){ + cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef); + } + + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + constraint->klass->applyCachedImpulse(constraint, dt_coef); } // Run the impulse solver. @@ -357,7 +417,7 @@ cpSpaceStep(cpSpace *space, cpFloat dt) } } - // run the post solve callbacks + // run the post-solve callbacks cpSpaceLock(space); for(int i=0; inum; i++){ cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; @@ -365,17 +425,7 @@ cpSpaceStep(cpSpace *space, cpFloat dt) cpCollisionHandler *handler = arb->handler; handler->postSolve(arb, space, handler->data); } - cpSpaceUnlock(space); - - // Run the post step callbacks - // Loop because post step callbacks may create more post step callbacks - while(space->postStepCallbacks){ - cpHashSet *callbacks = space->postStepCallbacks; - space->postStepCallbacks = NULL; - - cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space); - cpHashSetFree(callbacks); - } + cpSpaceUnlock(space, cpTrue); // Increment the stamp. space->stamp++; diff --git a/src/helper/chipmunk/cpSpatialIndex.c b/src/helper/chipmunk/cpSpatialIndex.c index 887082b5d..fff5aa62f 100644 --- a/src/helper/chipmunk/cpSpatialIndex.c +++ b/src/helper/chipmunk/cpSpatialIndex.c @@ -11,13 +11,6 @@ cpSpatialIndexFree(cpSpatialIndex *index) } } -typedef struct dynamicToStaticContext { - cpSpatialIndexBBFunc bbfunc; - cpSpatialIndex *staticIndex; - cpSpatialIndexQueryCallback queryFunc; - void *data; -} dynamicToStaticContext; - cpSpatialIndex * cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) { @@ -33,6 +26,13 @@ cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialI return index; } +typedef struct dynamicToStaticContext { + cpSpatialIndexBBFunc bbfunc; + cpSpatialIndex *staticIndex; + cpSpatialIndexQueryFunc queryFunc; + void *data; +} dynamicToStaticContext; + static void dynamicToStaticIter(void *obj, dynamicToStaticContext *context) { @@ -40,11 +40,11 @@ dynamicToStaticIter(void *obj, dynamicToStaticContext *context) } void -cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data) +cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data) { if(cpSpatialIndexCount(staticIndex) > 0){ dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; - cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIterator)dynamicToStaticIter, &context); + cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context); } } diff --git a/src/helper/chipmunk/cpSpatialIndex.h b/src/helper/chipmunk/cpSpatialIndex.h index 9d6ec0185..996c890d3 100644 --- a/src/helper/chipmunk/cpSpatialIndex.h +++ b/src/helper/chipmunk/cpSpatialIndex.h @@ -1,14 +1,60 @@ +/* Copyright (c) 2010 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + @defgroup cpSpatialIndex cpSpatialIndex + + Spatial indexes are data structures that are used to accelerate collision detection + and spatial queries. Chipmunk provides a number of spatial index algorithms to pick from + and they are programmed in a generic way so that you can use them for holding more than + just cpShape structs. + + It works by using @c void pointers to the objects you add and using a callback to ask your code + for bounding boxes when it needs them. Several types of queries can be performed an index as well + as reindexing and full collision information. All communication to the spatial indexes is performed + through callback functions. + + Spatial indexes should be treated as opaque structs. + This meanns you shouldn't be reading any of the struct fields. + @{ +*/ + #pragma mark Spatial Index +/// Spatial index bounding box callback function type. +/// The spatial index calls this function and passes you a pointer to an object you added +/// when it needs to get the bounding box associated with that object. typedef cpBB (*cpSpatialIndexBBFunc)(void *obj); -typedef void (*cpSpatialIndexIterator)(void *obj, void *data); -typedef void (*cpSpatialIndexQueryCallback)(void *obj1, void *obj2, void *data); -typedef cpFloat (*cpSpatialIndexSegmentQueryCallback)(void *obj1, void *obj2, void *data); +/// Spatial index/object iterator callback function type. +typedef void (*cpSpatialIndexIteratorFunc)(void *obj, void *data); +/// Spatial query callback function type. +typedef void (*cpSpatialIndexQueryFunc)(void *obj1, void *obj2, void *data); +/// Spatial segment query callback function type. +typedef cpFloat (*cpSpatialIndexSegmentQueryFunc)(void *obj1, void *obj2, void *data); typedef struct cpSpatialIndexClass cpSpatialIndexClass; typedef struct cpSpatialIndex cpSpatialIndex; +/// @private struct cpSpatialIndex { cpSpatialIndexClass *klass; @@ -22,138 +68,181 @@ struct cpSpatialIndex { typedef struct cpSpaceHash cpSpaceHash; +/// Allocate a spatial hash. cpSpaceHash *cpSpaceHashAlloc(void); +/// Initialize a spatial hash. cpSpatialIndex *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a spatial hash. cpSpatialIndex *cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Change the cell dimensions and table size of the spatial hash to tune it. +/// The cell dimensions should roughly match the average size of your objects +/// and the table size should be ~10 larger than the number of objects inserted. +/// Some trial and error is required to find the optimum numbers for efficiency. void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); #pragma mark AABB Tree typedef struct cpBBTree cpBBTree; +/// Allocate a bounding box tree. cpBBTree *cpBBTreeAlloc(void); +/// Initialize a bounding box tree. cpSpatialIndex *cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a bounding box tree. cpSpatialIndex *cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Perform a static top down optimization of the tree. void cpBBTreeOptimize(cpSpatialIndex *index); +/// Bounding box tree velocity callback function. +/// This function should return an estimate for the object's velocity. typedef cpVect (*cpBBTreeVelocityFunc)(void *obj); +/// Set the velocity function for the bounding box tree to enable temporal coherence. void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func); +#pragma mark Single Axis Sweep + +typedef struct cpSweep1D cpSweep1D; + +/// Allocate a 1D sort and sweep broadphase. +cpSweep1D *cpSweep1DAlloc(void); +/// Initialize a 1D sort and sweep broadphase. +cpSpatialIndex *cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a 1D sort and sweep broadphase. +cpSpatialIndex *cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + #pragma mark Spatial Index Implementation -typedef void (*cpSpatialIndexDestroyFunc)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexDestroyImpl)(cpSpatialIndex *index); -typedef int (*cpSpatialIndexCountFunc)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexEachFunc)(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data); +typedef int (*cpSpatialIndexCountImpl)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexEachImpl)(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data); -typedef cpBool (*cpSpatialIndexContainsFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexInsertFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexRemoveFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef cpBool (*cpSpatialIndexContainsImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexInsertImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexRemoveImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexReindexFunc)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexReindexObjectFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexReindexQueryFunc)(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data); +typedef void (*cpSpatialIndexReindexImpl)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexReindexObjectImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexReindexQueryImpl)(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data); -typedef void (*cpSpatialIndexPointQueryFunc)(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data); -typedef void (*cpSpatialIndexSegmentQueryFunc)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data); -typedef void (*cpSpatialIndexQueryFunc)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data); +typedef void (*cpSpatialIndexPointQueryImpl)(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryFunc func, void *data); +typedef void (*cpSpatialIndexSegmentQueryImpl)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data); +typedef void (*cpSpatialIndexQueryImpl)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data); struct cpSpatialIndexClass { - cpSpatialIndexDestroyFunc destroy; + cpSpatialIndexDestroyImpl destroy; - cpSpatialIndexCountFunc count; - cpSpatialIndexEachFunc each; + cpSpatialIndexCountImpl count; + cpSpatialIndexEachImpl each; - cpSpatialIndexContainsFunc contains; - cpSpatialIndexInsertFunc insert; - cpSpatialIndexRemoveFunc remove; + cpSpatialIndexContainsImpl contains; + cpSpatialIndexInsertImpl insert; + cpSpatialIndexRemoveImpl remove; - cpSpatialIndexReindexFunc reindex; - cpSpatialIndexReindexObjectFunc reindexObject; - cpSpatialIndexReindexQueryFunc reindexQuery; + cpSpatialIndexReindexImpl reindex; + cpSpatialIndexReindexObjectImpl reindexObject; + cpSpatialIndexReindexQueryImpl reindexQuery; - cpSpatialIndexPointQueryFunc pointQuery; - cpSpatialIndexSegmentQueryFunc segmentQuery; - cpSpatialIndexQueryFunc query; + cpSpatialIndexPointQueryImpl pointQuery; + cpSpatialIndexSegmentQueryImpl segmentQuery; + cpSpatialIndexQueryImpl query; }; - +/// Destroy and free a spatial index. void cpSpatialIndexFree(cpSpatialIndex *index); -void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data); +/// Collide the objects in @c dynamicIndex against the objects in @c staticIndex using the query callback function. +void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data); +/// Destroy a spatial index. static inline void cpSpatialIndexDestroy(cpSpatialIndex *index) { index->klass->destroy(index); } +/// Get the number of objects in the spatial index. static inline int cpSpatialIndexCount(cpSpatialIndex *index) { return index->klass->count(index); } +/// Iterate the objects in the spatial index. @c func will be called once for each object. static inline void -cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data) +cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data) { index->klass->each(index, func, data); } +/// Returns true if the spatial index contains the given object. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. static inline cpBool cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid) { return index->klass->contains(index, obj, hashid); } +/// Add an object to a spatial index. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. static inline void cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid) { index->klass->insert(index, obj, hashid); } +/// Remove an object from a spatial index. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. static inline void cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid) { index->klass->remove(index, obj, hashid); } +/// Perform a full reindex of a spatial index. static inline void cpSpatialIndexReindex(cpSpatialIndex *index) { index->klass->reindex(index); } +/// Reindex a single object in the spatial index. static inline void cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid) { index->klass->reindexObject(index, obj, hashid); } -// TODO make sure to doc the callback type, a pointer to the point is passed as obj1 +/// Perform a point query against the spatial index, calling @c func for each potential match. +/// A pointer to the point will be passed as @c obj1 of @c func. static inline void -cpSpatialIndexPointQuery(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data) +cpSpatialIndexPointQuery(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryFunc func, void *data) { index->klass->pointQuery(index, point, func, data); } -// TODO make sure to doc the callback type +/// Perform a segment query against the spatial index, calling @c func for each potential match. static inline void -cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { index->klass->segmentQuery(index, obj, a, b, t_exit, func, data); } - +/// Perform a rectangle query against the spatial index, calling @c func for each potential match. static inline void -cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) { index->klass->query(index, obj, bb, func, data); } +/// Simultaneously reindex and find all colliding objects. +/// @c func will be called once for each potentially overlapping pair of objects found. +/// If the spatial index was initialized with a static index, it will collide it's objects against that as well. static inline void -cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data) +cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data) { index->klass->reindexQuery(index, func, data); } + +///@} diff --git a/src/helper/chipmunk/cpSweep1D.c b/src/helper/chipmunk/cpSweep1D.c new file mode 100644 index 000000000..34afcc8eb --- /dev/null +++ b/src/helper/chipmunk/cpSweep1D.c @@ -0,0 +1,269 @@ +/* Copyright (c) 2010 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" + +#ifdef _MSC_VER +// Are you freaking kidding me? +// Can it really not tell the difference between a declaration and a definition? +// Have I ever mentioned how much I hate MSVC? +extern +#else +static +#endif +cpSpatialIndexClass klass; + +#pragma mark Basic Structures + +typedef struct Bounds { + cpFloat min, max; +} Bounds; + +typedef struct TableCell { + void *obj; + Bounds bounds; +} TableCell; + +struct cpSweep1D +{ + cpSpatialIndex spatialIndex; + + int num; + int max; + TableCell *table; +}; + +static inline cpBool +BoundsOverlap(Bounds a, Bounds b) +{ + return (a.min <= b.max && b.min <= a.max); +} + +static inline Bounds +BBToBounds(cpSweep1D *sweep, cpBB bb) +{ + Bounds bounds = {bb.l, bb.r}; + return bounds; +} + +static inline TableCell +MakeTableCell(cpSweep1D *sweep, void *obj) +{ + TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))}; + return cell; +} + +#pragma mark Memory Management Functions + +cpSweep1D * +cpSweep1DAlloc(void) +{ + return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D)); +} + +static void +ResizeTable(cpSweep1D *sweep, int size) +{ + sweep->max = size; + sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell)); +} + +cpSpatialIndex * +cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)sweep, &klass, bbfunc, staticIndex); + + sweep->num = 0; + ResizeTable(sweep, 32); + + return (cpSpatialIndex *)sweep; +} + +cpSpatialIndex * +cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex); +} + +static void +cpSweep1DDestroy(cpSweep1D *sweep) +{ + cpfree(sweep->table); + sweep->table = NULL; +} + +#pragma mark Misc + +static int +cpSweep1DCount(cpSweep1D *sweep) +{ + return sweep->num; +} + +static void +cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data) +{ + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; itable; + for(int i=0, count=sweep->num; inum == sweep->max) ResizeTable(sweep, sweep->max*2); + + sweep->table[sweep->num] = MakeTableCell(sweep, obj); + sweep->num++; +} + +static void +cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid) +{ + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; inum; + + table[i] = table[num]; + table[num].obj = NULL; + + return; + } + } +} + +#pragma mark Reindexing Functions + +static void +cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid) +{ + // Nothing to do here +} + +static void +cpSweep1DReindex(cpSweep1D *sweep) +{ + // Nothing to do here + // Could perform a sort, but queries are not accelerated anyway. +} + +#pragma mark Query Functions + +static void +cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) +{ + // Implementing binary search here would allow you to find an upper limit + // but not a lower limit. Probably not worth the hassle. + + Bounds bounds = BBToBounds(sweep, bb); + + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; itable; + for(int i=0, count=sweep->num; ibounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0)); +} + +static void +cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data) +{ + TableCell *table = sweep->table; + int count = sweep->num; + + // Update bounds and sort + for(int i=0; ispatialIndex.staticIndex, func, data); +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyImpl)cpSweep1DDestroy, + + (cpSpatialIndexCountImpl)cpSweep1DCount, + (cpSpatialIndexEachImpl)cpSweep1DEach, + (cpSpatialIndexContainsImpl)cpSweep1DContains, + + (cpSpatialIndexInsertImpl)cpSweep1DInsert, + (cpSpatialIndexRemoveImpl)cpSweep1DRemove, + + (cpSpatialIndexReindexImpl)cpSweep1DReindex, + (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject, + (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery, + + (cpSpatialIndexPointQueryImpl)cpSweep1DPointQuery, + (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, + (cpSpatialIndexQueryImpl)cpSweep1DQuery, +}; diff --git a/src/helper/chipmunk/cpVect.c b/src/helper/chipmunk/cpVect.c index 39554e620..4df30f192 100644 --- a/src/helper/chipmunk/cpVect.c +++ b/src/helper/chipmunk/cpVect.c @@ -27,7 +27,7 @@ cpFloat cpvlength(const cpVect v) { - return cpfsqrt( cpvdot(v, v) ); + return cpfsqrt(cpvdot(v, v)); } inline cpVect diff --git a/src/helper/chipmunk/cpVect.h b/src/helper/chipmunk/cpVect.h index 261cec522..085995ffb 100644 --- a/src/helper/chipmunk/cpVect.h +++ b/src/helper/chipmunk/cpVect.h @@ -19,6 +19,10 @@ * SOFTWARE. */ +/// @defgroup cpVect cpVect +/// Chipmunk's 2D vector type along with a handy 2D vector math lib. +/// @{ + /// Constant for the zero vector. static const cpVect cpvzero = {0.0f,0.0f}; @@ -30,8 +34,6 @@ cpv(const cpFloat x, const cpFloat y) return v; } -// non-inlined functions - /// Returns the length of v. cpFloat cpvlength(const cpVect v); @@ -47,12 +49,9 @@ 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. -*/ +/// 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!) @@ -97,11 +96,9 @@ 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. -*/ +/// 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) { @@ -205,3 +202,4 @@ cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) { return cpvdistsq(v1, v2) < dist*dist; } +/// @} diff --git a/src/physics/cbody.cpp b/src/physics/cbody.cpp index 706728c26..bb0a67d65 100644 --- a/src/physics/cbody.cpp +++ b/src/physics/cbody.cpp @@ -166,7 +166,7 @@ void cBody::AngVelLimit( const cpFloat& speed ) { } void cBody::Slew( cVect pos, cpFloat dt ) { - cpBodySlew( mBody, tocpv( pos ), dt ); + //cpBodySlew( mBody, tocpv( pos ), dt ); } void cBody::UpdateVelocity( cVect gravity, cpFloat damping, cpFloat dt ) { @@ -209,12 +209,4 @@ void cBody::Data( void * data ) { mData = data; } -bool cBody::IsGrounded() { - return cpTrue == cpBodyIsGrounded( mBody ); -} - -bool cBody::IsGroundedTolerance( cVect normal, cpFloat tolerance ) { - return cpTrue == cpBodyIsGroundedTolerance( mBody, tocpv( normal ), tolerance ); -} - CP_NAMESPACE_END diff --git a/src/physics/cbody.hpp b/src/physics/cbody.hpp index e46972ff7..64afc3ec2 100644 --- a/src/physics/cbody.hpp +++ b/src/physics/cbody.hpp @@ -5,6 +5,7 @@ CP_NAMESPACE_BEGIN +//! TODO: Search for a IsGrounded replacement. class CP_API cBody { public: static cBody * New( cpFloat m, cpFloat i ); @@ -100,10 +101,6 @@ class CP_API cBody { void * Data() const; void Data( void * data ); - - bool IsGrounded(); - - bool IsGroundedTolerance( cVect normal, cpFloat tolerance ); protected: friend class cSpace; diff --git a/src/physics/constraints/cconstraint.cpp b/src/physics/constraints/cconstraint.cpp index cfd06dbd2..f1870df76 100644 --- a/src/physics/constraints/cconstraint.cpp +++ b/src/physics/constraints/cconstraint.cpp @@ -46,14 +46,6 @@ void cConstraint::MaxForce( const cpFloat& maxforce ) { mConstraint->maxForce = maxforce; } -cpFloat cConstraint::BiasCoef() { - return mConstraint->biasCoef; -} - -void cConstraint::BiasCoef( const cpFloat& biascoef ) { - mConstraint->biasCoef = biascoef; -} - cpFloat cConstraint::MaxBias() { return mConstraint->maxBias; } diff --git a/src/physics/constraints/cconstraint.hpp b/src/physics/constraints/cconstraint.hpp index bd916cff1..32f68f1a1 100644 --- a/src/physics/constraints/cconstraint.hpp +++ b/src/physics/constraints/cconstraint.hpp @@ -24,10 +24,6 @@ class CP_API cConstraint { void MaxForce( const cpFloat& maxforce ); - cpFloat BiasCoef(); - - void BiasCoef( const cpFloat& biascoef ); - cpFloat MaxBias(); void MaxBias( const cpFloat& maxbias ); diff --git a/src/physics/cphysicsmanager.cpp b/src/physics/cphysicsmanager.cpp index 341fc33ae..81aa1c332 100644 --- a/src/physics/cphysicsmanager.cpp +++ b/src/physics/cphysicsmanager.cpp @@ -34,38 +34,6 @@ cPhysicsManager::~cPhysicsManager() { } } -const cpFloat& cPhysicsManager::BiasCoef() const { - return cp_bias_coef; -} - -void cPhysicsManager::BiasCoef( const cpFloat& biasCoef ) { - cp_bias_coef = biasCoef; -} - -const cpFloat& cPhysicsManager::ConstraintBiasCoef() const { - return cp_constraint_bias_coef; -} - -void cPhysicsManager::ConstraintBiasCoef( const cpFloat& constraintBiasCoef ) { - cp_constraint_bias_coef = constraintBiasCoef; -} - -const cpTimestamp& cPhysicsManager::ContactPersistence() const { - return cp_contact_persistence; -} - -void cPhysicsManager::ContactPersistence( const cpTimestamp& timestamp ) { - cp_contact_persistence = timestamp; -} - -const cpFloat& cPhysicsManager::CollisionSlop() const { - return cp_collision_slop; -} - -void cPhysicsManager::CollisionSlop( const cpFloat& slop ) { - cp_collision_slop = slop; -} - cPhysicsManager::cDrawSpaceOptions * cPhysicsManager::GetDrawOptions() { return &mOptions; } diff --git a/src/physics/cphysicsmanager.hpp b/src/physics/cphysicsmanager.hpp index 7965edf3b..6702ecd77 100644 --- a/src/physics/cphysicsmanager.hpp +++ b/src/physics/cphysicsmanager.hpp @@ -44,22 +44,6 @@ class CP_API cPhysicsManager : public tSingleton { const bool& MemoryManager() const; - const cpFloat& BiasCoef() const; - - void BiasCoef( const cpFloat& biasCoef ); - - const cpFloat& ConstraintBiasCoef() const; - - void ConstraintBiasCoef( const cpFloat& constraintBiasCoef ); - - const cpTimestamp& ContactPersistence() const; - - void ContactPersistence( const cpTimestamp& timestamp ); - - const cpFloat& CollisionSlop() const; - - void CollisionSlop( const cpFloat& slop ); - cPhysicsManager::cDrawSpaceOptions * GetDrawOptions(); protected: cDrawSpaceOptions mOptions; diff --git a/src/physics/cspace.cpp b/src/physics/cspace.cpp index 2aa0541ec..75798b072 100644 --- a/src/physics/cspace.cpp +++ b/src/physics/cspace.cpp @@ -60,18 +60,6 @@ void cSpace::Update() { #endif } -void cSpace::ResizeStaticHash( cpFloat dim, int count ) { - cpSpaceResizeStaticHash( mSpace, dim, count ); -} - -void cSpace::ResizeActiveHash( cpFloat dim, int count ) { - cpSpaceResizeActiveHash( mSpace, dim, count ); -} - -void cSpace::RehashStatic() { - cpSpaceRehashStatic( mSpace ); -} - const int& cSpace::Iterations() const { return mSpace->iterations; } @@ -252,10 +240,6 @@ void cSpace::ActivateShapesTouchingShape( cShape * shape ) { cpSpaceActivateShapesTouchingShape( mSpace, shape->Shape() ); } -void cSpace::RehashShape( cShape * shape ) { - cpSpaceRehashShape( mSpace, shape->Shape() ); -} - #ifdef PHYSICS_RENDERER_ENABLED static void drawObject( cpShape * shape, cpSpace * space ) { reinterpret_cast ( shape->data )->Draw( reinterpret_cast( space->data ) ); @@ -289,15 +273,15 @@ void cSpace::Draw() { BR->SetLineWidth( options->LineThickness ); if( options->DrawShapes ) { - cpSpatialIndexEach( mSpace->CP_PRIVATE(activeShapes), (cpSpatialIndexIterator)drawObject, mSpace ); - cpSpatialIndexEach( mSpace->CP_PRIVATE(staticShapes), (cpSpatialIndexIterator)drawObject, mSpace ); + cpSpatialIndexEach( mSpace->CP_PRIVATE(activeShapes), (cpSpatialIndexIteratorFunc)drawObject, mSpace ); + cpSpatialIndexEach( mSpace->CP_PRIVATE(staticShapes), (cpSpatialIndexIteratorFunc)drawObject, mSpace ); } BR->SetLineWidth( lw ); if( options->DrawBBs ){ - cpSpatialIndexEach( mSpace->CP_PRIVATE(activeShapes), (cpSpatialIndexIterator)drawBB, NULL ); - cpSpatialIndexEach( mSpace->CP_PRIVATE(staticShapes), (cpSpatialIndexIterator)drawBB, NULL ); + cpSpatialIndexEach( mSpace->CP_PRIVATE(activeShapes), (cpSpatialIndexIteratorFunc)drawBB, NULL ); + cpSpatialIndexEach( mSpace->CP_PRIVATE(staticShapes), (cpSpatialIndexIteratorFunc)drawBB, NULL ); } cpArray * constraints = mSpace->CP_PRIVATE(constraints); diff --git a/src/physics/cspace.hpp b/src/physics/cspace.hpp index 2b19114c2..6f18aaa84 100644 --- a/src/physics/cspace.hpp +++ b/src/physics/cspace.hpp @@ -154,14 +154,6 @@ class CP_API cSpace { void RemoveConstraint( cConstraint * constraint ); - void ResizeStaticHash( cpFloat dim, int count ); - - void ResizeActiveHash( cpFloat dim, int count ); - - void RehashStatic(); - - void RehashShape( cShape * shape ); - cpSpace * Space() const; void ActivateShapesTouchingShape( cShape * shape ); diff --git a/src/test/eetest.cpp b/src/test/eetest.cpp index aaef45b7b..7dcd9566d 100644 --- a/src/test/eetest.cpp +++ b/src/test/eetest.cpp @@ -1714,7 +1714,6 @@ void cEETest::ChangeDemo( Uint32 num ) { void cEETest::PhysicsCreate() { cPhysicsManager::CreateSingleton(); - cPhysicsManager::instance()->CollisionSlop( 0.2 ); cPhysicsManager * PM = cPhysicsManager::instance(); cPhysicsManager::cDrawSpaceOptions * DSO = PM->GetDrawOptions(); @@ -1759,7 +1758,6 @@ void cEETest::PhysicsUpdate() { mMouseJoint = eeNew( cPivotJoint, ( mMouseBody, shape->Body(), cVectZero, shape->Body()->World2Local( point ) ) ); mMouseJoint->MaxForce( 50000.0f ); - mMouseJoint->BiasCoef( 0.15f ); mSpace->AddConstraint( mMouseJoint ); } }