Add intersection with cylinders

src/Geometry/Intersect.hs

@@ -4,16 +4,16 @@
 {- Testing for and finding intersection points.  -}
 module Geometry.Intersect where
 
-import Linear
-import Control.Lens
 import Control.Applicative
+import Control.Lens
+import Control.Monad
 import Data.List
 import Data.Maybe
 import Geometry.Data
 import Geometry.LHS
 import Geometry.Vector
 import Geometry.Vector3D
-import Control.Monad
+import Linear
 
 -- | If two lines intersect, return 'Just' that point.
 intersectLineLine :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
@@ -25,11 +25,17 @@ intersectLineLine (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
     den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
     t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
 
--- note that the second argument is a vector, not the second point of the line
-intersectLinePlane :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Point3
-intersectLinePlane l v p n = case dot v n of
-    0 -> Nothing
-    x -> Just $ l + (dot (p - l) n / x) *.*.* v
+---- note that the second argument is a vector, not the second point of the line
+--intersectLinePlane :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Point3
+--intersectLinePlane l v p n = case dot v n of
+--    0 -> Nothing
+--    x -> Just $ l + (dot (p - l) n / x) *.*.* v
+
+intersectLinePlaneAlong :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Float
+intersectLinePlaneAlong x y p n = do
+    let den = dot (y - x) n
+    guard $ den /= 0
+    return $ dot (p - x) n / den
 
 -- this needs to be checked
 intersectSegPlane :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Point3
@@ -41,11 +47,16 @@ intersectSegPlane sp ep p n = case dot v n of
   where
     v = ep - sp
 
-intersectSegSurface :: Point3 -> Point3 -> Point3 -> Point3 
-    -> [(Point3,Point3)] -> Maybe Point3
+intersectSegSurface ::
+    Point3 ->
+    Point3 ->
+    Point3 ->
+    Point3 ->
+    [(Point3, Point3)] ->
+    Maybe Point3
 intersectSegSurface sp ep p n ss = do
     xp <- intersectSegPlane sp ep p n
-    let f (a,b) = isNHS a b xp
+    let f (a, b) = isNHS a b xp
     guard $ all f ss
     return xp
 
@@ -117,9 +128,10 @@ intersectSegLineext (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
     t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
     u' = (y1 - y2) * (x1 - x3) - (x1 - x2) * (y1 - y3)
 
--- | Intersect a segment with a line.
--- the line intersects with the first endpoint of the segment
--- but NOT the second
+{- | Intersect a segment with a line.
+ the line intersects with the first endpoint of the segment
+ but NOT the second
+-}
 intersectSegLine :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
 {-# INLINE intersectSegLine #-}
 intersectSegLine (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
@@ -220,8 +232,8 @@ myIntersectLineLine a@(V2 ax _) b c@(V2 cx _) d
             Just 0 -> Nothing
             _ -> liftA2 V2 newx ((linGrad a b ^*^ newx) ^+^ axisInt a b)
   where
---    (^-^) = liftA2 (-)
---    (^+^) = liftA2 (+)
+    --    (^-^) = liftA2 (-)
+    --    (^+^) = liftA2 (+)
     (^/^) = liftA2 (/)
     (^*^) = liftA2 (*)
     newx = (axisInt c d ^-^ axisInt a b) ^/^ (linGrad a b ^-^ linGrad c d)
@@ -345,7 +357,7 @@ intersectCircSeg c r a b
   where
     d = closestPointOnLine a b c
     x = dist d c
-    y = r ^ (2::Int) - x ^ (2::Int)
+    y = r ^ (2 :: Int) - x ^ (2 :: Int)
     z = sqrt y
     v = z *.* normalizeV (b -.- a)
 
@@ -353,20 +365,52 @@ intersectCircLineAlong :: Point2 -> Float -> Point2 -> Point2 -> Maybe (Float, F
 intersectCircLineAlong p r x y = do
     let d = y - x
         f = p - x
-        dis = (dot d f / dot d d) ** 2 + (dot f f - r ** 2)/dot d d
-    guard $ dis > 0
+        dsc = (dot d f / dot d d) ** 2 - (dot f f - r ** 2) / dot d d
+    guard $ dsc > 0
     let a = dot d f / dot d d
-    return (a - dis, a + dis)
+    return (a - sqrt dsc, a + sqrt dsc)
 
-intersectCylSeg :: Point3 -> Float -> Float -> Point3 -> Point3 -> Maybe (Point3, Point3)
-intersectCylSeg p r h x y = do
-    (a,b) <- intersectCircLineAlong (p ^. _xy) r (x ^. _xy) (y ^. _xy)
-    Nothing
+intersectCircLine :: Point2 -> Float -> Point2 -> Point2 -> Maybe (Point2, Point2)
+intersectCircLine c r x y =
+    intersectCircLineAlong c r x y
+        & _Just . _1 %~ f
+        & _Just . _2 %~ f
+  where
+    f :: Float -> Point2
+    f a = x + (a *.* (y - x))
+
+intersectCylSeg ::
+    Point3 ->
+    Float ->
+    Float ->
+    Point3 ->
+    Point3 ->
+    (Maybe (Point3, Point3), Maybe (Point3, Point3))
+intersectCylSeg p r h s e = fromMaybe (Nothing, Nothing) $ do
+    (a, b) <- intersectCircLineAlong (p ^. _xy) r (s ^. _xy) (e ^. _xy)
+    return (f min a, f max b)
+  where
+    mtopx = intersectLinePlaneAlong s e (V3 0 0 h) (V3 0 0 1)
+    mbotx = intersectLinePlaneAlong s e (V3 0 0 0) (V3 0 0 1)
+    v = e - s
+    f mm a = do
+        let x = s + a *.*.* v
+        if x ^. _z < (h + p ^. _z) && x ^. _z > (p ^. _z)
+            then return (x, v & _xy %~ reflectInNormal (x ^. _xy - p ^. _xy))
+            else do
+                topx <- mtopx
+                botx <- mbotx
+                let d = mm topx botx
+                guard $ d > 0 && d < 1
+                let int = s + d *.*.* v
+                guard $ dist (int ^. _xy) (p ^. _xy) <= r
+                return (int, reflectInNormal (V3 0 0 1) v)
 
 intersectCircSegTest :: Point2 -> Float -> Point2 -> Point2 -> Bool
-intersectCircSegTest c r x y = intersectSegSegTest (c + z) (c - z) x y
-    || dist c x <= r
-    || dist c y <= r
+intersectCircSegTest c r x y =
+    intersectSegSegTest (c + z) (c - z) x y
+        || dist c x <= r
+        || dist c y <= r
   where
     z = r *.* vNormal (normalizeV x - y)
 

src/Geometry/Vector.hs

@@ -2,6 +2,7 @@
 
 module Geometry.Vector where
 
+import Linear
 import Geometry.Data
 
 -- | Moves from two to three dimensions, adding zero in z direction.
@@ -218,3 +219,7 @@ xyzV4 (V4 x y z _) = V3 x y z
 xyV3 :: V3 a -> V2 a
 {-# INLINE xyV3 #-}
 xyV3 (V3 x y _) = V2 x y
+
+reflectInNormal :: (Metric v,Fractional a,Num (v a)) => v a -> v a -> v a
+{-# INLINE reflectInNormal #-}
+reflectInNormal n v = v - (2 * project n v)