Esempio n. 1
0
func apply_impulses(a, b *Body, r1, r2, j vect.Vect) {
	j1 := vect.Vect{-j.X, -j.Y}

	a.v.Add(vect.Mult(j1, a.m_inv))
	a.w += a.i_inv * vect.Cross(r1, j1)

	b.v.Add(vect.Mult(j, b.m_inv))
	b.w += b.i_inv * vect.Cross(r2, j)
}
Esempio n. 2
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)))
			}
		}
	}
}
Esempio n. 3
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")
}
Esempio n. 4
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")
}
Esempio n. 5
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
}
Esempio n. 6
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)
}
Esempio n. 7
0
func k_scalar_body(body *Body, r, n vect.Vect) vect.Float {
	rcn := vect.Cross(r, n)
	return body.m_inv + (body.i_inv * rcn * rcn)
}
Esempio n. 8
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
}