From 97d6564f51eeed4d4a81d17e95f8e0f48ae994ca Mon Sep 17 00:00:00 2001 From: jrheinlaender Date: Sat, 23 Mar 2013 20:20:16 +0430 Subject: [PATCH] Further suggestions for float -> double move --- src/App/PropertyUnits.cpp | 6 +- src/Base/Matrix.cpp | 171 +++++++++++++++++++++++++++++++++----- src/Base/Matrix.h | 7 +- src/Base/Persistence.h | 6 +- 4 files changed, 161 insertions(+), 29 deletions(-) diff --git a/src/App/PropertyUnits.cpp b/src/App/PropertyUnits.cpp index fae1df405..3a5926440 100644 --- a/src/App/PropertyUnits.cpp +++ b/src/App/PropertyUnits.cpp @@ -92,12 +92,12 @@ void PropertyLength::setPyObject(PyObject *value) setValue(UnitsApi::toDblWithUserPrefs(Length,value)); #else - float val=0.0f; + double val=0.0f; if (PyFloat_Check(value)) { - val = (float) PyFloat_AsDouble(value); + val = PyFloat_AsDouble(value); } else if(PyInt_Check(value)) { - val = (float) PyInt_AsLong(value); + val = (double) PyInt_AsLong(value); } else { std::string error = std::string("type must be float or int, not "); diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp index 18072b6b0..9656d3b3e 100644 --- a/src/Base/Matrix.cpp +++ b/src/Base/Matrix.cpp @@ -23,8 +23,8 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include -# include +# include +# include # include #endif @@ -449,6 +449,133 @@ bool Matrix4D::toAxisAngle (Vector3f& rclBase, Vector3f& rclDir, float& rfAngle, return true; } +bool Matrix4D::toAxisAngle (Vector3d& rclBase, Vector3d& rclDir, double& rfAngle, double& fTranslation) const +{ + // First check if the 3x3 submatrix is orthogonal + for ( int i=0; i<3; i++ ) { + // length must be one + if ( fabs(dMtrx4D[0][i]*dMtrx4D[0][i]+dMtrx4D[1][i]*dMtrx4D[1][i]+dMtrx4D[2][i]*dMtrx4D[2][i]-1.0) > 0.01 ) + return false; + // scalar product with other rows must be zero + if ( fabs(dMtrx4D[0][i]*dMtrx4D[0][(i+1)%3]+dMtrx4D[1][i]*dMtrx4D[1][(i+1)%3]+dMtrx4D[2][i]*dMtrx4D[2][(i+1)%3]) > 0.01 ) + return false; + } + + // Okay, the 3x3 matrix is orthogonal. + // Note: The section to get the rotation axis and angle was taken from WildMagic Library. + // + // Let (x,y,z) be the unit-length axis and let A be an angle of rotation. + // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where + // I is the identity and + // + // +- -+ + // P = | 0 -z +y | + // | +z 0 -x | + // | -y +x 0 | + // +- -+ + // + // If A > 0, R represents a counterclockwise rotation about the axis in + // the sense of looking from the tip of the axis vector towards the + // origin. Some algebra will show that + // + // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P + // + // In the event that A = pi, R-R^t = 0 which prevents us from extracting + // the axis through P. Instead note that R = I+2*P^2 when A = pi, so + // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and + // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi, + // it does not matter which sign you choose on the square roots. + // + // For more details see also http://www.math.niu.edu/~rusin/known-math/97/rotations + + double fTrace = dMtrx4D[0][0] + dMtrx4D[1][1] + dMtrx4D[2][2]; + double fCos = 0.5*(fTrace-1.0); + rfAngle = acos(fCos); // in [0,PI] + + if ( rfAngle > 0.0f ) + { + if ( rfAngle < F_PI ) + { + rclDir.x = (dMtrx4D[2][1]-dMtrx4D[1][2]); + rclDir.y = (dMtrx4D[0][2]-dMtrx4D[2][0]); + rclDir.z = (dMtrx4D[1][0]-dMtrx4D[0][1]); + rclDir.Normalize(); + } + else + { + // angle is PI + double fHalfInverse; + if ( dMtrx4D[0][0] >= dMtrx4D[1][1] ) + { + // r00 >= r11 + if ( dMtrx4D[0][0] >= dMtrx4D[2][2] ) + { + // r00 is maximum diagonal term + rclDir.x = (0.5*sqrt(dMtrx4D[0][0] - dMtrx4D[1][1] - dMtrx4D[2][2] + 1.0)); + fHalfInverse = 0.5/rclDir.x; + rclDir.y = (fHalfInverse*dMtrx4D[0][1]); + rclDir.z = (fHalfInverse*dMtrx4D[0][2]); + } + else + { + // r22 is maximum diagonal term + rclDir.z = (0.5*sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0)); + fHalfInverse = 0.5/rclDir.z; + rclDir.x = (fHalfInverse*dMtrx4D[0][2]); + rclDir.y = (fHalfInverse*dMtrx4D[1][2]); + } + } + else + { + // r11 > r00 + if ( dMtrx4D[1][1] >= dMtrx4D[2][2] ) + { + // r11 is maximum diagonal term + rclDir.y = (0.5*sqrt(dMtrx4D[1][1] - dMtrx4D[0][0] - dMtrx4D[2][2] + 1.0)); + fHalfInverse = 0.5/rclDir.y; + rclDir.x = (fHalfInverse*dMtrx4D[0][1]); + rclDir.z = (fHalfInverse*dMtrx4D[1][2]); + } + else + { + // r22 is maximum diagonal term + rclDir.z = (0.5*sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0)); + fHalfInverse = 0.5/rclDir.z; + rclDir.x = (fHalfInverse*dMtrx4D[0][2]); + rclDir.y = (fHalfInverse*dMtrx4D[1][2]); + } + } + } + } + else + { + // The angle is 0 and the matrix is the identity. Any axis will + // work, so just use the x-axis. + rclDir.x = 1.0; + rclDir.y = 0.0; + rclDir.z = 0.0; + rclBase.x = 0.0; + rclBase.y = 0.0; + rclBase.z = 0.0; + } + + // This is the translation part in axis direction + fTranslation = (dMtrx4D[0][3]*rclDir.x+dMtrx4D[1][3]*rclDir.y+dMtrx4D[2][3]*rclDir.z); + Vector3d cPnt(dMtrx4D[0][3],dMtrx4D[1][3],dMtrx4D[2][3]); + cPnt = cPnt - fTranslation * rclDir; + + // This is the base point of the rotation axis + if ( rfAngle > 0.0f ) + { + double factor = 0.5*(1.0+fTrace)/sin(rfAngle); + rclBase.x = (0.5*(cPnt.x+factor*(rclDir.y*cPnt.z-rclDir.z*cPnt.y))); + rclBase.y = (0.5*(cPnt.y+factor*(rclDir.z*cPnt.x-rclDir.x*cPnt.z))); + rclBase.z = (0.5*(cPnt.z+factor*(rclDir.x*cPnt.y-rclDir.y*cPnt.x))); + } + + return true; +} + void Matrix4D::transform (const Vector3f& rclVct, const Matrix4D& rclMtrx) { move(-rclVct); @@ -701,8 +828,8 @@ void Matrix4D::fromString(const std::string &str) input >> dMtrx4D[i][j]; } } - -// Analyse the a transformation Matrix and describe the transformation + +// Analyse the a transformation Matrix and describe the transformation std::string Matrix4D::analyse(void) const { const double eps=1.0e-06; @@ -723,22 +850,22 @@ std::string Matrix4D::analyse(void) const } else //translation and affine { - if (dMtrx4D[0][1] == 0.0 && dMtrx4D[0][2] == 0.0 && - dMtrx4D[1][0] == 0.0 && dMtrx4D[1][2] == 0.0 && + if (dMtrx4D[0][1] == 0.0 && dMtrx4D[0][2] == 0.0 && + dMtrx4D[1][0] == 0.0 && dMtrx4D[1][2] == 0.0 && dMtrx4D[2][0] == 0.0 && dMtrx4D[2][1] == 0.0) //scaling { - std::ostringstream stringStream; - stringStream << "Scale [" << dMtrx4D[0][0] << ", " << + std::ostringstream stringStream; + stringStream << "Scale [" << dMtrx4D[0][0] << ", " << dMtrx4D[1][1] << ", " << dMtrx4D[2][2] << "]"; - text = stringStream.str(); + text = stringStream.str(); } else { Base::Matrix4D sub; - sub[0][0] = dMtrx4D[0][0]; sub[0][1] = dMtrx4D[0][1]; - sub[0][2] = dMtrx4D[0][2]; sub[1][0] = dMtrx4D[1][0]; + sub[0][0] = dMtrx4D[0][0]; sub[0][1] = dMtrx4D[0][1]; + sub[0][2] = dMtrx4D[0][2]; sub[1][0] = dMtrx4D[1][0]; sub[1][1] = dMtrx4D[1][1]; sub[1][2] = dMtrx4D[1][2]; - sub[2][0] = dMtrx4D[2][0]; sub[2][1] = dMtrx4D[2][1]; + sub[2][0] = dMtrx4D[2][0]; sub[2][1] = dMtrx4D[2][1]; sub[2][2] = dMtrx4D[2][2]; Base::Matrix4D trp = sub; @@ -771,23 +898,23 @@ std::string Matrix4D::analyse(void) const } else //scaling with rotation { - std::ostringstream stringStream; - stringStream << "Scale and Rotate "; + std::ostringstream stringStream; + stringStream << "Scale and Rotate "; if (determinant<0.0 ) - stringStream << "and Invert "; - stringStream << "[ " << - sqrt(trp[0][0]) << ", " << sqrt(trp[1][1]) << ", " << + stringStream << "and Invert "; + stringStream << "[ " << + sqrt(trp[0][0]) << ", " << sqrt(trp[1][1]) << ", " << sqrt(trp[2][2]) << "]"; - text = stringStream.str(); + text = stringStream.str(); } } } else { - std::ostringstream stringStream; - stringStream << "Affine with det= " << + std::ostringstream stringStream; + stringStream << "Affine with det= " << determinant; - text = stringStream.str(); + text = stringStream.str(); } } } @@ -795,4 +922,4 @@ std::string Matrix4D::analyse(void) const text += " with Translation"; } return text; -} +} diff --git a/src/Base/Matrix.h b/src/Base/Matrix.h index 36444c667..99e18bd2b 100644 --- a/src/Base/Matrix.h +++ b/src/Base/Matrix.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include "Vector3D.h" @@ -108,12 +108,16 @@ public: /// moves the coordinatesystem for the x,y,z value void move (float x, float y, float z) { move(Vector3f(x,y,z)); } + void move (double x, double y, double z) + { move(Vector3d(x,y,z)); } /// moves the coordinatesystem for the vector void move (const Vector3f& rclVct); void move (const Vector3d& rclVct); /// scale for the vector void scale (float x, float y, float z) { scale(Vector3f(x,y,z)); } + void scale (double x, double y, double z) + { scale(Vector3d(x,y,z)); } /// scale for the x,y,z value void scale (const Vector3f& rclVct); void scale (const Vector3d& rclVct); @@ -131,6 +135,7 @@ public: void rotLine (const Vector3d& rclBase, const Vector3d& rclDir, double fAngle); /// Extract the rotation axis and angle. Therefore the 3x3 submatrix must be orthogonal. bool toAxisAngle (Vector3f& rclBase, Vector3f& rclDir, float& fAngle, float& fTranslation) const; + bool toAxisAngle (Vector3d& rclBase, Vector3d& rclDir, double& fAngle, double& fTranslation) const; /// transform (move,scale,rotate) around a point void transform (const Vector3f& rclVct, const Matrix4D& rclMtrx); void transform (const Vector3d& rclVct, const Matrix4D& rclMtrx); diff --git a/src/Base/Persistence.h b/src/Base/Persistence.h index 0ca3fb3e3..f05315653 100644 --- a/src/Base/Persistence.h +++ b/src/Base/Persistence.h @@ -75,9 +75,9 @@ public: * // read my Element * reader.readElement("PropertyVector"); * // get the value of my Attribute - * _cVec.x = (float)reader.getAttributeAsFloat("valueX"); - * _cVec.y = (float)reader.getAttributeAsFloat("valueY"); - * _cVec.z = (float)reader.getAttributeAsFloat("valueZ"); + * _cVec.x = reader.getAttributeAsFloat("valueX"); + * _cVec.y = reader.getAttributeAsFloat("valueY"); + * _cVec.z = reader.getAttributeAsFloat("valueZ"); * } * \endcode */