From 3b03699fbcd264acd0b82eb513b508287b468160 Mon Sep 17 00:00:00 2001
From: Julien Valentin <pascal@pcpascal.WORKGROUP>
Date: Wed, 23 Aug 2017 19:06:25 +0200
Subject: [PATCH] add a transpose method for 4x4 and a transpose3x3 to
 transpose only orthogonal part of a mat4x4

---
 include/osg/Matrixd               |  6 +++++
 include/osg/Matrixf               |  6 +++++
 src/osg/Matrix_implementation.cpp | 45 +++++++++++++++++++++++++++----
 3 files changed, 52 insertions(+), 5 deletions(-)

diff --git a/include/osg/Matrixd b/include/osg/Matrixd
index 199356255..318fbc775 100644
--- a/include/osg/Matrixd
+++ b/include/osg/Matrixd
@@ -242,6 +242,12 @@ class OSG_EXPORT Matrixd
         /** full 4x4 matrix invert. */
         bool invert_4x4( const Matrixd& rhs);
 
+        /** transpose a matrix */
+        bool transpose(const Matrixd&rhs);
+
+        /** transpose orthogonal part of the matrix **/
+        bool transpose3x3(const Matrixd&rhs);
+
         /** ortho-normalize the 3x3 rotation & scale matrix */
         void orthoNormalize(const Matrixd& rhs);
 
diff --git a/include/osg/Matrixf b/include/osg/Matrixf
index 0d60501f8..845c8a3a0 100644
--- a/include/osg/Matrixf
+++ b/include/osg/Matrixf
@@ -242,6 +242,12 @@ class OSG_EXPORT Matrixf
         /** full 4x4 matrix invert. */
         bool invert_4x4( const Matrixf& rhs);
 
+        /** transpose matrix **/
+        bool transpose(const Matrixf&rhs);
+
+        /** transpose orthogonal part of the matrix **/
+        bool transpose3x3(const Matrixf&rhs);
+
         /** ortho-normalize the 3x3 rotation & scale matrix */
         void orthoNormalize(const Matrixf& rhs);
 
diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp
index d06638056..14ad61e92 100644
--- a/src/osg/Matrix_implementation.cpp
+++ b/src/osg/Matrix_implementation.cpp
@@ -742,6 +742,41 @@ inline T SGL_ABS(T a)
 #define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
 #endif
 
+bool Matrix_implementation::transpose(const Matrix_implementation&mat){
+    if (&mat==this) {
+       Matrix_implementation tm(mat);
+       return transpose(tm);
+    }
+    _mat[0][1]=mat._mat[1][0];
+    _mat[0][2]=mat._mat[2][0];
+    _mat[0][3]=mat._mat[3][0];
+    _mat[1][0]=mat._mat[0][1];
+    _mat[1][2]=mat._mat[2][1];
+    _mat[1][3]=mat._mat[3][1];
+    _mat[2][0]=mat._mat[0][2];
+    _mat[2][1]=mat._mat[1][2];
+    _mat[2][3]=mat._mat[3][2];
+    _mat[3][0]=mat._mat[0][3];
+    _mat[3][1]=mat._mat[1][3];
+    _mat[3][2]=mat._mat[2][3];
+    return true;
+}
+
+bool Matrix_implementation::transpose3x3(const Matrix_implementation&mat){
+    if (&mat==this) {
+       Matrix_implementation tm(mat);
+       return transpose3x3(tm);
+    }
+    _mat[0][1]=mat._mat[1][0];
+    _mat[0][2]=mat._mat[2][0];
+    _mat[1][0]=mat._mat[0][1];
+    _mat[1][2]=mat._mat[2][1];
+    _mat[2][0]=mat._mat[0][2];
+    _mat[2][1]=mat._mat[1][2];
+
+    return true;
+}
+
 bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat )
 {
     if (&mat==this) {
@@ -763,7 +798,7 @@ bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat )
     for(i=0;i<4;i++)
     {
        big=0.0;
-       for (j=0; j<4; j++)
+       for (j=0; j<4; ++j)
           if (ipiv[j] != 1)
              for (k=0; k<4; k++)
              {
@@ -781,7 +816,7 @@ bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat )
              }
        ++(ipiv[icol]);
        if (irow != icol)
-          for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
+          for (l=0; l<4; ++l) SGL_SWAP(operator()(irow,l),
                                        operator()(icol,l),
                                        temp);
 
@@ -792,13 +827,13 @@ bool Matrix_implementation::invert_4x4( const Matrix_implementation& mat )
 
        pivinv = 1.0/operator()(icol,icol);
        operator()(icol,icol) = 1;
-       for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
-       for (ll=0; ll<4; ll++)
+       for (l=0; l<4; ++l) operator()(icol,l) *= pivinv;
+       for (ll=0; ll<4; ++ll)
           if (ll != icol)
           {
              dum=operator()(ll,icol);
              operator()(ll,icol) = 0;
-             for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
+             for (l=0; l<4; ++l) operator()(ll,l) -= operator()(icol,l)*dum;
           }
     }
     for (int lx=4; lx>0; --lx)