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) }