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
}
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)
}
// 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
}
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}
}
// 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
}
}
}
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)
}
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))
}
// 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
}
}
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
}
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()
}
func (body *Body) SetTorque(t float32) {
body.t = vect.Float(t)
}
func (body *Body) SetWBias(w float32) {
body.w_bias = vect.Float(w)
}
func (body *Body) AddTorque(t float32) {
body.t += vect.Float(t)
}
func (body *Body) SetVelocity(x, y float32) {
body.v.X = vect.Float(x)
body.v.Y = vect.Float(y)
}
func (body *Body) AddVelocity(x, y float32) {
body.v.X += vect.Float(x)
body.v.Y += vect.Float(y)
}
func (body *Body) SetForce(x, y float32) {
body.f.X = vect.Float(x)
body.f.Y = vect.Float(y)
}
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;
}
}
*/
}
func (circle *CircleShape) Moment(mass float32) vect.Float {
return (vect.Float(mass) * (0.5 * (circle.Radius * circle.Radius))) + vect.LengthSqr(circle.Position)
}
func (body *Body) AddAngularVelocity(w float32) {
body.w += vect.Float(w)
}
func bias_coef(errorBias, dt vect.Float) vect.Float {
return vect.Float(1.0 - math.Pow(float64(errorBias), float64(dt)))
}
func (body *Body) SetAngularVelocity(w float32) {
body.w = vect.Float(w)
}
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
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)
}
func (body *Body) AddAngle(angle float32) {
body.SetAngle(vect.Float(angle) + body.Angle())
}
func (body *Body) AddForce(x, y float32) {
body.f.X += vect.Float(x)
body.f.Y += vect.Float(y)
}
func (box *BoxShape) Moment(mass float32) vect.Float {
return (vect.Float(mass) * (box.Width*box.Width + box.Height*box.Height) / 12.0)
}
