Ejemplo n.º 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
	}
}
Ejemplo n.º 2
0
// Checks if verts forms a valid polygon.
// The vertices must be convex and winded clockwise.
func (verts Vertices) ValidatePolygon() bool {
	numVerts := len(verts)
	for i := 0; i < numVerts; i++ {
		a := verts[i]
		b := verts[(i+1)%numVerts]
		c := verts[(i+2)%numVerts]

		if vect.Cross(vect.Sub(b, a), vect.Sub(c, b)) > 0.0 {
			return false
		}
	}

	return true
}
Ejemplo n.º 3
0
func TestOverlap2(a, b AABB) bool {

	d1 := vect.Sub(b.Lower, a.Upper)
	d2 := vect.Sub(a.Lower, b.Upper)

	if d1.X > 0.0 || d1.Y > 0.0 {
		return false
	}

	if d2.X > 0.0 || d2.Y > 0.0 {
		return false
	}

	return true
}
Ejemplo n.º 4
0
func (this *PivotJoint) ApplyImpulse() {
	a, b := this.BodyA, this.BodyB
	r1, r2 := this.r1, this.r2

	// compute relative velocity
	vr := relative_velocity2(a, b, r1, r2)

	// compute normal impulse
	j := mult_k(vect.Sub(this.bias, vr), this.k1, this.k2)
	jOld := this.jAcc
	this.jAcc = vect.Clamp(vect.Add(this.jAcc, j), this.jMaxLen)
	j = vect.Sub(this.jAcc, jOld)
	// apply impulse
	apply_impulses(a, b, this.r1, this.r2, j)
}
Ejemplo n.º 5
0
func circle2circleQuery(p1, p2 vect.Vect, r1, r2 vect.Float, con *Contact) int {
	minDist := r1 + r2

	delta := vect.Sub(p2, p1)
	distSqr := delta.LengthSqr()

	if distSqr >= minDist*minDist {
		return 0
	}

	dist := vect.Float(math.Sqrt(float64(distSqr)))

	pDist := dist
	if dist == 0.0 {
		pDist = vect.Float(math.Inf(1))
	}

	pos := vect.Add(p1, vect.Mult(delta, 0.5+(r1-0.5*minDist)/pDist))

	norm := vect.Vect{1, 0}

	if dist != 0.0 {
		norm = vect.Mult(delta, 1.0/dist)
	}

	con.reset(pos, norm, dist-minDist, 0)

	return 1
}
Ejemplo n.º 6
0
func (arb *Arbiter) update(a, b *Shape, contacts []*Contact, numContacts int) {
	oldContacts := arb.Contacts
	arb.ShapeA, arb.ShapeB = a, b
	arb.BodyA, arb.BodyB = arb.ShapeA.Body, arb.ShapeB.Body

	for _, oldC := range oldContacts {
		for _, newC := range contacts {
			if newC.hash == oldC.hash {
				newC.jnAcc = oldC.jnAcc
				newC.jtAcc = oldC.jtAcc
				newC.jBias = oldC.jBias
			}
		}
	}

	arb.Contacts = contacts
	arb.NumContacts = numContacts

	arb.u = a.u * b.u
	arb.e = a.e * b.e

	arb.Surface_vr = vect.Sub(a.Surface_v, b.Surface_v)

	if arb.state == arbiterStateCached {
		arb.state = arbiterStateFirstColl
	}
}
Ejemplo n.º 7
0
// Recalculates the global center of the circle and the the bounding box.
func (circle *CircleShape) update(xf transform.Transform) AABB {
	//global center of the circle
	center := xf.TransformVect(circle.Position)
	circle.Tc = center
	rv := vect.Vect{circle.Radius, circle.Radius}

	return AABB{
		vect.Sub(center, rv),
		vect.Add(center, rv),
	}
}
Ejemplo n.º 8
0
func (this *PivotJoint) PreStep(dt vect.Float) {
	a, b := this.BodyA, this.BodyB

	this.r1 = transform.RotateVect(this.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
	this.r2 = transform.RotateVect(this.Anchor2, transform.Rotation{b.rot.X, b.rot.Y})

	// Calculate mass tensor
	k_tensor(a, b, this.r1, this.r2, &this.k1, &this.k2)

	// compute max impulse
	this.jMaxLen = this.MaxForce * dt

	// calculate bias velocity
	delta := vect.Sub(vect.Add(b.p, this.r2), vect.Add(a.p, this.r1))

	this.bias = vect.Clamp(vect.Mult(delta, -bias_coef(this.ErrorBias, dt)/dt), this.MaxBias)
}
Ejemplo n.º 9
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}))

	}
}
Ejemplo n.º 10
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,
	}
}
Ejemplo n.º 11
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)
	}
}
Ejemplo n.º 12
0
func circle2polyFunc(contacts []*Contact, circle *CircleShape, poly *PolygonShape) int {

	axes := poly.TAxes

	mini := 0
	min := vect.Dot(axes[0].N, circle.Tc) - axes[0].D - circle.Radius
	for i, axis := range axes {
		dist := vect.Dot(axis.N, circle.Tc) - axis.D - circle.Radius
		if dist > 0.0 {
			return 0
		} else if dist > min {
			min = dist
			mini = i
		}
	}

	n := axes[mini].N
	a := poly.TVerts[mini]
	b := poly.TVerts[(mini+1)%poly.NumVerts]
	dta := vect.Cross(n, a)
	dtb := vect.Cross(n, b)
	dt := vect.Cross(n, circle.Tc)

	if dt < dtb {
		return circle2circleQuery(circle.Tc, b, circle.Radius, 0.0, contacts[0])
	} else if dt < dta {
		contacts[0].reset(
			vect.Sub(circle.Tc, vect.Mult(n, circle.Radius+min/2.0)),
			vect.Mult(n, -1),
			min,
			0,
		)
		return 1
	} else {
		return circle2circleQuery(circle.Tc, a, circle.Radius, 0.0, contacts[0])
	}
	panic("Never reached")
}
Ejemplo n.º 13
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)
}
Ejemplo n.º 14
0
func (S1 *Segment) Intersects(S2 *Segment) bool {
	u := vect.Sub(S1.Point2, S1.Point1)
	v := vect.Sub(S2.Point2, S2.Point1)
	w := vect.Sub(S1.Point1, S2.Point1)
	D := vect.Cross(u, v)

	// test if  they are parallel (includes either being a point)
	if vect.FAbs(D) < 0.0000001 { // S1 and S2 are parallel
		if vect.Cross(u, w) != 0 || vect.Cross(v, w) != 0 {
			return false // they are NOT collinear
		}
		// they are collinear or degenerate
		// check if they are degenerate  points
		du := vect.Dot(u, u)
		dv := vect.Dot(v, v)
		if du == 0 && dv == 0 { // both segments are points
			if !vect.Equals(S1.Point1, S2.Point1) { // they are distinct  points
				return false
			}
			return true
		}
		if du == 0 { // S1 is a single point
			if !S2.Contains(S1.Point1) { // but is not in S2
				return false
			}
			return true
		}
		if dv == 0 { // S2 a single point
			if !S1.Contains(S2.Point1) { // but is not in S1
				return false
			}
			return true
		}
		// they are collinear segments - get  overlap (or not)
		var t0, t1 vect.Float // endpoints of S1 in eqn for S2
		w2 := vect.Sub(S1.Point2, S2.Point1)
		if v.X != 0 {
			t0 = w.X / v.X
			t1 = w2.X / v.X
		} else {
			t0 = w.Y / v.Y
			t1 = w2.Y / v.Y
		}
		if t0 > t1 { // must have t0 smaller than t1
			t0, t1 = t1, t0 // swap if not
		}
		if t0 > 1 || t1 < 0 {
			return false // NO overlap
		}
		return true
	}

	// the segments are skew and may intersect in a point
	// get the intersect parameter for S1
	sI := vect.Cross(v, w) / D
	if sI < 0 || sI > 1 { // no intersect with S1
		return false
	}
	// get the intersect parameter for S2
	tI := vect.Cross(u, w) / D
	if tI < 0 || tI > 1 { // no intersect with S2
		return false
	}
	return true
}
Ejemplo n.º 15
0
func (aabb *AABB) Extents() vect.Vect {
	return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5)
}
Ejemplo n.º 16
0
func (aabb *AABB) Perimeter() vect.Float {
	w := vect.Sub(aabb.Upper, aabb.Lower)
	return 2 * (w.X + w.Y)
}
Ejemplo n.º 17
0
// Returns true if the given point is located inside the circle.
func (circle *CircleShape) TestPoint(point vect.Vect) bool {
	d := vect.Sub(point, circle.Tc)

	return vect.Dot(d, d) <= circle.Radius*circle.Radius
}