PF_API 0.52
|
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