cpPolylineSet *
cpPolylineConvexDecomposition_BETA(cpPolyline *line, cpFloat tol)
{
cpAssertSoft(cpPolylineIsClosed(line), "Cannot decompose an open polygon.");
cpAssertSoft(cpAreaForPoly(line->count, line->verts, 0.0) >= 0.0, "Winding is backwards. (Are you passing a hole?)");
cpPolylineSet *set = cpPolylineSetNew();
ApproximateConcaveDecomposition(line->verts, line->count - 1, tol, set);
return set;
}
// QuickHull seemed like a neat algorithm, and efficient-ish for large input sets.
// My implementation performs an in place reduction using the result array as scratch space.
int
cpConvexHull(int count, cpVect *verts, cpVect *result, int *first, cpFloat tol)
{
if(result){
// Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer.
memcpy(result, verts, count*sizeof(cpVect));
} else {
// If a result array was not specified, reduce the input instead.
result = verts;
}
// Degenerate case, all poins are the same.
int start, end;
cpLoopIndexes(verts, count, &start, &end);
if(start == end){
if(first) (*first) = 0;
return 1;
}
SWAP(result[0], result[start]);
SWAP(result[1], result[end == 0 ? start : end]);
cpVect a = result[0];
cpVect b = result[1];
if(first) (*first) = start;
int resultCount = QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1;
cpAssertSoft(cpPolyValidate(result, resultCount),
"Internal error: cpConvexHull() and cpPolyValidate() did not agree."
"Please report this error with as much info as you can.");
return resultCount;
}
// Recursive implementation of the EPA loop.
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
// Find the closest segment hull[i] and hull[i + 1] to (0, 0)
for(int j=0, i=count-1; j<count; i=j, j++) {
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist) {
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
// Check if there is a point on the minkowski difference beyond this edge.
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
if(CheckArea(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab))) && iteration < MAX_EPA_ITERATIONS) {
// Rebuild the convex hull by inserting p.
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
int count2 = 1;
hull2[0] = p;
for(int i=0; i<count; i++) {
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
if(CheckArea(cpvsub(h2, h0), cpvadd(cpvsub(h1, h0), cpvsub(h1, h2)))) {
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
// Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
int
cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
{
// Their shape types must be in order.
cpAssertSoft(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;
}
static inline void
NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree)
{
cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf.");
cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent.");
if(parent->A == child){
NodeRecycle(tree, parent->A);
NodeSetA(parent, value);
} else {
NodeRecycle(tree, parent->B);
NodeSetB(parent, value);
}
for(Node *node=parent; node; node = node->parent){
node->bb = cpBBMerge(node->A->bb, node->B->bb);
}
}
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
for(int j=0, i=count-1; j<count; i=j, j++){
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist){
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS){
int count2 = 1;
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
hull2[0] = p;
for(int i=0; i<count; i++){
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
// TODO: Should this be changed to an area2x check?
if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f){
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
cpAssertSoft(!body->space, "This body is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpArrayPush(space->bodies, body);
body->space = space;
return body;
}
static inline void
cpCollisionInfoPushContact(struct cpCollisionInfo *info, cpVect p1, cpVect p2, cpHashValue hash)
{
cpAssertSoft(info->count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts.");
struct cpContact *con = &info->arr[info->count];
con->r1 = p1;
con->r2 = p2;
con->hash = hash;
info->count++;
}
void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertSoft(cpSpaceContainsShape(space, shape),
"Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBody *body = shape->body;
if(cpBodyIsStatic(body)) cpBodyActivateStatic(body, shape);
cpBodyRemoveShape(body, shape);
cpSpaceFilterArbiters(space, body, shape);
cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
shape->space = NULL;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpBody *body = shape->body;
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
static void *
cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space)
{
if(space->pooledArbiters->num == 0){
// arbiter pool is exhausted, make more
int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
cpAssertSoft(count, "Buffer size too small.");
cpArbiter *buffer = (cpArbiter *)cpcalloc(1, 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]);
}
void
cpSpaceActivateBody(cpSpace *space, cpBody *body)
{
cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body.");
if(space->locked){
// cpSpaceActivateBody() is called again once the space is unlocked
if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
} else {
cpAssertSoft(body->node.root == NULL && body->node.next == NULL, "Internal error: Activating body non-NULL node pointers.");
cpArrayPush(space->bodies, body);
CP_BODY_FOREACH_SHAPE(body, shape){
cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
}
CP_BODY_FOREACH_ARBITER(body, arb){
cpBody *bodyA = arb->body_a;
// Arbiters are shared between two bodies that are always woken up together.
// You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
// The edge case is when static bodies are involved as the static bodies never actually sleep.
// If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
if(body == bodyA || cpBodyIsStatic(bodyA)){
int numContacts = arb->numContacts;
cpContact *contacts = arb->contacts;
// Restore contact values back to the space's contact buffer memory
arb->contacts = cpContactBufferGetArray(space);
memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact));
cpSpacePushContacts(space, numContacts);
// Reinsert the arbiter into the arbiter cache
cpShape *a = arb->a, *b = arb->b;
cpShape *shape_pair[] = {a, b};
cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL);
// Update the arbiter's state
arb->stamp = space->stamp;
arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
cpArrayPush(space->arbiters, arb);
cpfree(contacts);
}
}
void
cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
{
space->locked--;
cpAssertSoft(space->locked >= 0, "Internal Error: Space lock underflow.");
if(!space->locked){
cpArray *waking = space->rousedBodies;
for(int i=0, count=waking->num; i<count; i++){
cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
}
waking->num = 0;
cpSpaceRunPostStepCallbacks(space);
}
}
cpConstraint *
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertSoft(!constraint->space, "This shape is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayPush(space->constraints, constraint);
// Push onto the heads of the bodies' constraint lists
cpBody *a = constraint->a, *b = constraint->b;
constraint->next_a = a->constraintList; a->constraintList = constraint;
constraint->next_b = b->constraintList; b->constraintList = constraint;
constraint->space = space;
return constraint;
}
static void
preStep(cpDampedRotarySpring *spring, cpFloat dt)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
cpFloat moment = a->i_inv + b->i_inv;
cpAssertSoft(moment != 0.0, "Unsolvable spring.");
spring->iSum = 1.0f/moment;
spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
spring->target_wrn = 0.0f;
// apply spring torque
cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
a->w -= j_spring*a->i_inv;
b->w += j_spring*b->i_inv;
}
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);
cpAssertSoft(count, "Buffer size is too small.");
cpHandle *buffer = (cpHandle *)cpcalloc(1, 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;
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
cpBody *body = shape->body;
if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
// TODO change these to check if it was added to a space at all.
cpAssertSoft(!shape->space, "This shape is already added to a space and cannot be added to another.");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
cpBodyAddShape(body, shape);
cpShapeUpdate(shape, body->p, body->rot);
cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
shape->space = space;
return shape;
}
// 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);
cpAssertSoft(count, "Buffer size is too small.");
cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES);
cpArrayPush(hash->allocatedBuffers, buffer);
// push all but the first one, return the first instead
for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
return buffer;
}
}
void
cpBodySanityCheck(cpBody *body)
{
cpAssertSoft(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is invalid.");
cpAssertSoft(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is invalid.");
cpv_assert_sane(body->p, "Body's position is invalid.");
cpv_assert_sane(body->v, "Body's velocity is invalid.");
cpv_assert_sane(body->f, "Body's force is invalid.");
cpAssertSoft(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid.");
cpAssertSoft(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid.");
cpAssertSoft(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid.");
cpv_assert_sane(body->rot, "Internal error: Body's rotation vector is invalid.");
cpAssertSoft(body->v_limit == body->v_limit, "Body's velocity limit is invalid.");
cpAssertSoft(body->w_limit == body->w_limit, "Body's angular velocity limit is invalid.");
}
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);
cpAssertSoft(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 it instead
for(int i=1; i<count; i++) recycleBin(set, buffer + i);
return buffer;
}
}
static void internalBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
cpBodyUpdateVelocity(body, cpvzero, damping, dt);
// Skip kinematic bodies.
if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return;
cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: f)", body->m, body->i);
cocos2d::PhysicsBody *physicsBody = static_cast<cocos2d::PhysicsBody*>(body->userData);
if(physicsBody->isGravityEnabled())
body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), physicsBody->getVelocityLimit());
else
body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvmult(body->f, body->m_inv), dt)), physicsBody->getVelocityLimit());
cpFloat w_limit = physicsBody->getAngularVelocityLimit();
body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit);
// Reset forces.
body->f = cpvzero;
//to check body sanity
cpBodySetTorque(body, 0.0f);
}
static void
preStep(cpDampedSpring *spring, cpFloat dt)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
cpAssertSoft(k != 0.0, "Unsolvable spring.");
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void cpv_assert_infinite(cpVect v, char *message){cpAssertSoft(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);}
static void cpv_assert_nan(cpVect v, char *message){cpAssertSoft(v.x == v.x && v.y == v.y, message);}
void
cpSpacePushContacts(cpSpace *space, int count)
{
cpAssertSoft(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error:contact buffer overflow!");
space->contactBuffersHead->numContacts += count;
}