From a799cdca2f854b8d8d3634abeda75941ca3a4685 Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Tue, 31 May 2005 06:21:16 +0000 Subject: [PATCH] From Tim Daust, "I fixed the getScale functions in matrixf and matrixd. It was returning the values of the diagonal of the matrix, which only returns the scale if there is not a rotation. I fixed this by returning the length of the vectors that form the basis. I also added a function to orthonormalize the rotation component of the matrix. I seem to always run into situations where non uniform (or even uniform) scale complicate my calculations, and I thought other members of the community could use this function as well." --- include/osg/Matrixd | 19 +++++++++- include/osg/Matrixf | 10 ++++- src/osg/Matrix_implementation.cpp | 61 ++++++++++++++++++++++++++++++- 3 files changed, 86 insertions(+), 4 deletions(-) diff --git a/include/osg/Matrixd b/include/osg/Matrixd index a2e132ccb..2f2519366 100644 --- a/include/osg/Matrixd +++ b/include/osg/Matrixd @@ -206,6 +206,9 @@ class OSG_EXPORT Matrixd /** full 4x4 matrix invert. */ bool invert_4x4_new( const Matrixd& ); + /** ortho-normalize the 3x3 rotation & scale matrix */ + void orthoNormalize(const Matrixd& rhs); + // basic utility functions to create new matrices inline static Matrixd identity( void ); inline static Matrixd scale( const Vec3f& sv); @@ -227,7 +230,7 @@ class OSG_EXPORT Matrixd value_type angle3, const Vec3d& axis3); inline static Matrixd rotate( const Quat& quat); inline static Matrixd inverse( const Matrixd& matrix); - + inline static Matrixd orthoNormal(const Matrixd& matrix); /** Create an orthographic projection matrix. * See glOrtho for further details. */ @@ -288,7 +291,12 @@ class OSG_EXPORT Matrixd inline Vec3d getTrans() const { return Vec3d(_mat[3][0],_mat[3][1],_mat[3][2]); } - inline Vec3d getScale() const { return Vec3d(_mat[0][0],_mat[1][1],_mat[2][2]); } + inline Vec3d getScale() const { + Vec3d x_vec(_mat[0][0],_mat[1][0],_mat[2][0]); + Vec3d y_vec(_mat[0][1],_mat[1][1],_mat[2][1]); + Vec3d z_vec(_mat[0][2],_mat[1][2],_mat[2][2]); + return Vec3d(x_vec.length(), y_vec.length(), z_vec.length()); + } /** apply a 3x3 transform of v*M[0..2,0..2]. */ inline static Vec3f transform3x3(const Vec3f& v,const Matrixd& m); @@ -458,6 +466,13 @@ inline Matrixd Matrixd::inverse( const Matrixd& matrix) return m; } +inline Matrixd orthoNormal(const Matrixd& matrix) +{ + Matrixd m; + m.orthoNormalize(matrix); + return m; +} + inline Matrixd Matrixd::ortho(double left, double right, double bottom, double top, double zNear, double zFar) diff --git a/include/osg/Matrixf b/include/osg/Matrixf index 78ecd3159..923d6d17d 100644 --- a/include/osg/Matrixf +++ b/include/osg/Matrixf @@ -206,6 +206,9 @@ class OSG_EXPORT Matrixf /** full 4x4 matrix invert. */ bool invert_4x4_new( const Matrixf& ); + /** ortho-normalize the 3x3 rotation & scale matrix */ + void orthoNormalize(const Matrixf& rhs); + //basic utility functions to create new matrices inline static Matrixf identity( void ); inline static Matrixf scale( const Vec3f& sv); @@ -288,7 +291,12 @@ class OSG_EXPORT Matrixf inline Vec3d getTrans() const { return Vec3d(_mat[3][0],_mat[3][1],_mat[3][2]); } - inline Vec3d getScale() const { return Vec3d(_mat[0][0],_mat[1][1],_mat[2][2]); } + inline Vec3d getScale() const { + Vec3d x_vec(_mat[0][0],_mat[1][0],_mat[2][0]); + Vec3d y_vec(_mat[0][1],_mat[1][1],_mat[2][1]); + Vec3d z_vec(_mat[0][2],_mat[1][2],_mat[2][2]); + return Vec3d(x_vec.length(), y_vec.length(), z_vec.length()); + } /** apply a 3x3 transform of v*M[0..2,0..2]. */ inline static Vec3f transform3x3(const Vec3f& v,const Matrixf& m); diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp index 127a3df98..a2cb54508 100644 --- a/src/osg/Matrix_implementation.cpp +++ b/src/osg/Matrix_implementation.cpp @@ -66,7 +66,7 @@ void Matrix_implementation::set(const Quat& q_in) { Quat q(q_in); double length2 = q.length2(); - if (length2!=1.0 && length2!=0) + if (length2!=1.0 && length2!=0) { // normalize quat if required. q /= sqrt(length2); @@ -386,6 +386,65 @@ void Matrix_implementation::postMult( const Matrix_implementation& other ) #undef INNER_PRODUCT +// orthoNormalize the 3x3 rotation matrix +void Matrix_implementation::orthoNormalize(const Matrix_implementation& rhs) +{ + value_type x_colMag = (rhs._mat[0][0] * rhs._mat[0][0]) + (rhs._mat[1][0] * rhs._mat[1][0]) + (rhs._mat[2][0] * rhs._mat[2][0]); + value_type y_colMag = (rhs._mat[0][1] * rhs._mat[0][1]) + (rhs._mat[1][1] * rhs._mat[1][1]) + (rhs._mat[2][1] * rhs._mat[2][1]); + value_type z_colMag = (rhs._mat[0][2] * rhs._mat[0][2]) + (rhs._mat[1][2] * rhs._mat[1][2]) + (rhs._mat[2][2] * rhs._mat[2][2]); + + if(!equivalent((double)x_colMag, 1.0) && !equivalent((double)x_colMag, 0.0)) + { + x_colMag = sqrt(x_colMag); + _mat[0][0] = rhs._mat[0][0] / x_colMag; + _mat[1][0] = rhs._mat[1][0] / x_colMag; + _mat[2][0] = rhs._mat[2][0] / x_colMag; + } + else + { + _mat[0][0] = rhs._mat[0][0]; + _mat[1][0] = rhs._mat[1][0]; + _mat[2][0] = rhs._mat[2][0]; + } + + if(!equivalent((double)y_colMag, 1.0) && !equivalent((double)y_colMag, 0.0)) + { + y_colMag = sqrt(y_colMag); + _mat[0][1] = rhs._mat[0][1] / y_colMag; + _mat[1][1] = rhs._mat[1][1] / y_colMag; + _mat[2][1] = rhs._mat[2][1] / y_colMag; + } + else + { + _mat[0][1] = rhs._mat[0][1]; + _mat[1][1] = rhs._mat[1][1]; + _mat[2][1] = rhs._mat[2][1]; + } + + if(!equivalent((double)z_colMag, 1.0) && !equivalent((double)z_colMag, 0.0)) + { + z_colMag = sqrt(z_colMag); + _mat[0][2] = rhs._mat[0][2] / z_colMag; + _mat[1][2] = rhs._mat[1][2] / z_colMag; + _mat[2][2] = rhs._mat[2][2] / z_colMag; + } + else + { + _mat[0][2] = rhs._mat[0][2]; + _mat[1][2] = rhs._mat[1][2]; + _mat[2][2] = rhs._mat[2][2]; + } + + _mat[3][0] = rhs._mat[3][0]; + _mat[3][1] = rhs._mat[3][1]; + _mat[3][2] = rhs._mat[3][2]; + + _mat[0][3] = rhs._mat[0][3]; + _mat[1][3] = rhs._mat[1][3]; + _mat[2][3] = rhs._mat[2][3]; + _mat[3][3] = rhs._mat[3][3]; + +} bool Matrix_implementation::invert( const Matrix_implementation& rhs) {