Example #1
0
func (poly *PolygonShape) valueOnAxis(n vect.Vect, d float64) float64 {
	verts := poly.TVerts
	min := vect.Dot(n, verts[0])

	for i := 1; i < poly.NumVerts; i++ {
		min = math.Min(min, vect.Dot(n, verts[i]))
	}

	return min - d
}
Example #2
0
func (poly *PolygonShape) ContainsVertPartial(v, n vect.Vect) bool {
	for _, axis := range poly.TAxes {
		if vect.Dot(axis.N, n) < 0.0 {
			continue
		}
		dist := vect.Dot(axis.N, v) - axis.D
		if dist > 0.0 {
			return false
		}
	}

	return true
}
Example #3
0
func findPoinsBehindSeg(contacts *[MaxPoints]Contact, num *int, seg *SegmentShape, poly *PolygonShape, pDist, coef float64) {
	dta := vect.Cross(seg.Tn, seg.Ta)
	dtb := vect.Cross(seg.Tn, seg.Tb)
	n := vect.Mult(seg.Tn, coef)

	for i := 0; i < poly.NumVerts; i++ {
		v := poly.TVerts[i]
		if vect.Dot(v, n) < vect.Dot(seg.Tn, seg.Ta)*coef+seg.Radius {
			dt := vect.Cross(seg.Tn, v)
			if dta >= dt && dt >= dtb {
				nextContact(contacts, num).reset(v, n, pDist)
			}
		}
	}
}
Example #4
0
func segmentEncapQuery(p1, p2 vect.Vect, r1, r2 float64, con *Contact, tangent vect.Vect) int {
	count := circle2circleQuery(p1, p2, r1, r2, con)
	if vect.Dot(con.Normal, tangent) >= 0.0 {
		return count
	} else {
		return 0
	}
	panic("Never reached")
}
Example #5
0
func (poly *PolygonShape) ContainsVert(v vect.Vect) bool {
	for _, axis := range poly.TAxes {
		dist := vect.Dot(axis.N, v) - axis.D
		if dist > 0.0 {
			return false
		}
	}

	return true
}
Example #6
0
func circle2segmentFunc(contacts *[MaxPoints]Contact, circle *CircleShape, segment *SegmentShape) int {
	rsum := circle.Radius + segment.Radius

	//Calculate normal distance from segment
	dn := vect.Dot(segment.Tn, circle.Tc) - vect.Dot(segment.Ta, segment.Tn)
	dist := math.Abs(dn) - rsum
	if dist > 0.0 {
		return 0
	}

	//Calculate tangential distance along segment
	dt := -vect.Cross(segment.Tn, circle.Tc)
	dtMin := -vect.Cross(segment.Tn, segment.Ta)
	dtMax := -vect.Cross(segment.Tn, segment.Tb)

	// Decision tree to decide which feature of the segment to collide with.
	if dt < dtMin {
		if dt < (dtMin - rsum) {
			return 0
		} else {
			return segmentEncapQuery(circle.Tc, segment.Ta, circle.Radius, segment.Radius, &contacts[0], segment.A_tangent)
		}
	} else {
		if dt < dtMax {
			n := segment.Tn
			if dn >= 0.0 {
				n.Mult(-1)
			}
			con := &contacts[0]
			pos := vect.Add(circle.Tc, vect.Mult(n, circle.Radius+dist*0.5))
			con.reset(pos, n, dist)
			return 1
		} else {
			if dt < (dtMax + rsum) {
				return segmentEncapQuery(circle.Tc, segment.Tb, circle.Radius, segment.Radius, &contacts[0], segment.B_tangent)
			} else {
				return 0
			}
		}
	}
	panic("Never reached")
}
Example #7
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)
		}

	}
}
Example #8
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")
}
Example #9
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)
	}
}
Example #10
0
// Calculates the transformed vertices and axes and the bounding box.
func (poly *PolygonShape) update(xf transform.Transform) aabb.AABB {
	//transform axes
	{
		src := poly.Axes
		dst := poly.TAxes

		for i := 0; i < poly.NumVerts; i++ {
			n := xf.RotateVect(src[i].N)
			dst[i].N = n
			dst[i].D = vect.Dot(xf.Position, n) + src[i].D
		}
	}
	//transform verts
	{
		inf := math.Inf(1)
		aabb := aabb.AABB{
			Lower: vect.Vect{inf, inf},
			Upper: vect.Vect{-inf, -inf},
		}

		src := poly.Verts
		dst := poly.TVerts

		for i := 0; i < poly.NumVerts; i++ {
			v := xf.TransformVect(src[i])

			dst[i] = v
			aabb.Lower.X = math.Min(aabb.Lower.X, v.X)
			aabb.Upper.X = math.Max(aabb.Upper.X, v.X)
			aabb.Lower.Y = math.Min(aabb.Lower.Y, v.Y)
			aabb.Upper.Y = math.Max(aabb.Upper.Y, v.Y)
		}

		return aabb
	}
}
Example #11
0
func seg2polyFunc(contacts *[MaxPoints]Contact, seg *SegmentShape, poly *PolygonShape) int {
	axes := poly.TAxes

	segD := vect.Dot(seg.Tn, seg.Ta)
	minNorm := poly.ValueOnAxis(seg.Tn, segD) - seg.Radius
	minNeg := poly.ValueOnAxis(vect.Mult(seg.Tn, -1), -segD) - seg.Radius
	if minNeg > 0.0 || minNorm > 0.0 {
		return 0
	}

	mini := 0
	poly_min := segValueOnAxis(seg, axes[0].N, axes[0].D)
	if poly_min > 0.0 {
		return 0
	}

	for i := 0; i < poly.NumVerts; i++ {
		dist := segValueOnAxis(seg, axes[i].N, axes[i].D)
		if dist > 0.0 {
			return 0
		} else if dist > poly_min {
			poly_min = dist
			mini = i
		}
	}

	num := 0

	poly_n := vect.Mult(axes[mini].N, -1)

	va := vect.Add(seg.Ta, vect.Mult(poly_n, seg.Radius))
	vb := vect.Add(seg.Tb, vect.Mult(poly_n, seg.Radius))
	if poly.ContainsVert(va) {
		nextContact(contacts, &num).reset(va, poly_n, poly_min)
	}
	if poly.ContainsVert(vb) {
		nextContact(contacts, &num).reset(vb, poly_n, poly_min)
	}

	if minNorm >= poly_min || minNeg >= poly_min {
		if minNorm > minNeg {
			findPoinsBehindSeg(contacts, &num, seg, poly, minNorm, 1.0)
		} else {
			findPoinsBehindSeg(contacts, &num, seg, poly, minNeg, -1.0)
		}
	}

	// If no other collision points are found, try colliding endpoints.
	if num == 0 {
		poly_a := poly.TVerts[mini]
		poly_b := poly.TVerts[(mini+1)%poly.NumVerts]

		if segmentEncapQuery(seg.Ta, poly_a, seg.Radius, 0.0, &contacts[0], vect.Mult(seg.A_tangent, -1)) != 0 {
			return 1
		}
		if segmentEncapQuery(seg.Tb, poly_a, seg.Radius, 0.0, &contacts[0], vect.Mult(seg.B_tangent, -1)) != 0 {
			return 1
		}
		if segmentEncapQuery(seg.Ta, poly_b, seg.Radius, 0.0, &contacts[0], vect.Mult(seg.A_tangent, -1)) != 0 {
			return 1
		}
		if segmentEncapQuery(seg.Tb, poly_b, seg.Radius, 0.0, &contacts[0], vect.Mult(seg.B_tangent, -1)) != 0 {
			return 1
		}
	}

	return num
}
Example #12
0
func segValueOnAxis(seg *SegmentShape, n vect.Vect, d float64) float64 {
	a := vect.Dot(n, seg.Ta) - seg.Radius
	b := vect.Dot(n, seg.Tb) - seg.Radius
	return math.Min(a, b) - d
}
Example #13
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
}
Example #14
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)
	}
}