Esempio n. 1
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
}
Esempio n. 2
0
func addBall() {
	x := rand.Intn(350-115) + 115
	ball := chipmunk.NewCircle(vect.Vector_Zero, float32(ballRadius))
	ball.SetElasticity(0.95)

	body := chipmunk.NewBody(vect.Float(ballMass), ball.Moment(float32(ballMass)))
	body.SetPosition(vect.Vect{vect.Float(x), 600.0})
	body.SetAngle(vect.Float(rand.Float32() * 2 * math.Pi))

	body.AddShape(ball)
	space.AddBody(body)
	balls = append(balls, ball)
}
Esempio n. 3
0
// Creates a new CircleShape with the given center and radius.
func NewCircle(pos vect.Vect, radius float32) *Shape {
	shape := newShape()
	circle := &CircleShape{
		Position: pos,
		Radius:   vect.Float(radius),
		Shape:    shape,
	}
	shape.ShapeClass = circle
	return shape
}
Esempio n. 4
0
func k_tensor(a, b *Body, r1, r2 vect.Vect, k1, k2 *vect.Vect) {
	// calculate mass matrix
	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
	m_sum := a.m_inv + b.m_inv

	// start with I*m_sum
	k11 := vect.Float(m_sum)
	k12 := vect.Float(0)
	k21 := vect.Float(0)
	k22 := vect.Float(m_sum)

	// add the influence from r1
	a_i_inv := a.i_inv
	r1xsq := r1.X * r1.X * a_i_inv
	r1ysq := r1.Y * r1.Y * a_i_inv
	r1nxy := -r1.X * r1.Y * a_i_inv
	k11 += r1ysq
	k12 += r1nxy
	k21 += r1nxy
	k22 += r1xsq

	// add the influnce from r2
	b_i_inv := b.i_inv
	r2xsq := r2.X * r2.X * b_i_inv
	r2ysq := r2.Y * r2.Y * b_i_inv
	r2nxy := -r2.X * r2.Y * b_i_inv
	k11 += r2ysq
	k12 += r2nxy
	k21 += r2nxy
	k22 += r2xsq

	// invert
	determinant := (k11 * k22) - (k12 * k21)
	if determinant == 0 {
		panic("Unsolvable constraint.")
	}

	det_inv := 1.0 / determinant
	*k1 = vect.Vect{k22 * det_inv, -k12 * det_inv}
	*k2 = vect.Vect{-k21 * det_inv, k11 * det_inv}
}
Esempio n. 5
0
// step advances the physics engine and cleans up any balls that are off-screen
func step(dt float32) {
	space.Step(vect.Float(dt))

	for i := 0; i < len(balls); i++ {
		p := balls[i].Body.Position()
		if p.Y < -100 {
			space.RemoveBody(balls[i].Body)
			balls[i] = nil
			balls = append(balls[:i], balls[i+1:]...)
			i-- // consider same index again
		}
	}
}
Esempio n. 6
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)
}
Esempio n. 7
0
func (spring *DampedSpring) PreStep(dt vect.Float) {
	a := spring.BodyA
	b := spring.BodyB

	spring.r1 = transform.RotateVect(spring.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
	spring.r2 = transform.RotateVect(spring.Anchor2, transform.Rotation{a.rot.X, a.rot.Y})

	delta := vect.Sub(vect.Add(b.p, spring.r2), vect.Add(a.p, spring.r1))
	dist := vect.Length(delta)
	if dist == 0 {
		dist = vect.Float(math.Inf(1))
	}
	spring.n = vect.Mult(delta, 1.0/dist)

	k := k_scalar(a, b, spring.r1, spring.r2, spring.n)
	spring.nMass = 1.0 / k

	spring.targetVRN = 0.0
	spring.vCoef = vect.Float(1.0 - math.Exp(float64(-spring.Damping*dt*k)))

	fSpring := spring.SpringForceFunc(spring, dist)
	apply_impulses(a, b, spring.r1, spring.r2, vect.Mult(spring.n, fSpring*dt))
}
Esempio n. 8
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
	}
}
Esempio n. 9
0
func NewSpace() (space *Space) {

	space = &Space{}
	space.Iterations = 20

	space.Gravity = vect.Vector_Zero

	space.damping = 1

	space.collisionSlop = 0.5
	space.collisionBias = vect.Float(math.Pow(1.0-0.1, 60))
	space.collisionPersistence = 3

	space.Constraints = make([]Constraint, 0)

	space.Bodies = make([]*Body, 0)
	space.deleteBodies = make([]*Body, 0)
	space.sleepingComponents = make([]*Body, 0)

	space.staticShapes = NewBBTree(nil)
	space.activeShapes = NewBBTree(space.staticShapes)
	space.cachedArbiters = make(map[HashPair]*Arbiter)
	space.Arbiters = make([]*Arbiter, 0)
	space.ArbiterBuffer = make([]*Arbiter, ArbiterBufferSize)

	for i := 0; i < len(space.ArbiterBuffer); i++ {
		space.ArbiterBuffer[i] = newArbiter()
	}

	space.ContactBuffer = make([][]*Contact, ContactBufferSize)

	for i := 0; i < len(space.ContactBuffer); i++ {
		var contacts []*Contact = make([]*Contact, MaxPoints)

		for i := 0; i < MaxPoints; i++ {
			contacts[i] = &Contact{}
		}
		space.ContactBuffer[i] = contacts
	}
	/*
		for i := 0; i < 8; i++ {
			go space.MultiThreadTest()
		}
	*/
	return
}
Esempio n. 10
0
func (tree *BBTree) GetBB(obj Indexable) AABB {
	v, ok := obj.Velocity()
	if ok {
		bb := obj.AABB()
		coef := vect.Float(0.1)

		l := bb.Lower.X
		b := bb.Lower.Y
		r := bb.Upper.X
		t := bb.Upper.Y

		x := (r - l) * coef
		y := (t - b) * coef

		v = vect.Mult(v, 0.1)

		return NewAABB(l+vect.FMin(-x, v.X), b+vect.FMin(-y, v.Y), r+vect.FMax(x, v.X), t+vect.FMax(y, v.Y))
	}

	return obj.AABB()
}
Esempio n. 11
0
func (body *Body) SetTorque(t float32) {
	body.t = vect.Float(t)
}
Esempio n. 12
0
func (body *Body) SetWBias(w float32) {
	body.w_bias = vect.Float(w)
}
Esempio n. 13
0
func (body *Body) AddTorque(t float32) {
	body.t += vect.Float(t)
}
Esempio n. 14
0
func (body *Body) SetVelocity(x, y float32) {
	body.v.X = vect.Float(x)
	body.v.Y = vect.Float(y)
}
Esempio n. 15
0
func (body *Body) AddVelocity(x, y float32) {
	body.v.X += vect.Float(x)
	body.v.Y += vect.Float(y)
}
Esempio n. 16
0
func (body *Body) SetForce(x, y float32) {
	body.f.X = vect.Float(x)
	body.f.Y = vect.Float(y)
}
Esempio n. 17
0
func (space *Space) ProcessComponents(dt vect.Float) {

	sleep := math.IsInf(float64(space.sleepTimeThreshold), 0)
	bodies := space.Bodies
	_ = bodies
	if sleep {
		dv := space.idleSpeedThreshold
		dvsq := vect.Float(0)
		if dv == 0 {
			dvsq = dv * dv
		} else {
			dvsq = space.Gravity.LengthSqr() * dt * dt
		}

		for _, body := range space.Bodies {
			keThreshold := vect.Float(0)
			if dvsq != 0 {
				keThreshold = body.m * dvsq
			}
			body.node.IdleTime = 0
			if body.KineticEnergy() <= keThreshold {
				body.node.IdleTime += dt
			}
		}
	}

	for _, arb := range space.Arbiters {
		a, b := arb.BodyA, arb.BodyB
		_, _ = a, b
		if sleep {

		}
	}
	/*
		// Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
		cpArray *arbiters = space->arbiters;
		for(int i=0, count=arbiters->num; i<count; i++){
			cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
			cpBody *a = arb->body_a, *b = arb->body_b;

			if(sleep){
				if((cpBodyIsRogue(b) && !cpBodyIsStatic(b)) || cpBodyIsSleeping(a)) cpBodyActivate(a);
				if((cpBodyIsRogue(a) && !cpBodyIsStatic(a)) || cpBodyIsSleeping(b)) cpBodyActivate(b);
			}

			cpBodyPushArbiter(a, arb);
			cpBodyPushArbiter(b, arb);
		}

		if(sleep){
			// Bodies should be held active if connected by a joint to a non-static rouge body.
			cpArray *constraints = space->constraints;
			for(int i=0; i<constraints->num; i++){
				cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
				cpBody *a = constraint->a, *b = constraint->b;

				if(cpBodyIsRogue(b) && !cpBodyIsStatic(b)) cpBodyActivate(a);
				if(cpBodyIsRogue(a) && !cpBodyIsStatic(a)) cpBodyActivate(b);
			}

			// Generate components and deactivate sleeping ones
			for(int i=0; i<bodies->num;){
				cpBody *body = (cpBody*)bodies->arr[i];

				if(ComponentRoot(body) == NULL){
					// Body not in a component yet. Perform a DFS to flood fill mark
					// the component in the contact graph using this body as the root.
					FloodFillComponent(body, body);

					// Check if the component should be put to sleep.
					if(!ComponentActive(body, space->sleepTimeThreshold)){
						cpArrayPush(space->sleepingComponents, body);
						CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other);

						// cpSpaceDeactivateBody() removed the current body from the list.
						// Skip incrementing the index counter.
						continue;
					}
				}

				i++;

				// Only sleeping bodies retain their component node pointers.
				body->node.root = NULL;
				body->node.next = NULL;
			}
		}
	*/
}
Esempio n. 18
0
func (circle *CircleShape) Moment(mass float32) vect.Float {
	return (vect.Float(mass) * (0.5 * (circle.Radius * circle.Radius))) + vect.LengthSqr(circle.Position)
}
Esempio n. 19
0
func (body *Body) AddAngularVelocity(w float32) {
	body.w += vect.Float(w)
}
Esempio n. 20
0
func bias_coef(errorBias, dt vect.Float) vect.Float {
	return vect.Float(1.0 - math.Pow(float64(errorBias), float64(dt)))
}
Esempio n. 21
0
func (body *Body) SetAngularVelocity(w float32) {
	body.w = vect.Float(w)
}
Esempio n. 22
0
type ComponentNode struct {
	Root     *Body
	Next     *Body
	IdleTime vect.Float
}

type BodyType uint8
type UpdatePositionFunction func(body *Body, dt vect.Float)
type UpdateVelocityFunction func(body *Body, gravity vect.Vect, damping, dt vect.Float)

const (
	BodyType_Static  = BodyType(0)
	BodyType_Dynamic = BodyType(1)
)

var Inf = vect.Float(math.Inf(1))

type CollisionCallback interface {
	CollisionEnter(arbiter *Arbiter) bool
	CollisionPreSolve(arbiter *Arbiter) bool
	CollisionPostSolve(arbiter *Arbiter)
	CollisionExit(arbiter *Arbiter)
}

type Body struct {
	/// Mass of the body.
	/// Must agree with cpBody.m_inv! Use cpBodySetMass() when changing the mass for this reason.
	m vect.Float

	/// Mass inverse.
	m_inv vect.Float
Esempio n. 23
0
func (space *Space) Step(dt vect.Float) {

	// don't step if the timestep is 0!
	if dt == 0 {
		return
	}

	stepStart := time.Now()

	bodies := space.Bodies

	for _, arb := range space.Arbiters {
		arb.state = arbiterStateNormal
	}

	space.Arbiters = space.Arbiters[0:0]

	prev_dt := space.curr_dt
	space.curr_dt = dt

	space.stamp++

	for _, body := range bodies {
		if body.Enabled {
			body.UpdatePosition(dt)
		}
	}

	for _, body := range bodies {
		if body.Enabled {
			body.UpdateShapes()
		}
	}

	start := time.Now()
	space.activeShapes.ReindexQuery(func(a, b Indexable) {
		SpaceCollideShapes(a.Shape(), b.Shape(), space)
	})
	space.ReindexQueryTime = time.Since(start)

	//axc := space.activeShapes.SpatialIndexClass.(*BBTree)
	//PrintTree(axc.root)

	for h, arb := range space.cachedArbiters {
		ticks := space.stamp - arb.stamp
		deleted := (arb.BodyA.deleted || arb.BodyB.deleted)
		disabled := !(arb.BodyA.Enabled || arb.BodyB.Enabled)
		if (ticks >= 1 && arb.state != arbiterStateCached) || deleted || disabled {
			arb.state = arbiterStateCached
			if arb.BodyA.CallbackHandler != nil {
				arb.BodyA.CallbackHandler.CollisionExit(arb)
			}
			if arb.BodyB.CallbackHandler != nil {
				arb.BodyB.CallbackHandler.CollisionExit(arb)
			}
		}
		if ticks > time.Duration(space.collisionPersistence) || deleted {
			delete(space.cachedArbiters, h)
			space.ArbiterBuffer = append(space.ArbiterBuffer, arb)
			c := arb.Contacts
			if c != nil {
				space.ContactBuffer = append(space.ContactBuffer, c)
			}
		}
	}

	slop := space.collisionSlop
	biasCoef := vect.Float(1.0 - math.Pow(float64(space.collisionBias), float64(dt)))
	invdt := vect.Float(1 / dt)
	for _, arb := range space.Arbiters {
		arb.preStep(invdt, slop, biasCoef)
	}

	for _, con := range space.Constraints {
		con.PreSolve()
		con.PreStep(dt)
	}

	damping := vect.Float(math.Pow(float64(space.damping), float64(dt)))

	for _, body := range bodies {
		if body.Enabled {
			if body.IgnoreGravity {
				body.UpdateVelocity(vect.Vector_Zero, damping, dt)
				continue
			}
			body.UpdateVelocity(space.Gravity, damping, dt)
		}
	}

	dt_coef := vect.Float(0)
	if prev_dt != 0 {
		dt_coef = dt / prev_dt
	}

	for _, arb := range space.Arbiters {
		arb.applyCachedImpulse(dt_coef)
	}

	for _, con := range space.Constraints {
		con.ApplyCachedImpulse(dt_coef)
	}

	//fmt.Println("STEP")
	start = time.Now()

	//fmt.Println("Arbiters", len(space.Arbiters), biasCoef, dt)
	//spew.Config.MaxDepth = 3
	//spew.Config.Indent = "\t"
	for i := 0; i < space.Iterations; i++ {
		for _, arb := range space.Arbiters {
			arb.applyImpulse()
			//spew.Dump(arb)
			//spew.Printf("%+v\n", arb)
		}

		for _, con := range space.Constraints {
			con.ApplyImpulse()
		}
	}

	//fmt.Println("####")
	//fmt.Println("")

	//MultiThreadGo()
	//for i:=0; i<8; i++ {
	//	<-done
	//}
	space.ApplyImpulsesTime = time.Since(start)

	for _, con := range space.Constraints {
		con.PostSolve()
	}

	for _, arb := range space.Arbiters {
		if arb.ShapeA.Body.CallbackHandler != nil {
			arb.ShapeA.Body.CallbackHandler.CollisionPostSolve(arb)
		}
		if arb.ShapeB.Body.CallbackHandler != nil {
			arb.ShapeB.Body.CallbackHandler.CollisionPostSolve(arb)
		}
	}

	if len(space.deleteBodies) > 0 {
		for _, body := range space.deleteBodies {
			space.removeBody(body)
		}
		space.deleteBodies = space.deleteBodies[0:0]
	}

	stepEnd := time.Now()
	space.StepTime = stepEnd.Sub(stepStart)
}
Esempio n. 24
0
func (body *Body) AddAngle(angle float32) {
	body.SetAngle(vect.Float(angle) + body.Angle())
}
Esempio n. 25
0
func (body *Body) AddForce(x, y float32) {
	body.f.X += vect.Float(x)
	body.f.Y += vect.Float(y)
}
Esempio n. 26
0
func (box *BoxShape) Moment(mass float32) vect.Float {
	return (vect.Float(mass) * (box.Width*box.Width + box.Height*box.Height) / 12.0)
}