@@ -664,104 +664,110 @@ Matrix4 Matrix4::perspectiveMatrix(float yfov, float aspect, float near, float f
664664}
665665
666666void Matrix4::decomposeStandard (Vector3& position, Vector3& scale, Quaternion& rotation) const {
667- // Extract position
667+ // Extract translation (assuming column-major, last column is translation)
668668 position.x = matrix[3 ][0 ];
669669 position.y = matrix[3 ][1 ];
670670 position.z = matrix[3 ][2 ];
671671
672- // Extract scale (column lengths )
673- scale. x = Vector3 (matrix[0 ][0 ], matrix[0 ][1 ], matrix[0 ][2 ]). length ( );
674- scale. y = Vector3 (matrix[1 ][0 ], matrix[1 ][1 ], matrix[1 ][2 ]). length ( );
675- scale. z = Vector3 (matrix[2 ][0 ], matrix[2 ][1 ], matrix[2 ][2 ]). length ( );
672+ // Extract basis vectors (upper-left 3x3 matrix )
673+ Vector3 col0 (matrix[0 ][0 ], matrix[0 ][1 ], matrix[0 ][2 ]);
674+ Vector3 col1 (matrix[1 ][0 ], matrix[1 ][1 ], matrix[1 ][2 ]);
675+ Vector3 col2 (matrix[2 ][0 ], matrix[2 ][1 ], matrix[2 ][2 ]);
676676
677- // Handle negative determinant (reflection )
678- if ( determinant () < 0 ) scale = -scale ;
679- // Create rotation matrix - MODIFIED to handle zero scales
680- Matrix4 rotationM ;
677+ // Extract scale (length of basis vectors )
678+ scale. x = col0. length () ;
679+ scale. y = col1. length ();
680+ scale. z = col2. length () ;
681681
682- // Check for near-zero scales to avoid division by zero
683682 const float EPSILON = 1e-6f ;
684683
685- // Initialize rotation matrix with identity
686- rotationM.identity ();
687-
688- // For each column with non-zero scale, normalize and copy to rotation matrix
689- if (std::abs (scale.x ) > EPSILON ) {
690- rotationM.set (0 , 0 , matrix[0 ][0 ] / scale.x );
691- rotationM.set (0 , 1 , matrix[0 ][1 ] / scale.x );
692- rotationM.set (0 , 2 , matrix[0 ][2 ] / scale.x );
693- }
694-
695- if (std::abs (scale.y ) > EPSILON ) {
696- rotationM.set (1 , 0 , matrix[1 ][0 ] / scale.y );
697- rotationM.set (1 , 1 , matrix[1 ][1 ] / scale.y );
698- rotationM.set (1 , 2 , matrix[1 ][2 ] / scale.y );
684+ // If all scales are degenerate, just return identity rotation and unit scale
685+ if (scale.x < EPSILON && scale.y < EPSILON && scale.z < EPSILON ) {
686+ scale = Vector3 (1 .0f , 1 .0f , 1 .0f );
687+ rotation = Quaternion (); // identity
688+ return ;
699689 }
700690
701- if ( std::abs (scale. z ) > EPSILON ) {
702- rotationM. set ( 2 , 0 , matrix[ 2 ][ 0 ] / scale. z );
703- rotationM. set ( 2 , 1 , matrix[ 2 ][ 1 ] / scale.z ) ;
704- rotationM. set ( 2 , 2 , matrix[ 2 ][ 2 ] / scale. z ) ;
691+ // Handle negative determinant (reflection): flip one axis only
692+ if ( determinant () < 0 ) {
693+ scale. x = - scale.x ;
694+ col0 = -col0 ;
705695 }
706696
707- // Special case: if we have degenerate scaling, ensure we still return a valid rotation
708- if (std::abs (scale.x ) <= EPSILON || std::abs (scale.y ) <= EPSILON || std::abs (scale.z ) <= EPSILON ) {
709- // If any scale is zero, we need to rebuild a valid rotation matrix
710- // This approach constructs an orthonormal basis where possible
711- Vector3 xAxis (rotationM.get (0 , 0 ), rotationM.get (0 , 1 ), rotationM.get (0 , 2 ));
712- Vector3 yAxis (rotationM.get (1 , 0 ), rotationM.get (1 , 1 ), rotationM.get (1 , 2 ));
713- Vector3 zAxis (rotationM.get (2 , 0 ), rotationM.get (2 , 1 ), rotationM.get (2 , 2 ));
714-
715- // Find the first non-zero axis
697+ // Clamp scale to avoid division by zero
698+ if (std::abs (scale.x ) < EPSILON ) scale.x = 1 .0f ;
699+ if (std::abs (scale.y ) < EPSILON ) scale.y = 1 .0f ;
700+ if (std::abs (scale.z ) < EPSILON ) scale.z = 1 .0f ;
701+
702+ // Normalize columns to get rotation axes
703+ Vector3 xAxis = col0 / scale.x ;
704+ Vector3 yAxis = col1 / scale.y ;
705+ Vector3 zAxis = col2 / scale.z ;
706+
707+ // Validate orthogonality
708+ // If not, reconstruct
709+ float dot01 = std::abs (xAxis.dotProduct (yAxis));
710+ float dot02 = std::abs (xAxis.dotProduct (zAxis));
711+ float dot12 = std::abs (yAxis.dotProduct (zAxis));
712+ if (dot01 > 0 .01f || dot02 > 0 .01f || dot12 > 0 .01f ) {
713+ // Orthonormalize using Gram-Schmidt process
716714 if (xAxis.length () > EPSILON ) {
717715 xAxis = xAxis.normalize ();
716+ } else {
717+ xAxis = Vector3 (1 , 0 , 0 );
718+ }
718719
719- // Find or create a valid y-axis
720- if (yAxis.length () > EPSILON ) {
721- yAxis = yAxis.normalize ();
720+ // Make yAxis orthogonal to xAxis
721+ yAxis = yAxis - xAxis * yAxis.dotProduct (xAxis);
722+ if (yAxis.length () > EPSILON ) {
723+ yAxis = yAxis.normalize ();
724+ } else {
725+ // Safe perpendicular vector generation
726+ Vector3 perp = xAxis.perpendicular ();
727+ if (perp.length () > EPSILON ) {
728+ yAxis = perp.normalize ();
722729 } else {
723- // Create orthogonal vector
724- yAxis = xAxis.perpendicular ().normalize ();
730+ // Fallback: choose an axis that's not parallel to xAxis
731+ if (std::abs (xAxis.x ) < 0 .9f ) {
732+ yAxis = Vector3 (1 , 0 , 0 );
733+ } else {
734+ yAxis = Vector3 (0 , 1 , 0 );
735+ }
736+
737+ // Make it orthogonal to xAxis
738+ yAxis = yAxis - xAxis * yAxis.dotProduct (xAxis);
739+ if (yAxis.length () > EPSILON ) {
740+ yAxis = yAxis.normalize ();
741+ } else {
742+ yAxis = Vector3 (0 , 1 , 0 ); // Ultimate fallback
743+ }
725744 }
726-
727- // Ensure z-axis is orthogonal to both
728- zAxis = xAxis.crossProduct (yAxis).normalize ();
729-
730- // Ensure y-axis is truly orthogonal (eliminate any drift)
731- yAxis = zAxis.crossProduct (xAxis).normalize ();
732745 }
733- else if (yAxis.length () > EPSILON ) {
734- yAxis = yAxis.normalize ();
735-
736- // Create orthogonal vector
737- zAxis = yAxis.perpendicular ().normalize ();
738746
739- // Complete the basis
740- xAxis = yAxis.crossProduct (zAxis).normalize ();
741- }
742- else if (zAxis.length () > EPSILON ) {
747+ // Calculate zAxis as cross product
748+ zAxis = xAxis.crossProduct (yAxis);
749+ if (zAxis.length () > EPSILON ) {
743750 zAxis = zAxis.normalize ();
744-
745- // Create orthogonal vector
746- xAxis = zAxis.perpendicular ().normalize ();
747-
748- // Complete the basis
749- yAxis = zAxis.crossProduct (xAxis).normalize ();
750- }
751- else {
752- // All axes are degenerate, just use identity rotation
753- rotation = Quaternion ();
754- return ;
751+ } else {
752+ zAxis = Vector3 (0 , 0 , 1 ); // Fallback
755753 }
756-
757- // Set the rotation matrix with our orthonormal basis
758- rotationM.set (0 , 0 , xAxis.x ); rotationM.set (0 , 1 , xAxis.y ); rotationM.set (0 , 2 , xAxis.z );
759- rotationM.set (1 , 0 , yAxis.x ); rotationM.set (1 , 1 , yAxis.y ); rotationM.set (1 , 2 , yAxis.z );
760- rotationM.set (2 , 0 , zAxis.x ); rotationM.set (2 , 1 , zAxis.y ); rotationM.set (2 , 2 , zAxis.z );
761754 }
762755
763- // Convert to quaternion and normalize
764- rotation = Quaternion ().fromRotationMatrix (rotationM).normalize ();
756+ // Build rotation matrix (column-major)
757+ Matrix4 rotM;
758+ rotM.identity ();
759+ rotM.set (0 , 0 , xAxis.x ); rotM.set (0 , 1 , xAxis.y ); rotM.set (0 , 2 , xAxis.z );
760+ rotM.set (1 , 0 , yAxis.x ); rotM.set (1 , 1 , yAxis.y ); rotM.set (1 , 2 , yAxis.z );
761+ rotM.set (2 , 0 , zAxis.x ); rotM.set (2 , 1 , zAxis.y ); rotM.set (2 , 2 , zAxis.z );
762+
763+ // Convert to quaternion
764+ rotation = Quaternion ().fromRotationMatrix (rotM);
765+ float qNorm = rotation.norm ();
766+ if (qNorm < EPSILON * EPSILON ) {
767+ rotation = Quaternion (); // identity fallback
768+ } else {
769+ rotation = rotation.normalize ();
770+ }
765771}
766772
767773void Matrix4::decomposeQDU (Vector3& position, Vector3& scale, Quaternion& rotation) const {
0 commit comments