public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
this.r1 = cpTransform.Vect(a.transform, cpVect.cpvsub(this.anchorA, a.cog));
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
// Calculate mass tensor
this.k = cp.k_tensor(a, b, this.r1, this.r2);
// calculate bias velocity
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
this.bias = cpVect.cpvclamp(cpVect.cpvmult(delta, -cp.bias_coef(this.errorBias, dt) / dt), this.maxBias);
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
// calculate endpoints in worldspace
cpVect ta = cpTransform.Point(a.transform, this.grv_a);
cpVect tb = cpTransform.Point(a.transform, this.grv_b);
// calculate axis
cpVect n = cpTransform.Vect(a.transform, this.grv_n);
float d = cpVect.cpvdot(ta, n);
this.grv_tn = n;
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
// calculate tangential distance along the axis of r2
float td = cpVect.cpvcross(cpVect.cpvadd(b.p, this.r2), n);
// calculate clamping factor and r2
if (td <= cpVect.cpvcross(ta, n))
{
this.clamp = 1.0f;
this.r1 = cpVect.cpvsub(ta, a.p);
}
else if (td >= cpVect.cpvcross(tb, n))
{
this.clamp = -1.0f;
this.r1 = cpVect.cpvsub(tb, a.p);
}
else
{
this.clamp = 0.0f;
this.r1 = cpVect.cpvsub(cpVect.cpvadd(cpVect.cpvmult(cpVect.cpvperp(n), -td), cpVect.cpvmult(n, d)), a.p);
}
// Calculate mass tensor
this.k = cp.k_tensor(a, b, this.r1, this.r2);
// calculate bias velocity
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
this.bias = cpVect.cpvclamp(cpVect.cpvmult(delta, -cp.bias_coef(this.errorBias, dt) / dt), this.maxBias);
}
public static cpVect cpMat2x2Transform(cpMat2x2 m, cpVect v)
{
return(cpv(v.x * m.a + v.y * m.b, v.x * m.c + v.y * m.d));
}
public static cpVect cpMat2x2Transform(cpMat2x2 m, cpVect v)
{
return cpv(v.x * m.a + v.y * m.b, v.x * m.c + v.y * m.d);
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
// calculate endpoints in worldspace
cpVect ta = cpTransform.Point(a.transform, this.grv_a);
cpVect tb = cpTransform.Point(a.transform, this.grv_b);
// calculate axis
cpVect n = cpTransform.Vect(a.transform, this.grv_n);
float d = cpVect.cpvdot(ta, n);
this.grv_tn = n;
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
// calculate tangential distance along the axis of r2
float td = cpVect.cpvcross(cpVect.cpvadd(b.p, this.r2), n);
// calculate clamping factor and r2
if (td <= cpVect.cpvcross(ta, n))
{
this.clamp = 1.0f;
this.r1 = cpVect.cpvsub(ta, a.p);
}
else if (td >= cpVect.cpvcross(tb, n))
{
this.clamp = -1.0f;
this.r1 = cpVect.cpvsub(tb, a.p);
}
else
{
this.clamp = 0.0f;
this.r1 = cpVect.cpvsub(cpVect.cpvadd(cpVect.cpvmult(cpVect.cpvperp(n), -td), cpVect.cpvmult(n, d)), a.p);
}
// Calculate mass tensor
this.k = cp.k_tensor(a, b, this.r1, this.r2);
// calculate bias velocity
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
this.bias = cpVect.cpvclamp(cpVect.cpvmult(delta, -cp.bias_coef(this.errorBias, dt) / dt), this.maxBias);
}
public static cpVect Transform(cpMat2x2 m, cpVect v)
{
return new cpVect(v.x * m.a + v.y * m.b, v.x * m.c + v.y * m.d);
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
this.r1 = cpTransform.Vect(a.transform, cpVect.cpvsub(this.anchorA, a.cog));
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
// Calculate mass tensor
this.k = cp.k_tensor(a, b, this.r1, this.r2);
// calculate bias velocity
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
this.bias = cpVect.cpvclamp(cpVect.cpvmult(delta, -cp.bias_coef(this.errorBias, dt) / dt), this.maxBias);
}
