void *
cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data)
{
int index = hash%set->size;
// Find the bin with the matching element.
cpHashSetBin *bin = set->table[index];
while(bin && !set->eql(ptr, bin->elt))
bin = bin->next;
// Create it necessary.
if(!bin){
bin = (cpHashSetBin *)cpmalloc(sizeof(cpHashSetBin));
bin->hash = hash;
bin->elt = set->trans(ptr, data); // Transform the pointer.
bin->next = set->table[index];
set->table[index] = bin;
set->entries++;
// Resize the set if it's full.
if(setIsFull(set))
cpHashSetResize(set);
}
return bin->elt;
}
static cpContactBufferHeader *
cpSpaceAllocContactBuffer(cpSpace *space)
{
cpContactBuffer *buffer = (cpContactBuffer *)cpmalloc(sizeof(cpContactBuffer));
cpArrayPush(space->allocatedBuffers, buffer);
return (cpContactBufferHeader *)buffer;
}
// Transformation function for collFuncSet.
static void *
collFuncSetTrans(cpCollisionHandler *handler, void *unused)
{
cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler));
(*copy) = (*handler);
return copy;
}
static void *
postStepFuncSetTrans(postStepCallback *callback, void *ignored)
{
postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback));
(*value) = (*callback);
return value;
}
cpArray*
cpArrayInit(cpArray *arr, int size)
{
arr->num = 0;
size = (size ? size : 4);
arr->max = size;
arr->arr = (void **)cpmalloc(size*sizeof(void**));
return arr;
}
// Get a recycled or new bin.
static inline cpSpaceHashBin *
getEmptyBin(cpSpaceHash *hash)
{
cpSpaceHashBin *bin = hash->bins;
// Make a new one if necessary.
if(bin == NULL) return (cpSpaceHashBin *)cpmalloc(sizeof(cpSpaceHashBin));
hash->bins = bin->next;
return bin;
}
// Collide circles to segment shapes.
static int
circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)circleShape;
cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
// Radius sum
cpFloat rsum = circ->r + seg->r;
// Calculate normal distance from segment.
cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn);
cpFloat dist = cpfabs(dn) - rsum;
if(dist > 0.0f) return 0;
// Calculate tangential distance along segment.
cpFloat dt = -cpvcross(seg->tn, circ->tc);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
// Decision tree to decide which feature of the segment to collide with.
if(dt < dtMin){
if(dt < (dtMin - rsum)){
return 0;
} else {
return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con);
}
} else {
if(dt < dtMax){
cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn);
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)),
n,
dist,
0
);
return 1;
} else {
if(dt < (dtMax + rsum)) {
return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con);
} else {
return 0;
}
}
}
return 1;
}
// Transformation function for contactSet.
static void *
contactSetTrans(cpShape **shapes, cpSpace *space)
{
if(space->pooledArbiters->num == 0){
// arbiter pool is exhausted, make more
int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
cpAssert(count, "Buffer size too small.");
cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
cpArrayPush(space->allocatedBuffers, buffer);
for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
}
return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
}
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpPolyShapeAxis *axes = poly->tAxes;
int mini = 0;
cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r;
if(dist > 0.0f){
return 0;
} else if(dist > min) {
min = dist;
mini = i;
}
}
cpVect n = axes[mini].n;
cpVect a = poly->tVerts[mini];
cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
cpFloat dta = cpvcross(n, a);
cpFloat dtb = cpvcross(n, b);
cpFloat dt = cpvcross(n, circ->tc);
if(dt < dtb){
return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
} else if(dt < dta) {
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
cpvneg(n),
min,
0
);
return 1;
} else {
return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
}
}
// Helper function for allocating contact point lists.
static cpContact *
addContactPoint(cpContact **arr, int *max, int *num)
{
if(*arr == NULL){
// Allocate the array if it hasn't been done.
(*max) = 2;
(*num) = 0;
(*arr) = (cpContact *)cpmalloc((*max)*sizeof(cpContact));
} else if(*num == *max){
// Extend it if necessary.
(*max) *= 2;
(*arr) = (cpContact *)cprealloc(*arr, (*max)*sizeof(cpContact));
}
cpContact *con = &(*arr)[*num];
(*num)++;
return con;
}
// Transformation function for the handleset.
static void *
handleSetTrans(void *obj, cpSpaceHash *hash)
{
if(hash->pooledHandles->num == 0){
// handle pool is exhausted, make more
int count = CP_BUFFER_BYTES/sizeof(cpHandle);
cpAssert(count, "Buffer size is too small.");
cpHandle *buffer = (cpHandle *)cpmalloc(CP_BUFFER_BYTES);
cpArrayPush(hash->allocatedBuffers, buffer);
for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
}
cpHandle *hand = cpHandleInit((cpHandle *) cpArrayPop(hash->pooledHandles), obj);
cpHandleRetain(hand);
return hand;
}
// Get a recycled or new bin.
static inline cpSpaceHashBin *
getEmptyBin(cpSpaceHash *hash)
{
cpSpaceHashBin *bin = hash->pooledBins;
if(bin){
hash->pooledBins = bin->next;
return bin;
} else {
// Pool is exhausted, make more
int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
cpAssert(count, "Buffer size is too small.");
cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES);
cpArrayPush(hash->allocatedBuffers, buffer);
// push all but the first one, return the first instead
for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
return buffer;
}
}
static cpHashSetBin *
getUnusedBin(cpHashSet *set)
{
int i;
cpHashSetBin *bin = set->pooledBins;
if(bin){
set->pooledBins = bin->next;
return bin;
} else {
// Pool is exhausted, make more
int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
cpAssert(count, "Buffer size is too small.");
cpHashSetBin *buffer = (cpHashSetBin *)cpmalloc(CP_BUFFER_BYTES);
cpArrayPush(set->allocatedBuffers, buffer);
// push all but the first one, return the first instead
for(i=1; i<count; i++) recycleBin(set, buffer + i);
return buffer;
}
}
// 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)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
cpFloat distsq = cpvlengthsq(delta);
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.
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
cpvmult(delta, 1.0f/non_zero_dist),
dist - mindist,
0
);
return 1;
}
cpBody*
cpBodyAlloc(void)
{
return (cpBody *)cpmalloc(sizeof(cpBody));
}
cpSimpleMotor *
cpSimpleMotorAlloc(void)
{
return (cpSimpleMotor *)cpmalloc(sizeof(cpSimpleMotor));
}
static cpHandle*
cpHandleAlloc(void)
{
return (cpHandle *)cpmalloc(sizeof(cpHandle));
}
cpRotaryLimitJoint *
cpRotaryLimitJointAlloc(void)
{
return (cpRotaryLimitJoint *)cpmalloc(sizeof(cpRotaryLimitJoint));
}
cpPulleyJoint *
cpPulleyJointAlloc(void)
{
return (cpPulleyJoint *)cpmalloc(sizeof(cpPulleyJoint));
}
cpGearJoint *
cpGearJointAlloc(void)
{
return (cpGearJoint *)cpmalloc(sizeof(cpGearJoint));
}
cpSlideJoint *
cpSlideJointAlloc(void)
{
return (cpSlideJoint *)cpmalloc(sizeof(cpSlideJoint));
}
cpRatchetJoint *
cpRatchetJointAlloc(void)
{
return (cpRatchetJoint *)cpmalloc(sizeof(cpRatchetJoint));
}
cpPivotJoint *
cpPivotJointAlloc(void)
{
return (cpPivotJoint *)cpmalloc(sizeof(cpPivotJoint));
}
cpGrooveJoint *
cpGrooveJointAlloc(void)
{
return (cpGrooveJoint *)cpmalloc(sizeof(cpGrooveJoint));
}
cpDampedRotarySpring *
cpDampedRotarySpringAlloc(void)
{
return (cpDampedRotarySpring *)cpmalloc(sizeof(cpDampedRotarySpring));
}
cpOscillatingMotor *
cpOscillatingMotorAlloc(void)
{
return (cpOscillatingMotor *)cpmalloc(sizeof(cpOscillatingMotor));
}