hets/FreeCAD/VecTools.hs

	VecTools.hs revision 239330cd665aac95fcf9cf95449594c96667cbc2
{- |
Module      :  $Header$
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)