VecTools.hs revision 3d3889e0cefcdce9b3f43c53aaa201943ac2e895
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingModule : $Header$
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingDescription : definition of 3-dimensional vector operations, transformations
b99dbaab171d91e1b664397cc40e039d0c087c65fielding between rotation matrices and rotation quaternions
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingCopyright : (c) Robert Savu and Uni Bremen 2011
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingLicense : GPLv2 or higher, see LICENSE.txt
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingMaintainer : Robert.Savu@dfki.de
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingStability : experimental
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingPortability : portable
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingDeclaration of the abstract datatypes of FreeCAD terms
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingdistance3 :: Vector3 -> Vector3 -> Double
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingdistance3 (Vector3 ax ay az) (Vector3 bx by bz) = sqrt (x1 * x1 + x2 * x2 + x3 * x3)
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding x1 = ax - bx
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding x2 = ay - by
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding x3 = az - bz
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingsubtract3 :: Vector3 -> Vector3 -> Vector3
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingsubtract3 a b = Vector3 ex ey ez
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding ex = x a - x b
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding ey = y a - y b
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding ez = z a - z b
64185f9824e42f21ca7b9ae6c004484215c031a7rbbnorm3 :: Vector3 -> Double
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingnorm3 a = distance3 a (Vector3 0 0 0)
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingscalarprod3 :: Double -> Vector3 -> Vector3
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingscalarprod3 a (Vector3 bx by bz) = Vector3 abx aby abz
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding abx = a * bx
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding aby = a * by
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding abz = a * bz
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingmedian3 :: [Vector3] -> Vector3
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingmedian3 a = scalarprod3 (fromIntegral (length a)) (v3Sum a)
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingv3Sum :: [Vector3] -> Vector3
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingv3Sum [] = Vector3 0 0 0
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingv3Sum [a] = Vector3 (x a) (y a) (z a)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingv3Sum [a, b] = Vector3 xc yc zc
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding xc = x a * x b
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding yc = y a * y b
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding zc = z a * z b
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffieldingv3Sum (a : b : as) = v3Sum (c : as)
7e79e8fd53348f9fc6e8009a4a2522425ab6f08ffielding xc = x a * x b
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding yc = y a * y b
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding zc = z a * z b
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding c = Vector3 xc yc zc
785be1b6298010956622771c870ab3cd8ca57a2faaronv3DotProd :: Vector3 -> Vector3 -> Double
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingv3DotProd v1 v2 = x v1 * x v2 + y v1 * y v2 + z v1 * z v2
1b21d7b3d97def358b2e923655edeb16613a1c31gsteinv4DotProd :: Vector4 -> Vector4 -> Double
1b21d7b3d97def358b2e923655edeb16613a1c31gsteinv4DotProd v1 v2 = q0 v1 * q0 v2 + q1 v1 * q1 v2 + q2 v1 * q2 v2 + q3 v1 * q3 v2
1b21d7b3d97def358b2e923655edeb16613a1c31gsteinv3VecProd :: Vector3 -> Vector3 -> Vector3
1b21d7b3d97def358b2e923655edeb16613a1c31gsteinv3VecProd v1 v2 = Vector3 m1 m2 m3
1b21d7b3d97def358b2e923655edeb16613a1c31gstein m1 = y v1 * z v2 - z v1 * y v2
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m2 = z v1 * x v2 - x v1 * z v2
2d71630471d1c23f0137309e3c3957c633ecbfd6rbb m3 = x v1 * y v2 - y v1 * x v2
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingquatProd :: Vector4 -> Vector4 -> Vector4
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingquatProd v1 v2 = Vector4 m1 m2 m3 m4
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m1 = q3 v2 * q0 v1 + q2 v2 * q1 v1 - q1 v2 * q2 v1 + q0 v2 * q3 v1
51af95bb51b5084e883bad250b2afa2838e9ceebfielding m2 = -q2 v2 * q0 v1 + q3 v2 * q1 v1 + q0 v2 * q2 v1 + q1 v2 * q3 v1
d4f1d9c1ff112a8ab9bee31f196973761329b236rbb m3 = q1 v2 * q0 v1 - q0 v2 * q1 v1 + q3 v2 * q2 v1 + q2 v2 * q3 v1
7fae9cc4639013f3c04c085547256c68814aee8ftrawick m4 = -q0 v2 * q0 v1 - q1 v2 * q1 v1 - q2 v2 * q2 v1 + q3 v2 * q3 v1
7fae9cc4639013f3c04c085547256c68814aee8ftrawick-- transforms quaternion to rotation matrix
7fae9cc4639013f3c04c085547256c68814aee8ftrawickquat2matrix :: Vector4 -> Matrix33
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingquat2matrix q = Matrix33 m11 m12 m13 m21 m22 m23 m31 m32 m33
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m11 = 1 - 2 * p2 ** 2 - 2 * p3 ** 2
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m12 = 2 * (p1 * p2 - p3 * p4)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m13 = 2 * (p1 * p3 + p2 * p4)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m21 = 2 * (p1 * p2 + p3 * p4)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m22 = 1 - 2 * p1 ** 2 - 2 * p3 ** 2
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m23 = 2 * (p2 * p3 - p1 * p4)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m31 = 2 * (p1 * p3 - p2 * p4)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m32 = 2 * (p1 * p4 + p2 * p3)
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding m33 = 1 - 2 * p1 ** 2 - 2 * p2 ** 2
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingrotate :: Matrix33 -> Vector3 -> Vector3
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fieldingrotate a v = Vector3 xx yy zz
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding xx = a11 a * x v + a12 a * y v + a13 a * z v
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding yy = a21 a * x v + a22 a * y v + a23 a * z v
09fe0b69d3d1e8c8041c9ce99ee77b8b44b5e3b1fielding zz = a31 a * x v + a32 a * y v + a33 a * z v