hets/FreeCAD/VecTools.hs

{- |
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