module Physics.Hpysics.Collision where import Physics.Hpysics.Types import Physics.Hpysics.Utils import Physics.Hpysics.Body import Physics.Hpysics.Poly import Physics.Hpysics.VClip import Data.List import Data.Ord (comparing) import Control.Monad.Reader import Control.Monad.Writer import Data.Maybe -- | Changes the direction of normal dualContact :: Contact -> Contact dualContact (p, n) = (p, neg n) detectBodyCollision :: Body -> Body -> Maybe Contact detectBodyCollision b1 b2 = let s1 = getShape b1 s2 = getShape b2 in detectCollision s1 s2 -- | Takes 2 shapes, returns Nothing if there's no collision, Just (concact point, -- unit normal vector pointing to the first body) otherwise detectCollision :: Shape -> Shape -> Maybe Contact detectCollision p1 p2 = let (status, f1, f2) = vClip p1 p2 in if status == Penetration then Just $ polyContact f1 f2 else Nothing -- Newton model: v' = - a v, where a is restitution coeff. -- TODO Physsim uses frictional model from [Guendelman, 2003] -- TODO Static bodies -- normal should point *to* the first body applyCollisionImpulse :: Body -> (Body, Maybe Contact) -> Body applyCollisionImpulse b1 (_, Nothing) = b1 applyCollisionImpulse b1 (b2, Just contact) | isStatic b1 = b1 | isStatic b2 = applyCollisionImpulse b1 (getPhantomBody b2, Just contact) | otherwise = let solution = uncurry gaussSolve (collisionLinearSystem b1 b2 contact) v = Vec (solution !! 0) (solution !! 1) (solution !! 2) w = Vec (solution !! 6) (solution !! 7) (solution !! 8) in setAngularVelocity w $ setLinearVelocity v b1 -- | Construct a system of 15 linear equations which determine new velocities. -- -- Based on /Collision Detection and Response for Computer Animation/ by -- Matthew Moore and Jane Wilhelms. -- -- Variables (in this order): v1, v2, w1, w2, R (R is the impulse applied to the -- first body) collisionLinearSystem :: Body -> Body -> Contact -> ([[FloatType]], [FloatType]) collisionLinearSystem b1 b2 (cp, n) = let -- linear velocities v1@(Vec v1x v1y v1z) = getLinearVelocity b1 v2@(Vec v2x v2y v2z) = getLinearVelocity b2 -- angular velocities w1@(Vec w1x w1y w1z) = getAngularVelocity b1 w2@(Vec w2x w2y w2z) = getAngularVelocity b2 -- ro_i -- vector from body's center mass to collision point ro1@(Vec ro1x ro1y ro1z) = cp `sub` getPosition b1 ro2@(Vec ro2x ro2y ro2z) = cp `sub` getPosition b2 -- masses m1 = getMass b1 m2 = getMass b2 -- inertia i@(Mat33 i11 i12 i13 i21 i22 i23 i31 i32 i33) = getInertia b1 j@(Mat33 j11 j12 j13 j21 j22 j23 j31 j32 j33) = getInertia b2 -- cross products ro_i x R as matrices Mat33 r1c11 r1c12 r1c13 r1c21 r1c22 r1c23 r1c31 r1c32 r1c33 = crossAsMatrix ro1 Mat33 r2c11 r2c12 r2c13 r2c21 r2c22 r2c23 r2c31 r2c32 r2c33 = crossAsMatrix ro2 -- collision frame (fi@(Vec fi1 fi2 fi3), fj@(Vec fj1 fj2 fj3), fk@(Vec fk1 fk2 fk3)) = makeOrthoBasis n -- cross products ro_i x k Vec r1ck1 r1ck2 r1ck3 = ro1 `cross` fk Vec r2ck1 r2ck2 r2ck3 = ro2 `cross` fk -- product I_i w_i Vec icw1 icw2 icw3 = i `mat33xVec` w1 Vec jcw1 jcw2 jcw3 = j `mat33xVec` w2 rhs = - fk <.> (v2 `add` w2`cross`ro2 `sub` v1 `sub` w1`cross`ro1) * alpha alpha = sqrt $ getRestitution b1 * getRestitution b2 in ( -- v1x v1y v1z v2x v2y v2z w1x w1y w1z w2x w2y w2z Rx Ry Rz [[ m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 0 ] -- \ ,[ 0, m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0 ] -- } v1' m1' - R = v1 m1 ,[ 0, 0, m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1 ] -- / ,[ 0, 0, 0, m2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 ] -- \ ,[ 0, 0, 0, 0, m2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0 ] -- } v2' m2' + R = v2 m2 ,[ 0, 0, 0, 0, 0, m2, 0, 0, 0, 0, 0, 0, 0, 0, 1 ] -- / ,[ 0, 0, 0, 0, 0, 0, i11, i12, i13, 0, 0, 0, -r1c11, -r1c12, -r1c13 ] -- \ ,[ 0, 0, 0, 0, 0, 0, i21, i22, i23, 0, 0, 0, -r1c21, -r1c22, -r1c23 ] -- } I1 w1' -ro1 x R = I1 w1 ,[ 0, 0, 0, 0, 0, 0, i31, i32, i33, 0, 0, 0, -r1c31, -r1c32, -r1c33 ] -- / ,[ 0, 0, 0, 0, 0, 0, 0, 0, 0, j11, j12, j13, r2c11, r2c12, r2c13 ] -- \ ,[ 0, 0, 0, 0, 0, 0, 0, 0, 0, j21, j22, j23, r2c21, r2c22, r2c23 ] -- } I2 w2' +ro2 x R = I2 w2 ,[ 0, 0, 0, 0, 0, 0, 0, 0, 0, j31, j32, j33, r2c31, r2c32, r2c33 ] -- / ,[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, fi1, fi2, fi3 ] -- R . i = 0 ,[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, fj1, fj2, fj3 ] -- R . j = 0 ,[-fk1,-fk2,-fk3,fk1,fk2,fk3,-r1ck1,-r1ck2,-r1ck3,r2ck1,r2ck2,r2ck3,0,0,0]]-- (v2 + w2 x ro2 - v1 - w1 x ro1) k = 0 ,[ m1*v1x, m1*v1y, m1*v1z, m2*v2x, m2*v2y, m2*v2z, icw1, icw2, icw3, jcw1, jcw2, jcw3, 0, 0, rhs]) -- | -- | Finds pair of closest points on two features. Works only for those -- pairs of features, which are terminal states for V-Clip. closestPoints :: PolyFeature -> PolyFeature -> (Vec, Vec) closestPoints vertex@(Vertex _,_) f2 = (v1, closestPoint) where closestPoint = case f2 of (Vertex _,_) -> computeVertex f2 (Edge _ _,_) -> let (a,b) = computeEdge f2 in projectToLine a b v1 (Face _ ,_) -> let plane = computeFacePlane f2 in projectToPlane plane v1 v1 = computeVertex vertex closestPoints f1@(Edge _ _,_) f2@(Edge _ _,_) = let -- v1 = l(a1-b1)+b1 -- v2 = m(a2-b2)+b2 -- (v1-v2)^2 -> min -- 0 = d(v1-v2)^2/dl = 2(a1-b1)(v1-v2) and the same for m -- (this can also be obtained from perpendicularity) -- l (a1-b1)^2+m (a1-b1)(a2-b2) = -(a1-b1)(b1+b2) -- l (a1-b1)(a2-b2)+m (a2-b2)^2 = -(a2-b2)(b1+b2) (a1,b1) = computeEdge f1 (a2,b2) = computeEdge f2 a1_b1 = a1`sub`b1 a2_b2 = a2`sub`b2 dot12 = a1_b1 <.> a2_b2 bb = b1`add`b2 sol = solveLinear2 (dotSquare a1_b1) dot12 (- a1_b1 <.> bb) dot12 (dotSquare a2_b2) (- a2_b2 <.> bb) in case sol of Just (l, m) -> let p1 = if l < 0 then b1 else if l > 1 then a1 else lc a1 b1 l p2 = if m < 0 then b2 else if m > 1 then a2 else lc a2 b2 m in (p1, p2) -- if the system of linear equations is singular, then segments are -- parallel. One of two alternatives is possible: either at least one of -- 4 vertices projects into interior of other segment ("good" vertex), or not. -- In the latter case we just need to find a pair of closest vertices Nothing -> let linePointPairs = [((a1,b1),a2), ((a1,b1),b2), ((a2,b2),a1), ((a2,b2),b1)] maybeGoodPoint = find ((\l->l>0 && l<1).(uncurry$uncurry projectToLineLambda)) linePointPairs in case maybeGoodPoint of Just (_, v) -> if v == a1 || v == b1 -- we need to know what segment contains this good point then (v, projectToLine a2 b2 v) else (projectToLine a1 b1 v, v) Nothing -> minimumBy (comparing $ uncurry distance) [(v1,v2) | v1 <- [a1,b1], v2 <- [a2,b2]] closestPoints edge@(Edge _ _,_) face@(Face _,_) = -- Edge-Square pair can be returned from V-Clip only if -- penetration is detected. Then just find intersection point of line and -- plane. Also edge can lie in the face's plane, then use the following -- algorithm: -- - if any of vertices projects inside the edge, return the projection -- - otherwise edge lies inside polygon => return its middle let (a,b) = computeEdge edge (plane,vs) = computeFace face -- case of degeneracy lambda = fromMaybe (1/2) $ find (\l->l>=0&&l<=1) $ map (projectToLineLambda a b) vs p1 = lc a b lambda p = maybe p1 id (planeLineIntersection plane a b) in (p,p) closestPoints (Face _,_) (Face _,_) = error "closestPoints is not implemented for two faces" closestPoints x y = swap $ closestPoints y x -- | Assuming two features contacting, returns contact point and normal to the -- first feature. polyContact :: PolyFeature -> PolyFeature -> Contact polyContact feature face@(Face _, _) = (contactPoint, normal) -- normal points ouside second body, so it's ok where (p1,p2) = closestPoints feature face contactPoint = (1/2)`scale`(p1`add`p2) Plane normal _ = computeFacePlane face polyContact face@(Face _, _) feature = dualContact $ polyContact feature face polyContact (f1,_) (f2,_) = error $ "polyContact is not implemented for "++featureType f1++" and "++featureType f2