// 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
}
}
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 (tree *BBTree) GetBB(obj Indexable) AABB {
v, ok := obj.Velocity()
if ok {
bb := obj.AABB()
coef := vect.Float(0.1)
l := bb.Lower.X
b := bb.Lower.Y
r := bb.Upper.X
t := bb.Upper.Y
x := (r - l) * coef
y := (t - b) * coef
v = vect.Mult(v, 0.1)
return NewAABB(l+vect.FMin(-x, v.X), b+vect.FMin(-y, v.Y), r+vect.FMax(x, v.X), t+vect.FMax(y, v.Y))
}
return obj.AABB()
}
func MergedAreaPtr(a, b *AABB) vect.Float {
return (vect.FMax(a.Upper.X, b.Upper.X) - vect.FMin(a.Lower.X, b.Lower.X)) * (vect.FMax(a.Upper.Y, b.Upper.Y) - vect.FMin(a.Lower.Y, b.Lower.Y))
}