dph-examples: pluecker. add matrix/vector mult for rotation
[packages/dph.git] / dph-examples / examples / spectral / Pluecker / Common.hs
1 module Common where
2
3 -- Type synonyms so they work with Data.Vector.Unboxed etc.
4
5 -- A 3-vector *or* points
6 type Vec3 = (Double, Double, Double)
7 -- Line or ray between two points in space
8 -- These are both points, so direction of (a,b) is the vector (b-a)
9 type Line = (Vec3, Vec3)
10
11 -- Pl\"{u}cker coordinates of a line.
12 -- To convert line (a,b) into pluecker, (b-a, a X b)
13 type Pluecker = (Vec3,Vec3)
14
15 -- Triangles are cool because they're obviously always planar.
16 type Triangle = (Vec3, Vec3, Vec3)
17
18 -- Normal and distance of normal from (0,0,0)
19 type Plane = (Vec3,Double)
20
21 {-# INLINE dot #-}
22 dot :: Vec3 -> Vec3 -> Double
23 dot (u,v,w) (x,y,z) = u*x + v*y + w*z
24
25 {-# INLINE cross #-}
26 cross :: Vec3 -> Vec3 -> Vec3
27 cross (u,v,w) (x,y,z) = ( v * z - w * y
28 , w * x - u * z
29 , u * y - v * x)
30
31 {-# INLINE over1 #-}
32 over1 :: (Double -> Double) -> Vec3 -> Vec3
33 over1 f (x,y,z) = (f x, f y, f z)
34
35 {-# INLINE over2 #-}
36 over2 :: (Double -> Double -> Double) -> Vec3 -> Vec3 -> Vec3
37 over2 f (u,v,w) (x,y,z) = (f u x, f v y, f w z)
38
39 {-# INLINE mag #-}
40 mag a = sqrt (a `dot` a)
41
42 {-# INLINE norm #-}
43 norm a = over1 (/m) a
44 where m = mag a
45
46 {-# INLINE vsub #-}
47 vadd = over2 (+)
48 vsub = over2 (-)
49
50 -- Convert a line into pluecker coordinates
51 {-# INLINE plueckerOfLine #-}
52 plueckerOfLine :: Line -> Pluecker
53 plueckerOfLine (p,q) = (q `vsub` p, q `cross` p)
54
55 -- Find intersection of a line and plucker, if exists.
56 -- Otherwise
57 {-# INLINE projectPluecker2 #-}
58 projectPluecker2 :: Pluecker -> Pluecker -> Double
59 projectPluecker2 (u1,v1) (u2,v2) = (u1 `dot` v2) + (u2 `dot` v1)
60
61 -- Check whether pluecker line intersects given triangle
62 {-# INLINE inside #-}
63 inside :: Pluecker -> Triangle -> Bool
64 inside p (a,b,c)
65 = let pr = projectPluecker2 p . plueckerOfLine
66 ab = pr (a,b) < 0
67 bc = pr (b,c) < 0
68 ca = pr (c,a) < 0
69 in (ab && bc && ca) || not (ab || bc || ca)
70
71
72 -- Get plane from triangle, eg for projection
73 {-# INLINE planeOfTriangle #-}
74 planeOfTriangle :: Triangle -> Plane
75 planeOfTriangle (a,b,c)
76 = let n = (a `vsub` b) `cross` (c `vsub` b)
77 n' = norm n
78 d = negate (n' `dot` b)
79 in (n', d)
80
81 -- should this return Maybe Double?
82 {-# INLINE lineOnPlane #-}
83 lineOnPlane :: Line -> Plane -> Double
84 lineOnPlane (p,q) (n,d)
85 = let d1 = (n `dot` p) + d
86 d2 = (n `dot` q) + d
87 in if d1 == d2
88 then 1e100 -- meh. big. fail.
89 else d1 / (d1 - d2)
90
91 -- Project line onto triangle's plane
92 -- Disregard whether intersection is actually inside triangle
93 -- Intersection point is scale from line
94 {-# INLINE lineOnTriangle #-}
95 lineOnTriangle :: Line -> Triangle -> Double
96 lineOnTriangle p t
97 = lineOnPlane p (planeOfTriangle t)
98
99 -- matrix mult
100 {-# INLINE mvecmul #-}
101 mvecmul :: (Vec3,Vec3,Vec3) -> Vec3 -> Vec3
102 mvecmul
103 ((xx, xy, xz)
104 ,(yx, yy, yz)
105 ,(zx, zy, zz))
106 (x, y, z)
107 = ( x * xx + y * yx + z * zx
108 , x * xy + y * yy + z * zz
109 , x * xz + y * yz + z * zz)
110
111 {-# INLINE rotate #-}
112 rotate :: Vec3 -> Double -> Vec3
113 rotate v d
114 =(mvecmul (( cos d, 0, sin d)
115 ,( 0, 1, 0)
116 ,(-(sin d), 0, cos d))
117 (v `vsub` (0,0,5))) `vadd` (0,0,15)