func circle2circleQuery(p1, p2 vect.Vect, r1, r2 float64, con *Contact) int {
minDist := r1 + r2
delta := vect.Sub(p2, p1)
distSqr := delta.LengthSqr()
if distSqr >= minDist*minDist {
return 0
}
dist := math.Sqrt(distSqr)
pDist := dist
if dist == 0.0 {
pDist = 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)
return 1
}
func (arb *Arbiter) preStep(inv_dt float64) {
const allowedPenetration = 0.01
biasFactor := 0.0
if Settings.PositionCorrection {
biasFactor = 0.2
}
b1 := arb.ShapeA.Body
b2 := arb.ShapeB.Body
for i := 0; i < arb.NumContacts; i++ {
c := &arb.Contacts[i]
c.R1 = vect.Sub(c.Position, b1.Transform.Position)
c.R2 = vect.Sub(c.Position, b2.Transform.Position)
r1 := c.R1
r2 := c.R2
//Precompute normal mass, tangent mass, and bias
rn1 := vect.Dot(r1, c.Normal)
rn2 := vect.Dot(r2, c.Normal)
kNormal := b1.invMass + b2.invMass
kNormal += b1.invI*(vect.Dot(r1, r1)-rn1*rn1) +
b2.invI*(vect.Dot(r2, r2)-rn2*rn2)
c.MassNormal = 1.0 / kNormal
tangent := vect.CrossVF(c.Normal, 1.0)
rt1 := vect.Dot(r1, tangent)
rt2 := vect.Dot(r2, tangent)
kTangent := b1.invMass + b2.invMass
kTangent += b1.invI*(vect.Dot(r1, r1)-rt1*rt1) +
b2.invI*(vect.Dot(r2, r2)-rt2*rt2)
c.MassTangent = 1.0 / kTangent
c.Bias = -biasFactor * inv_dt * math.Min(0.0, c.Separation+allowedPenetration)
if Settings.AccumulateImpulses {
//Apply normal + friction impulse
P := vect.Add(vect.Mult(c.Normal, c.Pn), vect.Mult(tangent, c.Pt))
b1.Velocity.Sub(vect.Mult(P, b1.invMass))
b1.AngularVelocity -= b1.invI * vect.Cross(r1, P)
b2.Velocity.Add(vect.Mult(P, b2.invMass))
b2.AngularVelocity += b2.invI * vect.Cross(r2, P)
}
}
}
func DrawDebugData(space *collision.Space) {
//Draw shapes
for _, b := range space.Bodies {
if b.Enabled == false {
//Inactive
gl.Color3f(.5, .8, .5)
} else if b.IsStatic() {
//Static
gl.Color3f(1, 1, 1)
} else {
//Normal
gl.Color3f(1, 0, 0)
}
for _, s := range b.Shapes {
DrawShape(s)
}
}
gl.Color3f(0, 1, 0.5)
for _, b := range space.Bodies {
DrawTransform(&b.Transform, 0.2)
}
if Settings.DrawAABBs {
for _, b := range space.Bodies {
gl.Color3f(.3, .7, .7)
for _, s := range b.Shapes {
DrawQuad(s.AABB.Lower, s.AABB.Upper, false)
}
}
}
const contactRadius = 0.2
const contactNormalScale = 0.5
for arb := space.ContactManager.ArbiterList.Arbiter; arb != nil; arb = arb.Next {
for i := 0; i < arb.NumContacts; i++ {
con := arb.Contacts[i]
gl.Color3f(0, 0, 1)
p1 := con.Position
p2 := vect.Add(p1, vect.Mult(con.Normal, contactNormalScale))
//p2 := vect.Add(p1, vect.Mult(con.Normal, con.Separation))
DrawLine(p1, p2)
gl.Color3f(0, 1, 0)
DrawCircle(con.Position, contactRadius, false)
}
}
if Settings.DrawTreeNodes {
for _, node := range space.GetDynamicTreeNodes() {
gl.Color3f(0.0, .7, .7)
DrawQuad(node.AABB().Lower, node.AABB().Upper, false)
}
}
}
func circle2polyFunc(contacts *[MaxPoints]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,
)
return 1
} else {
return circle2circleQuery(circle.Tc, a, circle.Radius, 0.0, &contacts[0])
}
panic("Never reached")
}
func findPoinsBehindSeg(contacts *[MaxPoints]Contact, num *int, seg *SegmentShape, poly *PolygonShape, pDist, coef float64) {
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)
}
}
}
}
func findVertsFallback(contacts *[MaxPoints]Contact, poly1, poly2 *PolygonShape, n vect.Vect, dist float64) int {
num := 0
for _, v := range poly1.TVerts {
if poly2.ContainsVertPartial(v, vect.Mult(n, -1)) {
nextContact(contacts, &num).reset(v, n, dist)
}
}
for _, v := range poly2.TVerts {
if poly1.ContainsVertPartial(v, n) {
nextContact(contacts, &num).reset(v, n, dist)
}
}
return num
}
func circle2segmentFunc(contacts *[MaxPoints]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 := math.Abs(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)
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")
}
func poly2polyFunc(contacts *[MaxPoints]Contact, poly1, poly2 *PolygonShape) int {
min1, mini1 := findMSA(poly2, poly1.TAxes, poly1.NumVerts)
if mini1 == -1 {
return 0
}
min2, mini2 := findMSA(poly1, poly2.TAxes, poly2.NumVerts)
if mini2 == -1 {
return 0
}
// There is overlap, find the penetrating verts
if min1 > min2 {
return findVerts(contacts, poly1, poly2, poly1.TAxes[mini1].N, min1)
} else {
return findVerts(contacts, poly1, poly2, vect.Mult(poly2.TAxes[mini2].N, -1), min2)
}
panic("Never reached")
}
func seg2polyFunc(contacts *[MaxPoints]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)
}
if poly.ContainsVert(vb) {
nextContact(contacts, &num).reset(vb, poly_n, poly_min)
}
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
}
// Advances the space by the given timestep.
func (space *Space) Step(dt float64) {
if dt <= 0.0 {
return
}
inv_dt := 1.0 / dt
cm := space.ContactManager
//broadphase
cm.findNewContacts()
cm.collide()
//Integrate forces
for _, body := range space.Bodies {
body.UpdateShapes()
if body.IsStatic() {
continue
}
//b.Velocity += dt * (gravity + b.invMass * b.Force)
newVel := vect.Mult(body.Force, body.invMass)
if !body.IgnoreGravity {
newVel.Add(space.Gravity)
}
newVel.Mult(dt)
body.Velocity.Add(newVel)
body.AngularVelocity += dt * body.invI * body.Torque
if Settings.AutoClearForces {
body.Force = vect.Vect{}
body.Torque = 0.0
}
}
//Perform pre-steps
for arb := cm.ArbiterList.Arbiter; arb != nil; arb = arb.Next {
if arb.ShapeA.IsSensor || arb.ShapeB.IsSensor {
continue
}
arb.preStep(inv_dt)
}
//Perform Iterations
for i := 0; i < Settings.Iterations; i++ {
for arb := cm.ArbiterList.Arbiter; arb != nil; arb = arb.Next {
if arb.ShapeA.IsSensor || arb.ShapeB.IsSensor {
continue
}
arb.applyImpulse()
}
}
//Integrate velocities
for _, body := range space.Bodies {
if body.IsStatic() {
continue
}
body.Transform.Position.Add(vect.Mult(body.Velocity, dt))
rot := body.Transform.Angle()
body.Transform.SetAngle(rot + dt*body.AngularVelocity)
body.UpdateShapes()
}
}
func (xf *Transform) TransformVectInv(v vect.Vect) vect.Vect {
return vect.Add(vect.Mult(xf.Position, -1), xf.RotateVectInv(v))
}
func (arb *Arbiter) applyImpulse() {
sA := arb.ShapeA
sB := arb.ShapeB
b1 := sA.Body
b2 := sB.Body
//xfA := b1.Transform
//xfB := b2.Transform
for i := 0; i < arb.NumContacts; i++ {
c := &arb.Contacts[i]
// Relative velocity at contact
dv := vect.Vect{}
{
t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2))
t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1))
dv = vect.Sub(t1, t2)
}
// Compute normal impulse
vn := vect.Dot(dv, c.Normal)
dPn := c.MassNormal * (-vn + c.Bias)
if Settings.AccumulateImpulses {
// Clamp the accumulated impulse
Pn0 := c.Pn
c.Pn = math.Max(Pn0+dPn, 0.0)
dPn = c.Pn - Pn0
} else {
dPn = math.Max(dPn, 0.0)
}
//Apply contact impulse
Pn := vect.Mult(c.Normal, dPn)
b1.Velocity.Sub(vect.Mult(Pn, b1.invMass))
b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pn)
b2.Velocity.Add(vect.Mult(Pn, b2.invMass))
b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pn)
//Relative velocity at contact
{
t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2))
t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1))
dv = vect.Sub(t1, t2)
}
tangent := vect.CrossVF(c.Normal, 1.0)
vt := vect.Dot(dv, tangent)
dPt := c.MassTangent * (-vt)
if Settings.AccumulateImpulses {
//Compute friction impulse
maxPt := arb.Friction * c.Pn
//Clamp Friction
oldTangentImpulse := c.Pt
c.Pt = clamp(oldTangentImpulse+dPt, -maxPt, maxPt)
dPt = c.Pt - oldTangentImpulse
} else {
maxPt := arb.Friction * dPn
dPt = clamp(dPt, -maxPt, maxPt)
}
// Apply contact impulse
Pt := vect.Mult(tangent, dPt)
b1.Velocity.Sub(vect.Mult(Pt, b1.invMass))
b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pt)
b2.Velocity.Add(vect.Mult(Pt, b2.invMass))
b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pt)
}
}
func (aabb *AABB) Extents() vect.Vect {
return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5)
}
//returns the center of the aabb
func (aabb *AABB) Center() vect.Vect {
return vect.Mult(vect.Add(aabb.Lower, aabb.Upper), 0.5)
}
