Пример #1
0
func (poly *PolygonShape) valueOnAxis(n vect.Vect, d vect.Float) vect.Float {
	verts := poly.TVerts
	min := vect.Dot(n, verts[0])

	for i := 1; i < poly.NumVerts; i++ {
		min = vect.FMin(min, vect.Dot(n, verts[i]))
	}
	//fmt.Println(min, d)
	return min - d
}
Пример #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
}
Пример #3
0
func findPoinsBehindSeg(contacts []*Contact, num *int, seg *SegmentShape, poly *PolygonShape, pDist, coef vect.Float) {
	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, hashPair(poly.Shape.Hash(), HashValue(i)))
			}
		}
	}
}
Пример #4
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}))

	}
}
Пример #5
0
func segmentEncapQuery(p1, p2 vect.Vect, r1, r2 vect.Float, con *Contact, tangent vect.Vect) int {
	count := circle2circleQuery(p1, p2, r1, r2, con)
	if vect.Dot(con.n, tangent) >= 0.0 {
		return count
	} else {
		return 0
	}
	panic("Never reached")
}
Пример #6
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
}
Пример #7
0
func circle2segmentFunc(contacts []*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 := vect.FAbs(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, 0)
			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")
}
Пример #8
0
func (body *Body) KineticEnergy() vect.Float {
	vsq := vect.Dot(body.v, body.v)
	wsq := body.w * body.w
	if vsq != 0 {
		vsq = vsq * body.m
	}
	if wsq != 0 {
		wsq = wsq * body.i
	}
	return vsq + wsq
}
Пример #9
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
	}
}
Пример #10
0
func (poly *PolygonShape) Moment(mass vect.Float) vect.Float {

	sum1 := vect.Float(0)
	sum2 := vect.Float(0)

	println("using bad Moment calculation")
	offset := vect.Vect{0, 0}

	for i := 0; i < poly.NumVerts; i++ {

		v1 := vect.Add(poly.Verts[i], offset)
		v2 := vect.Add(poly.Verts[(i+1)%poly.NumVerts], offset)

		a := vect.Cross(v2, v1)
		b := vect.Dot(v1, v1) + vect.Dot(v1, v2) + vect.Dot(v2, v2)

		sum1 += a * b
		sum2 += a
	}

	return (vect.Float(mass) * sum1) / (6.0 * sum2)
}
Пример #11
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")
}
Пример #12
0
// Calculates the transformed vertices and axes and the bounding box.
func (poly *PolygonShape) update(xf transform.Transform) 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
		}
		/*
			fmt.Println("")
			fmt.Println("Started Axes")
			fmt.Println(xf.Rotation, xf.Position)
			for i:=0;i<poly.NumVerts;i++ {
				fmt.Println(src[i], dst[i])
			}
		*/
	}
	//transform verts
	{
		inf := vect.Float(math.Inf(1))
		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 = vect.FMin(aabb.Lower.X, v.X)
			aabb.Upper.X = vect.FMax(aabb.Upper.X, v.X)
			aabb.Lower.Y = vect.FMin(aabb.Lower.Y, v.Y)
			aabb.Upper.Y = vect.FMax(aabb.Upper.Y, v.Y)
		}

		/*
			fmt.Println("Verts")
			for i:=0;i<poly.NumVerts;i++ {
				fmt.Println(src[i], dst[i])
			}
		*/
		return aabb
	}
}
Пример #13
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)
	}
}
Пример #14
0
func seg2polyFunc(contacts []*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, hashPair(seg.Shape.Hash(), 0))
	}
	if poly.ContainsVert(vb) {
		nextContact(contacts, &num).reset(vb, poly_n, poly_min, hashPair(seg.Shape.Hash(), 1))
	}

	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
}
Пример #15
0
func segValueOnAxis(seg *SegmentShape, n vect.Vect, d vect.Float) vect.Float {
	a := vect.Dot(n, seg.Ta) - seg.Radius
	b := vect.Dot(n, seg.Tb) - seg.Radius
	return vect.FMin(a, b) - d
}
Пример #16
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
}
Пример #17
0
func mult_k(vr, k1, k2 vect.Vect) vect.Vect {
	return vect.Vect{vect.Dot(vr, k1), vect.Dot(vr, k2)}
}
Пример #18
0
func normal_relative_velocity(a, b *Body, r1, r2, n vect.Vect) vect.Float {
	return vect.Dot(relative_velocity(a, b, r1, r2), n)
}
Пример #19
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
}