module Physics.Hipmunk.Shape
(
Shape,
ShapeType(..),
newShape,
CollisionType,
getCollisionType,
setCollisionType,
Group,
getGroup,
setGroup,
Layers,
getLayers,
setLayers,
Elasticity,
getElasticity,
setElasticity,
Friction,
getFriction,
setFriction,
SurfaceVel,
getSurfaceVel,
setSurfaceVel,
getBody,
moment,
momentForCircle,
momentForSegment,
momentForPoly,
shapePointQuery,
shapeSegmentQuery,
Segment,
Intersection(..),
epsilon,
(.==.),
isLeft,
isClockwise,
isConvex,
intersects,
polyReduce,
polyCenter,
convexHull
)
where
import Data.List (foldl', sortBy)
import Foreign hiding (rotate, new)
import Foreign.C
import Physics.Hipmunk.Common
import Physics.Hipmunk.Internal
import Physics.Hipmunk.Body (Mass, Moment)
data ShapeType =
Circle {radius :: !Distance}
| LineSegment {start :: !Position,
end :: !Position,
thickness :: !Distance}
| Polygon {vertices :: ![Position]}
deriving (Eq, Ord, Show)
newShape :: Body -> ShapeType -> Position -> IO Shape
newShape body@(B b) (Circle r) off =
withForeignPtr b $ \b_ptr ->
with off $ \off_ptr ->
mallocForeignPtrBytes (136) >>= \shape ->
withForeignPtr shape $ \shape_ptr -> do
wrCircleShapeInit shape_ptr b_ptr off_ptr r
return (S shape body)
newShape body@(B b) (LineSegment p1 p2 r) off =
withForeignPtr b $ \b_ptr ->
with (p1+off) $ \p1off_ptr ->
with (p2+off) $ \p2off_ptr ->
mallocForeignPtrBytes (200) >>= \shape ->
withForeignPtr shape $ \shape_ptr -> do
wrSegmentShapeInit shape_ptr b_ptr p1off_ptr p2off_ptr r
return (S shape body)
newShape body@(B b) (Polygon verts) off =
withForeignPtr b $ \b_ptr ->
with off $ \off_ptr ->
withArrayLen verts $ \verts_len verts_ptr ->
mallocForeignPtrBytes (116) >>= \shape ->
withForeignPtr shape $ \shape_ptr -> do
let verts_len' = fromIntegral verts_len
wrPolyShapeInit shape_ptr b_ptr verts_len' verts_ptr off_ptr
addForeignPtrFinalizer cpShapeDestroy shape
return (S shape body)
foreign import ccall unsafe "wrapper.h"
wrCircleShapeInit :: ShapePtr -> BodyPtr -> VectorPtr
-> CpFloat -> IO ()
foreign import ccall unsafe "wrapper.h"
wrSegmentShapeInit :: ShapePtr -> BodyPtr -> VectorPtr
-> VectorPtr -> CpFloat -> IO ()
foreign import ccall unsafe "wrapper.h"
wrPolyShapeInit :: ShapePtr -> BodyPtr -> CInt -> VectorPtr
-> VectorPtr -> IO ()
foreign import ccall unsafe "wrapper.h &cpShapeDestroy"
cpShapeDestroy :: FunPtr (ShapePtr -> IO ())
getBody :: Shape -> Body
getBody (S _ b) = b
type CollisionType = Word32
getCollisionType :: Shape -> IO CollisionType
getCollisionType (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 80)
setCollisionType :: Shape -> CollisionType -> IO ()
setCollisionType (S shape _) col =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 80) shape_ptr col
type Group = Word32
getGroup :: Shape -> IO Group
getGroup (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 84)
setGroup :: Shape -> Group -> IO ()
setGroup (S shape _) gr =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 84) shape_ptr gr
type Layers = Word32
getLayers :: Shape -> IO Layers
getLayers (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 88)
setLayers :: Shape -> Layers -> IO ()
setLayers (S shape _) lay =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 88) shape_ptr lay
type Elasticity = CpFloat
getElasticity :: Shape -> IO Elasticity
getElasticity (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 44)
setElasticity :: Shape -> Elasticity -> IO ()
setElasticity (S shape _) e =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 44) shape_ptr e
type Friction = CpFloat
getFriction :: Shape -> IO Friction
getFriction (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 52)
setFriction :: Shape -> Friction -> IO ()
setFriction (S shape _) u =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 52) shape_ptr u
type SurfaceVel = Vector
getSurfaceVel :: Shape -> IO SurfaceVel
getSurfaceVel (S shape _) =
withForeignPtr shape (\hsc_ptr -> peekByteOff hsc_ptr 60)
setSurfaceVel :: Shape -> SurfaceVel -> IO ()
setSurfaceVel (S shape _) sv =
withForeignPtr shape $ \shape_ptr -> do
(\hsc_ptr -> pokeByteOff hsc_ptr 60) shape_ptr sv
moment :: Mass -> ShapeType -> Position -> Moment
moment m (Circle r) off = m*(r*r + (off `dot` off))
moment m (LineSegment p1 p2 _) off = momentForSegment m (p1+off) (p2+off)
moment m (Polygon verts) off = momentForPoly m verts off
momentForCircle :: Mass -> (Distance, Distance) -> Position -> Moment
momentForCircle m (ri,ro) off = (m/2)*(ri*ri + ro*ro) + m*(off `dot` off)
momentForSegment :: Mass -> Position -> Position -> Moment
momentForSegment m p1 p2 =
let len' = len (p2 p1)
offset = scale (p1 + p2) (recip 2)
in m * len' * len' / 12 + m * offset `dot` offset
momentForPoly :: Mass -> [Position] -> Position -> Moment
momentForPoly m verts off = (m*sum1)/(6*sum2)
where
verts' = if off /= 0 then map (+off) verts else verts
(sum1,sum2) = calc (pairs (,) verts') 0 0
calc a b c | a `seq` b `seq` c `seq` False = undefined
calc [] acc1 acc2 = (acc1, acc2)
calc ((v1,v2):vs) acc1 acc2 =
let a = v2 `cross` v1
b = v1 `dot` v1 + v1 `dot` v2 + v2 `dot` v2
in calc vs (acc1 + a*b) (acc2 + a)
pairs :: (a -> a -> b) -> [a] -> [b]
pairs f l = zipWith f l (tail $ cycle l)
shapePointQuery :: Shape -> Position -> IO Bool
shapePointQuery (S shape _) p =
withForeignPtr shape $ \shape_ptr ->
with p $ \p_ptr -> do
i <- wrShapePointQuery shape_ptr p_ptr
return (i /= 0)
foreign import ccall unsafe "wrapper.h"
wrShapePointQuery :: ShapePtr -> VectorPtr -> IO CInt
shapeSegmentQuery :: Shape -> Position -> Position
-> IO (Maybe (CpFloat, Vector))
shapeSegmentQuery (S shape _) p1 p2 =
withForeignPtr shape $ \shape_ptr ->
with p1 $ \p1_ptr ->
with p2 $ \p2_ptr ->
allocaBytes (28) $ \info_ptr -> do
i <- wrShapeSegmentQuery shape_ptr p1_ptr p2_ptr info_ptr
if (i == 0) then return Nothing else do
t <- (\hsc_ptr -> peekByteOff hsc_ptr 4) info_ptr
n <- (\hsc_ptr -> peekByteOff hsc_ptr 12) info_ptr
return $ Just (t, n)
foreign import ccall unsafe "wrapper.h"
wrShapeSegmentQuery :: ShapePtr -> VectorPtr -> VectorPtr
-> Ptr () -> IO CInt
epsilon :: CpFloat
epsilon = 1e-25
(.==.) :: CpFloat -> CpFloat -> Bool
a .==. b = abs (a b) <= epsilon
type Segment = (Position, Position)
isClockwise :: [Position] -> Bool
isClockwise = (<= 0) . foldl' (+) 0 . pairs cross
isLeft :: (Position, Position) -> Position -> Ordering
isLeft (p1,p2) vert = compare 0 $ (p1 vert) `cross` (p2 vert)
isConvex :: [Position] -> Bool
isConvex = foldl1 (==) . map (0 <) . filter (0 /=) . pairs cross . pairs ()
intersects :: Segment -> Segment -> Intersection
intersects (a0,a1) (b0,b1) =
let u = a1 a0
v@(Vector vx vy) = b1 b0
w@(Vector wx wy) = a0 b0
d = u `cross` v
parallel = d .==. 0
collinear = all (.==. 0) [u `cross` w, v `cross` w]
a_is_point = u `dot` u .==. 0
b_is_point = v `dot` v .==. 0
(Vector w2x w2y) = a1 b0
(a_in_b, a_in_b') = if vx .==. 0
then swap (wy/vy, w2y/vy)
else swap (wx/vx, w2x/vx)
where swap t@(x,y) | x < y = t
| otherwise = (y,x)
sI = v `cross` w / d
tI = u `cross` w / d
inSegment p (c0,c1)
| vertical = test (gy p) (gy c0, gy c1)
| otherwise = test (gx p) (gx c0, gx c1)
where
vertical = gx c0 .==. gx c1
(gx, gy) = (\(Vector x _) -> x, \(Vector _ y) -> y)
test q (d0,d1) = any (inside q) [(d0,d1), (d1,d0)]
inside n (l,r) = l <= n && n <= r
in if parallel
then case (collinear, a_is_point, b_is_point) of
(False, _, _) ->
IntNowhere
(_, False, False) ->
case (a_in_b > 1 || a_in_b' < 0,
max a_in_b 0, min a_in_b' 1) of
(True, _, _) -> IntNowhere
(_, i0, i1)
| i0 .==. i1 -> IntPoint p0
| otherwise -> IntSegmt (p0,p1)
where p0 = b0 + v `scale` i0
p1 = b0 + v `scale` i1
(_, True, True) ->
if len (b0a0) .==. 0
then IntPoint a0 else IntNowhere
_ ->
let (point,segment)
| a_is_point = (a0, (b0,b1))
| otherwise = (b0, (a0,a1))
in if inSegment point segment
then IntPoint point else IntNowhere
else if all (\x -> inside x (0,1)) [sI, tI]
then IntPoint (a0 + u `scale` sI) else IntNowhere
data Intersection = IntNowhere
| IntPoint !Position
| IntSegmt !Segment
deriving (Eq, Ord, Show)
polyReduce :: Distance -> [Position] -> [Position]
polyReduce delta = go
where
go (p1:p2:ps) | len (p2p1) < delta = go (p1:ps)
| otherwise = p1 : go (p2:ps)
go other = other
polyCenter :: [Position] -> Position
polyCenter verts = foldl' (+) 0 verts `scale` s
where s = recip $ toEnum $ length verts
convexHull :: [Position] -> [Position]
convexHull verts =
let (p0,ps) = takeMinimum verts
(_:p1:points) = p0 : sortBy (isLeft . (,) p0) ps
go hull@(h1:h2:hs) (q1:qs) =
case (isLeft (h2,h1) q1, hs) of
(LT,_) -> go (q1:hull) qs
(_,[]) -> go (q1:hull) qs
_ -> go (h2:hs) (q1:qs)
go hull [] = hull
go _ _ = error "Physics.Hipmunk.Shape.convexHull: never get here"
in go [p1,p0] points
takeMinimum :: Ord a => [a] -> (a, [a])
takeMinimum [] = error "Physics.Hipmunk.Shape.takeMinimum: empty list"
takeMinimum (x:xs) = go x [] xs
where
go min_ acc (y:ys) | y < min_ = go y (min_:acc) ys
| otherwise = go min_ (y:acc) ys
go min_ acc [] = (min_, acc)