diff --git a/src/inv.jl b/src/inv.jl index b67df549..673230fc 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