// setupContactConstraint initializes contact based constraints.
// Expected to be called on solver setup for each contact point.
func (sol *solver) setupContactConstraint(sc *solverConstraint, sbodA, sbodB *solverBody,
poc *pointOfContact, info *solverInfo, relPosA, relPosB, vel *lin.V3) (relativeVelocity float64) {
bodyA, bodyB := sbodA.oBody, sbodB.oBody // either may be nil if body is static.
{ // scratch v0, v1
torqueAxis0 := sol.v0.Cross(relPosA, poc.sp.normalWorldB)
sc.angularComponentA.SetS(0, 0, 0)
if bodyA != nil {
sc.angularComponentA.MultMv(bodyA.iitw, torqueAxis0)
}
torqueAxis1 := sol.v1.Cross(relPosB, poc.sp.normalWorldB)
sc.angularComponentB.SetS(0, 0, 0)
if bodyB != nil { // scratch v2
sc.angularComponentB.MultMv(bodyB.iitw, sol.v2.Neg(torqueAxis1))
} // scratch v2 free
denom0, denom1 := 0.0, 0.0
if bodyA != nil { // scratch v2
vec := sol.v2.Cross(sc.angularComponentA, relPosA)
denom0 = bodyA.imass + poc.sp.normalWorldB.Dot(vec)
} // scratch v2 free
if bodyB != nil { // scratch v2
sol.v2.Neg(sc.angularComponentB).Cross(sol.v2, relPosB)
denom1 = bodyB.imass + poc.sp.normalWorldB.Dot(sol.v2)
} // scratch v2 free
relaxation := 1.0
sc.jacDiagABInv = relaxation / (denom0 + denom1)
sc.normal.Set(poc.sp.normalWorldB)
sc.relpos1CrossNormal.Set(torqueAxis0)
sc.relpos2CrossNormal.Neg(torqueAxis1)
} // scratch v0, v1 free
// Calculate penetration, friction, and restitution.
penetration := poc.sp.distance + info.linearSlop
{ // scratch v0, v1
v0, v1 := sol.v0.SetS(0, 0, 0), sol.v1.SetS(0, 0, 0)
if bodyA != nil {
bodyA.getVelocityInLocalPoint(relPosA, v0)
}
if bodyB != nil {
bodyB.getVelocityInLocalPoint(relPosB, v1)
}
vel.Sub(v0, v1)
} // scratch v0, v1 free
sc.friction = poc.sp.combinedFriction
relativeVelocity = poc.sp.normalWorldB.Dot(vel)
restitution := poc.sp.combinedRestitution * -relativeVelocity
if restitution <= 0.0 {
restitution = 0.0
}
// Warm start uses the previously applied impulse as an initial guess.
sc.appliedImpulse = poc.sp.warmImpulse * info.warmstartingFactor
{ // scratch v0, v1
linc, angc := sol.v0, sol.v1
if bodyA != nil {
sbodA.applyImpulse(linc.Scale(sc.normal, bodyA.imass), angc.Set(sc.angularComponentA), sc.appliedImpulse)
}
if bodyB != nil {
sbodB.applyImpulse(linc.Scale(sc.normal, bodyB.imass), angc.Neg(sc.angularComponentB), -sc.appliedImpulse)
}
} // scratch v0, v1 free
sc.appliedPushImpulse = 0.0
velocityError := 0.0
vel1Dotn, vel2Dotn := 0.0, 0.0
if bodyA != nil {
vel1Dotn = sc.normal.Dot(sbodA.linearVelocity) + sc.relpos1CrossNormal.Dot(sbodA.angularVelocity)
}
if bodyB != nil { // scratch v0
vel2Dotn = sol.v0.Neg(sc.normal).Dot(sbodB.linearVelocity) + sc.relpos2CrossNormal.Dot(sbodB.angularVelocity)
} // scratch v0 free
velocityError = restitution - (vel1Dotn + vel2Dotn)
erp := info.erp2
if !info.splitImpulse || (penetration > info.splitImpulsePenetrationLimit) {
erp = info.erp
}
positionalError := 0.0
if penetration > 0 {
velocityError -= penetration / info.timestep
} else {
positionalError = -penetration * erp / info.timestep
}
penetrationImpulse := positionalError * sc.jacDiagABInv
velocityImpulse := velocityError * sc.jacDiagABInv
if !info.splitImpulse || penetration > info.splitImpulsePenetrationLimit {
// combine position and velocity into rhs
sc.rhs = penetrationImpulse + velocityImpulse
sc.rhsPenetration = 0.0
} else {
// split position and velocity into rhs and m_rhsPenetration
sc.rhs = velocityImpulse
sc.rhsPenetration = penetrationImpulse
}
sc.cfm = 0
sc.lowerLimit = 0
sc.upperLimit = 1e10
return relativeVelocity
}
