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