func circle2circleQuery(p1, p2 vect.Vect, r1, r2 vect.Float, con *Contact) int {
minDist := r1 + r2
delta := vect.Sub(p2, p1)
distSqr := delta.LengthSqr()
if distSqr >= minDist*minDist {
return 0
}
dist := vect.Float(math.Sqrt(float64(distSqr)))
pDist := dist
if dist == 0.0 {
pDist = vect.Float(math.Inf(1))
}
pos := vect.Add(p1, vect.Mult(delta, 0.5+(r1-0.5*minDist)/pDist))
norm := vect.Vect{1, 0}
if dist != 0.0 {
norm = vect.Mult(delta, 1.0/dist)
}
con.reset(pos, norm, dist-minDist, 0)
return 1
}
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 (body *Body) UpdateVelocity(gravity vect.Vect, damping, dt vect.Float) {
if body.UpdateVelocityFunc != nil {
body.UpdateVelocityFunc(body, gravity, damping, dt)
return
}
body.v = vect.Add(vect.Mult(body.v, damping), vect.Mult(vect.Add(gravity, vect.Mult(body.f, body.m_inv)), dt))
body.w = (body.w * damping) + (body.t * body.i_inv * dt)
body.f = vect.Vector_Zero
}
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 (body *Body) UpdatePosition(dt vect.Float) {
if body.UpdatePositionFunc != nil {
body.UpdatePositionFunc(body, dt)
return
}
body.p = vect.Add(body.p, vect.Mult(vect.Add(body.v, body.v_bias), dt))
body.setAngle(body.a + (body.w+body.w_bias)*dt)
body.v_bias = vect.Vector_Zero
body.w_bias = 0.0
}
func (arb *Arbiter) applyCachedImpulse2(dt_coef vect.Float) {
if arb.state == arbiterStateFirstColl && arb.NumContacts > 0 {
return
}
//println("asd")
a := arb.ShapeA.Body
b := arb.ShapeB.Body
for _, con := range arb.Contacts {
j := transform.RotateVect(con.n, transform.Rotation{con.jnAcc, con.jtAcc})
apply_impulses(a, b, con.r1, con.r2, vect.Mult(j, dt_coef))
}
}
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")
}
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 (this *PivotJoint) PreStep(dt vect.Float) {
a, b := this.BodyA, this.BodyB
this.r1 = transform.RotateVect(this.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
this.r2 = transform.RotateVect(this.Anchor2, transform.Rotation{b.rot.X, b.rot.Y})
// Calculate mass tensor
k_tensor(a, b, this.r1, this.r2, &this.k1, &this.k2)
// compute max impulse
this.jMaxLen = this.MaxForce * dt
// calculate bias velocity
delta := vect.Sub(vect.Add(b.p, this.r2), vect.Add(a.p, this.r1))
this.bias = vect.Clamp(vect.Mult(delta, -bias_coef(this.ErrorBias, dt)/dt), this.MaxBias)
}
func findVertsFallback(contacts []*Contact, poly1, poly2 *PolygonShape, n vect.Vect, dist vect.Float) int {
num := 0
for i, v := range poly1.TVerts {
if poly2.ContainsVertPartial(v, vect.Mult(n, -1)) {
c := nextContact(contacts, &num)
c.reset(v, n, dist, hashPair(poly1.Shape.Hash(), HashValue(i)))
}
}
for i, v := range poly2.TVerts {
if poly1.ContainsVertPartial(v, n) {
nextContact(contacts, &num).reset(v, n, dist, hashPair(poly2.Shape.Hash(), HashValue(i)))
}
}
return num
}
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 poly2polyFunc(contacts []*Contact, poly1, poly2 *PolygonShape) int {
min1, mini1 := findMSA(poly2, poly1.TAxes, poly1.NumVerts)
if mini1 == -1 {
return 0
}
min2, mini2 := findMSA(poly1, poly2.TAxes, poly2.NumVerts)
if mini2 == -1 {
return 0
}
// There is overlap, find the penetrating verts
if min1 > min2 {
return findVerts(contacts, poly1, poly2, poly1.TAxes[mini1].N, min1)
} else {
return findVerts(contacts, poly1, poly2, vect.Mult(poly2.TAxes[mini2].N, -1), min2)
}
panic("Never reached")
}
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 (segment *SegmentShape) Moment(mass float32) vect.Float {
offset := vect.Mult(vect.Add(segment.A, segment.B), 0.5)
return vect.Float(mass) * (vect.DistSqr(segment.B, segment.A)/12.0 + vect.LengthSqr(offset))
}
//returns the center of the aabb
func (aabb *AABB) Center() vect.Vect {
return vect.Mult(vect.Add(aabb.Lower, aabb.Upper), 0.5)
}
func (aabb *AABB) Extents() vect.Vect {
return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5)
}
func (this *PivotJoint) ApplyCachedImpulse(dt_coef vect.Float) {
a, b := this.BodyA, this.BodyB
apply_impulses(a, b, this.r1, this.r2, vect.Mult(this.jAcc, dt_coef))
}
func relative_velocity2(a, b *Body, r1, r2 vect.Vect) vect.Vect {
v1 := vect.Add(b.v, vect.Mult(vect.Perp(r2), b.w))
v2 := vect.Add(a.v, vect.Mult(vect.Perp(r1), a.w))
return vect.Sub(v1, v2)
}
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 (xf *Transform) TransformVectInv(v vect.Vect) vect.Vect {
return vect.Add(vect.Mult(xf.Position, -1), xf.RotateVectInv(v))
}