func (arb *Arbiter) preStep(inv_dt float64) { const allowedPenetration = 0.01 biasFactor := 0.0 if Settings.PositionCorrection { biasFactor = 0.2 } b1 := arb.ShapeA.Body b2 := arb.ShapeB.Body for i := 0; i < arb.NumContacts; i++ { c := &arb.Contacts[i] c.R1 = vect.Sub(c.Position, b1.Transform.Position) c.R2 = vect.Sub(c.Position, b2.Transform.Position) r1 := c.R1 r2 := c.R2 //Precompute normal mass, tangent mass, and bias rn1 := vect.Dot(r1, c.Normal) rn2 := vect.Dot(r2, c.Normal) kNormal := b1.invMass + b2.invMass kNormal += b1.invI*(vect.Dot(r1, r1)-rn1*rn1) + b2.invI*(vect.Dot(r2, r2)-rn2*rn2) c.MassNormal = 1.0 / kNormal tangent := vect.CrossVF(c.Normal, 1.0) rt1 := vect.Dot(r1, tangent) rt2 := vect.Dot(r2, tangent) kTangent := b1.invMass + b2.invMass kTangent += b1.invI*(vect.Dot(r1, r1)-rt1*rt1) + b2.invI*(vect.Dot(r2, r2)-rt2*rt2) c.MassTangent = 1.0 / kTangent c.Bias = -biasFactor * inv_dt * math.Min(0.0, c.Separation+allowedPenetration) if Settings.AccumulateImpulses { //Apply normal + friction impulse P := vect.Add(vect.Mult(c.Normal, c.Pn), vect.Mult(tangent, c.Pt)) b1.Velocity.Sub(vect.Mult(P, b1.invMass)) b1.AngularVelocity -= b1.invI * vect.Cross(r1, P) b2.Velocity.Add(vect.Mult(P, b2.invMass)) b2.AngularVelocity += b2.invI * vect.Cross(r2, P) } } }
func (arb *Arbiter) applyImpulse() { sA := arb.ShapeA sB := arb.ShapeB b1 := sA.Body b2 := sB.Body //xfA := b1.Transform //xfB := b2.Transform for i := 0; i < arb.NumContacts; i++ { c := &arb.Contacts[i] // Relative velocity at contact dv := vect.Vect{} { t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2)) t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1)) dv = vect.Sub(t1, t2) } // Compute normal impulse vn := vect.Dot(dv, c.Normal) dPn := c.MassNormal * (-vn + c.Bias) if Settings.AccumulateImpulses { // Clamp the accumulated impulse Pn0 := c.Pn c.Pn = math.Max(Pn0+dPn, 0.0) dPn = c.Pn - Pn0 } else { dPn = math.Max(dPn, 0.0) } //Apply contact impulse Pn := vect.Mult(c.Normal, dPn) b1.Velocity.Sub(vect.Mult(Pn, b1.invMass)) b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pn) b2.Velocity.Add(vect.Mult(Pn, b2.invMass)) b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pn) //Relative velocity at contact { t1 := vect.Add(b2.Velocity, vect.CrossFV(b2.AngularVelocity, c.R2)) t2 := vect.Sub(b1.Velocity, vect.CrossFV(b1.AngularVelocity, c.R1)) dv = vect.Sub(t1, t2) } tangent := vect.CrossVF(c.Normal, 1.0) vt := vect.Dot(dv, tangent) dPt := c.MassTangent * (-vt) if Settings.AccumulateImpulses { //Compute friction impulse maxPt := arb.Friction * c.Pn //Clamp Friction oldTangentImpulse := c.Pt c.Pt = clamp(oldTangentImpulse+dPt, -maxPt, maxPt) dPt = c.Pt - oldTangentImpulse } else { maxPt := arb.Friction * dPn dPt = clamp(dPt, -maxPt, maxPt) } // Apply contact impulse Pt := vect.Mult(tangent, dPt) b1.Velocity.Sub(vect.Mult(Pt, b1.invMass)) b1.AngularVelocity -= b1.invI * vect.Cross(c.R1, Pt) b2.Velocity.Add(vect.Mult(Pt, b2.invMass)) b2.AngularVelocity += b2.invI * vect.Cross(c.R2, Pt) } }