From c031cd80f91e180337627e5e7572a0cf368c16b6 Mon Sep 17 00:00:00 2001 From: "spartanj@gmail.com" Date: Fri, 23 Nov 2012 22:55:47 -0300 Subject: [PATCH] Updated Chipmunk to 6.1.2 --- include/eepp/helper/chipmunk/chipmunk.h | 95 ++++-- include/eepp/helper/chipmunk/chipmunk_ffi.h | 21 +- .../eepp/helper/chipmunk/chipmunk_private.h | 69 ++++- include/eepp/helper/chipmunk/chipmunk_types.h | 35 ++- .../chipmunk/constraints/cpConstraint.h | 36 ++- .../constraints/cpDampedRotarySpring.h | 17 +- .../chipmunk/constraints/cpDampedSpring.h | 20 +- .../helper/chipmunk/constraints/cpGearJoint.h | 14 +- .../chipmunk/constraints/cpGrooveJoint.h | 17 +- .../helper/chipmunk/constraints/cpPinJoint.h | 16 +- .../chipmunk/constraints/cpPivotJoint.h | 17 +- .../chipmunk/constraints/cpRatchetJoint.h | 16 +- .../chipmunk/constraints/cpRotaryLimitJoint.h | 14 +- .../chipmunk/constraints/cpSimpleMotor.h | 12 +- .../chipmunk/constraints/cpSlideJoint.h | 18 +- .../eepp/helper/chipmunk/constraints/util.h | 56 ++-- include/eepp/helper/chipmunk/cpArbiter.h | 36 +-- include/eepp/helper/chipmunk/cpBB.h | 51 +++- include/eepp/helper/chipmunk/cpBody.h | 45 +-- include/eepp/helper/chipmunk/cpPolyShape.h | 25 +- include/eepp/helper/chipmunk/cpShape.h | 90 +++--- include/eepp/helper/chipmunk/cpSpace.h | 61 ++-- include/eepp/helper/chipmunk/cpSpatialIndex.h | 47 ++- include/eepp/helper/chipmunk/cpVect.h | 49 ++- projects/linux/ee.creator.user | 2 +- src/eepp/helper/chipmunk/chipmunk.c | 204 +++++++++++-- .../chipmunk/constraints/cpConstraint.c | 9 +- .../constraints/cpDampedRotarySpring.c | 5 +- .../chipmunk/constraints/cpDampedSpring.c | 5 +- .../helper/chipmunk/constraints/cpGearJoint.c | 12 +- .../chipmunk/constraints/cpGrooveJoint.c | 18 +- .../helper/chipmunk/constraints/cpPinJoint.c | 16 +- .../chipmunk/constraints/cpPivotJoint.c | 14 +- .../chipmunk/constraints/cpRatchetJoint.c | 12 +- .../chipmunk/constraints/cpRotaryLimitJoint.c | 14 +- .../chipmunk/constraints/cpSimpleMotor.c | 12 +- .../chipmunk/constraints/cpSlideJoint.c | 22 +- src/eepp/helper/chipmunk/cpArbiter.c | 84 ++++-- src/eepp/helper/chipmunk/cpArray.c | 1 - src/eepp/helper/chipmunk/cpBB.c | 11 - src/eepp/helper/chipmunk/cpBBTree.c | 181 +++++------ src/eepp/helper/chipmunk/cpBody.c | 44 ++- src/eepp/helper/chipmunk/cpCollision.c | 216 +++++++------- src/eepp/helper/chipmunk/cpHashSet.c | 23 +- src/eepp/helper/chipmunk/cpPolyShape.c | 74 +++-- src/eepp/helper/chipmunk/cpShape.c | 154 +++++----- src/eepp/helper/chipmunk/cpSpace.c | 116 ++++---- src/eepp/helper/chipmunk/cpSpaceComponent.c | 185 ++++++------ src/eepp/helper/chipmunk/cpSpaceHash.c | 44 +-- src/eepp/helper/chipmunk/cpSpaceQuery.c | 168 +++++++---- src/eepp/helper/chipmunk/cpSpaceStep.c | 281 +++++++++--------- src/eepp/helper/chipmunk/cpSpatialIndex.c | 25 +- src/eepp/helper/chipmunk/cpSweep1D.c | 26 +- src/eepp/helper/chipmunk/cpVect.c | 34 +-- 54 files changed, 1696 insertions(+), 1193 deletions(-) diff --git a/include/eepp/helper/chipmunk/chipmunk.h b/include/eepp/helper/chipmunk/chipmunk.h index e1b86e98b..abc50b0c9 100644 --- a/include/eepp/helper/chipmunk/chipmunk.h +++ b/include/eepp/helper/chipmunk/chipmunk.h @@ -19,12 +19,12 @@ * SOFTWARE. */ -/// @defgroup misc Misc -/// @{ - #ifndef CHIPMUNK_HEADER #define CHIPMUNK_HEADER +#include +#include + #ifdef __cplusplus extern "C" { #endif @@ -39,40 +39,45 @@ extern "C" { #define CP_PRIVATE(symbol) symbol##_private #endif -void cpMessage(const char *message, const char *condition, const char *file, int line, int isError); +void cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...); #ifdef NDEBUG - #define cpAssertWarn(condition, message) + #define cpAssertWarn(condition, ...) #else - #define cpAssertWarn(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 0) + #define cpAssertWarn(condition, ...) if(!(condition)) cpMessage(#condition, __FILE__, __LINE__, 0, 0, __VA_ARGS__) +#endif + +#ifdef NDEBUG + #define cpAssertSoft(condition, ...) +#else + #define cpAssertSoft(condition, ...) if(!(condition)) cpMessage(#condition, __FILE__, __LINE__, 1, 0, __VA_ARGS__) #endif // Hard assertions are important and cheap to execute. They are not disabled by compiling as debug. -#define cpAssertHard(condition, message) if(!(condition)) cpMessage(message, #condition, __FILE__, __LINE__, 1) - -#ifdef NDEBUG - #define cpAssertSoft(condition, message) -#else - #define cpAssertSoft(condition, message) cpAssertHard(condition, message) -#endif +#define cpAssertHard(condition, ...) if(!(condition)) cpMessage(#condition, __FILE__, __LINE__, 1, 1, __VA_ARGS__) #include "chipmunk_types.h" -// Allocated size for various Chipmunk buffers +/// @defgroup misc Misc +/// @{ + +/// Allocated size for various Chipmunk buffers #ifndef CP_BUFFER_BYTES #define CP_BUFFER_BYTES (32*1024) #endif -// Chipmunk memory function aliases. #ifndef cpcalloc + /// Chipmunk calloc() alias. #define cpcalloc calloc #endif #ifndef cprealloc + /// Chipmunk realloc() alias. #define cprealloc realloc #endif #ifndef cpfree + /// Chipmunk free() alias. #define cpfree free #endif @@ -101,9 +106,10 @@ typedef struct cpSpace cpSpace; #include "cpSpace.h" +// Chipmunk 6.1.2 #define CP_VERSION_MAJOR 6 -#define CP_VERSION_MINOR 0 -#define CP_VERSION_RELEASE 0 +#define CP_VERSION_MINOR 1 +#define CP_VERSION_RELEASE 2 /// Version string. extern const char *cpVersionString; @@ -111,6 +117,10 @@ extern const char *cpVersionString; /// @deprecated void cpInitChipmunk(void); +/// Enables segment to segment shape collisions. +void cpEnableSegmentToSegmentCollisions(void); + + /// 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); @@ -145,6 +155,56 @@ cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); /// Calculate the moment of inertia for a solid box. cpFloat cpMomentForBox2(cpFloat m, cpBB box); +/// Calculate the convex hull of a given set of points. Returns the count of points in the hull. +/// @c result must be a pointer to a @c cpVect array with at least @c count elements. If @c result is @c NULL, then @c verts will be reduced instead. +/// @c first is an optional pointer to an integer to store where the first vertex in the hull came from (i.e. verts[first] == result[0]) +/// @c tol is the allowed amount to shrink the hull when simplifying it. A tolerance of 0.0 creates an exact hull. +int cpConvexHull(int count, cpVect *verts, cpVect *result, int *first, cpFloat tol); + +#ifdef _MSC_VER +#include "malloc.h" +#endif + +/// Convenience macro to work with cpConvexHull. +/// @c count and @c verts is the input array passed to cpConvexHull(). +/// @c count_var and @c verts_var are the names of the variables the macro creates to store the result. +/// The output vertex array is allocated on the stack using alloca() so it will be freed automatically, but cannot be returned from the current scope. +#define CP_CONVEX_HULL(__count__, __verts__, __count_var__, __verts_var__) \ +cpVect *__verts_var__ = (cpVect *)alloca(__count__*sizeof(cpVect)); \ +int __count_var__ = cpConvexHull(__count__, __verts__, __verts_var__, NULL, 0.0); \ + +#if defined(__has_extension) +#if __has_extension(blocks) +// Define alternate block based alternatives for a few of the callback heavy functions. +// Collision handlers are post-step callbacks are not included to avoid memory management issues. +// If you want to use blocks for those and are aware of how to correctly manage the memory, the implementation is trivial. + +void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)); +void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)); +void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)); + +void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)); +void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)); +void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)); + +typedef void (^cpSpaceNearestPointQueryBlock)(cpShape *shape, cpFloat distance, cpVect point); +void cpSpaceNearestPointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryBlock block); + +typedef void (^cpSpaceSegmentQueryBlock)(cpShape *shape, cpFloat t, cpVect n); +void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryBlock block); + +typedef void (^cpSpaceBBQueryBlock)(cpShape *shape); +void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryBlock block); + +typedef void (^cpSpaceShapeQueryBlock)(cpShape *shape, cpContactPointSet *points); +cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block); + +#endif +#endif + + +//@} + #ifdef __cplusplus } @@ -156,4 +216,3 @@ static inline cpVect operator -(const cpVect v){return cpvneg(v);} #endif #endif -//@} diff --git a/include/eepp/helper/chipmunk/chipmunk_ffi.h b/include/eepp/helper/chipmunk/chipmunk_ffi.h index e88ca9a0d..1960f5cb5 100644 --- a/include/eepp/helper/chipmunk/chipmunk_ffi.h +++ b/include/eepp/helper/chipmunk/chipmunk_ffi.h @@ -14,7 +14,7 @@ #endif #define MAKE_PROPERTIES_REF(struct, property) \ - MAKE_REF(struct##Get##property); MAKE_REF(struct##Set##property); + MAKE_REF(struct##Get##property); MAKE_REF(struct##Set##property) MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() MAKE_REF(cpveql); @@ -47,6 +47,7 @@ MAKE_REF(cpflerp); MAKE_REF(cpflerpconst); MAKE_REF(cpBBNew); +MAKE_REF(cpBBNewForCircle); MAKE_REF(cpBBIntersects); MAKE_REF(cpBBContainsBB); MAKE_REF(cpBBContainsVect); @@ -54,6 +55,7 @@ MAKE_REF(cpBBMerge); MAKE_REF(cpBBExpand); MAKE_REF(cpBBArea); MAKE_REF(cpBBMergedArea); +MAKE_REF(cpBBSegmentQuery); MAKE_REF(cpBBIntersectsSegment); MAKE_REF(cpBodyGetMass); @@ -77,7 +79,7 @@ MAKE_REF(cpBodyKineticEnergy); MAKE_REF(cpShapeGetBB); MAKE_PROPERTIES_REF(cpShape, Body); -MAKE_PROPERTIES_REF(cpShape, IsSensor); +MAKE_PROPERTIES_REF(cpShape, Sensor); MAKE_PROPERTIES_REF(cpShape, Elasticity); MAKE_PROPERTIES_REF(cpShape, Friction); MAKE_PROPERTIES_REF(cpShape, SurfaceVelocity); @@ -150,9 +152,22 @@ MAKE_REF(cpSpatialIndexInsert); MAKE_REF(cpSpatialIndexRemove); MAKE_REF(cpSpatialIndexReindex); MAKE_REF(cpSpatialIndexReindexObject); -MAKE_REF(cpSpatialIndexPointQuery); MAKE_REF(cpSpatialIndexSegmentQuery); MAKE_REF(cpSpatialIndexQuery); MAKE_REF(cpSpatialIndexReindexQuery); +MAKE_PROPERTIES_REF(cpSpace, Iterations); +MAKE_PROPERTIES_REF(cpSpace, Gravity); +MAKE_PROPERTIES_REF(cpSpace, Damping); +MAKE_PROPERTIES_REF(cpSpace, IdleSpeedThreshold); +MAKE_PROPERTIES_REF(cpSpace, SleepTimeThreshold); +MAKE_PROPERTIES_REF(cpSpace, CollisionSlop); +MAKE_PROPERTIES_REF(cpSpace, CollisionBias); +MAKE_PROPERTIES_REF(cpSpace, CollisionPersistence); +MAKE_PROPERTIES_REF(cpSpace, EnableContactGraph); +MAKE_PROPERTIES_REF(cpSpace, UserData); +MAKE_REF(cpSpaceGetStaticBody); +MAKE_REF(cpSpaceGetCurrentTimeStep); +MAKE_REF(cpSpaceIsLocked); + #endif diff --git a/include/eepp/helper/chipmunk/chipmunk_private.h b/include/eepp/helper/chipmunk/chipmunk_private.h index bf848e9c9..335329226 100644 --- a/include/eepp/helper/chipmunk/chipmunk_private.h +++ b/include/eepp/helper/chipmunk/chipmunk_private.h @@ -25,7 +25,7 @@ #define CP_HASH_COEF (3344921057ul) #define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) -#pragma mark cpArray +//MARK: cpArray struct cpArray { int num, max; @@ -43,7 +43,7 @@ cpBool cpArrayContains(cpArray *arr, void *ptr); void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); -#pragma mark Foreach loops +//MARK: Foreach loops static inline cpConstraint * cpConstraintNext(cpConstraint *node, cpBody *body) @@ -69,7 +69,7 @@ cpArbiterNext(cpArbiter *node, cpBody *body) #define CP_BODY_FOREACH_COMPONENT(root, var)\ for(cpBody *var = root; var; var = var->node.next) -#pragma mark cpHashSet +//MARK: cpHashSet typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); @@ -90,25 +90,51 @@ 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 Body Functions +//MARK: Body Functions void cpBodyAddShape(cpBody *body, cpShape *shape); void cpBodyRemoveShape(cpBody *body, cpShape *shape); void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint); -#pragma mark Shape/Collision Functions +//MARK: Shape/Collision Functions + +// TODO should move this to the cpVect API. It's pretty useful. +static inline cpVect +cpClosetPointOnSegment(const cpVect p, const cpVect a, const cpVect b) +{ + cpVect delta = cpvsub(a, b); + cpFloat t = cpfclamp01(cpvdot(delta, cpvsub(p, b))/cpvlengthsq(delta)); + return cpvadd(b, cpvmult(delta, t)); +} cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body); static inline cpBool cpShapeActive(cpShape *shape) { - return shape->prev || shape->body->shapeList == shape; + return shape->prev || (shape->body && shape->body->shapeList == shape); } int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr); +// TODO doesn't really need to be inline, but need a better place to put this function +static inline cpSplittingPlane +cpSplittingPlaneNew(cpVect a, cpVect b) +{ + cpVect n = cpvnormalize(cpvperp(cpvsub(b, a))); + cpSplittingPlane plane = {n, cpvdot(n, a)}; + return plane; +} + +static inline cpFloat +cpSplittingPlaneCompare(cpSplittingPlane plane, cpVect v) +{ + return cpvdot(plane.n, v) - plane.d; +} + +void cpLoopIndexes(cpVect *verts, int count, int *start, int *end); + static inline cpFloat cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) { @@ -125,10 +151,10 @@ cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) static inline cpBool cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) { - cpPolyShapeAxis *axes = poly->tAxes; + cpSplittingPlane *planes = poly->tPlanes; for(int i=0; inumVerts; i++){ - cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + cpFloat dist = cpSplittingPlaneCompare(planes[i], v); if(dist > 0.0f) return cpFalse; } @@ -138,31 +164,39 @@ cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) static inline cpBool cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n) { - cpPolyShapeAxis *axes = poly->tAxes; + cpSplittingPlane *planes = poly->tPlanes; 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(cpvdot(planes[i].n, n) < 0.0f) continue; + cpFloat dist = cpSplittingPlaneCompare(planes[i], v); if(dist > 0.0f) return cpFalse; } return cpTrue; } -#pragma mark Spatial Index Functions +//MARK: Spatial Index Functions cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -#pragma mark Space Functions +//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 *cpSpaceGetPostStepData(cpSpace *space, void *obj); +typedef struct cpPostStepCallback { + cpPostStepFunc func; + void *key; + void *data; +} cpPostStepCallback; +cpPostStepCallback *cpSpaceGetPostStepCallback(cpSpace *space, void *key); + +cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space); void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter); void cpSpaceActivateBody(cpSpace *space, cpBody *body); @@ -186,7 +220,12 @@ cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb) cpArrayDeleteObj(space->arbiters, arb); } -#pragma mark Arbiters +void cpShapeUpdateFunc(cpShape *shape, void *unused); +void cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space); + + + +//MARK: Arbiters struct cpContact { cpVect p, n; diff --git a/include/eepp/helper/chipmunk/chipmunk_types.h b/include/eepp/helper/chipmunk/chipmunk_types.h index e8b040fdd..5124d4868 100644 --- a/include/eepp/helper/chipmunk/chipmunk_types.h +++ b/include/eepp/helper/chipmunk/chipmunk_types.h @@ -1,16 +1,18 @@ +#include + #ifdef __APPLE__ - #import "TargetConditionals.h" + #include "TargetConditionals.h" #endif -#if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS) - #define CP_USE_CGPOINTS +#if (TARGET_OS_IPHONE == 1) || (TARGET_OS_MAC == 1) && (!defined CP_USE_CGPOINTS) + #define CP_USE_CGPOINTS 1 #endif -#ifdef CP_USE_CGPOINTS +#if CP_USE_CGPOINTS == 1 #if TARGET_OS_IPHONE #import #elif TARGET_OS_MAC - #import + #include #endif #if defined(__LP64__) && __LP64__ @@ -58,7 +60,6 @@ #endif #ifndef INFINITY - //TODO use C++ infinity #ifdef _MSC_VER union MSVC_EVIL_FLOAT_HACK { @@ -132,15 +133,10 @@ static inline cpFloat cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) } /// Hash value type. -#include "../sophist/sophist.h" -#if 1 == SOPHIST_pointer64 -typedef SOPHIST_uint64 cpHashValue; -#else -typedef SOPHIST_uint32 cpHashValue; -#endif +typedef uintptr_t 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 @@ -168,18 +164,18 @@ typedef SOPHIST_uint32 cpHashValue; typedef CP_COLLISION_TYPE_TYPE cpCollisionType; #else /// Type used for cpSpace.collision_type. - typedef unsigned int cpCollisionType; + typedef uintptr_t cpCollisionType; #endif #ifdef CP_GROUP_TYPE typedef CP_GROUP_TYPE cpGroup; #else /// Type used for cpShape.group. - typedef unsigned int cpGroup; + typedef uintptr_t cpGroup; #endif #ifdef CP_LAYERS_TYPE - typedef CP_GROUP_TYPE cpLayers; + typedef CP_LAYERS_TYPE cpLayers; #else /// Type used for cpShape.layers. typedef unsigned int cpLayers; @@ -205,7 +201,7 @@ typedef SOPHIST_uint32 cpHashValue; // CGPoints are structurally the same, and allow // easy interoperability with other Cocoa libraries -#ifdef CP_USE_CGPOINTS +#if CP_USE_CGPOINTS typedef CGPoint cpVect; #else /// Chipmunk's 2D vector type. @@ -213,4 +209,7 @@ typedef SOPHIST_uint32 cpHashValue; typedef struct cpVect{cpFloat x,y;} cpVect; #endif - +typedef struct cpMat2x2 { + // Row major [[a, b][c d]] + cpFloat a, b, c, d; +} cpMat2x2; diff --git a/include/eepp/helper/chipmunk/constraints/cpConstraint.h b/include/eepp/helper/chipmunk/constraints/cpConstraint.h index d3b28e866..d1059fffa 100644 --- a/include/eepp/helper/chipmunk/constraints/cpConstraint.h +++ b/include/eepp/helper/chipmunk/constraints/cpConstraint.h @@ -26,7 +26,7 @@ typedef struct cpConstraintClass cpConstraintClass; typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); -typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint); +typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint, cpFloat dt); typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); /// @private @@ -37,6 +37,11 @@ struct cpConstraintClass { cpConstraintGetImpulseImpl getImpulse; }; +/// Callback function type that gets called before solving a joint. +typedef void (*cpConstraintPreSolveFunc)(cpConstraint *constraint, cpSpace *space); +/// Callback function type that gets called after solving a joint. +typedef void (*cpConstraintPostSolveFunc)(cpConstraint *constraint, cpSpace *space); + /// Opaque cpConstraint struct. struct cpConstraint { @@ -63,6 +68,14 @@ struct cpConstraint { /// Defaults to infinity. cpFloat maxBias; + /// Function called before the solver runs. + /// Animate your joint anchors, update your motor torque, etc. + cpConstraintPreSolveFunc preSolve; + + /// Function called after the solver runs. + /// Use the applied impulse to perform effects like breakable joints. + cpConstraintPostSolveFunc postSolve; + /// 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. @@ -81,27 +94,34 @@ static inline void cpConstraintActivateBodies(cpConstraint *constraint) cpBody *b = constraint->b; if(b) cpBodyActivate(b); } +/// @private #define CP_DefineConstraintStructGetter(type, member, name) \ static inline type cpConstraint##Get##name(const cpConstraint *constraint){return constraint->member;} +/// @private #define CP_DefineConstraintStructSetter(type, member, name) \ static inline void cpConstraint##Set##name(cpConstraint *constraint, type value){ \ cpConstraintActivateBodies(constraint); \ constraint->member = value; \ } +/// @private #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, UserData); +CP_DefineConstraintStructGetter(cpSpace*, CP_PRIVATE(space), Space) -/// Get the last impulse applied by this constraint. +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(cpConstraintPreSolveFunc, preSolve, PreSolveFunc) +CP_DefineConstraintStructProperty(cpConstraintPostSolveFunc, postSolve, PostSolveFunc) +CP_DefineConstraintStructProperty(cpDataPointer, data, UserData) + +// Get the last impulse applied by this constraint. static inline cpFloat cpConstraintGetImpulse(cpConstraint *constraint) { return constraint->CP_PRIVATE(klass)->getImpulse(constraint); diff --git a/include/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.h b/include/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.h index 9f3775a6b..ea0540ca9 100644 --- a/include/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.h +++ b/include/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.h @@ -24,8 +24,9 @@ typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); -const cpConstraintClass *cpDampedRotarySpringGetClass(); +const cpConstraintClass *cpDampedRotarySpringGetClass(void); +/// @private typedef struct cpDampedRotarySpring { cpConstraint constraint; cpFloat restAngle; @@ -40,15 +41,15 @@ typedef struct cpDampedRotarySpring { } cpDampedRotarySpring; /// Allocate a damped rotary spring. -cpDampedRotarySpring *cpDampedRotarySpringAlloc(void); +cpDampedRotarySpring* cpDampedRotarySpringAlloc(void); /// Initialize a damped rotary spring. -cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); +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); +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); +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/include/eepp/helper/chipmunk/constraints/cpDampedSpring.h b/include/eepp/helper/chipmunk/constraints/cpDampedSpring.h index b34f6639f..a3185ef4d 100644 --- a/include/eepp/helper/chipmunk/constraints/cpDampedSpring.h +++ b/include/eepp/helper/chipmunk/constraints/cpDampedSpring.h @@ -26,7 +26,7 @@ typedef struct cpDampedSpring cpDampedSpring; typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); -const cpConstraintClass *cpDampedSpringGetClass(); +const cpConstraintClass *cpDampedSpringGetClass(void); /// @private struct cpDampedSpring { @@ -46,17 +46,17 @@ struct cpDampedSpring { }; /// Allocate a damped spring. -cpDampedSpring *cpDampedSpringAlloc(void); +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); +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); +cpConstraint* cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); -CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1); -CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2); -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength); -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness); -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping); -CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc); +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1) +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2) +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/include/eepp/helper/chipmunk/constraints/cpGearJoint.h b/include/eepp/helper/chipmunk/constraints/cpGearJoint.h index 0b0682734..c9ebf9422 100644 --- a/include/eepp/helper/chipmunk/constraints/cpGearJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpGearJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpGearJoint cpGearJoint /// @{ -const cpConstraintClass *cpGearJointGetClass(); +const cpConstraintClass *cpGearJointGetClass(void); /// @private typedef struct cpGearJoint { @@ -33,18 +33,18 @@ typedef struct cpGearJoint { cpFloat iSum; cpFloat bias; - cpFloat jAcc, jMax; + cpFloat jAcc; } cpGearJoint; /// Allocate a gear joint. -cpGearJoint *cpGearJointAlloc(void); +cpGearJoint* cpGearJointAlloc(void); /// Initialize a gear joint. -cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); +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); +cpConstraint* cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); -CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase); -CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, 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/include/eepp/helper/chipmunk/constraints/cpGrooveJoint.h b/include/eepp/helper/chipmunk/constraints/cpGrooveJoint.h index bed2c52ff..b260218f4 100644 --- a/include/eepp/helper/chipmunk/constraints/cpGrooveJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpGrooveJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpGrooveJoint cpGrooveJoint /// @{ -const cpConstraintClass *cpGrooveJointGetClass(); +const cpConstraintClass *cpGrooveJointGetClass(void); /// @private typedef struct cpGrooveJoint { @@ -33,26 +33,25 @@ typedef struct cpGrooveJoint { cpVect grv_tn; cpFloat clamp; cpVect r1, r2; - cpVect k1, k2; + cpMat2x2 k; cpVect jAcc; - cpFloat jMaxLen; cpVect bias; } cpGrooveJoint; /// Allocate a groove joint. -cpGrooveJoint *cpGrooveJointAlloc(void); +cpGrooveJoint* cpGrooveJointAlloc(void); /// Initialize a groove joint. -cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); +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); +cpConstraint* cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); -CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA); +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); +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); +CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2) /// @} diff --git a/include/eepp/helper/chipmunk/constraints/cpPinJoint.h b/include/eepp/helper/chipmunk/constraints/cpPinJoint.h index 8b57f2664..2413e84a1 100644 --- a/include/eepp/helper/chipmunk/constraints/cpPinJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpPinJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpPinJoint cpPinJoint /// @{ -const cpConstraintClass *cpPinJointGetClass(); +const cpConstraintClass *cpPinJointGetClass(void); /// @private typedef struct cpPinJoint { @@ -34,19 +34,19 @@ typedef struct cpPinJoint { cpVect n; cpFloat nMass; - cpFloat jnAcc, jnMax; + cpFloat jnAcc; cpFloat bias; } cpPinJoint; /// Allocate a pin joint. -cpPinJoint *cpPinJointAlloc(void); +cpPinJoint* cpPinJointAlloc(void); /// Initialize a pin joint. -cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +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); +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); +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1) +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2) +CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist) ///@} diff --git a/include/eepp/helper/chipmunk/constraints/cpPivotJoint.h b/include/eepp/helper/chipmunk/constraints/cpPivotJoint.h index 13cf4a43a..a5d33174f 100644 --- a/include/eepp/helper/chipmunk/constraints/cpPivotJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpPivotJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpPivotJoint cpPivotJoint /// @{ -const cpConstraintClass *cpPivotJointGetClass(); +const cpConstraintClass *cpPivotJointGetClass(void); /// @private typedef struct cpPivotJoint { @@ -30,23 +30,22 @@ typedef struct cpPivotJoint { cpVect anchr1, anchr2; cpVect r1, r2; - cpVect k1, k2; + cpMat2x2 k; cpVect jAcc; - cpFloat jMaxLen; cpVect bias; } cpPivotJoint; /// Allocate a pivot joint -cpPivotJoint *cpPivotJointAlloc(void); +cpPivotJoint* cpPivotJointAlloc(void); /// Initialize a pivot joint. -cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +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); +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); +cpConstraint* cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); -CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1); -CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1) +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2) /// @} diff --git a/include/eepp/helper/chipmunk/constraints/cpRatchetJoint.h b/include/eepp/helper/chipmunk/constraints/cpRatchetJoint.h index a37339c48..e371b1088 100644 --- a/include/eepp/helper/chipmunk/constraints/cpRatchetJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpRatchetJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpRatchetJoint cpRatchetJoint /// @{ -const cpConstraintClass *cpRatchetJointGetClass(); +const cpConstraintClass *cpRatchetJointGetClass(void); /// @private typedef struct cpRatchetJoint { @@ -32,18 +32,18 @@ typedef struct cpRatchetJoint { cpFloat iSum; cpFloat bias; - cpFloat jAcc, jMax; + cpFloat jAcc; } cpRatchetJoint; /// Allocate a ratchet joint. -cpRatchetJoint *cpRatchetJointAlloc(void); +cpRatchetJoint* cpRatchetJointAlloc(void); /// Initialize a ratched joint. -cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); +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); +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); +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle) +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase) +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet) /// @} diff --git a/include/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.h b/include/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.h index 5d55e8c3f..2277e40f5 100644 --- a/include/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint /// @{ -const cpConstraintClass *cpRotaryLimitJointGetClass(); +const cpConstraintClass *cpRotaryLimitJointGetClass(void); /// @private typedef struct cpRotaryLimitJoint { @@ -32,17 +32,17 @@ typedef struct cpRotaryLimitJoint { cpFloat iSum; cpFloat bias; - cpFloat jAcc, jMax; + cpFloat jAcc; } cpRotaryLimitJoint; /// Allocate a damped rotary limit joint. -cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void); +cpRotaryLimitJoint* cpRotaryLimitJointAlloc(void); /// Initialize a damped rotary limit joint. -cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); +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); +cpConstraint* cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); -CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min); -CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max); +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min) +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max) /// @} diff --git a/include/eepp/helper/chipmunk/constraints/cpSimpleMotor.h b/include/eepp/helper/chipmunk/constraints/cpSimpleMotor.h index acd6ca256..37d28c5de 100644 --- a/include/eepp/helper/chipmunk/constraints/cpSimpleMotor.h +++ b/include/eepp/helper/chipmunk/constraints/cpSimpleMotor.h @@ -22,7 +22,7 @@ /// @defgroup cpSimpleMotor cpSimpleMotor /// @{ -const cpConstraintClass *cpSimpleMotorGetClass(); +const cpConstraintClass *cpSimpleMotorGetClass(void); /// @private typedef struct cpSimpleMotor { @@ -31,16 +31,16 @@ typedef struct cpSimpleMotor { cpFloat iSum; - cpFloat jAcc, jMax; + cpFloat jAcc; } cpSimpleMotor; /// Allocate a simple motor. -cpSimpleMotor *cpSimpleMotorAlloc(void); +cpSimpleMotor* cpSimpleMotorAlloc(void); /// initialize a simple motor. -cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); +cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); /// Allocate and initialize a simple motor. -cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); +cpConstraint* cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); -CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate); +CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate) /// @} diff --git a/include/eepp/helper/chipmunk/constraints/cpSlideJoint.h b/include/eepp/helper/chipmunk/constraints/cpSlideJoint.h index 86689538d..40db8bba2 100644 --- a/include/eepp/helper/chipmunk/constraints/cpSlideJoint.h +++ b/include/eepp/helper/chipmunk/constraints/cpSlideJoint.h @@ -22,7 +22,7 @@ /// @defgroup cpSlideJoint cpSlideJoint /// @{ -const cpConstraintClass *cpSlideJointGetClass(); +const cpConstraintClass *cpSlideJointGetClass(void); /// @private typedef struct cpSlideJoint { @@ -34,20 +34,20 @@ typedef struct cpSlideJoint { cpVect n; cpFloat nMass; - cpFloat jnAcc, jnMax; + cpFloat jnAcc; cpFloat bias; } cpSlideJoint; /// Allocate a slide joint. -cpSlideJoint *cpSlideJointAlloc(void); +cpSlideJoint* cpSlideJointAlloc(void); /// Initialize a slide joint. -cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); +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); +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); +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/include/eepp/helper/chipmunk/constraints/util.h b/include/eepp/helper/chipmunk/constraints/util.h index 3f3c9e084..0a6204758 100644 --- a/include/eepp/helper/chipmunk/constraints/util.h +++ b/include/eepp/helper/chipmunk/constraints/util.h @@ -19,12 +19,14 @@ * SOFTWARE. */ -#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(){return (cpConstraintClass *)&klass;} +// These are utility routines to use when creating custom constraints. +// I'm not sure if this should be part of the private API or not. +// I should probably clean up the naming conventions if it is... + +#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(void){return (cpConstraintClass *)&klass;} void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b); -#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt)) - static inline cpVect relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); @@ -54,8 +56,8 @@ apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) static inline void apply_bias_impulse(cpBody *body, cpVect j, cpVect r) { - body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); - body->w_bias += body->i_inv*cpvcross(r, j); + body->CP_PRIVATE(v_bias) = cpvadd(body->CP_PRIVATE(v_bias), cpvmult(j, body->m_inv)); + body->CP_PRIVATE(w_bias) += body->i_inv*cpvcross(r, j); } static inline void @@ -65,30 +67,30 @@ apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) apply_bias_impulse(b, j, r2); } +static inline cpFloat +k_scalar_body(cpBody *body, cpVect r, cpVect n) +{ + cpFloat rcn = cpvcross(r, n); + return body->m_inv + body->i_inv*rcn*rcn; +} + static inline cpFloat k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) { - cpFloat mass_sum = a->m_inv + b->m_inv; - cpFloat r1cn = cpvcross(r1, n); - cpFloat r2cn = cpvcross(r2, n); - - cpFloat value = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; + cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n); cpAssertSoft(value != 0.0, "Unsolvable collision or constraint."); return value; } -static inline void -k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2) +static inline cpMat2x2 +k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2) { - // calculate mass matrix - // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... - cpFloat k11, k12, k21, k22; cpFloat m_sum = a->m_inv + b->m_inv; - // start with I*m_sum - k11 = m_sum; k12 = 0.0f; - k21 = 0.0f; k22 = m_sum; + // start with Identity*m_sum + cpFloat k11 = m_sum, k12 = 0.0f; + cpFloat k21 = 0.0f, k22 = m_sum; // add the influence from r1 cpFloat a_i_inv = a->i_inv; @@ -107,18 +109,14 @@ k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2) k21 += r2nxy; k22 += r2xsq; // invert - cpFloat determinant = k11*k22 - k12*k21; - cpAssertSoft(determinant != 0.0, "Unsolvable constraint."); + cpFloat det = k11*k22 - k12*k21; + cpAssertSoft(det != 0.0, "Unsolvable constraint."); - cpFloat det_inv = 1.0f/determinant; - *k1 = cpv( k22*det_inv, -k12*det_inv); - *k2 = cpv(-k21*det_inv, k11*det_inv); -} - -static inline cpVect -mult_k(cpVect vr, cpVect k1, cpVect k2) -{ - return cpv(cpvdot(vr, k1), cpvdot(vr, k2)); + cpFloat det_inv = 1.0f/det; + return cpMat2x2New( + k22*det_inv, -k12*det_inv, + -k21*det_inv, k11*det_inv + ); } static inline cpFloat diff --git a/include/eepp/helper/chipmunk/cpArbiter.h b/include/eepp/helper/chipmunk/cpArbiter.h index b859aabd8..94e141bf5 100644 --- a/include/eepp/helper/chipmunk/cpArbiter.h +++ b/include/eepp/helper/chipmunk/cpArbiter.h @@ -83,6 +83,11 @@ struct cpArbiter { /// Override in a pre-solve collision handler for custom behavior. cpVect surface_vr; + /// User definable data pointer. + /// The value will persist for the pair of shapes until the separate() callback is called. + /// NOTE: If you need to clean up this pointer, you should implement the separate() callback to do it. + cpDataPointer data; + CP_PRIVATE(cpShape *a); CP_PRIVATE(cpShape *b); CP_PRIVATE(cpBody *body_a); @@ -110,16 +115,21 @@ static inline void cpArbiterSet##name(cpArbiter *arb, type value){arb->member = CP_DefineArbiterStructGetter(type, member, name) \ CP_DefineArbiterStructSetter(type, member, name) -CP_DefineArbiterStructProperty(cpFloat, e, Elasticity); -CP_DefineArbiterStructProperty(cpFloat, u, Friction); -CP_DefineArbiterStructProperty(cpVect, surface_vr, SurfaceVelocity); +CP_DefineArbiterStructProperty(cpFloat, e, Elasticity) +CP_DefineArbiterStructProperty(cpFloat, u, Friction) +CP_DefineArbiterStructProperty(cpVect, surface_vr, SurfaceVelocity) +CP_DefineArbiterStructProperty(cpDataPointer, data, UserData) /// Calculate the total impulse that was applied by this arbiter. -/// Calling this function from a begin or pre-solve callback is undefined. +/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. 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. +/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. cpVect cpArbiterTotalImpulseWithFriction(const cpArbiter *arb); +/// Calculate the amount of energy lost in a collision including static, but not dynamic friction. +/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. +cpFloat cpArbiterTotalKE(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 @@ -152,18 +162,6 @@ static inline void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody * /// 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); -} - /// A struct that wraps up the important collision data for an arbiter. typedef struct cpContactPointSet { /// The number of contact points in the set. @@ -182,6 +180,10 @@ typedef struct cpContactPointSet { /// Return a contact set from an arbiter. cpContactPointSet cpArbiterGetContactPointSet(const cpArbiter *arb); +/// Returns true if this is the first step a pair of objects started colliding. +cpBool cpArbiterIsFirstContact(const cpArbiter *arb); +/// Get the number of contact points for this arbiter. +int cpArbiterGetCount(const cpArbiter *arb); /// 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. diff --git a/include/eepp/helper/chipmunk/cpBB.h b/include/eepp/helper/chipmunk/cpBB.h index 9b98eb007..320a1af35 100644 --- a/include/eepp/helper/chipmunk/cpBB.h +++ b/include/eepp/helper/chipmunk/cpBB.h @@ -29,13 +29,18 @@ typedef struct cpBB{ } cpBB; /// Convenience constructor for cpBB structs. -static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, - const cpFloat r, const cpFloat t) +static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t) { cpBB bb = {l, b, r, t}; return bb; } +/// Constructs a cpBB for a circle with the given position and radius. +static inline cpBB cpBBNewForCircle(const cpVect p, const cpFloat r) +{ + return cpBBNew(p.x - r, p.y - r, p.x + r, p.y + r); +} + /// Returns true if @c a and @c b intersect. static inline cpBool cpBBIntersects(const cpBB a, const cpBB b) { @@ -86,23 +91,43 @@ 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)); } +/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit. +static inline cpFloat cpBBSegmentQuery(cpBB bb, cpVect a, cpVect b) +{ + cpFloat idx = 1.0f/(b.x - a.x); + cpFloat tx1 = (bb.l == a.x ? -INFINITY : (bb.l - a.x)*idx); + cpFloat tx2 = (bb.r == a.x ? INFINITY : (bb.r - a.x)*idx); + cpFloat txmin = cpfmin(tx1, tx2); + cpFloat txmax = cpfmax(tx1, tx2); + + cpFloat idy = 1.0f/(b.y - a.y); + cpFloat ty1 = (bb.b == a.y ? -INFINITY : (bb.b - a.y)*idy); + cpFloat ty2 = (bb.t == a.y ? INFINITY : (bb.t - a.y)*idy); + cpFloat tymin = cpfmin(ty1, ty2); + cpFloat tymax = cpfmax(ty1, ty2); + + if(tymin <= txmax && txmin <= tymax){ + cpFloat min = cpfmax(txmin, tymin); + cpFloat max = cpfmin(txmax, tymax); + + if(0.0 <= max && min <= 1.0) return cpfmax(min, 0.0); + } + + return INFINITY; +} + /// 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)){ - 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); - - return (cpfabs(cpvdot(axis, offset)) < cpfabs(axis.x*extents.x) + cpfabs(axis.y*extents.y)); - } - - return cpFalse; + return (cpBBSegmentQuery(bb, a, b) != INFINITY); } /// Clamp a vector to a bounding box. -cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox +static inline cpVect +cpBBClampVect(const cpBB bb, const cpVect v) +{ + return cpv(cpfclamp(v.x, bb.l, bb.r), cpfclamp(v.y, bb.b, bb.t)); +} // TODO edge case issue /// Wrap a vector to a bounding box. diff --git a/include/eepp/helper/chipmunk/cpBody.h b/include/eepp/helper/chipmunk/cpBody.h index 2d4764d78..eadd991db 100644 --- a/include/eepp/helper/chipmunk/cpBody.h +++ b/include/eepp/helper/chipmunk/cpBody.h @@ -100,16 +100,16 @@ struct cpBody { }; /// Allocate a cpBody. -cpBody *cpBodyAlloc(void); +cpBody* cpBodyAlloc(void); /// Initialize a cpBody. -cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); +cpBody* cpBodyInit(cpBody *body, cpFloat m, cpFloat i); /// Allocate and initialize a cpBody. -cpBody *cpBodyNew(cpFloat m, cpFloat i); +cpBody* cpBodyNew(cpFloat m, cpFloat i); /// Initialize a static cpBody. -cpBody *cpBodyInitStatic(cpBody *body); +cpBody* cpBodyInitStatic(cpBody *body); /// Allocate and initialize a static cpBody. -cpBody *cpBodyNewStatic(); +cpBody* cpBodyNewStatic(void); /// Destroy a cpBody. void cpBodyDestroy(cpBody *body); @@ -160,36 +160,39 @@ static inline type cpBodyGet##name(const cpBody *body){return body->member;} #define CP_DefineBodyStructSetter(type, member, name) \ static inline void cpBodySet##name(cpBody *body, const type value){ \ cpBodyActivate(body); \ - cpBodyAssertSane(body); \ body->member = value; \ + cpBodyAssertSane(body); \ } #define CP_DefineBodyStructProperty(type, member, name) \ CP_DefineBodyStructGetter(type, member, name) \ CP_DefineBodyStructSetter(type, member, name) -CP_DefineBodyStructGetter(cpFloat, m, Mass); +// TODO add to docs +CP_DefineBodyStructGetter(cpSpace*, CP_PRIVATE(space), Space) + +CP_DefineBodyStructGetter(cpFloat, m, Mass) /// Set the mass of a body. void cpBodySetMass(cpBody *body, cpFloat m); -CP_DefineBodyStructGetter(cpFloat, i, Moment); +CP_DefineBodyStructGetter(cpFloat, i, Moment) /// Set the moment of a body. void cpBodySetMoment(cpBody *body, cpFloat i); -CP_DefineBodyStructGetter(cpVect, p, Pos); +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); +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_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, UserData); +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, UserData) /// Default Integration functions. void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); @@ -214,6 +217,12 @@ 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 velocity on a body (in world units) at a point on the body in world coordinates. +cpVect cpBodyGetVelAtWorldPoint(cpBody *body, cpVect point); +/// Get the velocity on a body (in world units) at a point on the body in local coordinates. +cpVect cpBodyGetVelAtLocalPoint(cpBody *body, cpVect point); + + /// Get the kinetic energy of a body. static inline cpFloat cpBodyKineticEnergy(const cpBody *body) { diff --git a/include/eepp/helper/chipmunk/cpPolyShape.h b/include/eepp/helper/chipmunk/cpPolyShape.h index a34956ed7..40ecd8b47 100644 --- a/include/eepp/helper/chipmunk/cpPolyShape.h +++ b/include/eepp/helper/chipmunk/cpPolyShape.h @@ -23,10 +23,10 @@ /// @{ /// @private -typedef struct cpPolyShapeAxis { +typedef struct cpSplittingPlane { cpVect n; cpFloat d; -} cpPolyShapeAxis; +} cpSplittingPlane; /// @private typedef struct cpPolyShape { @@ -34,28 +34,29 @@ typedef struct cpPolyShape { int numVerts; cpVect *verts, *tVerts; - cpPolyShapeAxis *axes, *tAxes; + cpSplittingPlane *planes, *tPlanes; } cpPolyShape; /// Allocate a polygon shape. -cpPolyShape *cpPolyShapeAlloc(void); +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); +/// A convex hull will be created from the vertexes. +cpPolyShape* cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset); /// Allocate and initialize a polygon shape. -/// The vertexes must be convex and have a clockwise winding. -cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset); +/// A convex hull will be created from the vertexes. +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); +cpPolyShape* cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height); /// Initialize an offset box shaped polygon shape. -cpPolyShape *cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box); +cpPolyShape* cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box); /// Allocate and initialize a box shaped polygon shape. -cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); +cpShape* cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); /// Allocate and initialize an offset box shaped polygon shape. -cpShape *cpBoxShapeNew2(cpBody *body, cpBB box); +cpShape* cpBoxShapeNew2(cpBody *body, cpBB box); /// Check that a set of vertexes is convex and has a clockwise winding. +/// NOTE: Due to floating point precision issues, hulls created with cpQuickHull() are not guaranteed to validate! cpBool cpPolyValidate(const cpVect *verts, const int numVerts); /// Get the number of verts in a polygon shape. diff --git a/include/eepp/helper/chipmunk/cpShape.h b/include/eepp/helper/chipmunk/cpShape.h index 0159d6f0c..0d927a69b 100644 --- a/include/eepp/helper/chipmunk/cpShape.h +++ b/include/eepp/helper/chipmunk/cpShape.h @@ -25,6 +25,16 @@ typedef struct cpShapeClass cpShapeClass; +/// Nearest point query info struct. +typedef struct cpNearestPointQueryInfo { + /// The nearest shape, NULL if no shape was within range. + cpShape *shape; + /// The closest point on the shape's surface. (in world space coordinates) + cpVect p; + /// The distance to the point. The distance is negative if the point is inside the shape. + cpFloat d; +} cpNearestPointQueryInfo; + /// Segment query info struct. typedef struct cpSegmentQueryInfo { /// The shape that was hit, NULL if no collision occured. @@ -45,7 +55,7 @@ typedef enum cpShapeType{ typedef cpBB (*cpShapeCacheDataImpl)(cpShape *shape, cpVect p, cpVect rot); typedef void (*cpShapeDestroyImpl)(cpShape *shape); -typedef cpBool (*cpShapePointQueryImpl)(cpShape *shape, cpVect p); +typedef void (*cpShapeNearestPointQueryImpl)(cpShape *shape, cpVect p, cpNearestPointQueryInfo *info); typedef void (*cpShapeSegmentQueryImpl)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); /// @private @@ -54,7 +64,7 @@ struct cpShapeClass { cpShapeCacheDataImpl cacheData; cpShapeDestroyImpl destroy; - cpShapePointQueryImpl pointQuery; + cpShapeNearestPointQueryImpl nearestPointQuery; cpShapeSegmentQueryImpl segmentQuery; }; @@ -112,36 +122,9 @@ cpBB cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot); /// Test if a point lies within a shape. cpBool cpShapePointQuery(cpShape *shape, cpVect p); -#define CP_DefineShapeStructGetter(type, member, name) \ -static inline type cpShapeGet##name(const cpShape *shape){return shape->member;} - -#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) - -CP_DefineShapeStructGetter(cpBody *, body, Body); -void cpShapeSetBody(cpShape *shape, cpBody *body); - -CP_DefineShapeStructGetter(cpBB, bb, BB); -CP_DefineShapeStructProperty(cpBool, sensor, Sensor, 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 nearest point query. It finds the closest point on the surface of shape to a specific point. +/// The value returned is the distance between the points. A negative distance means the point is inside the shape. +cpFloat cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpNearestPointQueryInfo *out); /// 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); @@ -158,6 +141,39 @@ static inline cpFloat cpSegmentQueryHitDist(const cpVect start, const cpVect end return cpvdist(start, end)*info.t; } +#define CP_DefineShapeStructGetter(type, member, name) \ +static inline type cpShapeGet##name(const cpShape *shape){return shape->member;} + +#define CP_DefineShapeStructSetter(type, member, name, activates) \ +static inline void cpShapeSet##name(cpShape *shape, type value){ \ + if(activates && shape->body) cpBodyActivate(shape->body); \ + shape->member = value; \ +} + +#define CP_DefineShapeStructProperty(type, member, name, activates) \ +CP_DefineShapeStructGetter(type, member, name) \ +CP_DefineShapeStructSetter(type, member, name, activates) + +CP_DefineShapeStructGetter(cpSpace*, CP_PRIVATE(space), Space) + +CP_DefineShapeStructGetter(cpBody*, body, Body) +void cpShapeSetBody(cpShape *shape, cpBody *body); + +CP_DefineShapeStructGetter(cpBB, bb, BB) +CP_DefineShapeStructProperty(cpBool, sensor, Sensor, 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); + #define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(const cpShape *shape) /// @} @@ -172,11 +188,11 @@ typedef struct cpCircleShape { } cpCircleShape; /// Allocate a circle shape. -cpCircleShape *cpCircleShapeAlloc(void); +cpCircleShape* cpCircleShapeAlloc(void); /// Initialize a circle shape. -cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); +cpCircleShape* cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); /// Allocate and initialize a circle shape. -cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); +cpShape* cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset); CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); @@ -191,6 +207,8 @@ typedef struct cpSegmentShape { cpVect a, b, n; cpVect ta, tb, tn; cpFloat r; + + cpVect a_tangent, b_tangent; } cpSegmentShape; /// Allocate a segment shape. @@ -200,6 +218,8 @@ cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, /// Allocate and initialize a segment shape. cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); +void cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next); + CP_DeclareShapeGetter(cpSegmentShape, cpVect, A); CP_DeclareShapeGetter(cpSegmentShape, cpVect, B); CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal); diff --git a/include/eepp/helper/chipmunk/cpSpace.h b/include/eepp/helper/chipmunk/cpSpace.h index 3e4ca0aa1..58461eab7 100644 --- a/include/eepp/helper/chipmunk/cpSpace.h +++ b/include/eepp/helper/chipmunk/cpSpace.h @@ -23,6 +23,7 @@ /// @{ typedef struct cpContactBufferHeader cpContactBufferHeader; +typedef void (*cpSpaceArbiterApplyImpulseFunc)(cpArbiter *arb); /// Basic Unit of Simulation in Chipmunk struct cpSpace { @@ -47,7 +48,7 @@ struct cpSpace { /// The default value of INFINITY disables the sleeping algorithm. cpFloat sleepTimeThreshold; - /// Amount of encouraged penetration between colliding shapes.. + /// Amount of encouraged penetration between colliding shapes. /// 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. @@ -97,7 +98,9 @@ struct cpSpace { CP_PRIVATE(cpHashSet *collisionHandlers); CP_PRIVATE(cpCollisionHandler defaultHandler); - CP_PRIVATE(cpHashSet *postStepCallbacks); + + CP_PRIVATE(cpBool skipPostStep); + CP_PRIVATE(cpArray *postStepCallbacks); CP_PRIVATE(cpBody _staticBody); }; @@ -124,18 +127,25 @@ static inline void cpSpaceSet##name(cpSpace *space, type value){space->member = CP_DefineSpaceStructGetter(type, member, name) \ CP_DefineSpaceStructSetter(type, member, name) -CP_DefineSpaceStructProperty(int, iterations, Iterations); -CP_DefineSpaceStructProperty(cpVect, gravity, Gravity); -CP_DefineSpaceStructProperty(cpFloat, damping, Damping); -CP_DefineSpaceStructProperty(cpFloat, idleSpeedThreshold, IdleSpeedThreshold); -CP_DefineSpaceStructProperty(cpFloat, sleepTimeThreshold, SleepTimeThreshold); -CP_DefineSpaceStructProperty(cpFloat, collisionSlop, CollisionSlop); -CP_DefineSpaceStructProperty(cpFloat, collisionBias, CollisionBias); -CP_DefineSpaceStructProperty(cpTimestamp, collisionPersistence, CollisionPersistence); -CP_DefineSpaceStructProperty(cpBool, enableContactGraph, EnableContactGraph); -CP_DefineSpaceStructProperty(cpDataPointer, data, UserData); -CP_DefineSpaceStructGetter(cpBody *, staticBody, StaticBody); -CP_DefineSpaceStructGetter(cpFloat, CP_PRIVATE(curr_dt), CurrentTimeStep); +CP_DefineSpaceStructProperty(int, iterations, Iterations) +CP_DefineSpaceStructProperty(cpVect, gravity, Gravity) +CP_DefineSpaceStructProperty(cpFloat, damping, Damping) +CP_DefineSpaceStructProperty(cpFloat, idleSpeedThreshold, IdleSpeedThreshold) +CP_DefineSpaceStructProperty(cpFloat, sleepTimeThreshold, SleepTimeThreshold) +CP_DefineSpaceStructProperty(cpFloat, collisionSlop, CollisionSlop) +CP_DefineSpaceStructProperty(cpFloat, collisionBias, CollisionBias) +CP_DefineSpaceStructProperty(cpTimestamp, collisionPersistence, CollisionPersistence) +CP_DefineSpaceStructProperty(cpBool, enableContactGraph, EnableContactGraph) +CP_DefineSpaceStructProperty(cpDataPointer, data, UserData) +CP_DefineSpaceStructGetter(cpBody*, staticBody, StaticBody) +CP_DefineSpaceStructGetter(cpFloat, CP_PRIVATE(curr_dt), CurrentTimeStep) + +/// returns true from inside a callback and objects cannot be added/removed. +static inline cpBool +cpSpaceIsLocked(cpSpace *space) +{ + return space->CP_PRIVATE(locked); +} /// Set a default collision handler for this space. /// The default collision handler is invoked for each colliding pair of shapes @@ -167,13 +177,13 @@ void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisio /// 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); +cpShape* cpSpaceAddShape(cpSpace *space, cpShape *shape); /// Explicity add a shape as a static shape to the simulation. -cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); +cpShape* cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); /// Add a rigid body to the simulation. -cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body); +cpBody* cpSpaceAddBody(cpSpace *space, cpBody *body); /// Add a constraint to the simulation. -cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); +cpConstraint* cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); /// Remove a collision shape from the simulation. void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); @@ -192,10 +202,12 @@ cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body); cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint); /// Post Step callback function type. -typedef void (*cpPostStepFunc)(cpSpace *space, void *obj, void *data); +typedef void (*cpPostStepFunc)(cpSpace *space, void *key, void *data); /// 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); +/// You can only register one callback per unique value for @c key. +/// Returns true only if @c key has never been scheduled before. +/// It's possible to pass @c NULL for @c func if you only want to mark @c key as being used. +cpBool cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data); /// Point query callback function type. typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); @@ -204,6 +216,13 @@ void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup gr /// 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); +/// Nearest point query callback function type. +typedef void (*cpSpaceNearestPointQueryFunc)(cpShape *shape, cpFloat distance, cpVect point, void *data); +/// Query the space at a point and call @c func for each shape found. +void cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data); +/// Query the space at a point and return the nearest shape found. Returns NULL if no shapes were found. +cpShape *cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out); + /// 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. diff --git a/include/eepp/helper/chipmunk/cpSpatialIndex.h b/include/eepp/helper/chipmunk/cpSpatialIndex.h index ff0d05977..bf3d0da39 100644 --- a/include/eepp/helper/chipmunk/cpSpatialIndex.h +++ b/include/eepp/helper/chipmunk/cpSpatialIndex.h @@ -37,7 +37,7 @@ @{ */ -#pragma mark Spatial Index +//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 @@ -64,16 +64,16 @@ struct cpSpatialIndex { }; -#pragma mark Spatial Hash +//MARK: Spatial Hash typedef struct cpSpaceHash cpSpaceHash; /// Allocate a spatial hash. -cpSpaceHash *cpSpaceHashAlloc(void); +cpSpaceHash* cpSpaceHashAlloc(void); /// Initialize a spatial hash. -cpSpatialIndex *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +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); +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 @@ -81,16 +81,16 @@ cpSpatialIndex *cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc /// 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 +//MARK: AABB Tree typedef struct cpBBTree cpBBTree; /// Allocate a bounding box tree. -cpBBTree *cpBBTreeAlloc(void); +cpBBTree* cpBBTreeAlloc(void); /// Initialize a bounding box tree. -cpSpatialIndex *cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex* cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); /// Allocate and initialize a bounding box tree. -cpSpatialIndex *cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex* cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); /// Perform a static top down optimization of the tree. void cpBBTreeOptimize(cpSpatialIndex *index); @@ -101,18 +101,18 @@ 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 +//MARK: Single Axis Sweep typedef struct cpSweep1D cpSweep1D; /// Allocate a 1D sort and sweep broadphase. -cpSweep1D *cpSweep1DAlloc(void); +cpSweep1D* cpSweep1DAlloc(void); /// Initialize a 1D sort and sweep broadphase. -cpSpatialIndex *cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex* cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); /// Allocate and initialize a 1D sort and sweep broadphase. -cpSpatialIndex *cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex* cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -#pragma mark Spatial Index Implementation +//MARK: Spatial Index Implementation typedef void (*cpSpatialIndexDestroyImpl)(cpSpatialIndex *index); @@ -127,9 +127,8 @@ 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 (*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); +typedef void (*cpSpatialIndexSegmentQueryImpl)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data); struct cpSpatialIndexClass { cpSpatialIndexDestroyImpl destroy; @@ -145,9 +144,8 @@ struct cpSpatialIndexClass { cpSpatialIndexReindexObjectImpl reindexObject; cpSpatialIndexReindexQueryImpl reindexQuery; - cpSpatialIndexPointQueryImpl pointQuery; - cpSpatialIndexSegmentQueryImpl segmentQuery; cpSpatialIndexQueryImpl query; + cpSpatialIndexSegmentQueryImpl segmentQuery; }; /// Destroy and free a spatial index. @@ -206,11 +204,10 @@ static inline void cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, index->klass->reindexObject(index, obj, hashid); } -/// 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, cpSpatialIndexQueryFunc func, void *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, cpSpatialIndexQueryFunc func, void *data) { - index->klass->pointQuery(index, point, func, data); + index->klass->query(index, obj, bb, func, data); } /// Perform a segment query against the spatial index, calling @c func for each potential match. @@ -219,12 +216,6 @@ static inline void cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, 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, 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. diff --git a/include/eepp/helper/chipmunk/cpVect.h b/include/eepp/helper/chipmunk/cpVect.h index 235692954..a59a361e8 100644 --- a/include/eepp/helper/chipmunk/cpVect.h +++ b/include/eepp/helper/chipmunk/cpVect.h @@ -33,25 +33,16 @@ static inline cpVect cpv(const cpFloat x, const cpFloat y) return v; } -/// Returns the length of v. -cpFloat cpvlength(const cpVect v); - /// Spherical linearly interpolate between v1 and v2. cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t); /// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a); -/// Returns the unit length vector for the given angle (in radians). -cpVect cpvforangle(const cpFloat a); - -/// Returns the angular direction v is pointing in (in radians). -cpFloat cpvtoangle(const cpVect v); - /// Returns a string representation of v. Intended mostly for debugging purposes and not production use. /// @attention The string points to a static local and is reset every time the function is called. /// If you want to print more than one vector you will have to split up your printing onto separate lines. -char *cpvstr(const cpVect v); +char* cpvstr(const cpVect v); /// Check if two vectors are equal. (Be careful when comparing floating point numbers!) static inline cpBool cpveql(const cpVect v1, const cpVect v2) @@ -115,6 +106,18 @@ static inline cpVect cpvproject(const cpVect v1, const cpVect v2) return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); } +/// Returns the unit length vector for the given angle (in radians). +static inline cpVect cpvforangle(const cpFloat a) +{ + return cpv(cpfcos(a), cpfsin(a)); +} + +/// Returns the angular direction v is pointing in (in radians). +static inline cpFloat cpvtoangle(const cpVect v) +{ + return cpfatan2(v.y, v.x); +} + /// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector. static inline cpVect cpvrotate(const cpVect v1, const cpVect v2) { @@ -133,6 +136,12 @@ static inline cpFloat cpvlengthsq(const cpVect v) return cpvdot(v, v); } +/// Returns the length of v. +static inline cpFloat cpvlength(const cpVect v) +{ + return cpfsqrt(cpvdot(v, v)); +} + /// Linearly interpolate between v1 and v2. static inline cpVect cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) { @@ -180,4 +189,24 @@ static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dis { return cpvdistsq(v1, v2) < dist*dist; } + /// @} + +/// @defgroup cpMat2x2 cpMat2x2 +/// 2x2 matrix type used for tensors and such. +/// @{ + +static inline cpMat2x2 +cpMat2x2New(cpFloat a, cpFloat b, cpFloat c, cpFloat d) +{ + cpMat2x2 m = {a, b, c, d}; + return m; +} + +static inline cpVect +cpMat2x2Transform(cpMat2x2 m, cpVect v) +{ + return cpv(v.x*m.a + v.y*m.b, v.x*m.c + v.y*m.d); +} + +///@} diff --git a/projects/linux/ee.creator.user b/projects/linux/ee.creator.user index 40882c4a0..db0780ec3 100644 --- a/projects/linux/ee.creator.user +++ b/projects/linux/ee.creator.user @@ -1,6 +1,6 @@ - + ProjectExplorer.Project.ActiveTarget diff --git a/src/eepp/helper/chipmunk/chipmunk.c b/src/eepp/helper/chipmunk/chipmunk.c index 4ff860dfb..8487dd0b7 100644 --- a/src/eepp/helper/chipmunk/chipmunk.c +++ b/src/eepp/helper/chipmunk/chipmunk.c @@ -19,24 +19,23 @@ * SOFTWARE. */ -#include #include -#include +#include +#include #include "chipmunk_private.h" -//#ifdef __cplusplus -//extern "C" { -//#endif -// void cpInitCollisionFuncs(void); -//#ifdef __cplusplus -//} -//#endif - void -cpMessage(const char *message, const char *condition, const char *file, int line, int isError) +cpMessage(const char *condition, const char *file, int line, cpBool isError, cpBool isHardError, const char *message, ...) { - fprintf(stderr, (isError ? "Aborting due to Chipmunk error: %s\n" : "Chipmunk warning: %s\n"), message); + fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: ")); + + va_list vargs; + va_start(vargs, message); { + vfprintf(stderr, message, vargs); + fprintf(stderr, "\n"); + } va_end(vargs); + fprintf(stderr, "\tFailed condition: %s\n", condition); fprintf(stderr, "\tSource:%s:%d\n", file, line); @@ -51,14 +50,11 @@ const char *cpVersionString = XSTR(CP_VERSION_MAJOR)"."XSTR(CP_VERSION_MINOR)"." void cpInitChipmunk(void) { -//#ifndef NDEBUG -// printf("Initializing Chipmunk v%s (Debug Enabled)\n", cpVersionString); -// printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); -//#endif -// -// cpInitCollisionFuncs(); + cpAssertWarn(cpFalse, "cpInitChipmunk is deprecated and no longer required. It will be removed in the future."); } +//MARK: Misc Functions + cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) { @@ -74,10 +70,8 @@ cpAreaForCircle(cpFloat r1, cpFloat r2) cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b) { - cpFloat length = cpvlength(cpvsub(b, a)); - cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f); - - return m*(length*length/12.0f + cpvlengthsq(offset)); + cpVect offset = cpvmult(cpvadd(a, b), 0.5f); + return m*(cpvdistsq(b, a)/12.0f + cpvlengthsq(offset)); } cpFloat @@ -156,7 +150,173 @@ cpMomentForBox2(cpFloat m, cpBB box) cpFloat height = box.t - box.b; cpVect offset = cpvmult(cpv(box.l + box.r, box.b + box.t), 0.5f); + // TODO NaN when offset is 0 and m is INFINITY return cpMomentForBox(m, width, height) + m*cpvlengthsq(offset); } +//MARK: Quick Hull + +void +cpLoopIndexes(cpVect *verts, int count, int *start, int *end) +{ + (*start) = (*end) = 0; + cpVect min = verts[0]; + cpVect max = min; + + for(int i=1; i max.x || (v.x == max.x && v.y > max.y)){ + max = v; + (*end) = i; + } + } +} + +#define SWAP(__A__, __B__) {cpVect __TMP__ = __A__; __A__ = __B__; __B__ = __TMP__;} + +static int +QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol) +{ + if(count == 0) return 0; + + cpFloat max = 0; + int pivot = 0; + + cpVect delta = cpvsub(b, a); + cpFloat valueTol = tol*cpvlength(delta); + + int head = 0; + for(int tail = count-1; head <= tail;){ + cpFloat value = cpvcross(delta, cpvsub(verts[head], a)); + if(value > valueTol){ + if(value > max){ + max = value; + pivot = head; + } + + head++; + } else { + SWAP(verts[head], verts[tail]); + tail--; + } + } + + // move the new pivot to the front if it's not already there. + if(pivot != 0) SWAP(verts[0], verts[pivot]); + return head; +} + +static int +QHullReduce(cpFloat tol, cpVect *verts, int count, cpVect a, cpVect pivot, cpVect b, cpVect *result) +{ + if(count < 0){ + return 0; + } else if(count == 0) { + result[0] = pivot; + return 1; + } else { + int left_count = QHullPartition(verts, count, a, pivot, tol); + int index = QHullReduce(tol, verts + 1, left_count - 1, a, verts[0], pivot, result); + + result[index++] = pivot; + + int right_count = QHullPartition(verts + left_count, count - left_count, pivot, b, tol); + return index + QHullReduce(tol, verts + left_count + 1, right_count - 1, pivot, verts[left_count], b, result + index); + } +} + +// QuickHull seemed like a neat algorithm, and efficient-ish for large input sets. +// My implementation performs an in place reduction using the result array as scratch space. +int +cpConvexHull(int count, cpVect *verts, cpVect *result, int *first, cpFloat tol) +{ + if(result){ + // Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer. + memcpy(result, verts, count*sizeof(cpVect)); + } else { + // If a result array was not specified, reduce the input instead. + result = verts; + } + + // Degenerate case, all poins are the same. + int start, end; + cpLoopIndexes(verts, count, &start, &end); + if(start == end){ + if(first) (*first) = 0; + return 1; + } + + SWAP(result[0], result[start]); + SWAP(result[1], result[end == 0 ? start : end]); + + cpVect a = result[0]; + cpVect b = result[1]; + + if(first) (*first) = start; + int resultCount = QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1; + cpAssertSoft(cpPolyValidate(result, resultCount), + "Internal error: cpConvexHull() and cpPolyValidate() did not agree." + "Please report this error with as much info as you can."); + return resultCount; +} + +//MARK: Alternate Block Iterators + +#if defined(__has_extension) +#if __has_extension(blocks) + +static void IteratorFunc(void *ptr, void (^block)(void *ptr)){block(ptr);} + +void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)){ + cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)IteratorFunc, block); +} + +void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)){ + cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)IteratorFunc, block); +} + +void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)){ + cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)IteratorFunc, block); +} + +static void BodyIteratorFunc(cpBody *body, void *ptr, void (^block)(void *ptr)){block(ptr);} + +void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)){ + cpBodyEachShape(body, (cpBodyShapeIteratorFunc)BodyIteratorFunc, block); +} + +void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)){ + cpBodyEachConstraint(body, (cpBodyConstraintIteratorFunc)BodyIteratorFunc, block); +} + +void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)){ + cpBodyEachArbiter(body, (cpBodyArbiterIteratorFunc)BodyIteratorFunc, block); +} + +static void NearestPointQueryIteratorFunc(cpShape *shape, cpFloat distance, cpVect point, cpSpaceNearestPointQueryBlock block){block(shape, distance, point);} +void cpSpaceNearestPointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryBlock block){ + cpSpaceNearestPointQuery(space, point, maxDistance, layers, group, (cpSpaceNearestPointQueryFunc)NearestPointQueryIteratorFunc, block); +} + +static void SegmentQueryIteratorFunc(cpShape *shape, cpFloat t, cpVect n, cpSpaceSegmentQueryBlock block){block(shape, t, n);} +void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryBlock block){ + cpSpaceSegmentQuery(space, start, end, layers, group, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block); +} + +void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryBlock block){ + cpSpaceBBQuery(space, bb, layers, group, (cpSpaceBBQueryFunc)IteratorFunc, block); +} + +static void ShapeQueryIteratorFunc(cpShape *shape, cpContactPointSet *points, cpSpaceShapeQueryBlock block){block(shape, points);} +cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block){ + return cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)ShapeQueryIteratorFunc, block); +} + +#endif +#endif + #include "chipmunk_ffi.h" diff --git a/src/eepp/helper/chipmunk/constraints/cpConstraint.c b/src/eepp/helper/chipmunk/constraints/cpConstraint.c index e4c5c0cd0..d3902f46f 100644 --- a/src/eepp/helper/chipmunk/constraints/cpConstraint.c +++ b/src/eepp/helper/chipmunk/constraints/cpConstraint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -38,7 +35,7 @@ cpConstraintFree(cpConstraint *constraint) } } -// *** defined in util.h +// *** declared in util.h TODO move declaration to chipmunk_private.h void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b) @@ -47,6 +44,7 @@ cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBod constraint->a = a; constraint->b = b; + constraint->space = NULL; constraint->next_a = NULL; constraint->next_b = NULL; @@ -54,4 +52,7 @@ cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBod constraint->maxForce = (cpFloat)INFINITY; constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f); constraint->maxBias = (cpFloat)INFINITY; + + constraint->preSolve = NULL; + constraint->postSolve = NULL; } diff --git a/src/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.c b/src/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.c index 1720034b8..5bc20950a 100644 --- a/src/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.c +++ b/src/eepp/helper/chipmunk/constraints/cpDampedRotarySpring.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -52,7 +49,7 @@ preStep(cpDampedRotarySpring *spring, cpFloat dt) static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){} static void -applyImpulse(cpDampedRotarySpring *spring) +applyImpulse(cpDampedRotarySpring *spring, cpFloat dt) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; diff --git a/src/eepp/helper/chipmunk/constraints/cpDampedSpring.c b/src/eepp/helper/chipmunk/constraints/cpDampedSpring.c index 230943810..adf888b0c 100644 --- a/src/eepp/helper/chipmunk/constraints/cpDampedSpring.c +++ b/src/eepp/helper/chipmunk/constraints/cpDampedSpring.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -58,7 +55,7 @@ preStep(cpDampedSpring *spring, cpFloat dt) static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){} static void -applyImpulse(cpDampedSpring *spring) +applyImpulse(cpDampedSpring *spring, cpFloat dt) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; diff --git a/src/eepp/helper/chipmunk/constraints/cpGearJoint.c b/src/eepp/helper/chipmunk/constraints/cpGearJoint.c index 37cb45f32..818c1b466 100644 --- a/src/eepp/helper/chipmunk/constraints/cpGearJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpGearJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -37,9 +34,6 @@ preStep(cpGearJoint *joint, cpFloat dt) // calculate bias velocity cpFloat maxBias = joint->constraint.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); } static void @@ -54,7 +48,7 @@ applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpGearJoint *joint) +applyImpulse(cpGearJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -62,10 +56,12 @@ applyImpulse(cpGearJoint *joint) // compute relative rotational velocity cpFloat wr = b->w*joint->ratio - a->w; + cpFloat jMax = joint->constraint.maxForce*dt; + // compute normal impulse cpFloat j = (joint->bias - wr)*joint->iSum; cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); j = joint->jAcc - jOld; // apply impulse diff --git a/src/eepp/helper/chipmunk/constraints/cpGrooveJoint.c b/src/eepp/helper/chipmunk/constraints/cpGrooveJoint.c index 99abe7f6f..efe14aed7 100644 --- a/src/eepp/helper/chipmunk/constraints/cpGrooveJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpGrooveJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -57,10 +54,7 @@ preStep(cpGrooveJoint *joint, cpFloat dt) } // Calculate mass tensor - k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); - - // compute max impulse - joint->jMaxLen = J_MAX(joint, dt); + joint->k = k_tensor(a, b, joint->r1, joint->r2); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); @@ -77,14 +71,14 @@ applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef) } static inline cpVect -grooveConstrain(cpGrooveJoint *joint, cpVect j){ +grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){ cpVect n = joint->grv_tn; cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); - return cpvclamp(jClamp, joint->jMaxLen); + return cpvclamp(jClamp, joint->constraint.maxForce*dt); } static void -applyImpulse(cpGrooveJoint *joint) +applyImpulse(cpGrooveJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -95,9 +89,9 @@ applyImpulse(cpGrooveJoint *joint) // compute impulse cpVect vr = relative_velocity(a, b, r1, r2); - cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); cpVect jOld = joint->jAcc; - joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j)); + joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt); j = cpvsub(joint->jAcc, jOld); // apply impulse diff --git a/src/eepp/helper/chipmunk/constraints/cpPinJoint.c b/src/eepp/helper/chipmunk/constraints/cpPinJoint.c index 4210964b6..abff6cef2 100644 --- a/src/eepp/helper/chipmunk/constraints/cpPinJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpPinJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -44,9 +41,6 @@ preStep(cpPinJoint *joint, cpFloat dt) // calculate bias velocity cpFloat maxBias = joint->constraint.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 @@ -60,7 +54,7 @@ applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpPinJoint *joint) +applyImpulse(cpPinJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -69,10 +63,12 @@ applyImpulse(cpPinJoint *joint) // compute relative velocity cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n); + cpFloat jnMax = joint->constraint.maxForce*dt; + // compute normal impulse cpFloat jn = (joint->bias - vrn)*joint->nMass; cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax); + joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax); jn = joint->jnAcc - jnOld; // apply impulse @@ -91,7 +87,7 @@ static const cpConstraintClass klass = { (cpConstraintApplyImpulseImpl)applyImpulse, (cpConstraintGetImpulseImpl)getImpulse, }; -CP_DefineClassGetter(cpPinJoint); +CP_DefineClassGetter(cpPinJoint) cpPinJoint * @@ -112,6 +108,8 @@ cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect an cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1); cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2); joint->dist = cpvlength(cpvsub(p2, p1)); + + cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable."); joint->jnAcc = 0.0f; diff --git a/src/eepp/helper/chipmunk/constraints/cpPivotJoint.c b/src/eepp/helper/chipmunk/constraints/cpPivotJoint.c index 5f51144e4..5a042bc05 100644 --- a/src/eepp/helper/chipmunk/constraints/cpPivotJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpPivotJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -35,10 +32,7 @@ preStep(cpPivotJoint *joint, cpFloat dt) joint->r2 = cpvrotate(joint->anchr2, b->rot); // Calculate mass tensor - k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); - - // compute max impulse - joint->jMaxLen = J_MAX(joint, dt); + joint-> k = k_tensor(a, b, joint->r1, joint->r2); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); @@ -55,7 +49,7 @@ applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpPivotJoint *joint) +applyImpulse(cpPivotJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -67,9 +61,9 @@ applyImpulse(cpPivotJoint *joint) cpVect vr = relative_velocity(a, b, r1, r2); // compute normal impulse - cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); cpVect jOld = joint->jAcc; - joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen); + joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt); j = cpvsub(joint->jAcc, jOld); // apply impulse diff --git a/src/eepp/helper/chipmunk/constraints/cpRatchetJoint.c b/src/eepp/helper/chipmunk/constraints/cpRatchetJoint.c index 69defb0a0..4e30ba5c8 100644 --- a/src/eepp/helper/chipmunk/constraints/cpRatchetJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpRatchetJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -51,9 +48,6 @@ preStep(cpRatchetJoint *joint, cpFloat dt) // calculate bias velocity cpFloat maxBias = joint->constraint.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; @@ -71,7 +65,7 @@ applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpRatchetJoint *joint) +applyImpulse(cpRatchetJoint *joint, cpFloat dt) { if(!joint->bias) return; // early exit @@ -82,10 +76,12 @@ applyImpulse(cpRatchetJoint *joint) cpFloat wr = b->w - a->w; cpFloat ratchet = joint->ratchet; + cpFloat jMax = joint->constraint.maxForce*dt; + // compute normal impulse cpFloat j = -(joint->bias + wr)*joint->iSum; cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet; + joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, jMax*cpfabs(ratchet))/ratchet; j = joint->jAcc - jOld; // apply impulse diff --git a/src/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.c b/src/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.c index a314112c2..b9555b5c5 100644 --- a/src/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpRotaryLimitJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -45,9 +42,6 @@ preStep(cpRotaryLimitJoint *joint, cpFloat dt) // calculate bias velocity cpFloat maxBias = joint->constraint.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; @@ -65,7 +59,7 @@ applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpRotaryLimitJoint *joint) +applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt) { if(!joint->bias) return; // early exit @@ -75,13 +69,15 @@ applyImpulse(cpRotaryLimitJoint *joint) // compute relative rotational velocity cpFloat wr = b->w - a->w; + cpFloat jMax = joint->constraint.maxForce*dt; + // compute normal impulse cpFloat j = -(joint->bias + wr)*joint->iSum; cpFloat jOld = joint->jAcc; if(joint->bias < 0.0f){ - joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax); + joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax); } else { - joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f); + joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f); } j = joint->jAcc - jOld; diff --git a/src/eepp/helper/chipmunk/constraints/cpSimpleMotor.c b/src/eepp/helper/chipmunk/constraints/cpSimpleMotor.c index 45082bcc3..b7a023bd1 100644 --- a/src/eepp/helper/chipmunk/constraints/cpSimpleMotor.c +++ b/src/eepp/helper/chipmunk/constraints/cpSimpleMotor.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -33,9 +30,6 @@ preStep(cpSimpleMotor *joint, cpFloat dt) // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); - - // compute max impulse - joint->jMax = J_MAX(joint, dt); } static void @@ -50,7 +44,7 @@ applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef) } static void -applyImpulse(cpSimpleMotor *joint) +applyImpulse(cpSimpleMotor *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -58,10 +52,12 @@ applyImpulse(cpSimpleMotor *joint) // compute relative rotational velocity cpFloat wr = b->w - a->w + joint->rate; + cpFloat jMax = joint->constraint.maxForce*dt; + // compute normal impulse cpFloat j = -wr*joint->iSum; cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); j = joint->jAcc - jOld; // apply impulse diff --git a/src/eepp/helper/chipmunk/constraints/cpSlideJoint.c b/src/eepp/helper/chipmunk/constraints/cpSlideJoint.c index 6d19a5846..87e691d18 100644 --- a/src/eepp/helper/chipmunk/constraints/cpSlideJoint.c +++ b/src/eepp/helper/chipmunk/constraints/cpSlideJoint.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -39,11 +36,14 @@ preStep(cpSlideJoint *joint, cpFloat dt) cpFloat pdist = 0.0f; if(dist > joint->max) { pdist = dist - joint->max; + joint->n = cpvnormalize_safe(delta); } else if(dist < joint->min) { pdist = joint->min - dist; - dist = -dist; + joint->n = cpvneg(cpvnormalize_safe(delta)); + } else { + joint->n = cpvzero; + joint->jnAcc = 0.0f; } - joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); // calculate mass normal joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); @@ -51,12 +51,6 @@ preStep(cpSlideJoint *joint, cpFloat dt) // calculate bias velocity cpFloat maxBias = joint->constraint.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; } static void @@ -70,9 +64,9 @@ applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) } static void -applyImpulse(cpSlideJoint *joint) +applyImpulse(cpSlideJoint *joint, cpFloat dt) { - if(!joint->bias) return; // early exit + if(cpveql(joint->n, cpvzero)) return; // early exit cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; @@ -88,7 +82,7 @@ applyImpulse(cpSlideJoint *joint) // compute normal impulse cpFloat jn = (joint->bias - vrn)*joint->nMass; cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f); + joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f); jn = joint->jnAcc - jnOld; // apply impulse diff --git a/src/eepp/helper/chipmunk/cpArbiter.c b/src/eepp/helper/chipmunk/cpArbiter.c index 1e65d140c..690d2fd13 100644 --- a/src/eepp/helper/chipmunk/cpArbiter.c +++ b/src/eepp/helper/chipmunk/cpArbiter.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "constraints/util.h" @@ -68,10 +65,21 @@ cpArbiterUnthread(cpArbiter *arb) unthreadHelper(arb, arb->body_b); } +cpBool cpArbiterIsFirstContact(const cpArbiter *arb) +{ + return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl; +} + +int cpArbiterGetCount(const cpArbiter *arb) +{ + // Return 0 contacts if we are in a separate callback. + return (arb->CP_PRIVATE(state) != cpArbiterStateCached ? arb->CP_PRIVATE(numContacts) : 0); +} + cpVect cpArbiterGetNormal(const cpArbiter *arb, int i) { - cpAssertHard(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); cpVect n = arb->contacts[i].n; return arb->swappedColl ? cpvneg(n) : n; @@ -80,7 +88,7 @@ cpArbiterGetNormal(const cpArbiter *arb, int i) cpVect cpArbiterGetPoint(const cpArbiter *arb, int i) { - cpAssertHard(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); } @@ -88,7 +96,7 @@ cpArbiterGetPoint(const cpArbiter *arb, int i) cpFloat cpArbiterGetDepth(const cpArbiter *arb, int i) { - cpAssertHard(0 <= i && i < arb->numContacts, "Index error: The specified contact index is invalid for this arbiter"); + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); } @@ -116,12 +124,12 @@ cpArbiterTotalImpulse(const cpArbiter *arb) cpContact *contacts = arb->contacts; cpVect sum = cpvzero; - for(int i=0, count=arb->numContacts; in, con->jnAcc)); } - - return sum; + + return (arb->swappedColl ? sum : cpvneg(sum)); } cpVect @@ -130,11 +138,29 @@ cpArbiterTotalImpulseWithFriction(const cpArbiter *arb) cpContact *contacts = arb->contacts; cpVect sum = cpvzero; - for(int i=0, count=arb->numContacts; in, cpv(con->jnAcc, con->jtAcc))); } + return (arb->swappedColl ? sum : cpvneg(sum)); +} + +cpFloat +cpArbiterTotalKE(const cpArbiter *arb) +{ + cpFloat eCoef = (1 - arb->e)/(1 + arb->e); + cpFloat sum = 0.0; + + cpContact *contacts = arb->contacts; + for(int i=0, count=cpArbiterGetCount(arb); ijnAcc; + cpFloat jtAcc = con->jtAcc; + + sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass; + } + return sum; } @@ -186,6 +212,8 @@ cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) arb->stamp = 0; arb->state = cpArbiterStateFirstColl; + arb->data = NULL; + return arb; } @@ -277,48 +305,38 @@ cpArbiterApplyImpulse(cpArbiter *arb) { cpBody *a = arb->body_a; cpBody *b = arb->body_b; + cpVect surface_vr = arb->surface_vr; + cpFloat friction = arb->u; for(int i=0; inumContacts; i++){ cpContact *con = &arb->contacts[i]; + cpFloat nMass = con->nMass; cpVect n = con->n; cpVect r1 = con->r1; cpVect r2 = con->r2; - // Calculate the relative bias velocities. cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); - cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + cpVect vr = relative_velocity(a, b, r1, r2); - // Calculate and clamp the bias impulse. - cpFloat jbn = (con->bias - vbn)*con->nMass; + cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + cpFloat vrn = cpvdot(vr, n); + cpFloat vrt = cpvdot(cpvadd(vr, surface_vr), cpvperp(n)); + + cpFloat jbn = (con->bias - vbn)*nMass; cpFloat jbnOld = con->jBias; con->jBias = cpfmax(jbnOld + jbn, 0.0f); - jbn = con->jBias - jbnOld; - // Apply the bias impulse. - apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn)); - - // Calculate the relative velocity. - cpVect vr = relative_velocity(a, b, r1, r2); - cpFloat vrn = cpvdot(vr, n); - - // Calculate and clamp the normal impulse. - cpFloat jn = -(con->bounce + vrn)*con->nMass; + cpFloat jn = -(con->bounce + vrn)*nMass; cpFloat jnOld = con->jnAcc; con->jnAcc = cpfmax(jnOld + jn, 0.0f); - jn = con->jnAcc - jnOld; - // Calculate the relative tangent velocity. - cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n)); - - // Calculate and clamp the friction impulse. - cpFloat jtMax = arb->u*con->jnAcc; + cpFloat jtMax = friction*con->jnAcc; cpFloat jt = -vrt*con->tMass; cpFloat jtOld = con->jtAcc; con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); - jt = con->jtAcc - jtOld; - // Apply the final impulse. - apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt))); + apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld)); + apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld))); } } diff --git a/src/eepp/helper/chipmunk/cpArray.c b/src/eepp/helper/chipmunk/cpArray.c index a4a4fd87e..d944fd110 100644 --- a/src/eepp/helper/chipmunk/cpArray.c +++ b/src/eepp/helper/chipmunk/cpArray.c @@ -19,7 +19,6 @@ * SOFTWARE. */ -#include #include #include "chipmunk_private.h" diff --git a/src/eepp/helper/chipmunk/cpBB.c b/src/eepp/helper/chipmunk/cpBB.c index d2ced897f..5a8d20a6a 100644 --- a/src/eepp/helper/chipmunk/cpBB.c +++ b/src/eepp/helper/chipmunk/cpBB.c @@ -19,19 +19,8 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" -cpVect -cpBBClampVect(const cpBB bb, const cpVect v) -{ - cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r); - cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t); - return cpv(x, y); -} - cpVect cpBBWrapVect(const cpBB bb, const cpVect v) { diff --git a/src/eepp/helper/chipmunk/cpBBTree.c b/src/eepp/helper/chipmunk/cpBBTree.c index 9930e5591..b1286867d 100644 --- a/src/eepp/helper/chipmunk/cpBBTree.c +++ b/src/eepp/helper/chipmunk/cpBBTree.c @@ -50,16 +50,22 @@ struct Node { union { // Internal nodes - struct { Node *a, *b; }; + struct { Node *a, *b; } children; // Leaves struct { cpTimestamp stamp; Pair *pairs; - }; - }; + } leaf; + } node; }; +// Can't use anonymous unions and still get good x-compiler compatability +#define A node.children.a +#define B node.children.b +#define STAMP node.leaf.stamp +#define PAIRS node.leaf.pairs + typedef struct Thread { Pair *prev; Node *leaf; @@ -68,13 +74,7 @@ typedef struct Thread { struct Pair { Thread a, b; }; -#pragma mark Misc Functions - -//static inline cpFloat -//cpBBProximity(cpBB a, cpBB b) -//{ -// return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + b.t - b.b - b.t); -//} +//MARK: Misc Functions static inline cpBB GetBB(cpBBTree *tree, void *obj) @@ -105,11 +105,11 @@ GetRootIfTree(cpSpatialIndex *index){ return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL); } -static inline cpTimestamp -GetStamp(cpBBTree *tree) +static inline cpBBTree * +GetMasterTree(cpBBTree *tree) { cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); - return (dynamicTree ? dynamicTree->stamp : tree->stamp); + return (dynamicTree ? dynamicTree : tree); } static inline void @@ -123,11 +123,15 @@ IncrementStamp(cpBBTree *tree) } } -#pragma mark Pair/Thread Functions +//MARK: Pair/Thread Functions static void PairRecycle(cpBBTree *tree, Pair *pair) { + // Share the pool of the master tree. + // TODO would be lovely to move the pairs stuff into an external data structure. + tree = GetMasterTree(tree); + pair->a.next = tree->pooledPairs; tree->pooledPairs = pair; } @@ -135,6 +139,10 @@ PairRecycle(cpBBTree *tree, Pair *pair) static Pair * PairFromPool(cpBBTree *tree) { + // Share the pool of the master tree. + // TODO would be lovely to move the pairs stuff into an external data structure. + tree = GetMasterTree(tree); + Pair *pair = tree->pooledPairs; if(pair){ @@ -143,7 +151,7 @@ PairFromPool(cpBBTree *tree) } else { // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(Pair); - cpAssertSoft(count, "Buffer size is too small."); + cpAssertHard(count, "Internal Error: Buffer size is too small."); Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(tree->allocatedBuffers, buffer); @@ -167,15 +175,15 @@ ThreadUnlink(Thread thread) if(prev){ if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; } else { - thread.leaf->pairs = next; + thread.leaf->PAIRS = next; } } static void PairsClear(Node *leaf, cpBBTree *tree) { - Pair *pair = leaf->pairs; - leaf->pairs = NULL; + Pair *pair = leaf->PAIRS; + leaf->PAIRS = NULL; while(pair){ if(pair->a.leaf == leaf){ @@ -195,11 +203,11 @@ PairsClear(Node *leaf, cpBBTree *tree) static void PairInsert(Node *a, Node *b, cpBBTree *tree) { - Pair *nextA = a->pairs, *nextB = b->pairs; + Pair *nextA = a->PAIRS, *nextB = b->PAIRS; Pair *pair = PairFromPool(tree); Pair temp = {{NULL, a, nextA},{NULL, b, nextB}}; - a->pairs = b->pairs = pair; + a->PAIRS = b->PAIRS = pair; *pair = temp; if(nextA){ @@ -212,7 +220,7 @@ PairInsert(Node *a, Node *b, cpBBTree *tree) } -#pragma mark Node Functions +//MARK: Node Functions static void NodeRecycle(cpBBTree *tree, Node *node) @@ -232,7 +240,7 @@ NodeFromPool(cpBBTree *tree) } else { // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(Node); - cpAssertSoft(count, "Buffer size is too small."); + cpAssertHard(count, "Internal Error: Buffer size is too small."); Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(tree->allocatedBuffers, buffer); @@ -246,14 +254,14 @@ NodeFromPool(cpBBTree *tree) static inline void NodeSetA(Node *node, Node *value) { - node->a = value; + node->A = value; value->parent = node; } static inline void NodeSetB(Node *node, Node *value) { - node->b = value; + node->B = value; value->parent = node; } @@ -281,29 +289,35 @@ NodeIsLeaf(Node *node) static inline Node * NodeOther(Node *node, Node *child) { - return (node->a == child ? node->b : node->a); + return (node->A == child ? node->B : node->A); } static inline void NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) { - cpAssertSoft(!NodeIsLeaf(parent), "Cannot replace child of a leaf."); - cpAssertSoft(child == parent->a || child == parent->b, "Node is not a child of parent."); + cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf."); + cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent."); - if(parent->a == child){ - NodeRecycle(tree, parent->a); + if(parent->A == child){ + NodeRecycle(tree, parent->A); NodeSetA(parent, value); } else { - NodeRecycle(tree, parent->b); + 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); } } -#pragma mark Subtree Functions +//MARK: Subtree Functions + +static inline cpFloat +cpBBProximity(cpBB a, cpBB b) +{ + return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + a.t - b.b - b.t); +} static Node * SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) @@ -313,16 +327,18 @@ SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) } else if(NodeIsLeaf(subtree)){ return NodeNew(tree, leaf, subtree); } 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 = 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_a == cost_b){ + cost_a = cpBBProximity(subtree->A->bb, leaf->bb); + cost_b = cpBBProximity(subtree->B->bb, leaf->bb); + } if(cost_b < cost_a){ - NodeSetB(subtree, SubtreeInsert(subtree->b, leaf, tree)); + NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree)); } else { - NodeSetA(subtree, SubtreeInsert(subtree->a, leaf, tree)); + NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree)); } subtree->bb = cpBBMerge(subtree->bb, leaf->bb); @@ -337,24 +353,31 @@ SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, vo if(NodeIsLeaf(subtree)){ func(obj, subtree->obj, data); } else { - SubtreeQuery(subtree->a, obj, bb, func, data); - SubtreeQuery(subtree->b, obj, bb, func, data); + SubtreeQuery(subtree->A, obj, bb, func, data); + SubtreeQuery(subtree->B, obj, bb, func, data); } } } -// TODO Needs early exit optimization for ray queries -static void -SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpSpatialIndexSegmentQueryFunc func, void *data) +static cpFloat +SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { - if(cpBBIntersectsSegment(subtree->bb, a, b)){ - if(NodeIsLeaf(subtree)){ - func(obj, subtree->obj, data); + if(NodeIsLeaf(subtree)){ + return func(obj, subtree->obj, data); + } else { + cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b); + cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b); + + if(t_a < t_b){ + if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); + if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); } else { - SubtreeSegmentQuery(subtree->a, obj, a, b, func, data); - SubtreeSegmentQuery(subtree->b, obj, a, b, func, data); + if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); + if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); } + + return t_exit; } } @@ -362,8 +385,8 @@ static void SubtreeRecycle(cpBBTree *tree, Node *node) { if(!NodeIsLeaf(node)){ - SubtreeRecycle(tree, node->a); - SubtreeRecycle(tree, node->b); + SubtreeRecycle(tree, node->A); + SubtreeRecycle(tree, node->B); NodeRecycle(tree, node); } } @@ -387,7 +410,7 @@ SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) } } -#pragma mark Marking Functions +//MARK: Marking Functions typedef struct MarkContext { cpBBTree *tree; @@ -404,12 +427,12 @@ MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) if(left){ PairInsert(leaf, subtree, context->tree); } else { - if(subtree->stamp < leaf->stamp) PairInsert(subtree, leaf, context->tree); + if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree); context->func(leaf->obj, subtree->obj, context->data); } } else { - MarkLeafQuery(subtree->a, leaf, left, context); - MarkLeafQuery(subtree->b, leaf, left, context); + MarkLeafQuery(subtree->A, leaf, left, context); + MarkLeafQuery(subtree->B, leaf, left, context); } } } @@ -418,19 +441,19 @@ static void MarkLeaf(Node *leaf, MarkContext *context) { cpBBTree *tree = context->tree; - if(leaf->stamp == GetStamp(tree)){ + if(leaf->STAMP == GetMasterTree(tree)->stamp){ 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); + if(node == node->parent->A){ + MarkLeafQuery(node->parent->B, leaf, cpTrue, context); } else { - MarkLeafQuery(node->parent->a, leaf, cpFalse, context); + MarkLeafQuery(node->parent->A, leaf, cpFalse, context); } } } else { - Pair *pair = leaf->pairs; + Pair *pair = leaf->PAIRS; while(pair){ if(leaf == pair->b.leaf){ context->func(pair->a.leaf->obj, leaf->obj, context->data); @@ -448,12 +471,12 @@ MarkSubtree(Node *subtree, MarkContext *context) if(NodeIsLeaf(subtree)){ MarkLeaf(subtree, context); } else { - MarkSubtree(subtree->a, context); - MarkSubtree(subtree->b, context); + MarkSubtree(subtree->A, context); + MarkSubtree(subtree->B, context); } } -#pragma mark Leaf Functions +//MARK: Leaf Functions static Node * LeafNew(cpBBTree *tree, void *obj, cpBB bb) @@ -463,8 +486,8 @@ LeafNew(cpBBTree *tree, void *obj, cpBB bb) node->bb = GetBB(tree, obj); node->parent = NULL; - node->stamp = 0; - node->pairs = NULL; + node->STAMP = 0; + node->PAIRS = NULL; return node; } @@ -482,7 +505,7 @@ LeafUpdate(Node *leaf, cpBBTree *tree) tree->root = SubtreeInsert(root, leaf, tree); PairsClear(leaf, tree); - leaf->stamp = GetStamp(tree); + leaf->STAMP = GetMasterTree(tree)->stamp; return cpTrue; } @@ -510,7 +533,7 @@ LeafAddPairs(Node *leaf, cpBBTree *tree) } } -#pragma mark Memory Management Functions +//MARK: Memory Management Functions cpBBTree * cpBBTreeAlloc(void) @@ -574,7 +597,7 @@ cpBBTreeDestroy(cpBBTree *tree) cpArrayFree(tree->allocatedBuffers); } -#pragma mark Insert/Remove +//MARK: Insert/Remove static void cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) @@ -584,7 +607,7 @@ cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) Node *root = tree->root; tree->root = SubtreeInsert(root, leaf, tree); - leaf->stamp = GetStamp(tree); + leaf->STAMP = GetMasterTree(tree)->stamp; LeafAddPairs(leaf, tree); IncrementStamp(tree); } @@ -605,7 +628,7 @@ cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) return (cpHashSetFind(tree->leaves, hashid, obj) != NULL); } -#pragma mark Reindex +//MARK: Reindex static void cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data) @@ -641,20 +664,13 @@ cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) } } -#pragma mark Query - -static void -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); -} +//MARK: Query static void 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); + if(root) SubtreeSegmentQuery(root, obj, a, b, t_exit, func, data); } static void @@ -663,7 +679,7 @@ cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); } -#pragma mark Misc +//MARK: Misc static int cpBBTreeCount(cpBBTree *tree) @@ -699,15 +715,14 @@ static cpSpatialIndexClass klass = { (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject, (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery, - (cpSpatialIndexPointQueryImpl)cpBBTreePointQuery, - (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, (cpSpatialIndexQueryImpl)cpBBTreeQuery, + (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, }; static inline cpSpatialIndexClass *Klass(){return &klass;} -#pragma mark Tree Optimization +//MARK: Tree Optimization static int cpfcompare(const cpFloat *a, const cpFloat *b){ @@ -827,7 +842,7 @@ cpBBTreeOptimize(cpSpatialIndex *index) cpfree(nodes); } -#pragma mark Debug Draw +//MARK: Debug Draw //#define CP_BBTREE_DEBUG_DRAW #ifdef CP_BBTREE_DEBUG_DRAW diff --git a/src/eepp/helper/chipmunk/cpBody.c b/src/eepp/helper/chipmunk/cpBody.c index 6cef547b6..f4dc8420f 100644 --- a/src/eepp/helper/chipmunk/cpBody.c +++ b/src/eepp/helper/chipmunk/cpBody.c @@ -19,9 +19,7 @@ * SOFTWARE. */ -#include #include -#include #include "chipmunk_private.h" #include "constraints/util.h" @@ -88,7 +86,7 @@ cpBodyInitStatic(cpBody *body) } cpBody * -cpBodyNewStatic() +cpBodyNewStatic(void) { return cpBodyInitStatic(cpBodyAlloc()); } @@ -108,6 +106,10 @@ static void cpv_assert_nan(cpVect v, char *message){cpAssertSoft(v.x == v.x && v static void cpv_assert_infinite(cpVect v, char *message){cpAssertSoft(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);} +#ifdef __cplusplus +extern "C" { +#endif + void cpBodySanityCheck(cpBody *body) { @@ -122,26 +124,36 @@ cpBodySanityCheck(cpBody *body) cpAssertSoft(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid."); cpAssertSoft(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."); + cpv_assert_sane(body->rot, "Body's rotation vector is invalid."); cpAssertSoft(body->v_limit == body->v_limit, "Body's velocity limit is invalid."); cpAssertSoft(body->w_limit == body->w_limit, "Body's angular velocity limit is invalid."); } +#ifdef __cplusplus +} +#endif + void cpBodySetMass(cpBody *body, cpFloat mass) { + cpAssertHard(mass > 0.0f, "Mass must be positive and non-zero."); + cpBodyActivate(body); body->m = mass; body->m_inv = 1.0f/mass; + cpBodyAssertSane(body); } void cpBodySetMoment(cpBody *body, cpFloat moment) { + cpAssertHard(moment > 0.0f, "Moment of Inertia must be positive and non-zero."); + cpBodyActivate(body); body->i = moment; body->i_inv = 1.0f/moment; + cpBodyAssertSane(body); } void @@ -198,8 +210,8 @@ void cpBodySetPos(cpBody *body, cpVect pos) { cpBodyActivate(body); - cpBodyAssertSane(body); body->p = pos; + cpBodyAssertSane(body); } static inline void @@ -207,13 +219,13 @@ setAngle(cpBody *body, cpFloat angle) { body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f); body->rot = cpvforangle(angle); + cpBodyAssertSane(body); } void cpBodySetAngle(cpBody *body, cpFloat angle) { cpBodyActivate(body); - cpBodyAssertSane(body); setAngle(body, angle); } @@ -243,6 +255,7 @@ cpBodyUpdatePosition(cpBody *body, cpFloat dt) void cpBodyResetForces(cpBody *body) { + cpBodyActivate(body); body->f = cpvzero; body->t = 0.0f; } @@ -250,6 +263,7 @@ cpBodyResetForces(cpBody *body) void cpBodyApplyForce(cpBody *body, cpVect force, cpVect r) { + cpBodyActivate(body); body->f = cpvadd(body->f, force); body->t += cpvcross(r, force); } @@ -261,6 +275,24 @@ cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r) apply_impulse(body, j, r); } +static inline cpVect +cpBodyGetVelAtPoint(cpBody *body, cpVect r) +{ + return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); +} + +cpVect +cpBodyGetVelAtWorldPoint(cpBody *body, cpVect point) +{ + return cpBodyGetVelAtPoint(body, cpvsub(point, body->p)); +} + +cpVect +cpBodyGetVelAtLocalPoint(cpBody *body, cpVect point) +{ + return cpBodyGetVelAtPoint(body, cpvrotate(point, body->rot)); +} + void cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data) { diff --git a/src/eepp/helper/chipmunk/cpCollision.c b/src/eepp/helper/chipmunk/cpCollision.c index 6a8dccff4..78798116a 100644 --- a/src/eepp/helper/chipmunk/cpCollision.c +++ b/src/eepp/helper/chipmunk/cpCollision.c @@ -19,10 +19,6 @@ * SOFTWARE. */ -#include -#include -//#include - #include "chipmunk_private.h" typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *); @@ -61,54 +57,30 @@ circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); } -// Collide circles to segment shapes. static int -circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con) +circle2segment(const cpCircleShape *circleShape, const cpSegmentShape *segmentShape, cpContact *con) { - cpCircleShape *circ = (cpCircleShape *)circleShape; - cpSegmentShape *seg = (cpSegmentShape *)segmentShape; + cpVect seg_a = segmentShape->ta; + cpVect seg_b = segmentShape->tb; + cpVect center = circleShape->tc; - // Radius sum - cpFloat rsum = circ->r + seg->r; + cpVect seg_delta = cpvsub(seg_b, seg_a); + cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta)); + cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t)); - // Calculate normal distance from segment. - cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn); - cpFloat dist = cpfabs(dn) - rsum; - if(dist > 0.0f) return 0; - - // Calculate tangential distance along segment. - cpFloat dt = -cpvcross(seg->tn, circ->tc); - cpFloat dtMin = -cpvcross(seg->tn, seg->ta); - cpFloat dtMax = -cpvcross(seg->tn, seg->tb); - - // Decision tree to decide which feature of the segment to collide with. - if(dt < dtMin){ - if(dt < (dtMin - rsum)){ - return 0; - } else { - return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con); - } + if(circle2circleQuery(center, closest, circleShape->r, segmentShape->r, con)){ + cpVect n = con[0].n; + + // Reject endcap collisions if tangents are provided. + if( + (closest_t == 0.0f && cpvdot(n, segmentShape->a_tangent) < 0.0) || + (closest_t == 1.0f && cpvdot(n, segmentShape->b_tangent) < 0.0) + ) return 0; + + return 1; } else { - if(dt < dtMax){ - cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn); - cpContactInit( - con, - cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)), - n, - dist, - 0 - ); - return 1; - } else { - if(dt < (dtMax + rsum)) { - return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con); - } else { - return 0; - } - } + return 0; } - - return 1; } // Helper function for working with contact buffers @@ -128,14 +100,14 @@ nextContactPoint(cpContact *arr, int *numPtr) // Find the minimum separating axis for the give poly and axis list. static inline int -findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out) +findMSA(const cpPolyShape *poly, const cpSplittingPlane *planes, const int num, cpFloat *min_out) { int min_index = 0; - cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d); + cpFloat min = cpPolyShapeValueOnAxis(poly, planes->n, planes->d); if(min > 0.0f) return -1; for(int i=1; i 0.0f) { return -1; } else if(dist > min){ @@ -200,18 +172,18 @@ poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) cpPolyShape *poly2 = (cpPolyShape *)shape2; cpFloat min1; - int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1); + int mini1 = findMSA(poly2, poly1->tPlanes, poly1->numVerts, &min1); if(mini1 == -1) return 0; cpFloat min2; - int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2); + int mini2 = findMSA(poly1, poly2->tPlanes, poly2->numVerts, &min2); if(mini2 == -1) return 0; // There is overlap, find the penetrating verts if(min1 > min2) - return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1); + return findVerts(arr, poly1, poly2, poly1->tPlanes[mini1].n, min1); else - return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2); + return findVerts(arr, poly1, poly2, cpvneg(poly2->tPlanes[mini2].n), min2); } // Like cpPolyValueOnAxis(), but for segments. @@ -249,7 +221,7 @@ seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) { cpSegmentShape *seg = (cpSegmentShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; - cpPolyShapeAxis *axes = poly->tAxes; + cpSplittingPlane *planes = poly->tPlanes; cpFloat segD = cpvdot(seg->tn, seg->ta); cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; @@ -257,10 +229,10 @@ seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) if(minNeg > 0.0f || minNorm > 0.0f) return 0; int mini = 0; - cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); + cpFloat poly_min = segValueOnAxis(seg, planes->n, planes->d); if(poly_min > 0.0f) return 0; for(int i=0; inumVerts; i++){ - cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); + cpFloat dist = segValueOnAxis(seg, planes[i].n, planes[i].d); if(dist > 0.0f){ return 0; } else if(dist > poly_min){ @@ -271,7 +243,7 @@ seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) int num = 0; - cpVect poly_n = cpvneg(axes[mini].n); + cpVect poly_n = cpvneg(planes[mini].n); cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); @@ -296,17 +268,10 @@ seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) cpVect poly_a = poly->tVerts[mini]; cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts]; - if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) - return 1; + if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) return 1; + if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) return 1; + if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) return 1; + if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) return 1; } return num; @@ -319,12 +284,12 @@ circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) { cpCircleShape *circ = (cpCircleShape *)shape1; cpPolyShape *poly = (cpPolyShape *)shape2; - cpPolyShapeAxis *axes = poly->tAxes; + cpSplittingPlane *planes = poly->tPlanes; int mini = 0; - cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r; + cpFloat min = cpSplittingPlaneCompare(planes[0], circ->tc) - circ->r; for(int i=0; inumVerts; i++){ - cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r; + cpFloat dist = cpSplittingPlaneCompare(planes[i], circ->tc) - circ->r; if(dist > 0.0f){ return 0; } else if(dist > min) { @@ -333,7 +298,7 @@ circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) } } - cpVect n = axes[mini].n; + cpVect n = planes[mini].n; cpVect a = poly->tVerts[mini]; cpVect b = poly->tVerts[(mini + 1)%poly->numVerts]; cpFloat dta = cpvcross(n, a); @@ -357,11 +322,69 @@ circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) } } +// Submitted by LegoCyclon +static int +seg2seg(const cpShape* shape1, const cpShape* shape2, cpContact* con) +{ + cpSegmentShape* seg1 = (cpSegmentShape *)shape1; + cpSegmentShape* seg2 = (cpSegmentShape *)shape2; + + cpVect v1 = cpvsub(seg1->tb, seg1->ta); + cpVect v2 = cpvsub(seg2->tb, seg2->ta); + cpFloat v1lsq = cpvlengthsq(v1); + cpFloat v2lsq = cpvlengthsq(v2); + // project seg2 onto seg1 + cpVect p1a = cpvproject(cpvsub(seg2->ta, seg1->ta), v1); + cpVect p1b = cpvproject(cpvsub(seg2->tb, seg1->ta), v1); + // project seg1 onto seg2 + cpVect p2a = cpvproject(cpvsub(seg1->ta, seg2->ta), v2); + cpVect p2b = cpvproject(cpvsub(seg1->tb, seg2->ta), v2); + + // clamp projections to segment endcaps + if (cpvdot(p1a, v1) < 0.0f) + p1a = cpvzero; + else if (cpvdot(p1a, v1) > 0.0f && cpvlengthsq(p1a) > v1lsq) + p1a = v1; + if (cpvdot(p1b, v1) < 0.0f) + p1b = cpvzero; + else if (cpvdot(p1b, v1) > 0.0f && cpvlengthsq(p1b) > v1lsq) + p1b = v1; + if (cpvdot(p2a, v2) < 0.0f) + p2a = cpvzero; + else if (cpvdot(p2a, v2) > 0.0f && cpvlengthsq(p2a) > v2lsq) + p2a = v2; + if (cpvdot(p2b, v2) < 0.0f) + p2b = cpvzero; + else if (cpvdot(p2b, v2) > 0.0f && cpvlengthsq(p2b) > v2lsq) + p2b = v2; + + p1a = cpvadd(p1a, seg1->ta); + p1b = cpvadd(p1b, seg1->ta); + p2a = cpvadd(p2a, seg2->ta); + p2b = cpvadd(p2b, seg2->ta); + + int num = 0; + + if (!circle2circleQuery(p1a, p2a, seg1->r, seg2->r, nextContactPoint(con, &num))) + --num; + + if (!circle2circleQuery(p1b, p2b, seg1->r, seg2->r, nextContactPoint(con, &num))) + --num; + + if (!circle2circleQuery(p1a, p2b, seg1->r, seg2->r, nextContactPoint(con, &num))) + --num; + + if (!circle2circleQuery(p1b, p2a, seg1->r, seg2->r, nextContactPoint(con, &num))) + --num; + + return num; +} + static const collisionFunc builtinCollisionFuncs[9] = { circle2circle, NULL, NULL, - circle2segment, + (collisionFunc)circle2segment, NULL, NULL, circle2poly, @@ -370,36 +393,23 @@ static const collisionFunc builtinCollisionFuncs[9] = { }; static const collisionFunc *colfuncs = builtinCollisionFuncs; -//static collisionFunc *colfuncs = NULL; -// -//static void -//addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func) -//{ -// colfuncs[a + b*CP_NUM_SHAPES] = func; -//} -// -//#ifdef __cplusplus -//extern "C" { -//#endif -// void cpInitCollisionFuncs(void); -// -// // Initializes the array of collision functions. -// // Called by cpInitChipmunk(). -// void -// cpInitCollisionFuncs(void) -// { -// if(!colfuncs) -// colfuncs = (collisionFunc *)cpcalloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc)); -// -// addColFunc(CP_CIRCLE_SHAPE, CP_CIRCLE_SHAPE, circle2circle); -// addColFunc(CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, circle2segment); -// addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE, seg2poly); -// addColFunc(CP_CIRCLE_SHAPE, CP_POLY_SHAPE, circle2poly); -// addColFunc(CP_POLY_SHAPE, CP_POLY_SHAPE, poly2poly); -// } -//#ifdef __cplusplus -//} -//#endif +static const collisionFunc segmentCollisions[9] = { + circle2circle, + NULL, + NULL, + (collisionFunc)circle2segment, + seg2seg, + NULL, + circle2poly, + seg2poly, + poly2poly, +}; + +void +cpEnableSegmentToSegmentCollisions(void) +{ + colfuncs = segmentCollisions; +} int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr) diff --git a/src/eepp/helper/chipmunk/cpHashSet.c b/src/eepp/helper/chipmunk/cpHashSet.c index 550ac003c..0d68a0747 100644 --- a/src/eepp/helper/chipmunk/cpHashSet.c +++ b/src/eepp/helper/chipmunk/cpHashSet.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "prime.h" @@ -32,7 +29,7 @@ typedef struct cpHashSetBin { } cpHashSetBin; struct cpHashSet { - int entries, size; + unsigned int entries, size; cpHashSetEqlFunc eql; void *default_value; @@ -91,18 +88,18 @@ static void cpHashSetResize(cpHashSet *set) { // Get the next approximate doubled prime. - int newSize = next_prime(set->size + 1); + unsigned int newSize = next_prime(set->size + 1); // Allocate a new table. cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *)); // Iterate over the chains. - for(int i=0; isize; i++){ + for(unsigned int i=0; isize; i++){ // Rehash the bins into the new table. cpHashSetBin *bin = set->table[i]; while(bin){ cpHashSetBin *next = bin->next; - int idx = bin->hash%newSize; + cpHashValue idx = bin->hash%newSize; bin->next = newTable[idx]; newTable[idx] = bin; @@ -135,7 +132,7 @@ getUnusedBin(cpHashSet *set) } else { // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin); - cpAssertSoft(count, "Buffer size is too small."); + cpAssertHard(count, "Internal Error: Buffer size is too small."); cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(set->allocatedBuffers, buffer); @@ -155,7 +152,7 @@ cpHashSetCount(cpHashSet *set) void * cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans) { - int idx = hash%set->size; + cpHashValue idx = hash%set->size; // Find the bin with the matching element. cpHashSetBin *bin = set->table[idx]; @@ -181,7 +178,7 @@ cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashS void * cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) { - int idx = hash%set->size; + cpHashValue idx = hash%set->size; cpHashSetBin **prev_ptr = &set->table[idx]; cpHashSetBin *bin = set->table[idx]; @@ -210,7 +207,7 @@ cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) void * cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) { - int idx = hash%set->size; + cpHashValue idx = hash%set->size; cpHashSetBin *bin = set->table[idx]; while(bin && !set->eql(ptr, bin->elt)) bin = bin->next; @@ -221,7 +218,7 @@ cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) { - for(int i=0; isize; i++){ + for(unsigned int i=0; isize; i++){ cpHashSetBin *bin = set->table[i]; while(bin){ cpHashSetBin *next = bin->next; @@ -234,7 +231,7 @@ cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data) { - for(int i=0; isize; i++){ + for(unsigned int i=0; isize; i++){ // The rest works similarly to cpHashSetRemove() above. cpHashSetBin **prev_ptr = &set->table[i]; cpHashSetBin *bin = set->table[i]; diff --git a/src/eepp/helper/chipmunk/cpPolyShape.c b/src/eepp/helper/chipmunk/cpPolyShape.c index 4b1ce87fa..8250cbd51 100644 --- a/src/eepp/helper/chipmunk/cpPolyShape.c +++ b/src/eepp/helper/chipmunk/cpPolyShape.c @@ -19,8 +19,6 @@ * SOFTWARE. */ -#include - #include "chipmunk_private.h" #include "chipmunk_unsafe.h" @@ -55,8 +53,8 @@ cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) static void cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) { - cpPolyShapeAxis *src = poly->axes; - cpPolyShapeAxis *dst = poly->tAxes; + cpSplittingPlane *src = poly->planes; + cpSplittingPlane *dst = poly->tPlanes; for(int i=0; inumVerts; i++){ cpVect n = cpvrotate(src[i].n, rot); @@ -78,21 +76,44 @@ static void cpPolyShapeDestroy(cpPolyShape *poly) { cpfree(poly->verts); - cpfree(poly->tVerts); - - cpfree(poly->axes); - cpfree(poly->tAxes); + cpfree(poly->planes); } -static cpBool -cpPolyShapePointQuery(cpPolyShape *poly, cpVect p){ - return cpBBContainsVect(poly->shape.bb, p) && cpPolyShapeContainsVert(poly, p); +static void +cpPolyShapeNearestPointQuery(cpPolyShape *poly, cpVect p, cpNearestPointQueryInfo *info){ + int count = poly->numVerts; + cpSplittingPlane *planes = poly->tPlanes; + cpVect *verts = poly->tVerts; + + cpVect v0 = verts[count - 1]; + cpFloat minDist = INFINITY; + cpVect closestPoint = cpvzero; + cpBool outside = cpFalse; + + for(int i=0; i 0.0f) outside = cpTrue; + + cpVect v1 = verts[i]; + cpVect closest = cpClosetPointOnSegment(p, v0, v1); + + cpFloat dist = cpvdist(p, closest); + if(dist < minDist){ + minDist = dist; + closestPoint = closest; + } + + v0 = v1; + } + + info->shape = (cpShape *)poly; + info->p = closestPoint; // TODO div/0 + info->d = (outside ? minDist : -minDist); } static void cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info) { - cpPolyShapeAxis *axes = poly->tAxes; + cpSplittingPlane *axes = poly->tPlanes; cpVect *verts = poly->tVerts; int numVerts = poly->numVerts; @@ -122,7 +143,7 @@ static const cpShapeClass polyClass = { CP_POLY_SHAPE, (cpShapeCacheDataImpl)cpPolyShapeCacheData, (cpShapeDestroyImpl)cpPolyShapeDestroy, - (cpShapePointQueryImpl)cpPolyShapePointQuery, + (cpShapeNearestPointQueryImpl)cpPolyShapeNearestPointQuery, (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery, }; @@ -134,8 +155,9 @@ cpPolyValidate(const cpVect *verts, const int numVerts) cpVect b = verts[(i+1)%numVerts]; cpVect c = verts[(i+2)%numVerts]; - if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f) + if(cpvcross(cpvsub(b, a), cpvsub(c, a)) > 0.0f){ return cpFalse; + } } return cpTrue; @@ -159,14 +181,16 @@ cpPolyShapeGetVert(cpShape *shape, int idx) static void -setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset) +setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset) { + // Fail if the user attempts to pass a concave poly, or a bad winding. + cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL()."); + poly->numVerts = numVerts; - - poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect)); - poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect)); - poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis)); - poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis)); + poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect)); + poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane)); + poly->tVerts = poly->verts + numVerts; + poly->tPlanes = poly->planes + numVerts; for(int i=0; iverts[i] = a; - poly->axes[i].n = n; - poly->axes[i].d = cpvdot(n, a); + poly->planes[i].n = n; + poly->planes[i].d = cpvdot(n, a); } + } cpPolyShape * -cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset) +cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset) { - // Fail if the user attempts to pass a concave poly, or a bad winding. - cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding."); - setUpVerts(poly, numVerts, verts, offset); cpShapeInit((cpShape *)poly, &polyClass, body); diff --git a/src/eepp/helper/chipmunk/cpShape.c b/src/eepp/helper/chipmunk/cpShape.c index f57601a65..daaba77b2 100644 --- a/src/eepp/helper/chipmunk/cpShape.c +++ b/src/eepp/helper/chipmunk/cpShape.c @@ -19,10 +19,6 @@ * SOFTWARE. */ -#include -#include -#include - #include "chipmunk_private.h" #include "chipmunk_unsafe.h" @@ -61,6 +57,9 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) shape->layers = CP_ALL_LAYERS; shape->data = NULL; + + shape->space = NULL; + shape->next = NULL; shape->prev = NULL; @@ -85,7 +84,7 @@ cpShapeFree(cpShape *shape) void cpShapeSetBody(cpShape *shape, cpBody *body) { - cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape, then "); + cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body."); shape->body = body; } @@ -104,13 +103,35 @@ cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot) cpBool cpShapePointQuery(cpShape *shape, cpVect p){ - return shape->klass->pointQuery(shape, p); + cpNearestPointQueryInfo info = {NULL, cpvzero, INFINITY}; + cpShapeNearestPointQuery(shape, p, &info); + + return (info.d < 0.0f); } +cpFloat +cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpNearestPointQueryInfo *info) +{ + cpNearestPointQueryInfo blank = {NULL, cpvzero, INFINITY}; + if(info){ + (*info) = blank; + } else { + info = ␣ + } + + shape->klass->nearestPointQuery(shape, p, info); + return info->d; +} + + cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){ cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero}; - (*info) = blank; + if(info){ + (*info) = blank; + } else { + info = ␣ + } shape->klass->segmentQuery(shape, a, b, info); return (info->shape != NULL); @@ -126,13 +147,19 @@ 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); + return cpBBNewForCircle(c, circle->r); } -static cpBool -cpCircleShapePointQuery(cpCircleShape *circle, cpVect p){ - return cpvnear(circle->tc, p, circle->r); +static void +cpCicleShapeNearestPointQuery(cpCircleShape *circle, cpVect p, cpNearestPointQueryInfo *info) +{ + cpVect delta = cpvsub(p, circle->tc); + cpFloat d = cpvlength(delta); + cpFloat r = circle->r; + + info->shape = (cpShape *)circle; + info->p = cpvadd(circle->tc, cpvmult(delta, r/d)); // TODO div/0 + info->d = d - r; } static void @@ -168,7 +195,7 @@ static const cpShapeClass cpCircleShapeClass = { CP_CIRCLE_SHAPE, (cpShapeCacheDataImpl)cpCircleShapeCacheData, NULL, - (cpShapePointQueryImpl)cpCircleShapePointQuery, + (cpShapeNearestPointQueryImpl)cpCicleShapeNearestPointQuery, (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery, }; @@ -227,76 +254,46 @@ cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot) return cpBBNew(l - rad, b - rad, r + rad, t + rad); } -static cpBool -cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p){ - if(!cpBBContainsVect(seg->shape.bb, p)) return cpFalse; +static void +cpSegmentShapeNearestPointQuery(cpSegmentShape *seg, cpVect p, cpNearestPointQueryInfo *info) +{ + cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb); - // Calculate normal distance from segment. - cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); - cpFloat dist = cpfabs(dn) - seg->r; - if(dist > 0.0f) return cpFalse; + cpVect delta = cpvsub(p, closest); + cpFloat d = cpvlength(delta); + cpFloat r = seg->r; - // Calculate tangential distance along segment. - cpFloat dt = -cpvcross(seg->tn, p); - cpFloat dtMin = -cpvcross(seg->tn, seg->ta); - cpFloat dtMax = -cpvcross(seg->tn, seg->tb); - - // Decision tree to decide which feature of the segment to collide with. - if(dt <= dtMin){ - if(dt < (dtMin - seg->r)){ - return cpFalse; - } else { - return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); - } - } else { - if(dt < dtMax){ - return cpTrue; - } else { - if(dt < (dtMax + seg->r)) { - return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); - } else { - return cpFalse; - } - } - } - - return cpTrue; + info->shape = (cpShape *)seg; + info->p = (d ? cpvadd(closest, cpvmult(delta, r/d)) : closest); + info->d = d - r; } -static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);} - static void cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpSegmentQueryInfo *info) { cpVect n = seg->tn; - // flip n if a is behind the axis - if(cpvdot(a, n) < cpvdot(seg->ta, n)) - n = cpvneg(n); + cpFloat d = cpvdot(cpvsub(seg->ta, a), n); + cpFloat r = seg->r; - cpFloat an = cpvdot(a, n); - cpFloat bn = cpvdot(b, n); + cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n); + cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a); - if(an != bn){ - cpFloat d = cpvdot(seg->ta, n) + seg->r; - cpFloat t = (d - an)/(bn - an); + // Make the endpoints relative to 'a' and move them by the thickness of the segment. + cpVect seg_a = cpvadd(seg->ta, seg_offset); + cpVect seg_b = cpvadd(seg->tb, seg_offset); + cpVect delta = cpvsub(b, a); + + if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){ + cpFloat d_offset = d + (d > 0.0f ? -r : r); + cpFloat ad = -d_offset; + cpFloat bd = cpvdot(delta, n) - d_offset; - if(0.0f < t && t < 1.0f){ - cpVect point = cpvlerp(a, b, t); - cpFloat dt = -cpvcross(seg->tn, point); - cpFloat dtMin = -cpvcross(seg->tn, seg->ta); - cpFloat dtMax = -cpvcross(seg->tn, seg->tb); - - if(dtMin < dt && dt < dtMax){ - info->shape = (cpShape *)seg; - info->t = t; - info->n = n; - - return; // don't continue on and check endcaps - } + if(ad*bd < 0.0f){ + info->shape = (cpShape *)seg; + info->t = ad/(ad - bd); + info->n = flipped_n; } - } - - if(seg->r) { + } else if(r != 0.0f){ cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero}; cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero}; circleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, &info1); @@ -314,7 +311,7 @@ static const cpShapeClass cpSegmentShapeClass = { CP_SEGMENT_SHAPE, (cpShapeCacheDataImpl)cpSegmentShapeCacheData, NULL, - (cpShapePointQueryImpl)cpSegmentShapePointQuery, + (cpShapeNearestPointQueryImpl)cpSegmentShapeNearestPointQuery, (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery, }; @@ -327,6 +324,9 @@ cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloa seg->r = r; + seg->a_tangent = cpvzero; + seg->b_tangent = cpvzero; + cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body); return seg; @@ -343,6 +343,16 @@ CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B) CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal) CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius) +void +cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->a_tangent = cpvsub(prev, seg->a); + seg->b_tangent = cpvsub(next, seg->b); +} + // Unsafe API (chipmunk_unsafe.h) void diff --git a/src/eepp/helper/chipmunk/cpSpace.c b/src/eepp/helper/chipmunk/cpSpace.c index ee21e9f32..6501bca05 100644 --- a/src/eepp/helper/chipmunk/cpSpace.c +++ b/src/eepp/helper/chipmunk/cpSpace.c @@ -19,14 +19,12 @@ * SOFTWARE. */ -#include #include #include -#include #include "chipmunk_private.h" -#pragma mark Contact Set Helpers +//MARK: Contact Set Helpers // Equal function for arbiterSet. static cpBool @@ -38,7 +36,7 @@ arbiterSetEql(cpShape **shapes, cpArbiter *arb) return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); } -#pragma mark Collision Handler Set HelperFunctions +//MARK: Collision Handler Set HelperFunctions // Equals function for collisionHandlers. static cpBool @@ -57,7 +55,7 @@ handlerSetTrans(cpCollisionHandler *handler, void *unused) return copy; } -#pragma mark Misc Helper Funcs +//MARK: Misc Helper Funcs // Default collision functions. static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} @@ -68,7 +66,7 @@ static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;} static void freeWrap(void *ptr, void *unused){cpfree(ptr);} -#pragma mark Memory Management Functions +//MARK: Memory Management Functions cpSpace * cpSpaceAlloc(void) @@ -82,8 +80,12 @@ cpSpace* cpSpaceInit(cpSpace *space) { #ifndef NDEBUG - printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString); - //printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); + static cpBool done = cpFalse; + if(!done){ + printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString); + printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); + done = cpTrue; + } #endif space->iterations = 10; @@ -124,7 +126,8 @@ cpSpaceInit(cpSpace *space) space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql); cpHashSetSetDefaultValue(space->collisionHandlers, &cpDefaultCollisionHandler); - space->postStepCallbacks = NULL; + space->postStepCallbacks = cpArrayNew(0); + space->skipPostStep = cpFalse; cpBodyInitStatic(&space->_staticBody); space->staticBody = &space->_staticBody; @@ -141,6 +144,8 @@ cpSpaceNew(void) void cpSpaceDestroy(cpSpace *space) { + cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivate, NULL); + cpSpatialIndexFree(space->staticShapes); cpSpatialIndexFree(space->activeShapes); @@ -160,8 +165,10 @@ cpSpaceDestroy(cpSpace *space) cpArrayFree(space->allocatedBuffers); } - if(space->postStepCallbacks) cpHashSetEach(space->postStepCallbacks, freeWrap, NULL); - cpHashSetFree(space->postStepCallbacks); + if(space->postStepCallbacks){ + cpArrayFreeEach(space->postStepCallbacks, cpfree); + cpArrayFree(space->postStepCallbacks); + } if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, freeWrap, NULL); cpHashSetFree(space->collisionHandlers); @@ -182,7 +189,7 @@ cpSpaceFree(cpSpace *space) "Put these calls into a post-step callback." \ ); -#pragma mark Collision Handler Function Management +//MARK: Collision Handler Function Management void cpSpaceAddCollisionHandler( @@ -245,15 +252,14 @@ cpSpaceSetDefaultCollisionHandler( cpHashSetSetDefaultValue(space->collisionHandlers, &space->defaultHandler); } -#pragma mark Body, Shape, and Joint Management +//MARK: Body, Shape, and Joint Management cpShape * cpSpaceAddShape(cpSpace *space, cpShape *shape) { cpBody *body = shape->body; if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); - // TODO change these to check if it was added to a space at all. - cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another."); + cpAssertHard(!shape->space, "This shape is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpBodyActivate(body); @@ -269,7 +275,7 @@ cpSpaceAddShape(cpSpace *space, cpShape *shape) cpShape * cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) { - cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another."); + cpAssertHard(!shape->space, "This shape is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpBody *body = shape->body; @@ -284,8 +290,8 @@ cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { - cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); - cpAssertSoft(!body->space, "This body is already added to a space and cannot be added to another."); + cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); + cpAssertHard(!body->space, "This body is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); @@ -297,7 +303,7 @@ cpSpaceAddBody(cpSpace *space, cpBody *body) cpConstraint * cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) { - cpAssertSoft(!constraint->space, "This shape is already added to a space and cannot be added to another."); + cpAssertHard(!constraint->space, "This shape is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpBodyActivate(constraint->a); @@ -316,15 +322,28 @@ cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) struct arbiterFilterContext { cpSpace *space; cpBody *body; + cpShape *shape; }; static cpBool -contactSetFilterRemovedBody(cpArbiter *arb, struct arbiterFilterContext *context) +cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context) { + cpShape *shape = context->shape; cpBody *body = context->body; - if(body == arb->body_a || body == arb->body_b){ + + + // Match on the filter shape, or if it's NULL the filter body + if( + (body == arb->body_a && (shape == arb->a || shape == NULL)) || + (body == arb->body_b && (shape == arb->b || shape == NULL)) + ){ + // Call separate when removing shapes. + if(shape && arb->state != cpArbiterStateCached) cpArbiterCallSeparate(arb, context->space); + + cpArbiterUnthread(arb); cpArrayDeleteObj(context->space->arbiters, arb); cpArrayPush(context->space->pooledArbiters, arb); + return cpFalse; } @@ -334,25 +353,10 @@ contactSetFilterRemovedBody(cpArbiter *arb, struct arbiterFilterContext *context void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter) { - cpArbiter *arb = body->arbiterList; - while(arb){ - cpArbiter *next = cpArbiterNext(arb, body); - if(filter == NULL || filter == arb->a || filter == arb->b){ - if(arb->state != cpArbiterStateCached) cpArbiterCallSeparate(arb, space); - - cpArbiterUnthread(arb); - cpSpaceUncacheArbiter(space, arb); - cpArrayPush(space->pooledArbiters, arb); - } - arb = next; - } - - // TODO see note at cpSpaceArbiterSetFilter() - // When just removing the body, so we need to filter all cached arbiters to avoid dangling pointers. - if(filter == NULL){ - struct arbiterFilterContext context = {space, body}; - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)contactSetFilterRemovedBody, &context); - } + cpSpaceLock(space); { + struct arbiterFilterContext context = {space, body, filter}; + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context); + } cpSpaceUnlock(space, cpTrue); } void @@ -362,8 +366,7 @@ cpSpaceRemoveShape(cpSpace *space, cpShape *shape) if(cpBodyIsStatic(body)){ cpSpaceRemoveStaticShape(space, shape); } else { - cpAssertSoft(cpSpaceContainsShape(space, shape), - "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); + cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBodyActivate(body); @@ -377,12 +380,11 @@ cpSpaceRemoveShape(cpSpace *space, cpShape *shape) void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) { - cpAssertSoft(cpSpaceContainsShape(space, shape), - "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); + cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBody *body = shape->body; - cpBodyActivateStatic(body, shape); + if(cpBodyIsStatic(body)) cpBodyActivateStatic(body, shape); cpBodyRemoveShape(body, shape); cpSpaceFilterArbiters(space, body, shape); cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); @@ -392,12 +394,11 @@ cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) void cpSpaceRemoveBody(cpSpace *space, cpBody *body) { - cpAssertWarn(cpSpaceContainsBody(space, body), - "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); + cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBodyActivate(body); - cpSpaceFilterArbiters(space, body, NULL); +// cpSpaceFilterArbiters(space, body, NULL); cpArrayDeleteObj(space->bodies, body); body->space = NULL; } @@ -405,8 +406,7 @@ cpSpaceRemoveBody(cpSpace *space, cpBody *body) void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) { - cpAssertWarn(cpSpaceContainsConstraint(space, constraint), - "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); + cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBodyActivate(constraint->a); @@ -434,7 +434,7 @@ cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint) } -#pragma mark Iteration +//MARK: Iteration void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data) @@ -449,7 +449,13 @@ cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *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); + + cpBody *body = root; + while(body){ + cpBody *next = body->node.next; + func(body, data); + body = next; + } } } cpSpaceUnlock(space, cpTrue); } @@ -487,7 +493,7 @@ cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void * } cpSpaceUnlock(space, cpTrue); } -#pragma mark Spatial Index Management +//MARK: Spatial Index Management static void updateBBCache(cpShape *shape, void *unused) @@ -499,6 +505,8 @@ updateBBCache(cpShape *shape, void *unused) void cpSpaceReindexStatic(cpSpace *space) { + cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&updateBBCache, NULL); cpSpatialIndexReindex(space->staticShapes); } @@ -506,6 +514,8 @@ cpSpaceReindexStatic(cpSpace *space) void cpSpaceReindexShape(cpSpace *space, cpShape *shape) { + cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); + cpBody *body = shape->body; cpShapeUpdate(shape, body->p, body->rot); diff --git a/src/eepp/helper/chipmunk/cpSpaceComponent.c b/src/eepp/helper/chipmunk/cpSpaceComponent.c index a0e215c87..612ada0ce 100644 --- a/src/eepp/helper/chipmunk/cpSpaceComponent.c +++ b/src/eepp/helper/chipmunk/cpSpaceComponent.c @@ -19,18 +19,16 @@ * SOFTWARE. */ -#include -#include #include #include "chipmunk_private.h" -#pragma mark Sleeping Functions +//MARK: Sleeping Functions void cpSpaceActivateBody(cpSpace *space, cpBody *body) { - cpAssertSoft(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rouge body."); + cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rouge body."); if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked @@ -57,7 +55,7 @@ cpSpaceActivateBody(cpSpace *space, cpBody *body) // Reinsert the arbiter into the arbiter cache cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); + cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); // Update the arbiter's state @@ -79,7 +77,7 @@ cpSpaceActivateBody(cpSpace *space, cpBody *body) static void cpSpaceDeactivateBody(cpSpace *space, cpBody *body) { - cpAssertSoft(!cpBodyIsRogue(body), "Internal error: Attempting to deactivate a rouge body."); + cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to deactivate a rouge body."); cpArrayDeleteObj(space->bodies, body); @@ -117,13 +115,14 @@ static inline void ComponentActivate(cpBody *root) { if(!root || !cpBodyIsSleeping(root)) return; - cpAssertSoft(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body."); + cpAssertHard(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body."); cpSpace *space = root->space; cpBody *body = root; while(body){ cpBody *next = body->node.next; + body->node.idleTime = 0.0f; body->node.root = NULL; body->node.next = NULL; cpSpaceActivateBody(space, body); @@ -134,16 +133,6 @@ ComponentActivate(cpBody *root) cpArrayDeleteObj(space->sleepingComponents, root); } -static inline void -ComponentAdd(cpBody *root, cpBody *body){ - body->node.root = root; - - if(body != root){ - body->node.next = root->node.next; - root->node.next = body; - } -} - void cpBodyActivate(cpBody *body) { @@ -163,34 +152,10 @@ cpBodyActivateStatic(cpBody *body, cpShape *filter) cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a); } } -} - -static inline cpBool -ComponentActive(cpBody *root, cpFloat threshold) -{ - CP_BODY_FOREACH_COMPONENT(root, body){ - if(body->node.idleTime < threshold) return cpTrue; - } - return cpFalse; + // TODO should also activate joints? } -static inline void -FloodFillComponent(cpBody *root, cpBody *body) -{ - if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){ - 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 { - cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph."); - } - } -} - - static inline void cpBodyPushArbiter(cpBody *body, cpArbiter *arb) { @@ -205,24 +170,72 @@ cpBodyPushArbiter(cpBody *body, cpArbiter *arb) body->arbiterList = arb; } +static inline void +ComponentAdd(cpBody *root, cpBody *body){ + body->node.root = root; + + if(body != root){ + body->node.next = root->node.next; + root->node.next = body; + } +} + +static inline void +FloodFillComponent(cpBody *root, cpBody *body) +{ + // Rogue bodies cannot be put to sleep and prevent bodies they are touching from sleepining anyway. + // Static bodies (which are a type of rogue body) are effectively sleeping all the time. + if(!cpBodyIsRogue(body)){ + 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 { + cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph."); + } + } +} + +static inline cpBool +ComponentActive(cpBody *root, cpFloat threshold) +{ + CP_BODY_FOREACH_COMPONENT(root, body){ + if(body->node.idleTime < threshold) return cpTrue; + } + + return cpFalse; +} + void cpSpaceProcessComponents(cpSpace *space, cpFloat dt) { - cpFloat dv = space->idleSpeedThreshold; - cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); - - // update idling and reset component nodes + cpBool sleep = (space->sleepTimeThreshold != INFINITY); cpArray *bodies = space->bodies; + +#ifndef NDEBUG for(int i=0; inum; i++){ cpBody *body = (cpBody*)bodies->arr[i]; - // Need to deal with infinite mass objects - cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); - body->node.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->node.idleTime + dt); - cpAssertSoft(body->node.next == NULL, "Internal Error: Dangling next pointer detected in contact graph."); cpAssertSoft(body->node.root == NULL, "Internal Error: Dangling root pointer detected in contact graph."); } +#endif + + // Calculate the kinetic energy of all the bodies. + if(sleep){ + cpFloat dv = space->idleSpeedThreshold; + cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); + + // update idling and reset component nodes + for(int i=0; inum; i++){ + cpBody *body = (cpBody*)bodies->arr[i]; + + // Need to deal with infinite mass objects + cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); + body->node.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->node.idleTime + dt); + } + } // Awaken any sleeping bodies found and then push arbiters to the bodies' lists. cpArray *arbiters = space->arbiters; @@ -230,48 +243,52 @@ cpSpaceProcessComponents(cpSpace *space, cpFloat dt) cpArbiter *arb = (cpArbiter*)arbiters->arr[i]; cpBody *a = arb->body_a, *b = arb->body_b; - if((cpBodyIsRogue(b) && !cpBodyIsStatic(b)) || cpBodyIsSleeping(a)) cpBodyActivate(a); - if((cpBodyIsRogue(a) && !cpBodyIsStatic(a)) || cpBodyIsSleeping(b)) cpBodyActivate(b); + if(sleep){ + 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 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 = (cpConstraint *)constraints->arr[i]; - cpBody *a = constraint->a, *b = constraint->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(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); + if(sleep){ + // 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 = (cpConstraint *)constraints->arr[i]; + cpBody *a = constraint->a, *b = constraint->b; - // 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); - - // cpSpaceDeactivateBody() removed the current body from the list. - // Skip incrementing the index counter. - continue; - } + if(cpBodyIsRogue(b) && !cpBodyIsStatic(b)) cpBodyActivate(a); + if(cpBodyIsRogue(a) && !cpBodyIsStatic(a)) cpBodyActivate(b); } - i++; - - // Only sleeping bodies retain their component node pointers. - body->node.root = NULL; - body->node.next = NULL; + // Generate components and deactivate sleeping ones + for(int i=0; inum;){ + cpBody *body = (cpBody*)bodies->arr[i]; + + 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); + + // cpSpaceDeactivateBody() removed the current body from the list. + // Skip incrementing the index counter. + continue; + } + } + + i++; + + // Only sleeping bodies retain their component node pointers. + body->node.root = NULL; + body->node.next = NULL; + } } } diff --git a/src/eepp/helper/chipmunk/cpSpaceHash.c b/src/eepp/helper/chipmunk/cpSpaceHash.c index 193903e57..7479e9d3b 100644 --- a/src/eepp/helper/chipmunk/cpSpaceHash.c +++ b/src/eepp/helper/chipmunk/cpSpaceHash.c @@ -19,9 +19,6 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" #include "prime.h" @@ -45,7 +42,7 @@ struct cpSpaceHash { }; -#pragma mark Handle Functions +//MARK: Handle Functions struct cpHandle { void *obj; @@ -80,7 +77,7 @@ handleSetTrans(void *obj, cpSpaceHash *hash) if(hash->pooledHandles->num == 0){ // handle pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpHandle); - cpAssertSoft(count, "Buffer size is too small."); + cpAssertHard(count, "Internal Error: Buffer size is too small."); cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(hash->allocatedBuffers, buffer); @@ -94,7 +91,7 @@ handleSetTrans(void *obj, cpSpaceHash *hash) return hand; } -#pragma mark Bin Functions +//MARK: Bin Functions struct cpSpaceHashBin { cpHandle *handle; @@ -142,7 +139,7 @@ getEmptyBin(cpSpaceHash *hash) } else { // Pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); - cpAssertSoft(count, "Buffer size is too small."); + cpAssertHard(count, "Internal Error: Buffer size is too small."); cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(hash->allocatedBuffers, buffer); @@ -153,7 +150,7 @@ getEmptyBin(cpSpaceHash *hash) } } -#pragma mark Memory Management Functions +//MARK: Memory Management Functions cpSpaceHash * cpSpaceHashAlloc(void) @@ -212,7 +209,7 @@ cpSpaceHashDestroy(cpSpaceHash *hash) cpArrayFree(hash->pooledHandles); } -#pragma mark Helper Functions +//MARK: Helper Functions static inline cpBool containsHandle(cpSpaceHashBin *bin, cpHandle *hand) @@ -254,7 +251,7 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) 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); + cpHashValue idx = hash_func(i,j,n); cpSpaceHashBin *bin = hash->table[idx]; // Don't add an object twice to the same cell. @@ -270,7 +267,7 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) } } -#pragma mark Basic Operations +//MARK: Basic Operations static void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid) @@ -352,7 +349,7 @@ remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) } } -#pragma mark Query Functions +//MARK: Query Functions static inline void query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data) @@ -376,16 +373,6 @@ query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIn } } -static void -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, cpSpatialIndexQueryFunc func, void *data) { @@ -439,7 +426,7 @@ queryRehash_helper(cpHandle *hand, queryRehashContext *context) for(int i=l; i<=r; i++){ for(int j=b; j<=t; j++){ - int idx = hash_func(i,j,n); + cpHashValue idx = hash_func(i,j,n); cpSpaceHashBin *bin = table[idx]; if(containsHandle(bin, hand)) continue; @@ -497,7 +484,7 @@ segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSp } // modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html -void +static void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { a = cpvmult(a, 1.0f/hash->celldim); @@ -538,7 +525,7 @@ cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloa cpSpaceHashBin **table = hash->table; while(t < t_exit){ - int idx = hash_func(cell_x, cell_y, n); + cpHashValue idx = hash_func(cell_x, cell_y, n); t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data)); if (next_v < next_h){ @@ -555,7 +542,7 @@ cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloa hash->stamp++; } -#pragma mark Misc +//MARK: Misc void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) @@ -597,14 +584,13 @@ static cpSpatialIndexClass klass = { (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject, (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery, - (cpSpatialIndexPointQueryImpl)cpSpaceHashPointQuery, - (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, (cpSpatialIndexQueryImpl)cpSpaceHashQuery, + (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, }; static inline cpSpatialIndexClass *Klass(){return &klass;} -#pragma mark Debug Drawing +//MARK: Debug Drawing //#define CP_BBTREE_DEBUG_DRAW #ifdef CP_BBTREE_DEBUG_DRAW diff --git a/src/eepp/helper/chipmunk/cpSpaceQuery.c b/src/eepp/helper/chipmunk/cpSpaceQuery.c index cd7d14285..bf977dc28 100644 --- a/src/eepp/helper/chipmunk/cpSpaceQuery.c +++ b/src/eepp/helper/chipmunk/cpSpaceQuery.c @@ -19,25 +19,24 @@ * SOFTWARE. */ -#include - #include "chipmunk_private.h" -#pragma mark Point Query Functions +//MARK: Point Query Functions -typedef struct pointQueryContext { +struct PointQueryContext { + cpVect point; cpLayers layers; cpGroup group; cpSpacePointQueryFunc func; void *data; -} pointQueryContext; +}; static void -pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context) +PointQuery(struct PointQueryContext *context, cpShape *shape, void *data) { if( !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpShapePointQuery(shape, *point) + cpShapePointQuery(shape, context->point) ){ context->func(shape, context->data); } @@ -46,16 +45,17 @@ pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context) void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data) { - pointQueryContext context = {layers, group, func, data}; + struct PointQueryContext context = {point, layers, group, func, data}; + cpBB bb = cpBBNewForCircle(point, 0.0f); cpSpaceLock(space); { - cpSpatialIndexPointQuery(space->activeShapes, point, (cpSpatialIndexQueryFunc)pointQueryHelper, &context); - cpSpatialIndexPointQuery(space->staticShapes, point, (cpSpatialIndexQueryFunc)pointQueryHelper, &context); + cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data); } cpSpaceUnlock(space, cpTrue); } static void -rememberLastPointQuery(cpShape *shape, cpShape **outShape) +PointQueryFirst(cpShape *shape, cpShape **outShape) { if(!shape->sensor) *outShape = shape; } @@ -64,23 +64,94 @@ cpShape * cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) { cpShape *shape = NULL; - cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape); + cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)PointQueryFirst, &shape); return shape; } +//MARK: Nearest Point Query Functions -#pragma mark Segment Query Functions +struct NearestPointQueryContext { + cpVect point; + cpFloat maxDistance; + cpLayers layers; + cpGroup group; + cpSpaceNearestPointQueryFunc func; +}; -typedef struct segQueryContext { +static void +NearestPointQuery(struct NearestPointQueryContext *context, cpShape *shape, void *data) +{ + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) + ){ + cpNearestPointQueryInfo info; + cpShapeNearestPointQuery(shape, context->point, &info); + + if(info.shape && info.d < context->maxDistance) context->func(shape, info.d, info.p, data); + } +} + +void +cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data) +{ + struct NearestPointQueryContext context = {point, maxDistance, layers, group, func}; + cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); + } cpSpaceUnlock(space, cpTrue); +} + +static void +NearestPointQueryNearest(struct NearestPointQueryContext *context, cpShape *shape, cpNearestPointQueryInfo *out) +{ + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && !shape->sensor + ){ + cpNearestPointQueryInfo info; + cpShapeNearestPointQuery(shape, context->point, &info); + + if(info.d < out->d) (*out) = info; + } +} + +cpShape * +cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out) +{ + cpNearestPointQueryInfo info = {NULL, cpvzero, maxDistance}; + if(out){ + (*out) = info; + } else { + out = &info; + } + + struct NearestPointQueryContext context = { + point, maxDistance, + layers, group, + NULL + }; + + cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); + cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); + + return out->shape; +} + + +//MARK: Segment Query Functions + +struct SegmentQueryContext { cpVect start, end; cpLayers layers; cpGroup group; cpSpaceSegmentQueryFunc func; -} segQueryContext; +}; static cpFloat -segQueryFunc(segQueryContext *context, cpShape *shape, void *data) +SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data) { cpSegmentQueryInfo info; @@ -97,26 +168,20 @@ segQueryFunc(segQueryContext *context, cpShape *shape, void *data) void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data) { - segQueryContext context = { + struct SegmentQueryContext context = { start, end, layers, group, func, }; cpSpaceLock(space); { - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFunc, data); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFunc, data); + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); } cpSpaceUnlock(space, cpTrue); } -typedef struct segQueryFirstContext { - cpVect start, end; - cpLayers layers; - cpGroup group; -} segQueryFirstContext; - static cpFloat -segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out) +SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out) { cpSegmentQueryInfo info; @@ -126,7 +191,7 @@ segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo cpShapeSegmentQuery(shape, context->start, context->end, &info) && info.t < out->t ){ - *out = info; + (*out) = info; } return out->t; @@ -142,59 +207,60 @@ cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers laye out = &info; } - segQueryFirstContext context = { + struct SegmentQueryContext context = { start, end, - layers, group + layers, group, + NULL }; - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)segQueryFirst, out); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryFunc)segQueryFirst, out); + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); return out->shape; } -#pragma mark BB Query Functions +//MARK: BB Query Functions -typedef struct bbQueryContext { +struct BBQueryContext { + cpBB bb; cpLayers layers; cpGroup group; cpSpaceBBQueryFunc func; - void *data; -} bbQueryContext; +}; static void -bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context) +BBQuery(struct BBQueryContext *context, cpShape *shape, void *data) { if( !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpBBIntersects(*bb, shape->bb) + cpBBIntersects(context->bb, shape->bb) ){ - context->func(shape, context->data); + context->func(shape, data); } } void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data) { - bbQueryContext context = {layers, group, func, data}; + struct BBQueryContext context = {bb, layers, group, func}; cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, &bb, bb, (cpSpatialIndexQueryFunc)bbQueryHelper, &context); - cpSpatialIndexQuery(space->staticShapes, &bb, bb, (cpSpatialIndexQueryFunc)bbQueryHelper, &context); + cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); } cpSpaceUnlock(space, cpTrue); } -#pragma mark Shape Query Functions +//MARK: Shape Query Functions -typedef struct shapeQueryContext { +struct ShapeQueryContext { cpSpaceShapeQueryFunc func; void *data; cpBool anyCollision; -} shapeQueryContext; +}; // Callback from the spatial hash. static void -shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context) +ShapeQuery(cpShape *a, cpShape *b, struct ShapeQueryContext *context) { // Reject any of the simple cases if( @@ -218,10 +284,12 @@ shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context) context->anyCollision = !(a->sensor || b->sensor); if(context->func){ - cpContactPointSet set = {numContacts, {}}; + cpContactPointSet set; + set.count = numContacts; + for(int i=0; ibody; cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb); - shapeQueryContext context = {func, data, cpFalse}; + struct ShapeQueryContext context = {func, data, cpFalse}; cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)shapeQueryHelper, &context); - cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)shapeQueryHelper, &context); + cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); + cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); } cpSpaceUnlock(space, cpTrue); return context.anyCollision; diff --git a/src/eepp/helper/chipmunk/cpSpaceStep.c b/src/eepp/helper/chipmunk/cpSpaceStep.c index 59ed91083..b5cd20103 100644 --- a/src/eepp/helper/chipmunk/cpSpaceStep.c +++ b/src/eepp/helper/chipmunk/cpSpaceStep.c @@ -19,82 +19,45 @@ * SOFTWARE. */ -#include -#include -#include - #include "chipmunk_private.h" -#pragma mark Post Step Callback Functions +//MARK: Post Step Callback Functions -typedef struct cpPostStepCallback { - cpPostStepFunc func; - void *obj; - void *data; -} cpPostStepCallback; - -static cpBool -postStepFuncSetEql(cpPostStepCallback *a, cpPostStepCallback *b){ - return a->obj == b->obj; -} - -static void * -postStepFuncSetTrans(cpPostStepCallback *callback, void *ignored) +cpPostStepCallback * +cpSpaceGetPostStepCallback(cpSpace *space, void *key) { - cpPostStepCallback *value = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback)); - (*value) = (*callback); + cpArray *arr = space->postStepCallbacks; + for(int i=0; inum; i++){ + cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; + if(callback->key == key) return callback; + } - return value; + return NULL; } -void -cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data) +static void PostStepDoNothing(cpSpace *space, void *obj, void *data){} + +cpBool +cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, 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); - } - - 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; + if(!cpSpaceGetPostStepCallback(space, key)){ + cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback)); + callback->func = (func ? func : PostStepDoNothing); + callback->key = key; + callback->data = data; - cpHashSetEach(callbacks, (cpHashSetIteratorFunc)cpSpacePostStepCallbackSetIter, space); - cpHashSetFree(callbacks); + cpArrayPush(space->postStepCallbacks, callback); + return cpTrue; + } else { + return cpFalse; } } -#pragma mark Locking Functions +//MARK: Locking Functions void cpSpaceLock(cpSpace *space) @@ -106,21 +69,37 @@ void cpSpaceUnlock(cpSpace *space, cpBool runPostStep) { space->locked--; - cpAssertSoft(space->locked >= 0, "Internal Error: Space lock underflow."); + cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow."); - if(!space->locked){ + if(space->locked == 0 && runPostStep && !space->skipPostStep){ + space->skipPostStep = cpTrue; + cpArray *waking = space->rousedBodies; for(int i=0, count=waking->num; iarr[i]); + waking->arr[i] = NULL; + } + + cpArray *arr = space->postStepCallbacks; + for(int i=0; inum; i++){ + cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; + cpPostStepFunc func = callback->func; + + // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again. + callback->func = NULL; + if(func) func(space, callback->key, callback->data); + + arr->arr[i] = NULL; + cpfree(callback); } waking->num = 0; - - cpSpaceRunPostStepCallbacks(space); + arr->num = 0; + space->skipPostStep = cpFalse; } } -#pragma mark Contact Buffer Functions +//MARK: Contact Buffer Functions struct cpContactBufferHeader { cpTimestamp stamp; @@ -152,7 +131,7 @@ cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpCo return header; } -static void +void cpSpacePushFreshContactBuffer(cpSpace *space) { cpTimestamp stamp = space->stamp; @@ -189,7 +168,7 @@ cpContactBufferGetArray(cpSpace *space) void cpSpacePushContacts(cpSpace *space, int count) { - cpAssertSoft(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error:contact buffer overflow!"); + cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!"); space->contactBuffersHead->numContacts += count; } @@ -198,7 +177,7 @@ cpSpacePopContacts(cpSpace *space, int count){ space->contactBuffersHead->numContacts -= count; } -#pragma mark Collision Detection Functions +//MARK: Collision Detection Functions static void * cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) @@ -206,7 +185,7 @@ cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) if(space->pooledArbiters->num == 0){ // arbiter pool is exhausted, make more int count = CP_BUFFER_BYTES/sizeof(cpArbiter); - cpAssertSoft(count, "Buffer size too small."); + cpAssertHard(count, "Internal Error: Buffer size too small."); cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES); cpArrayPush(space->allocatedBuffers, buffer); @@ -229,12 +208,14 @@ queryReject(cpShape *a, cpShape *b) || (a->group && a->group == b->group) // Don't collide objects that don't share at least on layer. || !(a->layers & b->layers) + // Don't collide infinite mass objects + || (a->body->m == INFINITY && b->body->m == INFINITY) ); } // Callback from the spatial hash. -static void -collideShapes(cpShape *a, cpShape *b, cpSpace *space) +void +cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) { // Reject any of the simple cases if(queryReject(a,b)) return; @@ -260,7 +241,7 @@ collideShapes(cpShape *a, cpShape *b, cpSpace *space) // 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); + cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, space, (cpHashSetTransFunc)cpSpaceArbiterSetTrans); cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); @@ -294,7 +275,7 @@ collideShapes(cpShape *a, cpShape *b, cpSpace *space) } // Hashset filter func to throw away old arbiters. -static cpBool +cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) { cpTimestamp ticks = space->stamp - arb->stamp; @@ -303,6 +284,7 @@ cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) // TODO should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal. // Preserve arbiters on sensors and rejected arbiters for sleeping objects. + // This prevents errant separate callbacks from happenening. if( (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && (cpBodyIsStatic(b) || cpBodyIsSleeping(b)) @@ -312,8 +294,8 @@ cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) // Arbiter was used last frame, but not this one if(ticks >= 1 && arb->state != cpArbiterStateCached){ - cpArbiterCallSeparate(arb, space); arb->state = cpArbiterStateCached; + cpArbiterCallSeparate(arb, space); } if(ticks >= space->collisionPersistence){ @@ -327,9 +309,9 @@ cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) return cpTrue; } -#pragma mark All Important cpSpaceStep() Function +//MARK: All Important cpSpaceStep() Function -static void +void cpShapeUpdateFunc(cpShape *shape, void *unused) { cpBody *body = shape->body; @@ -339,13 +321,19 @@ cpShapeUpdateFunc(cpShape *shape, void *unused) void cpSpaceStep(cpSpace *space, cpFloat dt) { - if(dt == 0.0f) return; // don't step if the timestep is 0! + // don't step if the timestep is 0! + if(dt == 0.0f) return; + + space->stamp++; cpFloat prev_dt = space->curr_dt; space->curr_dt = dt; - // Reset and empty the arbiter list. + cpArray *bodies = space->bodies; + cpArray *constraints = space->constraints; cpArray *arbiters = space->arbiters; + + // Reset and empty the arbiter lists. for(int i=0; inum; i++){ cpArbiter *arb = (cpArbiter *)arbiters->arr[i]; arb->state = cpArbiterStateNormal; @@ -357,82 +345,87 @@ 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); { + // Integrate positions + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->position_func(body, dt); + } + + // Find colliding pairs. cpSpacePushFreshContactBuffer(space); cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); - cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)collideShapes, space); + cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); } cpSpaceUnlock(space, cpFalse); - // If body sleeping is enabled, do that now. - if(space->sleepTimeThreshold != INFINITY || space->enableContactGraph){ - cpSpaceProcessComponents(space, dt); - } + // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) + cpSpaceProcessComponents(space, dt); - // Clear out old cached arbiters and call separate callbacks - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); + cpSpaceLock(space); { + // Clear out old cached arbiters and call separate callbacks + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); - // Prestep the arbiters and constraints. - cpFloat slop = space->collisionSlop; - cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); - for(int i=0; inum; i++){ - 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); - } - - // Integrate velocities. - cpFloat damping = cpfpow(space->damping, dt); - cpVect gravity = space->gravity; - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, gravity, damping, dt); - } - - // Apply cached impulses - cpFloat dt_coef = (space->stamp ? dt/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. - for(int i=0; iiterations; i++){ - for(int j=0; jnum; j++){ - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]); + // Prestep the arbiters and constraints. + cpFloat slop = space->collisionSlop; + cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); + for(int i=0; inum; i++){ + cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef); } + + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint); + cpConstraintPreSolveFunc preSolve = constraint->preSolve; + if(preSolve) preSolve(constraint, space); + + constraint->klass->preStep(constraint, dt); } - } - // run the post-solve callbacks - cpSpaceLock(space); - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; + // Integrate velocities. + cpFloat damping = cpfpow(space->damping, dt); + cpVect gravity = space->gravity; + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->velocity_func(body, gravity, damping, dt); + } - cpCollisionHandler *handler = arb->handler; - handler->postSolve(arb, space, handler->data); - } - cpSpaceUnlock(space, cpTrue); - - // Increment the stamp. - space->stamp++; + // Apply cached impulses + cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt); + 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. + for(int i=0; iiterations; i++){ + for(int j=0; jnum; j++){ + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]); + } + + for(int j=0; jnum; j++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; + constraint->klass->applyImpulse(constraint, dt); + } + } + + // Run the constraint post-solve callbacks + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + + cpConstraintPostSolveFunc postSolve = constraint->postSolve; + if(postSolve) postSolve(constraint, space); + } + + // run the post-solve callbacks + for(int i=0; inum; i++){ + cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; + + cpCollisionHandler *handler = arb->handler; + handler->postSolve(arb, space, handler->data); + } + } cpSpaceUnlock(space, cpTrue); } diff --git a/src/eepp/helper/chipmunk/cpSpatialIndex.c b/src/eepp/helper/chipmunk/cpSpatialIndex.c index 6c39aad6c..e5c5c4291 100644 --- a/src/eepp/helper/chipmunk/cpSpatialIndex.c +++ b/src/eepp/helper/chipmunk/cpSpatialIndex.c @@ -1,4 +1,23 @@ -#include +/* 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 "chipmunk_private.h" @@ -19,7 +38,7 @@ cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialI index->staticIndex = staticIndex; if(staticIndex){ - cpAssertHard(!staticIndex->dynamicIndex, "This static index is already is already associated with a dynamic index."); + cpAssertHard(!staticIndex->dynamicIndex, "This static index is already associated with a dynamic index."); staticIndex->dynamicIndex = index; } @@ -42,7 +61,7 @@ dynamicToStaticIter(void *obj, dynamicToStaticContext *context) void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data) { - if(cpSpatialIndexCount(staticIndex) > 0){ + if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){ dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context); } diff --git a/src/eepp/helper/chipmunk/cpSweep1D.c b/src/eepp/helper/chipmunk/cpSweep1D.c index 7f7e01b29..f617bce02 100644 --- a/src/eepp/helper/chipmunk/cpSweep1D.c +++ b/src/eepp/helper/chipmunk/cpSweep1D.c @@ -19,14 +19,11 @@ * SOFTWARE. */ -#include -#include - #include "chipmunk_private.h" static inline cpSpatialIndexClass *Klass(); -#pragma mark Basic Structures +//MARK: Basic Structures typedef struct Bounds { cpFloat min, max; @@ -66,7 +63,7 @@ MakeTableCell(cpSweep1D *sweep, void *obj) return cell; } -#pragma mark Memory Management Functions +//MARK: Memory Management Functions cpSweep1D * cpSweep1DAlloc(void) @@ -105,7 +102,7 @@ cpSweep1DDestroy(cpSweep1D *sweep) sweep->table = NULL; } -#pragma mark Misc +//MARK: Misc static int cpSweep1DCount(cpSweep1D *sweep) @@ -131,7 +128,7 @@ cpSweep1DContains(cpSweep1D *sweep, void *obj, cpHashValue hashid) return cpFalse; } -#pragma mark Basic Operations +//MARK: Basic Operations static void cpSweep1DInsert(cpSweep1D *sweep, void *obj, cpHashValue hashid) @@ -158,7 +155,7 @@ cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid) } } -#pragma mark Reindexing Functions +//MARK: Reindexing Functions static void cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid) @@ -173,7 +170,7 @@ cpSweep1DReindex(cpSweep1D *sweep) // Could perform a sort, but queries are not accelerated anyway. } -#pragma mark Query Functions +//MARK: Query Functions static void cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) @@ -191,12 +188,6 @@ cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc fun } static void -cpSweep1DPointQuery(cpSweep1D *sweep, cpVect point, cpSpatialIndexQueryFunc func, void *data) -{ - cpSweep1DQuery(sweep, &point, cpBBNew(point.x, point.y, point.x, point.y), func, data); -} - -void cpSweep1DSegmentQuery(cpSweep1D *sweep, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) { cpBB bb = cpBBExpand(cpBBNew(a.x, a.y, a.x, a.y), b); @@ -209,7 +200,7 @@ cpSweep1DSegmentQuery(cpSweep1D *sweep, void *obj, cpVect a, cpVect b, cpFloat t } } -#pragma mark Reindex/Query +//MARK: Reindex/Query static int TableSort(TableCell *a, TableCell *b) @@ -255,9 +246,8 @@ static cpSpatialIndexClass klass = { (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject, (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery, - (cpSpatialIndexPointQueryImpl)cpSweep1DPointQuery, - (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, (cpSpatialIndexQueryImpl)cpSweep1DQuery, + (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, }; static inline cpSpatialIndexClass *Klass(){return &klass;} diff --git a/src/eepp/helper/chipmunk/cpVect.c b/src/eepp/helper/chipmunk/cpVect.c index 67ec21ece..ecd996049 100644 --- a/src/eepp/helper/chipmunk/cpVect.c +++ b/src/eepp/helper/chipmunk/cpVect.c @@ -20,46 +20,30 @@ */ #include -#include #include "chipmunk_private.h" -cpFloat -cpvlength(const cpVect v) -{ - return cpfsqrt(cpvdot(v, v)); -} - inline cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) { - cpFloat omega = cpfacos(cpvdot(v1, v2)); + cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); + cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); - if(omega){ + if(omega == 0.0){ + return v1; + } else { cpFloat denom = 1.0f/cpfsin(omega); return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); - } else { - return v1; } } cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a) { - cpFloat angle = cpfacos(cpvdot(v1, v2)); - return cpvslerp(v1, v2, cpfmin(a, angle)/angle); -} - -cpVect -cpvforangle(const cpFloat a) -{ - return cpv(cpfcos(a), cpfsin(a)); -} - -cpFloat -cpvtoangle(const cpVect v) -{ - return cpfatan2(v.y, v.x); + cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); + cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); + + return cpvslerp(v1, v2, cpfmin(a, omega)/omega); } char*