Merge pull request #597 from ryanelandt/faster_inv_4x4

Faster inv 4x4

Author: c42f

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