From c41c0badf5d4f586d7d49790471f4c5d9ac2b382 Mon Sep 17 00:00:00 2001 From: Marc Sunet Date: Sun, 2 Sep 2012 21:03:52 +0200 Subject: Added inverse --- Spear/Math/Matrix4.hs | 150 +++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 148 insertions(+), 2 deletions(-) diff --git a/Spear/Math/Matrix4.hs b/Spear/Math/Matrix4.hs index 5c54aea..ac2761f 100644 --- a/Spear/Math/Matrix4.hs +++ b/Spear/Math/Matrix4.hs @@ -40,7 +40,8 @@ module Spear.Math.Matrix4 , Spear.Math.Matrix4.zipWith , Spear.Math.Matrix4.map , transpose -, inverseTransform +, inverseTransform +, inverse , mul , mulp , muld @@ -443,7 +444,152 @@ inverseTransform mat = (V3.x r) (V3.y r) (V3.z r) (-t `V3.dot` r) (V3.x u) (V3.y u) (V3.z u) (-t `V3.dot` u) (V3.x f) (V3.y f) (V3.z f) (-t `V3.dot` f) - 0 0 0 1 + 0 0 0 1 + + +-- | Invert the given matrix. +inverse :: Matrix4 -> Matrix4 +inverse mat = + let + a00 = m00 mat + a01 = m01 mat + a02 = m02 mat + a03 = m03 mat + a04 = m10 mat + a05 = m11 mat + a06 = m12 mat + a07 = m13 mat + a08 = m20 mat + a09 = m21 mat + a10 = m22 mat + a11 = m23 mat + a12 = m30 mat + a13 = m31 mat + a14 = m32 mat + a15 = m33 mat + + m00' = a05 * a10 * a15 + - a05 * a11 * a14 + - a09 * a06 * a15 + + a09 * a07 * a14 + + a13 * a06 * a11 + - a13 * a07 * a10 + + m04' = -a04 * a10 * a15 + + a04 * a11 * a14 + + a08 * a06 * a15 + - a08 * a07 * a14 + - a12 * a06 * a11 + + a12 * a07 * a10 + + m08' = a04 * a09 * a15 + - a04 * a11 * a13 + - a08 * a05 * a15 + + a08 * a07 * a13 + + a12 * a05 * a11 + - a12 * a07 * a09 + + m12' = -a04 * a09 * a14 + + a04 * a10 * a13 + + a08 * a05 * a14 + - a08 * a06 * a13 + - a12 * a05 * a10 + + a12 * a06 * a09 + + m01' = -a01 * a10 * a15 + + a01 * a11 * a14 + + a09 * a02 * a15 + - a09 * a03 * a14 + - a13 * a02 * a11 + + a13 * a03 * a10 + + m05' = a00 * a10 * a15 + - a00 * a11 * a14 + - a08 * a02 * a15 + + a08 * a03 * a14 + + a12 * a02 * a11 + - a12 * a03 * a10 + + m09' = -a00 * a09 * a15 + + a00 * a11 * a13 + + a08 * a01 * a15 + - a08 * a03 * a13 + - a12 * a01 * a11 + + a12 * a03 * a09 + + m13' = a00 * a09 * a14 + - a00 * a10 * a13 + - a08 * a01 * a14 + + a08 * a02 * a13 + + a12 * a01 * a10 + - a12 * a02 * a09 + + m02' = a01 * a06 * a15 + - a01 * a07 * a14 + - a05 * a02 * a15 + + a05 * a03 * a14 + + a13 * a02 * a07 + - a13 * a03 * a06 + + m06' = -a00 * a06 * a15 + + a00 * a07 * a14 + + a04 * a02 * a15 + - a04 * a03 * a14 + - a12 * a02 * a07 + + a12 * a03 * a06 + + m10' = a00 * a05 * a15 + - a00 * a07 * a13 + - a04 * a01 * a15 + + a04 * a03 * a13 + + a12 * a01 * a07 + - a12 * a03 * a05 + + m14' = -a00 * a05 * a14 + + a00 * a06 * a13 + + a04 * a01 * a14 + - a04 * a02 * a13 + - a12 * a01 * a06 + + a12 * a02 * a05 + + m03' = -a01 * a06 * a11 + + a01 * a07 * a10 + + a05 * a02 * a11 + - a05 * a03 * a10 + - a09 * a02 * a07 + + a09 * a03 * a06 + + m07' = a00 * a06 * a11 + - a00 * a07 * a10 + - a04 * a02 * a11 + + a04 * a03 * a10 + + a08 * a02 * a07 + - a08 * a03 * a06 + + m11' = -a00 * a05 * a11 + + a00 * a07 * a09 + + a04 * a01 * a11 + - a04 * a03 * a09 + - a08 * a01 * a07 + + a08 * a03 * a05 + + m15' = a00 * a05 * a10 + - a00 * a06 * a09 + - a04 * a01 * a10 + + a04 * a02 * a09 + + a08 * a01 * a06 + - a08 * a02 * a05 + + det' = a00 * m00' + a01 * m04' + a02 * m08' + a03 * m12' + in + if det' == 0 then Spear.Math.Matrix4.id + else + let det = 1 / det' + in mat4 + (m00' * det) (m04' * det) (m08' * det) (m12' * det) + (m01' * det) (m05' * det) (m09' * det) (m13' * det) + (m02' * det) (m06' * det) (m10' * det) (m14' * det) + (m03' * det) (m07' * det) (m11' * det) (m15' * det) -- | Transform the given vector in 3D space with the given matrix. -- cgit v1.2.3