diff --git a/Chipmunk2D/include/chipmunk/chipmunk.h b/Chipmunk2D/include/chipmunk/chipmunk.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk.h @@ -0,0 +1,232 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_H +#define CHIPMUNK_H + +#include +#include + +#ifndef alloca + #ifdef _WIN32 + #include + #elif defined(__FreeBSD__) + /* already included in */ + #else + #include + #endif +#endif + +#ifdef _WIN32 + #define CP_EXPORT __declspec(dllexport) +#else + #define CP_EXPORT +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +CP_EXPORT void cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...); +#ifdef NDEBUG + #define cpAssertWarn(__condition__, ...) + #define cpAssertSoft(__condition__, ...) +#else + #define cpAssertSoft(__condition__, ...) if(!(__condition__)){cpMessage(#__condition__, __FILE__, __LINE__, 1, 0, __VA_ARGS__); abort();} + #define cpAssertWarn(__condition__, ...) if(!(__condition__)) cpMessage(#__condition__, __FILE__, __LINE__, 0, 0, __VA_ARGS__) +#endif + +// Hard assertions are used in situations where the program definitely will crash anyway, and the reason is inexpensive to detect. +#define cpAssertHard(__condition__, ...) if(!(__condition__)){cpMessage(#__condition__, __FILE__, __LINE__, 1, 1, __VA_ARGS__); abort();} + +#include "chipmunk_types.h" + +/// @defgroup misc Misc +/// @{ + +/// Allocated size for various Chipmunk buffers +#ifndef CP_BUFFER_BYTES + #define CP_BUFFER_BYTES (32*1024) +#endif + +#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 + +typedef struct cpArray cpArray; +typedef struct cpHashSet cpHashSet; + +typedef struct cpBody cpBody; + +typedef struct cpShape cpShape; +typedef struct cpCircleShape cpCircleShape; +typedef struct cpSegmentShape cpSegmentShape; +typedef struct cpPolyShape cpPolyShape; + +typedef struct cpConstraint cpConstraint; +typedef struct cpPinJoint cpPinJoint; +typedef struct cpSlideJoint cpSlideJoint; +typedef struct cpPivotJoint cpPivotJoint; +typedef struct cpGrooveJoint cpGrooveJoint; +typedef struct cpDampedSpring cpDampedSpring; +typedef struct cpDampedRotarySpring cpDampedRotarySpring; +typedef struct cpRotaryLimitJoint cpRotaryLimitJoint; +typedef struct cpRatchetJoint cpRatchetJoint; +typedef struct cpGearJoint cpGearJoint; +typedef struct cpSimpleMotorJoint cpSimpleMotorJoint; + +typedef struct cpCollisionHandler cpCollisionHandler; +typedef struct cpContactPointSet cpContactPointSet; +typedef struct cpArbiter cpArbiter; + +typedef struct cpSpace cpSpace; + +#include "cpVect.h" +#include "cpBB.h" +#include "cpTransform.h" +#include "cpSpatialIndex.h" + +#include "cpArbiter.h" + +#include "cpBody.h" +#include "cpShape.h" +#include "cpPolyShape.h" + +#include "cpConstraint.h" + +#include "cpSpace.h" + +// Chipmunk 7.0.2 +#define CP_VERSION_MAJOR 7 +#define CP_VERSION_MINOR 0 +#define CP_VERSION_RELEASE 2 + +/// Version string. +CP_EXPORT extern const char *cpVersionString; + +/// 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. +CP_EXPORT cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset); + +/// Calculate area of a hollow circle. +/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. +CP_EXPORT cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2); + +/// Calculate the moment of inertia for a line segment. +/// Beveling radius is not supported. +CP_EXPORT cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat radius); + +/// Calculate the area of a fattened (capsule shaped) line segment. +CP_EXPORT cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat radius); + +/// Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex. +CP_EXPORT cpFloat cpMomentForPoly(cpFloat m, int count, const cpVect *verts, cpVect offset, cpFloat radius); + +/// Calculate the signed area of a polygon. A Clockwise winding gives positive area. +/// This is probably backwards from what you expect, but matches Chipmunk's the winding for poly shapes. +CP_EXPORT cpFloat cpAreaForPoly(const int count, const cpVect *verts, cpFloat radius); + +/// Calculate the natural centroid of a polygon. +CP_EXPORT cpVect cpCentroidForPoly(const int count, const cpVect *verts); + +/// Calculate the moment of inertia for a solid box. +CP_EXPORT cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); + +/// Calculate the moment of inertia for a solid box. +CP_EXPORT 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 verts == @c result, then @c verts will be reduced inplace. +/// @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. +CP_EXPORT int cpConvexHull(int count, const cpVect *verts, cpVect *result, int *first, cpFloat tol); + +/// 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); \ + +/// Returns the closest point on the line segment ab, to the point p. +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)); +} + +#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 (^cpSpacePointQueryBlock)(cpShape *shape, cpVect point, cpFloat distance, cpVect gradient); +void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block); + +typedef void (^cpSpaceSegmentQueryBlock)(cpShape *shape, cpVect point, cpVect normal, cpFloat alpha); +void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block); + +typedef void (^cpSpaceBBQueryBlock)(cpShape *shape); +void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block); + +typedef void (^cpSpaceShapeQueryBlock)(cpShape *shape, cpContactPointSet *points); +cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block); + +#endif +#endif + + +//@} + +#ifdef __cplusplus +} + +static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);} +static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);} +static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);} +static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);} +static inline cpVect operator -(const cpVect v){return cpvneg(v);} + +#endif +#endif diff --git a/Chipmunk2D/include/chipmunk/chipmunk_ffi.h b/Chipmunk2D/include/chipmunk/chipmunk_ffi.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk_ffi.h @@ -0,0 +1,105 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifdef CHIPMUNK_FFI + +// Create non static inlined copies of Chipmunk functions, useful for working with dynamic FFIs +// For many languages, it may be faster to reimplement these functions natively instead. +// Note: This file should only be included by chipmunk.c. + +#ifdef _MSC_VER + #if _MSC_VER >= 1600 + #define MAKE_REF(name) CP_EXPORT decltype(name) *_##name = name + #else + #define MAKE_REF(name) + #endif +#else + #define MAKE_REF(name) __typeof__(name) *_##name = name +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() +MAKE_REF(cpveql); +MAKE_REF(cpvadd); +MAKE_REF(cpvneg); +MAKE_REF(cpvsub); +MAKE_REF(cpvmult); +MAKE_REF(cpvdot); +MAKE_REF(cpvcross); +MAKE_REF(cpvperp); +MAKE_REF(cpvrperp); +MAKE_REF(cpvproject); +MAKE_REF(cpvforangle); +MAKE_REF(cpvtoangle); +MAKE_REF(cpvrotate); +MAKE_REF(cpvunrotate); +MAKE_REF(cpvlengthsq); +MAKE_REF(cpvlength); +MAKE_REF(cpvlerp); +MAKE_REF(cpvnormalize); +MAKE_REF(cpvclamp); +MAKE_REF(cpvlerpconst); +MAKE_REF(cpvdist); +MAKE_REF(cpvdistsq); +MAKE_REF(cpvnear); + +MAKE_REF(cpfmax); +MAKE_REF(cpfmin); +MAKE_REF(cpfabs); +MAKE_REF(cpfclamp); +MAKE_REF(cpflerp); +MAKE_REF(cpflerpconst); + +MAKE_REF(cpBBNew); +MAKE_REF(cpBBNewForExtents); +MAKE_REF(cpBBNewForCircle); +MAKE_REF(cpBBIntersects); +MAKE_REF(cpBBContainsBB); +MAKE_REF(cpBBContainsVect); +MAKE_REF(cpBBMerge); +MAKE_REF(cpBBExpand); +MAKE_REF(cpBBCenter); +MAKE_REF(cpBBArea); +MAKE_REF(cpBBMergedArea); +MAKE_REF(cpBBSegmentQuery); +MAKE_REF(cpBBIntersectsSegment); +MAKE_REF(cpBBClampVect); + +MAKE_REF(cpSpatialIndexDestroy); +MAKE_REF(cpSpatialIndexCount); +MAKE_REF(cpSpatialIndexEach); +MAKE_REF(cpSpatialIndexContains); +MAKE_REF(cpSpatialIndexInsert); +MAKE_REF(cpSpatialIndexRemove); +MAKE_REF(cpSpatialIndexReindex); +MAKE_REF(cpSpatialIndexReindexObject); +MAKE_REF(cpSpatialIndexSegmentQuery); +MAKE_REF(cpSpatialIndexQuery); +MAKE_REF(cpSpatialIndexReindexQuery); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Chipmunk2D/include/chipmunk/chipmunk_private.h b/Chipmunk2D/include/chipmunk/chipmunk_private.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk_private.h @@ -0,0 +1,344 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_PRIVATE_H +#define CHIPMUNK_PRIVATE_H + +#include "chipmunk.h" +#include "chipmunk_structs.h" + +#define CP_HASH_COEF (3344921057ul) +#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) + +// TODO: Eww. Magic numbers. +#define MAGIC_EPSILON 1e-5 + + +//MARK: cpArray + +cpArray *cpArrayNew(int size); + +void cpArrayFree(cpArray *arr); + +void cpArrayPush(cpArray *arr, void *object); +void *cpArrayPop(cpArray *arr); +void cpArrayDeleteObj(cpArray *arr, void *obj); +cpBool cpArrayContains(cpArray *arr, void *ptr); + +void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); + + +//MARK: cpHashSet + +typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); +typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); + +cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc); +void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value); + +void cpHashSetFree(cpHashSet *set); + +int cpHashSetCount(cpHashSet *set); +void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, cpHashSetTransFunc trans, void *data); +void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); +void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); + +typedef void (*cpHashSetIteratorFunc)(void *elt, void *data); +void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data); + +typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); +void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); + + +//MARK: Bodies + +void cpBodyAddShape(cpBody *body, cpShape *shape); +void cpBodyRemoveShape(cpBody *body, cpShape *shape); + +//void cpBodyAccumulateMassForShape(cpBody *body, cpShape *shape); +void cpBodyAccumulateMassFromShapes(cpBody *body); + +void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint); + + +//MARK: Spatial Index Functions + +cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + + +//MARK: Arbiters + +cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); + +static inline struct cpArbiterThread * +cpArbiterThreadForBody(cpArbiter *arb, cpBody *body) +{ + return (arb->body_a == body ? &arb->thread_a : &arb->thread_b); +} + +void cpArbiterUnthread(cpArbiter *arb); + +void cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space); +void cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat bias, cpFloat slop); +void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef); +void cpArbiterApplyImpulse(cpArbiter *arb); + + +//MARK: Shapes/Collisions + +cpShape *cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo); + +static inline cpBool +cpShapeActive(cpShape *shape) +{ + // checks if the shape is added to a shape list. + // TODO could this just check the space now? + return (shape->prev || (shape->body && shape->body->shapeList == shape)); +} + +// Note: This function returns contact points with r1/r2 in absolute coordinates, not body relative. +struct cpCollisionInfo cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts); + +static inline void +CircleSegmentQuery(cpShape *shape, cpVect center, cpFloat r1, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) +{ + cpVect da = cpvsub(a, center); + cpVect db = cpvsub(b, center); + cpFloat rsum = r1 + r2; + + cpFloat qa = cpvdot(da, da) - 2.0f*cpvdot(da, db) + cpvdot(db, db); + cpFloat qb = cpvdot(da, db) - cpvdot(da, da); + cpFloat det = qb*qb - qa*(cpvdot(da, da) - rsum*rsum); + + if(det >= 0.0f){ + cpFloat t = (-qb - cpfsqrt(det))/(qa); + if(0.0f<= t && t <= 1.0f){ + cpVect n = cpvnormalize(cpvlerp(da, db, t)); + + info->shape = shape; + info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2)); + info->normal = n; + info->alpha = t; + } + } +} + +static inline cpBool +cpShapeFilterReject(cpShapeFilter a, cpShapeFilter b) +{ + // Reject the collision if: + return ( + // They are in the same non-zero group. + (a.group != 0 && a.group == b.group) || + // One of the category/mask combinations fails. + (a.categories & b.mask) == 0 || + (b.categories & a.mask) == 0 + ); +} + +void cpLoopIndexes(const cpVect *verts, int count, int *start, int *end); + + +//MARK: Constraints +// TODO naming conventions here + +void cpConstraintInit(cpConstraint *constraint, const struct cpConstraintClass *klass, cpBody *a, cpBody *b); + +static inline void +cpConstraintActivateBodies(cpConstraint *constraint) +{ + cpBody *a = constraint->a; cpBodyActivate(a); + cpBody *b = constraint->b; cpBodyActivate(b); +} + +static inline cpVect +relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ + cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + + return cpvsub(v2_sum, v1_sum); +} + +static inline cpFloat +normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ + return cpvdot(relative_velocity(a, b, r1, r2), n); +} + +static inline void +apply_impulse(cpBody *body, cpVect j, cpVect r){ + body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); + body->w += body->i_inv*cpvcross(r, j); +} + +static inline void +apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + apply_impulse(a, cpvneg(j), r1); + apply_impulse(b, j, r2); +} + +static inline void +apply_bias_impulse(cpBody *body, cpVect j, cpVect r) +{ + body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); + body->w_bias += body->i_inv*cpvcross(r, j); +} + +static inline void +apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + apply_bias_impulse(a, cpvneg(j), r1); + 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 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 cpMat2x2 +k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2) +{ + cpFloat m_sum = a->m_inv + b->m_inv; + + // 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; + cpFloat r1xsq = r1.x * r1.x * a_i_inv; + cpFloat r1ysq = r1.y * r1.y * a_i_inv; + cpFloat r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + cpFloat b_i_inv = b->i_inv; + cpFloat r2xsq = r2.x * r2.x * b_i_inv; + cpFloat r2ysq = r2.y * r2.y * b_i_inv; + cpFloat r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + cpFloat det = k11*k22 - k12*k21; + cpAssertSoft(det != 0.0, "Unsolvable constraint."); + + cpFloat det_inv = 1.0f/det; + return cpMat2x2New( + k22*det_inv, -k12*det_inv, + -k21*det_inv, k11*det_inv + ); +} + +static inline cpFloat +bias_coef(cpFloat errorBias, cpFloat dt) +{ + return 1.0f - cpfpow(errorBias, dt); +} + + +//MARK: Spaces + +#define cpAssertSpaceUnlocked(space) \ + cpAssertHard(!space->locked, \ + "This operation cannot be done safely during a call to cpSpaceStep() or during a query. " \ + "Put these calls into a post-step callback." \ + ); + +void cpSpaceSetStaticBody(cpSpace *space, cpBody *body); + +extern cpCollisionHandler cpCollisionHandlerDoNothing; + +void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); + +void cpSpacePushFreshContactBuffer(cpSpace *space); +struct cpContact *cpContactBufferGetArray(cpSpace *space); +void cpSpacePushContacts(cpSpace *space, int count); + +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); +void cpSpaceLock(cpSpace *space); +void cpSpaceUnlock(cpSpace *space, cpBool runPostStep); + +static inline void +cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb) +{ + const cpShape *a = arb->a, *b = arb->b; + const cpShape *shape_pair[] = {a, b}; + cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); + cpHashSetRemove(space->cachedArbiters, arbHashID, shape_pair); + cpArrayDeleteObj(space->arbiters, arb); +} + +static inline cpArray * +cpSpaceArrayForBodyType(cpSpace *space, cpBodyType type) +{ + return (type == CP_BODY_TYPE_STATIC ? space->staticBodies : space->dynamicBodies); +} + +void cpShapeUpdateFunc(cpShape *shape, void *unused); +cpCollisionID cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space); + + +//MARK: Foreach loops + +static inline cpConstraint * +cpConstraintNext(cpConstraint *node, cpBody *body) +{ + return (node->a == body ? node->next_a : node->next_b); +} + +#define CP_BODY_FOREACH_CONSTRAINT(bdy, var)\ + for(cpConstraint *var = bdy->constraintList; var; var = cpConstraintNext(var, bdy)) + +static inline cpArbiter * +cpArbiterNext(cpArbiter *node, cpBody *body) +{ + return (node->body_a == body ? node->thread_a.next : node->thread_b.next); +} + +#define CP_BODY_FOREACH_ARBITER(bdy, var)\ + for(cpArbiter *var = bdy->arbiterList; var; var = cpArbiterNext(var, bdy)) + +#define CP_BODY_FOREACH_SHAPE(body, var)\ + for(cpShape *var = body->shapeList; var; var = var->next) + +#define CP_BODY_FOREACH_COMPONENT(root, var)\ + for(cpBody *var = root; var; var = var->sleeping.next) + +#endif diff --git a/Chipmunk2D/include/chipmunk/chipmunk_structs.h b/Chipmunk2D/include/chipmunk/chipmunk_structs.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk_structs.h @@ -0,0 +1,450 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +// All of the struct definitions for Chipmunk should be considered part of the private API. +// However, it is very valuable to know the struct sizes for preallocating memory. + +#ifndef CHIPMUNK_STRUCTS_H +#define CHIPMUNK_STRUCTS_H + +#include "chipmunk.h" + +struct cpArray { + int num, max; + void **arr; +}; + +struct cpBody { + // Integration functions + cpBodyVelocityFunc velocity_func; + cpBodyPositionFunc position_func; + + // mass and it's inverse + cpFloat m; + cpFloat m_inv; + + // moment of inertia and it's inverse + cpFloat i; + cpFloat i_inv; + + // center of gravity + cpVect cog; + + // position, velocity, force + cpVect p; + cpVect v; + cpVect f; + + // Angle, angular velocity, torque (radians) + cpFloat a; + cpFloat w; + cpFloat t; + + cpTransform transform; + + cpDataPointer userData; + + // "pseudo-velocities" used for eliminating overlap. + // Erin Catto has some papers that talk about what these are. + cpVect v_bias; + cpFloat w_bias; + + cpSpace *space; + + cpShape *shapeList; + cpArbiter *arbiterList; + cpConstraint *constraintList; + + struct { + cpBody *root; + cpBody *next; + cpFloat idleTime; + } sleeping; +}; + +enum cpArbiterState { + // Arbiter is active and its the first collision. + CP_ARBITER_STATE_FIRST_COLLISION, + // Arbiter is active and its not the first collision. + CP_ARBITER_STATE_NORMAL, + // Collision has been explicitly ignored. + // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). + CP_ARBITER_STATE_IGNORE, + // Collison is no longer active. A space will cache an arbiter for up to cpSpace.collisionPersistence more steps. + CP_ARBITER_STATE_CACHED, + // Collison arbiter is invalid because one of the shapes was removed. + CP_ARBITER_STATE_INVALIDATED, +}; + +struct cpArbiterThread { + struct cpArbiter *next, *prev; +}; + +struct cpContact { + cpVect r1, r2; + + cpFloat nMass, tMass; + cpFloat bounce; // TODO: look for an alternate bounce solution. + + cpFloat jnAcc, jtAcc, jBias; + cpFloat bias; + + cpHashValue hash; +}; + +struct cpCollisionInfo { + const cpShape *a, *b; + cpCollisionID id; + + cpVect n; + + int count; + // TODO Should this be a unique struct type? + struct cpContact *arr; +}; + +struct cpArbiter { + cpFloat e; + cpFloat u; + cpVect surface_vr; + + cpDataPointer data; + + const cpShape *a, *b; + cpBody *body_a, *body_b; + struct cpArbiterThread thread_a, thread_b; + + int count; + struct cpContact *contacts; + cpVect n; + + // Regular, wildcard A and wildcard B collision handlers. + cpCollisionHandler *handler, *handlerA, *handlerB; + cpBool swapped; + + cpTimestamp stamp; + enum cpArbiterState state; +}; + +struct cpShapeMassInfo { + cpFloat m; + cpFloat i; + cpVect cog; + cpFloat area; +}; + +typedef enum cpShapeType{ + CP_CIRCLE_SHAPE, + CP_SEGMENT_SHAPE, + CP_POLY_SHAPE, + CP_NUM_SHAPES +} cpShapeType; + +typedef cpBB (*cpShapeCacheDataImpl)(cpShape *shape, cpTransform transform); +typedef void (*cpShapeDestroyImpl)(cpShape *shape); +typedef void (*cpShapePointQueryImpl)(const cpShape *shape, cpVect p, cpPointQueryInfo *info); +typedef void (*cpShapeSegmentQueryImpl)(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info); + +typedef struct cpShapeClass cpShapeClass; + +struct cpShapeClass { + cpShapeType type; + + cpShapeCacheDataImpl cacheData; + cpShapeDestroyImpl destroy; + cpShapePointQueryImpl pointQuery; + cpShapeSegmentQueryImpl segmentQuery; +}; + +struct cpShape { + const cpShapeClass *klass; + + cpSpace *space; + cpBody *body; + struct cpShapeMassInfo massInfo; + cpBB bb; + + cpBool sensor; + + cpFloat e; + cpFloat u; + cpVect surfaceV; + + cpDataPointer userData; + + cpCollisionType type; + cpShapeFilter filter; + + cpShape *next; + cpShape *prev; + + cpHashValue hashid; +}; + +struct cpCircleShape { + cpShape shape; + + cpVect c, tc; + cpFloat r; +}; + +struct cpSegmentShape { + cpShape shape; + + cpVect a, b, n; + cpVect ta, tb, tn; + cpFloat r; + + cpVect a_tangent, b_tangent; +}; + +struct cpSplittingPlane { + cpVect v0, n; +}; + +#define CP_POLY_SHAPE_INLINE_ALLOC 6 + +struct cpPolyShape { + cpShape shape; + + cpFloat r; + + int count; + // The untransformed planes are appended at the end of the transformed planes. + struct cpSplittingPlane *planes; + + // Allocate a small number of splitting planes internally for simple poly. + struct cpSplittingPlane _planes[2*CP_POLY_SHAPE_INLINE_ALLOC]; +}; + +typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); +typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); +typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint, cpFloat dt); +typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); + +typedef struct cpConstraintClass { + cpConstraintPreStepImpl preStep; + cpConstraintApplyCachedImpulseImpl applyCachedImpulse; + cpConstraintApplyImpulseImpl applyImpulse; + cpConstraintGetImpulseImpl getImpulse; +} cpConstraintClass; + +struct cpConstraint { + const cpConstraintClass *klass; + + cpSpace *space; + + cpBody *a, *b; + cpConstraint *next_a, *next_b; + + cpFloat maxForce; + cpFloat errorBias; + cpFloat maxBias; + + cpBool collideBodies; + + cpConstraintPreSolveFunc preSolve; + cpConstraintPostSolveFunc postSolve; + + cpDataPointer userData; +}; + +struct cpPinJoint { + cpConstraint constraint; + cpVect anchorA, anchorB; + cpFloat dist; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc; + cpFloat bias; +}; + +struct cpSlideJoint { + cpConstraint constraint; + cpVect anchorA, anchorB; + cpFloat min, max; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc; + cpFloat bias; +}; + +struct cpPivotJoint { + cpConstraint constraint; + cpVect anchorA, anchorB; + + cpVect r1, r2; + cpMat2x2 k; + + cpVect jAcc; + cpVect bias; +}; + +struct cpGrooveJoint { + cpConstraint constraint; + cpVect grv_n, grv_a, grv_b; + cpVect anchorB; + + cpVect grv_tn; + cpFloat clamp; + cpVect r1, r2; + cpMat2x2 k; + + cpVect jAcc; + cpVect bias; +}; + +struct cpDampedSpring { + cpConstraint constraint; + cpVect anchorA, anchorB; + cpFloat restLength; + cpFloat stiffness; + cpFloat damping; + cpDampedSpringForceFunc springForceFunc; + + cpFloat target_vrn; + cpFloat v_coef; + + cpVect r1, r2; + cpFloat nMass; + cpVect n; + + cpFloat jAcc; +}; + +struct cpDampedRotarySpring { + cpConstraint constraint; + cpFloat restAngle; + cpFloat stiffness; + cpFloat damping; + cpDampedRotarySpringTorqueFunc springTorqueFunc; + + cpFloat target_wrn; + cpFloat w_coef; + + cpFloat iSum; + cpFloat jAcc; +}; + +struct cpRotaryLimitJoint { + cpConstraint constraint; + cpFloat min, max; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc; +}; + +struct cpRatchetJoint { + cpConstraint constraint; + cpFloat angle, phase, ratchet; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc; +}; + +struct cpGearJoint { + cpConstraint constraint; + cpFloat phase, ratio; + cpFloat ratio_inv; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc; +}; + +struct cpSimpleMotor { + cpConstraint constraint; + cpFloat rate; + + cpFloat iSum; + + cpFloat jAcc; +}; + +typedef struct cpContactBufferHeader cpContactBufferHeader; +typedef void (*cpSpaceArbiterApplyImpulseFunc)(cpArbiter *arb); + +struct cpSpace { + int iterations; + + cpVect gravity; + cpFloat damping; + + cpFloat idleSpeedThreshold; + cpFloat sleepTimeThreshold; + + cpFloat collisionSlop; + cpFloat collisionBias; + cpTimestamp collisionPersistence; + + cpDataPointer userData; + + cpTimestamp stamp; + cpFloat curr_dt; + + cpArray *dynamicBodies; + cpArray *staticBodies; + cpArray *rousedBodies; + cpArray *sleepingComponents; + + cpHashValue shapeIDCounter; + cpSpatialIndex *staticShapes; + cpSpatialIndex *dynamicShapes; + + cpArray *constraints; + + cpArray *arbiters; + cpContactBufferHeader *contactBuffersHead; + cpHashSet *cachedArbiters; + cpArray *pooledArbiters; + + cpArray *allocatedBuffers; + unsigned int locked; + + cpBool usesWildcards; + cpHashSet *collisionHandlers; + cpCollisionHandler defaultHandler; + + cpBool skipPostStep; + cpArray *postStepCallbacks; + + cpBody *staticBody; + cpBody _staticBody; +}; + +typedef struct cpPostStepCallback { + cpPostStepFunc func; + void *key; + void *data; +} cpPostStepCallback; + +#endif diff --git a/Chipmunk2D/include/chipmunk/chipmunk_types.h b/Chipmunk2D/include/chipmunk/chipmunk_types.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk_types.h @@ -0,0 +1,268 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_TYPES_H +#define CHIPMUNK_TYPES_H + +#include +#include +#include + +#ifdef __APPLE__ + #include "TargetConditionals.h" +#endif + +// Use CGTypes by default on iOS and Mac. +// Also enables usage of doubles on 64 bit. +// Performance is usually very comparable when the CPU cache is well utilised. +#if (TARGET_OS_IPHONE || TARGET_OS_MAC) && (!defined CP_USE_CGTYPES) + #define CP_USE_CGTYPES 1 +#endif + +#if CP_USE_CGTYPES + #if TARGET_OS_IPHONE + #include + #include + #elif TARGET_OS_MAC + #include + #endif + + #if defined(__LP64__) && __LP64__ + #define CP_USE_DOUBLES 1 + #else + #define CP_USE_DOUBLES 0 + #endif +#endif + +#ifndef CP_USE_DOUBLES + // Use doubles by default for higher precision. + #define CP_USE_DOUBLES 1 +#endif + +/// @defgroup basicTypes Basic Types +/// Most of these types can be configured at compile time. +/// @{ + +#if CP_USE_DOUBLES +/// Chipmunk's floating point type. +/// Can be reconfigured at compile time. + typedef double cpFloat; + #define cpfsqrt sqrt + #define cpfsin sin + #define cpfcos cos + #define cpfacos acos + #define cpfatan2 atan2 + #define cpfmod fmod + #define cpfexp exp + #define cpfpow pow + #define cpffloor floor + #define cpfceil ceil + #define CPFLOAT_MIN DBL_MIN +#else + typedef float cpFloat; + #define cpfsqrt sqrtf + #define cpfsin sinf + #define cpfcos cosf + #define cpfacos acosf + #define cpfatan2 atan2f + #define cpfmod fmodf + #define cpfexp expf + #define cpfpow powf + #define cpffloor floorf + #define cpfceil ceilf + #define CPFLOAT_MIN FLT_MIN +#endif + +#ifndef INFINITY + #ifdef _MSC_VER + union MSVC_EVIL_FLOAT_HACK + { + unsigned __int8 Bytes[4]; + float Value; + }; + static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; + #define INFINITY (INFINITY_HACK.Value) + #endif + + #ifdef __GNUC__ + #define INFINITY (__builtin_inf()) + #endif + + #ifndef INFINITY + #define INFINITY (1e1000) + #endif +#endif + + +#define CP_PI ((cpFloat)3.14159265358979323846264338327950288) + + +/// Return the max of two cpFloats. +static inline cpFloat cpfmax(cpFloat a, cpFloat b) +{ + return (a > b) ? a : b; +} + +/// Return the min of two cpFloats. +static inline cpFloat cpfmin(cpFloat a, cpFloat b) +{ + return (a < b) ? a : b; +} + +/// Return the absolute value of a cpFloat. +static inline cpFloat cpfabs(cpFloat f) +{ + return (f < 0) ? -f : f; +} + +/// Clamp @c f to be between @c min and @c max. +static inline cpFloat cpfclamp(cpFloat f, cpFloat min, cpFloat max) +{ + return cpfmin(cpfmax(f, min), max); +} + +/// Clamp @c f to be between 0 and 1. +static inline cpFloat cpfclamp01(cpFloat f) +{ + return cpfmax(0.0f, cpfmin(f, 1.0f)); +} + + + +/// Linearly interpolate (or extrapolate) between @c f1 and @c f2 by @c t percent. +static inline cpFloat cpflerp(cpFloat f1, cpFloat f2, cpFloat t) +{ + return f1*(1.0f - t) + f2*t; +} + +/// Linearly interpolate from @c f1 to @c f2 by no more than @c d. +static inline cpFloat cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) +{ + return f1 + cpfclamp(f2 - f1, -d, d); +} + +/// Hash value type. +#ifdef CP_HASH_VALUE_TYPE + typedef CP_HASH_VALUE_TYPE cpHashValue; +#else + typedef uintptr_t cpHashValue; +#endif + +/// Type used internally to cache colliding object info for cpCollideShapes(). +/// Should be at least 32 bits. +typedef uint32_t cpCollisionID; + +// Oh C, how we love to define our own boolean types to get compiler compatibility +/// Chipmunk's boolean type. +#ifdef CP_BOOL_TYPE + typedef CP_BOOL_TYPE cpBool; +#else + typedef unsigned char cpBool; +#endif + +#ifndef cpTrue +/// true value. + #define cpTrue 1 +#endif + +#ifndef cpFalse +/// false value. + #define cpFalse 0 +#endif + +#ifdef CP_DATA_POINTER_TYPE + typedef CP_DATA_POINTER_TYPE cpDataPointer; +#else +/// Type used for user data pointers. + typedef void * cpDataPointer; +#endif + +#ifdef CP_COLLISION_TYPE_TYPE + typedef CP_COLLISION_TYPE_TYPE cpCollisionType; +#else +/// Type used for cpSpace.collision_type. + typedef uintptr_t cpCollisionType; +#endif + +#ifdef CP_GROUP_TYPE + typedef CP_GROUP_TYPE cpGroup; +#else +/// Type used for cpShape.group. + typedef uintptr_t cpGroup; +#endif + +#ifdef CP_BITMASK_TYPE + typedef CP_BITMASK_TYPE cpBitmask; +#else +/// Type used for cpShapeFilter category and mask. + typedef unsigned int cpBitmask; +#endif + +#ifdef CP_TIMESTAMP_TYPE + typedef CP_TIMESTAMP_TYPE cpTimestamp; +#else +/// Type used for various timestamps in Chipmunk. + typedef unsigned int cpTimestamp; +#endif + +#ifndef CP_NO_GROUP +/// Value for cpShape.group signifying that a shape is in no group. + #define CP_NO_GROUP ((cpGroup)0) +#endif + +#ifndef CP_ALL_CATEGORIES +/// Value for cpShape.layers signifying that a shape is in every layer. + #define CP_ALL_CATEGORIES (~(cpBitmask)0) +#endif + +#ifndef CP_WILDCARD_COLLISION_TYPE +/// cpCollisionType value internally reserved for hashing wildcard handlers. + #define CP_WILDCARD_COLLISION_TYPE (~(cpCollisionType)0) +#endif + +/// @} + +// CGPoints are structurally the same, and allow +// easy interoperability with other Cocoa libraries +#if CP_USE_CGTYPES + typedef CGPoint cpVect; +#else +/// Chipmunk's 2D vector type. +/// @addtogroup cpVect + typedef struct cpVect{cpFloat x,y;} cpVect; +#endif + +#if CP_USE_CGTYPES + typedef CGAffineTransform cpTransform; +#else + /// Column major affine transform. + typedef struct cpTransform { + cpFloat a, b, c, d, tx, ty; + } cpTransform; +#endif + +// NUKE +typedef struct cpMat2x2 { + // Row major [[a, b][c d]] + cpFloat a, b, c, d; +} cpMat2x2; + +#endif diff --git a/Chipmunk2D/include/chipmunk/chipmunk_unsafe.h b/Chipmunk2D/include/chipmunk/chipmunk_unsafe.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/chipmunk_unsafe.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +/* This header defines a number of "unsafe" operations on Chipmunk objects. + * In this case "unsafe" is referring to operations which may reduce the + * physical accuracy or numerical stability of the simulation, but will not + * cause crashes. + * + * The prime example is mutating collision shapes. Chipmunk does not support + * this directly. Mutating shapes using this API will caused objects in contact + * to be pushed apart using Chipmunk's overlap solver, but not using real + * persistent velocities. Probably not what you meant, but perhaps close enough. + */ + +/// @defgroup unsafe Chipmunk Unsafe Shape Operations +/// These functions are used for mutating collision shapes. +/// Chipmunk does not have any way to get velocity information on changing shapes, +/// so the results will be unrealistic. You must explicity include the chipmunk_unsafe.h header to use them. +/// @{ + +#ifndef CHIPMUNK_UNSAFE_H +#define CHIPMUNK_UNSAFE_H + +#ifdef __cplusplus +extern "C" { +#endif + +/// Set the radius of a circle shape. +CP_EXPORT void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); +/// Set the offset of a circle shape. +CP_EXPORT void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); + +/// Set the endpoints of a segment shape. +CP_EXPORT void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); +/// Set the radius of a segment shape. +CP_EXPORT void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); + +/// Set the vertexes of a poly shape. +CP_EXPORT void cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform); +CP_EXPORT void cpPolyShapeSetVertsRaw(cpShape *shape, int count, cpVect *verts); +/// Set the radius of a poly shape. +CP_EXPORT void cpPolyShapeSetRadius(cpShape *shape, cpFloat radius); + +#ifdef __cplusplus +} +#endif +#endif +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpArbiter.h b/Chipmunk2D/include/chipmunk/cpArbiter.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpArbiter.h @@ -0,0 +1,145 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpArbiter cpArbiter +/// The cpArbiter struct tracks pairs of colliding shapes. +/// They are also used in conjuction with collision handler callbacks +/// allowing you to retrieve information on the collision or change it. +/// A unique arbiter value is used for each pair of colliding objects. It persists until the shapes separate. +/// @{ + +#define CP_MAX_CONTACTS_PER_ARBITER 2 + +/// Get the restitution (elasticity) that will be applied to the pair of colliding objects. +CP_EXPORT cpFloat cpArbiterGetRestitution(const cpArbiter *arb); +/// Override the restitution (elasticity) that will be applied to the pair of colliding objects. +CP_EXPORT void cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution); +/// Get the friction coefficient that will be applied to the pair of colliding objects. +CP_EXPORT cpFloat cpArbiterGetFriction(const cpArbiter *arb); +/// Override the friction coefficient that will be applied to the pair of colliding objects. +CP_EXPORT void cpArbiterSetFriction(cpArbiter *arb, cpFloat friction); + +// Get the relative surface velocity of the two shapes in contact. +CP_EXPORT cpVect cpArbiterGetSurfaceVelocity(cpArbiter *arb); + +// Override the relative surface velocity of the two shapes in contact. +// By default this is calculated to be the difference of the two surface velocities clamped to the tangent plane. +CP_EXPORT void cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr); + +/// Get the user data pointer associated with this pair of colliding objects. +CP_EXPORT cpDataPointer cpArbiterGetUserData(const cpArbiter *arb); +/// Set a user data point associated with this pair of colliding objects. +/// If you need to perform any cleanup for this pointer, you must do it yourself, in the separate callback for instance. +CP_EXPORT void cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData); + +/// Calculate the total impulse including the friction that was applied by this arbiter. +/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. +CP_EXPORT cpVect cpArbiterTotalImpulse(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. +CP_EXPORT cpFloat cpArbiterTotalKE(const cpArbiter *arb); + +/// Mark a collision pair to be ignored until the two objects separate. +/// Pre-solve and post-solve callbacks will not be called, but the separate callback will be called. +CP_EXPORT cpBool cpArbiterIgnore(cpArbiter *arb); + +/// Return the colliding shapes involved for this arbiter. +/// The order of their cpSpace.collision_type values will match +/// the order set when the collision handler was registered. +CP_EXPORT void cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b); + +/// A macro shortcut for defining and retrieving the shapes from an arbiter. +#define CP_ARBITER_GET_SHAPES(__arb__, __a__, __b__) cpShape *__a__, *__b__; cpArbiterGetShapes(__arb__, &__a__, &__b__); + +/// Return the colliding bodies involved for this arbiter. +/// The order of the cpSpace.collision_type the bodies are associated with values will match +/// the order set when the collision handler was registered. +CP_EXPORT void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b); + +/// 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__); + +/// A struct that wraps up the important collision data for an arbiter. +struct cpContactPointSet { + /// The number of contact points in the set. + int count; + + /// The normal of the collision. + cpVect normal; + + /// The array of contact points. + struct { + /// The position of the contact on the surface of each shape. + cpVect pointA, pointB; + /// Penetration distance of the two shapes. Overlapping means it will be negative. + /// This value is calculated as cpvdot(cpvsub(point2, point1), normal) and is ignored by cpArbiterSetContactPointSet(). + cpFloat distance; + } points[CP_MAX_CONTACTS_PER_ARBITER]; +}; + +/// Return a contact set from an arbiter. +CP_EXPORT cpContactPointSet cpArbiterGetContactPointSet(const cpArbiter *arb); + +/// Replace the contact point set for an arbiter. +/// This can be a very powerful feature, but use it with caution! +CP_EXPORT void cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set); + +/// Returns true if this is the first step a pair of objects started colliding. +CP_EXPORT cpBool cpArbiterIsFirstContact(const cpArbiter *arb); +/// Returns true if the separate callback is due to a shape being removed from the space. +CP_EXPORT cpBool cpArbiterIsRemoval(const cpArbiter *arb); + +/// Get the number of contact points for this arbiter. +CP_EXPORT int cpArbiterGetCount(const cpArbiter *arb); +/// Get the normal of the collision. +CP_EXPORT cpVect cpArbiterGetNormal(const cpArbiter *arb); +/// Get the position of the @c ith contact point on the surface of the first shape. +CP_EXPORT cpVect cpArbiterGetPointA(const cpArbiter *arb, int i); +/// Get the position of the @c ith contact point on the surface of the second shape. +CP_EXPORT cpVect cpArbiterGetPointB(const cpArbiter *arb, int i); +/// Get the depth of the @c ith contact point. +CP_EXPORT cpFloat cpArbiterGetDepth(const cpArbiter *arb, int i); + +/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. +/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. +CP_EXPORT cpBool cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space); +/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. +/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. +CP_EXPORT cpBool cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space); + +/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. +/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. +CP_EXPORT cpBool cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space); +/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. +/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. +CP_EXPORT cpBool cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space); + +/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. +CP_EXPORT void cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space); +/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. +CP_EXPORT void cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space); + +/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. +CP_EXPORT void cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space); +/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. +CP_EXPORT void cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpBB.h b/Chipmunk2D/include/chipmunk/cpBB.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpBB.h @@ -0,0 +1,187 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_BB_H +#define CHIPMUNK_BB_H + +#include "chipmunk_types.h" +#include "cpVect.h" + +/// @defgroup cpBBB cpBB +/// Chipmunk's axis-aligned 2D bounding box type along with a few handy routines. +/// @{ + +/// Chipmunk's axis-aligned 2D bounding box type. (left, bottom, right, top) +typedef struct cpBB{ + cpFloat l, b, r ,t; +} cpBB; + +/// Convenience constructor for cpBB structs. +static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t) +{ + cpBB bb = {l, b, r, t}; + return bb; +} + +/// Constructs a cpBB centered on a point with the given extents (half sizes). +static inline cpBB +cpBBNewForExtents(const cpVect c, const cpFloat hw, const cpFloat hh) +{ + return cpBBNew(c.x - hw, c.y - hh, c.x + hw, c.y + hh); +} + +/// Constructs a cpBB for a circle with the given position and radius. +static inline cpBB cpBBNewForCircle(const cpVect p, const cpFloat r) +{ + return cpBBNewForExtents(p, r, r); +} + +/// Returns true if @c a and @c b intersect. +static inline cpBool cpBBIntersects(const cpBB a, const cpBB b) +{ + return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t); +} + +/// Returns true if @c other lies completely within @c bb. +static inline cpBool cpBBContainsBB(const cpBB bb, const cpBB other) +{ + return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t); +} + +/// Returns true if @c bb contains @c v. +static inline cpBool cpBBContainsVect(const cpBB bb, const cpVect v) +{ + return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y); +} + +/// Returns a bounding box that holds both bounding boxes. +static inline cpBB cpBBMerge(const cpBB a, const cpBB b){ + return cpBBNew( + cpfmin(a.l, b.l), + cpfmin(a.b, b.b), + cpfmax(a.r, b.r), + cpfmax(a.t, b.t) + ); +} + +/// Returns a bounding box that holds both @c bb and @c v. +static inline cpBB cpBBExpand(const cpBB bb, const cpVect v){ + return cpBBNew( + cpfmin(bb.l, v.x), + cpfmin(bb.b, v.y), + cpfmax(bb.r, v.x), + cpfmax(bb.t, v.y) + ); +} + +/// Returns the center of a bounding box. +static inline cpVect +cpBBCenter(cpBB bb) +{ + return cpvlerp(cpv(bb.l, bb.b), cpv(bb.r, bb.t), 0.5f); +} + +/// Returns the area of the bounding box. +static inline cpFloat cpBBArea(cpBB bb) +{ + return (bb.r - bb.l)*(bb.t - bb.b); +} + +/// Merges @c a and @c b and returns the area of the merged bounding box. +static inline cpFloat cpBBMergedArea(cpBB a, cpBB b) +{ + return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b)); +} + +/// 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) +{ + cpVect delta = cpvsub(b, a); + cpFloat tmin = -INFINITY, tmax = INFINITY; + + if(delta.x == 0.0f){ + if(a.x < bb.l || bb.r < a.x) return INFINITY; + } else { + cpFloat t1 = (bb.l - a.x)/delta.x; + cpFloat t2 = (bb.r - a.x)/delta.x; + tmin = cpfmax(tmin, cpfmin(t1, t2)); + tmax = cpfmin(tmax, cpfmax(t1, t2)); + } + + if(delta.y == 0.0f){ + if(a.y < bb.b || bb.t < a.y) return INFINITY; + } else { + cpFloat t1 = (bb.b - a.y)/delta.y; + cpFloat t2 = (bb.t - a.y)/delta.y; + tmin = cpfmax(tmin, cpfmin(t1, t2)); + tmax = cpfmin(tmax, cpfmax(t1, t2)); + } + + if(tmin <= tmax && 0.0f <= tmax && tmin <= 1.0f){ + return cpfmax(tmin, 0.0f); + } else { + 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) +{ + return (cpBBSegmentQuery(bb, a, b) != INFINITY); +} + +/// Clamp a vector to a bounding box. +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)); +} + +/// Wrap a vector to a bounding box. +static inline cpVect +cpBBWrapVect(const cpBB bb, const cpVect v) +{ + cpFloat dx = cpfabs(bb.r - bb.l); + cpFloat modx = cpfmod(v.x - bb.l, dx); + cpFloat x = (modx > 0.0f) ? modx : modx + dx; + + cpFloat dy = cpfabs(bb.t - bb.b); + cpFloat mody = cpfmod(v.y - bb.b, dy); + cpFloat y = (mody > 0.0f) ? mody : mody + dy; + + return cpv(x + bb.l, y + bb.b); +} + +/// Returns a bounding box offseted by @c v. +static inline cpBB +cpBBOffset(const cpBB bb, const cpVect v) +{ + return cpBBNew( + bb.l + v.x, + bb.b + v.y, + bb.r + v.x, + bb.t + v.y + ); +} + +///@} + +#endif diff --git a/Chipmunk2D/include/chipmunk/cpBody.h b/Chipmunk2D/include/chipmunk/cpBody.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpBody.h @@ -0,0 +1,189 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpBody cpBody +/// Chipmunk's rigid body type. Rigid bodies hold the physical properties of an object like +/// it's mass, and position and velocity of it's center of gravity. They don't have an shape on their own. +/// They are given a shape by creating collision shapes (cpShape) that point to the body. +/// @{ + +typedef enum cpBodyType { + /// A dynamic body is one that is affected by gravity, forces, and collisions. + /// This is the default body type. + CP_BODY_TYPE_DYNAMIC, + /// A kinematic body is an infinite mass, user controlled body that is not affected by gravity, forces or collisions. + /// Instead the body only moves based on it's velocity. + /// Dynamic bodies collide normally with kinematic bodies, though the kinematic body will be unaffected. + /// Collisions between two kinematic bodies, or a kinematic body and a static body produce collision callbacks, but no collision response. + CP_BODY_TYPE_KINEMATIC, + /// A static body is a body that never (or rarely) moves. If you move a static body, you must call one of the cpSpaceReindex*() functions. + /// Chipmunk uses this information to optimize the collision detection. + /// Static bodies do not produce collision callbacks when colliding with other static bodies. + CP_BODY_TYPE_STATIC, +} cpBodyType; + +/// Rigid body velocity update function type. +typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +/// Rigid body position update function type. +typedef void (*cpBodyPositionFunc)(cpBody *body, cpFloat dt); + +/// Allocate a cpBody. +CP_EXPORT cpBody* cpBodyAlloc(void); +/// Initialize a cpBody. +CP_EXPORT cpBody* cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment); +/// Allocate and initialize a cpBody. +CP_EXPORT cpBody* cpBodyNew(cpFloat mass, cpFloat moment); + +/// Allocate and initialize a cpBody, and set it as a kinematic body. +CP_EXPORT cpBody* cpBodyNewKinematic(void); +/// Allocate and initialize a cpBody, and set it as a static body. +CP_EXPORT cpBody* cpBodyNewStatic(void); + +/// Destroy a cpBody. +CP_EXPORT void cpBodyDestroy(cpBody *body); +/// Destroy and free a cpBody. +CP_EXPORT void cpBodyFree(cpBody *body); + +// Defined in cpSpace.c +/// Wake up a sleeping or idle body. +CP_EXPORT void cpBodyActivate(cpBody *body); +/// Wake up any sleeping or idle bodies touching a static body. +CP_EXPORT void cpBodyActivateStatic(cpBody *body, cpShape *filter); + +/// Force a body to fall asleep immediately. +CP_EXPORT void cpBodySleep(cpBody *body); +/// Force a body to fall asleep immediately along with other bodies in a group. +CP_EXPORT void cpBodySleepWithGroup(cpBody *body, cpBody *group); + +/// Returns true if the body is sleeping. +CP_EXPORT cpBool cpBodyIsSleeping(const cpBody *body); + +/// Get the type of the body. +CP_EXPORT cpBodyType cpBodyGetType(cpBody *body); +/// Set the type of the body. +CP_EXPORT void cpBodySetType(cpBody *body, cpBodyType type); + +/// Get the space this body is added to. +CP_EXPORT cpSpace* cpBodyGetSpace(const cpBody *body); + +/// Get the mass of the body. +CP_EXPORT cpFloat cpBodyGetMass(const cpBody *body); +/// Set the mass of the body. +CP_EXPORT void cpBodySetMass(cpBody *body, cpFloat m); + +/// Get the moment of inertia of the body. +CP_EXPORT cpFloat cpBodyGetMoment(const cpBody *body); +/// Set the moment of inertia of the body. +CP_EXPORT void cpBodySetMoment(cpBody *body, cpFloat i); + +/// Set the position of a body. +CP_EXPORT cpVect cpBodyGetPosition(const cpBody *body); +/// Set the position of the body. +CP_EXPORT void cpBodySetPosition(cpBody *body, cpVect pos); + +/// Get the offset of the center of gravity in body local coordinates. +CP_EXPORT cpVect cpBodyGetCenterOfGravity(const cpBody *body); +/// Set the offset of the center of gravity in body local coordinates. +CP_EXPORT void cpBodySetCenterOfGravity(cpBody *body, cpVect cog); + +/// Get the velocity of the body. +CP_EXPORT cpVect cpBodyGetVelocity(const cpBody *body); +/// Set the velocity of the body. +CP_EXPORT void cpBodySetVelocity(cpBody *body, cpVect velocity); + +/// Get the force applied to the body for the next time step. +CP_EXPORT cpVect cpBodyGetForce(const cpBody *body); +/// Set the force applied to the body for the next time step. +CP_EXPORT void cpBodySetForce(cpBody *body, cpVect force); + +/// Get the angle of the body. +CP_EXPORT cpFloat cpBodyGetAngle(const cpBody *body); +/// Set the angle of a body. +CP_EXPORT void cpBodySetAngle(cpBody *body, cpFloat a); + +/// Get the angular velocity of the body. +CP_EXPORT cpFloat cpBodyGetAngularVelocity(const cpBody *body); +/// Set the angular velocity of the body. +CP_EXPORT void cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity); + +/// Get the torque applied to the body for the next time step. +CP_EXPORT cpFloat cpBodyGetTorque(const cpBody *body); +/// Set the torque applied to the body for the next time step. +CP_EXPORT void cpBodySetTorque(cpBody *body, cpFloat torque); + +/// Get the rotation vector of the body. (The x basis vector of it's transform.) +CP_EXPORT cpVect cpBodyGetRotation(const cpBody *body); + +/// Get the user data pointer assigned to the body. +CP_EXPORT cpDataPointer cpBodyGetUserData(const cpBody *body); +/// Set the user data pointer assigned to the body. +CP_EXPORT void cpBodySetUserData(cpBody *body, cpDataPointer userData); + +/// Set the callback used to update a body's velocity. +CP_EXPORT void cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc); +/// Set the callback used to update a body's position. +/// NOTE: It's not generally recommended to override this unless you call the default position update function. +CP_EXPORT void cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc); + +/// Default velocity integration function.. +CP_EXPORT void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +/// Default position integration function. +CP_EXPORT void cpBodyUpdatePosition(cpBody *body, cpFloat dt); + +/// Convert body relative/local coordinates to absolute/world coordinates. +CP_EXPORT cpVect cpBodyLocalToWorld(const cpBody *body, const cpVect point); +/// Convert body absolute/world coordinates to relative/local coordinates. +CP_EXPORT cpVect cpBodyWorldToLocal(const cpBody *body, const cpVect point); + +/// Apply a force to a body. Both the force and point are expressed in world coordinates. +CP_EXPORT void cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point); +/// Apply a force to a body. Both the force and point are expressed in body local coordinates. +CP_EXPORT void cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point); + +/// Apply an impulse to a body. Both the impulse and point are expressed in world coordinates. +CP_EXPORT void cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point); +/// Apply an impulse to a body. Both the impulse and point are expressed in body local coordinates. +CP_EXPORT void cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point); + +/// Get the velocity on a body (in world units) at a point on the body in world coordinates. +CP_EXPORT cpVect cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point); +/// Get the velocity on a body (in world units) at a point on the body in local coordinates. +CP_EXPORT cpVect cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point); + +/// Get the amount of kinetic energy contained by the body. +CP_EXPORT cpFloat cpBodyKineticEnergy(const cpBody *body); + +/// Body/shape iterator callback function type. +typedef void (*cpBodyShapeIteratorFunc)(cpBody *body, cpShape *shape, void *data); +/// Call @c func once for each shape attached to @c body and added to the space. +CP_EXPORT void cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data); + +/// Body/constraint iterator callback function type. +typedef void (*cpBodyConstraintIteratorFunc)(cpBody *body, cpConstraint *constraint, void *data); +/// Call @c func once for each constraint attached to @c body and added to the space. +CP_EXPORT void cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data); + +/// Body/arbiter iterator callback function type. +typedef void (*cpBodyArbiterIteratorFunc)(cpBody *body, cpArbiter *arbiter, void *data); +/// Call @c func once for each arbiter that is currently active on the body. +CP_EXPORT void cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data); + +///@} diff --git a/Chipmunk2D/include/chipmunk/cpConstraint.h b/Chipmunk2D/include/chipmunk/cpConstraint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpConstraint.h @@ -0,0 +1,95 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpConstraint cpConstraint +/// @{ + +/// 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); + +/// Destroy a constraint. +CP_EXPORT void cpConstraintDestroy(cpConstraint *constraint); +/// Destroy and free a constraint. +CP_EXPORT void cpConstraintFree(cpConstraint *constraint); + +/// Get the cpSpace this constraint is added to. +CP_EXPORT cpSpace* cpConstraintGetSpace(const cpConstraint *constraint); + +/// Get the first body the constraint is attached to. +CP_EXPORT cpBody* cpConstraintGetBodyA(const cpConstraint *constraint); + +/// Get the second body the constraint is attached to. +CP_EXPORT cpBody* cpConstraintGetBodyB(const cpConstraint *constraint); + +/// Get the maximum force that this constraint is allowed to use. +CP_EXPORT cpFloat cpConstraintGetMaxForce(const cpConstraint *constraint); +/// Set the maximum force that this constraint is allowed to use. (defaults to INFINITY) +CP_EXPORT void cpConstraintSetMaxForce(cpConstraint *constraint, cpFloat maxForce); + +/// Get rate at which joint error is corrected. +CP_EXPORT cpFloat cpConstraintGetErrorBias(const cpConstraint *constraint); +/// Set rate at which joint error is corrected. +/// Defaults to pow(1.0 - 0.1, 60.0) meaning that it will +/// correct 10% of the error every 1/60th of a second. +CP_EXPORT void cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias); + +/// Get the maximum rate at which joint error is corrected. +CP_EXPORT cpFloat cpConstraintGetMaxBias(const cpConstraint *constraint); +/// Set the maximum rate at which joint error is corrected. (defaults to INFINITY) +CP_EXPORT void cpConstraintSetMaxBias(cpConstraint *constraint, cpFloat maxBias); + +/// Get if the two bodies connected by the constraint are allowed to collide or not. +CP_EXPORT cpBool cpConstraintGetCollideBodies(const cpConstraint *constraint); +/// Set if the two bodies connected by the constraint are allowed to collide or not. (defaults to cpFalse) +CP_EXPORT void cpConstraintSetCollideBodies(cpConstraint *constraint, cpBool collideBodies); + +/// Get the pre-solve function that is called before the solver runs. +CP_EXPORT cpConstraintPreSolveFunc cpConstraintGetPreSolveFunc(const cpConstraint *constraint); +/// Set the pre-solve function that is called before the solver runs. +CP_EXPORT void cpConstraintSetPreSolveFunc(cpConstraint *constraint, cpConstraintPreSolveFunc preSolveFunc); + +/// Get the post-solve function that is called before the solver runs. +CP_EXPORT cpConstraintPostSolveFunc cpConstraintGetPostSolveFunc(const cpConstraint *constraint); +/// Set the post-solve function that is called before the solver runs. +CP_EXPORT void cpConstraintSetPostSolveFunc(cpConstraint *constraint, cpConstraintPostSolveFunc postSolveFunc); + +/// Get the user definable data pointer for this constraint +CP_EXPORT cpDataPointer cpConstraintGetUserData(const cpConstraint *constraint); +/// Set the user definable data pointer for this constraint +CP_EXPORT void cpConstraintSetUserData(cpConstraint *constraint, cpDataPointer userData); + +/// Get the last impulse applied by this constraint. +CP_EXPORT cpFloat cpConstraintGetImpulse(cpConstraint *constraint); + +#include "cpPinJoint.h" +#include "cpSlideJoint.h" +#include "cpPivotJoint.h" +#include "cpGrooveJoint.h" +#include "cpDampedSpring.h" +#include "cpDampedRotarySpring.h" +#include "cpRotaryLimitJoint.h" +#include "cpRatchetJoint.h" +#include "cpGearJoint.h" +#include "cpSimpleMotor.h" + +///@} diff --git a/Chipmunk2D/include/chipmunk/cpDampedRotarySpring.h b/Chipmunk2D/include/chipmunk/cpDampedRotarySpring.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpDampedRotarySpring.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpDampedRotarySpring cpDampedRotarySpring +/// @{ + +/// Check if a constraint is a damped rotary springs. +CP_EXPORT cpBool cpConstraintIsDampedRotarySpring(const cpConstraint *constraint); + +/// Function type used for damped rotary spring force callbacks. +typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); + +/// Allocate a damped rotary spring. +CP_EXPORT cpDampedRotarySpring* cpDampedRotarySpringAlloc(void); +/// Initialize a damped rotary spring. +CP_EXPORT cpDampedRotarySpring* cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); +/// Allocate and initialize a damped rotary spring. +CP_EXPORT cpConstraint* cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); + +/// Get the rest length of the spring. +CP_EXPORT cpFloat cpDampedRotarySpringGetRestAngle(const cpConstraint *constraint); +/// Set the rest length of the spring. +CP_EXPORT void cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle); + +/// Get the stiffness of the spring in force/distance. +CP_EXPORT cpFloat cpDampedRotarySpringGetStiffness(const cpConstraint *constraint); +/// Set the stiffness of the spring in force/distance. +CP_EXPORT void cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness); + +/// Get the damping of the spring. +CP_EXPORT cpFloat cpDampedRotarySpringGetDamping(const cpConstraint *constraint); +/// Set the damping of the spring. +CP_EXPORT void cpDampedRotarySpringSetDamping(cpConstraint *constraint, cpFloat damping); + +/// Get the damping of the spring. +CP_EXPORT cpDampedRotarySpringTorqueFunc cpDampedRotarySpringGetSpringTorqueFunc(const cpConstraint *constraint); +/// Set the damping of the spring. +CP_EXPORT void cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpDampedSpring.h b/Chipmunk2D/include/chipmunk/cpDampedSpring.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpDampedSpring.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpDampedSpring cpDampedSpring +/// @{ + +/// Check if a constraint is a slide joint. +CP_EXPORT cpBool cpConstraintIsDampedSpring(const cpConstraint *constraint); + +/// Function type used for damped spring force callbacks. +typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); + +/// Allocate a damped spring. +CP_EXPORT cpDampedSpring* cpDampedSpringAlloc(void); +/// Initialize a damped spring. +CP_EXPORT cpDampedSpring* cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping); +/// Allocate and initialize a damped spring. +CP_EXPORT cpConstraint* cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping); + +/// Get the location of the first anchor relative to the first body. +CP_EXPORT cpVect cpDampedSpringGetAnchorA(const cpConstraint *constraint); +/// Set the location of the first anchor relative to the first body. +CP_EXPORT void cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA); + +/// Get the location of the second anchor relative to the second body. +CP_EXPORT cpVect cpDampedSpringGetAnchorB(const cpConstraint *constraint); +/// Set the location of the second anchor relative to the second body. +CP_EXPORT void cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB); + +/// Get the rest length of the spring. +CP_EXPORT cpFloat cpDampedSpringGetRestLength(const cpConstraint *constraint); +/// Set the rest length of the spring. +CP_EXPORT void cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength); + +/// Get the stiffness of the spring in force/distance. +CP_EXPORT cpFloat cpDampedSpringGetStiffness(const cpConstraint *constraint); +/// Set the stiffness of the spring in force/distance. +CP_EXPORT void cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness); + +/// Get the damping of the spring. +CP_EXPORT cpFloat cpDampedSpringGetDamping(const cpConstraint *constraint); +/// Set the damping of the spring. +CP_EXPORT void cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping); + +/// Get the damping of the spring. +CP_EXPORT cpDampedSpringForceFunc cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint); +/// Set the damping of the spring. +CP_EXPORT void cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpGearJoint.h b/Chipmunk2D/include/chipmunk/cpGearJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpGearJoint.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpGearJoint cpGearJoint +/// @{ + +/// Check if a constraint is a damped rotary springs. +CP_EXPORT cpBool cpConstraintIsGearJoint(const cpConstraint *constraint); + +/// Allocate a gear joint. +CP_EXPORT cpGearJoint* cpGearJointAlloc(void); +/// Initialize a gear joint. +CP_EXPORT cpGearJoint* cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); +/// Allocate and initialize a gear joint. +CP_EXPORT cpConstraint* cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); + +/// Get the phase offset of the gears. +CP_EXPORT cpFloat cpGearJointGetPhase(const cpConstraint *constraint); +/// Set the phase offset of the gears. +CP_EXPORT void cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase); + +/// Get the angular distance of each ratchet. +CP_EXPORT cpFloat cpGearJointGetRatio(const cpConstraint *constraint); +/// Set the ratio of a gear joint. +CP_EXPORT void cpGearJointSetRatio(cpConstraint *constraint, cpFloat ratio); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpGrooveJoint.h b/Chipmunk2D/include/chipmunk/cpGrooveJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpGrooveJoint.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpGrooveJoint cpGrooveJoint +/// @{ + +/// Check if a constraint is a slide joint. +CP_EXPORT cpBool cpConstraintIsGrooveJoint(const cpConstraint *constraint); + +/// Allocate a groove joint. +CP_EXPORT cpGrooveJoint* cpGrooveJointAlloc(void); +/// Initialize a groove joint. +CP_EXPORT cpGrooveJoint* cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB); +/// Allocate and initialize a groove joint. +CP_EXPORT cpConstraint* cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB); + +/// Get the first endpoint of the groove relative to the first body. +CP_EXPORT cpVect cpGrooveJointGetGrooveA(const cpConstraint *constraint); +/// Set the first endpoint of the groove relative to the first body. +CP_EXPORT void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect grooveA); + +/// Get the first endpoint of the groove relative to the first body. +CP_EXPORT cpVect cpGrooveJointGetGrooveB(const cpConstraint *constraint); +/// Set the first endpoint of the groove relative to the first body. +CP_EXPORT void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect grooveB); + +/// Get the location of the second anchor relative to the second body. +CP_EXPORT cpVect cpGrooveJointGetAnchorB(const cpConstraint *constraint); +/// Set the location of the second anchor relative to the second body. +CP_EXPORT void cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpHastySpace.h b/Chipmunk2D/include/chipmunk/cpHastySpace.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpHastySpace.h @@ -0,0 +1,27 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +/// cpHastySpace is exclusive to Chipmunk Pro +/// Currently it enables ARM NEON optimizations in the solver, but in the future will include other optimizations such as +/// a multi-threaded solver and multi-threaded collision broadphases. + +struct cpHastySpace; +typedef struct cpHastySpace cpHastySpace; + +/// Create a new hasty space. +/// On ARM platforms that support NEON, this will enable the vectorized solver. +/// cpHastySpace also supports multiple threads, but runs single threaded by default for determinism. +CP_EXPORT cpSpace *cpHastySpaceNew(void); +CP_EXPORT void cpHastySpaceFree(cpSpace *space); + +/// Set the number of threads to use for the solver. +/// Currently Chipmunk is limited to 2 threads as using more generally provides very minimal performance gains. +/// Passing 0 as the thread count on iOS or OS X will cause Chipmunk to automatically detect the number of threads it should use. +/// On other platforms passing 0 for the thread count will set 1 thread. +CP_EXPORT void cpHastySpaceSetThreads(cpSpace *space, unsigned long threads); + +/// Returns the number of threads the solver is using to run. +CP_EXPORT unsigned long cpHastySpaceGetThreads(cpSpace *space); + +/// When stepping a hasty space, you must use this function. +CP_EXPORT void cpHastySpaceStep(cpSpace *space, cpFloat dt); diff --git a/Chipmunk2D/include/chipmunk/cpMarch.h b/Chipmunk2D/include/chipmunk/cpMarch.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpMarch.h @@ -0,0 +1,28 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +/// Function type used as a callback from the marching squares algorithm to sample an image function. +/// It passes you the point to sample and your context pointer, and you return the density. +typedef cpFloat (*cpMarchSampleFunc)(cpVect point, void *data); + +/// Function type used as a callback from the marching squares algorithm to output a line segment. +/// It passes you the two endpoints and your context pointer. +typedef void (*cpMarchSegmentFunc)(cpVect v0, cpVect v1, void *data); + +/// Trace an anti-aliased contour of an image along a particular threshold. +/// The given number of samples will be taken and spread across the bounding box area using the sampling function and context. +/// The segment function will be called for each segment detected that lies along the density contour for @c threshold. +CP_EXPORT void cpMarchSoft( + cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat threshold, + cpMarchSegmentFunc segment, void *segment_data, + cpMarchSampleFunc sample, void *sample_data +); + +/// Trace an aliased curve of an image along a particular threshold. +/// The given number of samples will be taken and spread across the bounding box area using the sampling function and context. +/// The segment function will be called for each segment detected that lies along the density contour for @c threshold. +CP_EXPORT void cpMarchHard( + cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat threshold, + cpMarchSegmentFunc segment, void *segment_data, + cpMarchSampleFunc sample, void *sample_data +); diff --git a/Chipmunk2D/include/chipmunk/cpPinJoint.h b/Chipmunk2D/include/chipmunk/cpPinJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpPinJoint.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpPinJoint cpPinJoint +/// @{ + +/// Check if a constraint is a pin joint. +CP_EXPORT cpBool cpConstraintIsPinJoint(const cpConstraint *constraint); + +/// Allocate a pin joint. +CP_EXPORT cpPinJoint* cpPinJointAlloc(void); +/// Initialize a pin joint. +CP_EXPORT cpPinJoint* cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); +/// Allocate and initialize a pin joint. +CP_EXPORT cpConstraint* cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); + +/// Get the location of the first anchor relative to the first body. +CP_EXPORT cpVect cpPinJointGetAnchorA(const cpConstraint *constraint); +/// Set the location of the first anchor relative to the first body. +CP_EXPORT void cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); + +/// Get the location of the second anchor relative to the second body. +CP_EXPORT cpVect cpPinJointGetAnchorB(const cpConstraint *constraint); +/// Set the location of the second anchor relative to the second body. +CP_EXPORT void cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); + +/// Get the distance the joint will maintain between the two anchors. +CP_EXPORT cpFloat cpPinJointGetDist(const cpConstraint *constraint); +/// Set the distance the joint will maintain between the two anchors. +CP_EXPORT void cpPinJointSetDist(cpConstraint *constraint, cpFloat dist); + +///@} diff --git a/Chipmunk2D/include/chipmunk/cpPivotJoint.h b/Chipmunk2D/include/chipmunk/cpPivotJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpPivotJoint.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpPivotJoint cpPivotJoint +/// @{ + +/// Check if a constraint is a slide joint. +CP_EXPORT cpBool cpConstraintIsPivotJoint(const cpConstraint *constraint); + +/// Allocate a pivot joint +CP_EXPORT cpPivotJoint* cpPivotJointAlloc(void); +/// Initialize a pivot joint. +CP_EXPORT cpPivotJoint* cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); +/// Allocate and initialize a pivot joint. +CP_EXPORT cpConstraint* cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); +/// Allocate and initialize a pivot joint with specific anchors. +CP_EXPORT cpConstraint* cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); + +/// Get the location of the first anchor relative to the first body. +CP_EXPORT cpVect cpPivotJointGetAnchorA(const cpConstraint *constraint); +/// Set the location of the first anchor relative to the first body. +CP_EXPORT void cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); + +/// Get the location of the second anchor relative to the second body. +CP_EXPORT cpVect cpPivotJointGetAnchorB(const cpConstraint *constraint); +/// Set the location of the second anchor relative to the second body. +CP_EXPORT void cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpPolyShape.h b/Chipmunk2D/include/chipmunk/cpPolyShape.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpPolyShape.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpPolyShape cpPolyShape +/// @{ + +/// Allocate a polygon shape. +CP_EXPORT cpPolyShape* cpPolyShapeAlloc(void); +/// Initialize a polygon shape with rounded corners. +/// A convex hull will be created from the vertexes. +CP_EXPORT cpPolyShape* cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius); +/// Initialize a polygon shape with rounded corners. +/// The vertexes must be convex with a counter-clockwise winding. +CP_EXPORT cpPolyShape* cpPolyShapeInitRaw(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpFloat radius); +/// Allocate and initialize a polygon shape with rounded corners. +/// A convex hull will be created from the vertexes. +CP_EXPORT cpShape* cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius); +/// Allocate and initialize a polygon shape with rounded corners. +/// The vertexes must be convex with a counter-clockwise winding. +CP_EXPORT cpShape* cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius); + +/// Initialize a box shaped polygon shape with rounded corners. +CP_EXPORT cpPolyShape* cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius); +/// Initialize an offset box shaped polygon shape with rounded corners. +CP_EXPORT cpPolyShape* cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius); +/// Allocate and initialize a box shaped polygon shape. +CP_EXPORT cpShape* cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius); +/// Allocate and initialize an offset box shaped polygon shape. +CP_EXPORT cpShape* cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius); + +/// Get the number of verts in a polygon shape. +CP_EXPORT int cpPolyShapeGetCount(const cpShape *shape); +/// Get the @c ith vertex of a polygon shape. +CP_EXPORT cpVect cpPolyShapeGetVert(const cpShape *shape, int index); +/// Get the radius of a polygon shape. +CP_EXPORT cpFloat cpPolyShapeGetRadius(const cpShape *shape); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpPolyline.h b/Chipmunk2D/include/chipmunk/cpPolyline.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpPolyline.h @@ -0,0 +1,70 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +// Polylines are just arrays of vertexes. +// They are looped if the first vertex is equal to the last. +// cpPolyline structs are intended to be passed by value and destroyed when you are done with them. +typedef struct cpPolyline { + int count, capacity; + cpVect verts[]; +} cpPolyline; + +/// Destroy and free a polyline instance. +CP_EXPORT void cpPolylineFree(cpPolyline *line); + +/// Returns true if the first vertex is equal to the last. +CP_EXPORT cpBool cpPolylineIsClosed(cpPolyline *line); + +/** + Returns a copy of a polyline simplified by using the Douglas-Peucker algorithm. + This works very well on smooth or gently curved shapes, but not well on straight edged or angular shapes. +*/ +CP_EXPORT cpPolyline *cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol); + +/** + Returns a copy of a polyline simplified by discarding "flat" vertexes. + This works well on straigt edged or angular shapes, not as well on smooth shapes. +*/ +CP_EXPORT cpPolyline *cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol); + +/// Get the convex hull of a polyline as a looped polyline. +CP_EXPORT cpPolyline *cpPolylineToConvexHull(cpPolyline *line, cpFloat tol); + + +/// Polyline sets are collections of polylines, generally built by cpMarchSoft() or cpMarchHard(). +typedef struct cpPolylineSet { + int count, capacity; + cpPolyline **lines; +} cpPolylineSet; + +/// Allocate a new polyline set. +CP_EXPORT cpPolylineSet *cpPolylineSetAlloc(void); + +/// Initialize a new polyline set. +CP_EXPORT cpPolylineSet *cpPolylineSetInit(cpPolylineSet *set); + +/// Allocate and initialize a polyline set. +CP_EXPORT cpPolylineSet *cpPolylineSetNew(void); + +/// Destroy a polyline set. +CP_EXPORT void cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines); + +/// Destroy and free a polyline set. +CP_EXPORT void cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines); + +/** + Add a line segment to a polyline set. + A segment will either start a new polyline, join two others, or add to or loop an existing polyline. + This is mostly intended to be used as a callback directly from cpMarchSoft() or cpMarchHard(). +*/ +CP_EXPORT void cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines); + +/** + Get an approximate convex decomposition from a polyline. + Returns a cpPolylineSet of convex hulls that match the original shape to within 'tol'. + NOTE: If the input is a self intersecting polygon, the output might end up overly simplified. +*/ + +CP_EXPORT cpPolylineSet *cpPolylineConvexDecomposition(cpPolyline *line, cpFloat tol); + +#define cpPolylineConvexDecomposition_BETA cpPolylineConvexDecomposition diff --git a/Chipmunk2D/include/chipmunk/cpRatchetJoint.h b/Chipmunk2D/include/chipmunk/cpRatchetJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpRatchetJoint.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpRatchetJoint cpRatchetJoint +/// @{ + +/// Check if a constraint is a damped rotary springs. +CP_EXPORT cpBool cpConstraintIsRatchetJoint(const cpConstraint *constraint); + +/// Allocate a ratchet joint. +CP_EXPORT cpRatchetJoint* cpRatchetJointAlloc(void); +/// Initialize a ratched joint. +CP_EXPORT cpRatchetJoint* cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); +/// Allocate and initialize a ratchet joint. +CP_EXPORT cpConstraint* cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); + +/// Get the angle of the current ratchet tooth. +CP_EXPORT cpFloat cpRatchetJointGetAngle(const cpConstraint *constraint); +/// Set the angle of the current ratchet tooth. +CP_EXPORT void cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle); + +/// Get the phase offset of the ratchet. +CP_EXPORT cpFloat cpRatchetJointGetPhase(const cpConstraint *constraint); +/// Get the phase offset of the ratchet. +CP_EXPORT void cpRatchetJointSetPhase(cpConstraint *constraint, cpFloat phase); + +/// Get the angular distance of each ratchet. +CP_EXPORT cpFloat cpRatchetJointGetRatchet(const cpConstraint *constraint); +/// Set the angular distance of each ratchet. +CP_EXPORT void cpRatchetJointSetRatchet(cpConstraint *constraint, cpFloat ratchet); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpRobust.h b/Chipmunk2D/include/chipmunk/cpRobust.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpRobust.h @@ -0,0 +1,11 @@ +#include "cpVect.h" + +// This is a private header for functions (currently just one) that need strict floating point results. +// It was easier to put this in it's own file than to fiddle with 4 different compiler specific pragmas or attributes. +// "Fast math" should be disabled here. + +// Check if c is to the left of segment (a, b). +cpBool cpCheckPointGreater(const cpVect a, const cpVect b, const cpVect c); + +// Check if p is behind one of v0 or v1 on axis n. +cpBool cpCheckAxis(cpVect v0, cpVect v1, cpVect p, cpVect n); diff --git a/Chipmunk2D/include/chipmunk/cpRotaryLimitJoint.h b/Chipmunk2D/include/chipmunk/cpRotaryLimitJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpRotaryLimitJoint.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint +/// @{ + +/// Check if a constraint is a damped rotary springs. +CP_EXPORT cpBool cpConstraintIsRotaryLimitJoint(const cpConstraint *constraint); + +/// Allocate a damped rotary limit joint. +CP_EXPORT cpRotaryLimitJoint* cpRotaryLimitJointAlloc(void); +/// Initialize a damped rotary limit joint. +CP_EXPORT cpRotaryLimitJoint* cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); +/// Allocate and initialize a damped rotary limit joint. +CP_EXPORT cpConstraint* cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); + +/// Get the minimum distance the joint will maintain between the two anchors. +CP_EXPORT cpFloat cpRotaryLimitJointGetMin(const cpConstraint *constraint); +/// Set the minimum distance the joint will maintain between the two anchors. +CP_EXPORT void cpRotaryLimitJointSetMin(cpConstraint *constraint, cpFloat min); + +/// Get the maximum distance the joint will maintain between the two anchors. +CP_EXPORT cpFloat cpRotaryLimitJointGetMax(const cpConstraint *constraint); +/// Set the maximum distance the joint will maintain between the two anchors. +CP_EXPORT void cpRotaryLimitJointSetMax(cpConstraint *constraint, cpFloat max); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpShape.h b/Chipmunk2D/include/chipmunk/cpShape.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpShape.h @@ -0,0 +1,199 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpShape cpShape +/// The cpShape struct defines the shape of a rigid body. +/// @{ + +/// Point query info struct. +typedef struct cpPointQueryInfo { + /// The nearest shape, NULL if no shape was within range. + const cpShape *shape; + /// The closest point on the shape's surface. (in world space coordinates) + cpVect point; + /// The distance to the point. The distance is negative if the point is inside the shape. + cpFloat distance; + /// The gradient of the signed distance function. + /// The value should be similar to info.p/info.d, but accurate even for very small values of info.d. + cpVect gradient; +} cpPointQueryInfo; + +/// Segment query info struct. +typedef struct cpSegmentQueryInfo { + /// The shape that was hit, or NULL if no collision occured. + const cpShape *shape; + /// The point of impact. + cpVect point; + /// The normal of the surface hit. + cpVect normal; + /// The normalized distance along the query segment in the range [0, 1]. + cpFloat alpha; +} cpSegmentQueryInfo; + +/// Fast collision filtering type that is used to determine if two objects collide before calling collision or query callbacks. +typedef struct cpShapeFilter { + /// Two objects with the same non-zero group value do not collide. + /// This is generally used to group objects in a composite object together to disable self collisions. + cpGroup group; + /// A bitmask of user definable categories that this object belongs to. + /// The category/mask combinations of both objects in a collision must agree for a collision to occur. + cpBitmask categories; + /// A bitmask of user definable category types that this object object collides with. + /// The category/mask combinations of both objects in a collision must agree for a collision to occur. + cpBitmask mask; +} cpShapeFilter; + +/// Collision filter value for a shape that will collide with anything except CP_SHAPE_FILTER_NONE. +static const cpShapeFilter CP_SHAPE_FILTER_ALL = {CP_NO_GROUP, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES}; +/// Collision filter value for a shape that does not collide with anything. +static const cpShapeFilter CP_SHAPE_FILTER_NONE = {CP_NO_GROUP, ~CP_ALL_CATEGORIES, ~CP_ALL_CATEGORIES}; + +/// Create a new collision filter. +static inline cpShapeFilter +cpShapeFilterNew(cpGroup group, cpBitmask categories, cpBitmask mask) +{ + cpShapeFilter filter = {group, categories, mask}; + return filter; +} + +/// Destroy a shape. +CP_EXPORT void cpShapeDestroy(cpShape *shape); +/// Destroy and Free a shape. +CP_EXPORT void cpShapeFree(cpShape *shape); + +/// Update, cache and return the bounding box of a shape based on the body it's attached to. +CP_EXPORT cpBB cpShapeCacheBB(cpShape *shape); +/// Update, cache and return the bounding box of a shape with an explicit transformation. +CP_EXPORT cpBB cpShapeUpdate(cpShape *shape, cpTransform transform); + +/// 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. +CP_EXPORT cpFloat cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *out); + +/// Perform a segment query against a shape. @c info must be a pointer to a valid cpSegmentQueryInfo structure. +CP_EXPORT cpBool cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info); + +/// Return contact information about two shapes. +CP_EXPORT cpContactPointSet cpShapesCollide(const cpShape *a, const cpShape *b); + +/// The cpSpace this body is added to. +CP_EXPORT cpSpace* cpShapeGetSpace(const cpShape *shape); + +/// The cpBody this shape is connected to. +CP_EXPORT cpBody* cpShapeGetBody(const cpShape *shape); +/// Set the cpBody this shape is connected to. +/// Can only be used if the shape is not currently added to a space. +CP_EXPORT void cpShapeSetBody(cpShape *shape, cpBody *body); + +/// Get the mass of the shape if you are having Chipmunk calculate mass properties for you. +CP_EXPORT cpFloat cpShapeGetMass(cpShape *shape); +/// Set the mass of this shape to have Chipmunk calculate mass properties for you. +CP_EXPORT void cpShapeSetMass(cpShape *shape, cpFloat mass); + +/// Get the density of the shape if you are having Chipmunk calculate mass properties for you. +CP_EXPORT cpFloat cpShapeGetDensity(cpShape *shape); +/// Set the density of this shape to have Chipmunk calculate mass properties for you. +CP_EXPORT void cpShapeSetDensity(cpShape *shape, cpFloat density); + +/// Get the calculated moment of inertia for this shape. +CP_EXPORT cpFloat cpShapeGetMoment(cpShape *shape); +/// Get the calculated area of this shape. +CP_EXPORT cpFloat cpShapeGetArea(cpShape *shape); +/// Get the centroid of this shape. +CP_EXPORT cpVect cpShapeGetCenterOfGravity(cpShape *shape); + +/// Get the bounding box that contains the shape given it's current position and angle. +CP_EXPORT cpBB cpShapeGetBB(const cpShape *shape); + +/// Get if the shape is set to be a sensor or not. +CP_EXPORT cpBool cpShapeGetSensor(const cpShape *shape); +/// Set if the shape is a sensor or not. +CP_EXPORT void cpShapeSetSensor(cpShape *shape, cpBool sensor); + +/// Get the elasticity of this shape. +CP_EXPORT cpFloat cpShapeGetElasticity(const cpShape *shape); +/// Set the elasticity of this shape. +CP_EXPORT void cpShapeSetElasticity(cpShape *shape, cpFloat elasticity); + +/// Get the friction of this shape. +CP_EXPORT cpFloat cpShapeGetFriction(const cpShape *shape); +/// Set the friction of this shape. +CP_EXPORT void cpShapeSetFriction(cpShape *shape, cpFloat friction); + +/// Get the surface velocity of this shape. +CP_EXPORT cpVect cpShapeGetSurfaceVelocity(const cpShape *shape); +/// Set the surface velocity of this shape. +CP_EXPORT void cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity); + +/// Get the user definable data pointer of this shape. +CP_EXPORT cpDataPointer cpShapeGetUserData(const cpShape *shape); +/// Set the user definable data pointer of this shape. +CP_EXPORT void cpShapeSetUserData(cpShape *shape, cpDataPointer userData); + +/// Set the collision type of this shape. +CP_EXPORT cpCollisionType cpShapeGetCollisionType(const cpShape *shape); +/// Get the collision type of this shape. +CP_EXPORT void cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType); + +/// Get the collision filtering parameters of this shape. +CP_EXPORT cpShapeFilter cpShapeGetFilter(const cpShape *shape); +/// Set the collision filtering parameters of this shape. +CP_EXPORT void cpShapeSetFilter(cpShape *shape, cpShapeFilter filter); + + +/// @} +/// @defgroup cpCircleShape cpCircleShape + +/// Allocate a circle shape. +CP_EXPORT cpCircleShape* cpCircleShapeAlloc(void); +/// Initialize a circle shape. +CP_EXPORT cpCircleShape* cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); +/// Allocate and initialize a circle shape. +CP_EXPORT cpShape* cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); + +/// Get the offset of a circle shape. +CP_EXPORT cpVect cpCircleShapeGetOffset(const cpShape *shape); +/// Get the radius of a circle shape. +CP_EXPORT cpFloat cpCircleShapeGetRadius(const cpShape *shape); + +/// @} +/// @defgroup cpSegmentShape cpSegmentShape + +/// Allocate a segment shape. +CP_EXPORT cpSegmentShape* cpSegmentShapeAlloc(void); +/// Initialize a segment shape. +CP_EXPORT cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); +/// Allocate and initialize a segment shape. +CP_EXPORT cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); + +/// Let Chipmunk know about the geometry of adjacent segments to avoid colliding with endcaps. +CP_EXPORT void cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next); + +/// Get the first endpoint of a segment shape. +CP_EXPORT cpVect cpSegmentShapeGetA(const cpShape *shape); +/// Get the second endpoint of a segment shape. +CP_EXPORT cpVect cpSegmentShapeGetB(const cpShape *shape); +/// Get the normal of a segment shape. +CP_EXPORT cpVect cpSegmentShapeGetNormal(const cpShape *shape); +/// Get the first endpoint of a segment shape. +CP_EXPORT cpFloat cpSegmentShapeGetRadius(const cpShape *shape); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpSimpleMotor.h b/Chipmunk2D/include/chipmunk/cpSimpleMotor.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpSimpleMotor.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpSimpleMotor cpSimpleMotor +/// @{ + +/// Opaque struct type for damped rotary springs. +typedef struct cpSimpleMotor cpSimpleMotor; + +/// Check if a constraint is a damped rotary springs. +CP_EXPORT cpBool cpConstraintIsSimpleMotor(const cpConstraint *constraint); + +/// Allocate a simple motor. +CP_EXPORT cpSimpleMotor* cpSimpleMotorAlloc(void); +/// initialize a simple motor. +CP_EXPORT cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); +/// Allocate and initialize a simple motor. +CP_EXPORT cpConstraint* cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); + +/// Get the rate of the motor. +CP_EXPORT cpFloat cpSimpleMotorGetRate(const cpConstraint *constraint); +/// Set the rate of the motor. +CP_EXPORT void cpSimpleMotorSetRate(cpConstraint *constraint, cpFloat rate); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpSlideJoint.h b/Chipmunk2D/include/chipmunk/cpSlideJoint.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpSlideJoint.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpSlideJoint cpSlideJoint +/// @{ + +/// Check if a constraint is a slide joint. +CP_EXPORT cpBool cpConstraintIsSlideJoint(const cpConstraint *constraint); + +/// Allocate a slide joint. +CP_EXPORT cpSlideJoint* cpSlideJointAlloc(void); +/// Initialize a slide joint. +CP_EXPORT cpSlideJoint* cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max); +/// Allocate and initialize a slide joint. +CP_EXPORT cpConstraint* cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max); + +/// Get the location of the first anchor relative to the first body. +CP_EXPORT cpVect cpSlideJointGetAnchorA(const cpConstraint *constraint); +/// Set the location of the first anchor relative to the first body. +CP_EXPORT void cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); + +/// Get the location of the second anchor relative to the second body. +CP_EXPORT cpVect cpSlideJointGetAnchorB(const cpConstraint *constraint); +/// Set the location of the second anchor relative to the second body. +CP_EXPORT void cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); + +/// Get the minimum distance the joint will maintain between the two anchors. +CP_EXPORT cpFloat cpSlideJointGetMin(const cpConstraint *constraint); +/// Set the minimum distance the joint will maintain between the two anchors. +CP_EXPORT void cpSlideJointSetMin(cpConstraint *constraint, cpFloat min); + +/// Get the maximum distance the joint will maintain between the two anchors. +CP_EXPORT cpFloat cpSlideJointGetMax(const cpConstraint *constraint); +/// Set the maximum distance the joint will maintain between the two anchors. +CP_EXPORT void cpSlideJointSetMax(cpConstraint *constraint, cpFloat max); + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpSpace.h b/Chipmunk2D/include/chipmunk/cpSpace.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpSpace.h @@ -0,0 +1,319 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// @defgroup cpSpace cpSpace +/// @{ + +//MARK: Definitions + +/// Collision begin event function callback type. +/// Returning false from a begin callback causes the collision to be ignored until +/// the the separate callback is called when the objects stop colliding. +typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); +/// Collision pre-solve event function callback type. +/// Returning false from a pre-step callback causes the collision to be ignored until the next step. +typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); +/// Collision post-solve event function callback type. +typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); +/// Collision separate event function callback type. +typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); + +/// Struct that holds function callback pointers to configure custom collision handling. +/// Collision handlers have a pair of types; when a collision occurs between two shapes that have these types, the collision handler functions are triggered. +struct cpCollisionHandler { + /// Collision type identifier of the first shape that this handler recognizes. + /// In the collision handler callback, the shape with this type will be the first argument. Read only. + const cpCollisionType typeA; + /// Collision type identifier of the second shape that this handler recognizes. + /// In the collision handler callback, the shape with this type will be the second argument. Read only. + const cpCollisionType typeB; + /// This function is called when two shapes with types that match this collision handler begin colliding. + cpCollisionBeginFunc beginFunc; + /// This function is called each step when two shapes with types that match this collision handler are colliding. + /// It's called before the collision solver runs so that you can affect a collision's outcome. + cpCollisionPreSolveFunc preSolveFunc; + /// This function is called each step when two shapes with types that match this collision handler are colliding. + /// It's called after the collision solver runs so that you can read back information about the collision to trigger events in your game. + cpCollisionPostSolveFunc postSolveFunc; + /// This function is called when two shapes with types that match this collision handler stop colliding. + cpCollisionSeparateFunc separateFunc; + /// This is a user definable context pointer that is passed to all of the collision handler functions. + cpDataPointer userData; +}; + +// TODO: Make timestep a parameter? + + +//MARK: Memory and Initialization + +/// Allocate a cpSpace. +CP_EXPORT cpSpace* cpSpaceAlloc(void); +/// Initialize a cpSpace. +CP_EXPORT cpSpace* cpSpaceInit(cpSpace *space); +/// Allocate and initialize a cpSpace. +CP_EXPORT cpSpace* cpSpaceNew(void); + +/// Destroy a cpSpace. +CP_EXPORT void cpSpaceDestroy(cpSpace *space); +/// Destroy and free a cpSpace. +CP_EXPORT void cpSpaceFree(cpSpace *space); + + +//MARK: Properties + +/// Number of iterations to use in the impulse solver to solve contacts and other constraints. +CP_EXPORT int cpSpaceGetIterations(const cpSpace *space); +CP_EXPORT void cpSpaceSetIterations(cpSpace *space, int iterations); + +/// Gravity to pass to rigid bodies when integrating velocity. +CP_EXPORT cpVect cpSpaceGetGravity(const cpSpace *space); +CP_EXPORT void cpSpaceSetGravity(cpSpace *space, cpVect gravity); + +/// Damping rate expressed as the fraction of velocity bodies retain each second. +/// A value of 0.9 would mean that each body's velocity will drop 10% per second. +/// The default value is 1.0, meaning no damping is applied. +/// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring. +CP_EXPORT cpFloat cpSpaceGetDamping(const cpSpace *space); +CP_EXPORT void cpSpaceSetDamping(cpSpace *space, cpFloat damping); + +/// Speed threshold for a body to be considered idle. +/// The default value of 0 means to let the space guess a good threshold based on gravity. +CP_EXPORT cpFloat cpSpaceGetIdleSpeedThreshold(const cpSpace *space); +CP_EXPORT void cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold); + +/// Time a group of bodies must remain idle in order to fall asleep. +/// Enabling sleeping also implicitly enables the the contact graph. +/// The default value of INFINITY disables the sleeping algorithm. +CP_EXPORT cpFloat cpSpaceGetSleepTimeThreshold(const cpSpace *space); +CP_EXPORT void cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold); + +/// 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. +CP_EXPORT cpFloat cpSpaceGetCollisionSlop(const cpSpace *space); +CP_EXPORT void cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop); + +/// Determines how fast overlapping shapes are pushed apart. +/// Expressed as a fraction of the error remaining after each second. +/// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz. +CP_EXPORT cpFloat cpSpaceGetCollisionBias(const cpSpace *space); +CP_EXPORT void cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias); + +/// Number of frames that contact information should persist. +/// Defaults to 3. There is probably never a reason to change this value. +CP_EXPORT cpTimestamp cpSpaceGetCollisionPersistence(const cpSpace *space); +CP_EXPORT void cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence); + +/// User definable data pointer. +/// Generally this points to your game's controller or game state +/// class so you can access it when given a cpSpace reference in a callback. +CP_EXPORT cpDataPointer cpSpaceGetUserData(const cpSpace *space); +CP_EXPORT void cpSpaceSetUserData(cpSpace *space, cpDataPointer userData); + +/// The Space provided static body for a given cpSpace. +/// This is merely provided for convenience and you are not required to use it. +CP_EXPORT cpBody* cpSpaceGetStaticBody(const cpSpace *space); + +/// Returns the current (or most recent) time step used with the given space. +/// Useful from callbacks if your time step is not a compile-time global. +CP_EXPORT cpFloat cpSpaceGetCurrentTimeStep(const cpSpace *space); + +/// returns true from inside a callback when objects cannot be added/removed. +CP_EXPORT cpBool cpSpaceIsLocked(cpSpace *space); + + +//MARK: Collision Handlers + +/// Create or return the existing collision handler that is called for all collisions that are not handled by a more specific collision handler. +CP_EXPORT cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space); +/// Create or return the existing collision handler for the specified pair of collision types. +/// If wildcard handlers are used with either of the collision types, it's the responibility of the custom handler to invoke the wildcard handlers. +CP_EXPORT cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); +/// Create or return the existing wildcard collision handler for the specified type. +CP_EXPORT cpCollisionHandler *cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type); + + +//MARK: Add/Remove objects + +/// Add a collision shape to the simulation. +/// If the shape is attached to a static body, it will be added as a static shape. +CP_EXPORT cpShape* cpSpaceAddShape(cpSpace *space, cpShape *shape); +/// Add a rigid body to the simulation. +CP_EXPORT cpBody* cpSpaceAddBody(cpSpace *space, cpBody *body); +/// Add a constraint to the simulation. +CP_EXPORT cpConstraint* cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); + +/// Remove a collision shape from the simulation. +CP_EXPORT void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); +/// Remove a rigid body from the simulation. +CP_EXPORT void cpSpaceRemoveBody(cpSpace *space, cpBody *body); +/// Remove a constraint from the simulation. +CP_EXPORT void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); + +/// Test if a collision shape has been added to the space. +CP_EXPORT cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape); +/// Test if a rigid body has been added to the space. +CP_EXPORT cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body); +/// Test if a constraint has been added to the space. +CP_EXPORT cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint); + +//MARK: Post-Step Callbacks + +/// Post Step callback function type. +typedef void (*cpPostStepFunc)(cpSpace *space, void *key, void *data); +/// Schedule a post-step callback to be called when cpSpaceStep() finishes. +/// 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. +CP_EXPORT cpBool cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data); + + +//MARK: Queries + +// TODO: Queries and iterators should take a cpSpace parametery. +// TODO: They should also be abortable. + +/// Nearest point query callback function type. +typedef void (*cpSpacePointQueryFunc)(cpShape *shape, cpVect point, cpFloat distance, cpVect gradient, void *data); +/// Query the space at a point and call @c func for each shape found. +CP_EXPORT void cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data); +/// Query the space at a point and return the nearest shape found. Returns NULL if no shapes were found. +CP_EXPORT cpShape *cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out); + +/// Segment query callback function type. +typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpVect point, cpVect normal, cpFloat alpha, void *data); +/// Perform a directed line segment query (like a raycast) against the space calling @c func for each shape intersected. +CP_EXPORT void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data); +/// Perform a directed line segment query (like a raycast) against the space and return the first shape hit. Returns NULL if no shapes were hit. +CP_EXPORT cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out); + +/// Rectangle Query callback function type. +typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); +/// Perform a fast rectangle query on the space calling @c func for each shape found. +/// Only the shape's bounding boxes are checked for overlap, not their full shape. +CP_EXPORT void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data); + +/// Shape query callback function type. +typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); +/// Query a space for any shapes overlapping the given shape and call @c func for each shape found. +CP_EXPORT cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); + + +//MARK: Iteration + +/// Space/body iterator callback function type. +typedef void (*cpSpaceBodyIteratorFunc)(cpBody *body, void *data); +/// Call @c func for each body in the space. +CP_EXPORT void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data); + +/// Space/body iterator callback function type. +typedef void (*cpSpaceShapeIteratorFunc)(cpShape *shape, void *data); +/// Call @c func for each shape in the space. +CP_EXPORT void cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data); + +/// Space/constraint iterator callback function type. +typedef void (*cpSpaceConstraintIteratorFunc)(cpConstraint *constraint, void *data); +/// Call @c func for each shape in the space. +CP_EXPORT void cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data); + + +//MARK: Indexing + +/// Update the collision detection info for the static shapes in the space. +CP_EXPORT void cpSpaceReindexStatic(cpSpace *space); +/// Update the collision detection data for a specific shape in the space. +CP_EXPORT void cpSpaceReindexShape(cpSpace *space, cpShape *shape); +/// Update the collision detection data for all shapes attached to a body. +CP_EXPORT void cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body); + +/// Switch the space to use a spatial has as it's spatial index. +CP_EXPORT void cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count); + + +//MARK: Time Stepping + +/// Step the space forward in time by @c dt. +CP_EXPORT void cpSpaceStep(cpSpace *space, cpFloat dt); + + +//MARK: Debug API + +#ifndef CP_SPACE_DISABLE_DEBUG_API + +/// Color type to use with the space debug drawing API. +typedef struct cpSpaceDebugColor { + float r, g, b, a; +} cpSpaceDebugColor; + +/// Callback type for a function that draws a filled, stroked circle. +typedef void (*cpSpaceDebugDrawCircleImpl)(cpVect pos, cpFloat angle, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); +/// Callback type for a function that draws a line segment. +typedef void (*cpSpaceDebugDrawSegmentImpl)(cpVect a, cpVect b, cpSpaceDebugColor color, cpDataPointer data); +/// Callback type for a function that draws a thick line segment. +typedef void (*cpSpaceDebugDrawFatSegmentImpl)(cpVect a, cpVect b, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); +/// Callback type for a function that draws a convex polygon. +typedef void (*cpSpaceDebugDrawPolygonImpl)(int count, const cpVect *verts, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); +/// Callback type for a function that draws a dot. +typedef void (*cpSpaceDebugDrawDotImpl)(cpFloat size, cpVect pos, cpSpaceDebugColor color, cpDataPointer data); +/// Callback type for a function that returns a color for a given shape. This gives you an opportunity to color shapes based on how they are used in your engine. +typedef cpSpaceDebugColor (*cpSpaceDebugDrawColorForShapeImpl)(cpShape *shape, cpDataPointer data); + +typedef enum cpSpaceDebugDrawFlags { + CP_SPACE_DEBUG_DRAW_SHAPES = 1<<0, + CP_SPACE_DEBUG_DRAW_CONSTRAINTS = 1<<1, + CP_SPACE_DEBUG_DRAW_COLLISION_POINTS = 1<<2, +} cpSpaceDebugDrawFlags; + +/// Struct used with cpSpaceDebugDraw() containing drawing callbacks and other drawing settings. +typedef struct cpSpaceDebugDrawOptions { + /// Function that will be invoked to draw circles. + cpSpaceDebugDrawCircleImpl drawCircle; + /// Function that will be invoked to draw line segments. + cpSpaceDebugDrawSegmentImpl drawSegment; + /// Function that will be invoked to draw thick line segments. + cpSpaceDebugDrawFatSegmentImpl drawFatSegment; + /// Function that will be invoked to draw convex polygons. + cpSpaceDebugDrawPolygonImpl drawPolygon; + /// Function that will be invoked to draw dots. + cpSpaceDebugDrawDotImpl drawDot; + + /// Flags that request which things to draw (collision shapes, constraints, contact points). + cpSpaceDebugDrawFlags flags; + /// Outline color passed to the drawing function. + cpSpaceDebugColor shapeOutlineColor; + /// Function that decides what fill color to draw shapes using. + cpSpaceDebugDrawColorForShapeImpl colorForShape; + /// Color passed to drawing functions for constraints. + cpSpaceDebugColor constraintColor; + /// Color passed to drawing functions for collision points. + cpSpaceDebugColor collisionPointColor; + + /// User defined context pointer passed to all of the callback functions as the 'data' argument. + cpDataPointer data; +} cpSpaceDebugDrawOptions; + +/// Debug draw the current state of the space using the supplied drawing options. +CP_EXPORT void cpSpaceDebugDraw(cpSpace *space, cpSpaceDebugDrawOptions *options); + +#endif + +/// @} diff --git a/Chipmunk2D/include/chipmunk/cpSpatialIndex.h b/Chipmunk2D/include/chipmunk/cpSpatialIndex.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpSpatialIndex.h @@ -0,0 +1,227 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + @defgroup cpSpatialIndex cpSpatialIndex + + Spatial indexes are data structures that are used to accelerate collision detection + and spatial queries. Chipmunk provides a number of spatial index algorithms to pick from + and they are programmed in a generic way so that you can use them for holding more than + just cpShape structs. + + It works by using @c void pointers to the objects you add and using a callback to ask your code + for bounding boxes when it needs them. Several types of queries can be performed an index as well + as reindexing and full collision information. All communication to the spatial indexes is performed + through callback functions. + + Spatial indexes should be treated as opaque structs. + This meanns you shouldn't be reading any of the struct fields. + @{ +*/ + +//MARK: Spatial Index + +/// Spatial index bounding box callback function type. +/// The spatial index calls this function and passes you a pointer to an object you added +/// when it needs to get the bounding box associated with that object. +typedef cpBB (*cpSpatialIndexBBFunc)(void *obj); +/// Spatial index/object iterator callback function type. +typedef void (*cpSpatialIndexIteratorFunc)(void *obj, void *data); +/// Spatial query callback function type. +typedef cpCollisionID (*cpSpatialIndexQueryFunc)(void *obj1, void *obj2, cpCollisionID id, void *data); +/// Spatial segment query callback function type. +typedef cpFloat (*cpSpatialIndexSegmentQueryFunc)(void *obj1, void *obj2, void *data); + + +typedef struct cpSpatialIndexClass cpSpatialIndexClass; +typedef struct cpSpatialIndex cpSpatialIndex; + +/// @private +struct cpSpatialIndex { + cpSpatialIndexClass *klass; + + cpSpatialIndexBBFunc bbfunc; + + cpSpatialIndex *staticIndex, *dynamicIndex; +}; + + +//MARK: Spatial Hash + +typedef struct cpSpaceHash cpSpaceHash; + +/// Allocate a spatial hash. +CP_EXPORT cpSpaceHash* cpSpaceHashAlloc(void); +/// Initialize a spatial hash. +CP_EXPORT cpSpatialIndex* cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a spatial hash. +CP_EXPORT cpSpatialIndex* cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +/// Change the cell dimensions and table size of the spatial hash to tune it. +/// The cell dimensions should roughly match the average size of your objects +/// and the table size should be ~10 larger than the number of objects inserted. +/// Some trial and error is required to find the optimum numbers for efficiency. +CP_EXPORT void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); + +//MARK: AABB Tree + +typedef struct cpBBTree cpBBTree; + +/// Allocate a bounding box tree. +CP_EXPORT cpBBTree* cpBBTreeAlloc(void); +/// Initialize a bounding box tree. +CP_EXPORT cpSpatialIndex* cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a bounding box tree. +CP_EXPORT cpSpatialIndex* cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +/// Perform a static top down optimization of the tree. +CP_EXPORT void cpBBTreeOptimize(cpSpatialIndex *index); + +/// Bounding box tree velocity callback function. +/// This function should return an estimate for the object's velocity. +typedef cpVect (*cpBBTreeVelocityFunc)(void *obj); +/// Set the velocity function for the bounding box tree to enable temporal coherence. +CP_EXPORT void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func); + +//MARK: Single Axis Sweep + +typedef struct cpSweep1D cpSweep1D; + +/// Allocate a 1D sort and sweep broadphase. +CP_EXPORT cpSweep1D* cpSweep1DAlloc(void); +/// Initialize a 1D sort and sweep broadphase. +CP_EXPORT cpSpatialIndex* cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +/// Allocate and initialize a 1D sort and sweep broadphase. +CP_EXPORT cpSpatialIndex* cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +//MARK: Spatial Index Implementation + +typedef void (*cpSpatialIndexDestroyImpl)(cpSpatialIndex *index); + +typedef int (*cpSpatialIndexCountImpl)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexEachImpl)(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data); + +typedef cpBool (*cpSpatialIndexContainsImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexInsertImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexRemoveImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); + +typedef void (*cpSpatialIndexReindexImpl)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexReindexObjectImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexReindexQueryImpl)(cpSpatialIndex *index, cpSpatialIndexQueryFunc 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; + + cpSpatialIndexCountImpl count; + cpSpatialIndexEachImpl each; + + cpSpatialIndexContainsImpl contains; + cpSpatialIndexInsertImpl insert; + cpSpatialIndexRemoveImpl remove; + + cpSpatialIndexReindexImpl reindex; + cpSpatialIndexReindexObjectImpl reindexObject; + cpSpatialIndexReindexQueryImpl reindexQuery; + + cpSpatialIndexQueryImpl query; + cpSpatialIndexSegmentQueryImpl segmentQuery; +}; + +/// Destroy and free a spatial index. +CP_EXPORT void cpSpatialIndexFree(cpSpatialIndex *index); +/// Collide the objects in @c dynamicIndex against the objects in @c staticIndex using the query callback function. +CP_EXPORT void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data); + +/// Destroy a spatial index. +static inline void cpSpatialIndexDestroy(cpSpatialIndex *index) +{ + if(index->klass) index->klass->destroy(index); +} + +/// Get the number of objects in the spatial index. +static inline int cpSpatialIndexCount(cpSpatialIndex *index) +{ + return index->klass->count(index); +} + +/// Iterate the objects in the spatial index. @c func will be called once for each object. +static inline void cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data) +{ + index->klass->each(index, func, data); +} + +/// Returns true if the spatial index contains the given object. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. +static inline cpBool cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + return index->klass->contains(index, obj, hashid); +} + +/// Add an object to a spatial index. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. +static inline void cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->insert(index, obj, hashid); +} + +/// Remove an object from a spatial index. +/// Most spatial indexes use hashed storage, so you must provide a hash value too. +static inline void cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->remove(index, obj, hashid); +} + +/// Perform a full reindex of a spatial index. +static inline void cpSpatialIndexReindex(cpSpatialIndex *index) +{ + index->klass->reindex(index); +} + +/// Reindex a single object in the spatial index. +static inline void cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->reindexObject(index, obj, hashid); +} + +/// 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); +} + +/// Perform a segment query against the spatial index, calling @c func for each potential match. +static inline void cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) +{ + index->klass->segmentQuery(index, obj, a, b, t_exit, func, data); +} + +/// Simultaneously reindex and find all colliding objects. +/// @c func will be called once for each potentially overlapping pair of objects found. +/// If the spatial index was initialized with a static index, it will collide it's objects against that as well. +static inline void cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data) +{ + index->klass->reindexQuery(index, func, data); +} + +///@} diff --git a/Chipmunk2D/include/chipmunk/cpTransform.h b/Chipmunk2D/include/chipmunk/cpTransform.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpTransform.h @@ -0,0 +1,198 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_TRANSFORM_H +#define CHIPMUNK_TRANSFORM_H + +#include "chipmunk_types.h" +#include "cpVect.h" +#include "cpBB.h" + +/// Identity transform matrix. +static const cpTransform cpTransformIdentity = {1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f}; + +/// Construct a new transform matrix. +/// (a, b) is the x basis vector. +/// (c, d) is the y basis vector. +/// (tx, ty) is the translation. +static inline cpTransform +cpTransformNew(cpFloat a, cpFloat b, cpFloat c, cpFloat d, cpFloat tx, cpFloat ty) +{ + cpTransform t = {a, b, c, d, tx, ty}; + return t; +} + +/// Construct a new transform matrix in transposed order. +static inline cpTransform +cpTransformNewTranspose(cpFloat a, cpFloat c, cpFloat tx, cpFloat b, cpFloat d, cpFloat ty) +{ + cpTransform t = {a, b, c, d, tx, ty}; + return t; +} + +/// Get the inverse of a transform matrix. +static inline cpTransform +cpTransformInverse(cpTransform t) +{ + cpFloat inv_det = 1.0/(t.a*t.d - t.c*t.b); + return cpTransformNewTranspose( + t.d*inv_det, -t.c*inv_det, (t.c*t.ty - t.tx*t.d)*inv_det, + -t.b*inv_det, t.a*inv_det, (t.tx*t.b - t.a*t.ty)*inv_det + ); +} + +/// Multiply two transformation matrices. +static inline cpTransform +cpTransformMult(cpTransform t1, cpTransform t2) +{ + return cpTransformNewTranspose( + t1.a*t2.a + t1.c*t2.b, t1.a*t2.c + t1.c*t2.d, t1.a*t2.tx + t1.c*t2.ty + t1.tx, + t1.b*t2.a + t1.d*t2.b, t1.b*t2.c + t1.d*t2.d, t1.b*t2.tx + t1.d*t2.ty + t1.ty + ); +} + +/// Transform an absolute point. (i.e. a vertex) +static inline cpVect +cpTransformPoint(cpTransform t, cpVect p) +{ + return cpv(t.a*p.x + t.c*p.y + t.tx, t.b*p.x + t.d*p.y + t.ty); +} + +/// Transform a vector (i.e. a normal) +static inline cpVect +cpTransformVect(cpTransform t, cpVect v) +{ + return cpv(t.a*v.x + t.c*v.y, t.b*v.x + t.d*v.y); +} + +/// Transform a cpBB. +static inline cpBB +cpTransformbBB(cpTransform t, cpBB bb) +{ + cpVect center = cpBBCenter(bb); + cpFloat hw = (bb.r - bb.l)*0.5; + cpFloat hh = (bb.t - bb.b)*0.5; + + cpFloat a = t.a*hw, b = t.c*hh, d = t.b*hw, e = t.d*hh; + cpFloat hw_max = cpfmax(cpfabs(a + b), cpfabs(a - b)); + cpFloat hh_max = cpfmax(cpfabs(d + e), cpfabs(d - e)); + return cpBBNewForExtents(cpTransformPoint(t, center), hw_max, hh_max); +} + +/// Create a transation matrix. +static inline cpTransform +cpTransformTranslate(cpVect translate) +{ + return cpTransformNewTranspose( + 1.0, 0.0, translate.x, + 0.0, 1.0, translate.y + ); +} + +/// Create a scale matrix. +static inline cpTransform +cpTransformScale(cpFloat scaleX, cpFloat scaleY) +{ + return cpTransformNewTranspose( + scaleX, 0.0, 0.0, + 0.0, scaleY, 0.0 + ); +} + +/// Create a rotation matrix. +static inline cpTransform +cpTransformRotate(cpFloat radians) +{ + cpVect rot = cpvforangle(radians); + return cpTransformNewTranspose( + rot.x, -rot.y, 0.0, + rot.y, rot.x, 0.0 + ); +} + +/// Create a rigid transformation matrix. (transation + rotation) +static inline cpTransform +cpTransformRigid(cpVect translate, cpFloat radians) +{ + cpVect rot = cpvforangle(radians); + return cpTransformNewTranspose( + rot.x, -rot.y, translate.x, + rot.y, rot.x, translate.y + ); +} + +/// Fast inverse of a rigid transformation matrix. +static inline cpTransform +cpTransformRigidInverse(cpTransform t) +{ + return cpTransformNewTranspose( + t.d, -t.c, (t.c*t.ty - t.tx*t.d), + -t.b, t.a, (t.tx*t.b - t.a*t.ty) + ); +} + +//MARK: Miscellaneous (but useful) transformation matrices. +// See source for documentation... + +static inline cpTransform +cpTransformWrap(cpTransform outer, cpTransform inner) +{ + return cpTransformMult(cpTransformInverse(outer), cpTransformMult(inner, outer)); +} + +static inline cpTransform +cpTransformWrapInverse(cpTransform outer, cpTransform inner) +{ + return cpTransformMult(outer, cpTransformMult(inner, cpTransformInverse(outer))); +} + +static inline cpTransform +cpTransformOrtho(cpBB bb) +{ + return cpTransformNewTranspose( + 2.0/(bb.r - bb.l), 0.0, -(bb.r + bb.l)/(bb.r - bb.l), + 0.0, 2.0/(bb.t - bb.b), -(bb.t + bb.b)/(bb.t - bb.b) + ); +} + +static inline cpTransform +cpTransformBoneScale(cpVect v0, cpVect v1) +{ + cpVect d = cpvsub(v1, v0); + return cpTransformNewTranspose( + d.x, -d.y, v0.x, + d.y, d.x, v0.y + ); +} + +static inline cpTransform +cpTransformAxialScale(cpVect axis, cpVect pivot, cpFloat scale) +{ + cpFloat A = axis.x*axis.y*(scale - 1.0); + cpFloat B = cpvdot(axis, pivot)*(1.0 - scale); + + return cpTransformNewTranspose( + scale*axis.x*axis.x + axis.y*axis.y, A, axis.x*B, + A, axis.x*axis.x + scale*axis.y*axis.y, axis.y*B + ); +} + +#endif diff --git a/Chipmunk2D/include/chipmunk/cpVect.h b/Chipmunk2D/include/chipmunk/cpVect.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/cpVect.h @@ -0,0 +1,230 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +#ifndef CHIPMUNK_VECT_H +#define CHIPMUNK_VECT_H + +#include "chipmunk_types.h" + +/// @defgroup cpVect cpVect +/// Chipmunk's 2D vector type along with a handy 2D vector math lib. +/// @{ + +/// Constant for the zero vector. +static const cpVect cpvzero = {0.0f,0.0f}; + +/// Convenience constructor for cpVect structs. +static inline cpVect cpv(const cpFloat x, const cpFloat y) +{ + cpVect v = {x, y}; + return v; +} + +/// Check if two vectors are equal. (Be careful when comparing floating point numbers!) +static inline cpBool cpveql(const cpVect v1, const cpVect v2) +{ + return (v1.x == v2.x && v1.y == v2.y); +} + +/// Add two vectors +static inline cpVect cpvadd(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x + v2.x, v1.y + v2.y); +} + +/// Subtract two vectors. +static inline cpVect cpvsub(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x - v2.x, v1.y - v2.y); +} + +/// Negate a vector. +static inline cpVect cpvneg(const cpVect v) +{ + return cpv(-v.x, -v.y); +} + +/// Scalar multiplication. +static inline cpVect cpvmult(const cpVect v, const cpFloat s) +{ + return cpv(v.x*s, v.y*s); +} + +/// Vector dot product. +static inline cpFloat cpvdot(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.x + v1.y*v2.y; +} + +/// 2D vector cross product analog. +/// The cross product of 2D vectors results in a 3D vector with only a z component. +/// This function returns the magnitude of the z value. +static inline cpFloat cpvcross(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.y - v1.y*v2.x; +} + +/// Returns a perpendicular vector. (90 degree rotation) +static inline cpVect cpvperp(const cpVect v) +{ + return cpv(-v.y, v.x); +} + +/// Returns a perpendicular vector. (-90 degree rotation) +static inline cpVect cpvrperp(const cpVect v) +{ + return cpv(v.y, -v.x); +} + +/// Returns the vector projection of v1 onto v2. +static inline cpVect cpvproject(const cpVect v1, const cpVect v2) +{ + return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); +} + +/// 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) +{ + return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); +} + +/// Inverse of cpvrotate(). +static inline cpVect cpvunrotate(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); +} + +/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths. +static inline cpFloat cpvlengthsq(const cpVect v) +{ + return cpvdot(v, v); +} + +/// 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) +{ + return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); +} + +/// Returns a normalized copy of v. +static inline cpVect cpvnormalize(const cpVect v) +{ + // Neat trick I saw somewhere to avoid div/0. + return cpvmult(v, 1.0f/(cpvlength(v) + CPFLOAT_MIN)); +} + +/// Spherical linearly interpolate between v1 and v2. +static inline cpVect +cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) +{ + cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); + cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); + + if(omega < 1e-3){ + // If the angle between two vectors is very small, lerp instead to avoid precision issues. + return cpvlerp(v1, v2, t); + } else { + cpFloat denom = 1.0f/cpfsin(omega); + return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); + } +} + +/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians +static inline cpVect +cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a) +{ + cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); + cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); + + return cpvslerp(v1, v2, cpfmin(a, omega)/omega); +} + +/// Clamp v to length len. +static inline cpVect cpvclamp(const cpVect v, const cpFloat len) +{ + return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +/// Linearly interpolate between v1 towards v2 by distance d. +static inline cpVect cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) +{ + return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); +} + +/// Returns the distance between v1 and v2. +static inline cpFloat cpvdist(const cpVect v1, const cpVect v2) +{ + return cpvlength(cpvsub(v1, v2)); +} + +/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances. +static inline cpFloat cpvdistsq(const cpVect v1, const cpVect v2) +{ + return cpvlengthsq(cpvsub(v1, v2)); +} + +/// Returns true if the distance between v1 and v2 is less than dist. +static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) +{ + return cpvdistsq(v1, v2) < dist*dist; +} + +/// @} + +/// @defgroup cpMat2x2 cpMat2x2 +/// 2x2 matrix type used for tensors and such. +/// @{ + +// NUKE +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); +} + +///@} + +#endif diff --git a/Chipmunk2D/include/chipmunk/prime.h b/Chipmunk2D/include/chipmunk/prime.h new file mode 100644 --- /dev/null +++ b/Chipmunk2D/include/chipmunk/prime.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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. + */ + +// Used for resizing hash tables. +// Values approximately double. +// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html +static int primes[] = { + 5, + 13, + 23, + 47, + 97, + 193, + 389, + 769, + 1543, + 3079, + 6151, + 12289, + 24593, + 49157, + 98317, + 196613, + 393241, + 786433, + 1572869, + 3145739, + 6291469, + 12582917, + 25165843, + 50331653, + 100663319, + 201326611, + 402653189, + 805306457, + 1610612741, + 0, +}; + +static inline int +next_prime(int n) +{ + int i = 0; + while(n > primes[i]){ + i++; + cpAssertHard(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen + } + + return primes[i]; +} diff --git a/Chipmunk2D/src/chipmunk.c b/Chipmunk2D/src/chipmunk.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/chipmunk.c @@ -0,0 +1,331 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#if defined(ANDROID) +# include +#endif + +#include "chipmunk_private.h" + +void +cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...) +{ + fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: ")); + + va_list vargs; + va_start(vargs, message); { +#if defined(ANDROID) + __android_log_print( ANDROID_LOG_INFO, "Chipmunk", "%s(%d)", file, line ); + __android_log_print( ANDROID_LOG_INFO, "Chipmunk", message, vargs ); +#else + vfprintf(stderr, message, vargs); + fprintf(stderr, "\n"); +#endif + } va_end(vargs); + +#if defined(ANDROID) + __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tFailed condition: %s\n", condition); + __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tSource:%s:%d\n", file, line); +#else + fprintf(stderr, "\tFailed condition: %s\n", condition); + fprintf(stderr, "\tSource:%s:%d\n", file, line); +#endif +} + +#define STR(s) #s +#define XSTR(s) STR(s) + +const char *cpVersionString = XSTR(CP_VERSION_MAJOR) "." XSTR(CP_VERSION_MINOR) "." XSTR(CP_VERSION_RELEASE); + +//MARK: Misc Functions + +cpFloat +cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) +{ + return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset)); +} + +cpFloat +cpAreaForCircle(cpFloat r1, cpFloat r2) +{ + return (cpFloat)CP_PI*cpfabs(r1*r1 - r2*r2); +} + +cpFloat +cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r) +{ + cpVect offset = cpvlerp(a, b, 0.5f); + + // This approximates the shape as a box for rounded segments, but it's quite close. + cpFloat length = cpvdist(b, a) + 2.0f*r; + return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset)); +} + +cpFloat +cpAreaForSegment(cpVect a, cpVect b, cpFloat r) +{ + return r*((cpFloat)CP_PI*r + 2.0f*cpvdist(a, b)); +} + +cpFloat +cpMomentForPoly(cpFloat m, const int count, const cpVect *verts, cpVect offset, cpFloat r) +{ + // TODO account for radius. + if(count == 2) return cpMomentForSegment(m, verts[0], verts[1], 0.0f); + + cpFloat sum1 = 0.0f; + cpFloat sum2 = 0.0f; + for(int i=0; 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(cpvsub(verts[head], a), delta); + 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, const cpVect *verts, cpVect *result, int *first, cpFloat tol) +{ + if(verts != 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)); + } + + // Degenerate case, all points 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; + return QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1; +} + +//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 PointQueryIteratorFunc(cpShape *shape, cpVect p, cpFloat d, cpVect g, cpSpacePointQueryBlock block){block(shape, p, d, g);} +void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block){ + cpSpacePointQuery(space, point, maxDistance, filter, (cpSpacePointQueryFunc)PointQueryIteratorFunc, block); +} + +static void SegmentQueryIteratorFunc(cpShape *shape, cpVect p, cpVect n, cpFloat t, cpSpaceSegmentQueryBlock block){block(shape, p, n, t);} +void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block){ + cpSpaceSegmentQuery(space, start, end, radius, filter, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block); +} + +void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block){ + cpSpaceBBQuery(space, bb, filter, (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/Chipmunk2D/src/cpArbiter.c b/Chipmunk2D/src/cpArbiter.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpArbiter.c @@ -0,0 +1,496 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +// TODO: make this generic so I can reuse it for constraints also. +static inline void +unthreadHelper(cpArbiter *arb, cpBody *body) +{ + struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body); + cpArbiter *prev = thread->prev; + cpArbiter *next = thread->next; + + if(prev){ + cpArbiterThreadForBody(prev, body)->next = next; + } else if(body->arbiterList == arb) { + // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list. + // This function may be called for an arbiter that was never in a list. + // In that case, we need to protect it from wiping out the body->arbiterList pointer. + body->arbiterList = next; + } + + if(next) cpArbiterThreadForBody(next, body)->prev = prev; + + thread->prev = NULL; + thread->next = NULL; +} + +void +cpArbiterUnthread(cpArbiter *arb) +{ + unthreadHelper(arb, arb->body_a); + unthreadHelper(arb, arb->body_b); +} + +cpBool cpArbiterIsFirstContact(const cpArbiter *arb) +{ + return arb->state == CP_ARBITER_STATE_FIRST_COLLISION; +} + +cpBool cpArbiterIsRemoval(const cpArbiter *arb) +{ + return arb->state == CP_ARBITER_STATE_INVALIDATED; +} + +int cpArbiterGetCount(const cpArbiter *arb) +{ + // Return 0 contacts if we are in a separate callback. + return (arb->state < CP_ARBITER_STATE_CACHED ? arb->count : 0); +} + +cpVect +cpArbiterGetNormal(const cpArbiter *arb) +{ + return cpvmult(arb->n, arb->swapped ? -1.0f : 1.0); +} + +cpVect +cpArbiterGetPointA(const cpArbiter *arb, int i) +{ + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); + return cpvadd(arb->body_a->p, arb->contacts[i].r1); +} + +cpVect +cpArbiterGetPointB(const cpArbiter *arb, int i) +{ + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); + return cpvadd(arb->body_b->p, arb->contacts[i].r2); +} + +cpFloat +cpArbiterGetDepth(const cpArbiter *arb, int i) +{ + cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); + + struct cpContact *con = &arb->contacts[i]; + return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n); +} + +cpContactPointSet +cpArbiterGetContactPointSet(const cpArbiter *arb) +{ + cpContactPointSet set; + set.count = cpArbiterGetCount(arb); + + cpBool swapped = arb->swapped; + cpVect n = arb->n; + set.normal = (swapped ? cpvneg(n) : n); + + for(int i=0; ibody_a->p, arb->contacts[i].r1); + cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2); + + set.points[i].pointA = (swapped ? p2 : p1); + set.points[i].pointB = (swapped ? p1 : p2); + set.points[i].distance = cpvdot(cpvsub(p2, p1), n); + } + + return set; +} + +void +cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set) +{ + int count = set->count; + cpAssertHard(count == arb->count, "The number of contact points cannot be changed."); + + cpBool swapped = arb->swapped; + arb->n = (swapped ? cpvneg(set->normal) : set->normal); + + for(int i=0; ipoints[i].pointA; + cpVect p2 = set->points[i].pointB; + + arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p); + arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p); + } +} + +cpVect +cpArbiterTotalImpulse(const cpArbiter *arb) +{ + struct cpContact *contacts = arb->contacts; + cpVect n = arb->n; + cpVect sum = cpvzero; + + for(int i=0, count=cpArbiterGetCount(arb); ijnAcc, con->jtAcc))); + } + + return (arb->swapped ? sum : cpvneg(sum)); + return cpvzero; +} + +cpFloat +cpArbiterTotalKE(const cpArbiter *arb) +{ + cpFloat eCoef = (1 - arb->e)/(1 + arb->e); + cpFloat sum = 0.0; + + struct 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; +} + +cpBool +cpArbiterIgnore(cpArbiter *arb) +{ + arb->state = CP_ARBITER_STATE_IGNORE; + return cpFalse; +} + +cpFloat +cpArbiterGetRestitution(const cpArbiter *arb) +{ + return arb->e; +} + +void +cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution) +{ + arb->e = restitution; +} + +cpFloat +cpArbiterGetFriction(const cpArbiter *arb) +{ + return arb->u; +} + +void +cpArbiterSetFriction(cpArbiter *arb, cpFloat friction) +{ + arb->u = friction; +} + +cpVect +cpArbiterGetSurfaceVelocity(cpArbiter *arb) +{ + return cpvmult(arb->surface_vr, arb->swapped ? -1.0f : 1.0); +} + +void +cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr) +{ + arb->surface_vr = cpvmult(vr, arb->swapped ? -1.0f : 1.0); +} + +cpDataPointer +cpArbiterGetUserData(const cpArbiter *arb) +{ + return arb->data; +} + +void +cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData) +{ + arb->data = userData; +} + +void +cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) +{ + if(arb->swapped){ + (*a) = (cpShape *)arb->b, (*b) = (cpShape *)arb->a; + } else { + (*a) = (cpShape *)arb->a, (*b) = (cpShape *)arb->b; + } +} + +void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) +{ + CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); + (*a) = shape_a->body; + (*b) = shape_b->body; +} + +cpBool +cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerA; + return handler->beginFunc(arb, space, handler->userData); +} + +cpBool +cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerB; + arb->swapped = !arb->swapped; + cpBool retval = handler->beginFunc(arb, space, handler->userData); + arb->swapped = !arb->swapped; + return retval; +} + +cpBool +cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerA; + return handler->preSolveFunc(arb, space, handler->userData); +} + +cpBool +cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerB; + arb->swapped = !arb->swapped; + cpBool retval = handler->preSolveFunc(arb, space, handler->userData); + arb->swapped = !arb->swapped; + return retval; +} + +void +cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerA; + handler->postSolveFunc(arb, space, handler->userData); +} + +void +cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerB; + arb->swapped = !arb->swapped; + handler->postSolveFunc(arb, space, handler->userData); + arb->swapped = !arb->swapped; +} + +void +cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerA; + handler->separateFunc(arb, space, handler->userData); +} + +void +cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space) +{ + cpCollisionHandler *handler = arb->handlerB; + arb->swapped = !arb->swapped; + handler->separateFunc(arb, space, handler->userData); + arb->swapped = !arb->swapped; +} + +cpArbiter* +cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) +{ + arb->handler = NULL; + arb->swapped = cpFalse; + + arb->handler = NULL; + arb->handlerA = NULL; + arb->handlerB = NULL; + + arb->e = 0.0f; + arb->u = 0.0f; + arb->surface_vr = cpvzero; + + arb->count = 0; + arb->contacts = NULL; + + arb->a = a; arb->body_a = a->body; + arb->b = b; arb->body_b = b->body; + + arb->thread_a.next = NULL; + arb->thread_b.next = NULL; + arb->thread_a.prev = NULL; + arb->thread_b.prev = NULL; + + arb->stamp = 0; + arb->state = CP_ARBITER_STATE_FIRST_COLLISION; + + arb->data = NULL; + + return arb; +} + +static inline cpCollisionHandler * +cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue) +{ + cpCollisionType types[] = {a, b}; + cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types); + return (handler ? handler : defaultValue); +} + +void +cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space) +{ + const cpShape *a = info->a, *b = info->b; + + // For collisions between two similar primitive types, the order could have been swapped since the last frame. + arb->a = a; arb->body_a = a->body; + arb->b = b; arb->body_b = b->body; + + // Iterate over the possible pairs to look for hash value matches. + for(int i=0; icount; i++){ + struct cpContact *con = &info->arr[i]; + + // r1 and r2 store absolute offsets at init time. + // Need to convert them to relative offsets. + con->r1 = cpvsub(con->r1, a->body->p); + con->r2 = cpvsub(con->r2, b->body->p); + + // Cached impulses are not zeroed at init time. + con->jnAcc = con->jtAcc = 0.0f; + + for(int j=0; jcount; j++){ + struct cpContact *old = &arb->contacts[j]; + + // This could trigger false positives, but is fairly unlikely nor serious if it does. + if(con->hash == old->hash){ + // Copy the persistant contact information. + con->jnAcc = old->jnAcc; + con->jtAcc = old->jtAcc; + } + } + } + + arb->contacts = info->arr; + arb->count = info->count; + arb->n = info->n; + + arb->e = a->e * b->e; + arb->u = a->u * b->u; + + cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV); + arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n))); + + cpCollisionType typeA = info->a->type, typeB = info->b->type; + cpCollisionHandler *defaultHandler = &space->defaultHandler; + cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler); + + // Check if the types match, but don't swap for a default handler which use the wildcard for type A. + cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE); + + if(handler != defaultHandler || space->usesWildcards){ + // The order of the main handler swaps the wildcard handlers too. Uffda. + arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing); + arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing); + } + + // mark it as new if it's been cached + if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION; +} + +void +cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias) +{ + cpBody *a = arb->body_a; + cpBody *b = arb->body_b; + cpVect n = arb->n; + cpVect body_delta = cpvsub(b->p, a->p); + + for(int i=0; icount; i++){ + struct cpContact *con = &arb->contacts[i]; + + // Calculate the mass normal and mass tangent. + con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n); + con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n)); + + // Calculate the target bias velocity. + cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n); + con->bias = -bias*cpfmin(0.0f, dist + slop)/dt; + con->jBias = 0.0f; + + // Calculate the target bounce velocity. + con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e; + } +} + +void +cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef) +{ + if(cpArbiterIsFirstContact(arb)) return; + + cpBody *a = arb->body_a; + cpBody *b = arb->body_b; + cpVect n = arb->n; + + for(int i=0; icount; i++){ + struct cpContact *con = &arb->contacts[i]; + cpVect j = cpvrotate(n, cpv(con->jnAcc, con->jtAcc)); + apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef)); + } +} + +// TODO: is it worth splitting velocity/position correction? + +void +cpArbiterApplyImpulse(cpArbiter *arb) +{ + cpBody *a = arb->body_a; + cpBody *b = arb->body_b; + cpVect n = arb->n; + cpVect surface_vr = arb->surface_vr; + cpFloat friction = arb->u; + + for(int i=0; icount; i++){ + struct cpContact *con = &arb->contacts[i]; + cpFloat nMass = con->nMass; + cpVect r1 = con->r1; + cpVect r2 = con->r2; + + cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); + cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); + cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr); + + cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + cpFloat vrn = cpvdot(vr, n); + cpFloat vrt = cpvdot(vr, cpvperp(n)); + + cpFloat jbn = (con->bias - vbn)*nMass; + cpFloat jbnOld = con->jBias; + con->jBias = cpfmax(jbnOld + jbn, 0.0f); + + cpFloat jn = -(con->bounce + vrn)*nMass; + cpFloat jnOld = con->jnAcc; + con->jnAcc = cpfmax(jnOld + jn, 0.0f); + + cpFloat jtMax = friction*con->jnAcc; + cpFloat jt = -vrt*con->tMass; + cpFloat jtOld = con->jtAcc; + con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); + + 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/Chipmunk2D/src/cpArray.c b/Chipmunk2D/src/cpArray.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpArray.c @@ -0,0 +1,101 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" + + +cpArray * +cpArrayNew(int size) +{ + cpArray *arr = (cpArray *)cpcalloc(1, sizeof(cpArray)); + + arr->num = 0; + arr->max = (size ? size : 4); + arr->arr = (void **)cpcalloc(arr->max, sizeof(void*)); + + return arr; +} + +void +cpArrayFree(cpArray *arr) +{ + if(arr){ + cpfree(arr->arr); + arr->arr = NULL; + + cpfree(arr); + } +} + +void +cpArrayPush(cpArray *arr, void *object) +{ + if(arr->num == arr->max){ + arr->max = 3*(arr->max + 1)/2; + arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void*)); + } + + arr->arr[arr->num] = object; + arr->num++; +} + +void * +cpArrayPop(cpArray *arr) +{ + arr->num--; + + void *value = arr->arr[arr->num]; + arr->arr[arr->num] = NULL; + + return value; +} + +void +cpArrayDeleteObj(cpArray *arr, void *obj) +{ + for(int i=0; inum; i++){ + if(arr->arr[i] == obj){ + arr->num--; + + arr->arr[i] = arr->arr[arr->num]; + arr->arr[arr->num] = NULL; + + return; + } + } +} + +void +cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)) +{ + for(int i=0; inum; i++) freeFunc(arr->arr[i]); +} + +cpBool +cpArrayContains(cpArray *arr, void *ptr) +{ + for(int i=0; inum; i++) + if(arr->arr[i] == ptr) return cpTrue; + + return cpFalse; +} diff --git a/Chipmunk2D/src/cpBBTree.c b/Chipmunk2D/src/cpBBTree.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpBBTree.c @@ -0,0 +1,896 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "stdlib.h" +#include "stdio.h" + +#include "chipmunk_private.h" + +static inline cpSpatialIndexClass *Klass(); + +typedef struct Node Node; +typedef struct Pair Pair; + +struct cpBBTree { + cpSpatialIndex spatialIndex; + cpBBTreeVelocityFunc velocityFunc; + + cpHashSet *leaves; + Node *root; + + Node *pooledNodes; + Pair *pooledPairs; + cpArray *allocatedBuffers; + + cpTimestamp stamp; +}; + +struct Node { + void *obj; + cpBB bb; + Node *parent; + + union { + // Internal nodes + 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; + Pair *next; +} Thread; + +struct Pair { + Thread a, b; + cpCollisionID id; +}; + +//MARK: Misc Functions + +static inline cpBB +GetBB(cpBBTree *tree, void *obj) +{ + cpBB bb = tree->spatialIndex.bbfunc(obj); + + cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc; + if(velocityFunc){ + cpFloat coef = 0.1f; + cpFloat x = (bb.r - bb.l)*coef; + cpFloat y = (bb.t - bb.b)*coef; + + cpVect v = cpvmult(velocityFunc(obj), 0.1f); + return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y)); + } else { + return bb; + } +} + +static inline cpBBTree * +GetTree(cpSpatialIndex *index) +{ + return (index && index->klass == Klass() ? (cpBBTree *)index : NULL); +} + +static inline Node * +GetRootIfTree(cpSpatialIndex *index){ + return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL); +} + +static inline cpBBTree * +GetMasterTree(cpBBTree *tree) +{ + cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); + return (dynamicTree ? dynamicTree : tree); +} + +static inline void +IncrementStamp(cpBBTree *tree) +{ + cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); + if(dynamicTree){ + dynamicTree->stamp++; + } else { + tree->stamp++; + } +} + +//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; +} + +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){ + tree->pooledPairs = pair->a.next; + return pair; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(Pair); + cpAssertHard(count, "Internal Error: Buffer size is too small."); + + Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(tree->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; ia.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev; + } + + if(prev){ + if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; + } else { + thread.leaf->PAIRS = next; + } +} + +static void +PairsClear(Node *leaf, cpBBTree *tree) +{ + Pair *pair = leaf->PAIRS; + leaf->PAIRS = NULL; + + while(pair){ + if(pair->a.leaf == leaf){ + Pair *next = pair->a.next; + ThreadUnlink(pair->b); + PairRecycle(tree, pair); + pair = next; + } else { + Pair *next = pair->b.next; + ThreadUnlink(pair->a); + PairRecycle(tree, pair); + pair = next; + } + } +} + +static void +PairInsert(Node *a, Node *b, cpBBTree *tree) +{ + Pair *nextA = a->PAIRS, *nextB = b->PAIRS; + Pair *pair = PairFromPool(tree); + Pair temp = {{NULL, a, nextA},{NULL, b, nextB}, 0}; + + a->PAIRS = b->PAIRS = pair; + *pair = temp; + + if(nextA){ + if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair; + } + + if(nextB){ + if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair; + } +} + + +//MARK: Node Functions + +static void +NodeRecycle(cpBBTree *tree, Node *node) +{ + node->parent = tree->pooledNodes; + tree->pooledNodes = node; +} + +static Node * +NodeFromPool(cpBBTree *tree) +{ + Node *node = tree->pooledNodes; + + if(node){ + tree->pooledNodes = node->parent; + return node; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(Node); + cpAssertHard(count, "Internal Error: Buffer size is too small."); + + Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(tree->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; iA = value; + value->parent = node; +} + +static inline void +NodeSetB(Node *node, Node *value) +{ + node->B = value; + value->parent = node; +} + +static Node * +NodeNew(cpBBTree *tree, Node *a, Node *b) +{ + Node *node = NodeFromPool(tree); + + node->obj = NULL; + node->bb = cpBBMerge(a->bb, b->bb); + node->parent = NULL; + + NodeSetA(node, a); + NodeSetB(node, b); + + return node; +} + +static inline cpBool +NodeIsLeaf(Node *node) +{ + return (node->obj != NULL); +} + +static inline Node * +NodeOther(Node *node, Node *child) +{ + return (node->A == child ? node->B : node->A); +} + +static inline void +NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) +{ + 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); + NodeSetA(parent, value); + } else { + NodeRecycle(tree, parent->B); + NodeSetB(parent, value); + } + + for(Node *node=parent; node; node = node->parent){ + node->bb = cpBBMerge(node->A->bb, node->B->bb); + } +} + +//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) +{ + if(subtree == NULL){ + return leaf; + } 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); + + 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)); + } else { + NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree)); + } + + subtree->bb = cpBBMerge(subtree->bb, leaf->bb); + return subtree; + } +} + +static void +SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) +{ + if(cpBBIntersects(subtree->bb, bb)){ + if(NodeIsLeaf(subtree)){ + func(obj, subtree->obj, 0, data); + } else { + SubtreeQuery(subtree->A, obj, bb, func, data); + SubtreeQuery(subtree->B, obj, bb, func, data); + } + } +} + + +static cpFloat +SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *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 { + 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; + } +} + +static void +SubtreeRecycle(cpBBTree *tree, Node *node) +{ + if(!NodeIsLeaf(node)){ + SubtreeRecycle(tree, node->A); + SubtreeRecycle(tree, node->B); + NodeRecycle(tree, node); + } +} + +static inline Node * +SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) +{ + if(leaf == subtree){ + return NULL; + } else { + Node *parent = leaf->parent; + if(parent == subtree){ + Node *other = NodeOther(subtree, leaf); + other->parent = subtree->parent; + NodeRecycle(tree, subtree); + return other; + } else { + NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree); + return subtree; + } + } +} + +//MARK: Marking Functions + +typedef struct MarkContext { + cpBBTree *tree; + Node *staticRoot; + cpSpatialIndexQueryFunc func; + void *data; +} MarkContext; + +static void +MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) +{ + if(cpBBIntersects(leaf->bb, subtree->bb)){ + if(NodeIsLeaf(subtree)){ + if(left){ + PairInsert(leaf, subtree, context->tree); + } else { + if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree); + context->func(leaf->obj, subtree->obj, 0, context->data); + } + } else { + MarkLeafQuery(subtree->A, leaf, left, context); + MarkLeafQuery(subtree->B, leaf, left, context); + } + } +} + +static void +MarkLeaf(Node *leaf, MarkContext *context) +{ + cpBBTree *tree = context->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); + } else { + MarkLeafQuery(node->parent->A, leaf, cpFalse, context); + } + } + } else { + Pair *pair = leaf->PAIRS; + while(pair){ + if(leaf == pair->b.leaf){ + pair->id = context->func(pair->a.leaf->obj, leaf->obj, pair->id, context->data); + pair = pair->b.next; + } else { + pair = pair->a.next; + } + } + } +} + +static void +MarkSubtree(Node *subtree, MarkContext *context) +{ + if(NodeIsLeaf(subtree)){ + MarkLeaf(subtree, context); + } else { + MarkSubtree(subtree->A, context); + MarkSubtree(subtree->B, context); // TODO: Force TCO here? + } +} + +//MARK: Leaf Functions + +static Node * +LeafNew(cpBBTree *tree, void *obj, cpBB bb) +{ + Node *node = NodeFromPool(tree); + node->obj = obj; + node->bb = GetBB(tree, obj); + + node->parent = NULL; + node->STAMP = 0; + node->PAIRS = NULL; + + return node; +} + +static cpBool +LeafUpdate(Node *leaf, cpBBTree *tree) +{ + Node *root = tree->root; + cpBB bb = tree->spatialIndex.bbfunc(leaf->obj); + + if(!cpBBContainsBB(leaf->bb, bb)){ + leaf->bb = GetBB(tree, leaf->obj); + + root = SubtreeRemove(root, leaf, tree); + tree->root = SubtreeInsert(root, leaf, tree); + + PairsClear(leaf, tree); + leaf->STAMP = GetMasterTree(tree)->stamp; + + return cpTrue; + } else { + return cpFalse; + } +} + +static cpCollisionID VoidQueryFunc(void *obj1, void *obj2, cpCollisionID id, void *data){return id;} + +static void +LeafAddPairs(Node *leaf, cpBBTree *tree) +{ + cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex; + if(dynamicIndex){ + Node *dynamicRoot = GetRootIfTree(dynamicIndex); + if(dynamicRoot){ + cpBBTree *dynamicTree = GetTree(dynamicIndex); + MarkContext context = {dynamicTree, NULL, NULL, NULL}; + MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context); + } + } else { + Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex); + MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL}; + MarkLeaf(leaf, &context); + } +} + +//MARK: Memory Management Functions + +cpBBTree * +cpBBTreeAlloc(void) +{ + return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree)); +} + +static int +leafSetEql(void *obj, Node *node) +{ + return (obj == node->obj); +} + +static void * +leafSetTrans(void *obj, cpBBTree *tree) +{ + return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj)); +} + +cpSpatialIndex * +cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex); + + tree->velocityFunc = NULL; + + tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql); + tree->root = NULL; + + tree->pooledNodes = NULL; + tree->allocatedBuffers = cpArrayNew(0); + + tree->stamp = 0; + + return (cpSpatialIndex *)tree; +} + +void +cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func) +{ + if(index->klass != Klass()){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index."); + return; + } + + ((cpBBTree *)index)->velocityFunc = func; +} + +cpSpatialIndex * +cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex); +} + +static void +cpBBTreeDestroy(cpBBTree *tree) +{ + cpHashSetFree(tree->leaves); + + if(tree->allocatedBuffers) cpArrayFreeEach(tree->allocatedBuffers, cpfree); + cpArrayFree(tree->allocatedBuffers); +} + +//MARK: Insert/Remove + +static void +cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, (cpHashSetTransFunc)leafSetTrans, tree); + + Node *root = tree->root; + tree->root = SubtreeInsert(root, leaf, tree); + + leaf->STAMP = GetMasterTree(tree)->stamp; + LeafAddPairs(leaf, tree); + IncrementStamp(tree); +} + +static void +cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj); + + tree->root = SubtreeRemove(tree->root, leaf, tree); + PairsClear(leaf, tree); + NodeRecycle(tree, leaf); +} + +static cpBool +cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + return (cpHashSetFind(tree->leaves, hashid, obj) != NULL); +} + +//MARK: Reindex + +static void LeafUpdateWrap(Node *leaf, cpBBTree *tree) {LeafUpdate(leaf, tree);} + +static void +cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data) +{ + if(!tree->root) return; + + // LeafUpdate() may modify tree->root. Don't cache it. + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdateWrap, tree); + + cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex; + Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL); + + MarkContext context = {tree, staticRoot, func, data}; + MarkSubtree(tree->root, &context); + if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data); + + IncrementStamp(tree); +} + +static void +cpBBTreeReindex(cpBBTree *tree) +{ + cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL); +} + +static void +cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj); + if(leaf){ + if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree); + IncrementStamp(tree); + } +} + +//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, t_exit, func, data); +} + +static void +cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) +{ + if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); +} + +//MARK: Misc + +static int +cpBBTreeCount(cpBBTree *tree) +{ + return cpHashSetCount(tree->leaves); +} + +typedef struct eachContext { + cpSpatialIndexIteratorFunc func; + void *data; +} eachContext; + +static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);} + +static void +cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data) +{ + eachContext context = {func, data}; + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context); +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyImpl)cpBBTreeDestroy, + + (cpSpatialIndexCountImpl)cpBBTreeCount, + (cpSpatialIndexEachImpl)cpBBTreeEach, + + (cpSpatialIndexContainsImpl)cpBBTreeContains, + (cpSpatialIndexInsertImpl)cpBBTreeInsert, + (cpSpatialIndexRemoveImpl)cpBBTreeRemove, + + (cpSpatialIndexReindexImpl)cpBBTreeReindex, + (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject, + (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery, + + (cpSpatialIndexQueryImpl)cpBBTreeQuery, + (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, +}; + +static inline cpSpatialIndexClass *Klass(){return &klass;} + + +//MARK: Tree Optimization + +static int +cpfcompare(const cpFloat *a, const cpFloat *b){ + return (*a < *b ? -1 : (*b < *a ? 1 : 0)); +} + +static void +fillNodeArray(Node *node, Node ***cursor){ + (**cursor) = node; + (*cursor)++; +} + +static Node * +partitionNodes(cpBBTree *tree, Node **nodes, int count) +{ + if(count == 1){ + return nodes[0]; + } else if(count == 2) { + return NodeNew(tree, nodes[0], nodes[1]); + } + + // Find the AABB for these nodes + cpBB bb = nodes[0]->bb; + for(int i=1; ibb); + + // Split it on it's longest axis + cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); + + // Sort the bounds and use the median as the splitting point + cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat)); + if(splitWidth){ + for(int i=0; ibb.l; + bounds[2*i + 1] = nodes[i]->bb.r; + } + } else { + for(int i=0; ibb.b; + bounds[2*i + 1] = nodes[i]->bb.t; + } + } + + qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); + cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split + cpfree(bounds); + + // Generate the child BBs + cpBB a = bb, b = bb; + if(splitWidth) a.r = b.l = split; else a.t = b.b = split; + + // Partition the nodes + int right = count; + for(int left=0; left < right;){ + Node *node = nodes[left]; + if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){ +// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){ + right--; + nodes[left] = nodes[right]; + nodes[right] = node; + } else { + left++; + } + } + + if(right == count){ + Node *node = NULL; + for(int i=0; iroot; +// Node *node = root; +// int bit = 0; +// unsigned int path = tree->opath; +// +// while(!NodeIsLeaf(node)){ +// node = (path&(1<a : node->b); +// bit = (bit + 1)&(sizeof(unsigned int)*8 - 1); +// } +// +// root = subtreeRemove(root, node, tree); +// tree->root = subtreeInsert(root, node, tree); +// } +//} + +void +cpBBTreeOptimize(cpSpatialIndex *index) +{ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index."); + return; + } + + cpBBTree *tree = (cpBBTree *)index; + Node *root = tree->root; + if(!root) return; + + int count = cpBBTreeCount(tree); + Node **nodes = (Node **)cpcalloc(count, sizeof(Node *)); + Node **cursor = nodes; + + cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor); + + SubtreeRecycle(tree, root); + tree->root = partitionNodes(tree, nodes, count); + cpfree(nodes); +} + +//MARK: Debug Draw + +//#define CP_BBTREE_DEBUG_DRAW +#ifdef CP_BBTREE_DEBUG_DRAW +#include "OpenGL/gl.h" +#include "OpenGL/glu.h" +#include + +static void +NodeRender(Node *node, int depth) +{ + if(!NodeIsLeaf(node) && depth <= 10){ + NodeRender(node->a, depth + 1); + NodeRender(node->b, depth + 1); + } + + cpBB bb = node->bb; + +// GLfloat v = depth/2.0f; +// glColor3f(1.0f - v, v, 0.0f); + glLineWidth(cpfmax(5.0f - depth, 1.0f)); + glBegin(GL_LINES); { + glVertex2f(bb.l, bb.b); + glVertex2f(bb.l, bb.t); + + glVertex2f(bb.l, bb.t); + glVertex2f(bb.r, bb.t); + + glVertex2f(bb.r, bb.t); + glVertex2f(bb.r, bb.b); + + glVertex2f(bb.r, bb.b); + glVertex2f(bb.l, bb.b); + }; glEnd(); +} + +void +cpBBTreeRenderDebug(cpSpatialIndex *index){ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index."); + return; + } + + cpBBTree *tree = (cpBBTree *)index; + if(tree->root) NodeRender(tree->root, 0); +} +#endif diff --git a/Chipmunk2D/src/cpBody.c b/Chipmunk2D/src/cpBody.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpBody.c @@ -0,0 +1,626 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" + +cpBody* +cpBodyAlloc(void) +{ + return (cpBody *)cpcalloc(1, sizeof(cpBody)); +} + +cpBody * +cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment) +{ + body->space = NULL; + body->shapeList = NULL; + body->arbiterList = NULL; + body->constraintList = NULL; + + body->velocity_func = cpBodyUpdateVelocity; + body->position_func = cpBodyUpdatePosition; + + body->sleeping.root = NULL; + body->sleeping.next = NULL; + body->sleeping.idleTime = 0.0f; + + body->p = cpvzero; + body->v = cpvzero; + body->f = cpvzero; + + body->w = 0.0f; + body->t = 0.0f; + + body->v_bias = cpvzero; + body->w_bias = 0.0f; + + body->userData = NULL; + + // Setters must be called after full initialization so the sanity checks don't assert on garbage data. + cpBodySetMass(body, mass); + cpBodySetMoment(body, moment); + cpBodySetAngle(body, 0.0f); + + return body; +} + +cpBody* +cpBodyNew(cpFloat mass, cpFloat moment) +{ + return cpBodyInit(cpBodyAlloc(), mass, moment); +} + +cpBody* +cpBodyNewKinematic() +{ + cpBody *body = cpBodyNew(0.0f, 0.0f); + cpBodySetType(body, CP_BODY_TYPE_KINEMATIC); + + return body; +} + +cpBody* +cpBodyNewStatic() +{ + cpBody *body = cpBodyNew(0.0f, 0.0f); + cpBodySetType(body, CP_BODY_TYPE_STATIC); + + return body; +} + +void cpBodyDestroy(cpBody *body){} + +void +cpBodyFree(cpBody *body) +{ + if(body){ + cpBodyDestroy(body); + cpfree(body); + } +} + +#ifdef NDEBUG + #define cpAssertSaneBody(body) +#else + static void cpv_assert_nan(cpVect v, char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);} + static void cpv_assert_infinite(cpVect v, char *message){cpAssertHard(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);} + + static void + cpBodySanityCheck(const cpBody *body) + { + cpAssertHard(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is NaN."); + cpAssertHard(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is NaN."); + cpAssertHard(body->m >= 0.0f, "Body's mass is negative."); + cpAssertHard(body->i >= 0.0f, "Body's moment is negative."); + + cpv_assert_sane(body->p, "Body's position is invalid."); + cpv_assert_sane(body->v, "Body's velocity is invalid."); + cpv_assert_sane(body->f, "Body's force is invalid."); + + cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid."); + cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid."); + cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid."); + } + + #define cpAssertSaneBody(body) cpBodySanityCheck(body) +#endif + +cpBool +cpBodyIsSleeping(const cpBody *body) +{ + return (body->sleeping.root != ((cpBody*)0)); +} + +cpBodyType +cpBodyGetType(cpBody *body) +{ + if(body->sleeping.idleTime == INFINITY){ + return CP_BODY_TYPE_STATIC; + } else if(body->m == INFINITY){ + return CP_BODY_TYPE_KINEMATIC; + } else { + return CP_BODY_TYPE_DYNAMIC; + } +} + +void +cpBodySetType(cpBody *body, cpBodyType type) +{ + cpBodyType oldType = cpBodyGetType(body); + if(oldType == type) return; + + // Static bodies have their idle timers set to infinity. + // Non-static bodies should have their idle timer reset. + body->sleeping.idleTime = (type == CP_BODY_TYPE_STATIC ? INFINITY : 0.0f); + + if(type == CP_BODY_TYPE_DYNAMIC){ + body->m = body->i = 0.0f; + body->m_inv = body->i_inv = INFINITY; + + cpBodyAccumulateMassFromShapes(body); + } else { + body->m = body->i = INFINITY; + body->m_inv = body->i_inv = 0.0f; + + body->v = cpvzero; + body->w = 0.0f; + } + + // If the body is added to a space already, we'll need to update some space data structures. + cpSpace *space = cpBodyGetSpace(body); + if(space != NULL){ + cpAssertSpaceUnlocked(space); + + if(oldType == CP_BODY_TYPE_STATIC){ + // TODO This is probably not necessary +// cpBodyActivateStatic(body, NULL); + } else { + cpBodyActivate(body); + } + + // Move the bodies to the correct array. + cpArray *fromArray = cpSpaceArrayForBodyType(space, oldType); + cpArray *toArray = cpSpaceArrayForBodyType(space, type); + if(fromArray != toArray){ + cpArrayDeleteObj(fromArray, body); + cpArrayPush(toArray, body); + } + + // Move the body's shapes to the correct spatial index. + cpSpatialIndex *fromIndex = (oldType == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes); + cpSpatialIndex *toIndex = (type == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes); + if(fromIndex != toIndex){ + CP_BODY_FOREACH_SHAPE(body, shape){ + cpSpatialIndexRemove(fromIndex, shape, shape->hashid); + cpSpatialIndexInsert(toIndex, shape, shape->hashid); + } + } + } +} + + + +// Should *only* be called when shapes with mass info are modified, added or removed. +void +cpBodyAccumulateMassFromShapes(cpBody *body) +{ + if(body == NULL || cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) return; + + // Reset the body's mass data. + body->m = body->i = 0.0f; + body->cog = cpvzero; + + // Cache the position to realign it at the end. + cpVect pos = cpBodyGetPosition(body); + + // Accumulate mass from shapes. + CP_BODY_FOREACH_SHAPE(body, shape){ + struct cpShapeMassInfo *info = &shape->massInfo; + cpFloat m = info->m; + + if(m > 0.0f){ + cpFloat msum = body->m + m; + + body->i += m*info->i + cpvdistsq(body->cog, info->cog)*(m*body->m)/msum; + body->cog = cpvlerp(body->cog, info->cog, m/msum); + body->m = msum; + } + } + + // Recalculate the inverses. + body->m_inv = 1.0f/body->m; + body->i_inv = 1.0f/body->i; + + // Realign the body since the CoG has probably moved. + cpBodySetPosition(body, pos); + cpAssertSaneBody(body); +} + +cpSpace * +cpBodyGetSpace(const cpBody *body) +{ + return body->space; +} + +cpFloat +cpBodyGetMass(const cpBody *body) +{ + return body->m; +} + +void +cpBodySetMass(cpBody *body, cpFloat mass) +{ + cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies."); + cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite."); + + cpBodyActivate(body); + body->m = mass; + body->m_inv = 1.0f/mass; + cpAssertSaneBody(body); +} + +cpFloat +cpBodyGetMoment(const cpBody *body) +{ + return body->i; +} + +void +cpBodySetMoment(cpBody *body, cpFloat moment) +{ + cpAssertHard(moment >= 0.0f, "Moment of Inertia must be positive."); + + cpBodyActivate(body); + body->i = moment; + body->i_inv = 1.0f/moment; + cpAssertSaneBody(body); +} + +cpVect +cpBodyGetRotation(const cpBody *body) +{ + return cpv(body->transform.a, body->transform.b); +} + +void +cpBodyAddShape(cpBody *body, cpShape *shape) +{ + cpShape *next = body->shapeList; + if(next) next->prev = shape; + + shape->next = next; + body->shapeList = shape; + + if(shape->massInfo.m > 0.0f){ + cpBodyAccumulateMassFromShapes(body); + } +} + +void +cpBodyRemoveShape(cpBody *body, cpShape *shape) +{ + cpShape *prev = shape->prev; + cpShape *next = shape->next; + + if(prev){ + prev->next = next; + } else { + body->shapeList = next; + } + + if(next){ + next->prev = prev; + } + + shape->prev = NULL; + shape->next = NULL; + + if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC && shape->massInfo.m > 0.0f){ + cpBodyAccumulateMassFromShapes(body); + } +} + +static cpConstraint * +filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter) +{ + if(node == filter){ + return cpConstraintNext(node, body); + } else if(node->a == body){ + node->next_a = filterConstraints(node->next_a, body, filter); + } else { + node->next_b = filterConstraints(node->next_b, body, filter); + } + + return node; +} + +void +cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint) +{ + body->constraintList = filterConstraints(body->constraintList, body, constraint); +} + +// 'p' is the position of the CoG +static void +SetTransform(cpBody *body, cpVect p, cpFloat a) +{ + cpVect rot = cpvforangle(a); + cpVect c = body->cog; + + body->transform = cpTransformNewTranspose( + rot.x, -rot.y, p.x - (c.x*rot.x - c.y*rot.y), + rot.y, rot.x, p.y - (c.x*rot.y + c.y*rot.x) + ); +} + +static inline cpFloat +SetAngle(cpBody *body, cpFloat a) +{ + body->a = a; + cpAssertSaneBody(body); + + return a; +} + +cpVect +cpBodyGetPosition(const cpBody *body) +{ + return cpTransformPoint(body->transform, cpvzero); +} + +void +cpBodySetPosition(cpBody *body, cpVect position) +{ + cpBodyActivate(body); + cpVect p = body->p = cpvadd(cpTransformVect(body->transform, body->cog), position); + cpAssertSaneBody(body); + + SetTransform(body, p, body->a); +} + +cpVect +cpBodyGetCenterOfGravity(const cpBody *body) +{ + return body->cog; +} + +void +cpBodySetCenterOfGravity(cpBody *body, cpVect cog) +{ + cpBodyActivate(body); + body->cog = cog; + cpAssertSaneBody(body); +} + +cpVect +cpBodyGetVelocity(const cpBody *body) +{ + return body->v; +} + +void +cpBodySetVelocity(cpBody *body, cpVect velocity) +{ + cpBodyActivate(body); + body->v = velocity; + cpAssertSaneBody(body); +} + +cpVect +cpBodyGetForce(const cpBody *body) +{ + return body->f; +} + +void +cpBodySetForce(cpBody *body, cpVect force) +{ + cpBodyActivate(body); + body->f = force; + cpAssertSaneBody(body); +} + +cpFloat +cpBodyGetAngle(const cpBody *body) +{ + return body->a; +} + +void +cpBodySetAngle(cpBody *body, cpFloat angle) +{ + cpBodyActivate(body); + SetAngle(body, angle); + + SetTransform(body, body->p, angle); +} + +cpFloat +cpBodyGetAngularVelocity(const cpBody *body) +{ + return body->w; +} + +void +cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity) +{ + cpBodyActivate(body); + body->w = angularVelocity; + cpAssertSaneBody(body); +} + +cpFloat +cpBodyGetTorque(const cpBody *body) +{ + return body->t; +} + +void +cpBodySetTorque(cpBody *body, cpFloat torque) +{ + cpBodyActivate(body); + body->t = torque; + cpAssertSaneBody(body); +} + +cpDataPointer +cpBodyGetUserData(const cpBody *body) +{ + return body->userData; +} + +void +cpBodySetUserData(cpBody *body, cpDataPointer userData) +{ + body->userData = userData; +} + +void +cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc) +{ + body->velocity_func = velocityFunc; +} + +void +cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc) +{ + body->position_func = positionFunc; +} + +void +cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) +{ + // Skip kinematic bodies. + if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return; + + cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: %f)", body->m, body->i); + + body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)); + body->w = body->w*damping + body->t*body->i_inv*dt; + + // Reset forces. + body->f = cpvzero; + body->t = 0.0f; + + cpAssertSaneBody(body); +} + +void +cpBodyUpdatePosition(cpBody *body, cpFloat dt) +{ + cpVect p = body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); + cpFloat a = SetAngle(body, body->a + (body->w + body->w_bias)*dt); + SetTransform(body, p, a); + + body->v_bias = cpvzero; + body->w_bias = 0.0f; + + cpAssertSaneBody(body); +} + +cpVect +cpBodyLocalToWorld(const cpBody *body, const cpVect point) +{ + return cpTransformPoint(body->transform, point); +} + +cpVect +cpBodyWorldToLocal(const cpBody *body, const cpVect point) +{ + return cpTransformPoint(cpTransformRigidInverse(body->transform), point); +} + +void +cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point) +{ + cpBodyActivate(body); + body->f = cpvadd(body->f, force); + + cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); + body->t += cpvcross(r, force); +} + +void +cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point) +{ + cpBodyApplyForceAtWorldPoint(body, cpTransformVect(body->transform, force), cpTransformPoint(body->transform, point)); +} + +void +cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point) +{ + cpBodyActivate(body); + + cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); + apply_impulse(body, impulse, r); +} + +void +cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point) +{ + cpBodyApplyImpulseAtWorldPoint(body, cpTransformVect(body->transform, impulse), cpTransformPoint(body->transform, point)); +} + +cpVect +cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point) +{ + cpVect r = cpTransformVect(body->transform, cpvsub(point, body->cog)); + return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); +} + +cpVect +cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point) +{ + cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); + return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); +} + +cpFloat +cpBodyKineticEnergy(const cpBody *body) +{ + // Need to do some fudging to avoid NaNs + cpFloat vsq = cpvdot(body->v, body->v); + cpFloat wsq = body->w*body->w; + return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f); +} + +void +cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data) +{ + cpShape *shape = body->shapeList; + while(shape){ + cpShape *next = shape->next; + func(body, shape, data); + shape = next; + } +} + +void +cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data) +{ + cpConstraint *constraint = body->constraintList; + while(constraint){ + cpConstraint *next = cpConstraintNext(constraint, body); + func(body, constraint, data); + constraint = next; + } +} + +void +cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data) +{ + cpArbiter *arb = body->arbiterList; + while(arb){ + cpArbiter *next = cpArbiterNext(arb, body); + + cpBool swapped = arb->swapped; { + arb->swapped = (body == arb->body_b); + func(body, arb, data); + } arb->swapped = swapped; + + arb = next; + } +} diff --git a/Chipmunk2D/src/cpCollision.c b/Chipmunk2D/src/cpCollision.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpCollision.c @@ -0,0 +1,726 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "cpRobust.h" + +#if DEBUG && 0 +#include "ChipmunkDemo.h" +#define DRAW_ALL 0 +#define DRAW_GJK (0 || DRAW_ALL) +#define DRAW_EPA (0 || DRAW_ALL) +#define DRAW_CLOSEST (0 || DRAW_ALL) +#define DRAW_CLIP (0 || DRAW_ALL) + +#define PRINT_LOG 0 +#endif + +#define MAX_GJK_ITERATIONS 30 +#define MAX_EPA_ITERATIONS 30 +#define WARN_GJK_ITERATIONS 20 +#define WARN_EPA_ITERATIONS 20 + +static inline void +cpCollisionInfoPushContact(struct cpCollisionInfo *info, cpVect p1, cpVect p2, cpHashValue hash) +{ + cpAssertSoft(info->count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts."); + + struct cpContact *con = &info->arr[info->count]; + con->r1 = p1; + con->r2 = p2; + con->hash = hash; + + info->count++; +} + +//MARK: Support Points and Edges: + +// Support points are the maximal points on a shape's perimeter along a certain axis. +// The GJK and EPA algorithms use support points to iteratively sample the surface of the two shapes' minkowski difference. + +static inline int +PolySupportPointIndex(const int count, const struct cpSplittingPlane *planes, const cpVect n) +{ + cpFloat max = -INFINITY; + int index = 0; + + for(int i=0; i max){ + max = d; + index = i; + } + } + + return index; +} + +struct SupportPoint { + cpVect p; + // Save an index of the point so it can be cheaply looked up as a starting point for the next frame. + cpCollisionID index; +}; + +static inline struct SupportPoint +SupportPointNew(cpVect p, cpCollisionID index) +{ + struct SupportPoint point = {p, index}; + return point; +} + +typedef struct SupportPoint (*SupportPointFunc)(const cpShape *shape, const cpVect n); + +static inline struct SupportPoint +CircleSupportPoint(const cpCircleShape *circle, const cpVect n) +{ + return SupportPointNew(circle->tc, 0); +} + +static inline struct SupportPoint +SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n) +{ + if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)){ + return SupportPointNew(seg->ta, 0); + } else { + return SupportPointNew(seg->tb, 1); + } +} + +static inline struct SupportPoint +PolySupportPoint(const cpPolyShape *poly, const cpVect n) +{ + const struct cpSplittingPlane *planes = poly->planes; + int i = PolySupportPointIndex(poly->count, planes, n); + return SupportPointNew(planes[i].v0, i); +} + +// A point on the surface of two shape's minkowski difference. +struct MinkowskiPoint { + // Cache the two original support points. + cpVect a, b; + // b - a + cpVect ab; + // Concatenate the two support point indexes. + cpCollisionID id; +}; + +static inline struct MinkowskiPoint +MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b) +{ + struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.index & 0xFF)<<8 | (b.index & 0xFF)}; + return point; +} + +struct SupportContext { + const cpShape *shape1, *shape2; + SupportPointFunc func1, func2; +}; + +// Calculate the maximal point on the minkowski difference of two shapes along a particular axis. +static inline struct MinkowskiPoint +Support(const struct SupportContext *ctx, const cpVect n) +{ + struct SupportPoint a = ctx->func1(ctx->shape1, cpvneg(n)); + struct SupportPoint b = ctx->func2(ctx->shape2, n); + return MinkowskiPointNew(a, b); +} + +struct EdgePoint { + cpVect p; + // Keep a hash value for Chipmunk's collision hashing mechanism. + cpHashValue hash; +}; + +// Support edges are the edges of a polygon or segment shape that are in contact. +struct Edge { + struct EdgePoint a, b; + cpFloat r; + cpVect n; +}; + +static struct Edge +SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n) +{ + int count = poly->count; + int i1 = PolySupportPointIndex(poly->count, poly->planes, n); + + // TODO: get rid of mod eventually, very expensive on ARM + int i0 = (i1 - 1 + count)%count; + int i2 = (i1 + 1)%count; + + const struct cpSplittingPlane *planes = poly->planes; + cpHashValue hashid = poly->shape.hashid; + if(cpvdot(n, planes[i1].n) > cpvdot(n, planes[i2].n)){ + struct Edge edge = {{planes[i0].v0, CP_HASH_PAIR(hashid, i0)}, {planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, poly->r, planes[i1].n}; + return edge; + } else { + struct Edge edge = {{planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, {planes[i2].v0, CP_HASH_PAIR(hashid, i2)}, poly->r, planes[i2].n}; + return edge; + } +} + +static struct Edge +SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n) +{ + cpHashValue hashid = seg->shape.hashid; + if(cpvdot(seg->tn, n) > 0.0){ + struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn}; + return edge; + } else { + struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)}; + return edge; + } +} + +// Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2 +// The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped. +static inline cpFloat +ClosestT(const cpVect a, const cpVect b) +{ + cpVect delta = cpvsub(b, a); + return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f); +} + +// Basically the same as cpvlerp(), except t = [-1, 1] +static inline cpVect +LerpT(const cpVect a, const cpVect b, const cpFloat t) +{ + cpFloat ht = 0.5f*t; + return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht)); +} + +// Closest points on the surface of two shapes. +struct ClosestPoints { + // Surface points in absolute coordinates. + cpVect a, b; + // Minimum separating axis of the two shapes. + cpVect n; + // Signed distance between the points. + cpFloat d; + // Concatenation of the id's of the minkoski points. + cpCollisionID id; +}; + +// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0) +static inline struct ClosestPoints +ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1) +{ + // Find the closest p(t) on the minkowski difference to (0, 0) + cpFloat t = ClosestT(v0.ab, v1.ab); + cpVect p = LerpT(v0.ab, v1.ab, t); + + // Interpolate the original support points using the same 't' value as above. + // This gives you the closest surface points in absolute coordinates. NEAT! + cpVect pa = LerpT(v0.a, v1.a, t); + cpVect pb = LerpT(v0.b, v1.b, t); + cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF); + + // First try calculating the MSA from the minkowski difference edge. + // This gives us a nice, accurate MSA when the surfaces are close together. + cpVect delta = cpvsub(v1.ab, v0.ab); + cpVect n = cpvnormalize(cpvrperp(delta)); + cpFloat d = cpvdot(n, p); + + if(d <= 0.0f || (-1.0f < t && t < 1.0f)){ + // If the shapes are overlapping, or we have a regular vertex/edge collision, we are done. + struct ClosestPoints points = {pa, pb, n, d, id}; + return points; + } else { + // Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference. + cpFloat d2 = cpvlength(p); + cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN)); + + struct ClosestPoints points = {pa, pb, n2, d2, id}; + return points; + } +} + +//MARK: EPA Functions + +static inline cpFloat +ClosestDist(const cpVect v0,const cpVect v1) +{ + return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1))); +} + +// Recursive implementation of the EPA loop. +// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface. +static struct ClosestPoints +EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration) +{ + int mini = 0; + cpFloat minDist = INFINITY; + + // TODO: precalculate this when building the hull and save a step. + // Find the closest segment hull[i] and hull[i + 1] to (0, 0) + for(int j=0, i=count-1; j MAX_GJK_ITERATIONS){ + cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); + return ClosestPointsNew(v0, v1); + } + + if(cpCheckPointGreater(v1.ab, v0.ab, cpvzero)){ + // Origin is behind axis. Flip and try again. + return GJKRecurse(ctx, v1, v0, iteration); + } else { + cpFloat t = ClosestT(v0.ab, v1.ab); + cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(cpvsub(v1.ab, v0.ab)) : cpvneg(LerpT(v0.ab, v1.ab, t))); + struct MinkowskiPoint p = Support(ctx, n); + +#if DRAW_GJK + ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1)); + cpVect c = cpvlerp(v0.ab, v1.ab, 0.5); + ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1)); + + ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1)); +#endif + + if(cpCheckPointGreater(p.ab, v0.ab, cpvzero) && cpCheckPointGreater(v1.ab, p.ab, cpvzero)){ + // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA. + cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration); + return EPA(ctx, v0, p, v1); + } else { + if(cpCheckAxis(v0.ab, v1.ab, p.ab, n)){ + // The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer. + cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); + return ClosestPointsNew(v0, v1); + } else { + // p was closer to the origin than our existing edge. + // Need to figure out which existing point to drop. + if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){ + return GJKRecurse(ctx, v0, p, iteration + 1); + } else { + return GJKRecurse(ctx, p, v1, iteration + 1); + } + } + } + } +} + +// Get a SupportPoint from a cached shape and index. +static struct SupportPoint +ShapePoint(const cpShape *shape, const int i) +{ + switch(shape->klass->type){ + case CP_CIRCLE_SHAPE: { + return SupportPointNew(((cpCircleShape *)shape)->tc, 0); + } case CP_SEGMENT_SHAPE: { + cpSegmentShape *seg = (cpSegmentShape *)shape; + return SupportPointNew(i == 0 ? seg->ta : seg->tb, i); + } case CP_POLY_SHAPE: { + cpPolyShape *poly = (cpPolyShape *)shape; + // Poly shapes may change vertex count. + int index = (i < poly->count ? i : 0); + return SupportPointNew(poly->planes[index].v0, index); + } default: { + return SupportPointNew(cpvzero, 0); + } + } +} + +// Find the closest points between two shapes using the GJK algorithm. +static struct ClosestPoints +GJK(const struct SupportContext *ctx, cpCollisionID *id) +{ +#if DRAW_GJK || DRAW_EPA + int count1 = 1; + int count2 = 1; + + switch(ctx->shape1->klass->type){ + case CP_SEGMENT_SHAPE: count1 = 2; break; + case CP_POLY_SHAPE: count1 = ((cpPolyShape *)ctx->shape1)->count; break; + default: break; + } + + switch(ctx->shape2->klass->type){ + case CP_SEGMENT_SHAPE: count1 = 2; break; + case CP_POLY_SHAPE: count2 = ((cpPolyShape *)ctx->shape2)->count; break; + default: break; + } + + + // draw the minkowski difference origin + cpVect origin = cpvzero; + ChipmunkDebugDrawDot(5.0, origin, RGBAColor(1,0,0,1)); + + int mdiffCount = count1*count2; + cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect)); + + for(int i=0; ishape2, j).p, ShapePoint(ctx->shape1, i).p); + mdiffVerts[i*count2 + j] = v; + ChipmunkDebugDrawDot(2.0, v, RGBAColor(1, 0, 0, 1)); + } + } + + cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect)); + int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0); + + ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25)); +#endif + + struct MinkowskiPoint v0, v1; + if(*id){ + // Use the minkowski points from the last frame as a starting point using the cached indexes. + v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF)); + v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF)); + } else { + // No cached indexes, use the shapes' bounding box centers as a guess for a starting axis. + cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb))); + v0 = Support(ctx, axis); + v1 = Support(ctx, cpvneg(axis)); + } + + struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1); + *id = points.id; + return points; +} + +//MARK: Contact Clipping + +// Given two support edges, find contact point pairs on their surfaces. +static inline void +ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info) +{ + cpFloat mindist = e1.r + e2.r; + if(points.d <= mindist){ +#ifdef DRAW_CLIP + ChipmunkDebugDrawFatSegment(e1.a.p, e1.b.p, e1.r, RGBAColor(0, 1, 0, 1), LAColor(0, 0)); + ChipmunkDebugDrawFatSegment(e2.a.p, e2.b.p, e2.r, RGBAColor(1, 0, 0, 1), LAColor(0, 0)); +#endif + cpVect n = info->n = points.n; + + // Distances along the axis parallel to n + cpFloat d_e1_a = cpvcross(e1.a.p, n); + cpFloat d_e1_b = cpvcross(e1.b.p, n); + cpFloat d_e2_a = cpvcross(e2.a.p, n); + cpFloat d_e2_b = cpvcross(e2.b.p, n); + + // TODO + min isn't a complete fix. + cpFloat e1_denom = 1.0f/(d_e1_b - d_e1_a + CPFLOAT_MIN); + cpFloat e2_denom = 1.0f/(d_e2_b - d_e2_a + CPFLOAT_MIN); + + // Project the endpoints of the two edges onto the opposing edge, clamping them as necessary. + // Compare the projected points to the collision normal to see if the shapes overlap there. + { + cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_b - d_e1_a)*e1_denom))); + cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_a - d_e2_a)*e2_denom))); + cpFloat dist = cpvdot(cpvsub(p2, p1), n); + if(dist <= 0.0f){ + cpHashValue hash_1a2b = CP_HASH_PAIR(e1.a.hash, e2.b.hash); + cpCollisionInfoPushContact(info, p1, p2, hash_1a2b); + } + }{ + cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_a - d_e1_a)*e1_denom))); + cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_b - d_e2_a)*e2_denom))); + cpFloat dist = cpvdot(cpvsub(p2, p1), n); + if(dist <= 0.0f){ + cpHashValue hash_1b2a = CP_HASH_PAIR(e1.b.hash, e2.a.hash); + cpCollisionInfoPushContact(info, p1, p2, hash_1b2a); + } + } + } +} + +//MARK: Collision Functions + +typedef void (*CollisionFunc)(const cpShape *a, const cpShape *b, struct cpCollisionInfo *info); + +// Collide circle shapes. +static void +CircleToCircle(const cpCircleShape *c1, const cpCircleShape *c2, struct cpCollisionInfo *info) +{ + cpFloat mindist = c1->r + c2->r; + cpVect delta = cpvsub(c2->tc, c1->tc); + cpFloat distsq = cpvlengthsq(delta); + + if(distsq < mindist*mindist){ + cpFloat dist = cpfsqrt(distsq); + cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)); + cpCollisionInfoPushContact(info, cpvadd(c1->tc, cpvmult(n, c1->r)), cpvadd(c2->tc, cpvmult(n, -c2->r)), 0); + } +} + +static void +CircleToSegment(const cpCircleShape *circle, const cpSegmentShape *segment, struct cpCollisionInfo *info) +{ + cpVect seg_a = segment->ta; + cpVect seg_b = segment->tb; + cpVect center = circle->tc; + + // Find the closest point on the segment to the circle. + 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)); + + // Compare the radii of the two shapes to see if they are colliding. + cpFloat mindist = circle->r + segment->r; + cpVect delta = cpvsub(closest, center); + cpFloat distsq = cpvlengthsq(delta); + if(distsq < mindist*mindist){ + cpFloat dist = cpfsqrt(distsq); + // Handle coincident shapes as gracefully as possible. + cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : segment->tn); + + // Reject endcap collisions if tangents are provided. + cpVect rot = cpBodyGetRotation(segment->shape.body); + if( + (closest_t != 0.0f || cpvdot(n, cpvrotate(segment->a_tangent, rot)) >= 0.0) && + (closest_t != 1.0f || cpvdot(n, cpvrotate(segment->b_tangent, rot)) >= 0.0) + ){ + cpCollisionInfoPushContact(info, cpvadd(center, cpvmult(n, circle->r)), cpvadd(closest, cpvmult(n, -segment->r)), 0); + } + } +} + +static void +SegmentToSegment(const cpSegmentShape *seg1, const cpSegmentShape *seg2, struct cpCollisionInfo *info) +{ + struct SupportContext context = {(cpShape *)seg1, (cpShape *)seg2, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)SegmentSupportPoint}; + struct ClosestPoints points = GJK(&context, &info->id); + +#if DRAW_CLOSEST +#if PRINT_LOG +// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); +#endif + + ChipmunkDebugDrawDot(6.0, points.a, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawDot(6.0, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); +#endif + + cpVect n = points.n; + cpVect rot1 = cpBodyGetRotation(seg1->shape.body); + cpVect rot2 = cpBodyGetRotation(seg2->shape.body); + + // If the closest points are nearer than the sum of the radii... + if( + points.d <= (seg1->r + seg2->r) && ( + // Reject endcap collisions if tangents are provided. + (!cpveql(points.a, seg1->ta) || cpvdot(n, cpvrotate(seg1->a_tangent, rot1)) <= 0.0) && + (!cpveql(points.a, seg1->tb) || cpvdot(n, cpvrotate(seg1->b_tangent, rot1)) <= 0.0) && + (!cpveql(points.b, seg2->ta) || cpvdot(n, cpvrotate(seg2->a_tangent, rot2)) >= 0.0) && + (!cpveql(points.b, seg2->tb) || cpvdot(n, cpvrotate(seg2->b_tangent, rot2)) >= 0.0) + ) + ){ + ContactPoints(SupportEdgeForSegment(seg1, n), SupportEdgeForSegment(seg2, cpvneg(n)), points, info); + } +} + +static void +PolyToPoly(const cpPolyShape *poly1, const cpPolyShape *poly2, struct cpCollisionInfo *info) +{ + struct SupportContext context = {(cpShape *)poly1, (cpShape *)poly2, (SupportPointFunc)PolySupportPoint, (SupportPointFunc)PolySupportPoint}; + struct ClosestPoints points = GJK(&context, &info->id); + +#if DRAW_CLOSEST +#if PRINT_LOG +// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); +#endif + + ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); +#endif + + // If the closest points are nearer than the sum of the radii... + if(points.d - poly1->r - poly2->r <= 0.0){ + ContactPoints(SupportEdgeForPoly(poly1, points.n), SupportEdgeForPoly(poly2, cpvneg(points.n)), points, info); + } +} + +static void +SegmentToPoly(const cpSegmentShape *seg, const cpPolyShape *poly, struct cpCollisionInfo *info) +{ + struct SupportContext context = {(cpShape *)seg, (cpShape *)poly, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)PolySupportPoint}; + struct ClosestPoints points = GJK(&context, &info->id); + +#if DRAW_CLOSEST +#if PRINT_LOG +// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); +#endif + + ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); +#endif + + cpVect n = points.n; + cpVect rot = cpBodyGetRotation(seg->shape.body); + + if( + // If the closest points are nearer than the sum of the radii... + points.d - seg->r - poly->r <= 0.0 && ( + // Reject endcap collisions if tangents are provided. + (!cpveql(points.a, seg->ta) || cpvdot(n, cpvrotate(seg->a_tangent, rot)) <= 0.0) && + (!cpveql(points.a, seg->tb) || cpvdot(n, cpvrotate(seg->b_tangent, rot)) <= 0.0) + ) + ){ + ContactPoints(SupportEdgeForSegment(seg, n), SupportEdgeForPoly(poly, cpvneg(n)), points, info); + } +} + +static void +CircleToPoly(const cpCircleShape *circle, const cpPolyShape *poly, struct cpCollisionInfo *info) +{ + struct SupportContext context = {(cpShape *)circle, (cpShape *)poly, (SupportPointFunc)CircleSupportPoint, (SupportPointFunc)PolySupportPoint}; + struct ClosestPoints points = GJK(&context, &info->id); + +#if DRAW_CLOSEST + ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); + ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); +#endif + + // If the closest points are nearer than the sum of the radii... + if(points.d <= circle->r + poly->r){ + cpVect n = info->n = points.n; + cpCollisionInfoPushContact(info, cpvadd(points.a, cpvmult(n, circle->r)), cpvadd(points.b, cpvmult(n, poly->r)), 0); + } +} + +static void +CollisionError(const cpShape *circle, const cpShape *poly, struct cpCollisionInfo *info) +{ + cpAssertHard(cpFalse, "Internal Error: Shape types are not sorted."); +} + + +static const CollisionFunc BuiltinCollisionFuncs[9] = { + (CollisionFunc)CircleToCircle, + CollisionError, + CollisionError, + (CollisionFunc)CircleToSegment, + (CollisionFunc)SegmentToSegment, + CollisionError, + (CollisionFunc)CircleToPoly, + (CollisionFunc)SegmentToPoly, + (CollisionFunc)PolyToPoly, +}; +static const CollisionFunc *CollisionFuncs = BuiltinCollisionFuncs; + +struct cpCollisionInfo +cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts) +{ + struct cpCollisionInfo info = {a, b, id, cpvzero, 0, contacts}; + + // Make sure the shape types are in order. + if(a->klass->type > b->klass->type){ + info.a = b; + info.b = a; + } + + CollisionFuncs[info.a->klass->type + info.b->klass->type*CP_NUM_SHAPES](info.a, info.b, &info); + +// if(0){ +// for(int i=0; iklass = klass; + + constraint->a = a; + constraint->b = b; + constraint->space = NULL; + + constraint->next_a = NULL; + constraint->next_b = NULL; + + constraint->maxForce = (cpFloat)INFINITY; + constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f); + constraint->maxBias = (cpFloat)INFINITY; + + constraint->collideBodies = cpTrue; + + constraint->preSolve = NULL; + constraint->postSolve = NULL; +} + +cpSpace * +cpConstraintGetSpace(const cpConstraint *constraint) +{ + return constraint->space; +} + +cpBody * +cpConstraintGetBodyA(const cpConstraint *constraint) +{ + return constraint->a; +} + +cpBody * +cpConstraintGetBodyB(const cpConstraint *constraint) +{ + return constraint->b; +} + +cpFloat +cpConstraintGetMaxForce(const cpConstraint *constraint) +{ + return constraint->maxForce; +} + +void +cpConstraintSetMaxForce(cpConstraint *constraint, cpFloat maxForce) +{ + cpAssertHard(maxForce >= 0.0f, "maxForce must be positive."); + cpConstraintActivateBodies(constraint); + constraint->maxForce = maxForce; +} + +cpFloat +cpConstraintGetErrorBias(const cpConstraint *constraint) +{ + return constraint->errorBias; +} + +void +cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias) +{ + cpAssertHard(errorBias >= 0.0f, "errorBias must be positive."); + cpConstraintActivateBodies(constraint); + constraint->errorBias = errorBias; +} + +cpFloat +cpConstraintGetMaxBias(const cpConstraint *constraint) +{ + return constraint->maxBias; +} + +void +cpConstraintSetMaxBias(cpConstraint *constraint, cpFloat maxBias) +{ + cpAssertHard(maxBias >= 0.0f, "maxBias must be positive."); + cpConstraintActivateBodies(constraint); + constraint->maxBias = maxBias; +} + +cpBool +cpConstraintGetCollideBodies(const cpConstraint *constraint) +{ + return constraint->collideBodies; +} + +void +cpConstraintSetCollideBodies(cpConstraint *constraint, cpBool collideBodies) +{ + cpConstraintActivateBodies(constraint); + constraint->collideBodies = collideBodies; +} + +cpConstraintPreSolveFunc +cpConstraintGetPreSolveFunc(const cpConstraint *constraint) +{ + return constraint->preSolve; +} + +void +cpConstraintSetPreSolveFunc(cpConstraint *constraint, cpConstraintPreSolveFunc preSolveFunc) +{ + constraint->preSolve = preSolveFunc; +} + +cpConstraintPostSolveFunc +cpConstraintGetPostSolveFunc(const cpConstraint *constraint) +{ + return constraint->postSolve; +} + +void +cpConstraintSetPostSolveFunc(cpConstraint *constraint, cpConstraintPostSolveFunc postSolveFunc) +{ + constraint->postSolve = postSolveFunc; +} + +cpDataPointer +cpConstraintGetUserData(const cpConstraint *constraint) +{ + return constraint->userData; +} + +void +cpConstraintSetUserData(cpConstraint *constraint, cpDataPointer userData) +{ + constraint->userData = userData; +} + + +cpFloat +cpConstraintGetImpulse(cpConstraint *constraint) +{ + return constraint->klass->getImpulse(constraint); +} diff --git a/Chipmunk2D/src/cpDampedRotarySpring.c b/Chipmunk2D/src/cpDampedRotarySpring.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpDampedRotarySpring.c @@ -0,0 +1,178 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static cpFloat +defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ + return (relativeAngle - spring->restAngle)*spring->stiffness; +} + +static void +preStep(cpDampedRotarySpring *spring, cpFloat dt) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + cpFloat moment = a->i_inv + b->i_inv; + cpAssertSoft(moment != 0.0, "Unsolvable spring."); + spring->iSum = 1.0f/moment; + + spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); + spring->target_wrn = 0.0f; + + // apply spring torque + cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; + spring->jAcc = j_spring; + + a->w -= j_spring*a->i_inv; + b->w += j_spring*b->i_inv; +} + +static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){} + +static void +applyImpulse(cpDampedRotarySpring *spring, cpFloat dt) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + // compute relative velocity + cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat w_damp = (spring->target_wrn - wrn)*spring->w_coef; + spring->target_wrn = wrn + w_damp; + + //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); + cpFloat j_damp = w_damp*spring->iSum; + spring->jAcc += j_damp; + + a->w += j_damp*a->i_inv; + b->w -= j_damp*b->i_inv; +} + +static cpFloat +getImpulse(cpDampedRotarySpring *spring) +{ + return spring->jAcc; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpDampedRotarySpring * +cpDampedRotarySpringAlloc(void) +{ + return (cpDampedRotarySpring *)cpcalloc(1, sizeof(cpDampedRotarySpring)); +} + +cpDampedRotarySpring * +cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, &klass, a, b); + + spring->restAngle = restAngle; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque; + + spring->jAcc = 0.0f; + + return spring; +} + +cpConstraint * +cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping); +} + +cpBool +cpConstraintIsDampedRotarySpring(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpFloat +cpDampedRotarySpringGetRestAngle(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + return ((cpDampedRotarySpring *)constraint)->restAngle; +} + +void +cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedRotarySpring *)constraint)->restAngle = restAngle; +} + +cpFloat +cpDampedRotarySpringGetStiffness(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + return ((cpDampedRotarySpring *)constraint)->stiffness; +} + +void +cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedRotarySpring *)constraint)->stiffness = stiffness; +} + +cpFloat +cpDampedRotarySpringGetDamping(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + return ((cpDampedRotarySpring *)constraint)->damping; +} + +void +cpDampedRotarySpringSetDamping(cpConstraint *constraint, cpFloat damping) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedRotarySpring *)constraint)->damping = damping; +} + +cpDampedRotarySpringTorqueFunc +cpDampedRotarySpringGetSpringTorqueFunc(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + return ((cpDampedRotarySpring *)constraint)->springTorqueFunc; +} + +void +cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc) +{ + cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedRotarySpring *)constraint)->springTorqueFunc = springTorqueFunc; +} diff --git a/Chipmunk2D/src/cpDampedSpring.c b/Chipmunk2D/src/cpDampedSpring.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpDampedSpring.c @@ -0,0 +1,216 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static cpFloat +defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ + return (spring->restLength - dist)*spring->stiffness; +} + +static void +preStep(cpDampedSpring *spring, cpFloat dt) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + spring->r1 = cpTransformVect(a->transform, cpvsub(spring->anchorA, a->cog)); + spring->r2 = cpTransformVect(b->transform, cpvsub(spring->anchorB, b->cog)); + + cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); + cpFloat dist = cpvlength(delta); + spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); + cpAssertSoft(k != 0.0, "Unsolvable spring."); + spring->nMass = 1.0f/k; + + spring->target_vrn = 0.0f; + spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); + + // apply spring force + cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); + cpFloat j_spring = spring->jAcc = f_spring*dt; + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_spring)); +} + +static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){} + +static void +applyImpulse(cpDampedSpring *spring, cpFloat dt) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + cpVect n = spring->n; + cpVect r1 = spring->r1; + cpVect r2 = spring->r2; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n); + + // compute velocity loss from drag + cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef; + spring->target_vrn = vrn + v_damp; + + cpFloat j_damp = v_damp*spring->nMass; + spring->jAcc += j_damp; + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_damp)); +} + +static cpFloat +getImpulse(cpDampedSpring *spring) +{ + return spring->jAcc; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpDampedSpring * +cpDampedSpringAlloc(void) +{ + return (cpDampedSpring *)cpcalloc(1, sizeof(cpDampedSpring)); +} + +cpDampedSpring * +cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, &klass, a, b); + + spring->anchorA = anchorA; + spring->anchorB = anchorB; + + spring->restLength = restLength; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce; + + spring->jAcc = 0.0f; + + return spring; +} + +cpConstraint * +cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchorA, anchorB, restLength, stiffness, damping); +} + +cpBool +cpConstraintIsDampedSpring(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpVect +cpDampedSpringGetAnchorA(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->anchorA; +} + +void +cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->anchorA = anchorA; +} + +cpVect +cpDampedSpringGetAnchorB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->anchorB; +} + +void +cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->anchorB = anchorB; +} + +cpFloat +cpDampedSpringGetRestLength(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->restLength; +} + +void +cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->restLength = restLength; +} + +cpFloat +cpDampedSpringGetStiffness(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->stiffness; +} + +void +cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->stiffness = stiffness; +} + +cpFloat +cpDampedSpringGetDamping(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->damping; +} + +void +cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->damping = damping; +} + +cpDampedSpringForceFunc +cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + return ((cpDampedSpring *)constraint)->springForceFunc; +} + +void +cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc) +{ + cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); + cpConstraintActivateBodies(constraint); + ((cpDampedSpring *)constraint)->springForceFunc = springForceFunc; +} diff --git a/Chipmunk2D/src/cpGearJoint.c b/Chipmunk2D/src/cpGearJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpGearJoint.c @@ -0,0 +1,145 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpGearJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias); +} + +static void +applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv*joint->ratio_inv; + b->w += j*b->i_inv; +} + +static void +applyImpulse(cpGearJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // 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, -jMax, jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv*joint->ratio_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpGearJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpGearJoint * +cpGearJointAlloc(void) +{ + return (cpGearJoint *)cpcalloc(1, sizeof(cpGearJoint)); +} + +cpGearJoint * +cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->phase = phase; + joint->ratio = ratio; + joint->ratio_inv = 1.0f/ratio; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio); +} + +cpBool +cpConstraintIsGearJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpFloat +cpGearJointGetPhase(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); + return ((cpGearJoint *)constraint)->phase; +} + +void +cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase) +{ + cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); + cpConstraintActivateBodies(constraint); + ((cpGearJoint *)constraint)->phase = phase; +} + +cpFloat +cpGearJointGetRatio(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); + return ((cpGearJoint *)constraint)->ratio; +} + +void +cpGearJointSetRatio(cpConstraint *constraint, cpFloat ratio) +{ + cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); + cpConstraintActivateBodies(constraint); + ((cpGearJoint *)constraint)->ratio = ratio; + ((cpGearJoint *)constraint)->ratio_inv = 1.0f/ratio; +} diff --git a/Chipmunk2D/src/cpGrooveJoint.c b/Chipmunk2D/src/cpGrooveJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpGrooveJoint.c @@ -0,0 +1,197 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpGrooveJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate endpoints in worldspace + cpVect ta = cpTransformPoint(a->transform, joint->grv_a); + cpVect tb = cpTransformPoint(a->transform, joint->grv_b); + + // calculate axis + cpVect n = cpTransformVect(a->transform, joint->grv_n); + cpFloat d = cpvdot(ta, n); + + joint->grv_tn = n; + joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); + + // calculate tangential distance along the axis of r2 + cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); + // calculate clamping factor and r2 + if(td <= cpvcross(ta, n)){ + joint->clamp = 1.0f; + joint->r1 = cpvsub(ta, a->p); + } else if(td >= cpvcross(tb, n)){ + joint->clamp = -1.0f; + joint->r1 = cpvsub(tb, a->p); + } else { + joint->clamp = 0.0f; + joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + } + + // Calculate mass tensor + 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)); + joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); +} + +static void +applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); +} + +static inline cpVect +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->constraint.maxForce*dt); +} + +static void +applyImpulse(cpGrooveJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute impulse + cpVect vr = relative_velocity(a, b, r1, r2); + + cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); + cpVect jOld = joint->jAcc; + joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpGrooveJoint *joint) +{ + return cpvlength(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpGrooveJoint * +cpGrooveJointAlloc(void) +{ + return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint)); +} + +cpGrooveJoint * +cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->grv_a = groove_a; + joint->grv_b = groove_b; + joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); + joint->anchorB = anchorB; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB) +{ + return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchorB); +} + +cpBool +cpConstraintIsGrooveJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpVect +cpGrooveJointGetGrooveA(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + return ((cpGrooveJoint *)constraint)->grv_a; +} + +void +cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + + g->grv_a = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value))); + + cpConstraintActivateBodies(constraint); +} + +cpVect +cpGrooveJointGetGrooveB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + return ((cpGrooveJoint *)constraint)->grv_b; +} + +void +cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + + g->grv_b = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a))); + + cpConstraintActivateBodies(constraint); +} + +cpVect +cpGrooveJointGetAnchorB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + return ((cpGrooveJoint *)constraint)->anchorB; +} + +void +cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) +{ + cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); + cpConstraintActivateBodies(constraint); + ((cpGrooveJoint *)constraint)->anchorB = anchorB; +} diff --git a/Chipmunk2D/src/cpHashSet.c b/Chipmunk2D/src/cpHashSet.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpHashSet.c @@ -0,0 +1,253 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" +#include "prime.h" + +typedef struct cpHashSetBin { + void *elt; + cpHashValue hash; + struct cpHashSetBin *next; +} cpHashSetBin; + +struct cpHashSet { + unsigned int entries, size; + + cpHashSetEqlFunc eql; + void *default_value; + + cpHashSetBin **table; + cpHashSetBin *pooledBins; + + cpArray *allocatedBuffers; +}; + +void +cpHashSetFree(cpHashSet *set) +{ + if(set){ + cpfree(set->table); + + cpArrayFreeEach(set->allocatedBuffers, cpfree); + cpArrayFree(set->allocatedBuffers); + + cpfree(set); + } +} + +cpHashSet * +cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc) +{ + cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); + + set->size = next_prime(size); + set->entries = 0; + + set->eql = eqlFunc; + set->default_value = NULL; + + set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *)); + set->pooledBins = NULL; + + set->allocatedBuffers = cpArrayNew(0); + + return set; +} + +void +cpHashSetSetDefaultValue(cpHashSet *set, void *default_value) +{ + set->default_value = default_value; +} + +static int +setIsFull(cpHashSet *set) +{ + return (set->entries >= set->size); +} + +static void +cpHashSetResize(cpHashSet *set) +{ + // Get the next approximate doubled prime. + unsigned int newSize = next_prime(set->size + 1); + // Allocate a new table. + cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *)); + + // Iterate over the chains. + 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; + + cpHashValue idx = bin->hash%newSize; + bin->next = newTable[idx]; + newTable[idx] = bin; + + bin = next; + } + } + + cpfree(set->table); + + set->table = newTable; + set->size = newSize; +} + +static inline void +recycleBin(cpHashSet *set, cpHashSetBin *bin) +{ + bin->next = set->pooledBins; + set->pooledBins = bin; + bin->elt = NULL; +} + +static cpHashSetBin * +getUnusedBin(cpHashSet *set) +{ + cpHashSetBin *bin = set->pooledBins; + + if(bin){ + set->pooledBins = bin->next; + return bin; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin); + cpAssertHard(count, "Internal Error: Buffer size is too small."); + + cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(set->allocatedBuffers, buffer); + + // push all but the first one, return it instead + for(int i=1; ientries; +} + +void * +cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, cpHashSetTransFunc trans, void *data) +{ + cpHashValue idx = hash%set->size; + + // Find the bin with the matching element. + cpHashSetBin *bin = set->table[idx]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + // Create it if necessary. + if(!bin){ + bin = getUnusedBin(set); + bin->hash = hash; + bin->elt = (trans ? trans(ptr, data) : data); + + bin->next = set->table[idx]; + set->table[idx] = bin; + + set->entries++; + if(setIsFull(set)) cpHashSetResize(set); + } + + return bin->elt; +} + +void * +cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) +{ + cpHashValue idx = hash%set->size; + + cpHashSetBin **prev_ptr = &set->table[idx]; + cpHashSetBin *bin = set->table[idx]; + + // Find the bin + while(bin && !set->eql(ptr, bin->elt)){ + prev_ptr = &bin->next; + bin = bin->next; + } + + // Remove it if it exists. + if(bin){ + // Update the previous linked list pointer + (*prev_ptr) = bin->next; + set->entries--; + + void *elt = bin->elt; + recycleBin(set, bin); + + return elt; + } + + return NULL; +} + +void * +cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) +{ + cpHashValue idx = hash%set->size; + cpHashSetBin *bin = set->table[idx]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + return (bin ? bin->elt : set->default_value); +} + +void +cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) +{ + for(unsigned int i=0; isize; i++){ + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + func(bin->elt, data); + bin = next; + } + } +} + +void +cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data) +{ + 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]; + while(bin){ + cpHashSetBin *next = bin->next; + + if(func(bin->elt, data)){ + prev_ptr = &bin->next; + } else { + (*prev_ptr) = next; + + set->entries--; + recycleBin(set, bin); + } + + bin = next; + } + } +} diff --git a/Chipmunk2D/src/cpHastySpace.c b/Chipmunk2D/src/cpHastySpace.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpHastySpace.c @@ -0,0 +1,694 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +#include +#include + +//TODO: Move all the thread stuff to another file + +//#include +#ifndef _WIN32 +#include +#include +#else +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + +#ifndef NOMINMAX +#define NOMINMAX +#endif + +#include // _beginthreadex +#include + +#ifndef ETIMEDOUT +#define ETIMEDOUT 1 +#endif + +// Simple pthread implementation for Windows +// Made from scratch to avoid the LGPL licence from pthread-win32 +enum { + SIGNAL = 0, + BROADCAST = 1, + MAX_EVENTS = 2 +}; + +typedef HANDLE pthread_t; +typedef struct +{ + // Based on http://www.cs.wustl.edu/~schmidt/win32-cv-1.html since Windows has no condition variable until NT6 + UINT waiters_count; + // Count of the number of waiters. + + CRITICAL_SECTION waiters_count_lock; + // Serialize access to . + + HANDLE events[MAX_EVENTS]; +} pthread_cond_t; +typedef CRITICAL_SECTION pthread_mutex_t; + +typedef struct {} pthread_condattr_t; // Dummy; + +int pthread_cond_destroy(pthread_cond_t* cv) +{ + CloseHandle(cv->events[BROADCAST]); + CloseHandle(cv->events[SIGNAL]); + + DeleteCriticalSection(&cv->waiters_count_lock); + + return 0; +} + +int pthread_cond_init(pthread_cond_t* cv, const pthread_condattr_t* attr) +{ + // Initialize the count to 0. + cv->waiters_count = 0; + + // Create an auto-reset event. + cv->events[SIGNAL] = CreateEvent(NULL, // no security + FALSE, // auto-reset event + FALSE, // non-signaled initially + NULL); // unnamed + + // Create a manual-reset event. + cv->events[BROADCAST] = CreateEvent(NULL, // no security + TRUE, // manual-reset + FALSE, // non-signaled initially + NULL); // unnamed + + InitializeCriticalSection(&cv->waiters_count_lock); + + return 0; +} + +int pthread_cond_broadcast(pthread_cond_t *cv) +{ + // Avoid race conditions. + EnterCriticalSection(&cv->waiters_count_lock); + int have_waiters = cv->waiters_count > 0; + LeaveCriticalSection(&cv->waiters_count_lock); + + if (have_waiters) + SetEvent(cv->events[BROADCAST]); + + return 0; +} + +int pthread_cond_signal(pthread_cond_t* cv) +{ + // Avoid race conditions. + EnterCriticalSection(&cv->waiters_count_lock); + int have_waiters = cv->waiters_count > 0; + LeaveCriticalSection(&cv->waiters_count_lock); + + if (have_waiters) + SetEvent(cv->events[SIGNAL]); + + return 0; +} + +int pthread_cond_wait(pthread_cond_t* cv, pthread_mutex_t* external_mutex) +{ + // Avoid race conditions. + EnterCriticalSection(&cv->waiters_count_lock); + cv->waiters_count++; + LeaveCriticalSection(&cv->waiters_count_lock); + + // It's ok to release the here since Win32 + // manual-reset events maintain state when used with + // . This avoids the "lost wakeup" bug... + LeaveCriticalSection(external_mutex); + + // Wait for either event to become signaled due to + // being called or being called. + int result = WaitForMultipleObjects(2, cv->events, FALSE, INFINITE); + + EnterCriticalSection(&cv->waiters_count_lock); + cv->waiters_count--; + int last_waiter = + result == WAIT_OBJECT_0 + BROADCAST + && cv->waiters_count == 0; + LeaveCriticalSection(&cv->waiters_count_lock); + + // Some thread called . + if (last_waiter) + // We're the last waiter to be notified or to stop waiting, so + // reset the manual event. + ResetEvent(cv->events[BROADCAST]); + + // Reacquire the . + EnterCriticalSection(external_mutex); + + return result == WAIT_TIMEOUT ? ETIMEDOUT : 0; +} + +typedef struct {} pthread_mutexattr_t; //< Dummy + +int pthread_mutex_init(pthread_mutex_t* mutex, const pthread_mutexattr_t* attr) +{ + InitializeCriticalSection(mutex); + return 0; +} + +int pthread_mutex_destroy(pthread_mutex_t* mutex) +{ + DeleteCriticalSection(mutex); + return 0; +} + +int pthread_mutex_lock(pthread_mutex_t* mutex) +{ + EnterCriticalSection(mutex); + return 0; +} + +int pthread_mutex_unlock(pthread_mutex_t* mutex) +{ + LeaveCriticalSection(mutex); + return 0; +} + +typedef struct {} pthread_attr_t; + +typedef struct +{ + void *(*start_routine) (void *); + void* arg; +} pthread_internal_thread; + +unsigned int __stdcall ThreadProc(void* userdata) +{ + pthread_internal_thread* ud = (pthread_internal_thread*) userdata; + ud->start_routine(ud->arg); + + free(ud); + + return 0; +} + +int pthread_create(pthread_t* thread, const pthread_attr_t* attr, void *(*start_routine) (void *), void *arg) +{ + pthread_internal_thread* ud = (pthread_internal_thread*) malloc(sizeof(pthread_internal_thread)); + ud->start_routine = start_routine; + ud->arg = arg; + + *thread = (HANDLE) (_beginthreadex(NULL, 0, &ThreadProc, ud, 0, NULL)); + if (!*thread) + return 1; + + return 0; +} + +int pthread_join(pthread_t thread, void **value_ptr) +{ + WaitForSingleObject(thread, INFINITE); + CloseHandle(thread); + + return 0; +} + +#endif + +#include "chipmunk_private.h" +#include "cpHastySpace.h" + + +//MARK: ARM NEON Solver + +#if __ARM_NEON__ +#include + +// Tested and known to work fine with Clang 3.0 and GCC 4.2 +// Doesn't work with Clang 1.6, and I have no idea why. +#if defined(__clang_major__) && __clang_major__ < 3 + #error Compiler not supported. +#endif + +#if CP_USE_DOUBLES + #if !__arm64 + #error Cannot use CP_USE_DOUBLES on 32 bit ARM. + #endif + + typedef float64_t cpFloat_t; + typedef float64x2_t cpFloatx2_t; + #define vld vld1q_f64 + #define vdup_n vdupq_n_f64 + #define vst vst1q_f64 + #define vst_lane vst1q_lane_f64 + #define vadd vaddq_f64 + #define vsub vsubq_f64 + #define vpadd vpaddq_f64 + #define vmul vmulq_f64 + #define vmul_n vmulq_n_f64 + #define vneg vnegq_f64 + #define vget_lane vgetq_lane_f64 + #define vset_lane vsetq_lane_f64 + #define vmin vminq_f64 + #define vmax vmaxq_f64 + #define vrev(__a) __builtin_shufflevector(__a, __a, 1, 0) +#else + typedef float32_t cpFloat_t; + typedef float32x2_t cpFloatx2_t; + #define vld vld1_f32 + #define vdup_n vdup_n_f32 + #define vst vst1_f32 + #define vst_lane vst1_lane_f32 + #define vadd vadd_f32 + #define vsub vsub_f32 + #define vpadd vpadd_f32 + #define vmul vmul_f32 + #define vmul_n vmul_n_f32 + #define vneg vneg_f32 + #define vget_lane vget_lane_f32 + #define vset_lane vset_lane_f32 + #define vmin vmin_f32 + #define vmax vmax_f32 + #define vrev vrev64_f32 +#endif + +// TODO could probably do better here, maybe using vcreate? +// especially for the constants +// Maybe use the {} notation for GCC/Clang? +static inline cpFloatx2_t +vmake(cpFloat_t x, cpFloat_t y) +{ +// cpFloatx2_t v = {}; +// v = vset_lane(x, v, 0); +// v = vset_lane(y, v, 1); +// +// return v; + + // This might not be super compatible, but all the NEON headers use it... + return (cpFloatx2_t){x, y}; +} + +static void +cpArbiterApplyImpulse_NEON(cpArbiter *arb) +{ + cpBody *a = arb->body_a; + cpBody *b = arb->body_b; + cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr); + cpFloatx2_t n = vld((cpFloat_t *)&arb->n); + cpFloat_t friction = arb->u; + + int numContacts = arb->count; + struct cpContact *contacts = arb->contacts; + for(int i=0; ir1); + cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2); + + cpFloatx2_t perp = vmake(-1.0, 1.0); + cpFloatx2_t r1p = vmul(vrev(r1), perp); + cpFloatx2_t r2p = vmul(vrev(r2), perp); + + cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias); + cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias); + cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias); + + cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0))); + cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1))); + cpFloatx2_t vbr = vsub(vb2, vb1); + + cpFloatx2_t v_a = vld((cpFloat_t *)&a->v); + cpFloatx2_t v_b = vld((cpFloat_t *)&b->v); + cpFloatx2_t w = vmake(a->w, b->w); + cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0))); + cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1))); + cpFloatx2_t vr = vsub(v2, v1); + + cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n)); + + cpFloatx2_t v_offset = vmake(con->bias, -con->bounce); + cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc); + cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass); + jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0)); + cpFloatx2_t jApply = vsub(jbn_jn, jOld); + + cpFloatx2_t t = vmul(vrev(n), perp); + cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t); + cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp); + + cpFloatx2_t jtOld = {}; jtOld = vset_lane(con->jtAcc, jtOld, 0); + cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction)); + cpFloatx2_t jt = vmul_n(vrt, -con->tMass); + jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax)); + cpFloatx2_t jtApply = vsub(jt, jtOld); + + cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv); + cpFloatx2_t nperp = vmake(1.0, -1.0); + + cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0)); + cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp); + cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross)); + wBias = vadd(wBias, vmul(i_inv, biasCrosses)); + + vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv)); + vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv)); + + cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0))); + cpFloatx2_t jCross = vmul(vrev(j), nperp); + cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross)); + w = vadd(w, vmul(i_inv, crosses)); + + v_a = vsub(v_a, vmul_n(j, a->m_inv)); + v_b = vadd(v_b, vmul_n(j, b->m_inv)); + + // TODO would moving these earlier help pipeline them better? + vst((cpFloat_t *)&a->v_bias, vBias_a); + vst((cpFloat_t *)&b->v_bias, vBias_b); + vst_lane((cpFloat_t *)&a->w_bias, wBias, 0); + vst_lane((cpFloat_t *)&b->w_bias, wBias, 1); + + vst((cpFloat_t *)&a->v, v_a); + vst((cpFloat_t *)&b->v, v_b); + vst_lane((cpFloat_t *)&a->w, w, 0); + vst_lane((cpFloat_t *)&b->w, w, 1); + + vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0); + vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1); + vst_lane((cpFloat_t *)&con->jtAcc, jt, 0); + } +} + +#endif + +//MARK: PThreads + +// Right now using more than 2 threads probably wont help your performance any. +// If you are using a ridiculous number of iterations it could help though. +#define MAX_THREADS 2 + +struct ThreadContext { + pthread_t thread; + cpHastySpace *space; + unsigned long thread_num; +}; + +typedef void (*cpHastySpaceWorkFunction)(cpSpace *space, unsigned long worker, unsigned long worker_count); + +struct cpHastySpace { + cpSpace space; + + // Number of worker threads (including the main thread) + unsigned long num_threads; + + // Number of worker threads currently executing. (also including the main thread) + unsigned long num_working; + + // Number of constraints (plus contacts) that must exist per step to start the worker threads. + unsigned long constraint_count_threshold; + + pthread_mutex_t mutex; + pthread_cond_t cond_work, cond_resume; + + // Work function to invoke. + cpHastySpaceWorkFunction work; + + struct ThreadContext workers[MAX_THREADS - 1]; +}; + +static void * +WorkerThreadLoop(struct ThreadContext *context) +{ + cpHastySpace *hasty = context->space; + + unsigned long thread = context->thread_num; + unsigned long num_threads = hasty->num_threads; + + for(;;){ + pthread_mutex_lock(&hasty->mutex); { + if(--hasty->num_working == 0){ + pthread_cond_signal(&hasty->cond_resume); + } + + pthread_cond_wait(&hasty->cond_work, &hasty->mutex); + } pthread_mutex_unlock(&hasty->mutex); + + cpHastySpaceWorkFunction func = hasty->work; + if(func){ + hasty->work(&hasty->space, thread, num_threads); + } else { + break; + } + } + + return NULL; +} + +static void +RunWorkers(cpHastySpace *hasty, cpHastySpaceWorkFunction func) +{ + hasty->num_working = hasty->num_threads - 1; + hasty->work = func; + + if(hasty->num_working > 0){ + pthread_mutex_lock(&hasty->mutex); { + pthread_cond_broadcast(&hasty->cond_work); + } pthread_mutex_unlock(&hasty->mutex); + + func((cpSpace *)hasty, 0, hasty->num_threads); + + pthread_mutex_lock(&hasty->mutex); { + if(hasty->num_working > 0){ + pthread_cond_wait(&hasty->cond_resume, &hasty->mutex); + } + } pthread_mutex_unlock(&hasty->mutex); + } else { + func((cpSpace *)hasty, 0, hasty->num_threads); + } + + hasty->work = NULL; +} + +static void +Solver(cpSpace *space, unsigned long worker, unsigned long worker_count) +{ + cpArray *constraints = space->constraints; + cpArray *arbiters = space->arbiters; + + cpFloat dt = space->curr_dt; + unsigned long iterations = (space->iterations + worker_count - 1)/worker_count; + + for(unsigned long i=0; inum; j++){ + cpArbiter *arb = (cpArbiter *)arbiters->arr[j]; + #ifdef __ARM_NEON__ + cpArbiterApplyImpulse_NEON(arb); + #else + cpArbiterApplyImpulse(arb); + #endif + } + + for(int j=0; jnum; j++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; + constraint->klass->applyImpulse(constraint, dt); + } + } +} + +//MARK: Thread Management Functions + +static void +HaltThreads(cpHastySpace *hasty) +{ + pthread_mutex_t *mutex = &hasty->mutex; + pthread_mutex_lock(mutex); { + hasty->work = NULL; // NULL work function means break and exit + pthread_cond_broadcast(&hasty->cond_work); + } pthread_mutex_unlock(mutex); + + for(unsigned long i=0; i<(hasty->num_threads-1); i++){ + pthread_join(hasty->workers[i].thread, NULL); + } +} + +void +cpHastySpaceSetThreads(cpSpace *space, unsigned long threads) +{ +#if TARGET_IPHONE_SIMULATOR == 1 + // Individual values appear to be written non-atomically when compiled as debug for the simulator. + // No idea why, so threads are disabled. + threads = 1; +#endif + + cpHastySpace *hasty = (cpHastySpace *)space; + HaltThreads(hasty); + +#ifdef __APPLE__ + if(threads == 0){ + size_t size = sizeof(threads); + sysctlbyname("hw.ncpu", &threads, &size, NULL, 0); + } +#else + if(threads == 0) threads = 1; +#endif + + hasty->num_threads = (threads < MAX_THREADS ? threads : MAX_THREADS); + hasty->num_working = hasty->num_threads - 1; + + // Create the worker threads and wait for them to signal ready. + if(hasty->num_working > 0){ + pthread_mutex_lock(&hasty->mutex); + for(unsigned long i=0; i<(hasty->num_threads-1); i++){ + hasty->workers[i].space = hasty; + hasty->workers[i].thread_num = i + 1; + + pthread_create(&hasty->workers[i].thread, NULL, (void*(*)(void*))WorkerThreadLoop, &hasty->workers[i]); + } + + pthread_cond_wait(&hasty->cond_resume, &hasty->mutex); + pthread_mutex_unlock(&hasty->mutex); + } +} + +unsigned long +cpHastySpaceGetThreads(cpSpace *space) +{ + return ((cpHastySpace *)space)->num_threads; +} + +//MARK: Overriden cpSpace Functions. + +cpSpace * +cpHastySpaceNew(void) +{ + cpHastySpace *hasty = (cpHastySpace *)cpcalloc(1, sizeof(cpHastySpace)); + cpSpaceInit((cpSpace *)hasty); + + pthread_mutex_init(&hasty->mutex, NULL); + pthread_cond_init(&hasty->cond_work, NULL); + pthread_cond_init(&hasty->cond_resume, NULL); + + // TODO magic number, should test this more thoroughly. + hasty->constraint_count_threshold = 50; + + // Default to 1 thread for determinism. + hasty->num_threads = 1; + cpHastySpaceSetThreads((cpSpace *)hasty, 1); + + return (cpSpace *)hasty; +} + +void +cpHastySpaceFree(cpSpace *space) +{ + cpHastySpace *hasty = (cpHastySpace *)space; + + HaltThreads(hasty); + + pthread_mutex_destroy(&hasty->mutex); + pthread_cond_destroy(&hasty->cond_work); + pthread_cond_destroy(&hasty->cond_resume); + + cpSpaceFree(space); +} + +void +cpHastySpaceStep(cpSpace *space, cpFloat dt) +{ + // 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; + + cpArray *bodies = space->dynamicBodies; + cpArray *constraints = space->constraints; + cpArray *arbiters = space->arbiters; + + // Reset and empty the arbiter list. + for(int i=0; inum; i++){ + cpArbiter *arb = (cpArbiter *)arbiters->arr[i]; + arb->state = CP_ARBITER_STATE_NORMAL; + + // If both bodies are awake, unthread the arbiter from the contact graph. + if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){ + cpArbiterUnthread(arb); + } + } + arbiters->num = 0; + + 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->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); + cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); + } cpSpaceUnlock(space, cpFalse); + + // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) + cpSpaceProcessComponents(space, dt); + + 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); + } + + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + + cpConstraintPreSolveFunc preSolve = constraint->preSolve; + if(preSolve) preSolve(constraint, space); + + 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 = (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. + cpHastySpace *hasty = (cpHastySpace *)space; + if((unsigned long)(arbiters->num + constraints->num) > hasty->constraint_count_threshold){ + RunWorkers(hasty, Solver); + } else { + Solver(space, 0, 1); + } + + // 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->postSolveFunc(arb, space, handler->userData); + } + } cpSpaceUnlock(space, cpTrue); +} diff --git a/Chipmunk2D/src/cpMarch.c b/Chipmunk2D/src/cpMarch.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpMarch.c @@ -0,0 +1,157 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +#include +#include +#include + +#include "chipmunk.h" +#include "cpMarch.h" + + +typedef void (*cpMarchCellFunc)( + cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d, + cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1, + cpMarchSegmentFunc segment, void *segment_data +); + +// The looping and sample caching code is shared between cpMarchHard() and cpMarchSoft(). +static void +cpMarchCells( + cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, + cpMarchSegmentFunc segment, void *segment_data, + cpMarchSampleFunc sample, void *sample_data, + cpMarchCellFunc cell +){ + cpFloat x_denom = 1.0/(cpFloat)(x_samples - 1); + cpFloat y_denom = 1.0/(cpFloat)(y_samples - 1); + + // TODO range assertions and short circuit for 0 sized windows. + + // Keep a copy of the previous row to avoid double lookups. + cpFloat *buffer = (cpFloat *)cpcalloc(x_samples, sizeof(cpFloat)); + for(unsigned long i=0; it)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){ + case 0x1: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; + case 0x2: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; + case 0x3: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; + case 0x4: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; + case 0x5: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; + case 0x6: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); + seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; + case 0x7: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; + case 0x8: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; + case 0x9: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); + seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; + case 0xA: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; + case 0xB: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; + case 0xC: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; + case 0xD: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; + case 0xE: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; + default: break; // 0x0 and 0xF + } +} + +void +cpMarchSoft( + cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, + cpMarchSegmentFunc segment, void *segment_data, + cpMarchSampleFunc sample, void *sample_data +){ + cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellSoft); +} + + +// TODO should flip this around eventually. +static inline void +segs(cpVect a, cpVect b, cpVect c, cpMarchSegmentFunc f, void *data) +{ + seg(b, c, f, data); + seg(a, b, f, data); +} + +static void +cpMarchCellHard( + cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d, + cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1, + cpMarchSegmentFunc segment, void *segment_data +){ + // midpoints + cpFloat xm = cpflerp(x0, x1, 0.5f); + cpFloat ym = cpflerp(y0, y1, 0.5f); + + switch((a>t)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){ + case 0x1: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break; + case 0x2: segs(cpv(xm, y0), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; + case 0x3: seg(cpv(x0, ym), cpv(x1, ym), segment, segment_data); break; + case 0x4: segs(cpv(xm, y1), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break; + case 0x5: seg(cpv(xm, y1), cpv(xm, y0), segment, segment_data); break; + case 0x6: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data); + segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; + case 0x7: segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; + case 0x8: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; + case 0x9: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); + segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; + case 0xA: seg(cpv(xm, y0), cpv(xm, y1), segment, segment_data); break; + case 0xB: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; + case 0xC: seg(cpv(x1, ym), cpv(x0, ym), segment, segment_data); break; + case 0xD: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break; + case 0xE: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break; + default: break; // 0x0 and 0xF + } +} + +void +cpMarchHard( + cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, + cpMarchSegmentFunc segment, void *segment_data, + cpMarchSampleFunc sample, void *sample_data +){ + cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellHard); +} diff --git a/Chipmunk2D/src/cpPinJoint.c b/Chipmunk2D/src/cpPinJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpPinJoint.c @@ -0,0 +1,172 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpPinJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); + joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias); +} + +static void +applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static void +applyImpulse(cpPinJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + cpVect n = joint->n; + + // 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, -jnMax, jnMax); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpPinJoint *joint) +{ + return cpfabs(joint->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + + +cpPinJoint * +cpPinJointAlloc(void) +{ + return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint)); +} + +cpPinJoint * +cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchorA = anchorA; + joint->anchorB = anchorB; + + // STATIC_BODY_CHECK + cpVect p1 = (a ? cpTransformPoint(a->transform, anchorA) : anchorA); + cpVect p2 = (b ? cpTransformPoint(b->transform, anchorB) : anchorB); + 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; + + return joint; +} + +cpConstraint * +cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) +{ + return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchorA, anchorB); +} + +cpBool +cpConstraintIsPinJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpVect +cpPinJointGetAnchorA(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + return ((cpPinJoint *)constraint)->anchorA; +} + +void +cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + cpConstraintActivateBodies(constraint); + ((cpPinJoint *)constraint)->anchorA = anchorA; +} + +cpVect +cpPinJointGetAnchorB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + return ((cpPinJoint *)constraint)->anchorB; +} + +void +cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + cpConstraintActivateBodies(constraint); + ((cpPinJoint *)constraint)->anchorB = anchorB; +} + +cpFloat +cpPinJointGetDist(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + return ((cpPinJoint *)constraint)->dist; +} + +void +cpPinJointSetDist(cpConstraint *constraint, cpFloat dist) +{ + cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); + cpConstraintActivateBodies(constraint); + ((cpPinJoint *)constraint)->dist = dist; +} diff --git a/Chipmunk2D/src/cpPivotJoint.c b/Chipmunk2D/src/cpPivotJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpPivotJoint.c @@ -0,0 +1,152 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpPivotJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); + joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); + + // Calculate mass tensor + 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)); + joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); +} + +static void +applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); +} + +static void +applyImpulse(cpPivotJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + + // compute normal impulse + cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); + cpVect jOld = joint->jAcc; + joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpvlength(((cpPivotJoint *)joint)->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpPivotJoint * +cpPivotJointAlloc(void) +{ + return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint)); +} + +cpPivotJoint * +cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchorA = anchorA; + joint->anchorB = anchorB; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) +{ + return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchorA, anchorB); +} + +cpConstraint * +cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) +{ + cpVect anchorA = (a ? cpBodyWorldToLocal(a, pivot) : pivot); + cpVect anchorB = (b ? cpBodyWorldToLocal(b, pivot) : pivot); + return cpPivotJointNew2(a, b, anchorA, anchorB); +} + +cpBool +cpConstraintIsPivotJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpVect +cpPivotJointGetAnchorA(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); + return ((cpPivotJoint *)constraint)->anchorA; +} + +void +cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) +{ + cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); + cpConstraintActivateBodies(constraint); + ((cpPivotJoint *)constraint)->anchorA = anchorA; +} + +cpVect +cpPivotJointGetAnchorB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); + return ((cpPivotJoint *)constraint)->anchorB; +} + +void +cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) +{ + cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); + cpConstraintActivateBodies(constraint); + ((cpPivotJoint *)constraint)->anchorB = anchorB; +} diff --git a/Chipmunk2D/src/cpPolyShape.c b/Chipmunk2D/src/cpPolyShape.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpPolyShape.c @@ -0,0 +1,323 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" +#include "chipmunk_unsafe.h" + +cpPolyShape * +cpPolyShapeAlloc(void) +{ + return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape)); +} + +static void +cpPolyShapeDestroy(cpPolyShape *poly) +{ + if(poly->count > CP_POLY_SHAPE_INLINE_ALLOC){ + cpfree(poly->planes); + } +} + +static cpBB +cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform) +{ + int count = poly->count; + struct cpSplittingPlane *dst = poly->planes; + struct cpSplittingPlane *src = dst + count; + + cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY; + cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY; + + for(int i=0; ir; + return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius)); +} + +static void +cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){ + int count = poly->count; + struct cpSplittingPlane *planes = poly->planes; + cpFloat r = poly->r; + + cpVect v0 = planes[count - 1].v0; + cpFloat minDist = INFINITY; + cpVect closestPoint = cpvzero; + cpVect closestNormal = cpvzero; + cpBool outside = cpFalse; + + for(int i=0; i 0.0f); + + cpVect closest = cpClosetPointOnSegment(p, v0, v1); + + cpFloat dist = cpvdist(p, closest); + if(dist < minDist){ + minDist = dist; + closestPoint = closest; + closestNormal = planes[i].n; + } + + v0 = v1; + } + + cpFloat dist = (outside ? minDist : -minDist); + cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist); + + info->shape = (cpShape *)poly; + info->point = cpvadd(closestPoint, cpvmult(g, r)); + info->distance = dist - r; + + // Use the normal of the closest segment if the distance is small. + info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal); +} + +static void +cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) +{ + struct cpSplittingPlane *planes = poly->planes; + int count = poly->count; + cpFloat r = poly->r; + cpFloat rsum = r + r2; + + for(int i=0; ishape = (cpShape *)poly; + info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2)); + info->normal = n; + info->alpha = t; + } + } + + // Also check against the beveled vertexes. + if(rsum > 0.0f){ + for(int i=0; ishape, planes[i].v0, r, a, b, r2, &circle_info); + if(circle_info.alpha < info->alpha) (*info) = circle_info; + } + } +} + +static void +SetVerts(cpPolyShape *poly, int count, const cpVect *verts) +{ + poly->count = count; + if(count <= CP_POLY_SHAPE_INLINE_ALLOC){ + poly->planes = poly->_planes; + } else { + poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane)); + } + + for(int i=0; iplanes[i + count].v0 = b; + poly->planes[i + count].n = n; + } +} + +static struct cpShapeMassInfo +cpPolyShapeMassInfo(cpFloat mass, int count, const cpVect *verts, cpFloat radius) +{ + // TODO moment is approximate due to radius. + + cpVect centroid = cpCentroidForPoly(count, verts); + struct cpShapeMassInfo info = { + mass, cpMomentForPoly(1.0f, count, verts, cpvneg(centroid), radius), + centroid, + cpAreaForPoly(count, verts, radius), + }; + + return info; +} + +static const cpShapeClass polyClass = { + CP_POLY_SHAPE, + (cpShapeCacheDataImpl)cpPolyShapeCacheData, + (cpShapeDestroyImpl)cpPolyShapeDestroy, + (cpShapePointQueryImpl)cpPolyShapePointQuery, + (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery, +}; + +cpPolyShape * +cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius) +{ + cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect)); + + // Transform the verts before building the hull in case of a negative scale. + for(int i=0; ir = radius; + + return poly; +} + +cpShape * +cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius) +{ + return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, count, verts, transform, radius); +} + +cpShape * +cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius) +{ + return (cpShape *)cpPolyShapeInitRaw(cpPolyShapeAlloc(), body, count, verts, radius); +} + +cpPolyShape * +cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius) +{ + cpFloat hw = width/2.0f; + cpFloat hh = height/2.0f; + + return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh), radius); +} + +cpPolyShape * +cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius) +{ + cpVect verts[] = { + cpv(box.r, box.b), + cpv(box.r, box.t), + cpv(box.l, box.t), + cpv(box.l, box.b), + }; + + return cpPolyShapeInitRaw(poly, body, 4, verts, radius); +} + +cpShape * +cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius) +{ + return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height, radius); +} + +cpShape * +cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius) +{ + return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box, radius); +} + +int +cpPolyShapeGetCount(const cpShape *shape) +{ + cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); + return ((cpPolyShape *)shape)->count; +} + +cpVect +cpPolyShapeGetVert(const cpShape *shape, int i) +{ + cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); + + int count = cpPolyShapeGetCount(shape); + cpAssertHard(0 <= i && i < count, "Index out of range."); + + return ((cpPolyShape *)shape)->planes[i + count].v0; +} + +cpFloat +cpPolyShapeGetRadius(const cpShape *shape) +{ + cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); + return ((cpPolyShape *)shape)->r; +} + +// Unsafe API (chipmunk_unsafe.h) + +void +cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform) +{ + cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect)); + + // Transform the verts before building the hull in case of a negative scale. + for(int i=0; iklass == &polyClass, "Shape is not a poly shape."); + cpPolyShape *poly = (cpPolyShape *)shape; + cpPolyShapeDestroy(poly); + + SetVerts(poly, count, verts); + + cpFloat mass = shape->massInfo.m; + shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, count, verts, poly->r); + if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} + +void +cpPolyShapeSetRadius(cpShape *shape, cpFloat radius) +{ + cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); + cpPolyShape *poly = (cpPolyShape *)shape; + poly->r = radius; + + + // TODO radius is not handled by moment/area +// cpFloat mass = shape->massInfo.m; +// shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, poly->count, poly->verts, poly->r); +// if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} diff --git a/Chipmunk2D/src/cpPolyline.c b/Chipmunk2D/src/cpPolyline.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpPolyline.c @@ -0,0 +1,652 @@ +// Copyright 2013 Howling Moon Software. All rights reserved. +// See http://chipmunk2d.net/legal.php for more information. + +#include +#include +#include +#include + +#include "chipmunk_private.h" +#include "cpPolyline.h" + + +static inline int Next(int i, int count){return (i+1)%count;} + +//MARK: Polylines + +#define DEFAULT_POLYLINE_CAPACITY 16 + +static int +cpPolylineSizeForCapacity(int capacity) +{ + return sizeof(cpPolyline) + capacity*sizeof(cpVect); +} + +static cpPolyline * +cpPolylineMake(int capacity) +{ + capacity = (capacity > DEFAULT_POLYLINE_CAPACITY ? capacity : DEFAULT_POLYLINE_CAPACITY); + + cpPolyline *line = (cpPolyline *)cpcalloc(1, cpPolylineSizeForCapacity(capacity)); + line->count = 0; + line->capacity = capacity; + + return line; +} + +static cpPolyline * +cpPolylineMake2(int capacity, cpVect a, cpVect b) +{ + cpPolyline *line = cpPolylineMake(capacity); + line->count = 2; + line->verts[0] = a; + line->verts[1] = b; + + return line; +} + +static cpPolyline * +cpPolylineShrink(cpPolyline *line) +{ + line->capacity = line->count; + return (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(line->count)); +} + +void +cpPolylineFree(cpPolyline *line) +{ + cpfree(line); +} + +// Grow the allocated memory for a polyline. +static cpPolyline * +cpPolylineGrow(cpPolyline *line, int count) +{ + line->count += count; + + int capacity = line->capacity; + while(line->count > capacity) capacity *= 2; + + if(line->capacity < capacity){ + line->capacity = capacity; + line = (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(capacity)); + } + + return line; +} + +// Push v onto the end of line. +static cpPolyline * +cpPolylinePush(cpPolyline *line, cpVect v) +{ + int count = line->count; + line = cpPolylineGrow(line, 1); + line->verts[count] = v; + + return line; +} + +// Push v onto the beginning of line. +static cpPolyline * +cpPolylineEnqueue(cpPolyline *line, cpVect v) +{ + // TODO could optimize this to grow in both directions. + // Probably doesn't matter though. + int count = line->count; + line = cpPolylineGrow(line, 1); + memmove(line->verts + 1, line->verts, count*sizeof(cpVect)); + line->verts[0] = v; + + return line; +} + +// Returns true if the polyline starts and ends with the same vertex. +cpBool +cpPolylineIsClosed(cpPolyline *line) +{ + return (line->count > 1 && cpveql(line->verts[0], line->verts[line->count-1])); +} + +// Check if a cpPolyline is longer than a certain length +// Takes a range which can wrap around if the polyline is looped. +static cpBool +cpPolylineIsShort(cpVect *points, int count, int start, int end, cpFloat min) +{ + cpFloat length = 0.0f; + for(int i=start; i!=end; i=Next(i, count)){ + length += cpvdist(points[i], points[Next(i, count)]); + if(length > min) return cpFalse; + } + + return cpTrue; +} + +//MARK: Polyline Simplification + +static inline cpFloat +Sharpness(cpVect a, cpVect b, cpVect c) +{ + // TODO could speed this up by caching the normals instead of calculating each twice. + return cpvdot(cpvnormalize(cpvsub(a, b)), cpvnormalize(cpvsub(c, b))); +} + +// Join similar adjacent line segments together. Works well for hard edged shapes. +// 'tol' is the minimum anglular difference in radians of a vertex. +cpPolyline * +cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol) +{ + cpPolyline *reduced = cpPolylineMake2(0, line->verts[0], line->verts[1]); + + cpFloat minSharp = -cpfcos(tol); + + for(int i=2; icount; i++){ + cpVect vert = line->verts[i]; + cpFloat sharp = Sharpness(reduced->verts[reduced->count - 2], reduced->verts[reduced->count - 1], vert); + + if(sharp <= minSharp){ + reduced->verts[reduced->count - 1] = vert; + } else { + reduced = cpPolylinePush(reduced, vert); + } + } + + if( + cpPolylineIsClosed(line) && + Sharpness(reduced->verts[reduced->count - 2], reduced->verts[0], reduced->verts[1]) < minSharp + ){ + reduced->verts[0] = reduced->verts[reduced->count - 2]; + reduced->count--; + } + + // TODO shrink + return reduced; +} + +// Recursive function used by cpPolylineSimplifyCurves(). +static cpPolyline * +DouglasPeucker( + cpVect *verts, cpPolyline *reduced, + int length, int start, int end, + cpFloat min, cpFloat tol +){ + // Early exit if the points are adjacent + if((end - start + length)%length < 2) return reduced; + + cpVect a = verts[start]; + cpVect b = verts[end]; + + // Check if the length is below the threshold + if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced; + + // Find the maximal vertex to split and recurse on + cpFloat max = 0.0; + int maxi = start; + + cpVect n = cpvnormalize(cpvperp(cpvsub(b, a))); + cpFloat d = cpvdot(n, a); + + for(int i=Next(start, length); i!=end; i=Next(i, length)){ + cpFloat dist = fabs(cpvdot(n, verts[i]) - d); + + if(dist > max){ + max = dist; + maxi = i; + } + } + + if(max > tol){ + reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol); + reduced = cpPolylinePush(reduced, verts[maxi]); + reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol); + } + + return reduced; +} + +// Recursively reduce the vertex count on a polyline. Works best for smooth shapes. +// 'tol' is the maximum error for the reduction. +// The reduced polyline will never be farther than this distance from the original polyline. +cpPolyline * +cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol) +{ + cpPolyline *reduced = cpPolylineMake(line->count); + + cpFloat min = tol/2.0f; + + if(cpPolylineIsClosed(line)){ + int start, end; + cpLoopIndexes(line->verts, line->count - 1, &start, &end); + + reduced = cpPolylinePush(reduced, line->verts[start]); + reduced = DouglasPeucker(line->verts, reduced, line->count - 1, start, end, min, tol); + reduced = cpPolylinePush(reduced, line->verts[end]); + reduced = DouglasPeucker(line->verts, reduced, line->count - 1, end, start, min, tol); + reduced = cpPolylinePush(reduced, line->verts[start]); + } else { + reduced = cpPolylinePush(reduced, line->verts[0]); + reduced = DouglasPeucker(line->verts, reduced, line->count, 0, line->count - 1, min, tol); + reduced = cpPolylinePush(reduced, line->verts[line->count - 1]); + } + + return cpPolylineShrink(reduced); +} + +//MARK: Polyline Sets + +cpPolylineSet * +cpPolylineSetAlloc(void) +{ + return (cpPolylineSet *)cpcalloc(1, sizeof(cpPolylineSet)); +} + +cpPolylineSet * +cpPolylineSetInit(cpPolylineSet *set) +{ + set->count = 0; + set->capacity = 8; + set->lines = (cpPolyline**) cpcalloc(set->capacity, sizeof(cpPolyline)); + + return set; +} + + +cpPolylineSet * +cpPolylineSetNew(void) +{ + return cpPolylineSetInit(cpPolylineSetAlloc()); +} + +void +cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines) +{ + if(freePolylines){ + for(int i=0; icount; i++){ + cpPolylineFree(set->lines[i]); + } + } + + cpfree(set->lines); +} + + +void +cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines) +{ + if(set){ + cpPolylineSetDestroy(set, freePolylines); + cpfree(set); + } +} + +// Find the polyline that ends with v. +static int +cpPolylineSetFindEnds(cpPolylineSet *set, cpVect v){ + int count = set->count; + cpPolyline **lines = set->lines; + + for(int i=0; iverts[line->count - 1], v)) return i; + } + + return -1; +} + +// Find the polyline that starts with v. +static int +cpPolylineSetFindStarts(cpPolylineSet *set, cpVect v){ + int count = set->count; + cpPolyline **lines = set->lines; + + for(int i=0; iverts[0], v)) return i; + } + + return -1; +} + +// Add a new polyline to a polyline set. +static void +cpPolylineSetPush(cpPolylineSet *set, cpPolyline *line) +{ + // grow set + set->count++; + if(set->count > set->capacity){ + set->capacity *= 2; + set->lines = (cpPolyline**) cprealloc(set->lines, set->capacity*sizeof(cpPolyline)); + } + + set->lines[set->count - 1] = line; +} + +// Add a new polyline to a polyline set. +static void +cpPolylineSetAdd(cpPolylineSet *set, cpVect v0, cpVect v1) +{ + cpPolylineSetPush(set, cpPolylineMake2(DEFAULT_POLYLINE_CAPACITY, v0, v1)); +} + +// Join two cpPolylines in a polyline set together. +static void +cpPolylineSetJoin(cpPolylineSet *set, int before, int after) +{ + cpPolyline *lbefore = set->lines[before]; + cpPolyline *lafter = set->lines[after]; + + // append + int count = lbefore->count; + lbefore = cpPolylineGrow(lbefore, lafter->count); + memmove(lbefore->verts + count, lafter->verts, lafter->count*sizeof(cpVect)); + set->lines[before] = lbefore; + + // delete lafter + set->count--; + cpPolylineFree(set->lines[after]); + set->lines[after] = set->lines[set->count]; +} + +// Add a segment to a polyline set. +// A segment will either start a new polyline, join two others, or add to or loop an existing polyline. +void +cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines) +{ + int before = cpPolylineSetFindEnds(lines, v0); + int after = cpPolylineSetFindStarts(lines, v1); + + if(before >= 0 && after >= 0){ + if(before == after){ + // loop by pushing v1 onto before + lines->lines[before] = cpPolylinePush(lines->lines[before], v1); + } else { + // join before and after + cpPolylineSetJoin(lines, before, after); + } + } else if(before >= 0){ + // push v1 onto before + lines->lines[before] = cpPolylinePush(lines->lines[before], v1); + } else if(after >= 0){ + // enqueue v0 onto after + lines->lines[after] = cpPolylineEnqueue(lines->lines[after], v0); + } else { + // create new line from v0 and v1 + cpPolylineSetAdd(lines, v0, v1); + } +} + +//MARK: Convex Hull Functions + +cpPolyline * +cpPolylineToConvexHull(cpPolyline *line, cpFloat tol) +{ + cpPolyline *hull = cpPolylineMake(line->count + 1); + hull->count = cpConvexHull(line->count, line->verts, hull->verts, NULL, tol); + hull = cpPolylinePush(hull, hull->verts[0]); + + return cpPolylineShrink(hull); +} + +//MARK: Approximate Concave Decompostition + +struct Notch { + int i; + cpFloat d; + cpVect v; + cpVect n; +}; + +static cpFloat +FindSteiner(int count, cpVect *verts, struct Notch notch) +{ + cpFloat min = INFINITY; + cpFloat feature = -1.0; + + for(int i=1; i= 0.0 && dist <= min){ + min = dist; + feature = index + t; + } + } + } + + return feature; +} + +//static cpFloat +//FindSteiner2(cpVect *verts, int count, struct Notch notch) +//{ +// cpVect a = verts[(notch.i + count - 1)%count]; +// cpVect b = verts[(notch.i + 1)%count]; +// cpVect n = cpvnormalize(cpvadd(cpvnormalize(cpvsub(notch.v, a)), cpvnormalize(cpvsub(notch.v, b)))); +// +// cpFloat min = INFINITY; +// cpFloat feature = -1.0; +// +// for(int i=1; i= 0.0 && dist <= min){ +// min = dist; +// feature = index + t; +// } +// } +// } +// +// cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon."); +// return feature; +//} + +//struct Range {cpFloat min, max;}; +//static inline struct Range +//clip_range(cpVect delta_a, cpVect delta_b, cpVect clip) +//{ +// cpFloat da = cpvcross(delta_a, clip); +// cpFloat db = cpvcross(delta_b, clip); +// cpFloat clamp = da/(da - db); +// if(da > db){ +// return (struct Range){-INFINITY, clamp}; +// } else if(da < db){ +// return (struct Range){clamp, INFINITY}; +// } else { +// return (struct Range){-INFINITY, INFINITY}; +// } +//} +// +//static cpFloat +//FindSteiner3(cpVect *verts, int count, struct Notch notch) +//{ +// cpFloat min = INFINITY; +// cpFloat feature = -1.0; +// +// cpVect support_a = verts[(notch.i - 1 + count)%count]; +// cpVect support_b = verts[(notch.i + 1)%count]; +// +// cpVect clip_a = cpvlerp(support_a, support_b, 0.1); +// cpVect clip_b = cpvlerp(support_b, support_b, 0.9); +// +// for(int i=1; i 0.0){ +// struct Range range1 = clip_range(delta_a, delta_b, cpvsub(notch.v, clip_a)); +// struct Range range2 = clip_range(delta_a, delta_b, cpvsub(clip_b, notch.v)); +// +// cpFloat min_t = cpfmax(0.0, cpfmax(range1.min, range2.min)); +// cpFloat max_t = cpfmin(1.0, cpfmin(range1.max, range2.max)); +// +// // Ignore if the segment has been completely clipped away. +// if(min_t < max_t){ +// cpVect seg_delta = cpvsub(seg_b, seg_a); +// cpFloat closest_t = cpfclamp(cpvdot(seg_delta, cpvsub(notch.v, seg_a))/cpvlengthsq(seg_delta), min_t, max_t); +// cpVect closest = cpvlerp(seg_a, seg_b, closest_t); +// +// cpFloat dist = cpvdistsq(notch.v, closest); +// if(dist < min){ +// min = dist; +// feature = index + closest_t; +// } +// } +// } +// } +// +// cpAssertWarn(feature >= 0.0, "Internal Error: No closest features detected."); +// return feature; +//} + +//static cpBool +//VertexUnobscured(int count, cpVect *verts, int index, int notch_i) +//{ +// cpVect v = verts[notch_i]; +// cpVect n = cpvnormalize(cpvsub(verts[index], v)); +// +// for(int i=0; i= 0.0, "No closest features detected. This is likely due to a self intersecting polygon."); +// return feature; +//} + +static struct Notch +DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol) +{ + struct Notch notch = {}; + int j = Next(first, count); + + for(int i=0; i notch.d){ + notch.d = depth; + notch.i = j; + notch.v = v; + notch.n = n; + } + + j = Next(j, count); + v = verts[j]; + } + + j = Next(j, count); + } + + return notch; +} + +static inline int IMAX(int a, int b){return (a > b ? a : b);} + +static void +ApproximateConcaveDecomposition(cpVect *verts, int count, cpFloat tol, cpPolylineSet *set) +{ + int first; + cpVect *hullVerts = (cpVect*) alloca(count*sizeof(cpVect)); + int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0); + + if(hullCount != count){ + struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol); + + if(notch.d > tol){ + cpFloat steiner_it = FindSteiner(count, verts, notch); + + if(steiner_it >= 0.0){ + int steiner_i = (int)steiner_it; + cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i); + + // Vertex counts NOT including the steiner point. + int sub1_count = (steiner_i - notch.i + count)%count + 1; + int sub2_count = count - (steiner_i - notch.i + count)%count; + cpVect *scratch = (cpVect*) alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect)); + + for(int i=0; iverts, hullVerts, hullCount*sizeof(cpVect)); + hull->verts[hullCount] = hullVerts[0]; + hull->count = hullCount + 1; + + cpPolylineSetPush(set, hull); +} + +cpPolylineSet * +cpPolylineConvexDecomposition_BETA(cpPolyline *line, cpFloat tol) +{ + cpAssertSoft(cpPolylineIsClosed(line), "Cannot decompose an open polygon."); + cpAssertSoft(cpAreaForPoly(line->count, line->verts, 0.0) >= 0.0, "Winding is backwards. (Are you passing a hole?)"); + + cpPolylineSet *set = cpPolylineSetNew(); + ApproximateConcaveDecomposition(line->verts, line->count - 1, tol, set); + + return set; +} diff --git a/Chipmunk2D/src/cpRatchetJoint.c b/Chipmunk2D/src/cpRatchetJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpRatchetJoint.c @@ -0,0 +1,179 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpRatchetJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat angle = joint->angle; + cpFloat phase = joint->phase; + cpFloat ratchet = joint->ratchet; + + cpFloat delta = b->a - a->a; + cpFloat diff = angle - delta; + cpFloat pdist = 0.0f; + + if(diff*ratchet > 0.0f){ + pdist = diff; + } else { + joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; + } + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); + + // If the bias is 0, the joint is not at a limit. Reset the impulse. + if(!joint->bias) joint->jAcc = 0.0f; +} + +static void +applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static void +applyImpulse(cpRatchetJoint *joint, cpFloat dt) +{ + if(!joint->bias) return; // early exit + + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // compute relative rotational velocity + 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, jMax*cpfabs(ratchet))/ratchet; + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpRatchetJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpRatchetJoint * +cpRatchetJointAlloc(void) +{ + return (cpRatchetJoint *)cpcalloc(1, sizeof(cpRatchetJoint)); +} + +cpRatchetJoint * +cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->angle = 0.0f; + joint->phase = phase; + joint->ratchet = ratchet; + + // STATIC_BODY_CHECK + joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f); + + return joint; +} + +cpConstraint * +cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) +{ + return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); +} + +cpBool +cpConstraintIsRatchetJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpFloat +cpRatchetJointGetAngle(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + return ((cpRatchetJoint *)constraint)->angle; +} + +void +cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + cpConstraintActivateBodies(constraint); + ((cpRatchetJoint *)constraint)->angle = angle; +} + +cpFloat +cpRatchetJointGetPhase(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + return ((cpRatchetJoint *)constraint)->phase; +} + +void +cpRatchetJointSetPhase(cpConstraint *constraint, cpFloat phase) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + cpConstraintActivateBodies(constraint); + ((cpRatchetJoint *)constraint)->phase = phase; +} +cpFloat +cpRatchetJointGetRatchet(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + return ((cpRatchetJoint *)constraint)->ratchet; +} + +void +cpRatchetJointSetRatchet(cpConstraint *constraint, cpFloat ratchet) +{ + cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); + cpConstraintActivateBodies(constraint); + ((cpRatchetJoint *)constraint)->ratchet = ratchet; +} diff --git a/Chipmunk2D/src/cpRobust.c b/Chipmunk2D/src/cpRobust.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpRobust.c @@ -0,0 +1,13 @@ +#include "cpRobust.h" + + +cpBool +cpCheckPointGreater(const cpVect a, const cpVect b, const cpVect c) +{ + return (b.y - a.y)*(a.x + b.x - 2*c.x) > (b.x - a.x)*(a.y + b.y - 2*c.y); +} + +cpBool +cpCheckAxis(cpVect v0, cpVect v1, cpVect p, cpVect n){ + return cpvdot(p, n) <= cpfmax(cpvdot(v0, n), cpvdot(v1, n)); +} diff --git a/Chipmunk2D/src/cpRotaryLimitJoint.c b/Chipmunk2D/src/cpRotaryLimitJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpRotaryLimitJoint.c @@ -0,0 +1,160 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpRotaryLimitJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat dist = b->a - a->a; + cpFloat pdist = 0.0f; + if(dist > joint->max) { + pdist = joint->max - dist; + } else if(dist < joint->min) { + pdist = joint->min - dist; + } + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); + + // If the bias is 0, the joint is not at a limit. Reset the impulse. + if(!joint->bias) joint->jAcc = 0.0f; +} + +static void +applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static void +applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt) +{ + if(!joint->bias) return; // early exit + + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // 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, jMax); + } else { + joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f); + } + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpRotaryLimitJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpRotaryLimitJoint * +cpRotaryLimitJointAlloc(void) +{ + return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint)); +} + +cpRotaryLimitJoint * +cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->min = min; + joint->max = max; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max); +} + +cpBool +cpConstraintIsRotaryLimitJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpFloat +cpRotaryLimitJointGetMin(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); + return ((cpRotaryLimitJoint *)constraint)->min; +} + +void +cpRotaryLimitJointSetMin(cpConstraint *constraint, cpFloat min) +{ + cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); + cpConstraintActivateBodies(constraint); + ((cpRotaryLimitJoint *)constraint)->min = min; +} + +cpFloat +cpRotaryLimitJointGetMax(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); + return ((cpRotaryLimitJoint *)constraint)->max; +} + +void +cpRotaryLimitJointSetMax(cpConstraint *constraint, cpFloat max) +{ + cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); + cpConstraintActivateBodies(constraint); + ((cpRotaryLimitJoint *)constraint)->max = max; +} diff --git a/Chipmunk2D/src/cpShape.c b/Chipmunk2D/src/cpShape.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpShape.c @@ -0,0 +1,603 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" +#include "chipmunk_unsafe.h" + +#define CP_DefineShapeGetter(struct, type, member, name) \ +CP_DeclareShapeGetter(struct, type, name){ \ + cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \ + return ((struct *)shape)->member; \ +} + +cpShape * +cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo) +{ + shape->klass = klass; + + shape->body = body; + shape->massInfo = massInfo; + + shape->sensor = 0; + + shape->e = 0.0f; + shape->u = 0.0f; + shape->surfaceV = cpvzero; + + shape->type = 0; + shape->filter.group = CP_NO_GROUP; + shape->filter.categories = CP_ALL_CATEGORIES; + shape->filter.mask = CP_ALL_CATEGORIES; + + shape->userData = NULL; + + shape->space = NULL; + + shape->next = NULL; + shape->prev = NULL; + + return shape; +} + +void +cpShapeDestroy(cpShape *shape) +{ + if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape); +} + +void +cpShapeFree(cpShape *shape) +{ + if(shape){ + cpShapeDestroy(shape); + cpfree(shape); + } +} + +cpSpace * +cpShapeGetSpace(const cpShape *shape) +{ + return shape->space; +} + +cpBody * +cpShapeGetBody(const cpShape *shape) +{ + return shape->body; +} + +void +cpShapeSetBody(cpShape *shape, cpBody *body) +{ + 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; +} + +cpFloat cpShapeGetMass(cpShape *shape){ return shape->massInfo.m; } + +void +cpShapeSetMass(cpShape *shape, cpFloat mass){ + cpBody *body = shape->body; + cpBodyActivate(body); + + shape->massInfo.m = mass; + cpBodyAccumulateMassFromShapes(body); +} + +cpFloat cpShapeGetDensity(cpShape *shape){ return shape->massInfo.m/shape->massInfo.area; } +void cpShapeSetDensity(cpShape *shape, cpFloat density){ cpShapeSetMass(shape, density*shape->massInfo.area); } + +cpFloat cpShapeGetMoment(cpShape *shape){ return shape->massInfo.m*shape->massInfo.i; } +cpFloat cpShapeGetArea(cpShape *shape){ return shape->massInfo.area; } +cpVect cpShapeGetCenterOfGravity(cpShape *shape) { return shape->massInfo.cog; } + +cpBB +cpShapeGetBB(const cpShape *shape) +{ + return shape->bb; +} + +cpBool +cpShapeGetSensor(const cpShape *shape) +{ + return shape->sensor; +} + +void +cpShapeSetSensor(cpShape *shape, cpBool sensor) +{ + cpBodyActivate(shape->body); + shape->sensor = sensor; +} + +cpFloat +cpShapeGetElasticity(const cpShape *shape) +{ + return shape->e; +} + +void +cpShapeSetElasticity(cpShape *shape, cpFloat elasticity) +{ + cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive."); + cpBodyActivate(shape->body); + shape->e = elasticity; +} + +cpFloat +cpShapeGetFriction(const cpShape *shape) +{ + return shape->u; +} + +void +cpShapeSetFriction(cpShape *shape, cpFloat friction) +{ + cpAssertHard(friction >= 0.0f, "Friction must be postive."); + cpBodyActivate(shape->body); + shape->u = friction; +} + +cpVect +cpShapeGetSurfaceVelocity(const cpShape *shape) +{ + return shape->surfaceV; +} + +void +cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity) +{ + cpBodyActivate(shape->body); + shape->surfaceV = surfaceVelocity; +} + +cpDataPointer +cpShapeGetUserData(const cpShape *shape) +{ + return shape->userData; +} + +void +cpShapeSetUserData(cpShape *shape, cpDataPointer userData) +{ + shape->userData = userData; +} + +cpCollisionType +cpShapeGetCollisionType(const cpShape *shape) +{ + return shape->type; +} + +void +cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType) +{ + cpBodyActivate(shape->body); + shape->type = collisionType; +} + +cpShapeFilter +cpShapeGetFilter(const cpShape *shape) +{ + return shape->filter; +} + +void +cpShapeSetFilter(cpShape *shape, cpShapeFilter filter) +{ + cpBodyActivate(shape->body); + shape->filter = filter; +} + +cpBB +cpShapeCacheBB(cpShape *shape) +{ + return cpShapeUpdate(shape, shape->body->transform); +} + +cpBB +cpShapeUpdate(cpShape *shape, cpTransform transform) +{ + return (shape->bb = shape->klass->cacheData(shape, transform)); +} + +cpFloat +cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *info) +{ + cpPointQueryInfo blank = {NULL, cpvzero, INFINITY, cpvzero}; + if(info){ + (*info) = blank; + } else { + info = ␣ + } + + shape->klass->pointQuery(shape, p, info); + return info->distance; +} + + +cpBool +cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info){ + cpSegmentQueryInfo blank = {NULL, b, cpvzero, 1.0f}; + if(info){ + (*info) = blank; + } else { + info = ␣ + } + + cpPointQueryInfo nearest; + shape->klass->pointQuery(shape, a, &nearest); + if(nearest.distance <= radius){ + info->shape = shape; + info->alpha = 0.0; + info->normal = cpvnormalize(cpvsub(a, nearest.point)); + } else { + shape->klass->segmentQuery(shape, a, b, radius, info); + } + + return (info->shape != NULL); +} + +cpContactPointSet +cpShapesCollide(const cpShape *a, const cpShape *b) +{ + struct cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER]; + struct cpCollisionInfo info = cpCollide(a, b, 0, contacts); + + cpContactPointSet set; + set.count = info.count; + + // cpCollideShapes() may have swapped the contact order. Flip the normal. + cpBool swapped = (a != info.a); + set.normal = (swapped ? cpvneg(info.n) : info.n); + + for(int i=0; itc = cpTransformPoint(transform, circle->c); + return cpBBNewForCircle(c, circle->r); +} + +static void +cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryInfo *info) +{ + cpVect delta = cpvsub(p, circle->tc); + cpFloat d = cpvlength(delta); + cpFloat r = circle->r; + + info->shape = (cpShape *)circle; + info->point = cpvadd(circle->tc, cpvmult(delta, r/d)); // TODO: div/0 + info->distance = d - r; + + // Use up for the gradient if the distance is very small. + info->gradient = (d > MAGIC_EPSILON ? cpvmult(delta, 1.0f/d) : cpv(0.0f, 1.0f)); +} + +static void +cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info) +{ + CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info); +} + +static struct cpShapeMassInfo +cpCircleShapeMassInfo(cpFloat mass, cpFloat radius, cpVect center) +{ + struct cpShapeMassInfo info = { + mass, cpMomentForCircle(1.0f, 0.0f, radius, cpvzero), + center, + cpAreaForCircle(0.0f, radius), + }; + + return info; +} + +static const cpShapeClass cpCircleShapeClass = { + CP_CIRCLE_SHAPE, + (cpShapeCacheDataImpl)cpCircleShapeCacheData, + NULL, + (cpShapePointQueryImpl)cpCircleShapePointQuery, + (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery, +}; + +cpCircleShape * +cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) +{ + circle->c = offset; + circle->r = radius; + + cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body, cpCircleShapeMassInfo(0.0f, radius, offset)); + + return circle; +} + +cpShape * +cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) +{ + return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); +} + +cpVect +cpCircleShapeGetOffset(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + return ((cpCircleShape *)shape)->c; +} + +cpFloat +cpCircleShapeGetRadius(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + return ((cpCircleShape *)shape)->r; +} + + +cpSegmentShape * +cpSegmentShapeAlloc(void) +{ + return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape)); +} + +static cpBB +cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform) +{ + seg->ta = cpTransformPoint(transform, seg->a); + seg->tb = cpTransformPoint(transform, seg->b); + seg->tn = cpTransformVect(transform, seg->n); + + cpFloat l,r,b,t; + + if(seg->ta.x < seg->tb.x){ + l = seg->ta.x; + r = seg->tb.x; + } else { + l = seg->tb.x; + r = seg->ta.x; + } + + if(seg->ta.y < seg->tb.y){ + b = seg->ta.y; + t = seg->tb.y; + } else { + b = seg->tb.y; + t = seg->ta.y; + } + + cpFloat rad = seg->r; + return cpBBNew(l - rad, b - rad, r + rad, t + rad); +} + +static void +cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryInfo *info) +{ + cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb); + + cpVect delta = cpvsub(p, closest); + cpFloat d = cpvlength(delta); + cpFloat r = seg->r; + cpVect g = cpvmult(delta, 1.0f/d); + + info->shape = (cpShape *)seg; + info->point = (d ? cpvadd(closest, cpvmult(g, r)) : closest); + info->distance = d - r; + + // Use the segment's normal if the distance is very small. + info->gradient = (d > MAGIC_EPSILON ? g : seg->n); +} + +static void +cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) +{ + cpVect n = seg->tn; + cpFloat d = cpvdot(cpvsub(seg->ta, a), n); + cpFloat r = seg->r + r2; + + cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n); + cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a); + + // 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(ad*bd < 0.0f){ + cpFloat t = ad/(ad - bd); + + info->shape = (cpShape *)seg; + info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2)); + info->normal = flipped_n; + info->alpha = t; + } + } else if(r != 0.0f){ + cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f}; + cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f}; + CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1); + CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2); + + if(info1.alpha < info2.alpha){ + (*info) = info1; + } else { + (*info) = info2; + } + } +} + +static struct cpShapeMassInfo +cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r) +{ + struct cpShapeMassInfo info = { + mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation. + cpvlerp(a, b, 0.5f), + cpAreaForSegment(a, b, r), + }; + + return info; +} + +static const cpShapeClass cpSegmentShapeClass = { + CP_SEGMENT_SHAPE, + (cpShapeCacheDataImpl)cpSegmentShapeCacheData, + NULL, + (cpShapePointQueryImpl)cpSegmentShapePointQuery, + (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery, +}; + +cpSegmentShape * +cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + seg->a = a; + seg->b = b; + seg->n = cpvrperp(cpvnormalize(cpvsub(b, a))); + + seg->r = r; + + seg->a_tangent = cpvzero; + seg->b_tangent = cpvzero; + + cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body, cpSegmentShapeMassInfo(0.0f, a, b, r)); + + return seg; +} + +cpShape* +cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); +} + +cpVect +cpSegmentShapeGetA(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + return ((cpSegmentShape *)shape)->a; +} + +cpVect +cpSegmentShapeGetB(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + return ((cpSegmentShape *)shape)->b; +} + +cpVect +cpSegmentShapeGetNormal(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + return ((cpSegmentShape *)shape)->n; +} + +cpFloat +cpSegmentShapeGetRadius(const cpShape *shape) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + return ((cpSegmentShape *)shape)->r; +} + +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) + +// TODO setters should wake the shape up? + +void +cpCircleShapeSetRadius(cpShape *shape, cpFloat radius) +{ + cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->r = radius; + + cpFloat mass = shape->massInfo.m; + shape->massInfo = cpCircleShapeMassInfo(mass, circle->r, circle->c); + if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} + +void +cpCircleShapeSetOffset(cpShape *shape, cpVect offset) +{ + cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->c = offset; + + cpFloat mass = shape->massInfo.m; + shape->massInfo = cpCircleShapeMassInfo(shape->massInfo.m, circle->r, circle->c); + if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} + +void +cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); + + cpFloat mass = shape->massInfo.m; + shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r); + if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} + +void +cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius) +{ + cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->r = radius; + + cpFloat mass = shape->massInfo.m; + shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r); + if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); +} diff --git a/Chipmunk2D/src/cpSimpleMotor.c b/Chipmunk2D/src/cpSimpleMotor.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSimpleMotor.c @@ -0,0 +1,123 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpSimpleMotor *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); +} + +static void +applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat j = joint->jAcc*dt_coef; + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static void +applyImpulse(cpSimpleMotor *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // 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, -jMax, jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpSimpleMotor *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpSimpleMotor * +cpSimpleMotorAlloc(void) +{ + return (cpSimpleMotor *)cpcalloc(1, sizeof(cpSimpleMotor)); +} + +cpSimpleMotor * +cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->rate = rate; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate) +{ + return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate); +} + +cpBool +cpConstraintIsSimpleMotor(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpFloat +cpSimpleMotorGetRate(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a pin joint."); + return ((cpSimpleMotor *)constraint)->rate; +} + +void +cpSimpleMotorSetRate(cpConstraint *constraint, cpFloat rate) +{ + cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a pin joint."); + cpConstraintActivateBodies(constraint); + ((cpSimpleMotor *)constraint)->rate = rate; +} diff --git a/Chipmunk2D/src/cpSlideJoint.c b/Chipmunk2D/src/cpSlideJoint.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSlideJoint.c @@ -0,0 +1,195 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static void +preStep(cpSlideJoint *joint, cpFloat dt) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); + joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + cpFloat pdist = 0.0f; + if(dist > joint->max) { + pdist = dist - joint->max; + joint->n = cpvnormalize(delta); + } else if(dist < joint->min) { + pdist = joint->min - dist; + joint->n = cpvneg(cpvnormalize(delta)); + } else { + joint->n = cpvzero; + joint->jnAcc = 0.0f; + } + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); +} + +static void +applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static void +applyImpulse(cpSlideJoint *joint, cpFloat dt) +{ + if(cpveql(joint->n, cpvzero)) return; // early exit + + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect n = joint->n; + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + cpFloat vrn = cpvdot(vr, n); + + // compute normal impulse + cpFloat jn = (joint->bias - vrn)*joint->nMass; + cpFloat jnOld = joint->jnAcc; + joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpfabs(((cpSlideJoint *)joint)->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepImpl)preStep, + (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, + (cpConstraintApplyImpulseImpl)applyImpulse, + (cpConstraintGetImpulseImpl)getImpulse, +}; + +cpSlideJoint * +cpSlideJointAlloc(void) +{ + return (cpSlideJoint *)cpcalloc(1, sizeof(cpSlideJoint)); +} + +cpSlideJoint * +cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchorA = anchorA; + joint->anchorB = anchorB; + joint->min = min; + joint->max = max; + + joint->jnAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchorA, anchorB, min, max); +} + +cpBool +cpConstraintIsSlideJoint(const cpConstraint *constraint) +{ + return (constraint->klass == &klass); +} + +cpVect +cpSlideJointGetAnchorA(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + return ((cpSlideJoint *)constraint)->anchorA; +} + +void +cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + cpConstraintActivateBodies(constraint); + ((cpSlideJoint *)constraint)->anchorA = anchorA; +} + +cpVect +cpSlideJointGetAnchorB(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + return ((cpSlideJoint *)constraint)->anchorB; +} + +void +cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + cpConstraintActivateBodies(constraint); + ((cpSlideJoint *)constraint)->anchorB = anchorB; +} + +cpFloat +cpSlideJointGetMin(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + return ((cpSlideJoint *)constraint)->min; +} + +void +cpSlideJointSetMin(cpConstraint *constraint, cpFloat min) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + cpConstraintActivateBodies(constraint); + ((cpSlideJoint *)constraint)->min = min; +} + +cpFloat +cpSlideJointGetMax(const cpConstraint *constraint) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + return ((cpSlideJoint *)constraint)->max; +} + +void +cpSlideJointSetMax(cpConstraint *constraint, cpFloat max) +{ + cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); + cpConstraintActivateBodies(constraint); + ((cpSlideJoint *)constraint)->max = max; +} diff --git a/Chipmunk2D/src/cpSpace.c b/Chipmunk2D/src/cpSpace.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpace.c @@ -0,0 +1,701 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" + + +//MARK: Contact Set Helpers + +// Equal function for arbiterSet. +static cpBool +arbiterSetEql(cpShape **shapes, cpArbiter *arb) +{ + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); +} + +//MARK: Collision Handler Set HelperFunctions + +// Equals function for collisionHandlers. +static cpBool +handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) +{ + return ( + (check->typeA == pair->typeA && check->typeB == pair->typeB) || + (check->typeB == pair->typeA && check->typeA == pair->typeB) + ); +} + +// Transformation function for collisionHandlers. +static void * +handlerSetTrans(cpCollisionHandler *handler, void *unused) +{ + cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler)); + memcpy(copy, handler, sizeof(cpCollisionHandler)); + + return copy; +} + +//MARK: Misc Helper Funcs + +// Default collision functions. + +static cpBool +DefaultBegin(cpArbiter *arb, cpSpace *space, void *data){ + cpBool retA = cpArbiterCallWildcardBeginA(arb, space); + cpBool retB = cpArbiterCallWildcardBeginB(arb, space); + return retA && retB; +} + +static cpBool +DefaultPreSolve(cpArbiter *arb, cpSpace *space, void *data){ + cpBool retA = cpArbiterCallWildcardPreSolveA(arb, space); + cpBool retB = cpArbiterCallWildcardPreSolveB(arb, space); + return retA && retB; +} + +static void +DefaultPostSolve(cpArbiter *arb, cpSpace *space, void *data){ + cpArbiterCallWildcardPostSolveA(arb, space); + cpArbiterCallWildcardPostSolveB(arb, space); +} + +static void +DefaultSeparate(cpArbiter *arb, cpSpace *space, void *data){ + cpArbiterCallWildcardSeparateA(arb, space); + cpArbiterCallWildcardSeparateB(arb, space); +} + +// Use the wildcard identifier since the default handler should never match any type pair. +static cpCollisionHandler cpCollisionHandlerDefault = { + CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE, + DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL +}; + +static cpBool AlwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return cpTrue;} +static void DoNothing(cpArbiter *arb, cpSpace *space, void *data){} + +cpCollisionHandler cpCollisionHandlerDoNothing = { + CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE, + AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL +}; + +// function to get the estimated velocity of a shape for the cpBBTree. +static cpVect ShapeVelocityFunc(cpShape *shape){return shape->body->v;} + +// Used for disposing of collision handlers. +static void FreeWrap(void *ptr, void *unused){cpfree(ptr);} + +//MARK: Memory Management Functions + +cpSpace * +cpSpaceAlloc(void) +{ + return (cpSpace *)cpcalloc(1, sizeof(cpSpace)); +} + +cpSpace* +cpSpaceInit(cpSpace *space) +{ +#ifndef NDEBUG + static cpBool done = cpFalse; + if(!done){ + printf("phycs: Initializing new Space, Chipmunk v%s (Debug Enabled)\n", cpVersionString); + printf("phycs: Compile with the -frelease (cabal) or --flag phycs:release (stack) to disable debug mode and runtime assertion checks\n"); + done = cpTrue; + } +#endif + + space->iterations = 10; + + space->gravity = cpvzero; + space->damping = 1.0f; + + space->collisionSlop = 0.1f; + space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f); + space->collisionPersistence = 3; + + space->locked = 0; + space->stamp = 0; + + space->shapeIDCounter = 0; + space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL); + space->dynamicShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes); + cpBBTreeSetVelocityFunc(space->dynamicShapes, (cpBBTreeVelocityFunc)ShapeVelocityFunc); + + space->allocatedBuffers = cpArrayNew(0); + + space->dynamicBodies = cpArrayNew(0); + space->staticBodies = cpArrayNew(0); + space->sleepingComponents = cpArrayNew(0); + space->rousedBodies = cpArrayNew(0); + + space->sleepTimeThreshold = INFINITY; + space->idleSpeedThreshold = 0.0f; + + space->arbiters = cpArrayNew(0); + space->pooledArbiters = cpArrayNew(0); + + space->contactBuffersHead = NULL; + space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql); + + space->constraints = cpArrayNew(0); + + space->usesWildcards = cpFalse; + memcpy(&space->defaultHandler, &cpCollisionHandlerDoNothing, sizeof(cpCollisionHandler)); + space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql); + + space->postStepCallbacks = cpArrayNew(0); + space->skipPostStep = cpFalse; + + cpBody *staticBody = cpBodyInit(&space->_staticBody, 0.0f, 0.0f); + cpBodySetType(staticBody, CP_BODY_TYPE_STATIC); + cpSpaceSetStaticBody(space, staticBody); + + return space; +} + +cpSpace* +cpSpaceNew(void) +{ + return cpSpaceInit(cpSpaceAlloc()); +} + +static void cpBodyActivateWrap(cpBody *body, void *unused){cpBodyActivate(body);} + +void +cpSpaceDestroy(cpSpace *space) +{ + cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivateWrap, NULL); + + cpSpatialIndexFree(space->staticShapes); + cpSpatialIndexFree(space->dynamicShapes); + + cpArrayFree(space->dynamicBodies); + cpArrayFree(space->staticBodies); + cpArrayFree(space->sleepingComponents); + cpArrayFree(space->rousedBodies); + + cpArrayFree(space->constraints); + + cpHashSetFree(space->cachedArbiters); + + cpArrayFree(space->arbiters); + cpArrayFree(space->pooledArbiters); + + if(space->allocatedBuffers){ + cpArrayFreeEach(space->allocatedBuffers, cpfree); + cpArrayFree(space->allocatedBuffers); + } + + if(space->postStepCallbacks){ + cpArrayFreeEach(space->postStepCallbacks, cpfree); + cpArrayFree(space->postStepCallbacks); + } + + if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, FreeWrap, NULL); + cpHashSetFree(space->collisionHandlers); +} + +void +cpSpaceFree(cpSpace *space) +{ + if(space){ + cpSpaceDestroy(space); + cpfree(space); + } +} + + +//MARK: Basic properties: + +int +cpSpaceGetIterations(const cpSpace *space) +{ + return space->iterations; +} + +void +cpSpaceSetIterations(cpSpace *space, int iterations) +{ + cpAssertHard(iterations > 0, "Iterations must be positive and non-zero."); + space->iterations = iterations; +} + +cpVect +cpSpaceGetGravity(const cpSpace *space) +{ + return space->gravity; +} + +void +cpSpaceSetGravity(cpSpace *space, cpVect gravity) +{ + space->gravity = gravity; + + // Wake up all of the bodies since the gravity changed. + cpArray *components = space->sleepingComponents; + for(int i=0; inum; i++){ + cpBodyActivate((cpBody *)components->arr[i]); + } +} + +cpFloat +cpSpaceGetDamping(const cpSpace *space) +{ + return space->damping; +} + +void +cpSpaceSetDamping(cpSpace *space, cpFloat damping) +{ + cpAssertHard(damping >= 0.0, "Damping must be positive."); + space->damping = damping; +} + +cpFloat +cpSpaceGetIdleSpeedThreshold(const cpSpace *space) +{ + return space->idleSpeedThreshold; +} + +void +cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold) +{ + space->idleSpeedThreshold = idleSpeedThreshold; +} + +cpFloat +cpSpaceGetSleepTimeThreshold(const cpSpace *space) +{ + return space->sleepTimeThreshold; +} + +void +cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold) +{ + space->sleepTimeThreshold = sleepTimeThreshold; +} + +cpFloat +cpSpaceGetCollisionSlop(const cpSpace *space) +{ + return space->collisionSlop; +} + +void +cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop) +{ + space->collisionSlop = collisionSlop; +} + +cpFloat +cpSpaceGetCollisionBias(const cpSpace *space) +{ + return space->collisionBias; +} + +void +cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias) +{ + space->collisionBias = collisionBias; +} + +cpTimestamp +cpSpaceGetCollisionPersistence(const cpSpace *space) +{ + return space->collisionPersistence; +} + +void +cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence) +{ + space->collisionPersistence = collisionPersistence; +} + +cpDataPointer +cpSpaceGetUserData(const cpSpace *space) +{ + return space->userData; +} + +void +cpSpaceSetUserData(cpSpace *space, cpDataPointer userData) +{ + space->userData = userData; +} + +cpBody * +cpSpaceGetStaticBody(const cpSpace *space) +{ + return space->staticBody; +} + +cpFloat +cpSpaceGetCurrentTimeStep(const cpSpace *space) +{ + return space->curr_dt; +} + +void +cpSpaceSetStaticBody(cpSpace *space, cpBody *body) +{ + if(space->staticBody != NULL){ + cpAssertHard(space->staticBody->shapeList == NULL, "Internal Error: Changing the designated static body while the old one still had shapes attached."); + space->staticBody->space = NULL; + } + + space->staticBody = body; + body->space = space; +} + +cpBool +cpSpaceIsLocked(cpSpace *space) +{ + return (space->locked > 0); +} + +//MARK: Collision Handler Function Management + +static void +cpSpaceUseWildcardDefaultHandler(cpSpace *space) +{ + // Spaces default to using the slightly faster "do nothing" default handler until wildcards are potentially needed. + if(!space->usesWildcards){ + space->usesWildcards = cpTrue; + memcpy(&space->defaultHandler, &cpCollisionHandlerDefault, sizeof(cpCollisionHandler)); + } +} + +cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space) +{ + cpSpaceUseWildcardDefaultHandler(space); + return &space->defaultHandler; +} + +cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) +{ + cpHashValue hash = CP_HASH_PAIR(a, b); + cpCollisionHandler handler = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL}; + return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); +} + +cpCollisionHandler * +cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type) +{ + cpSpaceUseWildcardDefaultHandler(space); + + cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE); + cpCollisionHandler handler = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL}; + return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); +} + + +//MARK: Body, Shape, and Joint Management +cpShape * +cpSpaceAddShape(cpSpace *space, cpShape *shape) +{ + cpBody *body = shape->body; + + cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time."); + cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second."); +// cpAssertHard(body->space == space, "The shape's body must be added to the space before the shape."); + cpAssertSpaceUnlocked(space); + + cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC); + if(!isStatic) cpBodyActivate(body); + cpBodyAddShape(body, shape); + + shape->hashid = space->shapeIDCounter++; + cpShapeUpdate(shape, body->transform); + cpSpatialIndexInsert(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid); + shape->space = space; + + return shape; +} + +cpBody * +cpSpaceAddBody(cpSpace *space, cpBody *body) +{ + cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); + cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); + cpAssertSpaceUnlocked(space); + + cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body); + body->space = space; + + return body; +} + +cpConstraint * +cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time."); + cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second."); + cpAssertSpaceUnlocked(space); + + cpBody *a = constraint->a, *b = constraint->b; + cpAssertHard(a != NULL && b != NULL, "Constraint is attached to a NULL body."); +// cpAssertHard(a->space == space && b->space == space, "The constraint's bodies must be added to the space before the constraint."); + + cpBodyActivate(a); + cpBodyActivate(b); + cpArrayPush(space->constraints, constraint); + + // Push onto the heads of the bodies' constraint lists + constraint->next_a = a->constraintList; a->constraintList = constraint; + constraint->next_b = b->constraintList; b->constraintList = constraint; + constraint->space = space; + + return constraint; +} + +struct arbiterFilterContext { + cpSpace *space; + cpBody *body; + cpShape *shape; +}; + +static cpBool +cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context) +{ + cpShape *shape = context->shape; + cpBody *body = context->body; + + + // 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 != CP_ARBITER_STATE_CACHED){ + // Invalidate the arbiter since one of the shapes was removed. + arb->state = CP_ARBITER_STATE_INVALIDATED; + + cpCollisionHandler *handler = arb->handler; + handler->separateFunc(arb, context->space, handler->userData); + } + + cpArbiterUnthread(arb); + cpArrayDeleteObj(context->space->arbiters, arb); + cpArrayPush(context->space->pooledArbiters, arb); + + return cpFalse; + } + + return cpTrue; +} + +void +cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter) +{ + cpSpaceLock(space); { + struct arbiterFilterContext context = {space, body, filter}; + cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context); + } cpSpaceUnlock(space, cpTrue); +} + +void +cpSpaceRemoveShape(cpSpace *space, cpShape *shape) +{ + cpBody *body = shape->body; + cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC); + if(isStatic){ + cpBodyActivateStatic(body, shape); + } else { + cpBodyActivate(body); + } + + cpBodyRemoveShape(body, shape); + cpSpaceFilterArbiters(space, body, shape); + cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid); + shape->space = NULL; + shape->hashid = 0; +} + +void +cpSpaceRemoveBody(cpSpace *space, cpBody *body) +{ + cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space."); + cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); +// cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it."); +// cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it."); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(body); +// cpSpaceFilterArbiters(space, body, NULL); + cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body); + body->space = NULL; +} + +void +cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(constraint->a); + cpBodyActivate(constraint->b); + cpArrayDeleteObj(space->constraints, constraint); + + cpBodyRemoveConstraint(constraint->a, constraint); + cpBodyRemoveConstraint(constraint->b, constraint); + constraint->space = NULL; +} + +cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape) +{ + return (shape->space == space); +} + +cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body) +{ + return (body->space == space); +} + +cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint) +{ + return (constraint->space == space); +} + +//MARK: Iteration + +void +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data) +{ + cpSpaceLock(space); { + cpArray *bodies = space->dynamicBodies; + for(int i=0; inum; i++){ + func((cpBody *)bodies->arr[i], data); + } + + cpArray *otherBodies = space->staticBodies; + for(int i=0; inum; i++){ + func((cpBody *)otherBodies->arr[i], data); + } + + cpArray *components = space->sleepingComponents; + for(int i=0; inum; i++){ + cpBody *root = (cpBody *)components->arr[i]; + + cpBody *body = root; + while(body){ + cpBody *next = body->sleeping.next; + func(body, data); + body = next; + } + } + } cpSpaceUnlock(space, cpTrue); +} + +typedef struct spaceShapeContext { + cpSpaceShapeIteratorFunc func; + void *data; +} spaceShapeContext; + +static void +spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context) +{ + context->func(shape, context->data); +} + +void +cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data) +{ + cpSpaceLock(space); { + spaceShapeContext context = {func, data}; + cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); + } cpSpaceUnlock(space, cpTrue); +} + +void +cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data) +{ + cpSpaceLock(space); { + cpArray *constraints = space->constraints; + + for(int i=0; inum; i++){ + func((cpConstraint *)constraints->arr[i], data); + } + } cpSpaceUnlock(space, cpTrue); +} + +//MARK: Spatial Index Management + +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)&cpShapeUpdateFunc, NULL); + cpSpatialIndexReindex(space->staticShapes); +} + +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."); + + cpShapeCacheBB(shape); + + // attempt to rehash the shape in both hashes + cpSpatialIndexReindexObject(space->dynamicShapes, shape, shape->hashid); + cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid); +} + +void +cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body) +{ + CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape); +} + + +static void +copyShapes(cpShape *shape, cpSpatialIndex *index) +{ + cpSpatialIndexInsert(index, shape, shape->hashid); +} + +void +cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL); + cpSpatialIndex *dynamicShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes); + + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes); + cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)copyShapes, dynamicShapes); + + cpSpatialIndexFree(space->staticShapes); + cpSpatialIndexFree(space->dynamicShapes); + + space->staticShapes = staticShapes; + space->dynamicShapes = dynamicShapes; +} diff --git a/Chipmunk2D/src/cpSpaceComponent.c b/Chipmunk2D/src/cpSpaceComponent.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpaceComponent.c @@ -0,0 +1,349 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" + +//MARK: Sleeping Functions + +void +cpSpaceActivateBody(cpSpace *space, cpBody *body) +{ + cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to activate a non-dynamic body."); + + if(space->locked){ + // cpSpaceActivateBody() is called again once the space is unlocked + if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); + } else { + cpAssertSoft(body->sleeping.root == NULL && body->sleeping.next == NULL, "Internal error: Activating body non-NULL node pointers."); + cpArrayPush(space->dynamicBodies, body); + + CP_BODY_FOREACH_SHAPE(body, shape){ + cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); + cpSpatialIndexInsert(space->dynamicShapes, shape, shape->hashid); + } + + CP_BODY_FOREACH_ARBITER(body, arb){ + cpBody *bodyA = arb->body_a; + + // Arbiters are shared between two bodies that are always woken up together. + // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter. + // The edge case is when static bodies are involved as the static bodies never actually sleep. + // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked. + if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){ + int numContacts = arb->count; + struct cpContact *contacts = arb->contacts; + + // Restore contact values back to the space's contact buffer memory + arb->contacts = cpContactBufferGetArray(space); + memcpy(arb->contacts, contacts, numContacts*sizeof(struct cpContact)); + cpSpacePushContacts(space, numContacts); + + // Reinsert the arbiter into the arbiter cache + const cpShape *a = arb->a, *b = arb->b; + const cpShape *shape_pair[] = {a, b}; + cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); + cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, NULL, arb); + + // Update the arbiter's state + arb->stamp = space->stamp; + cpArrayPush(space->arbiters, arb); + + cpfree(contacts); + } + } + + CP_BODY_FOREACH_CONSTRAINT(body, constraint){ + cpBody *bodyA = constraint->a; + if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayPush(space->constraints, constraint); + } + } +} + +static void +cpSpaceDeactivateBody(cpSpace *space, cpBody *body) +{ + cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to deactivate a non-dynamic body."); + + cpArrayDeleteObj(space->dynamicBodies, body); + + CP_BODY_FOREACH_SHAPE(body, shape){ + cpSpatialIndexRemove(space->dynamicShapes, shape, shape->hashid); + cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); + } + + CP_BODY_FOREACH_ARBITER(body, arb){ + cpBody *bodyA = arb->body_a; + if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){ + cpSpaceUncacheArbiter(space, arb); + + // Save contact values to a new block of memory so they won't time out + size_t bytes = arb->count*sizeof(struct cpContact); + struct cpContact *contacts = (struct cpContact *)cpcalloc(1, bytes); + memcpy(contacts, arb->contacts, bytes); + arb->contacts = contacts; + } + } + + CP_BODY_FOREACH_CONSTRAINT(body, constraint){ + cpBody *bodyA = constraint->a; + if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayDeleteObj(space->constraints, constraint); + } +} + +static inline cpBody * +ComponentRoot(cpBody *body) +{ + return (body ? body->sleeping.root : NULL); +} + +void +cpBodyActivate(cpBody *body) +{ + if(body != NULL && cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){ + body->sleeping.idleTime = 0.0f; + + cpBody *root = ComponentRoot(body); + if(root && cpBodyIsSleeping(root)){ + // TODO should cpBodyIsSleeping(root) be an assertion? + cpAssertSoft(cpBodyGetType(root) == CP_BODY_TYPE_DYNAMIC, "Internal Error: Non-dynamic body component root detected."); + + cpSpace *space = root->space; + cpBody *body = root; + while(body){ + cpBody *next = body->sleeping.next; + + body->sleeping.idleTime = 0.0f; + body->sleeping.root = NULL; + body->sleeping.next = NULL; + cpSpaceActivateBody(space, body); + + body = next; + } + + cpArrayDeleteObj(space->sleepingComponents, root); + } + + CP_BODY_FOREACH_ARBITER(body, arb){ + // Reset the idle timer of things the body is touching as well. + // That way things don't get left hanging in the air. + cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a); + if(cpBodyGetType(other) != CP_BODY_TYPE_STATIC) other->sleeping.idleTime = 0.0f; + } + } +} + +void +cpBodyActivateStatic(cpBody *body, cpShape *filter) +{ + cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_STATIC, "cpBodyActivateStatic() called on a non-static body."); + + CP_BODY_FOREACH_ARBITER(body, arb){ + if(!filter || filter == arb->a || filter == arb->b){ + cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a); + } + } + + // TODO: should also activate joints? +} + +static inline void +cpBodyPushArbiter(cpBody *body, cpArbiter *arb) +{ + cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)"); + cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)"); + + cpArbiter *next = body->arbiterList; + cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)"); + cpArbiterThreadForBody(arb, body)->next = next; + + if(next) cpArbiterThreadForBody(next, body)->prev = arb; + body->arbiterList = arb; +} + +static inline void +ComponentAdd(cpBody *root, cpBody *body){ + body->sleeping.root = root; + + if(body != root){ + body->sleeping.next = root->sleeping.next; + root->sleeping.next = body; + } +} + +static inline void +FloodFillComponent(cpBody *root, cpBody *body) +{ + // Kinematic bodies cannot be put to sleep and prevent bodies they are touching from sleeping. + // Static bodies are effectively sleeping all the time. + if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){ + 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->sleeping.idleTime < threshold) return cpTrue; + } + + return cpFalse; +} + +void +cpSpaceProcessComponents(cpSpace *space, cpFloat dt) +{ + cpBool sleep = (space->sleepTimeThreshold != INFINITY); + cpArray *bodies = space->dynamicBodies; + +#ifndef NDEBUG + for(int i=0; inum; i++){ + cpBody *body = (cpBody*)bodies->arr[i]; + + cpAssertSoft(body->sleeping.next == NULL, "Internal Error: Dangling next pointer detected in contact graph."); + cpAssertSoft(body->sleeping.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]; + + // TODO should make a separate array for kinematic bodies. + if(cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) continue; + + // Need to deal with infinite mass objects + cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); + body->sleeping.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->sleeping.idleTime + dt); + } + } + + // Awaken any sleeping bodies found and then push arbiters to the bodies' lists. + cpArray *arbiters = space->arbiters; + for(int i=0, count=arbiters->num; iarr[i]; + cpBody *a = arb->body_a, *b = arb->body_b; + + if(sleep){ + // TODO checking cpBodyIsSleepin() redundant? + if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(a)) cpBodyActivate(a); + if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(b)) cpBodyActivate(b); + } + + cpBodyPushArbiter(a, arb); + cpBodyPushArbiter(b, arb); + } + + if(sleep){ + // Bodies should be held active if connected by a joint to a kinematic. + cpArray *constraints = space->constraints; + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + cpBody *a = constraint->a, *b = constraint->b; + + if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(a); + if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC) 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); + + // 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->sleeping.root = NULL; + body->sleeping.next = NULL; + } + } +} + +void +cpBodySleep(cpBody *body) +{ + cpBodySleepWithGroup(body, NULL); +} + +void +cpBodySleepWithGroup(cpBody *body, cpBody *group){ + cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Non-dynamic bodies cannot be put to sleep."); + + cpSpace *space = body->space; + cpAssertHard(!cpSpaceIsLocked(space), "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback."); + cpAssertHard(cpSpaceGetSleepTimeThreshold(space) < INFINITY, "Sleeping is not enabled on the space. You cannot sleep a body without setting a sleep time threshold on the space."); + cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); + + if(cpBodyIsSleeping(body)){ + cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned."); + return; + } + + CP_BODY_FOREACH_SHAPE(body, shape) cpShapeCacheBB(shape); + cpSpaceDeactivateBody(space, body); + + if(group){ + cpBody *root = ComponentRoot(group); + + body->sleeping.root = root; + body->sleeping.next = root->sleeping.next; + body->sleeping.idleTime = 0.0f; + + root->sleeping.next = body; + } else { + body->sleeping.root = body; + body->sleeping.next = NULL; + body->sleeping.idleTime = 0.0f; + + cpArrayPush(space->sleepingComponents, body); + } + + cpArrayDeleteObj(space->dynamicBodies, body); +} diff --git a/Chipmunk2D/src/cpSpaceDebug.c b/Chipmunk2D/src/cpSpaceDebug.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpaceDebug.c @@ -0,0 +1,189 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +#ifndef CP_SPACE_DISABLE_DEBUG_API + +static void +cpSpaceDebugDrawShape(cpShape *shape, cpSpaceDebugDrawOptions *options) +{ + cpBody *body = shape->body; + cpDataPointer data = options->data; + + cpSpaceDebugColor outline_color = options->shapeOutlineColor; + cpSpaceDebugColor fill_color = options->colorForShape(shape, data); + + switch(shape->klass->type){ + case CP_CIRCLE_SHAPE: { + cpCircleShape *circle = (cpCircleShape *)shape; + options->drawCircle(circle->tc, body->a, circle->r, outline_color, fill_color, data); + break; + } + case CP_SEGMENT_SHAPE: { + cpSegmentShape *seg = (cpSegmentShape *)shape; + options->drawFatSegment(seg->ta, seg->tb, seg->r, outline_color, fill_color, data); + break; + } + case CP_POLY_SHAPE: { + cpPolyShape *poly = (cpPolyShape *)shape; + + int count = poly->count; + struct cpSplittingPlane *planes = poly->planes; + cpVect *verts = (cpVect *)alloca(count*sizeof(cpVect)); + + for(int i=0; idrawPolygon(count, verts, poly->r, outline_color, fill_color, data); + break; + } + default: break; + } +} + +static const cpVect spring_verts[] = { + {0.00f, 0.0f}, + {0.20f, 0.0f}, + {0.25f, 3.0f}, + {0.30f,-6.0f}, + {0.35f, 6.0f}, + {0.40f,-6.0f}, + {0.45f, 6.0f}, + {0.50f,-6.0f}, + {0.55f, 6.0f}, + {0.60f,-6.0f}, + {0.65f, 6.0f}, + {0.70f,-3.0f}, + {0.75f, 6.0f}, + {0.80f, 0.0f}, + {1.00f, 0.0f}, +}; +static const int spring_count = sizeof(spring_verts)/sizeof(cpVect); + +static void +cpSpaceDebugDrawConstraint(cpConstraint *constraint, cpSpaceDebugDrawOptions *options) +{ + cpDataPointer data = options->data; + cpSpaceDebugColor color = options->constraintColor; + + cpBody *body_a = constraint->a; + cpBody *body_b = constraint->b; + + if(cpConstraintIsPinJoint(constraint)){ + cpPinJoint *joint = (cpPinJoint *)constraint; + + cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); + cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); + + options->drawDot(5, a, color, data); + options->drawDot(5, b, color, data); + options->drawSegment(a, b, color, data); + } else if(cpConstraintIsSlideJoint(constraint)){ + cpSlideJoint *joint = (cpSlideJoint *)constraint; + + cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); + cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); + + options->drawDot(5, a, color, data); + options->drawDot(5, b, color, data); + options->drawSegment(a, b, color, data); + } else if(cpConstraintIsPivotJoint(constraint)){ + cpPivotJoint *joint = (cpPivotJoint *)constraint; + + cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); + cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); + + options->drawDot(5, a, color, data); + options->drawDot(5, b, color, data); + } else if(cpConstraintIsGrooveJoint(constraint)){ + cpGrooveJoint *joint = (cpGrooveJoint *)constraint; + + cpVect a = cpTransformPoint(body_a->transform, joint->grv_a); + cpVect b = cpTransformPoint(body_a->transform, joint->grv_b); + cpVect c = cpTransformPoint(body_b->transform, joint->anchorB); + + options->drawDot(5, c, color, data); + options->drawSegment(a, b, color, data); + } else if(cpConstraintIsDampedSpring(constraint)){ + cpDampedSpring *spring = (cpDampedSpring *)constraint; + cpDataPointer data = options->data; + cpSpaceDebugColor color = options->constraintColor; + + cpVect a = cpTransformPoint(body_a->transform, spring->anchorA); + cpVect b = cpTransformPoint(body_b->transform, spring->anchorB); + + options->drawDot(5, a, color, data); + options->drawDot(5, b, color, data); + + cpVect delta = cpvsub(b, a); + cpFloat cos = delta.x; + cpFloat sin = delta.y; + cpFloat s = 1.0f/cpvlength(delta); + + cpVect r1 = cpv(cos, -sin*s); + cpVect r2 = cpv(sin, cos*s); + + cpVect *verts = (cpVect *)alloca(spring_count*sizeof(cpVect)); + for(int i=0; idrawSegment(verts[i], verts[i + 1], color, data); + } + } +} + +void +cpSpaceDebugDraw(cpSpace *space, cpSpaceDebugDrawOptions *options) +{ + if(options->flags & CP_SPACE_DEBUG_DRAW_SHAPES){ + cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)cpSpaceDebugDrawShape, options); + } + + if(options->flags & CP_SPACE_DEBUG_DRAW_CONSTRAINTS){ + cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)cpSpaceDebugDrawConstraint, options); + } + + if(options->flags & CP_SPACE_DEBUG_DRAW_COLLISION_POINTS){ + cpArray *arbiters = space->arbiters; + cpSpaceDebugColor color = options->collisionPointColor; + cpSpaceDebugDrawSegmentImpl draw_seg = options->drawSegment; + cpDataPointer data = options->data; + + for(int i=0; inum; i++){ + cpArbiter *arb = (cpArbiter*)arbiters->arr[i]; + cpVect n = arb->n; + + for(int j=0; jcount; j++){ + cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[j].r1); + cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[j].r2); + + cpFloat d = 2.0f; + cpVect a = cpvadd(p1, cpvmult(n, -d)); + cpVect b = cpvadd(p2, cpvmult(n, d)); + draw_seg(a, b, color, data); + } + } + } +} + +#endif diff --git a/Chipmunk2D/src/cpSpaceHash.c b/Chipmunk2D/src/cpSpaceHash.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpaceHash.c @@ -0,0 +1,634 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" +#include "prime.h" + +typedef struct cpSpaceHashBin cpSpaceHashBin; +typedef struct cpHandle cpHandle; + +struct cpSpaceHash { + cpSpatialIndex spatialIndex; + + int numcells; + cpFloat celldim; + + cpSpaceHashBin **table; + cpHashSet *handleSet; + + cpSpaceHashBin *pooledBins; + cpArray *pooledHandles; + cpArray *allocatedBuffers; + + cpTimestamp stamp; +}; + + +//MARK: Handle Functions + +struct cpHandle { + void *obj; + int retain; + cpTimestamp stamp; +}; + +static cpHandle* +cpHandleInit(cpHandle *hand, void *obj) +{ + hand->obj = obj; + hand->retain = 0; + hand->stamp = 0; + + return hand; +} + +static inline void cpHandleRetain(cpHandle *hand){hand->retain++;} + +static inline void +cpHandleRelease(cpHandle *hand, cpArray *pooledHandles) +{ + hand->retain--; + if(hand->retain == 0) cpArrayPush(pooledHandles, hand); +} + +static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);} + +static void * +handleSetTrans(void *obj, cpSpaceHash *hash) +{ + if(hash->pooledHandles->num == 0){ + // handle pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpHandle); + cpAssertHard(count, "Internal Error: Buffer size is too small."); + + cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(hash->allocatedBuffers, buffer); + + for(int i=0; ipooledHandles, buffer + i); + } + + cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); + cpHandleRetain(hand); + + return hand; +} + +//MARK: Bin Functions + +struct cpSpaceHashBin { + cpHandle *handle; + cpSpaceHashBin *next; +}; + +static inline void +recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin) +{ + bin->next = hash->pooledBins; + hash->pooledBins = bin; +} + +static inline void +clearTableCell(cpSpaceHash *hash, int idx) +{ + cpSpaceHashBin *bin = hash->table[idx]; + while(bin){ + cpSpaceHashBin *next = bin->next; + + cpHandleRelease(bin->handle, hash->pooledHandles); + recycleBin(hash, bin); + + bin = next; + } + + hash->table[idx] = NULL; +} + +static void +clearTable(cpSpaceHash *hash) +{ + for(int i=0; inumcells; i++) clearTableCell(hash, i); +} + +// Get a recycled or new bin. +static inline cpSpaceHashBin * +getEmptyBin(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->pooledBins; + + if(bin){ + hash->pooledBins = bin->next; + return bin; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); + cpAssertHard(count, "Internal Error: Buffer size is too small."); + + cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(hash->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; itable); + + hash->numcells = numcells; + hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); +} + +static inline cpSpatialIndexClass *Klass(); + +cpSpatialIndex * +cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex); + + cpSpaceHashAllocTable(hash, next_prime(numcells)); + hash->celldim = celldim; + + hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql); + + hash->pooledHandles = cpArrayNew(0); + + hash->pooledBins = NULL; + hash->allocatedBuffers = cpArrayNew(0); + + hash->stamp = 1; + + return (cpSpatialIndex *)hash; +} + +cpSpatialIndex * +cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex); +} + +static void +cpSpaceHashDestroy(cpSpaceHash *hash) +{ + if(hash->table) clearTable(hash); + cpfree(hash->table); + + cpHashSetFree(hash->handleSet); + + cpArrayFreeEach(hash->allocatedBuffers, cpfree); + cpArrayFree(hash->allocatedBuffers); + cpArrayFree(hash->pooledHandles); +} + +//MARK: Helper Functions + +static inline cpBool +containsHandle(cpSpaceHashBin *bin, cpHandle *hand) +{ + while(bin){ + if(bin->handle == hand) return cpTrue; + bin = bin->next; + } + + return cpFalse; +} + +// The hash function itself. +static inline cpHashValue +hash_func(cpHashValue x, cpHashValue y, cpHashValue n) +{ + return (x*1640531513ul ^ y*2654435789ul) % n; +} + +// Much faster than (int)floor(f) +// Profiling showed floor() to be a sizable performance hog +static inline int +floor_int(cpFloat f) +{ + int i = (int)f; + return (f < 0.0f && f != i ? i - 1 : i); +} + +static inline void +hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) +{ + // Find the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = floor_int(bb.l/dim); // Fix by ShiftZ + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + int n = hash->numcells; + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + cpHashValue idx = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[idx]; + + // Don't add an object twice to the same cell. + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); + // Insert a new bin for the handle in this cell. + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[idx] = newBin; + } + } +} + +//MARK: Basic Operations + +static void +cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, (cpHashSetTransFunc)handleSetTrans, hash); + hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj)); +} + +static void +cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand, hash->pooledHandles); + + cpSpaceHashInsert(hash, obj, hashid); + } +} + +static void +rehash_helper(cpHandle *hand, cpSpaceHash *hash) +{ + hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj)); +} + +static void +cpSpaceHashRehash(cpSpaceHash *hash) +{ + clearTable(hash); + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash); +} + +static void +cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand, hash->pooledHandles); + } +} + +typedef struct eachContext { + cpSpatialIndexIteratorFunc func; + void *data; +} eachContext; + +static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);} + +static void +cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data) +{ + eachContext context = {func, data}; + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context); +} + +static void +remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) +{ + cpSpaceHashBin *bin = *bin_ptr; + while(bin){ + cpHandle *hand = bin->handle; + cpSpaceHashBin *next = bin->next; + + if(!hand->obj){ + // orphaned handle, unlink and recycle the bin + (*bin_ptr) = bin->next; + recycleBin(hash, bin); + + cpHandleRelease(hand, hash->pooledHandles); + } else { + bin_ptr = &bin->next; + } + + bin = next; + } +} + +//MARK: Query Functions + +static inline void +query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data) +{ + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + if(hand->stamp == hash->stamp || obj == other){ + continue; + } else if(other){ + func(obj, other, 0, data); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + remove_orphaned_handles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } + } +} + +static void +cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) +{ + // Get the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = floor_int(bb.l/dim); // Fix by ShiftZ + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + int n = hash->numcells; + cpSpaceHashBin **table = hash->table; + + // Iterate over the cells and query them. + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + query_helper(hash, &table[hash_func(i,j,n)], obj, func, data); + } + } + + hash->stamp++; +} + +// Similar to struct eachPair above. +typedef struct queryRehashContext { + cpSpaceHash *hash; + cpSpatialIndexQueryFunc func; + void *data; +} queryRehashContext; + +// Hashset iterator func used with cpSpaceHashQueryRehash(). +static void +queryRehash_helper(cpHandle *hand, queryRehashContext *context) +{ + cpSpaceHash *hash = context->hash; + cpSpatialIndexQueryFunc func = context->func; + void *data = context->data; + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + void *obj = hand->obj; + cpBB bb = hash->spatialIndex.bbfunc(obj); + + int l = floor_int(bb.l/dim); + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + cpSpaceHashBin **table = hash->table; + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + cpHashValue idx = hash_func(i,j,n); + cpSpaceHashBin *bin = table[idx]; + + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() + query_helper(hash, &bin, obj, func, data); + + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + table[idx] = newBin; + } + } + + // Increment the stamp for each object hashed. + hash->stamp++; +} + +static void +cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data) +{ + clearTable(hash); + + queryRehashContext context = {hash, func, data}; + cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context); + + cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data); +} + +static inline cpFloat +segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data) +{ + cpFloat t = 1.0f; + + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + // Skip over certain conditions + if(hand->stamp == hash->stamp){ + continue; + } else if(other){ + t = cpfmin(t, func(obj, other, data)); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + remove_orphaned_handles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } + } + + return t; +} + +// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html +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); + b = cpvmult(b, 1.0f/hash->celldim); + + int cell_x = floor_int(a.x), cell_y = floor_int(a.y); + + cpFloat t = 0; + + int x_inc, y_inc; + cpFloat temp_v, temp_h; + + if (b.x > a.x){ + x_inc = 1; + temp_h = (cpffloor(a.x + 1.0f) - a.x); + } else { + x_inc = -1; + temp_h = (a.x - cpffloor(a.x)); + } + + if (b.y > a.y){ + y_inc = 1; + temp_v = (cpffloor(a.y + 1.0f) - a.y); + } else { + y_inc = -1; + temp_v = (a.y - cpffloor(a.y)); + } + + // Division by zero is *very* slow on ARM + cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); + cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); + + // fix NANs in horizontal directions + cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); + cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); + + int n = hash->numcells; + cpSpaceHashBin **table = hash->table; + + while(t < t_exit){ + 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){ + cell_y += y_inc; + t = next_v; + next_v += dt_dy; + } else { + cell_x += x_inc; + t = next_h; + next_h += dt_dx; + } + } + + hash->stamp++; +} + +//MARK: Misc + +void +cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) +{ + if(hash->spatialIndex.klass != Klass()){ + cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index."); + return; + } + + clearTable(hash); + + hash->celldim = celldim; + cpSpaceHashAllocTable(hash, next_prime(numcells)); +} + +static int +cpSpaceHashCount(cpSpaceHash *hash) +{ + return cpHashSetCount(hash->handleSet); +} + +static int +cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + return cpHashSetFind(hash->handleSet, hashid, obj) != NULL; +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyImpl)cpSpaceHashDestroy, + + (cpSpatialIndexCountImpl)cpSpaceHashCount, + (cpSpatialIndexEachImpl)cpSpaceHashEach, + (cpSpatialIndexContainsImpl)cpSpaceHashContains, + + (cpSpatialIndexInsertImpl)cpSpaceHashInsert, + (cpSpatialIndexRemoveImpl)cpSpaceHashRemove, + + (cpSpatialIndexReindexImpl)cpSpaceHashRehash, + (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject, + (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery, + + (cpSpatialIndexQueryImpl)cpSpaceHashQuery, + (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, +}; + +static inline cpSpatialIndexClass *Klass(){return &klass;} + +//MARK: Debug Drawing + +//#define CP_BBTREE_DEBUG_DRAW +#ifdef CP_BBTREE_DEBUG_DRAW +#include "OpenGL/gl.h" +#include "OpenGL/glu.h" +#include + +void +cpSpaceHashRenderDebug(cpSpatialIndex *index) +{ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index."); + return; + } + + cpSpaceHash *hash = (cpSpaceHash *)index; + cpBB bb = cpBBNew(-320, -240, 320, 240); + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + int l = (int)floor(bb.l/dim); + int r = (int)floor(bb.r/dim); + int b = (int)floor(bb.b/dim); + int t = (int)floor(bb.t/dim); + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int cell_count = 0; + + int index = hash_func(i,j,n); + for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next) + cell_count++; + + GLfloat v = 1.0f - (GLfloat)cell_count/10.0f; + glColor3f(v,v,v); + glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim); + } + } +} +#endif diff --git a/Chipmunk2D/src/cpSpaceQuery.c b/Chipmunk2D/src/cpSpaceQuery.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpaceQuery.c @@ -0,0 +1,246 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +//MARK: Nearest Point Query Functions + +struct PointQueryContext { + cpVect point; + cpFloat maxDistance; + cpShapeFilter filter; + cpSpacePointQueryFunc func; +}; + +static cpCollisionID +NearestPointQuery(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, void *data) +{ + if( + !cpShapeFilterReject(shape->filter, context->filter) + ){ + cpPointQueryInfo info; + cpShapePointQuery(shape, context->point, &info); + + if(info.shape && info.distance < context->maxDistance) context->func(shape, info.point, info.distance, info.gradient, data); + } + + return id; +} + +void +cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data) +{ + struct PointQueryContext context = {point, maxDistance, filter, func}; + cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); + } cpSpaceUnlock(space, cpTrue); +} + +static cpCollisionID +NearestPointQueryNearest(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, cpPointQueryInfo *out) +{ + if( + !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor + ){ + cpPointQueryInfo info; + cpShapePointQuery(shape, context->point, &info); + + if(info.distance < out->distance) (*out) = info; + } + + return id; +} + +cpShape * +cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out) +{ + cpPointQueryInfo info = {NULL, cpvzero, maxDistance, cpvzero}; + if(out){ + (*out) = info; + } else { + out = &info; + } + + struct PointQueryContext context = { + point, maxDistance, + filter, + NULL + }; + + cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); + cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); + + return (cpShape *)out->shape; +} + + +//MARK: Segment Query Functions + +struct SegmentQueryContext { + cpVect start, end; + cpFloat radius; + cpShapeFilter filter; + cpSpaceSegmentQueryFunc func; +}; + +static cpFloat +SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data) +{ + cpSegmentQueryInfo info; + + if( + !cpShapeFilterReject(shape->filter, context->filter) && + cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) + ){ + context->func(shape, info.point, info.normal, info.alpha, data); + } + + return 1.0f; +} + +void +cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data) +{ + struct SegmentQueryContext context = { + start, end, + radius, + filter, + func, + }; + + cpSpaceLock(space); { + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); + cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); + } cpSpaceUnlock(space, cpTrue); +} + +static cpFloat +SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out) +{ + cpSegmentQueryInfo info; + + if( + !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor && + cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) && + info.alpha < out->alpha + ){ + (*out) = info; + } + + return out->alpha; +} + +cpShape * +cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out) +{ + cpSegmentQueryInfo info = {NULL, end, cpvzero, 1.0f}; + if(out){ + (*out) = info; + } else { + out = &info; + } + + struct SegmentQueryContext context = { + start, end, + radius, + filter, + NULL + }; + + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); + cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, out->alpha, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); + + return (cpShape *)out->shape; +} + +//MARK: BB Query Functions + +struct BBQueryContext { + cpBB bb; + cpShapeFilter filter; + cpSpaceBBQueryFunc func; +}; + +static cpCollisionID +BBQuery(struct BBQueryContext *context, cpShape *shape, cpCollisionID id, void *data) +{ + if( + !cpShapeFilterReject(shape->filter, context->filter) && + cpBBIntersects(context->bb, shape->bb) + ){ + context->func(shape, data); + } + + return id; +} + +void +cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data) +{ + struct BBQueryContext context = {bb, filter, func}; + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); + cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); + } cpSpaceUnlock(space, cpTrue); +} + +//MARK: Shape Query Functions + +struct ShapeQueryContext { + cpSpaceShapeQueryFunc func; + void *data; + cpBool anyCollision; +}; + +// Callback from the spatial hash. +static cpCollisionID +ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context) +{ + if(cpShapeFilterReject(a->filter, b->filter) || a == b) return id; + + cpContactPointSet set = cpShapesCollide(a, b); + if(set.count){ + if(context->func) context->func(b, &set, context->data); + context->anyCollision = !(a->sensor || b->sensor); + } + + return id; +} + +cpBool +cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data) +{ + cpBody *body = shape->body; + cpBB bb = (body ? cpShapeUpdate(shape, body->transform) : shape->bb); + struct ShapeQueryContext context = {func, data, cpFalse}; + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->dynamicShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); + cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); + } cpSpaceUnlock(space, cpTrue); + + return context.anyCollision; +} diff --git a/Chipmunk2D/src/cpSpaceStep.c b/Chipmunk2D/src/cpSpaceStep.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpaceStep.c @@ -0,0 +1,445 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +//MARK: Post Step Callback Functions + +cpPostStepCallback * +cpSpaceGetPostStepCallback(cpSpace *space, void *key) +{ + cpArray *arr = space->postStepCallbacks; + for(int i=0; inum; i++){ + cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; + if(callback && callback->key == key) return callback; + } + + return NULL; +} + +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(!cpSpaceGetPostStepCallback(space, key)){ + cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback)); + callback->func = (func ? func : PostStepDoNothing); + callback->key = key; + callback->data = data; + + cpArrayPush(space->postStepCallbacks, callback); + return cpTrue; + } else { + return cpFalse; + } +} + +//MARK: Locking Functions + +void +cpSpaceLock(cpSpace *space) +{ + space->locked++; +} + +void +cpSpaceUnlock(cpSpace *space, cpBool runPostStep) +{ + space->locked--; + cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow."); + + if(space->locked == 0){ + cpArray *waking = space->rousedBodies; + + for(int i=0, count=waking->num; iarr[i]); + waking->arr[i] = NULL; + } + + waking->num = 0; + + if(space->locked == 0 && runPostStep && !space->skipPostStep){ + space->skipPostStep = cpTrue; + + 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. + // TODO: need more tests around this case I think. + callback->func = NULL; + if(func) func(space, callback->key, callback->data); + + arr->arr[i] = NULL; + cpfree(callback); + } + + arr->num = 0; + space->skipPostStep = cpFalse; + } + } +} + +//MARK: Contact Buffer Functions + +struct cpContactBufferHeader { + cpTimestamp stamp; + cpContactBufferHeader *next; + unsigned int numContacts; +}; + +#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(struct cpContact)) +typedef struct cpContactBuffer { + cpContactBufferHeader header; + struct cpContact contacts[CP_CONTACTS_BUFFER_SIZE]; +} cpContactBuffer; + +static cpContactBufferHeader * +cpSpaceAllocContactBuffer(cpSpace *space) +{ + cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer)); + cpArrayPush(space->allocatedBuffers, buffer); + return (cpContactBufferHeader *)buffer; +} + +static cpContactBufferHeader * +cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice) +{ + header->stamp = stamp; + header->next = (splice ? splice->next : header); + header->numContacts = 0; + + return header; +} + +void +cpSpacePushFreshContactBuffer(cpSpace *space) +{ + cpTimestamp stamp = space->stamp; + + cpContactBufferHeader *head = space->contactBuffersHead; + + if(!head){ + // No buffers have been allocated, make one + space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL); + } else if(stamp - head->next->stamp > space->collisionPersistence){ + // The tail buffer is available, rotate the ring + cpContactBufferHeader *tail = head->next; + space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); + } else { + // Allocate a new buffer and push it into the ring + cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); + space->contactBuffersHead = head->next = buffer; + } +} + + +struct cpContact * +cpContactBufferGetArray(cpSpace *space) +{ + if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){ + // contact buffer could overflow on the next collision, push a fresh one. + cpSpacePushFreshContactBuffer(space); + } + + cpContactBufferHeader *head = space->contactBuffersHead; + return ((cpContactBuffer *)head)->contacts + head->numContacts; +} + +void +cpSpacePushContacts(cpSpace *space, int count) +{ + cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!"); + space->contactBuffersHead->numContacts += count; +} + +static void +cpSpacePopContacts(cpSpace *space, int count){ + space->contactBuffersHead->numContacts -= count; +} + +//MARK: Collision Detection Functions + +static void * +cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) +{ + if(space->pooledArbiters->num == 0){ + // arbiter pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpArbiter); + cpAssertHard(count, "Internal Error: Buffer size too small."); + + cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES); + cpArrayPush(space->allocatedBuffers, buffer); + + for(int i=0; ipooledArbiters, buffer + i); + } + + return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); +} + +static inline cpBool +QueryRejectConstraint(cpBody *a, cpBody *b) +{ + CP_BODY_FOREACH_CONSTRAINT(a, constraint){ + if( + !constraint->collideBodies && ( + (constraint->a == a && constraint->b == b) || + (constraint->a == b && constraint->b == a) + ) + ) return cpTrue; + } + + return cpFalse; +} + +static inline cpBool +QueryReject(cpShape *a, cpShape *b) +{ + return ( + // BBoxes must overlap + !cpBBIntersects(a->bb, b->bb) + // Don't collide shapes attached to the same body. + || a->body == b->body + // Don't collide shapes that are filtered. + || cpShapeFilterReject(a->filter, b->filter) + // Don't collide bodies if they have a constraint with collideBodies == cpFalse. + || QueryRejectConstraint(a->body, b->body) + ); +} + +// Callback from the spatial hash. +cpCollisionID +cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space) +{ + // Reject any of the simple cases + if(QueryReject(a,b)) return id; + + // Narrow-phase collision detection. + struct cpCollisionInfo info = cpCollide(a, b, id, cpContactBufferGetArray(space)); + + if(info.count == 0) return info.id; // Shapes are not colliding. + cpSpacePushContacts(space, info.count); + + // Get an arbiter from space->arbiterSet for the two shapes. + // This is where the persistant contact magic comes from. + const cpShape *shape_pair[] = {info.a, info.b}; + cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)info.a, (cpHashValue)info.b); + cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, (cpHashSetTransFunc)cpSpaceArbiterSetTrans, space); + cpArbiterUpdate(arb, &info, space); + + cpCollisionHandler *handler = arb->handler; + + // Call the begin function first if it's the first step + if(arb->state == CP_ARBITER_STATE_FIRST_COLLISION && !handler->beginFunc(arb, space, handler->userData)){ + cpArbiterIgnore(arb); // permanently ignore the collision until separation + } + + if( + // Ignore the arbiter if it has been flagged + (arb->state != CP_ARBITER_STATE_IGNORE) && + // Call preSolve + handler->preSolveFunc(arb, space, handler->userData) && + // Check (again) in case the pre-solve() callback called cpArbiterIgnored(). + arb->state != CP_ARBITER_STATE_IGNORE && + // Process, but don't add collisions for sensors. + !(a->sensor || b->sensor) && + // Don't process collisions between two infinite mass bodies. + // This includes collisions between two kinematic bodies, or a kinematic body and a static body. + !(a->body->m == INFINITY && b->body->m == INFINITY) + ){ + cpArrayPush(space->arbiters, arb); + } else { + cpSpacePopContacts(space, info.count); + + arb->contacts = NULL; + arb->count = 0; + + // Normally arbiters are set as used after calling the post-solve callback. + // However, post-solve() callbacks are not called for sensors or arbiters rejected from pre-solve. + if(arb->state != CP_ARBITER_STATE_IGNORE) arb->state = CP_ARBITER_STATE_NORMAL; + } + + // Time stamp the arbiter so we know it was used recently. + arb->stamp = space->stamp; + return info.id; +} + +// Hashset filter func to throw away old arbiters. +cpBool +cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) +{ + cpTimestamp ticks = space->stamp - arb->stamp; + + cpBody *a = arb->body_a, *b = arb->body_b; + + // 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( + (cpBodyGetType(a) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(a)) && + (cpBodyGetType(b) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(b)) + ){ + return cpTrue; + } + + // Arbiter was used last frame, but not this one + if(ticks >= 1 && arb->state != CP_ARBITER_STATE_CACHED){ + arb->state = CP_ARBITER_STATE_CACHED; + cpCollisionHandler *handler = arb->handler; + handler->separateFunc(arb, space, handler->userData); + } + + if(ticks >= space->collisionPersistence){ + arb->contacts = NULL; + arb->count = 0; + + cpArrayPush(space->pooledArbiters, arb); + return cpFalse; + } + + return cpTrue; +} + +//MARK: All Important cpSpaceStep() Function + + void +cpShapeUpdateFunc(cpShape *shape, void *unused) +{ + cpShapeCacheBB(shape); +} + +void +cpSpaceStep(cpSpace *space, cpFloat dt) +{ + // 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; + + cpArray *bodies = space->dynamicBodies; + 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 = CP_ARBITER_STATE_NORMAL; + + // If both bodies are awake, unthread the arbiter from the contact graph. + if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){ + cpArbiterUnthread(arb); + } + } + arbiters->num = 0; + + 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->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); + cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); + } cpSpaceUnlock(space, cpFalse); + + // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) + cpSpaceProcessComponents(space, dt); + + 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); + } + + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + + cpConstraintPreSolveFunc preSolve = constraint->preSolve; + if(preSolve) preSolve(constraint, space); + + 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 = (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->postSolveFunc(arb, space, handler->userData); + } + } cpSpaceUnlock(space, cpTrue); +} diff --git a/Chipmunk2D/src/cpSpatialIndex.c b/Chipmunk2D/src/cpSpatialIndex.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSpatialIndex.c @@ -0,0 +1,69 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +void +cpSpatialIndexFree(cpSpatialIndex *index) +{ + if(index){ + cpSpatialIndexDestroy(index); + cpfree(index); + } +} + +cpSpatialIndex * +cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + index->klass = klass; + index->bbfunc = bbfunc; + index->staticIndex = staticIndex; + + if(staticIndex){ + cpAssertHard(!staticIndex->dynamicIndex, "This static index is already associated with a dynamic index."); + staticIndex->dynamicIndex = index; + } + + return index; +} + +typedef struct dynamicToStaticContext { + cpSpatialIndexBBFunc bbfunc; + cpSpatialIndex *staticIndex; + cpSpatialIndexQueryFunc queryFunc; + void *data; +} dynamicToStaticContext; + +static void +dynamicToStaticIter(void *obj, dynamicToStaticContext *context) +{ + cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data); +} + +void +cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data) +{ + if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){ + dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; + cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context); + } +} + diff --git a/Chipmunk2D/src/cpSweep1D.c b/Chipmunk2D/src/cpSweep1D.c new file mode 100644 --- /dev/null +++ b/Chipmunk2D/src/cpSweep1D.c @@ -0,0 +1,254 @@ +/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software + * + * 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" + +static inline cpSpatialIndexClass *Klass(); + +//MARK: Basic Structures + +typedef struct Bounds { + cpFloat min, max; +} Bounds; + +typedef struct TableCell { + void *obj; + Bounds bounds; +} TableCell; + +struct cpSweep1D +{ + cpSpatialIndex spatialIndex; + + int num; + int max; + TableCell *table; +}; + +static inline cpBool +BoundsOverlap(Bounds a, Bounds b) +{ + return (a.min <= b.max && b.min <= a.max); +} + +static inline Bounds +BBToBounds(cpSweep1D *sweep, cpBB bb) +{ + Bounds bounds = {bb.l, bb.r}; + return bounds; +} + +static inline TableCell +MakeTableCell(cpSweep1D *sweep, void *obj) +{ + TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))}; + return cell; +} + +//MARK: Memory Management Functions + +cpSweep1D * +cpSweep1DAlloc(void) +{ + return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D)); +} + +static void +ResizeTable(cpSweep1D *sweep, int size) +{ + sweep->max = size; + sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell)); +} + +cpSpatialIndex * +cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)sweep, Klass(), bbfunc, staticIndex); + + sweep->num = 0; + ResizeTable(sweep, 32); + + return (cpSpatialIndex *)sweep; +} + +cpSpatialIndex * +cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex); +} + +static void +cpSweep1DDestroy(cpSweep1D *sweep) +{ + cpfree(sweep->table); + sweep->table = NULL; +} + +//MARK: Misc + +static int +cpSweep1DCount(cpSweep1D *sweep) +{ + return sweep->num; +} + +static void +cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data) +{ + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; itable; + for(int i=0, count=sweep->num; inum == sweep->max) ResizeTable(sweep, sweep->max*2); + + sweep->table[sweep->num] = MakeTableCell(sweep, obj); + sweep->num++; +} + +static void +cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid) +{ + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; inum; + + table[i] = table[num]; + table[num].obj = NULL; + + return; + } + } +} + +//MARK: Reindexing Functions + +static void +cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid) +{ + // Nothing to do here +} + +static void +cpSweep1DReindex(cpSweep1D *sweep) +{ + // Nothing to do here + // Could perform a sort, but queries are not accelerated anyway. +} + +//MARK: Query Functions + +static void +cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) +{ + // Implementing binary search here would allow you to find an upper limit + // but not a lower limit. Probably not worth the hassle. + + Bounds bounds = BBToBounds(sweep, bb); + + TableCell *table = sweep->table; + for(int i=0, count=sweep->num; itable; + for(int i=0, count=sweep->num; ibounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0)); +} + +static void +cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data) +{ + TableCell *table = sweep->table; + int count = sweep->num; + + // Update bounds and sort + for(int i=0; ispatialIndex.staticIndex, func, data); +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyImpl)cpSweep1DDestroy, + + (cpSpatialIndexCountImpl)cpSweep1DCount, + (cpSpatialIndexEachImpl)cpSweep1DEach, + (cpSpatialIndexContainsImpl)cpSweep1DContains, + + (cpSpatialIndexInsertImpl)cpSweep1DInsert, + (cpSpatialIndexRemoveImpl)cpSweep1DRemove, + + (cpSpatialIndexReindexImpl)cpSweep1DReindex, + (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject, + (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery, + + (cpSpatialIndexQueryImpl)cpSweep1DQuery, + (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, +}; + +static inline cpSpatialIndexClass *Klass(){return &klass;} + diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,30 @@ +Copyright Jonas Carpay (c) 2017 + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of Jonas Carpay nor the names of other + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 --- /dev/null +++ b/README.md @@ -0,0 +1,157 @@ +# apecs-physics [![Build Status](https://travis-ci.org/jonascarpay/apecs-physics.svg?branch=master)](https://travis-ci.org/jonascarpay/apecs-physics) + +2D physics library for apecs. +Uses [Chipmunk](https://github.com/slembcke/Chipmunk2D) for C-speed physics. +The [apecs-physics-gloss](https://github.com/jonascarpay/apecs-physics/tree/master/apecs-physics-gloss) package provides a simple optional [gloss](https://github.com/benl23x5/gloss)-based renderer. + +Feel free to create an issue or PR for suggestions/questions/requests/critiques/spelling fixes/etc. +See [TODO.md](https://github.com/jonascarpay/apecs-physics/blob/master/TODO.md) for suggestions if you want to help out with the code. + +The examples directory contains a number of examples, each can be run with `stack build && stack `: + +### helloworld +![Screenshot](https://raw.githubusercontent.com/jonascarpay/apecs-physics/master/examples/helloworld.png) + +```haskell +makeWorld "World" [''Physics, ''BodyPicture, ''Camera] +``` +Generate a world. +The `Physics` component adds a physics space to the world. +The `BodyPicture` contains a gloss `Picture`, which the renderer will match to the `Body`'s position and orientation. +The `Camera` component tracks a camera position and zoom factor. + +```haskell +initialize = do + setGlobal ( Camera (V2 0 1) 60 + , earthGravity ) +``` +`setGlobal` comes from apecs, and is pretty straightforward. +`earthGravity = V2 0 (-9.81)`, normal earth surface gravity if we assume units to be mks. +Note that the positive y-axis points upwards. + +```haskell + let ballShape = cCircle 0.5 + newEntity ( DynamicBody + , Shape ballShape + , Position (V2 0 3) + , Density 1 + , Elasticity 0.9 + , BodyPicture . color red . toPicture $ ballShape ) +``` +Still in the initialize function, here we see our first object being instantiated. +The type of ballShape is `Convex`, the apecs-physics format for shapes. +`Convex` is a convex polygon, consisting of a number of vertices and a radius. +In the case of a circle, the polygon consists of a single point with a non-zero radius. +Both Chipmunk and gloss only support convex polygons, `Convex` is used to give them a common interface. + +A `DynamicBody` is one of three types of bodies. +It is a normal body, fully affected by physical forces. +The elasticity of a collision is the product of the elasticities of the colliding shapes. + +The final line shows how to do rendering. +`BodyPicture` expects a gloss `Picture`, in this case we derive one from `ballShape :: Convex` using `toPicture`. +`color red` comes from gloss, and is just one of the many `Picture` manipulation functions. +Alternatively, you can use a `Bitmap` to use actual sprites. + +```haskell + let lineShape = hLine 6 + newEntity ( StaticBody + , Angle (-pi/20) + , Shape lineShape + , Elasticity 0.9 + , BodyPicture . color white . toPicture $ lineShape ) +``` +Static bodies are not affected by physics, and generally rarely move. +They are equivalent to bodies with infinite mass and moment, and zero velocity. +Changing their position triggers an explicit rehash of their shapes, wish is relatively expensive. + +```haskell +main = do + w <- initWorld + runSystem initialize w + defaultSimulate w +``` +`defaultSimulate` is a convenience wrapper around gloss' `simulateIO`. +You can find its definition in `Apecs.Physics.Gloss`, in case you want to change the rendering behavior. + +### tumbler +![Screenshot](https://raw.githubusercontent.com/jonascarpay/apecs-physics/master/examples/tumbler.png) + +```haskell +initialize :: System World () +initialize = do + setGlobal ( Camera 0 50 + , earthGravity ) + + let sides = toEdges $ cRectangle 5 + tumbler <- newEntity ( KinematicBody + , AngularVelocity (-1) + , BodyPicture . color white . foldMap toPicture $ sides ) +``` +As previously stated, both Chipmunk and gloss exclusively have _convex_ polygon primitives. +Our tumbler, however, is obiously not convex. +Fortunately, composing shapes is really easy. +We use `toEdges` to turn a rectangle into an outline of one, and use `foldMap` to make a composite `Picture`. + +A `KinematicBody` is halfway between a `DynamicBody` and a `StaticBody`. +It can have an (angular) velocity, but will not respond to forces. +It can be used for e.g. moving platforms, or in this case. +Note that we did not add any shapes to the tumbler yet. + +```haskell + forM_ sides $ \line -> newEntity (ShapeExtend (cast tumbler) $ setRadius 0.05 line) +``` +The time has come to talk about the destinction between shapes and bodies. +A body can have multiple shapes. +Shapes belonging to the same body _cannot move relative to one another_, i.e. a body is a fixture for multiple shapes. +When using the normal `Shape` data constructor to add a shape to a body, we actually create two Chipmunk structs; one for the body, and one for the shape, even though they are addressed by the same entity in apecs. + +When we want to add multiple shapes to a body, however, we need to make new entities for each individual shape. +The reason for this is that this way, we can still easily change the properties of each individual shape. +`Shape` actually just represents a special case of `ShapeExtend`, the case in which the body has the same entity as the shape. + +When you use a tuple of components in apecs, they are added in the order you list them in the tuple. +This is important to realize, as _adding a shape to an entity wihout a body is a noop_. +Always make sure you first add a body, and then the shapes. +This also comes up when e.g. setting a shape's properties: you can only set a shape's `Mass` or `Density` when there is a shape in the first place. +If you don't, you will get a runtime error about simulating zero-mass `DynamicBodies`. + +```haskell + replicateM_ 200 $ do + x <- liftIO$ randomRIO (-2, 2) + y <- liftIO$ randomRIO (-2, 2) + r <- liftIO$ randomRIO (0.1, 0.2) + let ballshape = cCircle r + let c = (realToFrac x+2)/3 + newEntity ( DynamicBody + , Position (V2 x y) + , Shape ballshape + , BodyPicture . color (makeColor 1 c c 1) . toPicture $ ballshape + , Density 1 ) + + return () +``` +Finally, we randomly add a bunch of balls. + +### constraints +![Screenshot](https://raw.githubusercontent.com/jonascarpay/apecs-physics/master/examples/constraints.png) + +The final example is a gallery of (some of) the available constraints. +Drag shapes around with the left mouse button, create a new box with the right. + +This example is too large to fully include here, but if you have made it this far, I recommend looking at the source. +Aside from demonstrating constraints, queries and interaction it also contains some neat tricks like: +```haskell +let rubber = (Friction 0.5, Elasticity 0.5, Density 1) +newEntity ( DynamicBody + , someShape + , rubber ) +``` +Nesting tuples creates composable and reusable pieces of configuration (this is an apecs thing, not an apecs-physics thing). +This can also be useful if you find yourself needing bigger tuples than the current maximum. + +Constraints are a lot like shapes, but instead of having one associated `Body`, they have two. +It also comes in the varieties `Constraint` and `ConstraintExtend`. + +Dragging an object with the mouse is also done using a constraint. +The mouse position actually controls the position of a static body without shapes, and we use a PinJoint to attach whatever we are dragging to it. diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,50 @@ +{-# LANGUAGE CPP #-} +import Distribution.Simple +import Distribution.Verbosity +import Distribution.PackageDescription +#if defined(MIN_VERSION_Cabal) && MIN_VERSION_Cabal(2,2,0) +import Distribution.PackageDescription.Parsec +#else +import Distribution.PackageDescription.Parse +#endif + + +main = do +#if defined(MIN_VERSION_Cabal) && MIN_VERSION_Cabal(2,0,0) + pkgDescr <- readGenericPackageDescription verbose "apecs-physics.cabal" +#else + pkgDescr <- readPackageDescription verbose "apecs-physics.cabal" +#endif + defaultMainNoRead . addCFilesIfNeeded $ pkgDescr + where + addCFilesIfNeeded = +#if __GLASGOW_HASKELL__ >= 802 + id +#else + addCFiles +#endif + + addCFiles genPkgDescr = genPkgDescr + { condLibrary = fmap addCFiles' pkgLibrary + } + where + pkgLibrary = condLibrary genPkgDescr + + addCFiles' (CondNode lib constraints components) = + CondNode (addCFiles'' lib) constraints components + + addCFiles'' lib = lib + { libBuildInfo = + let buildInfo = libBuildInfo lib in + buildInfo + { cSources = cSources buildInfo ++ + [ "src/Apecs/Physics/Constraint.c" + , "src/Apecs/Physics/Space.c" + , "src/Apecs/Physics/Query.c" + , "src/Apecs/Physics/Body.c" + , "src/Apecs/Physics/Collision.c" + , "src/Apecs/Physics/Shape.c" + ] + } + } + diff --git a/apecs-physics.cabal b/apecs-physics.cabal new file mode 100644 --- /dev/null +++ b/apecs-physics.cabal @@ -0,0 +1,133 @@ +name: apecs-physics +version: 0.2.0.0 +synopsis: 2D physics for apecs +description: 2D physics for apecs. Uses Chipmunk physics library under the hood. +homepage: https://github.com/jonascarpay/apecs-physics#readme +license: BSD3 +license-file: LICENSE +author: Jonas Carpay +maintainer: jonascarpay@gmail.com +copyright: MIT +category: Web +build-type: Custom +cabal-version: >=1.10 +extra-source-files: + README.md, + Chipmunk2D/include/chipmunk/prime.h, + Chipmunk2D/include/chipmunk/chipmunk_private.h, + Chipmunk2D/include/chipmunk/chipmunk_types.h, + Chipmunk2D/include/chipmunk/cpSlideJoint.h, + Chipmunk2D/include/chipmunk/cpBB.h, + Chipmunk2D/include/chipmunk/cpPolyline.h, + Chipmunk2D/include/chipmunk/cpBody.h, + Chipmunk2D/include/chipmunk/chipmunk_unsafe.h, + Chipmunk2D/include/chipmunk/cpSimpleMotor.h, + Chipmunk2D/include/chipmunk/chipmunk.h, + Chipmunk2D/include/chipmunk/cpSpatialIndex.h, + Chipmunk2D/include/chipmunk/cpRobust.h, + Chipmunk2D/include/chipmunk/chipmunk_structs.h, + Chipmunk2D/include/chipmunk/cpMarch.h, + Chipmunk2D/include/chipmunk/cpTransform.h, + Chipmunk2D/include/chipmunk/cpShape.h, + Chipmunk2D/include/chipmunk/cpConstraint.h, + Chipmunk2D/include/chipmunk/cpGrooveJoint.h, + Chipmunk2D/include/chipmunk/cpHastySpace.h, + Chipmunk2D/include/chipmunk/cpGearJoint.h, + Chipmunk2D/include/chipmunk/chipmunk_ffi.h, + Chipmunk2D/include/chipmunk/cpRatchetJoint.h, + Chipmunk2D/include/chipmunk/cpRotaryLimitJoint.h, + Chipmunk2D/include/chipmunk/cpVect.h, + Chipmunk2D/include/chipmunk/cpArbiter.h, + Chipmunk2D/include/chipmunk/cpSpace.h, + Chipmunk2D/include/chipmunk/cpDampedRotarySpring.h, + Chipmunk2D/include/chipmunk/cpPinJoint.h, + Chipmunk2D/include/chipmunk/cpDampedSpring.h, + Chipmunk2D/include/chipmunk/cpPolyShape.h, + Chipmunk2D/include/chipmunk/cpPivotJoint.h + + + + +source-repository HEAD + type: git + location: git://github.com/jonascarpay/apecs.git + +custom-setup + setup-depends: + base >= 4.5 && < 4.13, + Cabal >= 1.14 + +flag release + description: Release mode, better performance but no longer provides debug info on the command line. + Manual: True + default: False + +library + hs-source-dirs: + src + default-language: + Haskell2010 + exposed-modules: + Apecs.Physics + other-modules: + Apecs.Physics.Body, + Apecs.Physics.Constraint, + Apecs.Physics.Collision, + Apecs.Physics.Geometry, + Apecs.Physics.Shape, + Apecs.Physics.Space, + Apecs.Physics.Types + Apecs.Physics.Query + build-depends: + base >= 4.7 && < 4.13, + apecs >= 0.5.0.0, + containers, + inline-c, + linear, + template-haskell, + vector + ghc-options: + -Wall + -O2 + -Wno-orphans + -Wno-unused-do-bind + -Wno-name-shadowing + cc-options: + -std=c99 + if flag(release) + cc-options: -DNDEBUG + include-dirs: + Chipmunk2D/include/chipmunk + c-sources: + Chipmunk2D/src/chipmunk.c + Chipmunk2D/src/cpBody.c + Chipmunk2D/src/cpArbiter.c + Chipmunk2D/src/cpArray.c + Chipmunk2D/src/cpBBTree.c + Chipmunk2D/src/cpCollision.c + Chipmunk2D/src/cpConstraint.c + Chipmunk2D/src/cpDampedRotarySpring.c + Chipmunk2D/src/cpDampedSpring.c + Chipmunk2D/src/cpGearJoint.c + Chipmunk2D/src/cpGrooveJoint.c + Chipmunk2D/src/cpHashSet.c + Chipmunk2D/src/cpHastySpace.c + Chipmunk2D/src/cpMarch.c + Chipmunk2D/src/cpPinJoint.c + Chipmunk2D/src/cpPivotJoint.c + Chipmunk2D/src/cpPolyShape.c + Chipmunk2D/src/cpPolyline.c + Chipmunk2D/src/cpRatchetJoint.c + Chipmunk2D/src/cpRobust.c + Chipmunk2D/src/cpRotaryLimitJoint.c + Chipmunk2D/src/cpShape.c + Chipmunk2D/src/cpSimpleMotor.c + Chipmunk2D/src/cpSlideJoint.c + Chipmunk2D/src/cpSpace.c + Chipmunk2D/src/cpSpaceComponent.c + Chipmunk2D/src/cpSpaceDebug.c + Chipmunk2D/src/cpSpaceHash.c + Chipmunk2D/src/cpSpaceQuery.c + Chipmunk2D/src/cpSpaceStep.c + Chipmunk2D/src/cpSpatialIndex.c + Chipmunk2D/src/cpSweep1D.c diff --git a/src/Apecs/Physics.hs b/src/Apecs/Physics.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics.hs @@ -0,0 +1,51 @@ +-- | apecs-physics prelude + +module Apecs.Physics ( + + -- * General + Physics, + + -- * Space + Gravity (..), Iterations (..), + stepPhysics, + earthGravity, + + -- * Body + Body (..), Position (..), Velocity (..), Angle (..), AngularVelocity (..), Force (..), + BodyMass (..), Moment (..), CenterOfGravity (..), Torque (..), + + -- * Shape + Convex (..), Shape (Shape, ShapeExtend), + Mass (..), Density (..), Sensor (..), Friction (..), Elasticity (..), SurfaceVelocity (..), CollisionFilter (..), + maskAll, maskNone, maskList, defaultFilter, boxShape, + + -- * Constraint + Constraint (..), ConstraintType (..), MaxForce (..), MaxBias (..), ErrorBias (..), CollideBodies (..), + + -- * Collision + Collision (..), CollisionHandler (..), defaultHandler, + CollisionSource(..), BeginCB, SeparateCB, PreSolveCB, PostSolveCB, + mkBeginCB, mkSeparateCB, mkPreSolveCB, mkPostSolveCB, + + -- * Query + PointQueryResult (..), + pointQuery, + + -- * Geometry + module Apecs.Physics.Geometry, + + module Apecs, + module Linear.V2, + ) where + +import Apecs +import Linear.V2 + +import Apecs.Physics.Body () +import Apecs.Physics.Collision +import Apecs.Physics.Constraint () +import Apecs.Physics.Geometry +import Apecs.Physics.Query +import Apecs.Physics.Shape +import Apecs.Physics.Space +import Apecs.Physics.Types diff --git a/src/Apecs/Physics/Body.hs b/src/Apecs/Physics/Body.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Body.hs @@ -0,0 +1,407 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeApplications #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Body where + +import Apecs +import Apecs.Core +import Control.Monad +import qualified Data.IntMap as M +import qualified Data.IntSet as S +import Data.IORef +import qualified Data.Vector.Unboxed as U +import Foreign.ForeignPtr (withForeignPtr) +import Foreign.Ptr +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Space () +import Apecs.Physics.Shape () +import Apecs.Physics.Constraint () +import Apecs.Physics.Types + +C.context phycsCtx +C.include "" + +-- Body +newBody :: SpacePtr -> Int -> IO (Ptr Body) +newBody spacePtr (fromIntegral -> ety) = withForeignPtr spacePtr $ \space -> [C.block| cpBody* { + cpBody* body = cpBodyNew(0,0); + cpSpaceAddBody($(cpSpace* space), body); + cpBodySetUserData(body, (void*) $(intptr_t ety)); + return body; } |] + +setBodyType :: Ptr Body -> Body -> IO () +setBodyType bodyPtr (fromIntegral . fromEnum -> bodyInt) = + [C.exp| void { cpBodySetType($(cpBody* bodyPtr), $(int bodyInt)) } |] + +getBodyType :: Ptr Body -> IO Body +getBodyType bodyPtr = toEnum . fromIntegral <$> [C.exp| int { cpBodyGetType($(cpBody* bodyPtr)) } |] + +destroyBody :: SpacePtr -> Ptr Body -> IO () +destroyBody spacePtr bodyPtr = withForeignPtr spacePtr $ \space -> [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpSpaceRemoveBody($(cpSpace* space), body); + cpBodyFree(body); }|] + +fromBodyPtr :: Ptr Body -> BodyRecord +fromBodyPtr ptr = BodyRecord ptr mempty mempty + +instance Component Body where + type Storage Body = Space Body + +instance Has w IO Physics => Has w IO Body where + getStore = (cast :: Space Physics -> Space Body) <$> getStore + +instance ExplSet IO (Space Body) where + explSet (Space bMap _ _ _ spcPtr) ety btype = do + rd <- M.lookup ety <$> readIORef bMap + bdyPtr <- case rd of + Just (BodyRecord bdyPtr _ _) -> return bdyPtr + Nothing -> do + bdyPtr <- newBody spcPtr ety + modifyIORef' bMap (M.insert ety $ fromBodyPtr bdyPtr) + return bdyPtr + setBodyType bdyPtr btype + +instance ExplDestroy IO (Space Body) where + explDestroy sp@(Space bMap _ _ _ spc) ety = do + rd <- M.lookup ety <$> readIORef bMap + modifyIORef' bMap (M.delete ety) + forM_ rd $ \(BodyRecord bPtr shapes constraints) -> do + forM_ (S.toList shapes) $ \s -> explDestroy (cast sp :: Space Shape) s + forM_ (S.toList constraints) $ \s -> explDestroy (cast sp :: Space Constraint) s + destroyBody spc bPtr + +instance ExplMembers IO (Space Body) where + explMembers (Space bMap _ _ _ _) = U.fromList . M.keys <$> readIORef bMap + +instance ExplGet IO (Space Body) where + explExists (Space bMap _ _ _ _) ety = M.member ety <$> readIORef bMap + + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + getBodyType b + +-- Position +getPosition :: Ptr Body -> IO (V2 Double) +getPosition bodyPtr = do + x <- [C.exp| double { cpBodyGetPosition ($(cpBody* bodyPtr)).x } |] + y <- [C.exp| double { cpBodyGetPosition ($(cpBody* bodyPtr)).y } |] + return (V2 (realToFrac x) (realToFrac y)) + +setPosition :: Ptr Body -> V2 Double -> IO () +setPosition bodyPtr (V2 (realToFrac -> x) (realToFrac -> y)) = [C.block| void { + const cpVect pos = { $(double x), $(double y) }; + cpBody *body = $(cpBody* bodyPtr); + cpBodySetPosition(body, pos); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + +instance Component Position where + type Storage Position = Space Position + +instance Has w IO Physics => Has w IO Position where + getStore = (cast :: Space Physics -> Space Position) <$> getStore + +instance ExplMembers IO (Space Position) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Position) where + explSet (Space bMap _ _ _ _) ety (Position pos) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd$ \(BodyRecord b _ _) -> setPosition b pos + +instance ExplGet IO (Space Position) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Position <$> getPosition b + +-- Velocity +getVelocity :: Ptr Body -> IO (V2 Double) +getVelocity bodyPtr = do + x <- [C.exp| double { cpBodyGetVelocity ($(cpBody* bodyPtr)).x } |] + y <- [C.exp| double { cpBodyGetVelocity ($(cpBody* bodyPtr)).y } |] + return (V2 (realToFrac x) (realToFrac y)) + +setVelocity :: Ptr Body -> V2 Double -> IO () +setVelocity bodyPtr (V2 (realToFrac -> x) (realToFrac -> y)) = [C.block| void { + const cpVect vel = { $(double x), $(double y) }; + cpBodySetVelocity($(cpBody* bodyPtr), vel); + } |] + +instance Component Velocity where + type Storage Velocity = Space Velocity + +instance Has w IO Physics => Has w IO Velocity where + getStore = (cast :: Space Physics -> Space Velocity) <$> getStore + +instance ExplMembers IO (Space Velocity) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Velocity) where + explSet (Space bMap _ _ _ _) ety (Velocity vel) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd$ \(BodyRecord b _ _) -> setVelocity b vel + +instance ExplGet IO (Space Velocity) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Velocity <$> getVelocity b + +-- Angle +getAngle :: Ptr Body -> IO Double +getAngle bodyPtr = do + angle <- [C.exp| double { cpBodyGetAngle ($(cpBody* bodyPtr)) } |] + return (realToFrac angle) + +setAngle :: Ptr Body -> Double -> IO () +setAngle bodyPtr (realToFrac -> angle) = [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpBodySetAngle(body, $(double angle)); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + -- FIXME reindex + +instance Component Angle where + type Storage Angle = Space Angle + +instance Has w IO Physics => Has w IO Angle where + getStore = (cast :: Space Physics -> Space Angle) <$> getStore + +instance ExplMembers IO (Space Angle) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Angle) where + explSet (Space bMap _ _ _ _) ety (Angle angle) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd $ \(BodyRecord b _ _) -> setAngle b angle + +instance ExplGet IO (Space Angle) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Angle <$> getAngle b + +-- AngularVelocity +getAngularVelocity :: Ptr Body -> IO Double +getAngularVelocity bodyPtr = do + angle <- [C.exp| double { cpBodyGetAngularVelocity ($(cpBody* bodyPtr)) } |] + return (realToFrac angle) + +setAngularVelocity :: Ptr Body -> Double -> IO () +setAngularVelocity bodyPtr (realToFrac -> angle) = [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpBodySetAngularVelocity(body, $(double angle)); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + -- FIXME reindex + +instance Component AngularVelocity where + type Storage AngularVelocity = Space AngularVelocity + +instance Has w IO Physics => Has w IO AngularVelocity where + getStore = (cast :: Space Physics -> Space AngularVelocity) <$> getStore + +instance ExplMembers IO (Space AngularVelocity) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space AngularVelocity) where + explSet (Space bMap _ _ _ _) ety (AngularVelocity angle) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd $ \(BodyRecord b _ _) -> setAngularVelocity b angle + +instance ExplGet IO (Space AngularVelocity) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + AngularVelocity <$> getAngularVelocity b + +-- Force +getForce :: Ptr Body -> IO (V2 Double) +getForce bodyPtr = do + x <- [C.exp| double { cpBodyGetForce ($(cpBody* bodyPtr)).x } |] + y <- [C.exp| double { cpBodyGetForce ($(cpBody* bodyPtr)).y } |] + return (V2 (realToFrac x) (realToFrac y)) + +setForce :: Ptr Body -> V2 Double -> IO () +setForce bodyPtr (V2 (realToFrac -> x) (realToFrac -> y)) = [C.block| void { + const cpVect frc = { $(double x), $(double y) }; + cpBodySetForce($(cpBody* bodyPtr), frc); + } |] + +instance Component Force where + type Storage Force = Space Force + +instance Has w IO Physics => Has w IO Force where + getStore = (cast :: Space Physics -> Space Force) <$> getStore + +instance ExplMembers IO (Space Force) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Force) where + explSet (Space bMap _ _ _ _) ety (Force frc) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd$ \(BodyRecord b _ _) -> setForce b frc + +instance ExplGet IO (Space Force) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Force <$> getForce b + +-- BodyMass +getBodyMass :: Ptr Body -> IO Double +getBodyMass bodyPtr = do + angle <- [C.exp| double { cpBodyGetMass ($(cpBody* bodyPtr)) } |] + return (realToFrac angle) + +setBodyMass :: Ptr Body -> Double -> IO () +setBodyMass bodyPtr (realToFrac -> angle) = [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpBodySetMass(body, $(double angle)); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + -- FIXME reindex + +instance Component BodyMass where + type Storage BodyMass = Space BodyMass + +instance Has w IO Physics => Has w IO BodyMass where + getStore = (cast :: Space Physics -> Space BodyMass) <$> getStore + +instance ExplMembers IO (Space BodyMass) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space BodyMass) where + explSet (Space bMap _ _ _ _) ety (BodyMass angle) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd $ \(BodyRecord b _ _) -> setBodyMass b angle + +instance ExplGet IO (Space BodyMass) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + BodyMass <$> getBodyMass b + +-- Moment +getMoment :: Ptr Body -> IO Double +getMoment bodyPtr = do + angle <- [C.exp| double { cpBodyGetMoment ($(cpBody* bodyPtr)) } |] + return (realToFrac angle) + +setMoment :: Ptr Body -> Double -> IO () +setMoment bodyPtr (realToFrac -> angle) = [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpBodySetMoment(body, $(double angle)); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + -- FIXME reindex + +instance Component Moment where + type Storage Moment = Space Moment + +instance Has w IO Physics => Has w IO Moment where + getStore = (cast :: Space Physics -> Space Moment) <$> getStore + +instance ExplMembers IO (Space Moment) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Moment) where + explSet (Space bMap _ _ _ _) ety (Moment angle) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd $ \(BodyRecord b _ _) -> setMoment b angle + +instance ExplGet IO (Space Moment) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Moment <$> getMoment b + +-- Torque +getTorque :: Ptr Body -> IO Double +getTorque bodyPtr = do + angle <- [C.exp| double { cpBodyGetTorque ($(cpBody* bodyPtr)) } |] + return (realToFrac angle) + +setTorque :: Ptr Body -> Double -> IO () +setTorque bodyPtr (realToFrac -> angle) = [C.block| void { + cpBody *body = $(cpBody* bodyPtr); + cpBodySetTorque(body, $(double angle)); + if (cpBodyGetType(body) == CP_BODY_TYPE_STATIC) + cpSpaceReindexShapesForBody(cpBodyGetSpace(body), body); + } |] + -- FIXME reindex + +instance Component Torque where + type Storage Torque = Space Torque + +instance Has w IO Physics => Has w IO Torque where + getStore = (cast :: Space Physics -> Space Torque) <$> getStore + +instance ExplMembers IO (Space Torque) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space Torque) where + explSet (Space bMap _ _ _ _) ety (Torque angle) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd $ \(BodyRecord b _ _) -> setTorque b angle + +instance ExplGet IO (Space Torque) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + Torque <$> getTorque b + +-- CenterOfGravity +getCenterOfGravity :: Ptr Body -> IO (V2 Double) +getCenterOfGravity bodyPtr = do + x <- [C.exp| double { cpBodyGetCenterOfGravity ($(cpBody* bodyPtr)).x } |] + y <- [C.exp| double { cpBodyGetCenterOfGravity ($(cpBody* bodyPtr)).y } |] + return (V2 (realToFrac x) (realToFrac y)) + +setCenterOfGravity :: Ptr Body -> V2 Double -> IO () +setCenterOfGravity bodyPtr (V2 (realToFrac -> x) (realToFrac -> y)) = [C.block| void { + const cpVect vel = { $(double x), $(double y) }; + cpBodySetCenterOfGravity($(cpBody* bodyPtr), vel); + } |] + +instance Component CenterOfGravity where + type Storage CenterOfGravity = Space CenterOfGravity + +instance Has w IO Physics => Has w IO CenterOfGravity where + getStore = (cast :: Space Physics -> Space CenterOfGravity) <$> getStore + +instance ExplMembers IO (Space CenterOfGravity) where + explMembers s = explMembers (cast s :: Space Body) + +instance ExplSet IO (Space CenterOfGravity) where + explSet (Space bMap _ _ _ _) ety (CenterOfGravity vel) = do + rd <- M.lookup ety <$> readIORef bMap + forM_ rd$ \(BodyRecord b _ _) -> setCenterOfGravity b vel + +instance ExplGet IO (Space CenterOfGravity) where + explExists s ety = explExists (cast s :: Space Body) ety + explGet (Space bMap _ _ _ _) ety = do + Just (BodyRecord b _ _) <- M.lookup ety <$> readIORef bMap + CenterOfGravity <$> getCenterOfGravity b + diff --git a/src/Apecs/Physics/Collision.hs b/src/Apecs/Physics/Collision.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Collision.hs @@ -0,0 +1,131 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeApplications #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Collision where + +import Apecs +import Apecs.Core +import Control.Monad +import qualified Data.IntMap as M +import Data.IORef +import qualified Data.Vector.Unboxed as U +import Foreign.ForeignPtr (withForeignPtr) +import Foreign.Ptr +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Body () +import Apecs.Physics.Space () +import Apecs.Physics.Types + +C.context (phycsCtx `mappend` C.funCtx) +C.include "" +C.include "" + +defaultHandler :: CollisionHandler +defaultHandler = CollisionHandler (Wildcard 0) Nothing Nothing Nothing Nothing + +mkBeginCB :: (Collision -> System w Bool) -> System w BeginCB +mkBeginCB sys = do + w <- ask + + let cb arb _ _ = do + nx <- realToFrac <$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).x } |] + ny <- realToFrac <$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).y } |] + ea <- fromIntegral <$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (ba->userData); } |] + eb <- fromIntegral <$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (bb->userData); } |] + r <- liftIO$ runSystem (sys (Collision (V2 nx ny) (Entity ea) (Entity eb))) w + return . fromIntegral . fromEnum $ r + + return (BeginCB cb) + +mkSeparateCB :: (Collision -> System w ()) -> System w SeparateCB +mkSeparateCB sys = do + w <- ask + + let cb arb _ _ = do + nx <- realToFrac <$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).x } |] + ny <- realToFrac <$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).y } |] + ea <- fromIntegral <$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (ba->userData); } |] + eb <- fromIntegral <$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (bb->userData); } |] + liftIO$ runSystem (sys (Collision (V2 nx ny) (Entity ea) (Entity eb))) w + + return (SeparateCB cb) + + +mkPreSolveCB :: (Collision -> System w Bool) -> System w PreSolveCB +mkPreSolveCB sys = (\(BeginCB cb) -> PreSolveCB cb) <$> mkBeginCB sys + +mkPostSolveCB :: (Collision -> System w ()) -> System w PostSolveCB +mkPostSolveCB sys = (\(SeparateCB cb) -> PostSolveCB cb) <$> mkSeparateCB sys + +newCollisionHandler :: SpacePtr -> CollisionHandler -> Int -> IO (Ptr CollisionHandler) +newCollisionHandler spcPtr (CollisionHandler source begin separate presolve postsolve) (fromIntegral -> ety) = + withForeignPtr spcPtr $ \space -> do + handler <- case source of + Between cta ctb -> [C.exp| cpCollisionHandler* {cpSpaceAddCollisionHandler($(cpSpace* space), $(unsigned int cta), $(unsigned int ctb))}|] + Wildcard ct -> [C.exp| cpCollisionHandler* {cpSpaceAddWildcardHandler($(cpSpace* space), $(unsigned int ct))}|] + + [C.exp| void { $(cpCollisionHandler* handler)->userData = (void*) $(intptr_t ety) }|] + + forM_ begin$ \(BeginCB cb) -> do + funPtr <- liftIO$ $(C.mkFunPtr [t| BeginFunc |]) cb + let fn = castFunPtrToPtr funPtr + [C.exp| void { $(cpCollisionHandler* handler)->beginFunc = $(void* fn) }|] + + forM_ separate$ \(SeparateCB cb) -> do + funPtr <- liftIO$ $(C.mkFunPtr [t| SeparateFunc |]) cb + let fn = castFunPtrToPtr funPtr + [C.exp| void { $(cpCollisionHandler* handler)->separateFunc = $(void* fn) }|] + + forM_ presolve$ \(PreSolveCB cb) -> do + funPtr <- liftIO$ $(C.mkFunPtr [t| PreSolveFunc |]) cb + let fn = castFunPtrToPtr funPtr + [C.exp| void { $(cpCollisionHandler* handler)->preSolveFunc = $(void* fn) }|] + + forM_ postsolve$ \(PostSolveCB cb) -> do + funPtr <- liftIO$ $(C.mkFunPtr [t| PostSolveFunc |]) cb + let fn = castFunPtrToPtr funPtr + [C.exp| void { $(cpCollisionHandler* handler)->postSolveFunc = $(void* fn) }|] + + return handler + +destroyCollisionHandler :: Ptr CollisionHandler -> IO () +destroyCollisionHandler = error "Destroy CollisionHandler not yet implemented" + +instance Component CollisionHandler where + type Storage CollisionHandler = Space CollisionHandler + +instance Has w IO Physics => Has w IO CollisionHandler where + getStore = (cast :: Space Physics -> Space CollisionHandler) <$> getStore + +instance ExplSet IO (Space CollisionHandler) where + explSet sp@(Space _ _ _ hMap spcPtr) ety handler = do + explDestroy sp ety + hPtr <- newCollisionHandler spcPtr handler ety + modifyIORef' hMap (M.insert ety hPtr) + +instance ExplDestroy IO (Space CollisionHandler) where + explDestroy (Space _ _ _ hMap _) ety = do + rd <- M.lookup ety <$> readIORef hMap + forM_ rd$ \c -> destroyCollisionHandler c >> modifyIORef' hMap (M.delete ety) + +instance ExplMembers IO (Space CollisionHandler) where + explMembers (Space _ _ _ hMap _) = U.fromList . M.keys <$> readIORef hMap + +instance ExplGet IO (Space CollisionHandler) where + explExists (Space _ _ _ hMap _) ety = M.member ety <$> readIORef hMap + explGet _ _ = return (error "CollisionHandler is a write-only component") + diff --git a/src/Apecs/Physics/Constraint.hs b/src/Apecs/Physics/Constraint.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Constraint.hs @@ -0,0 +1,327 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeApplications #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Constraint where + +import Apecs +import Apecs.Core +import Control.Monad +import qualified Data.IntMap as M +import qualified Data.IntSet as S +import Data.IORef +import qualified Data.Vector.Unboxed as U +import Foreign.ForeignPtr (withForeignPtr) +import Foreign.Ptr +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Space () +import Apecs.Physics.Types + +C.context phycsCtx +C.include "" + +-- Constraint +newConstraint :: SpacePtr -> Ptr Body -> Ptr Body -> Int -> ConstraintType -> IO (Ptr Constraint) +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (PinJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect anchorA = cpv( $(double ax), $(double ay) ); + cpVect anchorB = cpv( $(double bx), $(double by) ); + cpConstraint* constraint = cpPinJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (SlideJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (realToFrac -> min) (realToFrac -> max)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect anchorA = cpv( $(double ax), $(double ay) ); + cpVect anchorB = cpv( $(double bx), $(double by) ); + cpConstraint* constraint = cpSlideJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB,$(double min),$(double max)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (PivotJoint (fmap realToFrac -> V2 x y)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect anchor = cpv( $(double x), $(double y) ); + cpConstraint* constraint = cpPivotJointNew($(cpBody* bodyA), $(cpBody* bodyB), anchor); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (PivotJoint2 (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect va = cpv( $(double ax), $(double ay) ); + cpVect vb = cpv( $(double bx), $(double by) ); + cpConstraint* constraint = cpPivotJointNew2($(cpBody* bodyA), $(cpBody* bodyB), va, vb); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (GrooveJoint (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (fmap realToFrac -> V2 ancx ancy)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect va = cpv( $(double ax), $(double ay) ); + cpVect vb = cpv( $(double bx), $(double by) ); + cpVect anchor = cpv( $(double ancx), $(double ancy) ); + cpConstraint* constraint = cpGrooveJointNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, anchor); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (DampedSpring (fmap realToFrac -> V2 ax ay) (fmap realToFrac -> V2 bx by) (realToFrac -> rl) (realToFrac -> stf) (realToFrac -> damping)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpVect va = cpv( $(double ax), $(double ay) ); + cpVect vb = cpv( $(double bx), $(double by) ); + cpConstraint* constraint = cpDampedSpringNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, $(double rl), $(double stf), $(double damping)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (DampedRotarySpring (realToFrac -> ra) (realToFrac -> stf) (realToFrac -> damping)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpConstraint* constraint = cpDampedRotarySpringNew($(cpBody* bodyA), $(cpBody* bodyB), $(double ra), $(double stf), $(double damping)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (RotaryLimitJoint (realToFrac -> min) (realToFrac -> max)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpConstraint* constraint = cpRotaryLimitJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double min), $(double max)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (RatchetJoint (realToFrac -> phase) (realToFrac -> ratchet)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpConstraint* constraint = cpRatchetJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratchet)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (GearJoint (realToFrac -> phase) (realToFrac -> ratio)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpConstraint* constraint = cpGearJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratio)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +newConstraint spacePtr bodyA bodyB (fromIntegral -> ety) + (SimpleMotor (realToFrac -> rate)) = + withForeignPtr spacePtr $ \space -> [C.block| cpConstraint* { + cpConstraint* constraint = cpSimpleMotorNew($(cpBody* bodyA), $(cpBody* bodyB), $(double rate)); + cpConstraintSetUserData(constraint, (void*) $(intptr_t ety)); + return cpSpaceAddConstraint($(cpSpace* space), constraint); + } |] + +destroyConstraint :: SpacePtr -> Ptr Constraint -> IO () +destroyConstraint spacePtr constraintPtr = withForeignPtr spacePtr $ \space -> [C.block| void { + cpConstraint *constraint = $(cpConstraint* constraintPtr); + cpSpaceRemoveConstraint($(cpSpace* space), constraint); + cpConstraintFree(constraint); }|] + +instance Component Constraint where + type Storage Constraint = Space Constraint + +instance Has w IO Physics => Has w IO Constraint where + getStore = (cast :: Space Physics -> Space Constraint) <$> getStore + +instance ExplSet IO (Space Constraint) where + + explSet _ _ ConstraintRead = return () + explSet s ety (Constraint b ctype) = explSet s ety (ConstraintExtend (Entity ety) b ctype) + explSet sp@(Space bMap _ cMap _ spcPtr) cEty (ConstraintExtend (Entity bEtyA) (Entity bEtyB) ctype) = do + explDestroy sp cEty + mBrA <- M.lookup bEtyA <$> readIORef bMap + mBrB <- M.lookup bEtyB <$> readIORef bMap + case (mBrA,mBrB) of + (Just brA, Just brB) -> do + cPtr <- newConstraint spcPtr (brPtr brA) (brPtr brB) cEty ctype + + let brConstraintsA' = S.insert cEty (brConstraints brA) + brConstraintsB' = S.insert cEty (brConstraints brB) + + modifyIORef' cMap (M.insert cEty cPtr) + modifyIORef' bMap ( M.insert bEtyA (brA {brConstraints = brConstraintsA'}) + . M.insert bEtyB (brB {brConstraints = brConstraintsB'}) ) + _ -> return () + +instance ExplDestroy IO (Space Constraint) where + explDestroy (Space bMap _ cMap _ spc) cEty = do + rd <- M.lookup cEty <$> readIORef cMap + forM_ rd $ \cPtr -> do + bEtyA <- getBodyA cPtr + bEtyB <- getBodyB cPtr + bMapRd <- readIORef bMap + + let Just bRecA = M.lookup bEtyA bMapRd + Just bRecB = M.lookup bEtyB bMapRd + brConstraintsA' = S.delete cEty (brConstraints bRecA) + brConstraintsB' = S.delete cEty (brConstraints bRecB) + + modifyIORef' cMap (M.delete cEty) + modifyIORef' bMap ( M.insert bEtyA (bRecA {brConstraints = brConstraintsA'}) + . M.insert bEtyB (bRecB {brConstraints = brConstraintsB'}) ) + destroyConstraint spc cPtr + +instance ExplMembers IO (Space Constraint) where + explMembers (Space _ _ cMap _ _) = U.fromList . M.keys <$> readIORef cMap + +instance ExplGet IO (Space Constraint) where + explExists (Space _ _ cMap _ _) ety = M.member ety <$> readIORef cMap + + explGet _ _ = return ConstraintRead + +-- BodyAB +getBodyA :: Ptr Constraint -> IO Int +getBodyA c = fromIntegral <$> [C.exp| intptr_t { + (intptr_t) cpBodyGetUserData(cpConstraintGetBodyA($(cpConstraint* c))) }|] + +getBodyB :: Ptr Constraint -> IO Int +getBodyB c = fromIntegral <$> [C.exp| intptr_t { + (intptr_t) cpBodyGetUserData(cpConstraintGetBodyB($(cpConstraint* c))) }|] + +-- MaxForce +getMaxForce :: Ptr Constraint -> IO Double +getMaxForce c = do + maxForce <- [C.exp| double { cpConstraintGetMaxForce ($(cpConstraint* c)) } |] + return (realToFrac maxForce) + +setMaxForce :: Ptr Constraint -> Double -> IO () +setMaxForce c (realToFrac -> maxForce) = [C.exp| void { cpConstraintSetMaxForce($(cpConstraint* c), $(double maxForce)); } |] + +instance Component MaxForce where + type Storage MaxForce = Space MaxForce + +instance Has w IO Physics => Has w IO MaxForce where + getStore = (cast :: Space Physics -> Space MaxForce) <$> getStore + +instance ExplMembers IO (Space MaxForce) where + explMembers s = explMembers (cast s :: Space Constraint) + +instance ExplSet IO (Space MaxForce) where + explSet (Space _ _ cMap _ _) ety (MaxForce vec) = do + rd <- M.lookup ety <$> readIORef cMap + case rd of + Nothing -> return () + Just c -> setMaxForce c vec + +instance ExplGet IO (Space MaxForce) where + explExists s ety = explExists (cast s :: Space Constraint) ety + explGet (Space _ _ cMap _ _) ety = do + Just c <- M.lookup ety <$> readIORef cMap + MaxForce <$> getMaxForce c + +-- MaxBias +getMaxBias :: Ptr Constraint -> IO Double +getMaxBias c = do + maxBias <- [C.exp| double { cpConstraintGetMaxBias ($(cpConstraint* c)) } |] + return (realToFrac maxBias) + +setMaxBias :: Ptr Constraint -> Double -> IO () +setMaxBias c (realToFrac -> maxBias) = [C.exp| void { cpConstraintSetMaxBias($(cpConstraint* c), $(double maxBias)); } |] + +instance Component MaxBias where + type Storage MaxBias = Space MaxBias + +instance Has w IO Physics => Has w IO MaxBias where + getStore = (cast :: Space Physics -> Space MaxBias) <$> getStore + +instance ExplMembers IO (Space MaxBias) where + explMembers s = explMembers (cast s :: Space Constraint) + +instance ExplSet IO (Space MaxBias) where + explSet (Space _ _ cMap _ _) ety (MaxBias vec) = do + rd <- M.lookup ety <$> readIORef cMap + case rd of + Nothing -> return () + Just c -> setMaxBias c vec + +instance ExplGet IO (Space MaxBias) where + explGet (Space _ _ cMap _ _) ety = do + Just c <- M.lookup ety <$> readIORef cMap + MaxBias <$> getMaxBias c + explExists s ety = explExists (cast s :: Space Constraint) ety + +-- ErrorBias +getErrorBias :: Ptr Constraint -> IO Double +getErrorBias c = do + errorBias <- [C.exp| double { cpConstraintGetErrorBias ($(cpConstraint* c)) } |] + return (realToFrac errorBias) + +setErrorBias :: Ptr Constraint -> Double -> IO () +setErrorBias c (realToFrac -> errorBias) = [C.exp| void { cpConstraintSetErrorBias($(cpConstraint* c), $(double errorBias)); } |] + +instance Component ErrorBias where + type Storage ErrorBias = Space ErrorBias + +instance Has w IO Physics => Has w IO ErrorBias where + getStore = (cast :: Space Physics -> Space ErrorBias) <$> getStore + +instance ExplMembers IO (Space ErrorBias) where + explMembers s = explMembers (cast s :: Space Constraint) + +instance ExplSet IO (Space ErrorBias) where + explSet (Space _ _ cMap _ _) ety (ErrorBias vec) = do + rd <- M.lookup ety <$> readIORef cMap + case rd of + Nothing -> return () + Just c -> setErrorBias c vec + +instance ExplGet IO (Space ErrorBias) where + explExists s ety = explExists (cast s :: Space Constraint) ety + explGet (Space _ _ cMap _ _) ety = do + Just c <- M.lookup ety <$> readIORef cMap + ErrorBias <$> getErrorBias c + +-- CollideBodies +getCollideBodies :: Ptr Constraint -> IO Bool +getCollideBodies c = do + collide <- [C.exp| int { cpConstraintGetCollideBodies ($(cpConstraint* c)) } |] + return . toEnum . fromIntegral $ collide + +setCollideBodies :: Ptr Constraint -> Bool -> IO () +setCollideBodies c (fromIntegral . fromEnum -> collide) = [C.exp| void { cpConstraintSetCollideBodies($(cpConstraint* c), $(int collide)); } |] + +instance Component CollideBodies where + type Storage CollideBodies = Space CollideBodies + +instance Has w IO Physics => Has w IO CollideBodies where + getStore = (cast :: Space Physics -> Space CollideBodies) <$> getStore + +instance ExplMembers IO (Space CollideBodies) where + explMembers s = explMembers (cast s :: Space Constraint) + +instance ExplSet IO (Space CollideBodies) where + explSet (Space _ _ cMap _ _) ety (CollideBodies vec) = do + rd <- M.lookup ety <$> readIORef cMap + case rd of + Nothing -> return () + Just c -> setCollideBodies c vec + +instance ExplGet IO (Space CollideBodies) where + explExists s ety = explExists (cast s :: Space Constraint) ety + explGet (Space _ _ cMap _ _) ety = do + Just c <- M.lookup ety <$> readIORef cMap + CollideBodies <$> getCollideBodies c diff --git a/src/Apecs/Physics/Geometry.hs b/src/Apecs/Physics/Geometry.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Geometry.hs @@ -0,0 +1,64 @@ +module Apecs.Physics.Geometry where + +import Apecs.Physics.Types +import Linear + +vertices :: Convex -> [BVec] +vertices (Convex s _) = s + +-- | Map a function over all vertices +mapVertices :: (BVec -> BVec) -> Convex -> Convex +mapVertices f (Convex s r) = Convex (f <$> s) r + +-- | Translates all vertices. The name shift is to prevent collisions with gloss +shift :: BVec -> Convex -> Convex +shift = mapVertices . (+) + +getRadius :: Convex -> Double +getRadius (Convex _ r) = r + +setRadius :: Double -> Convex -> Convex +setRadius r (Convex s _) = Convex s r + +cCircle, zCircle :: Double -> Convex +cCircle r = oCircle 0 r +zCircle = cCircle + +oCircle :: BVec -> Double -> Convex +oCircle o r = Convex [o] r + +hLine, vLine :: Double -> Convex +hLine l = Convex [V2 (-l/2) 0, V2 (l/2) 0] 0 +vLine l = Convex [V2 0 (l/2), V2 0 (-l/2)] 0 + +-- | Centered rectangle with a given size +cRectangle :: BVec -> Convex +cRectangle s = oRectangle (-s*0.5) s + +-- | Rectangle with a given origin and size +oRectangle :: BVec -> BVec -> Convex +oRectangle (V2 x y) (V2 w h) = Convex [V2 x y, V2 x (y+h), V2 (x+w) (y+h), V2 (x+w) y] 0 + +-- | Rectangle with origin 0 and given size +zRectangle :: BVec -> Convex +zRectangle s = oRectangle 0 s + +-- | Split a shape into its edges. Will return no edges for points, but returns 2 for a line (in opposite directions) +toEdges :: Convex -> [Convex] +toEdges (Convex [] _) = [] +toEdges (Convex [_] _) = [] +toEdges (Convex vs r) = zipWith (\h t -> Convex [h,t] r) vs (tail . cycle $ vs) + +-- | A set of lines forming a grid. Returns (r + c + 2) segments. +gridLines :: Vec -> Int -> Int -> [Convex] +gridLines size c r = + [ shift (V2 x 0) (vLine h) | x <- xs ] ++ + [ shift (V2 0 y) (hLine w) | y <- ys ] + where + V2 w h = size + V2 x y = -size*0.5 + dx = w/fromIntegral c + dy = h/fromIntegral r + xs = [x + fromIntegral n * dx | n <- [0..c]] + ys = [y + fromIntegral n * dy | n <- [0..r]] + diff --git a/src/Apecs/Physics/Query.hs b/src/Apecs/Physics/Query.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Query.hs @@ -0,0 +1,73 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeApplications #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Query where + +import Apecs +import Foreign.C.Types +import Foreign.ForeignPtr (withForeignPtr) +import Foreign.Marshal.Alloc +import Foreign.Ptr +import Foreign.Storable +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Space () +import Apecs.Physics.Types + +C.context phycsCtx +C.include "" + + +-- cpFloat cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpPointQueryInfo *out) +-- cpShape *cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out) + +pointQuery :: Has w IO Physics => WVec -> Double -> CollisionFilter -> System w (Maybe PointQueryResult) +pointQuery (fmap realToFrac -> V2 px py) (realToFrac -> maxDistance) (CollisionFilter gr (Bitmask cs) (Bitmask mk)) = do + Space _ _ _ _ spcPtr :: Space Physics <- getStore + liftIO$ do + pq <- malloc + withForeignPtr spcPtr $ \space -> [C.block| void { + cpPointQueryInfo *pq = $(cpPointQueryInfo *pq); + cpSpacePointQueryNearest + ( $(cpSpace *space) + , cpv($(double px), $(double py)) + , $(double maxDistance) + , cpShapeFilterNew($(unsigned int gr), $(unsigned int cs), $(unsigned int mk)) + , pq); + }|] + res <- peek pq + free pq + if unEntity (pqShape res) == -1 + then return Nothing + else return (Just res) + +instance Storable PointQueryResult where + sizeOf ~_ = 40 -- sizeOf (undefined :: Ptr Shape) + 2*sizeOf (undefined :: CDouble) + sizeOf (undefined :: V2 CDouble) + alignment ~_ = 8 + peek ptr = do + sPtr :: Ptr Shape <- peekByteOff ptr 0 + s <- [C.block| intptr_t { + cpShape *shape = $(cpShape *sPtr); + if (shape==NULL) { + return -1; + } else { + return (intptr_t) cpShapeGetUserData(shape); + } }|] + p :: V2 CDouble <- peekByteOff ptr 8 + d :: CDouble <- peekByteOff ptr 24 + g :: CDouble <- peekByteOff ptr 32 + return $ PointQueryResult (Entity . fromIntegral $ s) (realToFrac <$> p) (realToFrac d) (realToFrac g) + poke = undefined diff --git a/src/Apecs/Physics/Shape.hs b/src/Apecs/Physics/Shape.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Shape.hs @@ -0,0 +1,382 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Shape where + +import Apecs.Core +import Control.Monad +import Data.Bits +import qualified Data.IntMap as M +import qualified Data.IntSet as S +import Data.IORef +import Data.Monoid ((<>)) +import qualified Data.Vector.Storable as V +import qualified Data.Vector.Unboxed as U +import Foreign.ForeignPtr +import Foreign.Ptr +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Space () +import Apecs.Physics.Types + +C.context (phycsCtx <> C.vecCtx) +C.include "" + +maskAll, maskNone :: Bitmask +maskAll = complement zeroBits +maskNone = zeroBits +maskList :: [Int] -> Bitmask +maskList = foldr (flip setBit) maskNone + +defaultFilter :: CollisionFilter +defaultFilter = CollisionFilter 0 maskAll maskAll + +boxShape :: Double -> Double -> Vec -> Convex +boxShape w h offset = Convex ((+offset) <$> verts) 0 + where + w' = w/2 + h' = h/2 + verts = [ V2 (-w') (-h') + , V2 (-w') h' + , V2 w' h' + , V2 w' (-h') ] + +instance Component Shape where + type Storage Shape = Space Shape + +instance Has w IO Physics => Has w IO Shape where + getStore = (cast :: Space Physics -> Space Shape) <$> getStore + +instance ExplMembers IO (Space Shape) where + explMembers (Space _ sMap _ _ _) = U.fromList . M.keys <$> readIORef sMap + +instance ExplDestroy IO (Space Shape) where + explDestroy (Space bMap sMap _ _ spc) sEty = do + rd <- M.lookup sEty <$> readIORef sMap + forM_ rd $ \sPtr -> do + bEty <- fromIntegral <$> getShapeBody sPtr + + Just bRec <- M.lookup bEty <$> readIORef bMap + let brShapes' = S.delete sEty (brShapes bRec) + + modifyIORef' sMap (M.delete sEty) + modifyIORef' bMap (M.insert bEty (bRec {brShapes = brShapes'})) + destroyShape spc sPtr + +instance ExplSet IO (Space Shape) where + explSet _ _ ShapeRead = return () + explSet sp ety (Shape sh) = explSet sp ety (ShapeExtend (Entity ety) sh) + + explSet sp@(Space bMap sMap _ _ spcPtr) sEty (ShapeExtend (Entity bEty) sh) = do + explDestroy sp sEty + rd <- M.lookup bEty <$> readIORef bMap + forM_ rd $ \bRec -> do + s <- newShape spcPtr (brPtr bRec) sh sEty + let brShapes' = S.insert sEty (brShapes bRec) + modifyIORef' bMap (M.insert bEty (bRec {brShapes = brShapes'})) + modifyIORef' sMap (M.insert sEty s) + +instance ExplGet IO (Space Shape) where + explGet _ _ = return ShapeRead + explExists (Space _ sMap _ _ _) ety = M.member ety <$> readIORef sMap + +newShape :: SpacePtr -> Ptr Body -> Convex -> Int -> IO (Ptr Shape) +newShape spacePtr' bodyPtr shape (fromIntegral -> ety) = withForeignPtr spacePtr' (go shape) + where + + go (Convex [fmap realToFrac -> V2 x y] (realToFrac -> radius)) spacePtr = [C.block| cpShape* { + const cpVect vec = { $(double x), $(double y) }; + cpShape* sh = cpCircleShapeNew($(cpBody* bodyPtr), $(double radius), vec); + cpShapeSetUserData(sh, (void*) $(intptr_t ety)); + return cpSpaceAddShape( $(cpSpace* spacePtr), sh); } |] + + go (Convex [ fmap realToFrac -> V2 xa ya + , fmap realToFrac -> V2 xb yb ] + (realToFrac -> radius) + ) spacePtr = [C.block| cpShape* { + const cpVect va = { $(double xa), $(double ya) }; + const cpVect vb = { $(double xb), $(double yb) }; + cpShape* sh = cpSegmentShapeNew($(cpBody* bodyPtr), va, vb, $(double radius)); + cpShapeSetUserData(sh, (void*) $(intptr_t ety)); + return cpSpaceAddShape( $(cpSpace* spacePtr), sh); } |] + + go (Convex ((fmap.fmap) realToFrac -> verts) + (realToFrac -> radius) + ) spacePtr = do + vec <- V.thaw (V.fromList verts) + [C.block| cpShape* { + cpTransform trans = cpTransformIdentity; + cpShape* sh = cpPolyShapeNew($(cpBody* bodyPtr), $vec-len:vec, $vec-ptr:(cpVect *vec), trans, $(double radius)); + cpShapeSetUserData(sh, (void*) $(intptr_t ety)); + return cpSpaceAddShape( $(cpSpace* spacePtr), sh); } |] + +destroyShape :: SpacePtr -> Ptr Shape -> IO () +destroyShape spacePtr shapePtr = withForeignPtr spacePtr $ \space -> [C.block| void { + cpShape *shape = $(cpShape* shapePtr); + cpSpaceRemoveShape($(cpSpace* space), shape); + cpShapeFree (shape); }|] + +-- Sensor +getSensor :: Ptr Shape -> IO Bool +getSensor shape = toEnum . fromIntegral <$> [C.exp| int { + cpShapeGetSensor($(cpShape* shape)) }|] + +setSensor :: Ptr Shape -> Bool -> IO () +setSensor shape (fromIntegral . fromEnum -> isSensor) = [C.exp| void { + cpShapeSetSensor($(cpShape* shape), $(int isSensor)) }|] + +instance Component Sensor where + type Storage Sensor = Space Sensor +instance Has w IO Physics => Has w IO Sensor where + getStore = (cast :: Space Physics -> Space Sensor) <$> getStore + +instance ExplMembers IO (Space Sensor) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space Sensor) where + explSet (Space _ sMap _ _ _) ety (Sensor vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setSensor s vec + +instance ExplGet IO (Space Sensor) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + Sensor <$> getSensor s + +-- Elasticity +getElasticity :: Ptr Shape -> IO Double +getElasticity shape = realToFrac <$> [C.exp| double { + cpShapeGetElasticity($(cpShape* shape)) }|] + +setElasticity :: Ptr Shape -> Double -> IO () +setElasticity shape (realToFrac -> elasticity) = [C.exp| void { + cpShapeSetElasticity($(cpShape* shape), $(double elasticity)) }|] + +instance Component Elasticity where + type Storage Elasticity = Space Elasticity +instance Has w IO Physics => Has w IO Elasticity where + getStore = (cast :: Space Physics -> Space Elasticity) <$> getStore + +instance ExplMembers IO (Space Elasticity) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space Elasticity) where + explSet (Space _ sMap _ _ _) ety (Elasticity vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setElasticity s vec + +instance ExplGet IO (Space Elasticity) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + Elasticity <$> getElasticity s + +-- Mass +getMass :: Ptr Shape -> IO Double +getMass shape = realToFrac <$> [C.exp| double { + cpShapeGetMass($(cpShape* shape)) }|] + +setMass :: Ptr Shape -> Double -> IO () +setMass shape (realToFrac -> mass) = [C.exp| void { + cpShapeSetMass($(cpShape* shape), $(double mass)) }|] + +instance Component Mass where + type Storage Mass = Space Mass +instance Has w IO Physics => Has w IO Mass where + getStore = (cast :: Space Physics -> Space Mass) <$> getStore + +instance ExplMembers IO (Space Mass) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space Mass) where + explSet (Space _ sMap _ _ _) ety (Mass vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setMass s vec + +instance ExplGet IO (Space Mass) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + Mass <$> getMass s + +-- Density +getDensity :: Ptr Shape -> IO Double +getDensity shape = realToFrac <$> [C.exp| double { + cpShapeGetDensity($(cpShape* shape)) }|] + +setDensity :: Ptr Shape -> Double -> IO () +setDensity shape (realToFrac -> density) = [C.exp| void { + cpShapeSetDensity($(cpShape* shape), $(double density)) }|] + +instance Component Density where + type Storage Density = Space Density +instance Has w IO Physics => Has w IO Density where + getStore = (cast :: Space Physics -> Space Density) <$> getStore + +instance ExplMembers IO (Space Density) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space Density) where + explSet (Space _ sMap _ _ _) ety (Density vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setDensity s vec + +instance ExplGet IO (Space Density) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + Density <$> getDensity s + +-- Friction +getFriction :: Ptr Shape -> IO Double +getFriction shape = realToFrac <$> [C.exp| double { + cpShapeGetFriction($(cpShape* shape)) }|] + +setFriction :: Ptr Shape -> Double -> IO () +setFriction shape (realToFrac -> friction) = [C.exp| void { + cpShapeSetFriction($(cpShape* shape), $(double friction)) }|] + +instance Component Friction where + type Storage Friction = Space Friction +instance Has w IO Physics => Has w IO Friction where + getStore = (cast :: Space Physics -> Space Friction) <$> getStore + +instance ExplMembers IO (Space Friction) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space Friction) where + explSet (Space _ sMap _ _ _) ety (Friction vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setFriction s vec + +instance ExplGet IO (Space Friction) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + Friction <$> getFriction s + +-- SurfaceVelocity +getSurfaceVelocity :: Ptr Shape -> IO Vec +getSurfaceVelocity shape = do + x <- [C.exp| double { cpShapeGetSurfaceVelocity($(cpShape* shape)).x }|] + y <- [C.exp| double { cpShapeGetSurfaceVelocity($(cpShape* shape)).y }|] + return (V2 (realToFrac x) (realToFrac y)) + +setSurfaceVelocity :: Ptr Shape -> Vec -> IO () +setSurfaceVelocity shape (V2 (realToFrac -> x) (realToFrac -> y)) = [C.block| void { + const cpVect vec = { $(double x), $(double y) }; + cpShapeSetSurfaceVelocity($(cpShape* shape), vec); + }|] + +instance Component SurfaceVelocity where + type Storage SurfaceVelocity = Space SurfaceVelocity +instance Has w IO Physics => Has w IO SurfaceVelocity where + getStore = (cast :: Space Physics -> Space SurfaceVelocity) <$> getStore + +instance ExplMembers IO (Space SurfaceVelocity) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space SurfaceVelocity) where + explSet (Space _ sMap _ _ _) ety (SurfaceVelocity vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setSurfaceVelocity s vec + +instance ExplGet IO (Space SurfaceVelocity) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + SurfaceVelocity <$> getSurfaceVelocity s + +-- CollisionFilter +getFilter :: Ptr Shape -> IO CollisionFilter +getFilter shape = do + group <- [C.exp| unsigned int { cpShapeGetFilter($(cpShape* shape)).group }|] + cats <- [C.exp| unsigned int { cpShapeGetFilter($(cpShape* shape)).categories }|] + mask <- [C.exp| unsigned int { cpShapeGetFilter($(cpShape* shape)).mask }|] + return$ CollisionFilter group (Bitmask cats) (Bitmask mask) + +setFilter :: Ptr Shape -> CollisionFilter -> IO () +setFilter shape (CollisionFilter group (Bitmask cats) (Bitmask mask)) = + [C.block| void { + const cpShapeFilter filter = { $(unsigned int group) + , $(unsigned int cats) + , $(unsigned int mask) }; + cpShapeSetFilter($(cpShape* shape), filter); + }|] + +instance Component CollisionFilter where + type Storage CollisionFilter = Space CollisionFilter +instance Has w IO Physics => Has w IO CollisionFilter where + getStore = (cast :: Space Physics -> Space CollisionFilter) <$> getStore + +instance ExplMembers IO (Space CollisionFilter) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space CollisionFilter) where + explSet (Space _ sMap _ _ _) ety cfilter = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setFilter s cfilter + +instance ExplGet IO (Space CollisionFilter) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + getFilter s + +-- CollisionType +getCollisionType :: Ptr Shape -> IO C.CUIntPtr +getCollisionType shape = [C.exp| uintptr_t { + cpShapeGetCollisionType($(cpShape* shape)) }|] + +setCollisionType :: Ptr Shape -> C.CUIntPtr -> IO () +setCollisionType shape ctype = [C.exp| void { + cpShapeSetCollisionType($(cpShape* shape), $(uintptr_t ctype)) }|] + +instance Component CollisionType where + type Storage CollisionType = Space CollisionType +instance Has w IO Physics => Has w IO CollisionType where + getStore = (cast :: Space Physics -> Space CollisionType) <$> getStore + +instance ExplMembers IO (Space CollisionType) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplSet IO (Space CollisionType) where + explSet (Space _ sMap _ _ _) ety (CollisionType vec) = do + rd <- M.lookup ety <$> readIORef sMap + forM_ rd$ \s -> setCollisionType s vec + +instance ExplGet IO (Space CollisionType) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + CollisionType <$> getCollisionType s + +-- ShapeBody +getShapeBody :: Ptr Shape -> IO C.CUIntPtr +getShapeBody shape = [C.exp| uintptr_t { + (intptr_t) cpBodyGetUserData(cpShapeGetBody($(cpShape* shape))) }|] + +instance Component ShapeBody where + type Storage ShapeBody = Space ShapeBody +instance Has w IO Physics => Has w IO ShapeBody where + getStore = (cast :: Space Physics -> Space ShapeBody) <$> getStore + +instance ExplMembers IO (Space ShapeBody) where + explMembers s = explMembers (cast s :: Space Shape) + +instance ExplGet IO (Space ShapeBody) where + explExists s ety = explExists (cast s :: Space Shape) ety + explGet (Space _ sMap _ _ _) ety = do + Just s <- M.lookup ety <$> readIORef sMap + ShapeBody . Entity . fromIntegral <$> getShapeBody s diff --git a/src/Apecs/Physics/Space.hs b/src/Apecs/Physics/Space.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Space.hs @@ -0,0 +1,111 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE Strict #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE ViewPatterns #-} + +module Apecs.Physics.Space where + +import Apecs +import Apecs.Core +import Data.IORef +import Foreign.Concurrent +import Foreign.ForeignPtr (withForeignPtr) +import qualified Language.C.Inline as C +import Linear.V2 + +import Apecs.Physics.Types + +C.context phycsCtx +C.include "" + +-- Space +newSpace :: IO SpacePtr +newSpace = do + spaceRaw <- [C.exp| cpSpace* { cpSpaceNew() } |] + newForeignPtr spaceRaw [C.exp| void { cpSpaceFree($(cpSpace* spaceRaw)) } |] + +explStepPhysics :: SpacePtr -> Double -> IO () +explStepPhysics spacePtr (realToFrac -> dT) = withForeignPtr spacePtr $ \space -> + [C.exp| void { cpSpaceStep( $(cpSpace* space), $(double dT) ) } |] + +stepPhysics :: Has w IO Physics => Double -> System w () +stepPhysics dT = do + s :: Space Physics <- getStore + liftIO$ explStepPhysics (spacePtr s) dT + +instance Component Physics where + type Storage Physics = Space Physics + +type instance Elem (Space Physics) = Physics + +instance ExplInit IO (Space Physics) where + explInit = do + spacePtr <- newSpace + bRef <- newIORef mempty + sRef <- newIORef mempty + cRef <- newIORef mempty + hRef <- newIORef mempty + return (Space bRef sRef cRef hRef spacePtr) + +-- Gravity +earthGravity :: Gravity +earthGravity = Gravity $ V2 0 (-9.81) + +getGravity :: SpacePtr -> IO (V2 Double) +getGravity spacePtr = withForeignPtr spacePtr $ \space -> do + x <- [C.exp| double { cpSpaceGetGravity ($(cpSpace* space)).x } |] + y <- [C.exp| double { cpSpaceGetGravity ($(cpSpace* space)).y } |] + return (V2 (realToFrac x) (realToFrac y)) + +setGravity :: SpacePtr -> V2 Double -> IO () +setGravity spacePtr (V2 (realToFrac -> x) (realToFrac -> y)) = withForeignPtr spacePtr $ \space -> [C.block| void { + const cpVect vec = { $(double x), $(double y) }; + cpSpaceSetGravity($(cpSpace* space), vec); + } |] + +instance Component Gravity where + type Storage Gravity = Space Gravity + +instance Has w IO Physics => Has w IO Gravity where + getStore = (cast :: Space Physics -> Space Gravity) <$> getStore + +type instance Elem (Space Gravity) = Gravity + +instance ExplGet IO (Space Gravity) where + explExists _ _ = return True + explGet (Space _ _ _ _ spcPtr) _ = Gravity <$> getGravity spcPtr +instance ExplSet IO (Space Gravity) where + explSet (Space _ _ _ _ spcPtr) _ (Gravity v) = setGravity spcPtr v + +-- Iterations +getIterations :: SpacePtr -> IO Int +getIterations spacePtr = withForeignPtr spacePtr $ \space -> fromIntegral <$> [C.exp| int { cpSpaceGetIterations ($(cpSpace* space)) } |] + +setIterations :: SpacePtr -> Int -> IO () +setIterations spacePtr (fromIntegral -> its) = withForeignPtr spacePtr $ \space -> [C.block| void { + cpSpaceSetIterations($(cpSpace* space), $(int its)); + } |] + +instance Component Iterations where + type Storage Iterations = Space Iterations + +instance Has w IO Physics => Has w IO Iterations where + getStore = (cast :: Space Physics -> Space Iterations) <$> getStore + +type instance Elem (Space Iterations) = Iterations + +instance ExplGet IO (Space Iterations) where + explExists _ _ = return False + explGet (Space _ _ _ _ spcPtr) _ = Iterations <$> getIterations spcPtr +instance ExplSet IO (Space Iterations) where + explSet (Space _ _ _ _ spcPtr) _ (Iterations v) = setIterations spcPtr v + diff --git a/src/Apecs/Physics/Types.hs b/src/Apecs/Physics/Types.hs new file mode 100644 --- /dev/null +++ b/src/Apecs/Physics/Types.hs @@ -0,0 +1,228 @@ +{-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE GeneralizedNewtypeDeriving #-} +{-# LANGUAGE MultiParamTypeClasses #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE QuasiQuotes #-} +{-# LANGUAGE RankNTypes #-} +{-# LANGUAGE RecordWildCards #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE TypeFamilies #-} +{-# LANGUAGE UndecidableInstances #-} + +module Apecs.Physics.Types where + +import Apecs +import Apecs.Core +import Data.Bits +import Data.Char (intToDigit) +import qualified Data.IntMap as M +import qualified Data.IntSet as S +import Data.IORef +import qualified Data.Map as Map +import Data.Monoid ((<>)) +import qualified Foreign.C.Types as C +import Foreign.ForeignPtr +import Foreign.Ptr +import Language.C.Inline +import Language.C.Inline.Context +import qualified Language.C.Types as C +import qualified Language.Haskell.TH as TH +import Linear.V2 +import Numeric (showIntAtBase) + +phycsCtx :: Context +phycsCtx = baseCtx <> funCtx <> ctx + where ctx = mempty { ctxTypesTable = phycsTypesTable } + +phycsTypesTable :: Map.Map C.TypeSpecifier TH.TypeQ +phycsTypesTable = Map.fromList + [ (C.TypeName "cpArbiter", [t| Collision |]) + , (C.TypeName "cpBody", [t| Body |]) + , (C.TypeName "cpCollisionHandler", [t| CollisionHandler |]) + , (C.TypeName "cpConstraint", [t| Constraint |]) + , (C.TypeName "cpDataPointer", [t| C.CUInt |]) + , (C.TypeName "cpShape", [t| Shape |]) + , (C.TypeName "cpPointQueryInfo", [t| PointQueryResult |]) + , (C.TypeName "cpVect", [t| V2 C.CDouble |]) + , (C.TypeName "cpSpace", [t| FrnSpace |]) + ] + +-- | Uninhabited data type for constructing a world with a chipmunk space. +data Physics + +-- | Vector type used by the library +type Vec = V2 Double +-- | Type synonym indicating that a vector is expected to be in body-space coordinates +type BVec = Vec +-- | Type synonym indicating that a vector is expected to be in world-space coordinates +type WVec = Vec + +-- | Added to a component to add it to the physics space. +-- Deleting it will also delete all associated shapes and constraints. +-- A body has a number of subcomponents: @Position@, @Velocity@, @Force@, @Torque@, @BodyMass@, @Moment@, @Angle@, @AngularVelocity@, and @CenterOfGravity@. +-- These components cannot be added or removed from an entity, but rather are present as long as the entity has a @Body@. +data Body = DynamicBody | KinematicBody | StaticBody deriving (Eq, Ord, Enum) + +newtype Position = Position WVec +newtype Velocity = Velocity WVec +newtype Force = Force Vec +newtype Torque = Torque Double +newtype BodyMass = BodyMass Double deriving (Eq, Show) +newtype Moment = Moment Double deriving (Eq, Show) +newtype Angle = Angle Double deriving (Eq, Show) +newtype AngularVelocity = AngularVelocity Double +newtype CenterOfGravity = CenterOfGravity BVec + +-- | Shape component. +-- Adding a shape to an entity that has no @Body@ is a noop. +data Shape = Shape Convex + | ShapeExtend Entity Convex + | ShapeRead -- ^ Shapes are write-only, this is returned when you attempt to read + +-- | A convex polygon. +-- Consists of a list of vertices, and a radius. +data Convex = Convex [BVec] Double deriving (Eq, Show) + +newtype Sensor = Sensor Bool deriving (Eq, Show) +newtype Elasticity = Elasticity Double deriving (Eq, Show) +newtype Mass = Mass Double deriving (Eq, Show) +newtype Density = Density Double deriving (Eq, Show) +newtype Friction = Friction Double deriving (Eq, Show) +newtype SurfaceVelocity = SurfaceVelocity Vec deriving (Eq, Show) +newtype CollisionType = CollisionType C.CUIntPtr deriving (Eq, Show) +newtype ShapeBody = ShapeBody Entity deriving (Eq, Show) + +type CollisionGroup = CUInt + +data CollisionFilter = CollisionFilter + { filterGroup :: CollisionGroup + , filterCategories :: Bitmask + , filterMask :: Bitmask + } deriving (Eq, Show) + +newtype Bitmask = Bitmask CUInt deriving (Eq, Bits) +instance Show Bitmask where + show (Bitmask mask) = "Bitmask " ++ showIntAtBase 2 intToDigit mask "" + +data FrnSpace +data FrnVec + +data Space c = Space + { spBodies :: IOMap BodyRecord + , spShapes :: PtrMap Shape + , spConstraints :: PtrMap Constraint + , spHandlers :: PtrMap CollisionHandler + , spacePtr :: SpacePtr + } + +type instance Elem (Space a) = a + +data BodyRecord = BodyRecord + { brPtr :: Ptr Body + , brShapes :: S.IntSet + , brConstraints :: S.IntSet + } + +type IOMap a = IORef (M.IntMap a) +type PtrMap a = IOMap (Ptr a) +type SpacePtr = ForeignPtr FrnSpace + +-- | Number of iterations per step, global value +newtype Iterations = Iterations Int +-- | Gravity force vector, global value +newtype Gravity = Gravity Vec deriving (Eq, Show) +-- | Daming factor, global value +newtype Damping = Damping Double +-- | Speed threshold to be considered idle, and a candidate for being put to sleep. Global value +newtype IdleSpeedThreshold = IdleSpeedThreshold Double +-- | Sleep idle time threshold, global value +newtype SleepIdleTime = SleepIdleTime Double +-- | Collision parameter, global value +newtype CollisionSlop = CollisionSlop Double +-- | Collision parameter, global value +newtype CollisionBias = CollisionBias Double + +cast :: Space a -> Space b +cast (Space b s c h w) = Space b s c h w + +-- Constraint subcomponents +newtype MaxForce = MaxForce Double +newtype MaxBias = MaxBias Double +newtype ErrorBias = ErrorBias Double +newtype CollideBodies = CollideBodies Bool + +data Constraint = Constraint Entity ConstraintType + | ConstraintExtend Entity Entity ConstraintType + | ConstraintRead + +data ConstraintType + = PinJoint BVec BVec -- ^ Maintains a fixed distance between two anchor points + | SlideJoint BVec BVec Double Double -- offsetA offsetB min max + | PivotJoint WVec -- ^ Creates a pivot point at the given world coordinate + | PivotJoint2 BVec BVec -- ^ Creates a pivot point at the given body coordinates + | GrooveJoint BVec BVec BVec + | DampedSpring BVec BVec Double Double Double -- offA offB restlength stiffness damping + | DampedRotarySpring Double Double Double -- restAngle stiffness damping + | RotaryLimitJoint Double Double -- min max + | RatchetJoint Double Double -- phase ratchet + | GearJoint Double Double -- phase ratio + | SimpleMotor Double -- rate + +-- TODO +-- getConstraintImpulse +-- getPinJointDistance +-- getSlideJointDistance? + + +newtype BeginCB = BeginCB BeginFunc +newtype SeparateCB = SeparateCB SeparateFunc +newtype PreSolveCB = PreSolveCB PreSolveFunc +newtype PostSolveCB = PostSolveCB PostSolveFunc + +-- Collision, Space, Handler data pointer +type BeginFunc = Ptr Collision -> Ptr FrnSpace -> C.CUInt -> IO C.CUChar +type SeparateFunc = Ptr Collision -> Ptr FrnSpace -> C.CUInt -> IO () +type PreSolveFunc = Ptr Collision -> Ptr FrnSpace -> C.CUInt -> IO C.CUChar +type PostSolveFunc = Ptr Collision -> Ptr FrnSpace -> C.CUInt -> IO () + +data CollisionHandler = CollisionHandler + { source :: CollisionSource + , beginCB :: Maybe BeginCB + , separateCB :: Maybe SeparateCB + , preSolveCB :: Maybe PreSolveCB + , postSolveCB :: Maybe PostSolveCB + } + +data CollisionSource + = Wildcard CollisionGroup + | Between CollisionGroup CollisionGroup + +-- Corresponds to an 'arbiter' in Chipmunk +data Collision = Collision + { collisionNormal :: Vec + , collisionA :: Entity + , collisionB :: Entity + } deriving (Eq, Show) + +data CollisionProperties = CollisionProperties + { collisionElasticity :: Double + , collisionFriction :: Double + , collisionSurfaceVelocity :: Vec + } deriving (Eq, Show) + +data SegmentQueryResult = SegmentQueryResult + { sqShape :: Entity + , sqImpactPoint :: Vec + , sqImpactNormal :: Vec + , sqImpactAlpha :: Double + } deriving (Eq, Show) + +data PointQueryResult = PointQueryResult + { pqShape :: Entity + , pqPoint :: WVec + , pqDistance :: Double + , pqGradient :: Double + } deriving (Eq, Show) +