// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
void
cpSpaceHashRenderDebug(cpSpatialIndex *index)
{
if(index->klass != &klass){
cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index.");
return;
}
cpSpaceHash *hash = (cpSpaceHash *)index;
cpBB bb = cpBBNew(-320, -240, 320, 240);
cpFloat dim = hash->celldim;
int n = hash->numcells;
int l = (int)floor(bb.l/dim);
int r = (int)floor(bb.r/dim);
int b = (int)floor(bb.b/dim);
int t = (int)floor(bb.t/dim);
for(int i=l; i<=r; i++){
for(int j=b; j<=t; j++){
int cell_count = 0;
int index = hash_func(i,j,n);
for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next)
cell_count++;
GLfloat v = 1.0f - (GLfloat)cell_count/10.0f;
glColor3f(v,v,v);
glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim);
}
}
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
// 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);
}
}
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?)");
cpAssertSpaceUnlocked(space);
cpArrayDeleteObj(space->bodies, body);
}
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);
}
}
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?)");
// cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
cpArrayDeleteObj(space->constraints, constraint);
}
void
cpBBTreeRenderDebug(cpSpatialIndex *index){
if(index->klass != &klass){
cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index.");
return;
}
cpBBTree *tree = (cpBBTree *)index;
if(tree->root) NodeRender(tree->root, 0);
}
void
cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func)
{
if(index->klass != Klass()){
cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index.");
return;
}
((cpBBTree *)index)->velocityFunc = func;
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
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
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?)");
cpAssertSpaceUnlocked(space);
removalContext context = {space, shape};
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}
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;
}
void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
cpAssertWarn(cpSpaceContainsBody(space, body),
"Cannot remove a body that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(body);
// cpSpaceFilterArbiters(space, body, NULL);
cpArrayDeleteObj(space->bodies, body);
body->space = NULL;
}
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;
}
void
cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
{
if(hash->spatialIndex.klass != Klass()){
cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index.");
return;
}
clearTable(hash);
hash->celldim = celldim;
cpSpaceHashAllocTable(hash, next_prime(numcells));
}
void
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
{
cpAssertWarn(space->locked,
"Adding a post-step callback when the space is not locked is unnecessary. "
"Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
if(!space->postStepCallbacks){
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql);
}
cpPostStepCallback callback = {func, obj, data};
cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL, (cpHashSetTransFunc)postStepFuncSetTrans);
}
void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
cpAssertWarn(cpSpaceContainsConstraint(space, constraint),
"Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
cpAssertSpaceUnlocked(space);
cpBodyActivate(constraint->a);
cpBodyActivate(constraint->b);
cpArrayDeleteObj(space->constraints, constraint);
cpBodyRemoveConstraint(constraint->a, constraint);
cpBodyRemoveConstraint(constraint->b, constraint);
constraint->space = NULL;
}
cpPinJoint *
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
joint->anchr1 = anchr1;
joint->anchr2 = anchr2;
// 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));
cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
joint->jnAcc = 0.0f;
return joint;
}
cpBool
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data)
{
cpAssertWarn(space->locked,
"Adding a post-step callback when the space is not locked is unnecessary. "
"Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
if(!cpSpaceGetPostStepCallback(space, key)){
cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
callback->func = (func ? func : PostStepDoNothing);
callback->key = key;
callback->data = data;
cpArrayPush(space->postStepCallbacks, callback);
return cpTrue;
} else {
return cpFalse;
}
}
cpPinJoint *
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
{
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
joint->anchorA = anchorA;
joint->anchorB = anchorB;
// STATIC_BODY_CHECK
cpVect p1 = (a ? cpTransformPoint(a->transform, anchorA) : anchorA);
cpVect p2 = (b ? cpTransformPoint(b->transform, anchorB) : anchorB);
joint->dist = cpvlength(cpvsub(p2, p1));
cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
joint->jnAcc = 0.0f;
return joint;
}
void
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
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);
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
}
void
cpBBTreeOptimize(cpSpatialIndex *index)
{
if(index->klass != &klass){
cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
return;
}
cpBBTree *tree = (cpBBTree *)index;
Node *root = tree->root;
if(!root) return;
int count = cpBBTreeCount(tree);
Node **nodes = (Node **)cpcalloc(count, sizeof(Node *));
Node **cursor = nodes;
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor);
SubtreeRecycle(tree, root);
tree->root = partitionNodes(tree, nodes, count);
cpfree(nodes);
}
void
cpInitChipmunk(void)
{
cpAssertWarn(cpFalse, "cpInitChipmunk is deprecated and no longer required. It will be removed in the future.");
}
