// Checks if verts forms a valid polygon. // The vertices must be convex and winded clockwise. func (verts Vertices) ValidatePolygon() bool { numVerts := len(verts) for i := 0; i < numVerts; i++ { a := verts[i] b := verts[(i+1)%numVerts] c := verts[(i+2)%numVerts] if vect.Cross(vect.Sub(b, a), vect.Sub(c, b)) > 0.0 { return false } } return true }
func TestOverlap(a, b AABB) bool { d1 := vect.Sub(b.Lower, a.Upper) d2 := vect.Sub(a.Lower, b.Upper) if d1.X > 0.0 || d1.Y > 0.0 { return false } if d2.X > 0.0 || d2.Y > 0.0 { return false } return true }
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 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 }
// Recalculates the global center of the circle and the the bounding box. func (circle *CircleShape) update(xf transform.Transform) aabb.AABB { //global center of the circle center := xf.TransformVect(circle.Position) circle.Tc = center rv := vect.Vect{circle.Radius, circle.Radius} return aabb.AABB{ vect.Sub(center, rv), vect.Add(center, rv), } }
// Calls ShapeClass.update and sets the new AABB. func (shape *Shape) Update() { if shape.Body == nil { log.Printf("Error: uninitialized shape") return } body := shape.Body shape.AABB = shape.ShapeClass.update(body.Transform) proxy := &shape.proxy proxy.AABB = shape.AABB if body.Space != nil { d := vect.Sub(body.Transform.Position, body.prevTransform.Position) body.Space.BroadPhase.moveProxy(proxy.ProxyId, proxy.AABB, d) } }
//Called to update N, Tn, Ta, Tb and the the bounding box. func (segment *SegmentShape) update(xf transform.Transform) aabb.AABB { a := xf.TransformVect(segment.A) b := xf.TransformVect(segment.B) segment.Ta = a segment.Tb = b segment.N = vect.Perp(vect.Normalize(vect.Sub(segment.B, segment.A))) segment.Tn = xf.RotateVect(segment.N) rv := vect.Vect{segment.Radius, segment.Radius} min := vect.Min(a, b) min.Sub(rv) max := vect.Max(a, b) max.Add(rv) return aabb.AABB{ min, max, } }
// Sets the vertices offset by the offset and calculates the PolygonAxes. func (poly *PolygonShape) SetVerts(verts Vertices, offset vect.Vect) { if verts == nil { log.Printf("Error: no vertices passed!") return } if verts.ValidatePolygon() == false { log.Printf("Warning: vertices not valid") } numVerts := len(verts) oldnumVerts := len(poly.Verts) poly.NumVerts = numVerts if oldnumVerts < numVerts { //create new slices poly.Verts = make(Vertices, numVerts) poly.TVerts = make(Vertices, numVerts) poly.Axes = make([]PolygonAxis, numVerts) poly.TAxes = make([]PolygonAxis, numVerts) } else { //reuse old slices poly.Verts = poly.Verts[:numVerts] poly.TVerts = poly.TVerts[:numVerts] poly.Axes = poly.Axes[:numVerts] poly.TAxes = poly.TAxes[:numVerts] } for i := 0; i < numVerts; i++ { a := vect.Add(offset, verts[i]) b := vect.Add(offset, verts[(i+1)%numVerts]) n := vect.Normalize(vect.Perp(vect.Sub(b, a))) poly.Verts[i] = a poly.Axes[i].N = n poly.Axes[i].D = vect.Dot(n, a) } }
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 DrawShape(shape *collision.Shape) { switch shape.ShapeType() { case collision.ShapeType_Circle: circle := shape.ShapeClass.(*collision.CircleShape) DrawCircle(circle.Tc, circle.Radius, false) const circleMarkerSize = 0.08 { p1 := vect.Add(circle.Tc, vect.Vect{0, circleMarkerSize}) p2 := vect.Sub(circle.Tc, vect.Vect{0, circleMarkerSize}) DrawLine(p1, p2) } { p1 := vect.Add(circle.Tc, vect.Vect{circleMarkerSize, 0}) p2 := vect.Sub(circle.Tc, vect.Vect{circleMarkerSize, 0}) DrawLine(p1, p2) } break case collision.ShapeType_Segment: segment := shape.ShapeClass.(*collision.SegmentShape) a := segment.Ta b := segment.Tb r := segment.Radius DrawLine(a, b) if segment.Radius > 0.0 { DrawCircle(a, r, false) DrawCircle(b, r, false) verts := [4]vect.Vect{ vect.Add(a, vect.Vect{0, r}), vect.Add(a, vect.Vect{0, -r}), vect.Add(b, vect.Vect{0, -r}), vect.Add(b, vect.Vect{0, r}), } DrawPoly(verts[:], 4, false) } if Settings.DrawNormals { n := segment.Tn DrawLine(a, vect.Add(a, n)) DrawLine(b, vect.Add(b, n)) } case collision.ShapeType_Polygon: poly := shape.ShapeClass.(*collision.PolygonShape) verts := poly.TVerts DrawPoly(verts, poly.NumVerts, false) if Settings.DrawNormals { axes := poly.TAxes for i, v := range verts { a := axes[i] v1 := v v2 := verts[(i+1)%len(verts)] DrawLine(v1, vect.Add(v1, a.N)) DrawLine(v2, vect.Add(v2, a.N)) } } case collision.ShapeType_Box: poly := shape.ShapeClass.(*collision.BoxShape).Polygon verts := poly.TVerts DrawPoly(verts, poly.NumVerts, false) } }
// Returns true if the given point is located inside the circle. func (circle *CircleShape) TestPoint(point vect.Vect) bool { d := vect.Sub(point, circle.Tc) return vect.Dot(d, d) <= circle.Radius*circle.Radius }
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) Perimeter() float64 { w := vect.Sub(aabb.Upper, aabb.Lower) return 2 * (w.X + w.Y) }
func (aabb *AABB) Extents() vect.Vect { return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5) }