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