// 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++; }
static void preStep(cpRatchetJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpFloat angle = joint->angle; cpFloat phase = joint->phase; cpFloat ratchet = joint->ratchet; cpFloat delta = b->a - a->a; cpFloat diff = angle - delta; cpFloat pdist = 0.0f; if(diff*ratchet > 0.0f){ pdist = diff; } else { joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; } // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); // If the bias is 0, the joint is not at a limit. Reset the impulse. if(!joint->bias) joint->jAcc = 0.0f; }
static void preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpFloat angle = joint->angle; cpFloat phase = joint->phase; cpFloat ratchet = joint->ratchet; cpFloat delta = b->a - a->a; cpFloat diff = angle - delta; cpFloat pdist = 0.0f; if(diff*ratchet > 0.0f){ pdist = diff; } else { joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; } // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias); // compute max impulse joint->jMax = J_MAX(joint, dt); // If the bias is 0, the joint is not at a limit. Reset the impulse. if(!joint->bias) joint->jAcc = 0.0f; // apply joint torque a->w -= joint->jAcc*a->i_inv; b->w += joint->jAcc*b->i_inv; }