issue #503, chippmunk library is upgraded from 5.1 to 5.3.4, which is usd in cocos2d-iphone 0.99.5 & 1.0.0-rc

This commit is contained in:
Walzer 2011-05-21 14:58:22 +08:00
parent 57a14e90c9
commit 948c6d750e
43 changed files with 1074 additions and 1034 deletions

View File

@ -26,7 +26,10 @@ src/cpHashSet.c \
src/cpPolyShape.c \
src/cpShape.c \
src/cpSpace.c \
src/cpSpaceComponent.c \
src/cpSpaceHash.c \
src/cpSpaceQuery.c \
src/cpSpaceStep.c \
src/cpVect.c
LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/chipmunk

View File

@ -26,6 +26,16 @@
extern "C" {
#endif
#ifndef CP_ALLOW_PRIVATE_ACCESS
#define CP_ALLOW_PRIVATE_ACCESS 1
#endif
#if CP_ALLOW_PRIVATE_ACCESS == 1
#define CP_PRIVATE(symbol) symbol
#else
#define CP_PRIVATE(symbol) symbol##_private
#endif
void cpMessage(const char *message, const char *condition, const char *file, int line, int isError);
#ifdef NDEBUG
#define cpAssertWarn(condition, message)
@ -41,42 +51,6 @@ void cpMessage(const char *message, const char *condition, const char *file, int
#include "chipmunk_types.h"
static inline cpFloat
cpfmax(cpFloat a, cpFloat b)
{
return (a > b) ? a : b;
}
static inline cpFloat
cpfmin(cpFloat a, cpFloat b)
{
return (a < b) ? a : b;
}
static inline cpFloat
cpfabs(cpFloat n)
{
return (n < 0) ? -n : n;
}
static inline cpFloat
cpfclamp(cpFloat f, cpFloat min, cpFloat max)
{
return cpfmin(cpfmax(f, min), max);
}
static inline cpFloat
cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
{
return f1*(1.0f - t) + f2*t;
}
static inline cpFloat
cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
{
return f1 + cpfclamp(f2 - f1, -d, d);
}
#ifndef INFINITY
#ifdef _MSC_VER
union MSVC_EVIL_FLOAT_HACK
@ -97,7 +71,7 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
#endif
#endif
// Maximum allocated size for various Chipmunk buffer sizes
// Maximum allocated size for various Chipmunk buffers
#define CP_BUFFER_BYTES (32*1024)
#define cpmalloc malloc
@ -107,11 +81,11 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
#include "cpVect.h"
#include "cpBB.h"
#include "cpBody.h"
#include "cpArray.h"
#include "cpHashSet.h"
#include "cpSpaceHash.h"
#include "cpBody.h"
#include "cpShape.h"
#include "cpPolyShape.h"
@ -125,24 +99,65 @@ cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
#define CP_HASH_COEF (3344921057ul)
#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF)
extern char *cpVersionString;
extern const char *cpVersionString;
void cpInitChipmunk(void);
// Calculate the moment of inertia for a circle, r1 and r2 are the inner and outer diameters.
// (A solid circle has an inner diameter of 0)
/**
Calculate the moment of inertia for a circle.
r1 and r2 are the inner and outer diameters. A solid circle has an inner diameter of 0.
*/
cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset);
// Calculate the moment of inertia for a line segment. (beveling radius not supported)
/**
Calculate area of a hollow circle.
*/
cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2);
/**
Calculate the moment of inertia for a line segment.
Beveling radius is not supported.
*/
cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b);
// Calculate the moment of inertia for a solid polygon shape.
cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset);
/**
Calculate the area of a fattened (capsule shaped) line segment.
*/
cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r);
// Calculate the moment of inertia for a solid box.
/**
Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex.
*/
cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset);
/**
Calculate the signed area of a polygon.
*/
cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts);
/**
Calculate the natural centroid of a polygon.
*/
cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts);
/**
Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex)
*/
void cpRecenterPoly(const int numVerts, cpVect *verts);
/**
Calculate the moment of inertia for a solid box.
*/
cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height);
#ifdef __cplusplus
}
static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);}
static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);}
static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);}
static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);}
static inline cpVect operator -(const cpVect v){return cpvneg(v);}
#endif
#endif

View File

@ -15,6 +15,7 @@
#endif
MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv()
MAKE_REF(cpveql);
MAKE_REF(cpvadd);
MAKE_REF(cpvneg);
MAKE_REF(cpvsub);
@ -33,8 +34,8 @@ MAKE_REF(cpvnormalize_safe);
MAKE_REF(cpvclamp);
MAKE_REF(cpvlerpconst);
MAKE_REF(cpvdist);
MAKE_REF(cpvnear);
MAKE_REF(cpvdistsq);
MAKE_REF(cpvnear);
MAKE_REF(cpBBNew);
MAKE_REF(cpBBintersects);
@ -46,10 +47,18 @@ MAKE_REF(cpBBexpand);
MAKE_REF(cpBodyWorld2Local);
MAKE_REF(cpBodyLocal2World);
MAKE_REF(cpBodyApplyImpulse);
MAKE_REF(cpBodyIsSleeping);
MAKE_REF(cpBodyIsRogue);
MAKE_REF(cpBodyKineticEnergy);
MAKE_REF(cpArbiterIsFirstContact);
MAKE_REF(cpArbiterGetShapes);
MAKE_REF(cpArbiterGetNormal);
MAKE_REF(cpArbiterGetPoint);
MAKE_REF(cpConstraintGetImpulse);
MAKE_REF(cpSegmentQueryHitPoint);
MAKE_REF(cpSegmentQueryHitDist);
#endif // _CHIPMUNK_FFI_H_

View File

@ -3,14 +3,29 @@
#ifdef __APPLE__
#ifndef AIRPLAYUSECHIPMUNK
#import "TargetConditionals.h"
#endif
#import "TargetConditionals.h"
#endif
// Use single precision floats on the iPhone.
#if TARGET_OS_IPHONE==1
#define CP_USE_DOUBLES 0
#else
// dont' use CGPoints in cocos2d-x to make your code multi-platform
// #if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS)
// #define CP_USE_CGPOINTS
// #endif
#ifdef CP_USE_CGPOINTS
#if TARGET_OS_IPHONE
#import <CoreGraphics/CGGeometry.h>
#elif TARGET_OS_MAC
#import <ApplicationServices/ApplicationServices.h>
#endif
#if defined(__LP64__) && __LP64__
#define CP_USE_DOUBLES 1
#else
#define CP_USE_DOUBLES 0
#endif
#endif
#ifndef CP_USE_DOUBLES
// use doubles by default for higher precision
#define CP_USE_DOUBLES 1
#endif
@ -41,17 +56,67 @@
#define cpfceil ceilf
#endif
//#if TARGET_OS_IPHONE
// CCPoints are structurally the same, and allow
// easy interoperability with other iPhone libraries
//#import <CoreGraphics/CCGeometry.h>
//typedef CCPoint cpVect;
//#else
static inline cpFloat
cpfmax(cpFloat a, cpFloat b)
{
return (a > b) ? a : b;
}
static inline cpFloat
cpfmin(cpFloat a, cpFloat b)
{
return (a < b) ? a : b;
}
static inline cpFloat
cpfabs(cpFloat n)
{
return (n < 0) ? -n : n;
}
static inline cpFloat
cpfclamp(cpFloat f, cpFloat min, cpFloat max)
{
return cpfmin(cpfmax(f, min), max);
}
static inline cpFloat
cpflerp(cpFloat f1, cpFloat f2, cpFloat t)
{
return f1*(1.0f - t) + f2*t;
}
static inline cpFloat
cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d)
{
return f1 + cpfclamp(f2 - f1, -d, d);
}
// CGPoints are structurally the same, and allow
// easy interoperability with other Cocoa libraries
#ifdef CP_USE_CGPOINTS
typedef CGPoint cpVect;
#else
typedef struct cpVect{cpFloat x,y;} cpVect;
//#endif
#endif
typedef unsigned int cpHashValue;
// Oh C, how we love to define our own boolean types to get compiler compatibility
#ifdef CP_BOOL_TYPE
typedef CP_BOOL_TYPE cpBool;
#else
typedef int cpBool;
#endif
#ifndef cpTrue
#define cpTrue 1
#endif
#ifndef cpFalse
#define cpFalse 0
#endif
#ifdef CP_DATA_POINTER_TYPE
typedef CP_DATA_POINTER_TYPE cpDataPointer;
#else
@ -76,6 +141,12 @@ typedef unsigned int cpHashValue;
typedef unsigned int cpLayers;
#endif
#ifdef CP_TIMESTAMP_TYPE
typedef CP_TIMESTAMP_TYPE cpTimestamp;
#else
typedef unsigned int cpTimestamp;
#endif
#ifndef CP_NO_GROUP
#define CP_NO_GROUP ((cpGroup)0)
#endif

View File

@ -39,7 +39,7 @@ typedef struct cpConstraintClass {
typedef struct cpConstraint {
const cpConstraintClass *klass;
CP_PRIVATE(const cpConstraintClass *klass);
cpBody *a, *b;
cpFloat maxForce;
@ -56,14 +56,26 @@ typedef cpConstraint cpJoint;
void cpConstraintDestroy(cpConstraint *constraint);
void cpConstraintFree(cpConstraint *constraint);
static inline void
cpConstraintActivateBodies(cpConstraint *constraint)
{
cpBody *a = constraint->a; if(a) cpBodyActivate(a);
cpBody *b = constraint->b; if(b) cpBodyActivate(b);
}
static inline cpFloat
cpConstraintGetImpulse(cpConstraint *constraint)
{
return constraint->CP_PRIVATE(klass)->getImpulse(constraint);
}
#define cpConstraintCheckCast(constraint, struct) \
cpAssert(constraint->klass == struct##GetClass(), "Constraint is not a "#struct);
cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct);
#define CP_DefineConstraintGetter(struct, type, member, name) \
static inline type \
struct##Get##name(cpConstraint *constraint){ \
struct##Get##name(const cpConstraint *constraint){ \
cpConstraintCheckCast(constraint, struct); \
return ((struct *)constraint)->member; \
} \
@ -72,6 +84,7 @@ struct##Get##name(cpConstraint *constraint){ \
static inline void \
struct##Set##name(cpConstraint *constraint, type value){ \
cpConstraintCheckCast(constraint, struct); \
cpConstraintActivateBodies(constraint); \
((struct *)constraint)->member = value; \
} \

View File

@ -30,8 +30,8 @@ typedef struct cpDampedRotarySpring {
cpFloat damping;
cpDampedRotarySpringTorqueFunc springTorqueFunc;
cpFloat dt;
cpFloat target_wrn;
cpFloat w_coef;
cpFloat iSum;
} cpDampedRotarySpring;

View File

@ -33,8 +33,8 @@ typedef struct cpDampedSpring {
cpFloat damping;
cpDampedSpringForceFunc springForceFunc;
cpFloat dt;
cpFloat target_vrn;
cpFloat v_coef;
cpVect r1, r2;
cpFloat nMass;

View File

@ -40,5 +40,9 @@ cpGrooveJoint *cpGrooveJointAlloc(void);
cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
// TODO setters for the groove.
CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);

View File

@ -25,6 +25,17 @@ void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass,
#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt))
// Get valid body pointers and exit early if the bodies are idle
#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \
cpBody *a_var, *b_var; { \
a_var = ((cpConstraint *)constraint)->a; \
b_var = ((cpConstraint *)constraint)->b; \
if( \
(cpBodyIsSleeping(a_var) || cpBodyIsStatic(a_var)) && \
(cpBodyIsSleeping(b_var) || cpBodyIsStatic(b_var)) \
) return; \
}
static inline cpVect
relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
@ -45,11 +56,18 @@ apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
cpBodyApplyImpulse(b, j, r2);
}
static inline void
apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
{
body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
body->w_bias += body->i_inv*cpvcross(r, j);
}
static inline void
apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
{
cpBodyApplyBiasImpulse(a, cpvneg(j), r1);
cpBodyApplyBiasImpulse(b, j, r2);
apply_bias_impulse(a, cpvneg(j), r1);
apply_bias_impulse(b, j, r2);
}
static inline cpVect

View File

@ -31,73 +31,79 @@ extern cpFloat cp_collision_slop;
// Data structure for contact points.
typedef struct cpContact {
// Contact point and normal.
cpVect p, n;
cpVect CP_PRIVATE(p), CP_PRIVATE(n);
// Penetration distance.
cpFloat dist;
CP_PRIVATE(cpFloat dist);
// Calculated by cpArbiterPreStep().
cpVect r1, r2;
cpFloat nMass, tMass, bounce;
cpVect CP_PRIVATE(r1), CP_PRIVATE(r2);
cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce);
// Persistant contact information.
cpFloat jnAcc, jtAcc, jBias;
cpFloat bias;
cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias);
CP_PRIVATE(cpFloat bias);
// Hash value used to (mostly) uniquely identify a contact.
cpHashValue hash;
CP_PRIVATE(cpHashValue hash);
} cpContact;
// Contacts are always allocated in groups.
cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash);
// Sum the contact impulses. (Can be used after cpSpaceStep() returns)
cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
cpVect CP_PRIVATE(cpContactsSumImpulses)(cpContact *contacts, int numContacts);
cpVect CP_PRIVATE(cpContactsSumImpulsesWithFriction)(cpContact *contacts, int numContacts);
#define CP_MAX_CONTACTS_PER_ARBITER 6
typedef enum cpArbiterState {
cpArbiterStateNormal,
cpArbiterStateFirstColl,
cpArbiterStateIgnore,
cpArbiterStateSleep,
cpArbiterStateCached,
} cpArbiterState;
// Data structure for tracking collisions between shapes.
typedef struct cpArbiter {
// Information on the contact points between the objects.
int numContacts;
cpContact *contacts;
CP_PRIVATE(int numContacts);
CP_PRIVATE(cpContact *contacts);
// The two shapes involved in the collision.
// The two shapes and bodies involved in the collision.
// These variables are NOT in the order defined by the collision handler.
cpShape *private_a, *private_b;
// Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from
// many headaches
cpShape CP_PRIVATE(*a), CP_PRIVATE(*b);
// Calculated before calling the pre-solve collision handler
// Override them with custom values if you want specialized behavior
cpFloat e;
cpFloat u;
CP_PRIVATE(cpFloat e);
CP_PRIVATE(cpFloat u);
// Used for surface_v calculations, implementation may change
cpVect surface_vr;
CP_PRIVATE(cpVect surface_vr);
// Time stamp of the arbiter. (from cpSpace)
int stamp;
CP_PRIVATE(cpTimestamp stamp);
struct cpCollisionHandler *handler;
CP_PRIVATE(struct cpCollisionHandler *handler);
// Are the shapes swapped in relation to the collision handler?
char swappedColl;
char state;
CP_PRIVATE(cpBool swappedColl);
CP_PRIVATE(cpArbiterState state);
} cpArbiter;
// Arbiters are allocated in large buffers by the space and don't require a destroy function
cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
cpArbiter* CP_PRIVATE(cpArbiterInit)(cpArbiter *arb, cpShape *a, cpShape *b);
// These functions are all intended to be used internally.
// Inject new contact points into the arbiter while preserving contact history.
void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
void CP_PRIVATE(cpArbiterUpdate)(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
// Precalculate values used by the solver.
void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
void cpArbiterApplyCachedImpulse(cpArbiter *arb);
void CP_PRIVATE(cpArbiterPreStep)(cpArbiter *arb, cpFloat dt_inv);
void CP_PRIVATE(cpArbiterApplyCachedImpulse)(cpArbiter *arb);
// Run an iteration of the solver on the arbiter.
void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
void CP_PRIVATE(cpArbiterApplyImpulse)(cpArbiter *arb, cpFloat eCoef);
// Arbiter Helper Functions
cpVect cpArbiterTotalImpulse(cpArbiter *arb);
@ -106,31 +112,77 @@ void cpArbiterIgnore(cpArbiter *arb);
static inline void
cpArbiterGetShapes(cpArbiter *arb, cpShape **a, cpShape **b)
cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
{
if(arb->swappedColl){
(*a) = arb->private_b, (*b) = arb->private_a;
if(arb->CP_PRIVATE(swappedColl)){
(*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a);
} else {
(*a) = arb->private_a, (*b) = arb->private_b;
(*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b);
}
}
#define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b);
static inline void
cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
{
CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
(*a) = shape_a->body;
(*b) = shape_b->body;
}
#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b);
static inline cpBool
cpArbiterIsFirstContact(const cpArbiter *arb)
{
return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl;
}
static inline int
cpArbiterIsFirstContact(cpArbiter *arb)
cpArbiterGetCount(const cpArbiter *arb)
{
return arb->state == cpArbiterStateFirstColl;
return arb->CP_PRIVATE(numContacts);
}
static inline cpVect
cpArbiterGetNormal(cpArbiter *arb, int i)
cpArbiterGetNormal(const cpArbiter *arb, int i)
{
cpVect n = arb->contacts[i].n;
return arb->swappedColl ? cpvneg(n) : n;
cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n;
}
static inline cpVect
cpArbiterGetPoint(cpArbiter *arb, int i)
cpArbiterGetPoint(const cpArbiter *arb, int i)
{
return arb->contacts[i].p;
return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
}
static inline cpFloat
cpArbiteGetDepth(const cpArbiter *arb, int i)
{
return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
}
typedef struct cpContactPointSet {
int count;
struct {
cpVect point, normal;
cpFloat dist;
} points[CP_MAX_CONTACTS_PER_ARBITER];
} cpContactPointSet;
static inline cpContactPointSet
cpArbiterGetContactPointSet(const cpArbiter *arb)
{
cpContactPointSet set;
set.count = cpArbiterGetCount(arb);
int i;
for(i=0; i<set.count; i++){
set.points[i].point = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
}
return set;
}

View File

@ -22,8 +22,9 @@
// NOTE: cpArray is rarely used and will probably go away.
typedef struct cpArray{
int num, max;
void **arr;
CP_PRIVATE(int num);
CP_PRIVATE(int max);
CP_PRIVATE(void **arr);
} cpArray;
typedef void (*cpArrayIter)(void *ptr, void *data);
@ -42,5 +43,7 @@ void *cpArrayPop(cpArray *arr);
void cpArrayDeleteIndex(cpArray *arr, int idx);
void cpArrayDeleteObj(cpArray *arr, void *obj);
void cpArrayAppend(cpArray *arr, cpArray *other);
void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data);
int cpArrayContains(cpArray *arr, void *ptr);
cpBool cpArrayContains(cpArray *arr, void *ptr);

View File

@ -31,19 +31,19 @@ cpBBNew(const cpFloat l, const cpFloat b,
return bb;
}
static inline int
static inline cpBool
cpBBintersects(const cpBB a, const cpBB b)
{
return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t);
}
static inline int
static inline cpBool
cpBBcontainsBB(const cpBB bb, const cpBB other)
{
return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t);
}
static inline int
static inline cpBool
cpBBcontainsVect(const cpBB bb, const cpVect v)
{
return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y);

View File

@ -20,14 +20,27 @@
*/
struct cpBody;
struct cpShape;
struct cpSpace;
typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt);
extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault;
extern cpBodyPositionFunc cpBodyUpdatePositionDefault;
// Structure to hold information about the contact graph components
// when putting groups of objects to sleep.
// No interesting user accessible fields.
typedef struct cpComponentNode {
struct cpBody *parent;
struct cpBody *next;
int rank;
cpFloat idleTime;
} cpComponentNode;
typedef struct cpBody{
// *** Integration Functions.ntoehu
// *** Integration Functions.
// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
cpBodyVelocityFunc velocity_func;
@ -63,16 +76,26 @@ typedef struct cpBody{
// User defined data pointer.
cpDataPointer data;
// *** Other Fields
// Maximum velocities this body can move at after integrating velocity
cpFloat v_limit, w_limit;
// *** Internally Used Fields
// Velocity bias values used when solving penetrations and correcting constraints.
cpVect v_bias;
cpFloat w_bias;
CP_PRIVATE(cpVect v_bias);
CP_PRIVATE(cpFloat w_bias);
// int active;
// Space this body has been added to
CP_PRIVATE(struct cpSpace *space);
// Pointer to the shape list.
// Shapes form a linked list using cpShape.next when added to a space.
CP_PRIVATE(struct cpShape *shapesList);
// Used by cpSpaceStep() to store contact graph information.
CP_PRIVATE(cpComponentNode node);
} cpBody;
// Basic allocation/destruction functions
@ -80,11 +103,48 @@ cpBody *cpBodyAlloc(void);
cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i);
cpBody *cpBodyNew(cpFloat m, cpFloat i);
cpBody *cpBodyInitStatic(cpBody *body);
cpBody *cpBodyNewStatic();
void cpBodyDestroy(cpBody *body);
void cpBodyFree(cpBody *body);
#define CP_DefineBodyGetter(type, member, name) static inline type cpBodyGet##name(cpBody *body){return body->member;}
#define CP_DefineBodySetter(type, member, name) static inline void cpBodySet##name(cpBody *body, type value){body->member = value;}
// Wake up a sleeping or idle body. (defined in cpSpace.c)
void cpBodyActivate(cpBody *body);
// Force a body to sleep;
// defined in cpSpaceComponent.c
void cpBodySleep(cpBody *body);
void cpBodySleepWithGroup(cpBody *body, cpBody *group);
static inline cpBool
cpBodyIsSleeping(const cpBody *body)
{
return (CP_PRIVATE(body->node).next != ((cpBody*)0));
}
static inline cpBool
cpBodyIsStatic(const cpBody *body)
{
return CP_PRIVATE(body->node).idleTime == INFINITY;
}
static inline cpBool
cpBodyIsRogue(const cpBody *body)
{
return (body->CP_PRIVATE(space) == ((struct cpSpace*)0));
}
#define CP_DefineBodyGetter(type, member, name) \
static inline type cpBodyGet##name(const cpBody *body){return body->member;}
#define CP_DefineBodySetter(type, member, name) \
static inline void \
cpBodySet##name(cpBody *body, const type value){ \
cpBodyActivate(body); \
body->member = value; \
} \
#define CP_DefineBodyProperty(type, member, name) \
CP_DefineBodyGetter(type, member, name) \
@ -120,41 +180,40 @@ void cpBodyUpdatePosition(cpBody *body, cpFloat dt);
// Convert body local to world coordinates
static inline cpVect
cpBodyLocal2World(cpBody *body, cpVect v)
cpBodyLocal2World(const cpBody *body, const cpVect v)
{
return cpvadd(body->p, cpvrotate(v, body->rot));
}
// Convert world to body local coordinates
static inline cpVect
cpBodyWorld2Local(cpBody *body, cpVect v)
cpBodyWorld2Local(const cpBody *body, const cpVect v)
{
return cpvunrotate(cpvsub(v, body->p), body->rot);
}
// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates).
static inline void
cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r)
cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r)
{
body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
body->w += body->i_inv*cpvcross(r, j);
}
// Not intended for external use. Used by cpArbiter.c and cpConstraint.c.
static inline void
cpBodyApplyBiasImpulse(cpBody *body, cpVect j, cpVect r)
{
body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
body->w_bias += body->i_inv*cpvcross(r, j);
}
// Zero the forces on a body.
void cpBodyResetForces(cpBody *body);
// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates).
void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r);
void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);
static inline cpFloat
cpBodyKineticEnergy(const cpBody *body)
{
// Need to do some fudging to avoid NaNs
cpFloat vsq = cpvdot(body->v, body->v);
cpFloat wsq = body->w*body->w;
return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f);
}
// Apply a damped spring force between two bodies.
// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead.
void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt);
//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max);

View File

@ -19,5 +19,10 @@
* SOFTWARE.
*/
// Collides two cpShape structures. (this function is lonely :( )
int cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr);
//TODO delete this header?
// Collides two cpShape structures.
// Returns the number of contact points added to arr
// which should be at least CP_MAX_CONTACTS_PER_ARBITER in length.
// This function is very lonely in this header :(
int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr);

View File

@ -25,39 +25,36 @@
// cpHashSetBin's form the linked lists in the chained hash table.
typedef struct cpHashSetBin {
// Pointer to the element.
void *elt;
CP_PRIVATE(void *elt);
// Hash value of the element.
cpHashValue hash;
CP_PRIVATE(cpHashValue hash);
// Next element in the chain.
struct cpHashSetBin *next;
CP_PRIVATE(struct cpHashSetBin *next);
} cpHashSetBin;
// Equality function. Returns true if ptr is equal to elt.
typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt);
typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt);
// Used by cpHashSetInsert(). Called to transform the ptr into an element.
typedef void *(*cpHashSetTransFunc)(void *ptr, void *data);
// Iterator function for a hashset.
typedef void (*cpHashSetIterFunc)(void *elt, void *data);
// Filter function. Returns false if elt should be dropped.
typedef int (*cpHashSetFilterFunc)(void *elt, void *data);
typedef struct cpHashSet {
// Number of elements stored in the table.
int entries;
CP_PRIVATE(int entries);
// Number of cells in the table.
int size;
CP_PRIVATE(int size);
cpHashSetEqlFunc eql;
cpHashSetTransFunc trans;
CP_PRIVATE(cpHashSetEqlFunc eql);
CP_PRIVATE(cpHashSetTransFunc trans);
// Default value returned by cpHashSetFind() when no element is found.
// Defaults to NULL.
void *default_value;
CP_PRIVATE(void *default_value);
// The table and recycled bins
cpHashSetBin **table, *pooledBins;
CP_PRIVATE(cpHashSetBin **table);
CP_PRIVATE(cpHashSetBin *pooledBins);
cpArray *allocatedBuffers;
CP_PRIVATE(cpArray *allocatedBuffers);
} cpHashSet;
// Basic allocation/destruction functions.
@ -77,6 +74,9 @@ void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr);
void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr);
// Iterate over a hashset.
typedef void (*cpHashSetIterFunc)(void *elt, void *data);
void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data);
// Iterate over a hashset, retain .
// Iterate over a hashset, drop the element if the func returns false.
typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data);
void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data);

View File

@ -29,16 +29,16 @@ typedef struct cpPolyShapeAxis{
// Convex polygon shape structure.
typedef struct cpPolyShape{
cpShape shape;
CP_PRIVATE(cpShape shape);
// Vertex and axis lists.
int numVerts;
cpVect *verts;
cpPolyShapeAxis *axes;
CP_PRIVATE(int numVerts);
CP_PRIVATE(cpVect *verts);
CP_PRIVATE(cpPolyShapeAxis *axes);
// Transformed vertex and axis lists.
cpVect *tVerts;
cpPolyShapeAxis *tAxes;
CP_PRIVATE(cpVect *tVerts);
CP_PRIVATE(cpPolyShapeAxis *tAxes);
} cpPolyShape;
// Basic allocation functions.
@ -50,7 +50,7 @@ cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFl
cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height);
// Check that a set of vertexes has a correct winding and that they are convex
int cpPolyValidate(cpVect *verts, int numVerts);
cpBool cpPolyValidate(const cpVect *verts, const int numVerts);
int cpPolyShapeGetNumVerts(cpShape *shape);
cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
@ -61,43 +61,43 @@ cpVect cpPolyShapeGetVert(cpShape *shape, int idx);
static inline cpFloat
cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d)
{
cpVect *verts = poly->tVerts;
cpVect *verts = poly->CP_PRIVATE(tVerts);
cpFloat min = cpvdot(n, verts[0]);
int i;
for(i=1; i<poly->numVerts; i++)
for(i=1; i<poly->CP_PRIVATE(numVerts); i++)
min = cpfmin(min, cpvdot(n, verts[i]));
return min - d;
}
// Returns true if the polygon contains the vertex.
static inline int
cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v)
static inline cpBool
cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v)
{
cpPolyShapeAxis *axes = poly->tAxes;
cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
int i;
for(i=0; i<poly->numVerts; i++){
for(i=0; i<poly->CP_PRIVATE(numVerts); i++){
cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
if(dist > 0.0f) return 0;
if(dist > 0.0f) return cpFalse;
}
return 1;
return cpTrue;
}
// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal.
static inline int
cpPolyShapeContainsVertPartial(cpPolyShape *poly, cpVect v, cpVect n)
static inline cpBool
cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n)
{
cpPolyShapeAxis *axes = poly->tAxes;
cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes);
int i;
for(i=0; i<poly->numVerts; i++){
for(i=0; i<poly->CP_PRIVATE(numVerts); i++){
if(cpvdot(axes[i].n, n) < 0.0f) continue;
cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;
if(dist > 0.0f) return 0;
if(dist > 0.0f) return cpFalse;
}
return 1;
return cpTrue;
}

View File

@ -47,7 +47,7 @@ typedef struct cpShapeClass {
void (*destroy)(struct cpShape *shape);
// called by cpShapePointQuery().
int (*pointQuery)(struct cpShape *shape, cpVect p);
cpBool (*pointQuery)(struct cpShape *shape, cpVect p);
// called by cpShapeSegmentQuery()
void (*segmentQuery)(struct cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
@ -56,7 +56,7 @@ typedef struct cpShapeClass {
// Basic shape struct that the others inherit from.
typedef struct cpShape{
// The "class" of a shape as defined above
const cpShapeClass *klass;
CP_PRIVATE(const cpShapeClass *klass);
// cpBody that the shape is attached to.
cpBody *body;
@ -65,7 +65,7 @@ typedef struct cpShape{
cpBB bb;
// Sensors invoke callbacks, but do not generate collisions
int sensor;
cpBool sensor;
// *** Surface properties.
@ -90,8 +90,11 @@ typedef struct cpShape{
// *** Internally Used Fields
// Shapes form a linked list when added to space on a non-NULL body
CP_PRIVATE(struct cpShape *next);
// Unique id used as the hash value.
cpHashValue hashid;
CP_PRIVATE(cpHashValue hashid);
} cpShape;
// Low level shape initialization func.
@ -105,21 +108,21 @@ void cpShapeFree(cpShape *shape);
cpBB cpShapeCacheBB(cpShape *shape);
// Test if a point lies within a shape.
int cpShapePointQuery(cpShape *shape, cpVect p);
cpBool cpShapePointQuery(cpShape *shape, cpVect p);
#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape)
// Circle shape structure.
typedef struct cpCircleShape{
cpShape shape;
CP_PRIVATE(cpShape shape);
// Center in body space coordinates
cpVect c;
CP_PRIVATE(cpVect c);
// Radius.
cpFloat r;
CP_PRIVATE(cpFloat r);
// Transformed center. (world space coordinates)
cpVect tc;
CP_PRIVATE(cpVect tc);
} cpCircleShape;
// Basic allocation functions for cpCircleShape.
@ -132,15 +135,15 @@ CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius);
// Segment shape structure.
typedef struct cpSegmentShape{
cpShape shape;
CP_PRIVATE(cpShape shape);
// Endpoints and normal of the segment. (body space coordinates)
cpVect a, b, n;
cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n);
// Radius of the segment. (Thickness)
cpFloat r;
cpFloat CP_PRIVATE(r);
// Transformed endpoints and normal. (world space coordinates)
cpVect ta, tb, tn;
cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn);
} cpSegmentShape;
// Basic allocation functions for cpSegmentShape.
@ -159,16 +162,16 @@ void cpResetShapeIdCounter(void);
// Directed segment queries against individual shapes.
void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);
int cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
static inline cpVect
cpSegmentQueryHitPoint(cpVect start, cpVect end, cpSegmentQueryInfo info)
cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
{
return cpvlerp(start, end, info.t);
}
static inline cpFloat
cpSegmentQueryHitDist(cpVect start, cpVect end, cpSegmentQueryInfo info)
cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
{
return cpvdist(start, end)*info.t;
}

View File

@ -22,11 +22,11 @@
struct cpSpace;
// Number of frames that contact information should persist.
extern int cp_contact_persistence;
extern cpTimestamp cp_contact_persistence;
// User collision handler function types.
typedef int (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
typedef int (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, struct cpSpace *space, void *data);
@ -42,9 +42,8 @@ typedef struct cpCollisionHandler {
void *data;
} cpCollisionHandler;
#define CP_MAX_CONTACTS_PER_ARBITER 6
typedef struct cpContactBufferHeader {
int stamp;
cpTimestamp stamp;
struct cpContactBufferHeader *next;
unsigned int numContacts;
} cpContactBufferHeader;
@ -64,44 +63,62 @@ typedef struct cpSpace{
// Default damping to supply when integrating rigid body motions.
cpFloat damping;
// Speed threshold for a body to be considered idle.
// The default value of 0 means to let the space guess a good threshold based on gravity.
cpFloat idleSpeedThreshold;
// Time a group of bodies must remain idle in order to fall asleep
// The default value of INFINITY disables the sleeping algorithm.
cpFloat sleepTimeThreshold;
// *** Internally Used Fields
// When the space is locked, you should not add or remove objects;
int locked;
// When the space lock count is non zero you cannot add or remove objects
CP_PRIVATE(int locked);
// Time stamp. Is incremented on every call to cpSpaceStep().
int stamp;
CP_PRIVATE(cpTimestamp stamp);
// The static and active shape spatial hashes.
cpSpaceHash *staticShapes;
cpSpaceHash *activeShapes;
CP_PRIVATE(cpSpaceHash *staticShapes);
CP_PRIVATE(cpSpaceHash *activeShapes);
// List of bodies in the system.
cpArray *bodies;
CP_PRIVATE(cpArray *bodies);
// List of groups of sleeping bodies.
CP_PRIVATE(cpArray *sleepingComponents);
// List of bodies that have been flagged to be awoken.
CP_PRIVATE(cpArray *rousedBodies);
// List of active arbiters for the impulse solver.
cpArray *arbiters, *pooledArbiters;
CP_PRIVATE(cpArray *arbiters);
CP_PRIVATE(cpArray *pooledArbiters);
// Linked list ring of contact buffers.
// Head is the current buffer. Tail is the oldest buffer.
// The list points in the direction of tail->head.
cpContactBufferHeader *contactBuffersHead, *contactBuffersTail;
// Head is the newest buffer, and each buffer points to a newer buffer.
// Head wraps around and points to the oldest (tail) buffer.
CP_PRIVATE(cpContactBufferHeader *contactBuffersHead);
CP_PRIVATE(cpContactBufferHeader *_contactBuffersTail_Deprecated);
// List of buffers to be free()ed when destroying the space.
cpArray *allocatedBuffers;
CP_PRIVATE(cpArray *allocatedBuffers);
// Persistant contact set.
cpHashSet *contactSet;
CP_PRIVATE(cpHashSet *contactSet);
// List of constraints in the system.
cpArray *constraints;
CP_PRIVATE(cpArray *constraints);
// Set of collisionpair functions.
cpHashSet *collFuncSet;
CP_PRIVATE(cpHashSet *collFuncSet);
// Default collision handler.
cpCollisionHandler defaultHandler;
CP_PRIVATE(cpCollisionHandler defaultHandler);
cpHashSet *postStepCallbacks;
CP_PRIVATE(cpHashSet *postStepCallbacks);
cpBody staticBody;
} cpSpace;
// Basic allocation/destruction functions.
@ -159,13 +176,20 @@ cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, c
// Segment query callback function
typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data);
int cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
// BB query callback function
typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data);
void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);
// Shape query callback function
typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data);
cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data);
void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape);
// Iterator function for iterating the bodies in a space.
typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data);
@ -176,5 +200,7 @@ void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
void cpSpaceRehashStatic(cpSpace *space);
void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
// Update the space.
void cpSpaceStep(cpSpace *space, cpFloat dt);

View File

@ -30,7 +30,7 @@ typedef struct cpHandle{
int retain;
// Query stamp. Used to make sure two objects
// aren't identified twice in the same query.
int stamp;
cpTimestamp stamp;
} cpHandle;
// Linked list element for in the chains.
@ -44,25 +44,26 @@ typedef cpBB (*cpSpaceHashBBFunc)(void *obj);
typedef struct cpSpaceHash{
// Number of cells in the table.
int numcells;
CP_PRIVATE(int numcells);
// Dimentions of the cells.
cpFloat celldim;
CP_PRIVATE(cpFloat celldim);
// BBox callback.
cpSpaceHashBBFunc bbfunc;
CP_PRIVATE(cpSpaceHashBBFunc bbfunc);
// Hashset of the handles and the recycled ones.
cpHashSet *handleSet;
cpArray *pooledHandles;
CP_PRIVATE(cpHashSet *handleSet);
CP_PRIVATE(cpArray *pooledHandles);
// The table and the recycled bins.
cpSpaceHashBin **table, *pooledBins;
CP_PRIVATE(cpSpaceHashBin **table);
CP_PRIVATE(cpSpaceHashBin *pooledBins);
// list of buffers to free on destruction.
cpArray *allocatedBuffers;
CP_PRIVATE(cpArray *allocatedBuffers);
// Incremented on each query. See cpHandle.stamp.
int stamp;
CP_PRIVATE(cpTimestamp stamp);
} cpSpaceHash;
//Basic allocation/destruction functions.
@ -77,7 +78,7 @@ void cpSpaceHashFree(cpSpaceHash *hash);
void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells);
// Add an object to the hash.
void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB bb);
void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB _deprecated_ignored);
// Remove an object from the hash.
void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id);

View File

@ -19,8 +19,10 @@
* SOFTWARE.
*/
static const cpVect cpvzero={0.0f,0.0f};
/// Constant for the zero vector.
static const cpVect cpvzero = {0.0f,0.0f};
/// Convenience constructor for cpVect structs.
static inline cpVect
cpv(const cpFloat x, const cpFloat y)
{
@ -29,128 +31,176 @@ cpv(const cpFloat x, const cpFloat y)
}
// non-inlined functions
cpFloat cpvlength(const cpVect v);
cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t);
cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a);
cpVect cpvforangle(const cpFloat a); // convert radians to a normalized vector
cpFloat cpvtoangle(const cpVect v); // convert a vector to radians
char *cpvstr(const cpVect v); // get a string representation of a vector
/// Returns the length of v.
cpFloat cpvlength(const cpVect v);
/// Spherical linearly interpolate between v1 and v2.
cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t);
/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians
cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a);
/// Returns the unit length vector for the given angle (in radians).
cpVect cpvforangle(const cpFloat a);
/// Returns the angular direction v is pointing in (in radians).
cpFloat cpvtoangle(const cpVect v);
/**
Returns a string representation of v. Intended mostly for debugging purposes and not production use.
@attention The string points to a static local and is reset every time the function is called.
If you want to print more than one vector you will have to split up your printing onto separate lines.
*/
char *cpvstr(const cpVect v);
/// Check if two vectors are equal. (Be careful when comparing floating point numbers!)
static inline cpBool
cpveql(const cpVect v1, const cpVect v2)
{
return (v1.x == v2.x && v1.y == v2.y);
}
/// Add two vectors
static inline cpVect
cpvadd(const cpVect v1, const cpVect v2)
{
return cpv(v1.x + v2.x, v1.y + v2.y);
}
/// Negate a vector.
static inline cpVect
cpvneg(const cpVect v)
{
return cpv(-v.x, -v.y);
}
/// Subtract two vectors.
static inline cpVect
cpvsub(const cpVect v1, const cpVect v2)
{
return cpv(v1.x - v2.x, v1.y - v2.y);
}
/// Scalar multiplication.
static inline cpVect
cpvmult(const cpVect v, const cpFloat s)
{
return cpv(v.x*s, v.y*s);
}
/// Vector dot product.
static inline cpFloat
cpvdot(const cpVect v1, const cpVect v2)
{
return v1.x*v2.x + v1.y*v2.y;
}
/**
2D vector cross product analog.
The cross product of 2D vectors results in a 3D vector with only a z component.
This function returns the magnitude of the z value.
*/
static inline cpFloat
cpvcross(const cpVect v1, const cpVect v2)
{
return v1.x*v2.y - v1.y*v2.x;
}
/// Returns a perpendicular vector. (90 degree rotation)
static inline cpVect
cpvperp(const cpVect v)
{
return cpv(-v.y, v.x);
}
/// Returns a perpendicular vector. (-90 degree rotation)
static inline cpVect
cpvrperp(const cpVect v)
{
return cpv(v.y, -v.x);
}
/// Returns the vector projection of v1 onto v2.
static inline cpVect
cpvproject(const cpVect v1, const cpVect v2)
{
return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2));
}
/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector.
static inline cpVect
cpvrotate(const cpVect v1, const cpVect v2)
{
return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x);
}
/// Inverse of cpvrotate().
static inline cpVect
cpvunrotate(const cpVect v1, const cpVect v2)
{
return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y);
}
/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths.
static inline cpFloat
cpvlengthsq(const cpVect v)
{
return cpvdot(v, v);
}
/// Linearly interpolate between v1 and v2.
static inline cpVect
cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t)
{
return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t));
}
/// Returns a normalized copy of v.
static inline cpVect
cpvnormalize(const cpVect v)
{
return cpvmult(v, 1.0f/cpvlength(v));
}
/// Returns a normalized copy of v or cpvzero if v was already cpvzero. Protects against divide by zero errors.
static inline cpVect
cpvnormalize_safe(const cpVect v)
{
return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v));
}
/// Clamp v to length len.
static inline cpVect
cpvclamp(const cpVect v, const cpFloat len)
{
return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
}
/// Linearly interpolate between v1 towards v2 by distance d.
static inline cpVect
cpvlerpconst(cpVect v1, cpVect v2, cpFloat d)
{
return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d));
}
/// Returns the distance between v1 and v2.
static inline cpFloat
cpvdist(const cpVect v1, const cpVect v2)
{
return cpvlength(cpvsub(v1, v2));
}
/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances.
static inline cpFloat
cpvdistsq(const cpVect v1, const cpVect v2)
{
return cpvlengthsq(cpvsub(v1, v2));
}
static inline int
/// Returns true if the distance between v1 and v2 is less than dist.
static inline cpBool
cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist)
{
return cpvdistsq(v1, v2) < dist*dist;

View File

@ -39,7 +39,10 @@ OBJECTS = \
$(OBJECTS_DIR)/cpPolyShape.o \
$(OBJECTS_DIR)/cpShape.o \
$(OBJECTS_DIR)/cpSpace.o \
$(OBJECTS_DIR)/cpSpaceComponent.o \
$(OBJECTS_DIR)/cpSpaceHash.o \
$(OBJECTS_DIR)/cpSpaceQuery.o \
$(OBJECTS_DIR)/cpSpaceStep.o \
$(OBJECTS_DIR)/cpVect.o \
$(OBJECTS_DIR)/cpConstraint.o \
$(OBJECTS_DIR)/cpDampedRotarySpring.o \
@ -100,9 +103,18 @@ $(OBJECTS_DIR)/cpShape.o : ../src/cpShape.c
$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpace.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpace.c
$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceComponent.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceComponent.c
$(OBJECTS_DIR)/cpSpaceHash.o : ../src/cpSpaceHash.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpaceHash.o ../src/cpSpaceHash.c
$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceQuery.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceQuery.c
$(OBJECTS_DIR)/cpSpace.o : ../src/cpSpaceStep.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpSpace.o ../src/cpSpaceStep.c
$(OBJECTS_DIR)/cpVect.o : ../src/cpVect.c
$(CC) -c $(CC_FLAGS) $(INCLUDE_PATH) $(LAST_INCLUDE_PATH) -o $(OBJECTS_DIR)/cpVect.o ../src/cpVect.c

View File

@ -9,7 +9,7 @@ if(BUILD_SHARED)
${chipmunk_source_files}
)
# set the lib's version number
set_target_properties(chipmunk PROPERTIES VERSION 5.1)
set_target_properties(chipmunk PROPERTIES VERSION 5.3.4)
install(TARGETS chipmunk RUNTIME DESTINATION lib LIBRARY DESTINATION lib)
endif(BUILD_SHARED)

View File

@ -21,6 +21,8 @@
#include <stdlib.h>
#include <stdio.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include "chipmunk.h"
@ -43,7 +45,7 @@ cpMessage(const char *message, const char *condition, const char *file, int line
}
char *cpVersionString = "5.x.x";
const char *cpVersionString = "5.3.4";
void
cpInitChipmunk(void)
@ -59,7 +61,13 @@ cpInitChipmunk(void)
cpFloat
cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
{
return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset);
return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset));
}
cpFloat
cpAreaForCircle(cpFloat r1, cpFloat r2)
{
return 2.0f*(cpFloat)M_PI*cpfabs(r1*r1 - r2*r2);
}
cpFloat
@ -68,21 +76,23 @@ cpMomentForSegment(cpFloat m, cpVect a, cpVect b)
cpFloat length = cpvlength(cpvsub(b, a));
cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f);
return m*length*length/12.0f + m*cpvdot(offset, offset);
return m*(length*length/12.0f + cpvlengthsq(offset));
}
cpFloat
cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
{
cpVect *tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
for(int i=0; i<numVerts; i++)
tVerts[i] = cpvadd(verts[i], offset);
return 2.0f*r*((cpFloat)M_PI*r + cpvdist(a, b));
}
cpFloat
cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset)
{
cpFloat sum1 = 0.0f;
cpFloat sum2 = 0.0f;
for(int i=0; i<numVerts; i++){
cpVect v1 = tVerts[i];
cpVect v2 = tVerts[(i+1)%numVerts];
cpVect v1 = cpvadd(verts[i], offset);
cpVect v2 = cpvadd(verts[(i+1)%numVerts], offset);
cpFloat a = cpvcross(v2, v1);
cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
@ -91,15 +101,51 @@ cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset)
sum2 += a;
}
cpfree(tVerts);
return (m*sum1)/(6.0f*sum2);
}
cpFloat
cpAreaForPoly(const int numVerts, const cpVect *verts)
{
cpFloat area = 0.0f;
for(int i=0; i<numVerts; i++){
area += cpvcross(verts[i], verts[(i+1)%numVerts]);
}
return area/2.0f;
}
cpVect
cpCentroidForPoly(const int numVerts, const cpVect *verts)
{
cpFloat sum = 0.0f;
cpVect vsum = cpvzero;
for(int i=0; i<numVerts; i++){
cpVect v1 = verts[i];
cpVect v2 = verts[(i+1)%numVerts];
cpFloat cross = cpvcross(v1, v2);
sum += cross;
vsum = cpvadd(vsum, cpvmult(cpvadd(v1, v2), cross));
}
return cpvmult(vsum, 1.0f/(3.0f*sum));
}
void
cpRecenterPoly(const int numVerts, cpVect *verts){
cpVect centroid = cpCentroidForPoly(numVerts, verts);
for(int i=0; i<numVerts; i++){
verts[i] = cpvsub(verts[i], centroid);
}
}
cpFloat
cpMomentForBox(cpFloat m, cpFloat width, cpFloat height)
{
return m*(width*width + height*height)/12;
return m*(width*width + height*height)/12.0f;
}
#include "chipmunk_ffi.h"

View File

@ -21,7 +21,7 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
// TODO: Comment me!

View File

@ -22,7 +22,7 @@
#include <stdlib.h>
#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static cpFloat
@ -33,12 +33,12 @@ defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
static void
preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
CONSTRAINT_BEGIN(spring, a, b);
spring->iSum = 1.0f/(a->i_inv + b->i_inv);
cpFloat moment = a->i_inv + b->i_inv;
spring->iSum = 1.0f/moment;
spring->dt = dt;
spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
spring->target_wrn = 0.0f;
// apply spring torque
@ -50,15 +50,14 @@ preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpDampedRotarySpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
CONSTRAINT_BEGIN(spring, a, b);
// compute relative velocity
cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum));
cpFloat w_damp = wrn*spring->w_coef;
spring->target_wrn = wrn - w_damp;
//apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));

View File

@ -22,7 +22,7 @@
#include <stdlib.h>
#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static cpFloat
@ -33,8 +33,7 @@ defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
CONSTRAINT_BEGIN(spring, a, b);
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
@ -43,11 +42,11 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
// calculate mass normal
spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->nMass = 1.0f/k;
spring->dt = dt;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
@ -57,8 +56,7 @@ preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpDampedSpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
CONSTRAINT_BEGIN(spring, a, b);
cpVect n = spring->n;
cpVect r1 = spring->r1;
@ -69,7 +67,7 @@ applyImpulse(cpDampedSpring *spring)
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
cpFloat v_damp = -vrn*spring->v_coef;
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// calculate moment of inertia coefficient.
joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv);
@ -49,8 +48,7 @@ preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpGearJoint *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// compute relative rotational velocity
cpFloat wr = b->w*joint->ratio - a->w;
@ -111,4 +109,5 @@ cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
cpConstraintCheckCast(constraint, cpGearJoint);
((cpGearJoint *)constraint)->ratio = value;
((cpGearJoint *)constraint)->ratio_inv = 1.0f/value;
cpConstraintActivateBodies(constraint);
}

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// calculate endpoints in worldspace
cpVect ta = cpBodyLocal2World(a, joint->grv_a);
@ -79,8 +78,7 @@ grooveConstrain(cpGrooveJoint *joint, cpVect j){
static void
applyImpulse(cpGrooveJoint *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
@ -136,3 +134,28 @@ cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect
{
return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
}
void
cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
{
cpGrooveJoint *g = (cpGrooveJoint *)constraint;
cpConstraintCheckCast(constraint, cpGrooveJoint);
g->grv_a = value;
g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
cpConstraintActivateBodies(constraint);
}
void
cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
{
cpGrooveJoint *g = (cpGrooveJoint *)constraint;
cpConstraintCheckCast(constraint, cpGrooveJoint);
g->grv_b = value;
g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
cpConstraintActivateBodies(constraint);
}

View File

@ -22,14 +22,13 @@
#include <stdlib.h>
//#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -56,8 +55,7 @@ preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpPinJoint *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpVect n = joint->n;
// compute relative velocity
@ -101,8 +99,9 @@ cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect an
joint->anchr1 = anchr1;
joint->anchr2 = anchr2;
cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot));
cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot));
// STATIC_BODY_CHECK
cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1);
cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2);
joint->dist = cpvlength(cpvsub(p2, p1));
joint->jnAcc = 0.0f;

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -50,8 +49,7 @@ preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpPivotJoint *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
@ -110,5 +108,7 @@ cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
cpConstraint *
cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
{
return cpPivotJointNew2(a, b, cpBodyWorld2Local(a, pivot), cpBodyWorld2Local(b, pivot));
cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
return cpPivotJointNew2(a, b, anchr1, anchr2);
}

View File

@ -19,18 +19,16 @@
* SOFTWARE.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpFloat angle = joint->angle;
cpFloat phase = joint->phase;
@ -70,8 +68,7 @@ applyImpulse(cpRatchetJoint *joint)
{
if(!joint->bias) return; // early exit
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// compute relative rotational velocity
cpFloat wr = b->w - a->w;
@ -116,7 +113,8 @@ cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, c
joint->phase = phase;
joint->ratchet = ratchet;
joint->angle = b->a - a->a;
// STATIC_BODY_CHECK
joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f);
return joint;
}

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpFloat dist = b->a - a->a;
cpFloat pdist = 0.0f;
@ -62,8 +61,7 @@ applyImpulse(cpRotaryLimitJoint *joint)
{
if(!joint->bias) return; // early exit
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// compute relative rotational velocity
cpFloat wr = b->w - a->w;

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// calculate moment of inertia coefficient.
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
@ -44,8 +43,7 @@ preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
static void
applyImpulse(cpSimpleMotor *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
// compute relative rotational velocity
cpFloat wr = b->w - a->w + joint->rate;

View File

@ -21,14 +21,13 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
static void
preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
@ -69,8 +68,7 @@ applyImpulse(cpSlideJoint *joint)
{
if(!joint->bias) return; // early exit
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
CONSTRAINT_BEGIN(joint, a, b);
cpVect n = joint->n;
cpVect r1 = joint->r1;

View File

@ -21,7 +21,7 @@
#include <stdlib.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "constraints/util.h"
cpFloat cp_bias_coef = 0.1f;
@ -104,13 +104,20 @@ cpArbiterAlloc(void)
cpArbiter*
cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
{
arb->handler = NULL;
arb->swappedColl = cpFalse;
arb->e = 0.0f;
arb->u = 0.0f;
arb->surface_vr = cpvzero;
arb->numContacts = 0;
arb->contacts = NULL;
arb->private_a = a;
arb->private_b = b;
arb->a = a;
arb->b = b;
arb->stamp = -1;
arb->stamp = 0;
arb->state = cpArbiterStateFirstColl;
return arb;
@ -157,8 +164,6 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio
}
}
}
// cpfree(arb->contacts);
}
arb->contacts = contacts;
@ -172,17 +177,18 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio
arb->surface_vr = cpvsub(a->surface_v, b->surface_v);
// For collisions between two similar primitive types, the order could have been swapped.
arb->private_a = a; arb->private_b = b;
arb->a = a;
arb->b = b;
// mark it as new if it's been cached
if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
cpShape *shapea = arb->private_a;
cpShape *shapeb = arb->private_b;
cpBody *a = shapea->body;
cpBody *b = shapeb->body;
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
@ -207,8 +213,8 @@ cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
void
cpArbiterApplyCachedImpulse(cpArbiter *arb)
{
cpShape *shapea = arb->private_a;
cpShape *shapeb = arb->private_b;
cpShape *shapea = arb->a;
cpShape *shapeb = arb->b;
arb->u = shapea->u * shapeb->u;
arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v);
@ -225,8 +231,8 @@ cpArbiterApplyCachedImpulse(cpArbiter *arb)
void
cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
{
cpBody *a = arb->private_a->body;
cpBody *b = arb->private_b->body;
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];

View File

@ -22,7 +22,7 @@
#include <stdlib.h>
#include <string.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
//#define CP_ARRAY_INCREMENT 10
@ -57,6 +57,7 @@ void
cpArrayDestroy(cpArray *arr)
{
cpfree(arr->arr);
arr->arr = NULL;
}
void
@ -111,6 +112,20 @@ cpArrayDeleteObj(cpArray *arr, void *obj)
}
}
void
cpArrayAppend(cpArray *arr, cpArray *other)
{
void *tail = &arr->arr[arr->num];
arr->num += other->num;
if(arr->num >= arr->max){
arr->max = arr->num;
arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**));
}
memcpy(tail, other->arr, other->num*sizeof(void**));
}
void
cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
{
@ -118,11 +133,11 @@ cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)
iterFunc(arr->arr[i], data);
}
int
cpBool
cpArrayContains(cpArray *arr, void *ptr)
{
for(int i=0; i<arr->num; i++)
if(arr->arr[i] == ptr) return 1;
if(arr->arr[i] == ptr) return cpTrue;
return 0;
return cpFalse;
}

View File

@ -21,8 +21,12 @@
#include <stdlib.h>
#include <float.h>
#include <stdarg.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
// initialized in cpInitChipmunk()
cpBody cpStaticBodySingleton;
cpBody*
cpBodyAlloc(void)
@ -56,7 +60,12 @@ cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
body->data = NULL;
body->v_limit = (cpFloat)INFINITY;
body->w_limit = (cpFloat)INFINITY;
// body->active = 1;
body->space = NULL;
body->shapesList = NULL;
cpComponentNode node = {NULL, NULL, 0, 0.0f};
body->node = node;
return body;
}
@ -67,6 +76,21 @@ cpBodyNew(cpFloat m, cpFloat i)
return cpBodyInit(cpBodyAlloc(), m, i);
}
cpBody *
cpBodyInitStatic(cpBody *body)
{
cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY);
body->node.idleTime = (cpFloat)INFINITY;
return body;
}
cpBody *
cpBodyNewStatic()
{
return cpBodyInitStatic(cpBodyAlloc());
}
void cpBodyDestroy(cpBody *body){}
void
@ -166,23 +190,3 @@ cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat
cpBodyApplyForce(a, f, r1);
cpBodyApplyForce(b, cpvneg(f), r2);
}
//int
//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max)
//{
// cpFloat ke = body->m*cpvdot(body->v, body->v);
// cpFloat re = body->i*body->w*body->w;
//
// if(ke + re > body->m*dvsq)
// body->active = 1;
// else if(body->active)
// body->active = (body->active + 1)%(max + 1);
// else {
// body->v = cpvzero;
// body->v_bias = cpvzero;
// body->w = 0.0f;
// body->w_bias = 0.0f;
// }
//
// return body->active;
//}

View File

@ -21,16 +21,16 @@
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
//#include <stdio.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
typedef int (*collisionFunc)(cpShape *, cpShape *, cpContact *);
typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *);
// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
@ -38,14 +38,12 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
if(distsq >= mindist*mindist) return 0;
cpFloat dist = cpfsqrt(distsq);
// To avoid singularities, do nothing in the case of dist = 0.
cpFloat non_zero_dist = (dist ? dist : INFINITY);
// Allocate and initialize the contact.
cpContactInit(
con,
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
cpvmult(delta, 1.0f/non_zero_dist),
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
dist - mindist,
0
);
@ -55,7 +53,7 @@ circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
// Collide circle shapes.
static int
circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr)
circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
{
cpCircleShape *circ1 = (cpCircleShape *)shape1;
cpCircleShape *circ2 = (cpCircleShape *)shape2;
@ -65,7 +63,7 @@ circle2circle(cpShape *shape1, cpShape *shape2, cpContact *arr)
// Collide circles to segment shapes.
static int
circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con)
circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con)
{
cpCircleShape *circ = (cpCircleShape *)circleShape;
cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
@ -118,17 +116,19 @@ circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact *con)
static cpContact *
nextContactPoint(cpContact *arr, int *numPtr)
{
int num = *numPtr;
int index = *numPtr;
if(num <= CP_MAX_CONTACTS_PER_ARBITER)
(*numPtr) = num + 1;
return &arr[num];
if(index < CP_MAX_CONTACTS_PER_ARBITER){
(*numPtr) = index + 1;
return &arr[index];
} else {
return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1];
}
}
// Find the minimum separating axis for the give poly and axis list.
static inline int
findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out)
findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out)
{
int min_index = 0;
cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d);
@ -148,9 +148,11 @@ findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out)
return min_index;
}
// Add contacts for penetrating vertexes.
// Add contacts for probably penetrating vertexes.
// This handles the degenerate case where an overlap was detected, but no vertexes fall inside
// the opposing polygon. (like a star of david)
static inline int
findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist)
findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
{
int num = 0;
@ -166,15 +168,33 @@ findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFl
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
}
// if(!num)
// addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0));
return num;
}
// Add contacts for penetrating vertexes.
static inline int
findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
{
int num = 0;
for(int i=0; i<poly1->numVerts; i++){
cpVect v = poly1->tVerts[i];
if(cpPolyShapeContainsVert(poly2, v))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i));
}
for(int i=0; i<poly2->numVerts; i++){
cpVect v = poly2->tVerts[i];
if(cpPolyShapeContainsVert(poly1, v))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
}
return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
}
// Collide poly shapes together.
static int
poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
{
cpPolyShape *poly1 = (cpPolyShape *)shape1;
cpPolyShape *poly2 = (cpPolyShape *)shape2;
@ -196,7 +216,7 @@ poly2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
// Like cpPolyValueOnAxis(), but for segments.
static inline cpFloat
segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)
segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d)
{
cpFloat a = cpvdot(n, seg->ta) - seg->r;
cpFloat b = cpvdot(n, seg->tb) - seg->r;
@ -205,7 +225,7 @@ segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)
// Identify vertexes that have penetrated the segment.
static inline void
findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef)
findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef)
{
cpFloat dta = cpvcross(seg->tn, seg->ta);
cpFloat dtb = cpvcross(seg->tn, seg->tb);
@ -225,7 +245,7 @@ findPointsBehindSeg(cpContact *arr, int *num, cpSegmentShape *seg, cpPolyShape *
// This one is complicated and gross. Just don't go there...
// TODO: Comment me!
static int
seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
{
cpSegmentShape *seg = (cpSegmentShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
@ -294,7 +314,7 @@ seg2poly(cpShape *shape1, cpShape *shape2, cpContact *arr)
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con)
circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
@ -352,7 +372,7 @@ circle2poly(cpShape *shape1, cpShape *shape2, cpContact *con)
static collisionFunc *colfuncs = NULL;
static void
addColFunc(cpShapeType a, cpShapeType b, collisionFunc func)
addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func)
{
colfuncs[a + b*CP_NUM_SHAPES] = func;
}
@ -381,7 +401,7 @@ extern "C" {
#endif
int
cpCollideShapes(cpShape *a, cpShape *b, cpContact *arr)
cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
{
// Their shape types must be in order.
cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted.");

View File

@ -22,7 +22,7 @@
#include <stdlib.h>
#include <assert.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "prime.h"
static void freeWrap(void *ptr, void *unused){cpfree(ptr);}

View File

@ -20,9 +20,8 @@
*/
#include <stdlib.h>
#include <stdio.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "chipmunk_unsafe.h"
cpPolyShape *
@ -94,7 +93,7 @@ cpPolyShapeDestroy(cpShape *shape)
cpfree(poly->tAxes);
}
static int
static cpBool
cpPolyShapePointQuery(cpShape *shape, cpVect p){
return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p);
}
@ -137,8 +136,8 @@ static const cpShapeClass polyClass = {
cpPolyShapeSegmentQuery,
};
int
cpPolyValidate(cpVect *verts, int numVerts)
cpBool
cpPolyValidate(const cpVect *verts, const int numVerts)
{
for(int i=0; i<numVerts; i++){
cpVect a = verts[i];
@ -146,10 +145,10 @@ cpPolyValidate(cpVect *verts, int numVerts)
cpVect c = verts[(i+2)%numVerts];
if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
return 0;
return cpFalse;
}
return 1;
return cpTrue;
}
int
@ -211,8 +210,8 @@ cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset)
cpPolyShape *
cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height)
{
cpFloat hw = width/2;
cpFloat hh = height/2;
cpFloat hw = width/2.0f;
cpFloat hh = height/2.0f;
cpVect verts[] = {
cpv(-hw,-hh),

View File

@ -23,7 +23,7 @@
#include <stdio.h>
#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "chipmunk_unsafe.h"
#define CP_DefineShapeGetter(struct, type, member, name) \
@ -60,8 +60,9 @@ cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body)
shape->layers = CP_ALL_LAYERS;
shape->data = NULL;
shape->next = NULL;
cpShapeCacheBB(shape);
// cpShapeCacheBB(shape);
return shape;
}
@ -81,6 +82,7 @@ cpShapeFree(cpShape *shape)
}
}
// TODO this function should really take a position and rotation explicitly and be renamed
cpBB
cpShapeCacheBB(cpShape *shape)
{
@ -90,12 +92,12 @@ cpShapeCacheBB(cpShape *shape)
return shape->bb;
}
int
cpBool
cpShapePointQuery(cpShape *shape, cpVect p){
return shape->klass->pointQuery(shape, p);
}
int
cpBool
cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero};
(*info) = blank;
@ -138,7 +140,7 @@ cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
return bbFromCircle(circle->tc, circle->r);
}
static int
static cpBool
cpCircleShapePointQuery(cpShape *shape, cpVect p){
cpCircleShape *circle = (cpCircleShape *)shape;
return cpvnear(circle->tc, p, circle->r);
@ -147,7 +149,7 @@ cpCircleShapePointQuery(cpShape *shape, cpVect p){
static void
circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
// umm... gross I normally frown upon such things
// offset the line to be relative to the circle
a = cpvsub(a, center);
b = cpvsub(b, center);
@ -239,16 +241,16 @@ cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
return cpBBNew(l - rad, s - rad, r + rad, t + rad);
}
static int
static cpBool
cpSegmentShapePointQuery(cpShape *shape, cpVect p){
if(!cpBBcontainsVect(shape->bb, p)) return 0;
if(!cpBBcontainsVect(shape->bb, p)) return cpFalse;
cpSegmentShape *seg = (cpSegmentShape *)shape;
// Calculate normal distance from segment.
cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
cpFloat dist = cpfabs(dn) - seg->r;
if(dist > 0.0f) return 0;
if(dist > 0.0f) return cpFalse;
// Calculate tangential distance along segment.
cpFloat dt = -cpvcross(seg->tn, p);
@ -258,28 +260,32 @@ cpSegmentShapePointQuery(cpShape *shape, cpVect p){
// Decision tree to decide which feature of the segment to collide with.
if(dt <= dtMin){
if(dt < (dtMin - seg->r)){
return 0;
return cpFalse;
} else {
return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
}
} else {
if(dt < dtMax){
return 1;
return cpTrue;
} else {
if(dt < (dtMax + seg->r)) {
return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
} else {
return 0;
return cpFalse;
}
}
}
return 1;
return cpTrue;
}
static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
static void
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
// TODO this function could be optimized better.
cpSegmentShape *seg = (cpSegmentShape *)shape;
cpVect n = seg->tn;
// flip n if a is behind the axis
@ -288,40 +294,37 @@ cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInf
cpFloat an = cpvdot(a, n);
cpFloat bn = cpvdot(b, n);
cpFloat d = cpvdot(seg->ta, n) + seg->r;
cpFloat t = (d - an)/(bn - an);
if(0.0f < t && t < 1.0f){
cpVect point = cpvlerp(a, b, t);
cpFloat dt = -cpvcross(seg->tn, point);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
if(an != bn){
cpFloat d = cpvdot(seg->ta, n) + seg->r;
cpFloat t = (d - an)/(bn - an);
if(dtMin < dt && dt < dtMax){
info->shape = shape;
info->t = t;
info->n = n;
if(0.0f < t && t < 1.0f){
cpVect point = cpvlerp(a, b, t);
cpFloat dt = -cpvcross(seg->tn, point);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
return; // don't continue on and check endcaps
if(dtMin < dt && dt < dtMax){
info->shape = shape;
info->t = t;
info->n = n;
return; // don't continue on and check endcaps
}
}
}
if(seg->r) {
cpSegmentQueryInfo info1; info1.shape = NULL;
cpSegmentQueryInfo info2; info2.shape = NULL;
cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero};
cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero};
circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);
if(info1.shape && !info2.shape){
if(info1.t < info2.t){
(*info) = info1;
} else if(info2.shape && !info1.shape){
} else {
(*info) = info2;
} else if(info1.shape && info2.shape){
if(info1.t < info2.t){
(*info) = info1;
} else {
(*info) = info2;
}
}
}
}

View File

@ -20,24 +20,24 @@
*/
#include <stdlib.h>
#include <stdio.h>
//#include <stdio.h>
#include <string.h>
#include <math.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
int cp_contact_persistence = 1;
cpTimestamp cp_contact_persistence = 3;
#pragma mark Contact Set Helpers
// Equal function for contactSet.
static int
static cpBool
contactSetEql(cpShape **shapes, cpArbiter *arb)
{
cpShape *a = shapes[0];
cpShape *b = shapes[1];
return ((a == arb->private_a && b == arb->private_b) || (b == arb->private_a && a == arb->private_b));
return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
}
// Transformation function for contactSet.
@ -55,13 +55,13 @@ contactSetTrans(cpShape **shapes, cpSpace *space)
for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
}
return cpArbiterInit( (cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
}
#pragma mark Collision Pair Function Helpers
// Equals function for collFuncSet.
static int
static cpBool
collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
{
return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b));
@ -77,32 +77,10 @@ collFuncSetTrans(cpCollisionHandler *handler, void *unused)
return copy;
}
#pragma mark Post Step Function Helpers
typedef struct postStepCallback {
cpPostStepFunc func;
void *obj;
void *data;
} postStepCallback;
static int
postStepFuncSetEql(postStepCallback *a, postStepCallback *b){
return a->obj == b->obj;
}
static void *
postStepFuncSetTrans(postStepCallback *callback, void *ignored)
{
postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback));
(*value) = (*callback);
return value;
}
#pragma mark Misc Helper Funcs
// Default collision functions.
static int alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
static void nothing(cpArbiter *arb, cpSpace *space, void *data){}
// BBfunc callback for the spatial hash.
@ -116,30 +94,6 @@ static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFr
#pragma mark Memory Management Functions
#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
typedef struct cpContactBuffer {
cpContactBufferHeader header;
cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
} cpContactBuffer;
static cpContactBufferHeader *
cpSpaceAllocContactBuffer(cpSpace *space)
{
cpContactBuffer *buffer = (cpContactBuffer *)malloc(sizeof(cpContactBuffer));
cpArrayPush(space->allocatedBuffers, buffer);
return (cpContactBufferHeader *)buffer;
}
static cpContactBufferHeader *
cpContactBufferHeaderInit(cpContactBufferHeader *header, cpSpace *space)
{
header->stamp = space->stamp;
header->next = space->contactBuffersTail;
header->numContacts = 0;
return header;
}
cpSpace *
cpSpaceAlloc(void)
{
@ -151,8 +105,6 @@ cpSpaceAlloc(void)
#define DEFAULT_ITERATIONS 10
#define DEFAULT_ELASTIC_ITERATIONS 0
#define MAX_CONTACTS 10000
cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL};
cpSpace*
@ -174,14 +126,16 @@ cpSpaceInit(cpSpace *space)
space->allocatedBuffers = cpArrayNew(0);
space->bodies = cpArrayNew(0);
space->sleepingComponents = cpArrayNew(0);
space->rousedBodies = cpArrayNew(0);
space->sleepTimeThreshold = INFINITY;
space->idleSpeedThreshold = 0.0f;
space->arbiters = cpArrayNew(0);
space->pooledArbiters = cpArrayNew(0);
cpContactBufferHeader *header = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), space);
space->contactBuffersHead = header;
space->contactBuffersTail = header;
header->next = header; // Buffers will form a ring, start the ring explicitly
space->contactBuffersHead = NULL;
space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans);
space->constraints = cpArrayNew(0);
@ -190,7 +144,9 @@ cpSpaceInit(cpSpace *space)
space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans);
space->collFuncSet->default_value = &space->defaultHandler;
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
space->postStepCallbacks = NULL;
cpBodyInitStatic(&space->staticBody);
return space;
}
@ -208,6 +164,8 @@ cpSpaceDestroy(cpSpace *space)
cpSpaceHashFree(space->activeShapes);
cpArrayFree(space->bodies);
cpArrayFree(space->sleepingComponents);
cpArrayFree(space->rousedBodies);
cpArrayFree(space->constraints);
@ -244,6 +202,9 @@ cpSpaceFree(cpSpace *space)
void
cpSpaceFreeChildren(cpSpace *space)
{
cpArray *components = space->sleepingComponents;
while(components->num) cpBodyActivate((cpBody *)components->arr[0]);
cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL);
cpArrayEach(space->bodies, (cpArrayIter)&bodyFreeWrap, NULL);
@ -310,31 +271,62 @@ cpSpaceSetDefaultCollisionHandler(
#define cpAssertSpaceUnlocked(space) \
cpAssert(!space->locked, \
"This addition/removal cannot be done safely during a call to cpSpaceStep(). " \
"Put these calls into a Post Step Callback." \
"This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \
"Put these calls into a post-step callback." \
);
static void
cpBodyAddShape(cpBody *body, cpShape *shape)
{
shape->next = shape->body->shapesList;
shape->body->shapesList = shape;
}
static void
cpBodyRemoveShape(cpBody *body, cpShape *shape)
{
cpShape **prev_ptr = &body->shapesList;
cpShape *node = body->shapesList;
while(node && node != shape){
prev_ptr = &node->next;
node = node->next;
}
cpAssert(node, "Attempted to remove a shape from a body it was never attached to.");
(*prev_ptr) = node->next;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpAssert(shape->body, "Cannot add a shape with a NULL body.");
cpBody *body = shape->body;
if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot add the same shape more than once.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
cpShapeCacheBB(shape);
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
return shape;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpAssert(shape->body, "Cannot add a static shape with a NULL body.");
cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot add the same static shape more than once.");
cpAssertSpaceUnlocked(space);
if(!shape->body) shape->body = &space->staticBody;
cpShapeCacheBB(shape);
cpSpaceActivateShapesTouchingShape(space, shape);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
return shape;
@ -343,10 +335,12 @@ cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
cpAssert(!body->space, "Cannot add a body to a more than one space or to the same space twice.");
// cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
cpArrayPush(space->bodies, body);
body->space = space;
return body;
}
@ -357,6 +351,11 @@ cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once.");
// cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
if(!constraint->a) constraint->a = &space->staticBody;
if(!constraint->b) constraint->b = &space->staticBody;
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayPush(space->constraints, constraint);
return constraint;
@ -368,24 +367,37 @@ typedef struct removalContext {
} removalContext;
// Hashset filter func to throw away old arbiters.
static int
static cpBool
contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
{
if(context->shape == arb->private_a || context->shape == arb->private_b){
arb->handler->separate(arb, context->space, arb->handler->data);
if(context->shape == arb->a || context->shape == arb->b){
if(arb->state != cpArbiterStateCached){
arb->handler->separate(arb, context->space, arb->handler->data);
}
cpArrayPush(context->space->pooledArbiters, arb);
return 0;
return cpFalse;
}
return 1;
return cpTrue;
}
void
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot remove a shape that was never added to the space. (Removed twice maybe?)");
cpBody *body = shape->body;
if(cpBodyIsStatic(body)){
cpSpaceRemoveStaticShape(space, shape);
return;
}
cpBodyActivate(body);
cpAssertSpaceUnlocked(space);
cpAssertWarn(cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
"Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
cpBodyRemoveShape(body, shape);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
@ -396,238 +408,43 @@ void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertWarn(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot remove a static shape that was never added to the space. (Removed twice maybe?)");
"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
cpSpaceActivateShapesTouchingShape(space, shape);
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssertWarn(cpArrayContains(space->bodies, body),
"Cannot remove a body that was never added to the space. (Removed twice maybe?)");
cpAssertWarn(body->space == space,
"Cannot remove a body that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpArrayDeleteObj(space->bodies, body);
body->space = NULL;
}
void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertWarn(cpArrayContains(space->constraints, constraint),
"Cannot remove a constraint that was never added to the space. (Removed twice maybe?)");
"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayDeleteObj(space->constraints, constraint);
}
#pragma mark Post Step Functions
void
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
{
postStepCallback callback = {func, obj, data};
cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL);
}
static void
removeAndFreeShapeAndBody(cpShape *shape, cpSpace *space)
{
cpSpaceRemoveShape(space, shape);
cpShapeFree(shape);
}
void
cpSpacePostStepRemoveAndFreeShapeAndBody(cpSpace *space, cpShape *shape)
{
cpSpaceAddPostStepCallback(space, (cpPostStepFunc)removeAndFreeShapeAndBody, shape, space);
}
#pragma mark Point Query Functions
typedef struct pointQueryContext {
cpLayers layers;
cpGroup group;
cpSpacePointQueryFunc func;
void *data;
} pointQueryContext;
static void
pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
{
if(
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
cpShapePointQuery(shape, *point)
){
context->func(shape, context->data);
}
}
void
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
{
pointQueryContext context = {layers, group, func, data};
cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
}
static void
rememberLastPointQuery(cpShape *shape, cpShape **outShape)
{
(*outShape) = shape;
}
cpShape *
cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
{
cpShape *shape = NULL;
cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape);
return shape;
}
void
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
{
cpArray *bodies = space->bodies;
for(int i=0; i<bodies->num; i++)
func((cpBody *)bodies->arr[i], data);
}
#pragma mark Segment Query Functions
typedef struct segQueryContext {
cpVect start, end;
cpLayers layers;
cpGroup group;
cpSpaceSegmentQueryFunc func;
int anyCollision;
} segQueryContext;
static cpFloat
segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
{
cpSegmentQueryInfo info;
if(
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
cpShapeSegmentQuery(shape, context->start, context->end, &info)
){
if(context->func){
context->func(shape, info.t, info.n, data);
}
context->anyCollision = 1;
}
return 1.0f;
}
int
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
{
segQueryContext context = {
start, end,
layers, group,
func,
0,
};
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
return context.anyCollision;
}
typedef struct segQueryFirstContext {
cpVect start, end;
cpLayers layers;
cpGroup group;
} segQueryFirstContext;
static cpFloat
segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out)
{
cpSegmentQueryInfo info;// = {NULL, 1.0f, cpvzero};
if(
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
cpShapeSegmentQuery(shape, context->start, context->end, &info)
){
if(info.t < out->t){
out->shape = info.shape;
out->t = info.t;
out->n = info.n;
}
return info.t;
}
return 1.0f;
}
cpShape *
cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
{
cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
if(out){
(*out) = info;
} else {
out = &info;
}
out->t = 1.0f;
segQueryFirstContext context = {
start, end,
layers, group
};
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
return out->shape;
}
#pragma mark BB Query functions
typedef struct bbQueryContext {
cpLayers layers;
cpGroup group;
cpSpaceBBQueryFunc func;
void *data;
} bbQueryContext;
static void
bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
{
if(
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
cpBBintersects(*bb, shape->bb)
){
context->func(shape, context->data);
}
}
void
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
{
bbQueryContext context = {layers, group, func, data};
cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
}
#pragma mark Spatial Hash Management
// Iterator function used for updating shape BBoxes.
static void
updateBBCache(cpShape *shape, void *unused)
{
cpShapeCacheBB(shape);
}
static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
void
cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
@ -649,251 +466,34 @@ cpSpaceRehashStatic(cpSpace *space)
cpSpaceHashRehash(space->staticShapes);
}
#pragma mark Collision Detection Functions
static cpContactBufferHeader *
cpSpaceGetFreeContactBuffer(cpSpace *space)
void
cpSpaceRehashShape(cpSpace *space, cpShape *shape)
{
if(space->stamp - space->contactBuffersTail->stamp > cp_contact_persistence){
cpContactBufferHeader *header = space->contactBuffersTail;
space->contactBuffersTail = header->next;
cpShapeCacheBB(shape);
return cpContactBufferHeaderInit(header, space);
} else {
cpContactBufferHeader *header = cpSpaceAllocContactBuffer(space);
return cpContactBufferHeaderInit(header, space);
}
// attempt to rehash the shape in both hashes
cpSpaceHashRehashObject(space->activeShapes, shape, shape->hashid);
cpSpaceHashRehashObject(space->staticShapes, shape, shape->hashid);
}
static void
cpSpacePushNewContactBuffer(cpSpace *space)
{
// for(cpContactBuffer *buffer = space->contactBuffersTail; buffer != space->contactBuffersHead; buffer = buffer->next){
// printf("%p -> ", buffer);
// }
// printf("%p (head)\n", space->contactBuffersHead);
cpContactBufferHeader *buffer = cpSpaceGetFreeContactBuffer(space);
space->contactBuffersHead->next = buffer;
space->contactBuffersHead = buffer;
}
static inline int
queryReject(cpShape *a, cpShape *b)
{
return
// BBoxes must overlap
!cpBBintersects(a->bb, b->bb)
// Don't collide shapes attached to the same body.
|| a->body == b->body
// Don't collide objects in the same non-zero group
|| (a->group && b->group && a->group == b->group)
// Don't collide objects that don't share at least on layer.
|| !(a->layers & b->layers);
}
// Callback from the spatial hash.
static void
queryFunc(cpShape *a, cpShape *b, cpSpace *space)
{
// Reject any of the simple cases
if(queryReject(a,b)) return;
// Find the collision pair function for the shapes.
struct{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type};
cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids);
int sensor = a->sensor || b->sensor;
if(sensor && handler == &space->defaultHandler) return;
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
if(a->klass->type > b->klass->type){
cpShape *temp = a;
a = b;
b = temp;
}
if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
// contact buffer could overflow on the next collision, push a fresh one.
cpSpacePushNewContactBuffer(space);
}
// Narrow-phase collision detection.
cpContact *contacts = ((cpContactBuffer *)(space->contactBuffersHead))->contacts + space->contactBuffersHead->numContacts;
int numContacts = cpCollideShapes(a, b, contacts);
if(!numContacts) return; // Shapes are not colliding.
space->contactBuffersHead->numContacts += numContacts;
// Get an arbiter from space->contactSet for the two shapes.
// This is where the persistant contact magic comes from.
cpShape *shape_pair[] = {a, b};
cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space);
cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
// Call the begin function first if it's the first step
if(arb->stamp == -1 && !handler->begin(arb, space, handler->data)){
cpArbiterIgnore(arb); // permanently ignore the collision until separation
}
if(
// Ignore the arbiter if it has been flagged
(arb->state != cpArbiterStateIgnore) &&
// Call preSolve
handler->preSolve(arb, space, handler->data) &&
// Process, but don't add collisions for sensors.
!sensor
){
cpArrayPush(space->arbiters, arb);
} else {
// cpfree(arb->contacts);
space->contactBuffersHead->numContacts -= numContacts;
arb->contacts = NULL;
arb->numContacts = 0;
}
// Time stamp the arbiter so we know it was used recently.
arb->stamp = space->stamp;
}
// Iterator for active/static hash collisions.
static void
active2staticIter(cpShape *shape, cpSpace *space)
{
cpSpaceHashQuery(space->staticShapes, shape, shape->bb, (cpSpaceHashQueryFunc)queryFunc, space);
}
// Hashset filter func to throw away old arbiters.
static int
contactSetFilter(cpArbiter *arb, cpSpace *space)
{
int ticks = space->stamp - arb->stamp;
// was used last frame, but not this one
if(ticks == 1){
arb->handler->separate(arb, space, arb->handler->data);
arb->stamp = -1; // mark it as a new pair again.
}
if(ticks >= cp_contact_persistence){
cpArrayPush(space->pooledArbiters, arb);
return 0;
}
return 1;
}
// Hashset filter func to call and throw away post step callbacks.
static int
postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space)
{
callback->func(space, callback->obj, callback->data);
cpfree(callback);
return 0;
}
#pragma mark All Important cpSpaceStep() Function
void
cpSpaceStep(cpSpace *space, cpFloat dt)
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
{
if(!dt) return; // don't step if the timestep is 0!
cpFloat dt_inv = 1.0f/dt;
cpArray *bodies = space->bodies;
cpArray *constraints = space->constraints;
space->locked = 1;
// Empty the arbiter list.
space->arbiters->num = 0;
// Integrate positions.
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
func((cpBody *)bodies->arr[i], data);
}
// Pre-cache BBoxes and shape data.
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
// Collide!
cpSpacePushNewContactBuffer(space);
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
// Clear out old cached arbiters and dispatch untouch functions
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
// Prestep the arbiters.
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++)
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
// Prestep the constraints.
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->preStep(constraint, dt, dt_inv);
cpArray *components = space->sleepingComponents;
for(int i=0; i<components->num; i++){
cpBody *root = (cpBody *)components->arr[i];
cpBody *body = root, *next;
do {
next = body->node.next;
func(body, data);
} while((body = next) != root);
}
for(int i=0; i<space->elasticIterations; i++){
for(int j=0; j<arbiters->num; j++)
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
// Integrate velocities.
cpFloat damping = cpfpow(1.0f/space->damping, -dt);
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, space->gravity, damping, dt);
}
for(int i=0; i<arbiters->num; i++)
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
// run the old-style elastic solver if elastic iterations are disabled
cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
// Run the impulse solver.
for(int i=0; i<space->iterations; i++){
for(int j=0; j<arbiters->num; j++)
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef);
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
space->locked = 0;
// run the post solve callbacks
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
arb->state = cpArbiterStateNormal;
}
// Run the post step callbacks
// Use filter as an easy way to clear out the queue as it runs
cpHashSetFilter(space->postStepCallbacks, (cpHashSetFilterFunc)postStepCallbackSetFilter, space);
// cpFloat dvsq = cpvdot(space->gravity, space->gravity);
// dvsq *= dt*dt * space->damping*space->damping;
// for(int i=0; i<bodies->num; i++)
// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);
// Increment the stamp.
space->stamp++;
}

View File

@ -21,9 +21,8 @@
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "chipmunk.h"
#include "chipmunk_private.h"
#include "prime.h"
static cpHandle*
@ -36,11 +35,7 @@ cpHandleInit(cpHandle *hand, void *obj)
return hand;
}
static inline void
cpHandleRetain(cpHandle *hand)
{
hand->retain++;
}
static inline void cpHandleRetain(cpHandle *hand){hand->retain++;}
static inline void
cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
@ -55,7 +50,7 @@ cpSpaceHashAlloc(void)
return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash));
}
// Frees the old table, and allocates a new one.
// Frees the old table, and allocate a new one.
static void
cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
{
@ -66,12 +61,7 @@ cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
}
// Equality function for the handleset.
static int
handleSetEql(void *obj, void *elt)
{
cpHandle *hand = (cpHandle *)elt;
return (obj == hand->obj);
}
static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);}
// Transformation function for the handleset.
static void *
@ -101,7 +91,7 @@ cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBF
hash->celldim = celldim;
hash->bbfunc = bbfunc;
hash->handleSet = cpHashSetNew(0, handleSetEql, (cpHashSetTransFunc)handleSetTrans);
hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, (cpHashSetTransFunc)handleSetTrans);
hash->pooledHandles = cpArrayNew(0);
hash->pooledBins = NULL;
@ -132,7 +122,6 @@ clearHashCell(cpSpaceHash *hash, int idx)
while(bin){
cpSpaceHashBin *next = bin->next;
// Release the lock on the handle.
cpHandleRelease(bin->handle, hash->pooledHandles);
recycleBin(hash, bin);
@ -186,15 +175,15 @@ cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
}
// Return true if the chain contains the handle.
static inline int
static inline cpBool
containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
{
while(bin){
if(bin->handle == hand) return 1;
if(bin->handle == hand) return cpTrue;
bin = bin->next;
}
return 0;
return cpFalse;
}
// Get a recycled or new bin.
@ -266,36 +255,32 @@ hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
}
void
cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB bb)
cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB _deprecated_unused)
{
cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash);
hashHandle(hash, hand, bb);
hashHandle(hash, hand, hash->bbfunc(obj));
}
void
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, hashid, obj);
hashHandle(hash, hand, hash->bbfunc(obj));
cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
if(hand){
hand->obj = NULL;
cpHandleRelease(hand, hash->pooledHandles);
cpSpaceHashInsert(hash, obj, hashid, cpBBNew(0.0f, 0.0f, 0.0f, 0.0f));
}
}
// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?)
static void
handleRehashHelper(void *elt, void *data)
{
cpHandle *hand = (cpHandle *)elt;
cpSpaceHash *hash = (cpSpaceHash *)data;
hashHandle(hash, hand, hash->bbfunc(hand->obj));
}
static void handleRehashHelper(cpHandle *hand, cpSpaceHash *hash){hashHandle(hash, hand, hash->bbfunc(hand->obj));}
void
cpSpaceHashRehash(cpSpaceHash *hash)
{
clearHash(hash);
// Rehash all of the handles.
cpHashSetEach(hash->handleSet, &handleRehashHelper, hash);
cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)handleRehashHelper, hash);
}
void
@ -309,54 +294,63 @@ cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
}
}
// Used by the cpSpaceHashEach() iterator.
typedef struct eachPair {
cpSpaceHashIterator func;
void *data;
} eachPair;
// Calls the user iterator function. (Gross I know.)
static void
eachHelper(void *elt, void *data)
{
cpHandle *hand = (cpHandle *)elt;
eachPair *pair = (eachPair *)data;
pair->func(hand->obj, pair->data);
}
static void eachHelper(cpHandle *hand, eachPair *pair){pair->func(hand->obj, pair->data);}
// Iterate over the objects in the spatial hash.
void
cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data)
{
// Bundle the callback up to send to the hashset iterator.
eachPair pair = {func, data};
cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &pair);
}
cpHashSetEach(hash->handleSet, &eachHelper, &pair);
static inline void
removeOrphanedHandles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
{
cpSpaceHashBin *bin = *bin_ptr;
while(bin){
cpHandle *hand = bin->handle;
cpSpaceHashBin *next = bin->next;
if(!hand->obj){
// orphaned handle, unlink and recycle the bin
(*bin_ptr) = bin->next;
recycleBin(hash, bin);
cpHandleRelease(hand, hash->pooledHandles);
} else {
bin_ptr = &bin->next;
}
bin = next;
}
}
// Calls the callback function for the objects in a given chain.
static inline void
query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data)
query(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashQueryFunc func, void *data)
{
for(; bin; bin = bin->next){
restart:
for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
cpHandle *hand = bin->handle;
void *other = hand->obj;
// Skip over certain conditions
if(
// Have we already tried this pair in this query?
hand->stamp == hash->stamp
// Is obj the same as other?
|| obj == other
// Has other been removed since the last rehash?
|| !other
) continue;
func(obj, other, data);
// Stamp that the handle was checked already against this object.
hand->stamp = hash->stamp;
if(hand->stamp == hash->stamp || obj == other){
continue;
} else if(other){
func(obj, other, data);
hand->stamp = hash->stamp;
} else {
// The object for this handle has been removed
// cleanup this cell and restart the query
removeOrphanedHandles(hash, bin_ptr);
goto restart; // GCC not smart enough/able to tail call an inlined function.
}
}
}
@ -366,10 +360,7 @@ cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func
cpFloat dim = hash->celldim;
int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells); // Fix by ShiftZ
query(hash, hash->table[idx], &point, func, data);
// Increment the stamp.
// Only one cell is checked, but query() requires it anyway.
query(hash, &hash->table[idx], &point, func, data);
hash->stamp++;
}
@ -384,16 +375,15 @@ cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc fun
int t = floor_int(bb.t/dim);
int n = hash->numcells;
cpSpaceHashBin **table = hash->table;
// Iterate over the cells and query them.
for(int i=l; i<=r; i++){
for(int j=b; j<=t; j++){
int idx = hash_func(i,j,n);
query(hash, hash->table[idx], obj, func, data);
query(hash, &table[hash_func(i,j,n)], obj, func, data);
}
}
// Increment the stamp.
hash->stamp++;
}
@ -426,28 +416,26 @@ handleQueryRehashHelper(void *elt, void *data)
int b = floor_int(bb.b/dim);
int t = floor_int(bb.t/dim);
cpSpaceHashBin **table = hash->table;
for(int i=l; i<=r; i++){
for(int j=b; j<=t; j++){
// // exit the loops if the object has been deleted in func().
// if(!hand->obj) goto break_out;
int idx = hash_func(i,j,n);
cpSpaceHashBin *bin = hash->table[idx];
cpSpaceHashBin *bin = table[idx];
if(containsHandle(bin, hand)) continue;
cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
query(hash, bin, obj, func, pair->data);
query(hash, &bin, obj, func, pair->data);
cpSpaceHashBin *newBin = getEmptyBin(hash);
newBin->handle = hand;
newBin->next = bin;
hash->table[idx] = newBin;
table[idx] = newBin;
}
}
// break_out:
// Increment the stamp for each object we hash.
// Increment the stamp for each object hashed.
hash->stamp++;
}
@ -461,26 +449,27 @@ cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data)
}
static inline cpFloat
segmentQuery(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
segmentQuery(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
{
cpFloat t = 1.0f;
for(; bin; bin = bin->next){
restart:
for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
cpHandle *hand = bin->handle;
void *other = hand->obj;
// Skip over certain conditions
if(
// Have we already tried this pair in this query?
hand->stamp == hash->stamp
// Has other been removed since the last rehash?
|| !other
) continue;
// Stamp that the handle was checked already against this object.
hand->stamp = hash->stamp;
t = cpfmin(t, func(obj, other, data));
if(hand->stamp == hash->stamp){
continue;
} else if(other){
t = cpfmin(t, func(obj, other, data));
hand->stamp = hash->stamp;
} else {
// The object for this handle has been removed
// cleanup this cell and restart the query
removeOrphanedHandles(hash, bin_ptr);
goto restart; // GCC not smart enough/able to tail call an inlined function.
}
}
return t;
@ -492,8 +481,6 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c
a = cpvmult(a, 1.0f/hash->celldim);
b = cpvmult(b, 1.0f/hash->celldim);
cpFloat dt_dx = 1.0f/cpfabs(b.x - a.x), dt_dy = 1.0f/cpfabs(b.y - a.y);
int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
cpFloat t = 0;
@ -517,14 +504,20 @@ void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, c
temp_v = (a.y - cpffloor(a.y));
}
// Division by zero is *very* slow on ARM
cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
// fix NANs in horizontal directions
cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
cpSpaceHashBin **table = hash->table;
int n = hash->numcells;
while(t < t_exit){
int idx = hash_func(cell_x, cell_y, n);
t_exit = cpfmin(t_exit, segmentQuery(hash, hash->table[idx], obj, func, data));
t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data));
if (next_v < next_h){
cell_y += y_inc;