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