func (poly *PolygonShape) valueOnAxis(n vect.Vect, d vect.Float) vect.Float {
verts := poly.TVerts
min := vect.Dot(n, verts[0])
for i := 1; i < poly.NumVerts; i++ {
min = vect.FMin(min, vect.Dot(n, verts[i]))
}
//fmt.Println(min, d)
return min - d
}
func (poly *PolygonShape) ContainsVertPartial(v, n vect.Vect) bool {
for _, axis := range poly.TAxes {
if vect.Dot(axis.N, n) < 0.0 {
continue
}
dist := vect.Dot(axis.N, v) - axis.D
if dist > 0.0 {
return false
}
}
return true
}
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 (arb *Arbiter) applyImpulse3() {
a := arb.ShapeA.Body
b := arb.ShapeB.Body
for i := 0; i < arb.NumContacts; i++ {
con := arb.Contacts[i]
n := con.n
r1 := con.r1
r2 := con.r2
// Calculate the relative bias velocities.
vb1 := vect.Add(a.v_bias, vect.Mult(vect.Perp(r1), a.w_bias))
vb2 := vect.Add(b.v_bias, vect.Mult(vect.Perp(r2), b.w_bias))
vbn := vect.Dot(vect.Sub(vb2, vb1), n)
// Calculate the relative velocity.
vr := relative_velocity(a, b, r1, r2)
vrn := vect.Dot(vr, n)
// Calculate the relative tangent velocity.
vrt := vect.Dot(vect.Add(vr, arb.Surface_vr), vect.Perp(n))
// Calculate and clamp the bias impulse.
jbn := (con.bias - vbn) * con.nMass
jbnOld := con.jBias
con.jBias = vect.FMax(jbnOld+jbn, 0.0)
// Calculate and clamp the normal impulse.
jn := -(con.bounce + vrn) * con.nMass
jnOld := con.jnAcc
con.jnAcc = vect.FMax(jnOld+jn, 0.0)
// Calculate and clamp the friction impulse.
jtMax := arb.u * con.jnAcc
jt := -vrt * con.tMass
jtOld := con.jtAcc
con.jtAcc = vect.FClamp(jtOld+jt, -jtMax, jtMax)
// Apply the bias impulse.
apply_bias_impulses(a, b, r1, r2, vect.Mult(n, con.jBias-jbnOld))
// Apply the final impulse.
apply_impulses(a, b, r1, r2, transform.RotateVect(n, transform.Rotation{con.jnAcc - jnOld, con.jtAcc - jtOld}))
}
}
func segmentEncapQuery(p1, p2 vect.Vect, r1, r2 vect.Float, con *Contact, tangent vect.Vect) int {
count := circle2circleQuery(p1, p2, r1, r2, con)
if vect.Dot(con.n, tangent) >= 0.0 {
return count
} else {
return 0
}
panic("Never reached")
}
func (arb *Arbiter) preStep(inv_dt, slop, bias vect.Float) {
a := arb.ShapeA.Body
b := arb.ShapeB.Body
for _, con := range arb.Contacts {
// Calculate the offsets.
x, y := con.p.X, con.p.Y
r1 := vect.Vect{x - a.p.X, y - a.p.Y}
r2 := vect.Vect{x - b.p.X, y - b.p.Y}
//con.Normal = vect.Vect{-1,0}
// Calculate the mass normal and mass tangent.
n := con.n
rcn := (r1.X * n.Y) - (r1.Y * n.X)
rcn = a.m_inv + (a.i_inv * rcn * rcn)
rcn2 := (r2.X * n.Y) - (r2.Y * n.X)
rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)
value := rcn + rcn2
if value == 0.0 {
fmt.Printf("Warning: Unsolvable collision or constraint.")
}
con.nMass = 1.0 / value
n = vect.Perp(con.n)
rcn = (r1.X * n.Y) - (r1.Y * n.X)
rcn = a.m_inv + (a.i_inv * rcn * rcn)
rcn2 = (r2.X * n.Y) - (r2.Y * n.X)
rcn2 = b.m_inv + (b.i_inv * rcn2 * rcn2)
value = rcn + rcn2
if value == 0.0 {
fmt.Printf("Warning: Unsolvable collision or constraint.")
}
con.tMass = 1.0 / value
// Calculate the target bias velocity.
//ds := con.dist + slop
//if 0 > ds {
//con.bias = -bias * inv_dt * (con.dist + slop)
min := math.Min(0.0, float64(con.dist)+0.01)
con.bias = -bias * inv_dt * vect.Float(min)
//c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + 0.01);
//} else {
// con.bias = 0
//}
con.jBias = 0.0
con.bounce = vect.Dot(vect.Vect{(-r2.Y*b.w + b.v.X) - (-r1.Y*a.w + a.v.X), (r2.X*b.w + b.v.Y) - (r1.X*a.w + a.v.Y)}, con.n) * arb.e
con.r1 = r1
con.r2 = r2
}
}
func (poly *PolygonShape) ContainsVert(v vect.Vect) bool {
for _, axis := range poly.TAxes {
dist := vect.Dot(axis.N, v) - axis.D
if dist > 0.0 {
return false
}
}
return true
}
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 (body *Body) KineticEnergy() vect.Float {
vsq := vect.Dot(body.v, body.v)
wsq := body.w * body.w
if vsq != 0 {
vsq = vsq * body.m
}
if wsq != 0 {
wsq = wsq * body.i
}
return vsq + wsq
}
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 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")
}
// Calculates the transformed vertices and axes and the bounding box.
func (poly *PolygonShape) update(xf transform.Transform) AABB {
//transform axes
{
src := poly.Axes
dst := poly.TAxes
for i := 0; i < poly.NumVerts; i++ {
n := xf.RotateVect(src[i].N)
dst[i].N = n
dst[i].D = vect.Dot(xf.Position, n) + src[i].D
}
/*
fmt.Println("")
fmt.Println("Started Axes")
fmt.Println(xf.Rotation, xf.Position)
for i:=0;i<poly.NumVerts;i++ {
fmt.Println(src[i], dst[i])
}
*/
}
//transform verts
{
inf := vect.Float(math.Inf(1))
aabb := AABB{
Lower: vect.Vect{inf, inf},
Upper: vect.Vect{-inf, -inf},
}
src := poly.Verts
dst := poly.TVerts
for i := 0; i < poly.NumVerts; i++ {
v := xf.TransformVect(src[i])
dst[i] = v
aabb.Lower.X = vect.FMin(aabb.Lower.X, v.X)
aabb.Upper.X = vect.FMax(aabb.Upper.X, v.X)
aabb.Lower.Y = vect.FMin(aabb.Lower.Y, v.Y)
aabb.Upper.Y = vect.FMax(aabb.Upper.Y, v.Y)
}
/*
fmt.Println("Verts")
for i:=0;i<poly.NumVerts;i++ {
fmt.Println(src[i], dst[i])
}
*/
return aabb
}
}
// 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 segValueOnAxis(seg *SegmentShape, n vect.Vect, d vect.Float) vect.Float {
a := vect.Dot(n, seg.Ta) - seg.Radius
b := vect.Dot(n, seg.Tb) - seg.Radius
return vect.FMin(a, b) - d
}
func mult_k(vr, k1, k2 vect.Vect) vect.Vect {
return vect.Vect{vect.Dot(vr, k1), vect.Dot(vr, k2)}
}
// 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 seg2polyFunc(contacts []*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, hashPair(seg.Shape.Hash(), 0))
}
if poly.ContainsVert(vb) {
nextContact(contacts, &num).reset(vb, poly_n, poly_min, hashPair(seg.Shape.Hash(), 1))
}
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
}
func normal_relative_velocity(a, b *Body, r1, r2, n vect.Vect) vect.Float {
return vect.Dot(relative_velocity(a, b, r1, r2), n)
}