//Optimized applyCachedImpulse
func (arb *Arbiter) applyCachedImpulse(dt_coef vect.Float) {
if arb.state == arbiterStateFirstColl && arb.NumContacts > 0 {
return
}
//println("asd")
a := arb.ShapeA.Body
b := arb.ShapeB.Body
var j vect.Vect
for _, con := range arb.Contacts {
//transform.RotateVect(con.n, transform.Rotation{con.jnAcc, con.jtAcc})
j.X = ((con.n.X * con.jnAcc) - (con.n.Y * con.jtAcc)) * dt_coef
j.Y = ((con.n.X * con.jtAcc) + (con.n.Y * con.jnAcc)) * dt_coef
//apply_impulses(a, b, con.r1, con.r2, vect.Mult(j, dt_coef))
a.v.X = (-j.X * a.m_inv) + a.v.X
a.v.Y = (-j.Y * a.m_inv) + a.v.Y
a.w += a.i_inv * ((con.r1.X * -j.Y) - (con.r1.Y * -j.X))
b.v.X = (j.X * b.m_inv) + b.v.X
b.v.Y = (j.Y * b.m_inv) + b.v.Y
b.w += b.i_inv * ((con.r2.X * j.Y) - (con.r2.Y * j.X))
}
}
//Optimized applyImpulse
func (arb *Arbiter) applyImpulse() {
a := arb.ShapeA.Body
b := arb.ShapeB.Body
vr := vect.Vect{}
for _, con := range arb.Contacts {
n := con.n
r1 := con.r1
r2 := con.r2
vr.X = (-r2.Y*b.w + b.v.X) - (-r1.Y*a.w + a.v.X)
vr.Y = (r2.X*b.w + b.v.Y) - (r1.X*a.w + a.v.Y)
// Calculate and clamp the bias impulse.
jbnOld := con.jBias
con.jBias = jbnOld + (con.bias-(((((-r2.Y*b.w_bias)+b.v_bias.X)-((-r1.Y*a.w_bias)+a.v_bias.X))*n.X)+((((r2.X*b.w_bias)+b.v_bias.Y)-((r1.X*a.w_bias)+a.v_bias.Y))*n.Y)))*con.nMass
if 0 > con.jBias {
con.jBias = 0
}
// Calculate and clamp the normal impulse.
jnOld := con.jnAcc
con.jnAcc = jnOld - (con.bounce+(vr.X*n.X)+(vr.Y*n.Y))*con.nMass
if 0 > con.jnAcc {
con.jnAcc = 0
}
// Calculate and clamp the friction impulse.
jtMax := arb.u * con.jnAcc
jtOld := con.jtAcc
con.jtAcc = jtOld - (((vr.X+arb.Surface_vr.X)*-n.Y)+((vr.Y+arb.Surface_vr.Y)*n.X))*con.tMass
if con.jtAcc > jtMax {
con.jtAcc = jtMax
} else if con.jtAcc < -jtMax {
con.jtAcc = -jtMax
}
jbnOld = (con.jBias - jbnOld)
vr.X = n.X * jbnOld
vr.Y = n.Y * jbnOld
a.v_bias.X = (-vr.X * a.m_inv) + a.v_bias.X
a.v_bias.Y = (-vr.Y * a.m_inv) + a.v_bias.Y
a.w_bias += a.i_inv * ((r1.X * -vr.Y) - (r1.Y * -vr.X))
b.v_bias.X = (vr.X * b.m_inv) + b.v_bias.X
b.v_bias.Y = (vr.Y * b.m_inv) + b.v_bias.Y
b.w_bias += b.i_inv * ((r2.X * vr.Y) - (r2.Y * vr.X))
jnOld = con.jnAcc - jnOld
jtOld = con.jtAcc - jtOld
vr.X = (n.X * jnOld) - (n.Y * jtOld)
vr.Y = (n.X * jtOld) + (n.Y * jnOld)
a.v.X = (-vr.X * a.m_inv) + a.v.X
a.v.Y = (-vr.Y * a.m_inv) + a.v.Y
a.w += a.i_inv * ((r1.X * -vr.Y) - (r1.Y * -vr.X))
b.v.X = (vr.X * b.m_inv) + b.v.X
b.v.Y = (vr.Y * b.m_inv) + b.v.Y
b.w += b.i_inv * ((r2.X * vr.Y) - (r2.Y * vr.X))
}
}
