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 (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 (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) } 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) 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 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 (line *Line) Setup(seg *SegmentShape) { line.a = seg.Tn.X line.b = seg.Tn.Y line.c = -vect.Dot(seg.Tn, seg.Ta) }
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 }
// 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 mult_k(vr, k1, k2 vect.Vect) vect.Vect { return vect.Vect{vect.Dot(vr, k1), vect.Dot(vr, k2)} }