frag-1.1: src/Collision.hs
{-# LANGUAGE BangPatterns #-}
{- Collision.hs; Mun Hon Cheong (mhch295@cse.unsw.edu.au) 2005
Collision detection with BSP
A tutorial for collision detection along with source code can be found at
http://www.devmaster.net/articles/quake3collision/
-}
module Collision (clipCamera, clipObject, clipRay, CollisionType(..)) where
import BSP
import Matrix
import Camera
data CollisionType = Box !Vec3 !Vec3 !Vec3 | SphereT !Double
epsilon :: Double
epsilon = 0.03125
-------------------------------------------------------------------------------
-- Movement clipping
-- clips a camera against the environment
clipCamera :: BSPMap -> Camera -> Camera -> (Camera, Bool)
clipCamera mp newcam oldcam =
let
(colPos,_,ground,nsteps) =
traceStep (createBox (-20, -50, -20) (20, 50, 20))
mp (cpos oldcam) (cpos newcam)
diffp = vectorSub colPos (cpos oldcam)
newv = vectorAdd diffp (viewPos oldcam)
stepPos = vectorAdd colPos (0,nsteps,0)
stepView = vectorAdd newv (0,nsteps,0)
in (Camera {cpos = stepPos,
viewPos = stepView,
upVec = (0,1,0)}, ground)
-- clips an object against the environment
clipObject :: BSPMap -> Vec3 -> Vec3 -> Vec3 -> (Vec3, Bool)
clipObject mp pos oldpos (sx,sy,sz) =
let
(colPos,_,ground,_) =
traceStep (createBox (-sx, -sy, -sz) (sx,sy,sz)) mp oldpos pos
in (colPos, ground)
-- clips a ray against the environment
clipRay :: BSPMap -> Vec3 -> Vec3 -> Vec3 -> (Vec3, Bool)
clipRay mp pos oldpos (_,_,_) =
let
(colPos,collided,_,_) =
traceo (createSphere 0) mp oldpos pos
in (colPos, collided)
-------------------------------------------------------------------------------
-- Tracing functions
-- Note: If your movement vector is too small you might move through walls
-- or fall through the floor. If your movement vector is too large you won't
-- be able to move at all.
-- allows an object to step across low obstacles
traceStep :: CollisionType -> BSPMap -> Vec3 -> Vec3 -> (Vec3,Bool,Bool,Double)
traceStep cType mp start end
| (col && step) =
let (nsteps,finalPos) = tryToStep cType mp newPos end 1 15
in (finalPos, col, ground, nsteps)
|otherwise = (newPos, col, ground, 0)
where (newPos, col,step,ground,_) = trace cType mp start end
-- returns the point where the collision occured, no sliding or stepping
traceo :: CollisionType -> BSPMap -> Vec3 -> Vec3 -> (Vec3,Bool,Bool,Bool)
traceo cType mp start end
|(newRatio /= 1.0) = (
vectorAdd
start
(vectorMult (vectorSub end start) newRatio),hasCol,step,grounded)
| otherwise = (end,False,step,grounded)
where
(hasCol,step,grounded,newRatio,(_,_,_)) =
fixCheck $
(checkNode cType
(False,False,False,1.0,(0.0,0.0,0.0))
(tree mp) 0.0 1.0 start end)
-- allows an object to slide against the environment
trace :: CollisionType -> BSPMap -> Vec3 -> Vec3 -> (Vec3,Bool,Bool,Bool,Vec3)
trace cType mp start end
| (newRatio /= 1.0) =
let newPos = vectorAdd start (vectorMult (vectorSub end start) newRatio)
moveVec = vectorSub end newPos
dst = dotProd moveVec newNorm
end2 = vectorSub end $ vectorMult newNorm dst
(newPos2,_,_,_,(n2x,n2y,n2z)) = trace cType mp newPos end2
in case (n2y > 0.2 || (grounded)) of
True -> (newPos2, True,step,True ,(n2x,n2y,n2z))
_ -> (newPos2, True,step,False,(n2x,n2y,n2z))
| otherwise = (end,False,step,grounded,newNorm)
where
(_,step,grounded,newRatio,newNorm@(_,_,_)) =
fixCheck $
(checkNode
cType
(False,False,False,1.0, (0.0,0.0,0.0))
(tree mp) 0.0 1.0 start end)
-------------------------------------------------------------------------------
-- creates an AABB for use in collision detection
createBox :: Vec3 -> Vec3 -> CollisionType
createBox v1 v2 = Box v1 v2 (getBoxExtents v1 v2)
-- creates a sphere for collision detection
createSphere :: Double -> CollisionType
createSphere rad = SphereT rad
-- gets the largest ends of the box
getBoxExtents :: Vec3 -> Vec3 -> Vec3
getBoxExtents (x,y,z) (x1,y1,z1) =
(max (-x) x1,max (-y) y1, max (-z) z1)
getBoxOffs :: Vec3 -> Vec3 -> Double
getBoxOffs (x,y,z) (x1,y1,z1) =
(abs (x*x1))+(abs (y*y1))+(abs (z*z1))
getOffset :: CollisionType -> Vec3 -> Double
getOffset (Box _ _ extents) plane = getBoxOffs extents plane
getOffset (SphereT rad) _ = rad
fixCheck ::
Maybe (Bool, Bool, Bool, Double, Vec3) ->
(Bool, Bool, Bool, Double, Vec3)
fixCheck (Just a) = a
fixCheck Nothing = (False,False,False,1.0, (0.0,0.0,0.0))
-- checks if we can step across a low obstacle
tryToStep ::
CollisionType -> BSPMap ->
Vec3 -> Vec3 ->
Double -> Double -> (Double,Vec3)
tryToStep cType mp (x,y,z) (x1,y1,z1) i maxheight
| i < maxheight =
let (pos,col,_,_,_) = trace cType mp (x,y+i,z) (x1,y1+i,z1)
in case (col) of
False -> (i,pos)
_ -> tryToStep cType mp (x,y,z) (x1,y1,z1) (i+1) maxheight
| otherwise = (0,(x,y,z))
-------------------------------------------------------------------------------
-- checks for collisions with brushes
checkBrushes ::
CollisionType -> (Bool, Bool, Bool, Double,Vec3) ->
Tree -> Vec3 -> Vec3 ->
Maybe (Bool, Bool, Bool, Double, Vec3)
checkBrushes cType _ (Leaf leaf) start end =
case (map (checkBrush start end cType) (leafBrushes leaf)) of
[] -> Nothing
[r] -> r
(r:rs) -> foldr brushCompare r rs
checkBrushes _ _ (Branch _ _ _) _ _ = Nothing
checkBrush ::
Vec3-> Vec3 -> CollisionType ->
BSPBrush -> Maybe (Bool,Bool,Bool,Double,Vec3)
checkBrush start end cType brush
|((numOfBrushSides brush) > 0) && ((textureType brush)==1) =
let colout =
checkBrushSides
start end cType
False False False False
(-1.0) (1.0) (0,0,0) (brushSides brush)
in case colout of
Just (out,collided,step,grounded,startR,endR,newNorm) ->
case (startR < endR && startR > -1 && out) of
True -> Just (collided,step,grounded,fixRatio startR,newNorm)
_ -> Nothing
_ -> Nothing
|otherwise = Nothing
where
fixRatio x
| x < 0.0 = 0.0
| otherwise = x
-- we use this function to compare the results
-- of a brush check so that we get the closest
-- collision
brushCompare ::
Maybe (Bool, Bool, Bool, Double, Vec3) ->
Maybe (Bool, Bool, Bool, Double, Vec3) ->
Maybe (Bool, Bool, Bool, Double, Vec3)
brushCompare (Just (a1,b1,c1,d1,e1)) (Just(a2,b2,c2,d2,e2))
| d1 < d2 = Just (a1 || a2, b1 || b2, c1 || c2, d1, e1)
| otherwise = Just (a1 || a2, b1 || b2, c1 || c2, d2, e2)
brushCompare (Just a) Nothing = Just a
brushCompare Nothing (Just b) = Just b
brushCompare Nothing Nothing = Nothing
-- this checks if we have collided with a brush
checkBrushSides ::
Vec3 -> Vec3 -> CollisionType->
Bool -> Bool -> Bool -> Bool -> Double ->
Double -> Vec3 -> [BSPBrushSide] ->
(Maybe (Bool,Bool,Bool,Bool,Double,Double,Vec3))
checkBrushSides (_,_,_) (_,_,_)
_ out collided step ground startR endR cNorm [] =
Just (out,collided,step,ground,startR,endR,cNorm)
checkBrushSides start@(x,_,z) end@(x1,_,z1)
cType out collided step ground startR endR cNorm (b:bs)
| startDist > 0 && endDist > 0 = Nothing
| startDist <= 0 && endDist <= 0 = continue
| startDist > endDist =
case (ratio1 > startR) of
True -> (checkBrushSides start end cType checkout
True mayStep grounded ratio1 endR (bsPlaneNorm b) bs)
_ -> continue
| otherwise =
case (ratio2 < endR) of
True -> (checkBrushSides start end cType checkout
collided step ground startR ratio2 cNorm bs)
_ -> continue
where
checkout
| startDist > 0 = True
| otherwise = out
mayStep
| ((x /= x1 || z /= z1) && planey /= 1) = True
| otherwise = step
grounded
| planey >= 0.2 = True
| otherwise = ground
get2nd3 (_,q,_) = q
planey = get2nd3 (bsPlaneNorm b)
ratio1 = (startDist - epsilon) / (startDist - endDist)
ratio2 = (startDist + epsilon) / (startDist - endDist)
continue = checkBrushSides start end cType checkout
collided step ground startR endR cNorm bs
startDist = vDist start cType (bsPlaneNorm b) (bsPlaneDist b)
endDist = vDist end cType (bsPlaneNorm b) (bsPlaneDist b)
vDist :: Vec3 -> CollisionType -> Vec3 -> Double -> Double
vDist vec box@(Box _ _ _) pnorm pdist =
(dotProd (vectorAdd vec (getVOffs pnorm box)) pnorm) - pdist
vDist vec (SphereT rad) pnorm pdist =
(dotProd vec pnorm) - (pdist + rad)
getVOffs:: Vec3 -> CollisionType -> Vec3
getVOffs (p1,p2,p3) (Box (x,y,z) (x1,y1,z1) _) =
(chooseMin' p1 x x1,chooseMin' p2 y y1, chooseMin' p3 z z1)
where
chooseMin' p mn mx
| p < 0 = mx
| otherwise = mn
getVOffs (_, _, _) (SphereT _) = (0,0,0)
-------------------------------------------------------------------------------
-- recurse down the bsp tree checking for collisions
checkNode ::
CollisionType -> (Bool,Bool,Bool,Double,Vec3) ->
Tree -> Double-> Double -> Vec3 -> Vec3 ->
Maybe (Bool,Bool,Bool,Double,Vec3)
checkNode cType cState (Leaf leaf) _ _ start end =
checkBrushes cType cState (Leaf leaf) start end
checkNode cType cState (Branch node left right) startRatio endRatio start end =
let
sDist = (dotProd (planeNormal node) start) - (dist node)
eDist = (dotProd (planeNormal node) end) - (dist node)
in recurse sDist eDist
where
boffset = getOffset cType (planeNormal node)
recurse sDist eDist
|(sDist >= boffset && eDist >= boffset) =
checkNode cType cState left sDist eDist start end
|(sDist < ((-1)*boffset) && eDist < ((-1)*boffset)) =
checkNode cType cState right sDist eDist start end
|otherwise = split cType cState sDist eDist
startRatio endRatio
start end (Branch node left right)
-- splits the movement vector if it lies on both sides of a splitting plane
split ::
CollisionType -> (Bool,Bool,Bool,Double,Vec3) ->
Double -> Double -> Double -> Double ->
Vec3 -> Vec3 -> Tree -> Maybe (Bool,Bool,Bool,Double,Vec3)
split cType cState startDist endDist startRatio endRatio
start end (Branch node left right)
| startDist < endDist =
let
result1 =
checkNode cType cState
right startRatio (middleR r1) start (middleV r1)
result2 =
checkNode cType cState
left (middleR r2) endRatio (middleV r2) end
in brushCompare result1 result2
| startDist > endDist =
let
result1 =
checkNode cType cState
left startRatio (middleR r2) start (middleV r2)
result2 =
checkNode cType cState
right (middleR r1) endRatio (middleV r1) end
in brushCompare result1 result2
| otherwise =
let
result1 =
checkNode cType cState
left startRatio (middleR 1.0) start (middleV 1.0)
result2 =
checkNode cType cState
right (middleR 0.0) endRatio (middleV 0.0) end
in brushCompare result1 result2
where
inverseDist = 1.0/(startDist - endDist)
boffset = getOffset cType (planeNormal node)
r1 = fixDouble $ (startDist - boffset - epsilon)*inverseDist
r2 = fixDouble $ (startDist + boffset + epsilon)*inverseDist
middleR = getMiddleRatio startRatio endRatio
middleV = getHalfVec start end
fixDouble x
| x < 0.0 = 0.0
| x > 1.0 = 1.0
| otherwise = x
split _ _ _ _ _ _ _ _ (Leaf _) = Nothing
getMiddleRatio :: Double -> Double -> Double -> Double
getMiddleRatio startRatio endRatio ratio =
startRatio + (ratio * (endRatio - startRatio))
getHalfVec :: Vec3 -> Vec3 -> Double -> Vec3
getHalfVec start end ratio =
vectorAdd start (mapTup (ratio*) (vectorSub end start))