static void
preStep(cpPivotJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
// Calculate mass tensor
joint-> k = k_tensor(a, b, joint->r1, joint->r2);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
static cpBB
cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform)
{
seg->ta = cpTransformPoint(transform, seg->a);
seg->tb = cpTransformPoint(transform, seg->b);
seg->tn = cpTransformVect(transform, seg->n);
cpFloat l,r,b,t;
if(seg->ta.x < seg->tb.x){
l = seg->ta.x;
r = seg->tb.x;
} else {
l = seg->tb.x;
r = seg->ta.x;
}
if(seg->ta.y < seg->tb.y){
b = seg->ta.y;
t = seg->tb.y;
} else {
b = seg->tb.y;
t = seg->ta.y;
}
cpFloat rad = seg->r;
return cpBBNew(l - rad, b - rad, r + rad, t + rad);
}
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));
}
/*
* Chipmunk2d::Transform#vect(vect)
* @param [Chipmunk2d::Vect] vect
* @return [Chipmunk2d::Vect]
*/
static mrb_value
transform_vect(mrb_state* mrb, mrb_value self)
{
cpTransform* transform;
cpVect* vect;
mrb_get_args(mrb, "d", &vect, &mrb_cp_vect_type);
transform = mrb_cp_get_transform_ptr(mrb, self);
return mrb_cp_vect_value(mrb, cpTransformVect(*transform, *vect));
}
static void
preStep(cpPinJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate endpoints in worldspace
cpVect ta = cpTransformPoint(a->transform, joint->grv_a);
cpVect tb = cpTransformPoint(a->transform, joint->grv_b);
// calculate axis
cpVect n = cpTransformVect(a->transform, joint->grv_n);
cpFloat d = cpvdot(ta, n);
joint->grv_tn = n;
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
// calculate tangential distance along the axis of r2
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
// calculate clamping factor and r2
if(td <= cpvcross(ta, n)){
joint->clamp = 1.0f;
joint->r1 = cpvsub(ta, a->p);
} else if(td >= cpvcross(tb, n)){
joint->clamp = -1.0f;
joint->r1 = cpvsub(tb, a->p);
} else {
joint->clamp = 0.0f;
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
}
// Calculate mass tensor
joint->k = k_tensor(a, b, joint->r1, joint->r2);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
