static void
preStep(cpPulleyJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody* b1 = joint->constraint.a;
cpBody* b2 = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, b1->rot);
joint->r2 = cpvrotate(joint->anchr2, b2->rot);
cpVect p1 = cpvadd(b1->p, joint->r1);
cpVect p2 = cpvadd(b2->p, joint->r2);
//Catto claimed that these needed to be "grounded" pts
cpVect s1 = cpBodyLocal2World(joint->c, joint->anchr3a);
cpVect s2 = cpBodyLocal2World(joint->c, joint->anchr3b);
// Get the pulley axes.
joint->u1 = cpvsub(p1, s1);
joint->u2 = cpvsub(p2, s2);
// Lengths
cpFloat length1 = cpvlength(joint->u1);
cpFloat length2 = cpvlength(joint->u2);
// Check constraints
joint->u1 = (length1 > cp_collision_slop) ? cpvmult(joint->u1, 1.0f/length1) : cpvzero;
joint->u2 = (length2 > cp_collision_slop) ? cpvmult(joint->u2, 1.0f/length2) : cpvzero;
// Compute 'C'
cpFloat C = joint->constant - length1 - joint->ratio * length2;
// Set state based on lengths
joint->state = (C > 0.0f) ? 0 : 1;
joint->limitState1 = (length1 < joint->max1) ? 0 : 1;
joint->limitState2 = (length2 < joint->max2) ? 0 : 1;
// Compute effective mass.
cpFloat cr1u1 = cpvcross(joint->r1, joint->u1);
cpFloat cr2u2 = cpvcross(joint->r2, joint->u2);
// Set Mass Limits
joint->limitMass1 = b1->m_inv + b1->i_inv * cr1u1 * cr1u1;
joint->limitMass2 = b2->m_inv + b2->i_inv * cr2u2 * cr2u2;
joint->pulleyMass = joint->limitMass1 + joint->ratio * joint->ratio * joint->limitMass2;
// Check against evil
cpAssert(joint->limitMass1 != 0.0f, "Calculated Pulley Limit(1) is Zero");
cpAssert(joint->limitMass2 != 0.0f, "Calculated Pulley Limit(2) is Zero");
cpAssert(joint->pulleyMass != 0.0f, "Calculated Pulley Mass is Zero");
// We want the inverse's
joint->limitMass1 = 1.0f / joint->limitMass1;
joint->limitMass2 = 1.0f / joint->limitMass2;
joint->pulleyMass = 1.0f / joint->pulleyMass;
// Reset accumulations, could also warm start here
joint->jnAcc = 0.0f;
joint->jnAccLim1 = 0.0f;
joint->jnAccLim2 = 0.0f;
}
void
cpBodySleepWithGroup(cpBody *body, cpBody *group){
cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep.");
cpSpace *space = body->space;
cpAssert(space, "Cannot put a body to sleep that has not been added to a space.");
cpAssert(!space->locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
if(cpBodyIsSleeping(body)) return;
for(cpShape *shape = body->shapesList; shape; shape = shape->next){
cpShapeCacheBB(shape);
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
}
if(group){
cpBody *root = componentNodeRoot(group);
cpComponentNode node = {root, root->node.next, 0, 0.0f};
body->node = node;
root->node.next = body;
} else {
cpComponentNode node = {NULL, body, 0, 0.0f};
body->node = node;
cpArrayPush(space->sleepingComponents, body);
}
cpArrayDeleteObj(space->bodies, body);
}
cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
return ((cpPolyShape *)shape)->verts[idx];
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpAssert(shape->body, "Cannot add a shape with a NULL body.");
cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), "Cannot add the same shape more than once.");
cpAssertSpaceUnlocked(space);
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
return shape;
}
void
cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
{
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
cpPolyShapeDestroy(shape);
setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
}
cpPulleyJoint *
cpPulleyJointInit(cpPulleyJoint *joint,
cpBody* a, cpBody* b, cpBody* c,
cpVect anchor1, cpVect anchor2,
cpVect anchor3a, cpVect anchor3b,
cpFloat ratio)
{
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
joint->c = c;
joint->anchr3a = anchor3a;
joint->anchr3b = anchor3b;
joint->anchr1 = anchor1;
joint->anchr2 = anchor2;
cpVect d1 = cpvsub(cpBodyLocal2World(a, anchor1), cpBodyLocal2World(c, anchor3a));
cpVect d2 = cpvsub(cpBodyLocal2World(b, anchor2), cpBodyLocal2World(c, anchor3b));
joint->dist1 = cpvlength(d1);
joint->dist2 = cpvlength(d2);
joint->ratio = ratio;
cpAssert(ratio != 0.0f, "Pulley Ratio is Zero");
// Calculate max and constant
joint->constant = joint->dist1 + ratio * joint->dist2;
joint->max1 = joint->constant - ratio * cp_min_pulley_len;
joint->max2 = (joint->constant - cp_min_pulley_len) / joint->ratio;
// Initialize
joint->jnAcc = 0.0f;
joint->jnAccLim1 = 0.0f;
joint->jnAccLim2 = 0.0f;
return joint;
}
void
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
{
cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
cpSegmentShape *seg = (cpSegmentShape *)shape;
seg->r = radius;
}
void
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
{
cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
cpCircleShape *circle = (cpCircleShape *)shape;
circle->c = offset;
}
void
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
{
cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
cpCircleShape *circle = (cpCircleShape *)shape;
circle->r = radius;
}
void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssert(cpArrayContains(space->constraints, constraint), "Cannot remove a constraint that was never added to the space.");
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
cpArrayDeleteObj(space->constraints, constraint);
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssert(cpArrayContains(space->bodies, body), "Cannot remove a body that was never added to the space.");
cpAssertSpaceUnlocked(space);
cpArrayDeleteObj(space->bodies, body);
}
int
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.");
collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES];
return (cfunc) ? cfunc(a, b, arr) : 0;
}
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
// cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
cpArrayPush(space->bodies, body);
return body;
}
void
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
{
cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
cpSegmentShape *seg = (cpSegmentShape *)shape;
seg->a = a;
seg->b = b;
seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssert(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), "Cannot remove a static shape that was never added to the space.");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}
cpConstraint *
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.
cpArrayPush(space->constraints, constraint);
return constraint;
}
cpPolyShape *
cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
{
// Fail if the user attempts to pass a concave poly, or a bad winding.
cpAssert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");
setUpVerts(poly, numVerts, verts, offset);
cpShapeInit((cpShape *)poly, &polyClass, body);
return poly;
}
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
cpAssertWarn(body->m != INFINITY, "Did you really mean to add an infinite mass body to the space?");
cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
// cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
cpArrayPush(space->bodies, body);
return body;
}
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
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;
}
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 *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
// cpAssert(shape->body, "Cannot add a static shape with a NULL body.");
if(!shape->body) shape->body = &space->staticBody;
cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
"Cannot add the same static shape more than once.");
cpAssertSpaceUnlocked(space);
cpShapeCacheBB(shape);
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
return shape;
}
cpConstraint *
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;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
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);
activateShapesTouchingShape(space, shape);
addShapeRaw(shape, space->staticShapes);
return shape;
}
// 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]);
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(!body || body == &space->staticBody) 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);
addShapeRaw(shape, space->activeShapes);
return shape;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
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;
}
// 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;
}
static inline void
componentActivate(cpBody *root)
{
if(!cpBodyIsSleeping(root)) return;
cpSpace *space = root->space;
cpAssert(space, "Trying to activate a body that was never added to a space.");
cpBody *body = root, *next;
do {
next = body->node.next;
cpComponentNode node = {NULL, NULL, 0, 0.0f};
body->node = node;
cpSpaceActivateBody(space, body);
} while((body = next) != root);
cpArrayDeleteObj(space->sleepingComponents, root);
}
// 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)
{
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 *)cpcalloc(1, CP_BUFFER_BYTES);
cpArrayPush(set->allocatedBuffers, buffer);
// push all but the first one, return the first instead
for(int i=1; i<count; i++) recycleBin(set, buffer + i);
return buffer;
}
}
