Beispiel #1
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
}
Beispiel #2
0
func TestOverlap(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
}
Beispiel #3
0
func (arb *Arbiter) preStep(inv_dt float64) {
	const allowedPenetration = 0.01
	biasFactor := 0.0
	if Settings.PositionCorrection {
		biasFactor = 0.2
	}

	b1 := arb.ShapeA.Body
	b2 := arb.ShapeB.Body

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

		c.R1 = vect.Sub(c.Position, b1.Transform.Position)
		c.R2 = vect.Sub(c.Position, b2.Transform.Position)
		r1 := c.R1
		r2 := c.R2

		//Precompute normal mass, tangent mass, and bias
		rn1 := vect.Dot(r1, c.Normal)
		rn2 := vect.Dot(r2, c.Normal)
		kNormal := b1.invMass + b2.invMass
		kNormal += b1.invI*(vect.Dot(r1, r1)-rn1*rn1) +
			b2.invI*(vect.Dot(r2, r2)-rn2*rn2)
		c.MassNormal = 1.0 / kNormal

		tangent := vect.CrossVF(c.Normal, 1.0)
		rt1 := vect.Dot(r1, tangent)
		rt2 := vect.Dot(r2, tangent)
		kTangent := b1.invMass + b2.invMass
		kTangent += b1.invI*(vect.Dot(r1, r1)-rt1*rt1) +
			b2.invI*(vect.Dot(r2, r2)-rt2*rt2)
		c.MassTangent = 1.0 / kTangent

		c.Bias = -biasFactor * inv_dt * math.Min(0.0, c.Separation+allowedPenetration)

		if Settings.AccumulateImpulses {
			//Apply normal + friction impulse
			P := vect.Add(vect.Mult(c.Normal, c.Pn), vect.Mult(tangent, c.Pt))

			b1.Velocity.Sub(vect.Mult(P, b1.invMass))
			b1.AngularVelocity -= b1.invI * vect.Cross(r1, P)

			b2.Velocity.Add(vect.Mult(P, b2.invMass))
			b2.AngularVelocity += b2.invI * vect.Cross(r2, P)
		}

	}
}
Beispiel #4
0
func circle2circleQuery(p1, p2 vect.Vect, r1, r2 float64, con *Contact) int {
	minDist := r1 + r2

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

	if distSqr >= minDist*minDist {
		return 0
	}

	dist := math.Sqrt(distSqr)

	pDist := dist
	if dist == 0.0 {
		pDist = 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)

	return 1
}
Beispiel #5
0
// Recalculates the global center of the circle and the the bounding box.
func (circle *CircleShape) update(xf transform.Transform) aabb.AABB {
	//global center of the circle
	center := xf.TransformVect(circle.Position)
	circle.Tc = center
	rv := vect.Vect{circle.Radius, circle.Radius}

	return aabb.AABB{
		vect.Sub(center, rv),
		vect.Add(center, rv),
	}
}
Beispiel #6
0
// Calls ShapeClass.update and sets the new AABB.
func (shape *Shape) Update() {
	if shape.Body == nil {
		log.Printf("Error: uninitialized shape")
		return
	}
	body := shape.Body

	shape.AABB = shape.ShapeClass.update(body.Transform)
	proxy := &shape.proxy
	proxy.AABB = shape.AABB

	if body.Space != nil {
		d := vect.Sub(body.Transform.Position, body.prevTransform.Position)
		body.Space.BroadPhase.moveProxy(proxy.ProxyId, proxy.AABB, d)
	}
}
Beispiel #7
0
//Called to update N, Tn, Ta, Tb and the the bounding box.
func (segment *SegmentShape) update(xf transform.Transform) aabb.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.AABB{
		min,
		max,
	}
}
Beispiel #8
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)
	}
}
Beispiel #9
0
func circle2polyFunc(contacts *[MaxPoints]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,
		)
		return 1
	} else {
		return circle2circleQuery(circle.Tc, a, circle.Radius, 0.0, &contacts[0])
	}
	panic("Never reached")
}
Beispiel #10
0
func DrawShape(shape *collision.Shape) {
	switch shape.ShapeType() {
	case collision.ShapeType_Circle:
		circle := shape.ShapeClass.(*collision.CircleShape)
		DrawCircle(circle.Tc, circle.Radius, false)
		const circleMarkerSize = 0.08
		{
			p1 := vect.Add(circle.Tc, vect.Vect{0, circleMarkerSize})
			p2 := vect.Sub(circle.Tc, vect.Vect{0, circleMarkerSize})
			DrawLine(p1, p2)
		}
		{
			p1 := vect.Add(circle.Tc, vect.Vect{circleMarkerSize, 0})
			p2 := vect.Sub(circle.Tc, vect.Vect{circleMarkerSize, 0})
			DrawLine(p1, p2)
		}
		break
	case collision.ShapeType_Segment:
		segment := shape.ShapeClass.(*collision.SegmentShape)
		a := segment.Ta
		b := segment.Tb
		r := segment.Radius
		DrawLine(a, b)
		if segment.Radius > 0.0 {
			DrawCircle(a, r, false)
			DrawCircle(b, r, false)

			verts := [4]vect.Vect{
				vect.Add(a, vect.Vect{0, r}),
				vect.Add(a, vect.Vect{0, -r}),
				vect.Add(b, vect.Vect{0, -r}),
				vect.Add(b, vect.Vect{0, r}),
			}
			DrawPoly(verts[:], 4, false)

		}
		if Settings.DrawNormals {
			n := segment.Tn
			DrawLine(a, vect.Add(a, n))
			DrawLine(b, vect.Add(b, n))
		}
	case collision.ShapeType_Polygon:
		poly := shape.ShapeClass.(*collision.PolygonShape)
		verts := poly.TVerts
		DrawPoly(verts, poly.NumVerts, false)
		if Settings.DrawNormals {
			axes := poly.TAxes
			for i, v := range verts {
				a := axes[i]
				v1 := v
				v2 := verts[(i+1)%len(verts)]
				DrawLine(v1, vect.Add(v1, a.N))
				DrawLine(v2, vect.Add(v2, a.N))
			}
		}

	case collision.ShapeType_Box:
		poly := shape.ShapeClass.(*collision.BoxShape).Polygon
		verts := poly.TVerts
		DrawPoly(verts, poly.NumVerts, false)
	}
}
Beispiel #11
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
}
Beispiel #12
0
func (arb *Arbiter) applyImpulse() {
	sA := arb.ShapeA
	sB := arb.ShapeB

	b1 := sA.Body
	b2 := sB.Body

	//xfA := b1.Transform
	//xfB := b2.Transform

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

		// Relative velocity at contact
		dv := vect.Vect{}
		{
			t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2))
			t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1))

			dv = vect.Sub(t1, t2)
		}

		// Compute normal impulse
		vn := vect.Dot(dv, c.Normal)

		dPn := c.MassNormal * (-vn + c.Bias)

		if Settings.AccumulateImpulses {
			// Clamp the accumulated impulse
			Pn0 := c.Pn
			c.Pn = math.Max(Pn0+dPn, 0.0)
			dPn = c.Pn - Pn0
		} else {
			dPn = math.Max(dPn, 0.0)
		}

		//Apply contact impulse
		Pn := vect.Mult(c.Normal, dPn)

		b1.Velocity.Sub(vect.Mult(Pn, b1.invMass))
		b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pn)

		b2.Velocity.Add(vect.Mult(Pn, b2.invMass))
		b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pn)

		//Relative velocity at contact
		{
			t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2))
			t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1))

			dv = vect.Sub(t1, t2)
		}

		tangent := vect.CrossVF(c.Normal, 1.0)
		vt := vect.Dot(dv, tangent)
		dPt := c.MassTangent * (-vt)

		if Settings.AccumulateImpulses {
			//Compute friction impulse
			maxPt := arb.Friction * c.Pn

			//Clamp Friction
			oldTangentImpulse := c.Pt
			c.Pt = clamp(oldTangentImpulse+dPt, -maxPt, maxPt)
			dPt = c.Pt - oldTangentImpulse
		} else {
			maxPt := arb.Friction * dPn
			dPt = clamp(dPt, -maxPt, maxPt)
		}

		// Apply contact impulse
		Pt := vect.Mult(tangent, dPt)

		b1.Velocity.Sub(vect.Mult(Pt, b1.invMass))
		b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pt)

		b2.Velocity.Add(vect.Mult(Pt, b2.invMass))
		b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pt)
	}
}
Beispiel #13
0
func (aabb *AABB) Perimeter() float64 {
	w := vect.Sub(aabb.Upper, aabb.Lower)
	return 2 * (w.X + w.Y)
}
Beispiel #14
0
func (aabb *AABB) Extents() vect.Vect {
	return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5)
}