Пример #1
0
func (arb *Arbiter) preStep2(inv_dt, slop, bias vect.Float) {

	a := arb.ShapeA.Body
	b := arb.ShapeB.Body

	for i := 0; i < arb.NumContacts; i++ {
		con := arb.Contacts[i]

		// Calculate the offsets.
		con.r1 = vect.Sub(con.p, a.p)
		con.r2 = vect.Sub(con.p, b.p)

		//con.Normal = vect.Vect{-1,0}

		// Calculate the mass normal and mass tangent.
		con.nMass = 1.0 / k_scalar(a, b, con.r1, con.r2, con.n)
		con.tMass = 1.0 / k_scalar(a, b, con.r1, con.r2, vect.Perp(con.n))

		// Calculate the target bias velocity.
		con.bias = -bias * inv_dt * vect.FMin(0.0, con.dist+slop)
		con.jBias = 0.0
		//con.jtAcc = 0
		//con.jnAcc = 0
		//fmt.Println("con.dist", con.dist)

		// Calculate the target bounce velocity.
		con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n) * arb.e
	}
}
Пример #2
0
func (arb *Arbiter) applyImpulse3() {
	a := arb.ShapeA.Body
	b := arb.ShapeB.Body

	for i := 0; i < arb.NumContacts; i++ {
		con := arb.Contacts[i]
		n := con.n
		r1 := con.r1
		r2 := con.r2

		// Calculate the relative bias velocities.
		vb1 := vect.Add(a.v_bias, vect.Mult(vect.Perp(r1), a.w_bias))
		vb2 := vect.Add(b.v_bias, vect.Mult(vect.Perp(r2), b.w_bias))
		vbn := vect.Dot(vect.Sub(vb2, vb1), n)

		// Calculate the relative velocity.
		vr := relative_velocity(a, b, r1, r2)
		vrn := vect.Dot(vr, n)
		// Calculate the relative tangent velocity.
		vrt := vect.Dot(vect.Add(vr, arb.Surface_vr), vect.Perp(n))

		// Calculate and clamp the bias impulse.
		jbn := (con.bias - vbn) * con.nMass
		jbnOld := con.jBias
		con.jBias = vect.FMax(jbnOld+jbn, 0.0)

		// Calculate and clamp the normal impulse.
		jn := -(con.bounce + vrn) * con.nMass
		jnOld := con.jnAcc
		con.jnAcc = vect.FMax(jnOld+jn, 0.0)

		// Calculate and clamp the friction impulse.
		jtMax := arb.u * con.jnAcc
		jt := -vrt * con.tMass
		jtOld := con.jtAcc
		con.jtAcc = vect.FClamp(jtOld+jt, -jtMax, jtMax)

		// Apply the bias impulse.
		apply_bias_impulses(a, b, r1, r2, vect.Mult(n, con.jBias-jbnOld))

		// Apply the final impulse.
		apply_impulses(a, b, r1, r2, transform.RotateVect(n, transform.Rotation{con.jnAcc - jnOld, con.jtAcc - jtOld}))

	}
}
Пример #3
0
func (arb *Arbiter) preStep(inv_dt, slop, bias vect.Float) {

	a := arb.ShapeA.Body
	b := arb.ShapeB.Body

	for _, con := range arb.Contacts {
		// Calculate the offsets.
		x, y := con.p.X, con.p.Y
		r1 := vect.Vect{x - a.p.X, y - a.p.Y}
		r2 := vect.Vect{x - b.p.X, y - b.p.Y}

		//con.Normal = vect.Vect{-1,0}

		// Calculate the mass normal and mass tangent.
		n := con.n
		rcn := (r1.X * n.Y) - (r1.Y * n.X)
		rcn = a.m_inv + (a.i_inv * rcn * rcn)

		rcn2 := (r2.X * n.Y) - (r2.Y * n.X)
		rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)

		value := rcn + rcn2
		if value == 0.0 {
			fmt.Printf("Warning: Unsolvable collision or constraint.")
		}
		con.nMass = 1.0 / value

		n = vect.Perp(con.n)
		rcn = (r1.X * n.Y) - (r1.Y * n.X)
		rcn = a.m_inv + (a.i_inv * rcn * rcn)

		rcn2 = (r2.X * n.Y) - (r2.Y * n.X)
		rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)

		value = rcn + rcn2
		if value == 0.0 {
			fmt.Printf("Warning: Unsolvable collision or constraint.")
		}
		con.tMass = 1.0 / value

		// Calculate the target bias velocity.
		ds := con.dist + slop
		if 0 > ds {
			con.bias = -bias * inv_dt * (con.dist + slop)
		} else {
			con.bias = 0
		}
		con.jBias = 0.0
		con.bounce = vect.Dot(vect.Vect{(-r2.Y*b.w + b.v.X) - (-r1.Y*a.w + a.v.X), (r2.X*b.w + b.v.Y) - (r1.X*a.w + a.v.Y)}, con.n) * arb.e
		con.r1 = r1
		con.r2 = r2
	}
}
Пример #4
0
//Called to update N, Tn, Ta, Tb and the the bounding box.
func (segment *SegmentShape) update(xf transform.Transform) AABB {
	a := xf.TransformVect(segment.A)
	b := xf.TransformVect(segment.B)
	segment.Ta = a
	segment.Tb = b
	segment.N = vect.Perp(vect.Normalize(vect.Sub(segment.B, segment.A)))
	segment.Tn = xf.RotateVect(segment.N)

	rv := vect.Vect{segment.Radius, segment.Radius}

	min := vect.Min(a, b)
	min.Sub(rv)

	max := vect.Max(a, b)
	max.Add(rv)

	return AABB{
		min,
		max,
	}
}
Пример #5
0
// Sets the vertices offset by the offset and calculates the PolygonAxes.
func (poly *PolygonShape) SetVerts(verts Vertices, offset vect.Vect) {

	if verts == nil {
		log.Printf("Error: no vertices passed!")
		return
	}

	if verts.ValidatePolygon() == false {
		log.Printf("Warning: vertices not valid")
	}

	numVerts := len(verts)
	oldnumVerts := len(poly.Verts)
	poly.NumVerts = numVerts

	if oldnumVerts < numVerts {
		//create new slices
		poly.Verts = make(Vertices, numVerts)
		poly.TVerts = make(Vertices, numVerts)
		poly.Axes = make([]PolygonAxis, numVerts)
		poly.TAxes = make([]PolygonAxis, numVerts)

	} else {
		//reuse old slices
		poly.Verts = poly.Verts[:numVerts]
		poly.TVerts = poly.TVerts[:numVerts]
		poly.Axes = poly.Axes[:numVerts]
		poly.TAxes = poly.TAxes[:numVerts]
	}

	for i := 0; i < numVerts; i++ {
		a := vect.Add(offset, verts[i])
		b := vect.Add(offset, verts[(i+1)%numVerts])
		n := vect.Normalize(vect.Perp(vect.Sub(b, a)))

		poly.Verts[i] = a
		poly.Axes[i].N = n
		poly.Axes[i].D = vect.Dot(n, a)
	}
}
Пример #6
0
func relative_velocity2(a, b *Body, r1, r2 vect.Vect) vect.Vect {
	v1 := vect.Add(b.v, vect.Mult(vect.Perp(r2), b.w))
	v2 := vect.Add(a.v, vect.Mult(vect.Perp(r1), a.w))
	return vect.Sub(v1, v2)
}