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 preStepGameObject(g *GameObject) {
if g.Physics != nil && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
pos := g.Transform().WorldPosition()
angle := g.Transform().Angle() * RadianConst
if g.Physics.Interpolate {
//Interpolation check: if position/angle has been changed directly and not by the physics engine, change g.Physics.LastPosition/LastAngle
if vect.Float(pos.X) != g.Physics.InterpolatedPosition.X || vect.Float(pos.Y) != g.Physics.InterpolatedPosition.Y {
g.Physics.InterpolatedPosition = vect.Vect{vect.Float(pos.X), vect.Float(pos.Y)}
g.Physics.Body.SetPosition(g.Physics.InterpolatedPosition)
}
if vect.Float(angle) != g.Physics.InterpolatedAngle {
g.Physics.InterpolatedAngle = vect.Float(angle)
g.Physics.Body.SetAngle(g.Physics.InterpolatedAngle)
}
} else {
var pPos vect.Vect
pPos.X, pPos.Y = vect.Float(pos.X), vect.Float(pos.Y)
g.Physics.Body.SetAngle(vect.Float(angle))
g.Physics.Body.SetPosition(pPos)
}
g.Physics.LastPosition = g.Physics.Body.Position()
g.Physics.LastAngle = g.Physics.Body.Angle()
}
}
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)
}
func NewPhysics(static bool, space *chipmunk.Space) *Physics {
var body *chipmunk.Body
box := chipmunk.NewBox(vect.Vect{0, 0}, vect.Float(1), vect.Float(1))
if static {
body = chipmunk.NewBodyStatic()
} else {
body = chipmunk.NewBody(1, box.Moment(1))
}
p := &Physics{BaseComponent: NewComponent(), Space: space, Body: body, Box: box.GetAsBox(), Shape: box}
body.AddShape(box)
return p
}
func (p *Physics) Start() {
//p.Interpolate = true
pos := p.GameObject().Transform().WorldPosition()
p.Body.SetAngle(vect.Float(p.GameObject().Transform().WorldRotation().Z) * RadianConst)
p.Body.SetPosition(vect.Vect{vect.Float(pos.X), vect.Float(pos.Y)})
p.LastPosition = p.Body.Position()
p.LastAngle = p.Body.Angle()
//if p.GameObject().Sprite != nil {
// p.GameObject().Sprite.UpdateShape()
// p.Body.UpdateShapes()
//}
p.Body.UpdateShapes()
//log.Printf("3x=%f, y=%f", p.Body.Shapes[0].BB.Extents().X, p.Body.Shapes[0].BB.Extents().Y)
p.Space.AddBody(p.Body)
}
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)
min := math.Min(0.0, float64(con.dist)+0.01)
con.bias = -bias * inv_dt * vect.Float(min)
//c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + 0.01);
//} 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
}
}
// 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}
}
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)
}
// 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
}
}
}
// 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 postStepGameObject(g *GameObject) {
if g.Physics != nil && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
/*
When parent changes his position/rotation it changes his children position/rotation too but the physics engine thinks its in different position
so we need to check how much it changed and apply to the new position/rotation so we wont f**k up things too much.
Note:If position/angle is changed in between preStep and postStep it will be overrided.
*/
if CorrectWrongPhysics {
b := g.Physics.Body
angle := float32(b.Angle())
lAngle := float32(g.Physics.LastAngle)
lAngle += angle - lAngle
pos := b.Position()
lPos := g.Physics.LastPosition
lPos.X += (pos.X - lPos.X)
lPos.Y += (pos.Y - lPos.Y)
if g.Physics.Interpolate {
g.Physics.InterpolatedAngle = vect.Float(lAngle)
g.Physics.InterpolatedPosition = lPos
}
b.SetPosition(lPos)
//b.SetAngle(g.Physics.InterpolatedAngle)
//g.Transform().SetWorldRotationf(lAngle * DegreeConst)
g.Transform().SetWorldPositionf(float32(pos.X), float32(pos.Y))
} else {
//b := g.Physics.Body
//angle := b.Angle()
//pos := b.Position()
//if g.Physics.Interpolate {
// g.Physics.InterpolatedAngle = angle
// g.Physics.InterpolatedPosition = pos
//}
////g.Transform().SetWorldRotationf(float32(angle) * DegreeConst)
//g.Transform().SetWorldPositionf(float32(pos.X), float32(pos.Y))
}
}
}
func NewPhysicsShapes(static bool, shapes []*chipmunk.Shape) *Physics {
var body *chipmunk.Body
if static {
body = chipmunk.NewBodyStatic()
} else {
moment := vect.Float(0)
for _, shape := range shapes {
moment += shape.Moment(1)
}
body = chipmunk.NewBody(1, moment)
}
p := &Physics{BaseComponent: NewComponent(), Body: body, Box: nil, Shape: nil}
for _, shape := range shapes {
body.AddShape(shape)
}
return p
}
func interpolateGameObject(game *Game_t, g *GameObject) {
if g.Physics != nil && g.Physics.Interpolate && g.IsActive() && !g.Physics.Body.IsStatic() && g.Physics.Started() {
nextPos := g.Physics.Body.Position()
currPos := g.Physics.LastPosition
//nextAngle := g.Physics.Body.Angle()
//currAngle := g.Physics.LastAngle
alpha := vect.Float(game.fixedTime / stepTime)
x := currPos.X + ((nextPos.X - currPos.X) * alpha)
y := currPos.Y + ((nextPos.Y - currPos.Y) * alpha)
//a := currAngle + ((nextAngle - currAngle) * alpha)
g.Transform().SetWorldPositionf(float32(x), float32(y))
//g.Transform().SetWorldRotationf(float32(a) * DegreeConst)
//g.Physics.InterpolatedAngle = a
g.Physics.InterpolatedPosition.X, g.Physics.InterpolatedPosition.Y = x, y
}
}
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) SetAngularVelocity(w float32) {
body.w = vect.Float(w)
}
func (body *Body) AddAngularVelocity(w float32) {
body.w += vect.Float(w)
}
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) AddAngle(angle float32) {
body.SetAngle(vect.Float(angle) + body.Angle())
}
func (body *Body) SetVelocity(x, y float32) {
body.v.X = vect.Float(x)
body.v.Y = vect.Float(y)
}
func (body *Body) SetVelocityY(y float32) {
//body.v.X = vect.Float(x)
body.v.Y = vect.Float(y)
}
func (body *Body) SetVelocityX(x 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 (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))
}
func (body *Body) AddTorque(t float32) {
body.t += vect.Float(t)
}
func (body *Body) AddForce(x, y float32) {
body.f.X += vect.Float(x)
body.f.Y += vect.Float(y)
}
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