{-# LANGUAGE RecordWildCards #-}
module Physics.Constraints.Contact.Friction where
import Control.Lens
import Physics.Constraint
import Physics.Constraints.Types
import Physics.Constraints.SolutionProcessors
import Physics.Contact.Types
import Physics.Linear
import Utils.Utils
constraintGen :: Flipping Contact'
-> (PhysicalObj, PhysicalObj)
-> Constraint
constraintGen fContact ab =
flipExtract $ flipMap toConstraint fContact ab
{-# INLINE constraintGen #-}
toConstraint :: Contact'
-> (PhysicalObj, PhysicalObj)
-> Constraint
toConstraint c ab = Constraint (jacobian c ab) 0
{-# INLINE toConstraint #-}
jacobian :: Contact'
-> (PhysicalObj, PhysicalObj)
-> V6
jacobian Contact'{..} (a, b) = ja `join3v3` jb
where ja = ta `append2` ((p' `minusV2` xa) `crossV2` ta)
jb = tb `append2` ((p' `minusV2` xb) `crossV2` tb)
xa = _physObjPos a
xb = _physObjPos b
(P2 p') = _contactPenetrator'
ta = negateV2 tb
tb = clockwiseV2 n
n = _contactEdgeNormal'
{-# INLINE jacobian #-}
pairMu :: (Double, Double) -> Double
pairMu (ua, ub) = (ua + ub) / 2
{-# INLINE pairMu #-}
solutionProcessor :: (Double, Double)
-> Lagrangian
-> Lagrangian
-> Lagrangian
-> Processed Lagrangian
solutionProcessor ab nonpen = clampAbs (nonpen & lagrangianVal *~ pairMu ab)
{-# INLINE solutionProcessor #-}