func (arb *Arbiter) preStep2(inv_dt, slop, bias vect.Float) {
a := arb.ShapeA.Body
b := arb.ShapeB.Body
for i := 0; i < arb.NumContacts; i++ {
con := arb.Contacts[i]
// Calculate the offsets.
con.r1 = vect.Sub(con.p, a.p)
con.r2 = vect.Sub(con.p, b.p)
//con.Normal = vect.Vect{-1,0}
// Calculate the mass normal and mass tangent.
con.nMass = 1.0 / k_scalar(a, b, con.r1, con.r2, con.n)
con.tMass = 1.0 / k_scalar(a, b, con.r1, con.r2, vect.Perp(con.n))
// Calculate the target bias velocity.
con.bias = -bias * inv_dt * vect.FMin(0.0, con.Dist+slop)
con.jBias = 0.0
//con.jtAcc = 0
//con.jnAcc = 0
//fmt.Println("con.dist", con.dist)
// Calculate the target bounce velocity.
con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n) * arb.e
}
}
// Checks if verts forms a valid polygon.
// The vertices must be convex and winded clockwise.
func (verts Vertices) ValidatePolygon() bool {
numVerts := len(verts)
for i := 0; i < numVerts; i++ {
a := verts[i]
b := verts[(i+1)%numVerts]
c := verts[(i+2)%numVerts]
if vect.Cross(vect.Sub(b, a), vect.Sub(c, b)) > 0.0 {
return false
}
}
return true
}
func TestOverlap2(a, b AABB) bool {
d1 := vect.Sub(b.Lower, a.Upper)
d2 := vect.Sub(a.Lower, b.Upper)
if d1.X > 0.0 || d1.Y > 0.0 {
return false
}
if d2.X > 0.0 || d2.Y > 0.0 {
return false
}
return true
}
func (this *PivotJoint) ApplyImpulse() {
a, b := this.BodyA, this.BodyB
r1, r2 := this.r1, this.r2
// compute relative velocity
vr := relative_velocity2(a, b, r1, r2)
// compute normal impulse
j := mult_k(vect.Sub(this.bias, vr), this.k1, this.k2)
jOld := this.jAcc
this.jAcc = vect.Clamp(vect.Add(this.jAcc, j), this.jMaxLen)
j = vect.Sub(this.jAcc, jOld)
// apply impulse
apply_impulses(a, b, this.r1, this.r2, j)
}
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 (arb *Arbiter) update(a, b *Shape, contacts []*Contact, numContacts int) {
oldContacts := arb.Contacts
arb.ShapeA, arb.ShapeB = a, b
arb.BodyA, arb.BodyB = arb.ShapeA.Body, arb.ShapeB.Body
for _, oldC := range oldContacts {
for _, newC := range contacts {
if newC.hash == oldC.hash {
newC.jnAcc = oldC.jnAcc
newC.jtAcc = oldC.jtAcc
newC.jBias = oldC.jBias
}
}
}
arb.Contacts = contacts
arb.NumContacts = numContacts
arb.u = a.u * b.u
arb.e = a.e * b.e
arb.Surface_vr = vect.Sub(a.Surface_v, b.Surface_v)
if arb.state == arbiterStateCached {
arb.state = arbiterStateFirstColl
}
}
// Recalculates the global center of the circle and the the bounding box.
func (circle *CircleShape) update(xf transform.Transform) AABB {
//global center of the circle
center := xf.TransformVect(circle.Position)
circle.Tc = center
rv := vect.Vect{circle.Radius, circle.Radius}
return AABB{
vect.Sub(center, rv),
vect.Add(center, rv),
}
}
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 (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}))
}
}
// 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 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 (spring *DampedSpring) PreStep(dt vect.Float) {
a := spring.BodyA
b := spring.BodyB
spring.r1 = transform.RotateVect(spring.Anchor1, transform.Rotation{a.rot.X, a.rot.Y})
spring.r2 = transform.RotateVect(spring.Anchor2, transform.Rotation{a.rot.X, a.rot.Y})
delta := vect.Sub(vect.Add(b.p, spring.r2), vect.Add(a.p, spring.r1))
dist := vect.Length(delta)
if dist == 0 {
dist = vect.Float(math.Inf(1))
}
spring.n = vect.Mult(delta, 1.0/dist)
k := k_scalar(a, b, spring.r1, spring.r2, spring.n)
spring.nMass = 1.0 / k
spring.targetVRN = 0.0
spring.vCoef = vect.Float(1.0 - math.Exp(float64(-spring.Damping*dt*k)))
fSpring := spring.SpringForceFunc(spring, dist)
apply_impulses(a, b, spring.r1, spring.r2, vect.Mult(spring.n, fSpring*dt))
}
func seg2segFunc(contacts []*Contact, _shapeA *SegmentShape, _shapeB *SegmentShape) int {
A1B1 := vect.Sub(_shapeA.B, _shapeA.A)
A1B2 := vect.Sub(_shapeB.B, _shapeA.A)
A1A2 := vect.Sub(_shapeB.A, _shapeA.A)
a := float32(vect.Cross(A1B1, A1B2) * vect.Cross(A1B1, A1A2))
A2B2 := vect.Sub(_shapeB.B, _shapeB.A)
A2B1 := vect.Sub(_shapeA.B, _shapeB.A)
A2A1 := vect.Sub(_shapeA.A, _shapeB.A)
b := float32(vect.Cross(A2B2, A2B1) * vect.Cross(A2B2, A2A1))
if a < 0.0 && b < 0.0 {
var l1, l2 Line
l1.Setup(_shapeA)
l2.Setup(_shapeB)
v1 := LineLine(&l1, &l2)
con := contacts[0]
con.reset(v1, _shapeA.Tn, 0.0, 0)
return 1
}
return 0
}
func (aabb *AABB) Perimeter() vect.Float {
w := vect.Sub(aabb.Upper, aabb.Lower)
return 2 * (w.X + w.Y)
}
func (aabb *AABB) Extents() vect.Vect {
return vect.Mult(vect.Sub(aabb.Upper, aabb.Lower), .5)
}
// 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
}