func apply_impulses(a, b *Body, r1, r2, j vect.Vect) {
j1 := vect.Vect{-j.X, -j.Y}
a.v.Add(vect.Mult(j1, a.m_inv))
a.w += a.i_inv * vect.Cross(r1, j1)
b.v.Add(vect.Mult(j, b.m_inv))
b.w += b.i_inv * vect.Cross(r2, j)
}
func findPoinsBehindSeg(contacts []*Contact, num *int, seg *SegmentShape, poly *PolygonShape, pDist, coef vect.Float) {
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, hashPair(poly.Shape.Hash(), HashValue(i)))
}
}
}
}
func circle2segmentFunc(contacts []*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 := vect.FAbs(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, 0)
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 circle2polyFunc(contacts []*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,
0,
)
return 1
} else {
return circle2circleQuery(circle.Tc, a, circle.Radius, 0.0, contacts[0])
}
panic("Never reached")
}
// 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 (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)
}
func k_scalar_body(body *Body, r, n vect.Vect) vect.Float {
rcn := vect.Cross(r, n)
return body.m_inv + (body.i_inv * rcn * rcn)
}