{- |
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 -> Vector3 -> Double
distance3 (Vector3 ax :: Double
ax ay :: Double
ay az :: Double
az) (Vector3 bx :: Double
bx by :: Double
by bz :: Double
bz) = Double -> Double
forall a. Floating a => a -> a
sqrt (Double
x1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
x2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
x3 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x3)
    where
        x1 :: Double
x1 = Double
ax Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
bx
        x2 :: Double
x2 = Double
ay Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
by
        x3 :: Double
x3 = Double
az Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
bz

subtract3 :: Vector3 -> Vector3 -> Vector3
subtract3 :: Vector3 -> Vector3 -> Vector3
subtract3 a :: Vector3
a b :: Vector3
b = Double -> Double -> Double -> Vector3
Vector3 Double
ex Double
ey Double
ez
    where
        ex :: Double
ex = Vector3 -> Double
x Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
x Vector3
b
        ey :: Double
ey = Vector3 -> Double
y Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
y Vector3
b
        ez :: Double
ez = Vector3 -> Double
z Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
z Vector3
b

norm3 :: Vector3 -> Double
norm3 :: Vector3 -> Double
norm3 a :: Vector3
a = Vector3 -> Vector3 -> Double
distance3 Vector3
a (Double -> Double -> Double -> Vector3
Vector3 0 0 0)

scalarprod3 :: Double -> Vector3 -> Vector3
scalarprod3 :: Double -> Vector3 -> Vector3
scalarprod3 a :: Double
a (Vector3 bx :: Double
bx by :: Double
by bz :: Double
bz) = Double -> Double -> Double -> Vector3
Vector3 Double
abx Double
aby Double
abz
    where
        abx :: Double
abx = Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
bx
        aby :: Double
aby = Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
by
        abz :: Double
abz = Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
bz

median3 :: [Vector3] -> Vector3
median3 :: [Vector3] -> Vector3
median3 a :: [Vector3]
a = Double -> Vector3 -> Vector3
scalarprod3 (Int -> Double
forall a b. (Integral a, Num b) => a -> b
fromIntegral ([Vector3] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Vector3]
a)) ([Vector3] -> Vector3
v3Sum [Vector3]
a)

v3Sum :: [Vector3] -> Vector3
v3Sum :: [Vector3] -> Vector3
v3Sum [] = Double -> Double -> Double -> Vector3
Vector3 0 0 0
v3Sum [a :: Vector3
a] = Double -> Double -> Double -> Vector3
Vector3 (Vector3 -> Double
x Vector3
a) (Vector3 -> Double
y Vector3
a) (Vector3 -> Double
z Vector3
a)
v3Sum [a :: Vector3
a, b :: Vector3
b] = Double -> Double -> Double -> Vector3
Vector3 Double
xc Double
yc Double
zc
    where
        xc :: Double
xc = Vector3 -> Double
x Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
b
        yc :: Double
yc = Vector3 -> Double
y Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
b
        zc :: Double
zc = Vector3 -> Double
z Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
b
v3Sum (a :: Vector3
a : b :: Vector3
b : as :: [Vector3]
as) = [Vector3] -> Vector3
v3Sum (Vector3
c Vector3 -> [Vector3] -> [Vector3]
forall a. a -> [a] -> [a]
: [Vector3]
as)
    where
        xc :: Double
xc = Vector3 -> Double
x Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
b
        yc :: Double
yc = Vector3 -> Double
y Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
b
        zc :: Double
zc = Vector3 -> Double
z Vector3
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
b
        c :: Vector3
c = Double -> Double -> Double -> Vector3
Vector3 Double
xc Double
yc Double
zc

v3DotProd :: Vector3 -> Vector3 -> Double
v3DotProd :: Vector3 -> Vector3 -> Double
v3DotProd v1 :: Vector3
v1 v2 :: Vector3
v2 = Vector3 -> Double
x Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector3 -> Double
y Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector3 -> Double
z Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v2

v4DotProd :: Vector4 -> Vector4 -> Double
v4DotProd :: Vector4 -> Vector4 -> Double
v4DotProd v1 :: Vector4
v1 v2 :: Vector4
v2 = Vector4 -> Double
q0 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q0 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q1 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q1 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q2 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q2 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q3 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q3 Vector4
v2

v3VecProd :: Vector3 -> Vector3 -> Vector3
v3VecProd :: Vector3 -> Vector3 -> Vector3
v3VecProd v1 :: Vector3
v1 v2 :: Vector3
v2 = Double -> Double -> Double -> Vector3
Vector3 Double
m1 Double
m2 Double
m3
    where
        m1 :: Double
m1 = Vector3 -> Double
y Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
z Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v2
        m2 :: Double
m2 = Vector3 -> Double
z Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
x Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v2
        m3 :: Double
m3 = Vector3 -> Double
x Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector3 -> Double
y Vector3
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v2

quatProd :: Vector4 -> Vector4 -> Vector4
quatProd :: Vector4 -> Vector4 -> Vector4
quatProd v1 :: Vector4
v1 v2 :: Vector4
v2 = Double -> Double -> Double -> Double -> Vector4
Vector4 Double
m1 Double
m2 Double
m3 Double
m4
    where
        m1 :: Double
m1 = Vector4 -> Double
q3 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q0 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q2 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q1 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector4 -> Double
q1 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q2 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q0 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q3 Vector4
v1
        m2 :: Double
m2 = -Vector4 -> Double
q2 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q0 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q3 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q1 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q0 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q2 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q1 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q3 Vector4
v1
        m3 :: Double
m3 = Vector4 -> Double
q1 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q0 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector4 -> Double
q0 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q1 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q3 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q2 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q2 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q3 Vector4
v1
        m4 :: Double
m4 = -Vector4 -> Double
q0 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q0 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector4 -> Double
q1 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q1 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Vector4 -> Double
q2 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q2 Vector4
v1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Vector4 -> Double
q3 Vector4
v2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector4 -> Double
q3 Vector4
v1

-- transforms quaternion to rotation matrix
quat2matrix :: Vector4 -> Matrix33
quat2matrix :: Vector4 -> Matrix33
quat2matrix q :: Vector4
q = Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Matrix33
Matrix33 Double
m11 Double
m12 Double
m13 Double
m21 Double
m22 Double
m23 Double
m31 Double
m32 Double
m33
             where
                m11 :: Double
m11 = 1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p2 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2
                m12 :: Double
m12 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
p3 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4)
                m13 :: Double
m13 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4)
                m21 :: Double
m21 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
p3 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4)
                m22 :: Double
m22 = 1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p1 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2
                m23 :: Double
m23 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4)
                m31 :: Double
m31 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4)
                m32 :: Double
m32 = 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
p1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p4 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
p2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p3)
                m33 :: Double
m33 = 1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p1 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- 2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
p2 Double -> Double -> Double
forall a. Floating a => a -> a -> a
** 2
                p1 :: Double
p1 = Vector4 -> Double
q0 Vector4
q
                p2 :: Double
p2 = Vector4 -> Double
q1 Vector4
q
                p3 :: Double
p3 = Vector4 -> Double
q2 Vector4
q
                p4 :: Double
p4 = Vector4 -> Double
q3 Vector4
q

rotate :: Matrix33 -> Vector3 -> Vector3
rotate :: Matrix33 -> Vector3 -> Vector3
rotate a :: Matrix33
a v :: Vector3
v = Double -> Double -> Double -> Vector3
Vector3 Double
xx Double
yy Double
zz
    where
        xx :: Double
xx = Matrix33 -> Double
a11 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a12 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a13 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v
        yy :: Double
yy = Matrix33 -> Double
a21 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a22 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a23 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v
        zz :: Double
zz = Matrix33 -> Double
a31 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
x Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a32 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
y Vector3
v Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Matrix33 -> Double
a33 Matrix33
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Vector3 -> Double
z Vector3
v