From 92763c4d23e2c01c1967160556071e3415d2ac29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CRyan?= <“ryan.elandt@gmail.com”> Date: Mon, 18 Mar 2019 13:01:36 -0400 Subject: [PATCH 1/2] added faster inverse for 4x4 matrices --- src/inv.jl | 52 +++++++++++++++++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 19 deletions(-) diff --git a/src/inv.jl b/src/inv.jl index b67df549..0ec03aec 100644 --- a/src/inv.jl +++ b/src/inv.jl @@ -31,28 +31,42 @@ end end @inline function _inv(::Size{(4,4)}, A) - idet = 1/det(A) - B = @SMatrix [ - (A[2,3]*A[3,4]*A[4,2] - A[2,4]*A[3,3]*A[4,2] + A[2,4]*A[3,2]*A[4,3] - A[2,2]*A[3,4]*A[4,3] - A[2,3]*A[3,2]*A[4,4] + A[2,2]*A[3,3]*A[4,4]) * idet - (A[2,4]*A[3,3]*A[4,1] - A[2,3]*A[3,4]*A[4,1] - A[2,4]*A[3,1]*A[4,3] + A[2,1]*A[3,4]*A[4,3] + A[2,3]*A[3,1]*A[4,4] - A[2,1]*A[3,3]*A[4,4]) * idet - (A[2,2]*A[3,4]*A[4,1] - A[2,4]*A[3,2]*A[4,1] + A[2,4]*A[3,1]*A[4,2] - A[2,1]*A[3,4]*A[4,2] - A[2,2]*A[3,1]*A[4,4] + A[2,1]*A[3,2]*A[4,4]) * idet - (A[2,3]*A[3,2]*A[4,1] - A[2,2]*A[3,3]*A[4,1] - A[2,3]*A[3,1]*A[4,2] + A[2,1]*A[3,3]*A[4,2] + A[2,2]*A[3,1]*A[4,3] - A[2,1]*A[3,2]*A[4,3]) * idet + # https://www.geometrictools.com/Documentation/LaplaceExpansionTheorem.pdf - (A[1,4]*A[3,3]*A[4,2] - A[1,3]*A[3,4]*A[4,2] - A[1,4]*A[3,2]*A[4,3] + A[1,2]*A[3,4]*A[4,3] + A[1,3]*A[3,2]*A[4,4] - A[1,2]*A[3,3]*A[4,4]) * idet - (A[1,3]*A[3,4]*A[4,1] - A[1,4]*A[3,3]*A[4,1] + A[1,4]*A[3,1]*A[4,3] - A[1,1]*A[3,4]*A[4,3] - A[1,3]*A[3,1]*A[4,4] + A[1,1]*A[3,3]*A[4,4]) * idet - (A[1,4]*A[3,2]*A[4,1] - A[1,2]*A[3,4]*A[4,1] - A[1,4]*A[3,1]*A[4,2] + A[1,1]*A[3,4]*A[4,2] + A[1,2]*A[3,1]*A[4,4] - A[1,1]*A[3,2]*A[4,4]) * idet - (A[1,2]*A[3,3]*A[4,1] - A[1,3]*A[3,2]*A[4,1] + A[1,3]*A[3,1]*A[4,2] - A[1,1]*A[3,3]*A[4,2] - A[1,2]*A[3,1]*A[4,3] + A[1,1]*A[3,2]*A[4,3]) * idet + s0 = A[1] * A[6] - A[2] * A[5] + s1 = A[1] * A[10] - A[2] * A[9] + s2 = A[1] * A[14] - A[2] * A[13] + s3 = A[5] * A[10] - A[6] * A[9] + s4 = A[5] * A[14] - A[6] * A[13] + s5 = A[9] * A[14] - A[10] * A[13] - (A[1,3]*A[2,4]*A[4,2] - A[1,4]*A[2,3]*A[4,2] + A[1,4]*A[2,2]*A[4,3] - A[1,2]*A[2,4]*A[4,3] - A[1,3]*A[2,2]*A[4,4] + A[1,2]*A[2,3]*A[4,4]) * idet - (A[1,4]*A[2,3]*A[4,1] - A[1,3]*A[2,4]*A[4,1] - A[1,4]*A[2,1]*A[4,3] + A[1,1]*A[2,4]*A[4,3] + A[1,3]*A[2,1]*A[4,4] - A[1,1]*A[2,3]*A[4,4]) * idet - (A[1,2]*A[2,4]*A[4,1] - A[1,4]*A[2,2]*A[4,1] + A[1,4]*A[2,1]*A[4,2] - A[1,1]*A[2,4]*A[4,2] - A[1,2]*A[2,1]*A[4,4] + A[1,1]*A[2,2]*A[4,4]) * idet - (A[1,3]*A[2,2]*A[4,1] - A[1,2]*A[2,3]*A[4,1] - A[1,3]*A[2,1]*A[4,2] + A[1,1]*A[2,3]*A[4,2] + A[1,2]*A[2,1]*A[4,3] - A[1,1]*A[2,2]*A[4,3]) * idet + c5 = A[11] * A[16] - A[12] * A[15] + c4 = A[7] * A[16] - A[8] * A[15] + c3 = A[7] * A[12] - A[8] * A[11] + c2 = A[3] * A[16] - A[4] * A[15] + c1 = A[3] * A[12] - A[4] * A[11] + c0 = A[3] * A[8] - A[4] * A[7] - (A[1,4]*A[2,3]*A[3,2] - A[1,3]*A[2,4]*A[3,2] - A[1,4]*A[2,2]*A[3,3] + A[1,2]*A[2,4]*A[3,3] + A[1,3]*A[2,2]*A[3,4] - A[1,2]*A[2,3]*A[3,4]) * idet - (A[1,3]*A[2,4]*A[3,1] - A[1,4]*A[2,3]*A[3,1] + A[1,4]*A[2,1]*A[3,3] - A[1,1]*A[2,4]*A[3,3] - A[1,3]*A[2,1]*A[3,4] + A[1,1]*A[2,3]*A[3,4]) * idet - (A[1,4]*A[2,2]*A[3,1] - A[1,2]*A[2,4]*A[3,1] - A[1,4]*A[2,1]*A[3,2] + A[1,1]*A[2,4]*A[3,2] + A[1,2]*A[2,1]*A[3,4] - A[1,1]*A[2,2]*A[3,4]) * idet - (A[1,2]*A[2,3]*A[3,1] - A[1,3]*A[2,2]*A[3,1] + A[1,3]*A[2,1]*A[3,2] - A[1,1]*A[2,3]*A[3,2] - A[1,2]*A[2,1]*A[3,3] + A[1,1]*A[2,2]*A[3,3]) * idet] - return similar_type(A)(B) + invdet = 1 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0) + + B = @SMatrix [ + ( A[6] * c5 - A[10] * c4 + A[14] * c3) * invdet + (-A[2] * c5 + A[10] * c2 - A[14] * c1) * invdet + ( A[2] * c4 - A[6] * c2 + A[14] * c0) * invdet + (-A[2] * c3 + A[6] * c1 - A[10] * c0) * invdet + (-A[5] * c5 + A[9] * c4 - A[13] * c3) * invdet + ( A[1] * c5 - A[9] * c2 + A[13] * c1) * invdet + (-A[1] * c4 + A[5] * c2 - A[13] * c0) * invdet + ( A[1] * c3 - A[5] * c1 + A[9] * c0) * invdet + ( A[8] * s5 - A[12] * s4 + A[16] * s3) * invdet + (-A[4] * s5 + A[12] * s2 - A[16] * s1) * invdet + ( A[4] * s4 - A[8] * s2 + A[16] * s0) * invdet + (-A[4] * s3 + A[8] * s1 - A[12] * s0) * invdet + (-A[7] * s5 + A[11] * s4 - A[15] * s3) * invdet + ( A[3] * s5 - A[11] * s2 + A[15] * s1) * invdet + (-A[3] * s4 + A[7] * s2 - A[15] * s0) * invdet + ( A[3] * s3 - A[7] * s1 + A[11] * s0) * invdet] + return similar_type(A)(B) end @generated function _inv(::Size{S}, A) where S From 4bbcf7bb8846b3033408439f7a5362af4f64642c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CRyan?= <“ryan.elandt@gmail.com”> Date: Mon, 18 Mar 2019 13:06:26 -0400 Subject: [PATCH 2/2] fixed spacing issues --- src/inv.jl | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/inv.jl b/src/inv.jl index 0ec03aec..673230fc 100644 --- a/src/inv.jl +++ b/src/inv.jl @@ -33,7 +33,7 @@ end @inline function _inv(::Size{(4,4)}, A) # https://www.geometrictools.com/Documentation/LaplaceExpansionTheorem.pdf - s0 = A[1] * A[6] - A[2] * A[5] + s0 = A[1] * A[6] - A[2] * A[5] s1 = A[1] * A[10] - A[2] * A[9] s2 = A[1] * A[14] - A[2] * A[13] s3 = A[5] * A[10] - A[6] * A[9] @@ -50,23 +50,23 @@ end invdet = 1 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0) B = @SMatrix [ - ( A[6] * c5 - A[10] * c4 + A[14] * c3) * invdet - (-A[2] * c5 + A[10] * c2 - A[14] * c1) * invdet - ( A[2] * c4 - A[6] * c2 + A[14] * c0) * invdet - (-A[2] * c3 + A[6] * c1 - A[10] * c0) * invdet - (-A[5] * c5 + A[9] * c4 - A[13] * c3) * invdet - ( A[1] * c5 - A[9] * c2 + A[13] * c1) * invdet - (-A[1] * c4 + A[5] * c2 - A[13] * c0) * invdet - ( A[1] * c3 - A[5] * c1 + A[9] * c0) * invdet - ( A[8] * s5 - A[12] * s4 + A[16] * s3) * invdet - (-A[4] * s5 + A[12] * s2 - A[16] * s1) * invdet - ( A[4] * s4 - A[8] * s2 + A[16] * s0) * invdet - (-A[4] * s3 + A[8] * s1 - A[12] * s0) * invdet - (-A[7] * s5 + A[11] * s4 - A[15] * s3) * invdet - ( A[3] * s5 - A[11] * s2 + A[15] * s1) * invdet - (-A[3] * s4 + A[7] * s2 - A[15] * s0) * invdet - ( A[3] * s3 - A[7] * s1 + A[11] * s0) * invdet] - return similar_type(A)(B) + ( A[6] * c5 - A[10] * c4 + A[14] * c3) * invdet + (-A[2] * c5 + A[10] * c2 - A[14] * c1) * invdet + ( A[2] * c4 - A[6] * c2 + A[14] * c0) * invdet + (-A[2] * c3 + A[6] * c1 - A[10] * c0) * invdet + (-A[5] * c5 + A[9] * c4 - A[13] * c3) * invdet + ( A[1] * c5 - A[9] * c2 + A[13] * c1) * invdet + (-A[1] * c4 + A[5] * c2 - A[13] * c0) * invdet + ( A[1] * c3 - A[5] * c1 + A[9] * c0) * invdet + ( A[8] * s5 - A[12] * s4 + A[16] * s3) * invdet + (-A[4] * s5 + A[12] * s2 - A[16] * s1) * invdet + ( A[4] * s4 - A[8] * s2 + A[16] * s0) * invdet + (-A[4] * s3 + A[8] * s1 - A[12] * s0) * invdet + (-A[7] * s5 + A[11] * s4 - A[15] * s3) * invdet + ( A[3] * s5 - A[11] * s2 + A[15] * s1) * invdet + (-A[3] * s4 + A[7] * s2 - A[15] * s0) * invdet + ( A[3] * s3 - A[7] * s1 + A[11] * s0) * invdet] + return similar_type(A)(B) end @generated function _inv(::Size{S}, A) where S