@@ -638,5 +638,173 @@ public Matrix4x4 ToMatrix4x4()
638638 ( float ) M31 , ( float ) M32 , ( float ) M33 , ( float ) M34 ,
639639 ( float ) M41 , ( float ) M42 , ( float ) M43 , ( float ) M44 ) ;
640640 }
641+
642+ /// <summary>
643+ /// Attempts to calculate the inverse of the given matrix. If successful, result will contain the inverted matrix.
644+ /// </summary>
645+ /// <param name="matrix">The source matrix to invert.</param>
646+ /// <param name="result">If successful, contains the inverted matrix.</param>
647+ /// <returns>True if the source matrix could be inverted; False otherwise.</returns>
648+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
649+ public static bool Invert ( DMatrix4x4 matrix , out DMatrix4x4 result )
650+ {
651+ // -1
652+ // If you have matrix M, inverse Matrix M can compute
653+ //
654+ // -1 1
655+ // M = --------- A
656+ // det(M)
657+ //
658+ // A is adjugate (adjoint) of M, where,
659+ //
660+ // T
661+ // A = C
662+ //
663+ // C is Cofactor matrix of M, where,
664+ // i + j
665+ // C = (-1) * det(M )
666+ // ij ij
667+ //
668+ // [ a b c d ]
669+ // M = [ e f g h ]
670+ // [ i j k l ]
671+ // [ m n o p ]
672+ //
673+ // First Row
674+ // 2 | f g h |
675+ // C = (-1) | j k l | = + ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
676+ // 11 | n o p |
677+ //
678+ // 3 | e g h |
679+ // C = (-1) | i k l | = - ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
680+ // 12 | m o p |
681+ //
682+ // 4 | e f h |
683+ // C = (-1) | i j l | = + ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
684+ // 13 | m n p |
685+ //
686+ // 5 | e f g |
687+ // C = (-1) | i j k | = - ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
688+ // 14 | m n o |
689+ //
690+ // Second Row
691+ // 3 | b c d |
692+ // C = (-1) | j k l | = - ( b ( kp - lo ) - c ( jp - ln ) + d ( jo - kn ) )
693+ // 21 | n o p |
694+ //
695+ // 4 | a c d |
696+ // C = (-1) | i k l | = + ( a ( kp - lo ) - c ( ip - lm ) + d ( io - km ) )
697+ // 22 | m o p |
698+ //
699+ // 5 | a b d |
700+ // C = (-1) | i j l | = - ( a ( jp - ln ) - b ( ip - lm ) + d ( in - jm ) )
701+ // 23 | m n p |
702+ //
703+ // 6 | a b c |
704+ // C = (-1) | i j k | = + ( a ( jo - kn ) - b ( io - km ) + c ( in - jm ) )
705+ // 24 | m n o |
706+ //
707+ // Third Row
708+ // 4 | b c d |
709+ // C = (-1) | f g h | = + ( b ( gp - ho ) - c ( fp - hn ) + d ( fo - gn ) )
710+ // 31 | n o p |
711+ //
712+ // 5 | a c d |
713+ // C = (-1) | e g h | = - ( a ( gp - ho ) - c ( ep - hm ) + d ( eo - gm ) )
714+ // 32 | m o p |
715+ //
716+ // 6 | a b d |
717+ // C = (-1) | e f h | = + ( a ( fp - hn ) - b ( ep - hm ) + d ( en - fm ) )
718+ // 33 | m n p |
719+ //
720+ // 7 | a b c |
721+ // C = (-1) | e f g | = - ( a ( fo - gn ) - b ( eo - gm ) + c ( en - fm ) )
722+ // 34 | m n o |
723+ //
724+ // Fourth Row
725+ // 5 | b c d |
726+ // C = (-1) | f g h | = - ( b ( gl - hk ) - c ( fl - hj ) + d ( fk - gj ) )
727+ // 41 | j k l |
728+ //
729+ // 6 | a c d |
730+ // C = (-1) | e g h | = + ( a ( gl - hk ) - c ( el - hi ) + d ( ek - gi ) )
731+ // 42 | i k l |
732+ //
733+ // 7 | a b d |
734+ // C = (-1) | e f h | = - ( a ( fl - hj ) - b ( el - hi ) + d ( ej - fi ) )
735+ // 43 | i j l |
736+ //
737+ // 8 | a b c |
738+ // C = (-1) | e f g | = + ( a ( fk - gj ) - b ( ek - gi ) + c ( ej - fi ) )
739+ // 44 | i j k |
740+ //
741+ // Cost of operation
742+ // 53 adds, 104 muls, and 1 div.
743+ double a = matrix . M11 , b = matrix . M12 , c = matrix . M13 , d = matrix . M14 ;
744+ double e = matrix . M21 , f = matrix . M22 , g = matrix . M23 , h = matrix . M24 ;
745+ double i = matrix . M31 , j = matrix . M32 , k = matrix . M33 , l = matrix . M34 ;
746+ double m = matrix . M41 , n = matrix . M42 , o = matrix . M43 , p = matrix . M44 ;
747+
748+ var kp_lo = k * p - l * o ;
749+ var jp_ln = j * p - l * n ;
750+ var jo_kn = j * o - k * n ;
751+ var ip_lm = i * p - l * m ;
752+ var io_km = i * o - k * m ;
753+ var in_jm = i * n - j * m ;
754+
755+ var a11 = + ( f * kp_lo - g * jp_ln + h * jo_kn ) ;
756+ var a12 = - ( e * kp_lo - g * ip_lm + h * io_km ) ;
757+ var a13 = + ( e * jp_ln - f * ip_lm + h * in_jm ) ;
758+ var a14 = - ( e * jo_kn - f * io_km + g * in_jm ) ;
759+
760+ var det = a * a11 + b * a12 + c * a13 + d * a14 ;
761+
762+ if ( det . Abs ( ) < float . Epsilon )
763+ {
764+ result = new DMatrix4x4 ( float . NaN , float . NaN , float . NaN , float . NaN ,
765+ float . NaN , float . NaN , float . NaN , float . NaN ,
766+ float . NaN , float . NaN , float . NaN , float . NaN ,
767+ float . NaN , float . NaN , float . NaN , float . NaN ) ;
768+ return false ;
769+ }
770+
771+ var invDet = 1.0f / det ;
772+
773+ result . M11 = a11 * invDet ;
774+ result . M21 = a12 * invDet ;
775+ result . M31 = a13 * invDet ;
776+ result . M41 = a14 * invDet ;
777+
778+ result . M12 = - ( b * kp_lo - c * jp_ln + d * jo_kn ) * invDet ;
779+ result . M22 = + ( a * kp_lo - c * ip_lm + d * io_km ) * invDet ;
780+ result . M32 = - ( a * jp_ln - b * ip_lm + d * in_jm ) * invDet ;
781+ result . M42 = + ( a * jo_kn - b * io_km + c * in_jm ) * invDet ;
782+
783+ var gp_ho = g * p - h * o ;
784+ var fp_hn = f * p - h * n ;
785+ var fo_gn = f * o - g * n ;
786+ var ep_hm = e * p - h * m ;
787+ var eo_gm = e * o - g * m ;
788+ var en_fm = e * n - f * m ;
789+
790+ result . M13 = + ( b * gp_ho - c * fp_hn + d * fo_gn ) * invDet ;
791+ result . M23 = - ( a * gp_ho - c * ep_hm + d * eo_gm ) * invDet ;
792+ result . M33 = + ( a * fp_hn - b * ep_hm + d * en_fm ) * invDet ;
793+ result . M43 = - ( a * fo_gn - b * eo_gm + c * en_fm ) * invDet ;
794+
795+ var gl_hk = g * l - h * k ;
796+ var fl_hj = f * l - h * j ;
797+ var fk_gj = f * k - g * j ;
798+ var el_hi = e * l - h * i ;
799+ var ek_gi = e * k - g * i ;
800+ var ej_fi = e * j - f * i ;
801+
802+ result . M14 = - ( b * gl_hk - c * fl_hj + d * fk_gj ) * invDet ;
803+ result . M24 = + ( a * gl_hk - c * el_hi + d * ek_gi ) * invDet ;
804+ result . M34 = - ( a * fl_hj - b * el_hi + d * ej_fi ) * invDet ;
805+ result . M44 = + ( a * fk_gj - b * ek_gi + c * ej_fi ) * invDet ;
806+
807+ return true ;
808+ }
641809 }
642810}
0 commit comments