static cpBB
cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
cpPolyShape *poly = (cpPolyShape *)shape;
cpFloat l, b, r, t;
cpPolyShapeTransformAxes(poly, p, rot);
cpPolyShapeTransformVerts(poly, p, rot);
cpVect *verts = poly->tVerts;
l = r = verts[0].x;
b = t = verts[0].y;
// TODO do as part of cpPolyShapeTransformVerts?
for(int i=1; i<poly->numVerts; i++){
cpVect v = verts[i];
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
return cpBBNew(l, b, r, t);
}
cpVect
cpBBClampVect(const cpBB bb, const cpVect v)
{
cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r);
cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t);
return cpv(x, y);
}
static cpBB
cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform)
{
int count = poly->count;
struct cpSplittingPlane *dst = poly->planes;
struct cpSplittingPlane *src = dst + count;
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
for(int i=0; i<count; i++){
cpVect v = cpTransformPoint(transform, src[i].v0);
cpVect n = cpTransformVect(transform, src[i].n);
dst[i].v0 = v;
dst[i].n = n;
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
cpFloat radius = poly->r;
return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius));
}
static cpBool
StickyPreSolve(cpArbiter *arb, cpSpace *space, void *data)
{
// We want to fudge the collisions a bit to allow shapes to overlap more.
// This simulates their squishy sticky surface, and more importantly
// keeps them from separating and destroying the joint.
// Track the deepest collision point and use that to determine if a rigid collision should occur.
cpFloat deepest = INFINITY;
// Grab the contact set and iterate over them.
cpContactPointSet contacts = cpArbiterGetContactPointSet(arb);
for(int i=0; i<contacts.count; i++){
// Increase the distance (negative means overlaping) of the
// collision to allow them to overlap more.
// This value is used only for fixing the positions of overlapping shapes.
cpFloat dist = contacts.points[i].dist + 2.0f*STICK_SENSOR_THICKNESS;
contacts.points[i].dist = cpfmin(0.0f, dist);
deepest = cpfmin(deepest, dist);
}
// Set the new contact point data.
cpArbiterSetContactPointSet(arb, &contacts);
// If the shapes are overlapping enough, then create a
// joint that sticks them together at the first contact point.
if(!cpArbiterGetUserData(arb) && deepest <= 0.0f){
CP_ARBITER_GET_BODIES(arb, bodyA, bodyB);
// Create a joint at the contact point to hold the body in place.
cpConstraint *joint = cpPivotJointNew(bodyA, bodyB, contacts.points[0].point);
// Give it a finite force for the stickyness.
cpConstraintSetMaxForce(joint, 3e3);
// Schedule a post-step() callback to add the joint.
cpSpaceAddPostStepCallback(space, PostStepAddJoint, joint, NULL);
// Store the joint on the arbiter so we can remove it later.
cpArbiterSetUserData(arb, joint);
}
// Position correction and velocity are handled separately so changing
// the overlap distance alone won't prevent the collision from occuring.
// Explicitly the collision for this frame if the shapes don't overlap using the new distance.
return (deepest <= 0.0f);
// Lots more that you could improve upon here as well:
// * Modify the joint over time to make it plastic.
// * Modify the joint in the post-step to make it conditionally plastic (like clay).
// * Track a joint for the deepest contact point instead of the first.
// * Track a joint for each contact point. (more complicated since you only get one data pointer).
}
static inline cpFloat
segmentQuery(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
{
cpFloat t = 1.0f;
for(; 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));
}
return t;
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
// Calculate the target bias velocity.
con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
con->jBias = 0.0f;
// Calculate the target bounce velocity.
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e;
}
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
// Calculate the target bias velocity.
con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt;
con->jBias = 0.0f;
// Calculate the target bounce velocity.
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;
}
}
// Like cpPolyValueOnAxis(), but for segments.
static inline cpFloat
segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d)
{
cpFloat a = cpvdot(n, seg->ta) - seg->r;
cpFloat b = cpvdot(n, seg->tb) - seg->r;
return cpfmin(a, b) - d;
}
void
cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
{
// Calculate the world space anchor coordinates.
cpVect r1 = cpvrotate(anchr1, a->rot);
cpVect r2 = cpvrotate(anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1));
cpFloat dist = cpvlength(delta);
cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
cpFloat f_spring = (dist - rlen)*k;
// Calculate the world relative velocities of the anchor points.
cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
// Calculate the damping force.
// This really should be in the impulse solver and can produce problems when using large damping values.
cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv)));
// Apply!
cpVect f = cpvmult(n, f_spring + f_damp);
cpBodyApplyForce(a, f, r1);
cpBodyApplyForce(b, cpvneg(f), r2);
}
static inline cpFloat
segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data)
{
cpFloat t = 1.0f;
restart:
for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
cpHandle *hand = bin->handle;
void *other = hand->obj;
// Skip over certain conditions
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
remove_orphaned_handles(hash, bin_ptr);
goto restart; // GCC not smart enough/able to tail call an inlined function.
}
}
return t;
}
cpVect
cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a)
{
cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2));
cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f));
return cpvslerp(v1, v2, cpfmin(a, omega)/omega);
}
static inline cpBB
GetBB(cpBBTree *tree, void *obj)
{
cpBB bb = tree->spatialIndex.bbfunc(obj);
cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc;
if(velocityFunc){
cpFloat coef = 0.1f;
cpFloat x = (bb.r - bb.l)*coef;
cpFloat y = (bb.t - bb.b)*coef;
cpVect v = cpvmult(velocityFunc(obj), 0.1f);
return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y));
} else {
return bb;
}
}
// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
void
cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
{
a = cpvmult(a, 1.0f/hash->celldim);
b = cpvmult(b, 1.0f/hash->celldim);
int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
cpFloat t = 0;
int x_inc, y_inc;
cpFloat temp_v, temp_h;
if (b.x > a.x){
x_inc = 1;
temp_h = (cpffloor(a.x + 1.0f) - a.x);
} else {
x_inc = -1;
temp_h = (a.x - cpffloor(a.x));
}
if (b.y > a.y){
y_inc = 1;
temp_v = (cpffloor(a.y + 1.0f) - a.y);
} else {
y_inc = -1;
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);
int n = hash->numcells;
cpSpaceHashBin **table = hash->table;
while(t < t_exit){
int idx = hash_func(cell_x, cell_y, n);
t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data));
if (next_v < next_h){
cell_y += y_inc;
t = next_v;
next_v += dt_dy;
} else {
cell_x += x_inc;
t = next_h;
next_h += dt_dx;
}
}
hash->stamp++;
}
void
cpArbiterApplyImpulse(cpArbiter *arb)
{
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
cpVect n = con->n;
cpVect r1 = con->r1;
cpVect r2 = con->r2;
// Calculate the relative bias velocities.
cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
// Calculate and clamp the bias impulse.
cpFloat jbn = (con->bias - vbn)*con->nMass;
cpFloat jbnOld = con->jBias;
con->jBias = cpfmax(jbnOld + jbn, 0.0f);
jbn = con->jBias - jbnOld;
// Apply the bias impulse.
cpVect jb = cpvmult(n, jbn);
cpBodyApplyBiasImpulse(a, cpvneg(jb), r1);
cpBodyApplyBiasImpulse(b, jb, r2);
// Calculate the relative velocity.
cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
cpVect vr = cpvsub(v2, v1);
cpFloat vrn = cpvdot(vr, n);
// Calculate and clamp the normal impulse.
cpFloat jn = -(con->bounce + vrn)*con->nMass;
cpFloat jnOld = con->jnAcc;
con->jnAcc = cpfmax(jnOld + jn, 0.0f);
jn = con->jnAcc - jnOld;
// Calculate the relative tangent velocity.
cpVect t = cpvperp(n);
cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), t);
// Calculate and clamp the friction impulse.
cpFloat jtMax = arb->u*con->jnAcc;
cpFloat jt = -vrt*con->tMass;
cpFloat jtOld = con->jtAcc;
con->jtAcc = cpfmin(cpfmax(jtOld + jt, -jtMax), jtMax);
jt = con->jtAcc - jtOld;
// Apply the final impulse.
cpVect j = cpvadd(cpvmult(n, jn), cpvmult(t, jt));
cpBodyApplyImpulse(a, cpvneg(j), r1);
cpBodyApplyImpulse(b, j, r2);
}
}
static cpFloat
SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
{
if(NodeIsLeaf(subtree)){
return func(obj, subtree->obj, data);
} else {
cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b);
cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b);
if(t_a < t_b){
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
} else {
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
}
return t_exit;
}
}
static cpBB
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
cpVect *src = poly->verts;
cpVect *dst = poly->tVerts;
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
for(int i=0; i<poly->numVerts; i++) {
cpVect v = cpvadd(p, cpvrotate(src[i], rot));
dst[i] = v;
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
return cpBBNew(l, b, r, t);
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
cpShape *shapea = arb->a;
cpShape *shapeb = arb->b;
arb->e = shapea->e * shapeb->e;
arb->u = shapea->u * shapeb->u;
arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v);
cpBody *a = shapea->body;
cpBody *b = shapeb->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal.
cpFloat mass_sum = a->m_inv + b->m_inv;
cpFloat r1cn = cpvcross(con->r1, con->n);
cpFloat r2cn = cpvcross(con->r2, con->n);
cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
con->nMass = 1.0f/kn;
// Calculate the mass tangent.
cpVect t = cpvperp(con->n);
cpFloat r1ct = cpvcross(con->r1, t);
cpFloat r2ct = cpvcross(con->r2, t);
cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct;
con->tMass = 1.0f/kt;
// Calculate the target bias velocity.
con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
con->jBias = 0.0f;
// Calculate the target bounce velocity.
cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w));
cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w));
con->bounce = cpvdot(con->n, cpvsub(v2, v1))*arb->e;
// Apply the previous accumulated impulse.
cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
cpBodyApplyImpulse(a, cpvneg(j), con->r1);
cpBodyApplyImpulse(b, j, con->r2);
}
}
static void
reshape(int width, int height)
{
glViewport(0, 0, width, height);
double scale = 0.5/cpfmin(width/640.0, height/480.0);
double hw = width*scale;
double hh = height*scale;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-hw, hw, -hh, hh, -1.0, 1.0);
glTranslated(0.5, 0.5, 0.0);
}
static void
reshape(int width, int height)
{
glViewport(0, 0, width, height);
double scale = cpfmin(width/640.0, height/480.0);
double hw = width*(0.5/scale);
double hh = height*(0.5/scale);
ChipmunkDebugDrawPointLineScale = scale;
glLineWidth(scale);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-hw, hw, -hh, hh, -1.0, 1.0);
glTranslated(0.5, 0.5, 0.0);
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpVect n = arb->n;
cpVect body_delta = cpvsub(b->p, a->p);
for(int i=0; i<arb->count; i++){
struct cpContact *con = &arb->contacts[i];
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
// Calculate the target bias velocity.
cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
con->jBias = 0.0f;
// Calculate the target bounce velocity.
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
}
}
cpVect
cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a)
{
cpFloat angle = cpfacos(cpvdot(v1, v2));
return cpvslerp(v1, v2, cpfmin(a, angle)/angle);
}
