예제 #1
0
//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))
	}
}
예제 #2
0
//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))
	}
}