func (s ssort) Dtoc(v *lin.V3) float64 {
normal := &lin.V3{s.x, s.y, s.z}
dot := v.Dot(normal)
dx := normal.X * dot
dy := normal.Y * dot
dz := normal.Z * dot
return dx*dx + dy*dy + dz*dz
}
// setupFrictionConstraint initializes contact based constraints. Expected to be called on
// solver setup for each point of contact.
func (sol *solver) setupFrictionConstraint(sc *solverConstraint, normalAxis *lin.V3, sbodA, sbodB *solverBody,
sp *solverPoint, relPosA, relPosB *lin.V3) {
bodyA, bodyB := sbodA.oBody, sbodB.oBody // either may be nil if body is static.
sc.sbodA, sc.sbodB = sbodA, sbodB
sc.normal.Set(normalAxis)
sc.friction = sp.combinedFriction
sc.oPoint = nil
sc.appliedImpulse = 0.0
sc.appliedPushImpulse = 0.0
// compute torque
ftorqueAxis := sc.relpos1CrossNormal.Cross(relPosA, sc.normal)
sc.angularComponentA.SetS(0, 0, 0)
if bodyA != nil {
sc.angularComponentA.MultMV(bodyA.iitw, ftorqueAxis)
}
{ // scratch v0
ftorqueAxis = sc.relpos2CrossNormal.Cross(relPosB, sol.v0.Neg(sc.normal))
} // scratch v0 free
sc.angularComponentB.SetS(0, 0, 0)
if bodyB != nil {
sc.angularComponentB.MultMV(bodyB.iitw, ftorqueAxis)
}
// compute sc.jacDiagABInv
denom0, denom1 := 0.0, 0.0
if bodyA != nil { // scratch v0
sol.v0.Cross(sc.angularComponentA, relPosA)
denom0 = bodyA.imass + normalAxis.Dot(sol.v0)
} // scratch v0 free
if bodyB != nil { // scratch v0, v1
sol.v0.Cross(sol.v1.Neg(sc.angularComponentB), relPosB)
denom1 = bodyB.imass + normalAxis.Dot(sol.v0)
} // scratch v0, v1 free
relaxation := 1.0
sc.jacDiagABInv = relaxation / (denom0 + denom1)
// compute limits.
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 := -(vel1Dotn + vel2Dotn) // negative relative velocity
sc.rhs = velocityError * sc.jacDiagABInv // velocity impulse
sc.cfm = 0
sc.lowerLimit = 0
sc.upperLimit = 1e10
sc.rhsPenetration = 0
}