Пример #1
0
func (body *Body) UpdatePosition(dt vect.Float) {
	if body.UpdatePositionFunc != nil {
		body.UpdatePositionFunc(body, dt)
		return
	}
	body.p = vect.Add(body.p, vect.Mult(vect.Add(body.v, body.v_bias), dt))
	body.setAngle(body.a + (body.w+body.w_bias)*dt)

	body.v_bias = vect.Vector_Zero
	body.w_bias = 0.0
}
Пример #2
0
func (body *Body) UpdateVelocity(gravity vect.Vect, damping, dt vect.Float) {
	if body.UpdateVelocityFunc != nil {
		body.UpdateVelocityFunc(body, gravity, damping, dt)
		return
	}
	body.v = vect.Add(vect.Mult(body.v, damping), vect.Mult(vect.Add(gravity, vect.Mult(body.f, body.m_inv)), dt))

	body.w = (body.w * damping) + (body.t * body.i_inv * dt)

	body.f = vect.Vector_Zero

}
Пример #3
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
}
Пример #4
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)
}
Пример #5
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}))

	}
}
Пример #6
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),
	}
}
Пример #7
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)
	}
}
Пример #8
0
func (poly *PolygonShape) Moment(mass float32) 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)
}
Пример #9
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)
}
Пример #10
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")
}
Пример #11
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
}
Пример #12
0
func (segment *SegmentShape) Moment(mass float32) vect.Float {

	offset := vect.Mult(vect.Add(segment.A, segment.B), 0.5)

	return vect.Float(mass) * (vect.DistSqr(segment.B, segment.A)/12.0 + vect.LengthSqr(offset))
}
Пример #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)
}
Пример #14
0
//returns the center of the aabb
func (aabb *AABB) Center() vect.Vect {
	return vect.Mult(vect.Add(aabb.Lower, aabb.Upper), 0.5)
}
Пример #15
0
//moves and roates the input vector.
func (xf *Transform) TransformVect(v vect.Vect) vect.Vect {
	return vect.Add(xf.Position, xf.RotateVect(v))
}
Пример #16
0
func (xf *Transform) TransformVectInv(v vect.Vect) vect.Vect {
	return vect.Add(vect.Mult(xf.Position, -1), xf.RotateVectInv(v))
}