{- |
Module : ./FreeCAD/VecTools.hs
Description : definition of 3-dimensional vector operations, transformations
between rotation matrices and rotation quaternions
Copyright : (c) Robert Savu and Uni Bremen 2011
License : GPLv2 or higher, see LICENSE.txt
Maintainer : Robert.Savu@dfki.de
Stability : experimental
Portability : portable
Declaration of the abstract datatypes of FreeCAD terms
-}
module FreeCAD.VecTools where
import FreeCAD.As
distance3 :: Vector3 -> Vector3 -> Double
distance3 (Vector3 ax ay az) (Vector3 bx by bz) = sqrt (x1 * x1 + x2 * x2 + x3 * x3)
where
x1 = ax - bx
x2 = ay - by
x3 = az - bz
subtract3 :: Vector3 -> Vector3 -> Vector3
subtract3 a b = Vector3 ex ey ez
where
ex = x a - x b
ey = y a - y b
ez = z a - z b
norm3 :: Vector3 -> Double
norm3 a = distance3 a (Vector3 0 0 0)
scalarprod3 :: Double -> Vector3 -> Vector3
scalarprod3 a (Vector3 bx by bz) = Vector3 abx aby abz
where
abx = a * bx
aby = a * by
abz = a * bz
median3 :: [Vector3] -> Vector3
median3 a = scalarprod3 (fromIntegral (length a)) (v3Sum a)
v3Sum :: [Vector3] -> Vector3
v3Sum [] = Vector3 0 0 0
v3Sum [a] = Vector3 (x a) (y a) (z a)
v3Sum [a, b] = Vector3 xc yc zc
where
xc = x a * x b
yc = y a * y b
zc = z a * z b
v3Sum (a : b : as) = v3Sum (c : as)
where
xc = x a * x b
yc = y a * y b
zc = z a * z b
c = Vector3 xc yc zc
v3DotProd :: Vector3 -> Vector3 -> Double
v3DotProd v1 v2 = x v1 * x v2 + y v1 * y v2 + z v1 * z v2
v4DotProd :: Vector4 -> Vector4 -> Double
v4DotProd v1 v2 = q0 v1 * q0 v2 + q1 v1 * q1 v2 + q2 v1 * q2 v2 + q3 v1 * q3 v2
v3VecProd :: Vector3 -> Vector3 -> Vector3
v3VecProd v1 v2 = Vector3 m1 m2 m3
where
m1 = y v1 * z v2 - z v1 * y v2
m2 = z v1 * x v2 - x v1 * z v2
m3 = x v1 * y v2 - y v1 * x v2
quatProd :: Vector4 -> Vector4 -> Vector4
quatProd v1 v2 = Vector4 m1 m2 m3 m4
where
m1 = q3 v2 * q0 v1 + q2 v2 * q1 v1 - q1 v2 * q2 v1 + q0 v2 * q3 v1
m2 = -q2 v2 * q0 v1 + q3 v2 * q1 v1 + q0 v2 * q2 v1 + q1 v2 * q3 v1
m3 = q1 v2 * q0 v1 - q0 v2 * q1 v1 + q3 v2 * q2 v1 + q2 v2 * q3 v1
m4 = -q0 v2 * q0 v1 - q1 v2 * q1 v1 - q2 v2 * q2 v1 + q3 v2 * q3 v1
-- transforms quaternion to rotation matrix
quat2matrix :: Vector4 -> Matrix33
quat2matrix q = Matrix33 m11 m12 m13 m21 m22 m23 m31 m32 m33
where
m11 = 1 - 2 * p2 ** 2 - 2 * p3 ** 2
m12 = 2 * (p1 * p2 - p3 * p4)
m13 = 2 * (p1 * p3 + p2 * p4)
m21 = 2 * (p1 * p2 + p3 * p4)
m22 = 1 - 2 * p1 ** 2 - 2 * p3 ** 2
m23 = 2 * (p2 * p3 - p1 * p4)
m31 = 2 * (p1 * p3 - p2 * p4)
m32 = 2 * (p1 * p4 + p2 * p3)
m33 = 1 - 2 * p1 ** 2 - 2 * p2 ** 2
p1 = q0 q
p2 = q1 q
p3 = q2 q
p4 = q3 q
rotate :: Matrix33 -> Vector3 -> Vector3
rotate a v = Vector3 xx yy zz
where
xx = a11 a * x v + a12 a * y v + a13 a * z v
yy = a21 a * x v + a22 a * y v + a23 a * z v
zz = a31 a * x v + a32 a * y v + a33 a * z v