PF_API 0.52

Code/Libs/Math_LIB/MMatrix3x3.h

Go to the documentation of this file.
00001 
00002 //    Copyright (C) 2004-2007 Dylan Blair
00003 //
00004 //    email: dblair@alumni.cs.utexas.edu
00005 //
00006 //    This library is free software; you can redistribute it and/or
00007 //    modify it under the terms of the GNU Lesser General Public
00008 //    License as published by the Free Software Foundation; either
00009 //    version 2.1 of the License, or (at your option) any later version.
00010 //
00011 //    This library is distributed in the hope that it will be useful,
00012 //    but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 //    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014 //    Lesser General Public License for more details.
00015 //
00016 //    You should have received a copy of the GNU Lesser General Public
00017 //    License along with this library; if not, write to the Free Software
00018 //    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020 
00021 #ifndef MMATRIX3X3_H
00022 #define MMATRIX3X3_H
00023 
00024 #include "MPoint.h"
00025 
00026 namespace OpenSkyNet {
00027     namespace Math {
00028         class Matrix3x3;
00029         Matrix3x3 operator*(const Matrix3x3& lhs_, const Matrix3x3& rhs_);
00030 
00032         class Matrix3x3 {
00033             Math::Point<> _columns[3];
00034         public:
00035             Matrix3x3() {
00036                 _columns[0][0] = 1.0f;
00037                 _columns[1][1] = 1.0f;
00038                 _columns[2][2] = 1.0f;
00039             }
00040 
00041             Matrix3x3(const Math::Point<>& localXAxis_, const Math::Point<>& localYAxis_, const Math::Point<>& localZAxis_) {
00042                 _columns[0] = localXAxis_;
00043                 _columns[1] = localYAxis_;
00044                 _columns[2] = localZAxis_;
00045             }
00046 
00049             inline Math::Point<>& col(int i_) { return _columns[i_]; }
00050             inline const Math::Point<>& col(int i_) const { return _columns[i_]; }
00052 
00053             inline float getDeterminant() const {
00054                 return (col(0)[0]*col(1)[1]*col(2)[2] + 
00055                         col(1)[0]*col(2)[1]*col(0)[2] + 
00056                         col(2)[0]*col(0)[1]*col(1)[2] - 
00057                         col(0)[0]*col(2)[1]*col(1)[2] - 
00058                         col(1)[0]*col(0)[1]*col(2)[2] - 
00059                         col(2)[0]*col(1)[1]*col(0)[2]);
00060             }
00061 
00062             inline void cofactor() {
00063                 Matrix3x3 tmp(*this);
00064                 col(0)[0] =   tmp.col(1)[1]*tmp.col(2)[2] - tmp.col(1)[2]*tmp.col(2)[1];
00065                 col(0)[1] = -(tmp.col(1)[0]*tmp.col(2)[2] - tmp.col(1)[2]*tmp.col(2)[0]);
00066                 col(0)[2] =   tmp.col(1)[0]*tmp.col(2)[1] - tmp.col(1)[1]*tmp.col(2)[0];
00067 
00068                 col(1)[0] = -(tmp.col(0)[1]*tmp.col(2)[2] - tmp.col(0)[2]*tmp.col(2)[1]);
00069                 col(1)[1] =   tmp.col(0)[0]*tmp.col(2)[2] - tmp.col(0)[2]*tmp.col(2)[0];
00070                 col(1)[2] = -(tmp.col(0)[0]*tmp.col(2)[1] - tmp.col(0)[1]*tmp.col(2)[0]);
00071 
00072                 col(2)[0] =   tmp.col(0)[1]*tmp.col(1)[2] - tmp.col(0)[2]*tmp.col(1)[1];
00073                 col(2)[1] = -(tmp.col(0)[0]*tmp.col(1)[2] - tmp.col(0)[2]*tmp.col(1)[0]);
00074                 col(2)[2] =   tmp.col(0)[0]*tmp.col(1)[1] - tmp.col(0)[1]*tmp.col(1)[0];
00075             }
00076 
00077             inline void transpose() {
00078                 float tmp = col(0)[1];
00079                 col(0)[1] = col(1)[0];
00080                 col(1)[0] = tmp;
00081 
00082                 tmp = col(0)[2];
00083                 col(0)[2] = col(2)[0];
00084                 col(2)[0] = tmp;
00085 
00086                 tmp = col(1)[2];
00087                 col(1)[2] = col(2)[1];
00088                 col(2)[1] = tmp;
00089             }
00090 
00092             inline void orthonormalize() {
00093                 col(0).normalize();
00094 
00095                 col(1) = col(1) - col(0) * col(0).getDot3(col(1));
00096                 col(1).normalize();
00097 
00098                 col(2) = col(2) - col(0) * col(0).getDot3(col(2)) - col(1) * col(1).getDot3(col(2));
00099                 col(2).normalize();
00100             }
00101 
00102             inline void invert() {
00103                 float invDet = 1.0f / getDeterminant();
00104                 cofactor();
00105                 transpose();
00106                 col(0) *= invDet;
00107                 col(1) *= invDet;
00108                 col(2) *= invDet;
00109                 orthonormalize();
00110             }
00111 
00112             inline Matrix3x3 getInverse() const {
00113                 float invDet = 1.0f / getDeterminant();
00114                 Matrix3x3 result(*this);
00115                 result.cofactor();
00116                 result.transpose();
00117                 result.col(0) *= invDet;
00118                 result.col(1) *= invDet;
00119                 result.col(2) *= invDet;
00120                 result.orthonormalize();
00121                 return result;
00122             }
00123         };
00124 
00125         extern const Math::Matrix3x3 g_identityMatrix3x3;
00126 
00129         inline Matrix3x3 getRotation(const Math::Point<>& axis_, float radians_) {
00130             Matrix3x3 rotMat;
00131             float cosAngle = cosf(radians_);
00132             float sinAngle = sinf(radians_);
00133             float oneMinusCos = 1.0f - cosAngle;
00134 
00135             rotMat.col(0)[0] = cosAngle + oneMinusCos * axis_.x() * axis_.x();
00136             rotMat.col(0)[1] = oneMinusCos * axis_.x() * axis_.y() + axis_.z() * sinAngle;
00137             rotMat.col(0)[2] = oneMinusCos * axis_.x() * axis_.z() - axis_.y() * sinAngle;
00138 
00139             rotMat.col(1)[0] = oneMinusCos * axis_.x() * axis_.y() - axis_.z() * sinAngle;
00140             rotMat.col(1)[1] = cosAngle + oneMinusCos * axis_.y() * axis_.y();
00141             rotMat.col(1)[2] = oneMinusCos * axis_.y() * axis_.z() + axis_.x() * sinAngle;
00142 
00143             rotMat.col(2)[0] = oneMinusCos * axis_.x() * axis_.z() + axis_.y() * sinAngle;
00144             rotMat.col(2)[1] = oneMinusCos * axis_.y() * axis_.z() - axis_.x() * sinAngle;
00145             rotMat.col(2)[2] = cosAngle + oneMinusCos * axis_.z() * axis_.z();
00146 
00147             return rotMat;
00148         }
00149 
00153         inline void getAxisAngle(const Matrix3x3& rot_, Math::Point<>& axis_, float& radians_) {
00154             if (((fabs(rot_.col(1)[0] - rot_.col(0)[1]) < CLOSE_TO_ZERO) && 
00155                 (fabs(rot_.col(2)[0] - rot_.col(0)[2]) < CLOSE_TO_ZERO)) && 
00156                 (fabs(rot_.col(2)[1] - rot_.col(1)[2]) < CLOSE_TO_ZERO)) {
00157                 // singularity found
00158                 if (Point<>(rot_.col(0)[0],rot_.col(1)[1],rot_.col(2)[2]).equals(Point<>(1.0f,1.0f,1.0f),CLOSE_TO_ZERO)) {
00159                     radians_ = 0.0f;
00160                     // axis is arbitrary
00161                     axis_ = Math::g_xAxis;
00162                 }
00163                 else {
00164                     // otherwise this singularity is angle = 180
00165                     radians_ = PI;
00166 
00167                     assert(!((rot_.col(0)[0] + 1.0f) / 2.0f < 0.0f));
00168                     assert(!((rot_.col(1)[1] + 1.0f) / 2.0f < 0.0f));
00169                     assert(!((rot_.col(2)[2] + 1.0f) / 2.0f < 0.0f));
00170 
00171                     axis_ = Point<>(sqrt((rot_.col(0)[0] + 1.0f) / 2.0f),
00172                         sqrt((rot_.col(1)[1] + 1.0f) / 2.0f),
00173                         sqrt((rot_.col(2)[2] + 1.0f) / 2.0f));
00174                     bool isXZero = (fabs(axis_.x()) < CLOSE_TO_ZERO);
00175                     bool isYZero = (fabs(axis_.y()) < CLOSE_TO_ZERO);
00176                     bool isZZero = (fabs(axis_.z()) < CLOSE_TO_ZERO);
00177                     bool isXYPos = (rot_.col(1)[0] > CLOSE_TO_ZERO);
00178                     bool isXZPos = (rot_.col(2)[0] > CLOSE_TO_ZERO);
00179                     bool isYZPos = (rot_.col(2)[1] > CLOSE_TO_ZERO);
00180                     if (isXZero && !isYZero && !isZZero) {
00181                         if (!isYZPos) axis_.y() = -axis_.y();
00182                     } else if (isYZero && !isZZero) {
00183                         if (!isXZPos) axis_.z() = -axis_.z();
00184                     } else if (isZZero) {
00185                         if (!isXYPos) axis_.x() = -axis_.x();
00186                     }
00187                 }
00188             }
00189             else {
00190                 radians_ = acos((rot_.col(0)[0] + rot_.col(1)[1] + rot_.col(2)[2] - 1.0f) / 2.0f);
00191                 axis_ = Point<>((rot_.col(1)[2] - rot_.col(2)[1]),
00192                     (rot_.col(2)[0] - rot_.col(0)[2]),
00193                     (rot_.col(0)[1] - rot_.col(1)[0]));
00194                 axis_.normalize();
00195             }
00196         }
00197 
00199         inline Math::Point<> operator*(const Math::Point<>& lhs_, const Matrix3x3& rhs_) {
00200             Math::Point<> result;
00201             result.x() = lhs_.x() * rhs_.col(0)[0] + lhs_.y() * rhs_.col(1)[0] + lhs_.z() * rhs_.col(2)[0];
00202             result.y() = lhs_.x() * rhs_.col(0)[1] + lhs_.y() * rhs_.col(1)[1] + lhs_.z() * rhs_.col(2)[1];
00203             result.z() = lhs_.x() * rhs_.col(0)[2] + lhs_.y() * rhs_.col(1)[2] + lhs_.z() * rhs_.col(2)[2];
00204             return result;
00205         }
00206 
00208         inline Matrix3x3 operator*(const Matrix3x3& lhs_, const Matrix3x3& rhs_) {
00209             Matrix3x3 result;
00210             result.col(0)[0] = rhs_.col(0)[0]*lhs_.col(0)[0] + rhs_.col(1)[0]*lhs_.col(0)[1] + rhs_.col(2)[0]*lhs_.col(0)[2];
00211             result.col(0)[1] = rhs_.col(0)[1]*lhs_.col(0)[0] + rhs_.col(1)[1]*lhs_.col(0)[1] + rhs_.col(2)[1]*lhs_.col(0)[2];
00212             result.col(0)[2] = rhs_.col(0)[2]*lhs_.col(0)[0] + rhs_.col(1)[2]*lhs_.col(0)[1] + rhs_.col(2)[2]*lhs_.col(0)[2];
00213 
00214             result.col(1)[0] = rhs_.col(0)[0]*lhs_.col(1)[0] + rhs_.col(1)[0]*lhs_.col(1)[1] + rhs_.col(2)[0]*lhs_.col(1)[2];
00215             result.col(1)[1] = rhs_.col(0)[1]*lhs_.col(1)[0] + rhs_.col(1)[1]*lhs_.col(1)[1] + rhs_.col(2)[1]*lhs_.col(1)[2];
00216             result.col(1)[2] = rhs_.col(0)[2]*lhs_.col(1)[0] + rhs_.col(1)[2]*lhs_.col(1)[1] + rhs_.col(2)[2]*lhs_.col(1)[2];
00217 
00218             result.col(2)[0] = rhs_.col(0)[0]*lhs_.col(2)[0] + rhs_.col(1)[0]*lhs_.col(2)[1] + rhs_.col(2)[0]*lhs_.col(2)[2];
00219             result.col(2)[1] = rhs_.col(0)[1]*lhs_.col(2)[0] + rhs_.col(1)[1]*lhs_.col(2)[1] + rhs_.col(2)[1]*lhs_.col(2)[2];
00220             result.col(2)[2] = rhs_.col(0)[2]*lhs_.col(2)[0] + rhs_.col(1)[2]*lhs_.col(2)[1] + rhs_.col(2)[2]*lhs_.col(2)[2];
00221 
00222             return result;
00223         }
00224     }
00225 }
00226 
00227 #endif //MMATRIX3X3_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines