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); }