69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuDescription : definition of 3-dimensional vector operations, transformations
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savu between rotation matrices and rotation quaternions
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuCopyright : (c) Robert Savu and Uni Bremen 2011
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuLicense : GPLv2 or higher, see LICENSE.txt
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuMaintainer : Robert.Savu@dfki.de
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuStability : experimental
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuPortability : portable
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuDeclaration of the abstract datatypes of FreeCAD terms
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederdistance3 :: Vector3 -> Vector3 -> Double
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederdistance3 (Vector3 ax ay az) (Vector3 bx by bz) = sqrt (x1 * x1 + x2 * x2 + x3 * x3)
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savu x1 = ax - bx
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savu x2 = ay - by
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savu x3 = az - bz
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroedersubtract3 :: Vector3 -> Vector3 -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savusubtract3 a b = Vector3 ex ey ez
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroedernorm3 :: Vector3 -> Double
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savunorm3 a = distance3 a (Vector3 0 0 0)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederscalarprod3 :: Double -> Vector3 -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savuscalarprod3 a (Vector3 bx by bz) = Vector3 abx aby abz
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroedermedian3 :: [Vector3] -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savumedian3 a = scalarprod3 (fromIntegral (length a)) (v3Sum a)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3Sum :: [Vector3] -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savuv3Sum [] = Vector3 0 0 0
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savuv3Sum [a] = Vector3 (x a) (y a) (z a)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3Sum [a, b] = Vector3 xc yc zc
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3Sum (a : b : as) = v3Sum (c : as)
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savu c = Vector3 xc yc zc
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3DotProd :: Vector3 -> Vector3 -> Double
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3DotProd v1 v2 = x v1 * x v2 + y v1 * y v2 + z v1 * z v2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv4DotProd :: Vector4 -> Vector4 -> Double
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv4DotProd v1 v2 = q0 v1 * q0 v2 + q1 v1 * q1 v2 + q2 v1 * q2 v2 + q3 v1 * q3 v2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederv3VecProd :: Vector3 -> Vector3 -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savuv3VecProd v1 v2 = Vector3 m1 m2 m3
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m1 = y v1 * z v2 - z v1 * y v2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m2 = z v1 * x v2 - x v1 * z v2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m3 = x v1 * y v2 - y v1 * x v2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von SchroederquatProd :: Vector4 -> Vector4 -> Vector4
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert SavuquatProd v1 v2 = Vector4 m1 m2 m3 m4
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m1 = q3 v2 * q0 v1 + q2 v2 * q1 v1 - q1 v2 * q2 v1 + q0 v2 * q3 v1
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m2 = -q2 v2 * q0 v1 + q3 v2 * q1 v1 + q0 v2 * q2 v1 + q1 v2 * q3 v1
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m3 = q1 v2 * q0 v1 - q0 v2 * q1 v1 + q3 v2 * q2 v1 + q2 v2 * q3 v1
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m4 = -q0 v2 * q0 v1 - q1 v2 * q1 v1 - q2 v2 * q2 v1 + q3 v2 * q3 v1
239330cd665aac95fcf9cf95449594c96667cbc2Robert Savu-- transforms quaternion to rotation matrix
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederquat2matrix :: Vector4 -> Matrix33
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savuquat2matrix q = Matrix33 m11 m12 m13 m21 m22 m23 m31 m32 m33
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m11 = 1 - 2 * p2 ** 2 - 2 * p3 ** 2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m12 = 2 * (p1 * p2 - p3 * p4)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m13 = 2 * (p1 * p3 + p2 * p4)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m21 = 2 * (p1 * p2 + p3 * p4)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m22 = 1 - 2 * p1 ** 2 - 2 * p3 ** 2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m23 = 2 * (p2 * p3 - p1 * p4)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m31 = 2 * (p1 * p3 - p2 * p4)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m32 = 2 * (p1 * p4 + p2 * p3)
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder m33 = 1 - 2 * p1 ** 2 - 2 * p2 ** 2
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroederrotate :: Matrix33 -> Vector3 -> Vector3
69b1e90bbb27ce2dd365628c07c0f03a3ae97b26Robert Savurotate a v = Vector3 xx yy zz
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder xx = a11 a * x v + a12 a * y v + a13 a * z v
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder yy = a21 a * x v + a22 a * y v + a23 a * z v
3d3889e0cefcdce9b3f43c53aaa201943ac2e895Jonathan von Schroeder zz = a31 a * x v + a32 a * y v + a33 a * z v