diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 1907371e..edb1aa3c 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -56,9 +56,9 @@ Matrix3x3::~Matrix3x3() { // Copy-constructor Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { - setAllValues(matrix.mArray[0][0], matrix.mArray[0][1], matrix.mArray[0][2], - matrix.mArray[1][0], matrix.mArray[1][1], matrix.mArray[1][2], - matrix.mArray[2][0], matrix.mArray[2][1], matrix.mArray[2][2]); + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], + matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], + matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); } // Assignment operator @@ -66,9 +66,9 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { // Check for self-assignment if (&matrix != this) { - setAllValues(matrix.mArray[0][0], matrix.mArray[0][1], matrix.mArray[0][2], - matrix.mArray[1][0], matrix.mArray[1][1], matrix.mArray[1][2], - matrix.mArray[2][0], matrix.mArray[2][1], matrix.mArray[2][2]); + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], + matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], + matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); } return *this; } @@ -84,15 +84,15 @@ Matrix3x3 Matrix3x3::getInverse() const { decimal invDeterminant = decimal(1.0) / determinant; - Matrix3x3 tempMatrix((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]), - -(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]), - (mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]), - -(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]), - (mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]), - -(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]), - (mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]), - -(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]), - (mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0])); + Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]), + -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]), + (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]), + -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]), + (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]), + -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]), + (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]), + -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]), + (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0])); // Return the inverse matrix return (invDeterminant * tempMatrix); diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index aceb7916..906a19fc 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -46,8 +46,8 @@ class Matrix3x3 { // -------------------- Attributes -------------------- // - // Array with the values of the matrix - decimal mArray[3][3]; + /// Rows of the matrix; + Vector3 mRows[3]; public : @@ -72,19 +72,19 @@ class Matrix3x3 { // Assignment operator Matrix3x3& operator=(const Matrix3x3& matrix); - // Get a value in the matrix - decimal getValue(int i, int j) const; - - // Set a value in the matrix - void setValue(int i, int j, decimal value); - // Set all the values in the matrix void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3); + /// Set the matrix to zero + void setToZero(); + // Return a column Vector3 getColumn(int i) const; + /// Return a row + Vector3 getRow(int i) const; + // Return the transpose matrix Matrix3x3 getTranspose() const; @@ -141,110 +141,120 @@ class Matrix3x3 { // Overloaded operator for multiplication with a number with assignment Matrix3x3& operator*=(decimal nb); + + /// Overloaded operator to read element of the matrix. + const Vector3& operator[](int row) const; + + /// Overloaded operator to read/write element of the matrix. + Vector3& operator[](int row); }; - -// Method to get a value in the matrix (inline) -inline decimal Matrix3x3::getValue(int i, int j) const { - assert(i>=0 && i<3 && j>=0 && j<3); - return mArray[i][j]; -} - -// Method to set a value in the matrix (inline) -inline void Matrix3x3::setValue(int i, int j, decimal value) { - assert(i>=0 && i<3 && j>=0 && j<3); - mArray[i][j] = value; -} - // Method to set all the values in the matrix inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) { - mArray[0][0] = a1; mArray[0][1] = a2; mArray[0][2] = a3; - mArray[1][0] = b1; mArray[1][1] = b2; mArray[1][2] = b3; - mArray[2][0] = c1; mArray[2][1] = c2; mArray[2][2] = c3; + mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3; + mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3; + mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3; +} + +/// Set the matrix to zero +inline void Matrix3x3::setToZero() { + mRows[0].setToZero(); + mRows[1].setToZero(); + mRows[2].setToZero(); } // Return a column inline Vector3 Matrix3x3::getColumn(int i) const { assert(i>= 0 && i<3); - return Vector3(mArray[0][i], mArray[1][i], mArray[2][i]); + return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]); +} + +/// Return a row +inline Vector3 Matrix3x3::getRow(int i) const { + assert(i>= 0 && i<3); + return mRows[i]; } // Return the transpose matrix inline Matrix3x3 Matrix3x3::getTranspose() const { + // Return the transpose matrix - return Matrix3x3(mArray[0][0], mArray[1][0], mArray[2][0], - mArray[0][1], mArray[1][1], mArray[2][1], - mArray[0][2], mArray[1][2], mArray[2][2]); + return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0], + mRows[0][1], mRows[1][1], mRows[2][1], + mRows[0][2], mRows[1][2], mRows[2][2]); } // Return the determinant of the matrix inline decimal Matrix3x3::getDeterminant() const { + // Compute and return the determinant of the matrix - return (mArray[0][0]*(mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]) - - mArray[0][1]*(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]) + - mArray[0][2]*(mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1])); + return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) - + mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) + + mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1])); } // Return the trace of the matrix inline decimal Matrix3x3::getTrace() const { + // Compute and return the trace - return (mArray[0][0] + mArray[1][1] + mArray[2][2]); + return (mRows[0][0] + mRows[1][1] + mRows[2][2]); } // Set the matrix to the identity matrix inline void Matrix3x3::setToIdentity() { - mArray[0][0] = 1.0; mArray[0][1] = 0.0; mArray[0][2] = 0.0; - mArray[1][0] = 0.0; mArray[1][1] = 1.0; mArray[1][2] = 0.0; - mArray[2][0] = 0.0; mArray[2][1] = 0.0; mArray[2][2] = 1.0; + mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[0][2] = 0.0; + mRows[1][0] = 0.0; mRows[1][1] = 1.0; mRows[1][2] = 0.0; + mRows[2][0] = 0.0; mRows[2][1] = 0.0; mRows[2][2] = 1.0; } // Return the 3x3 identity matrix inline Matrix3x3 Matrix3x3::identity() { + // Return the isdentity matrix return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); } // Return the matrix with absolute values inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(fabs(mArray[0][0]), fabs(mArray[0][1]), fabs(mArray[0][2]), - fabs(mArray[1][0]), fabs(mArray[1][1]), fabs(mArray[1][2]), - fabs(mArray[2][0]), fabs(mArray[2][1]), fabs(mArray[2][2])); + return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), + fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), + fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); } // Overloaded operator for addition inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mArray[0][0] + matrix2.mArray[0][0], matrix1.mArray[0][1] + - matrix2.mArray[0][1], matrix1.mArray[0][2] + matrix2.mArray[0][2], - matrix1.mArray[1][0] + matrix2.mArray[1][0], matrix1.mArray[1][1] + - matrix2.mArray[1][1], matrix1.mArray[1][2] + matrix2.mArray[1][2], - matrix1.mArray[2][0] + matrix2.mArray[2][0], matrix1.mArray[2][1] + - matrix2.mArray[2][1], matrix1.mArray[2][2] + matrix2.mArray[2][2]); + return Matrix3x3(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] + + matrix2.mRows[0][1], matrix1.mRows[0][2] + matrix2.mRows[0][2], + matrix1.mRows[1][0] + matrix2.mRows[1][0], matrix1.mRows[1][1] + + matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2], + matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] + + matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]); } // Overloaded operator for substraction inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mArray[0][0] - matrix2.mArray[0][0], matrix1.mArray[0][1] - - matrix2.mArray[0][1], matrix1.mArray[0][2] - matrix2.mArray[0][2], - matrix1.mArray[1][0] - matrix2.mArray[1][0], matrix1.mArray[1][1] - - matrix2.mArray[1][1], matrix1.mArray[1][2] - matrix2.mArray[1][2], - matrix1.mArray[2][0] - matrix2.mArray[2][0], matrix1.mArray[2][1] - - matrix2.mArray[2][1], matrix1.mArray[2][2] - matrix2.mArray[2][2]); + return Matrix3x3(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] - + matrix2.mRows[0][1], matrix1.mRows[0][2] - matrix2.mRows[0][2], + matrix1.mRows[1][0] - matrix2.mRows[1][0], matrix1.mRows[1][1] - + matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2], + matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] - + matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]); } // Overloaded operator for the negative of the matrix inline Matrix3x3 operator-(const Matrix3x3& matrix) { - return Matrix3x3(-matrix.mArray[0][0], -matrix.mArray[0][1], -matrix.mArray[0][2], - -matrix.mArray[1][0], -matrix.mArray[1][1], -matrix.mArray[1][2], - -matrix.mArray[2][0], -matrix.mArray[2][1], -matrix.mArray[2][2]); + return Matrix3x3(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[0][2], + -matrix.mRows[1][0], -matrix.mRows[1][1], -matrix.mRows[1][2], + -matrix.mRows[2][0], -matrix.mRows[2][1], -matrix.mRows[2][2]); } // Overloaded operator for multiplication with a number inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) { - return Matrix3x3(matrix.mArray[0][0] * nb, matrix.mArray[0][1] * nb, matrix.mArray[0][2] * nb, - matrix.mArray[1][0] * nb, matrix.mArray[1][1] * nb, matrix.mArray[1][2] * nb, - matrix.mArray[2][0] * nb, matrix.mArray[2][1] * nb, matrix.mArray[2][2] * nb); + return Matrix3x3(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[0][2] * nb, + matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb, matrix.mRows[1][2] * nb, + matrix.mRows[2][0] * nb, matrix.mRows[2][1] * nb, matrix.mRows[2][2] * nb); } // Overloaded operator for multiplication with a matrix @@ -254,44 +264,44 @@ inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) { // Overloaded operator for matrix multiplication inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) { - return Matrix3x3(matrix1.mArray[0][0]*matrix2.mArray[0][0] + matrix1.mArray[0][1] * - matrix2.mArray[1][0] + matrix1.mArray[0][2]*matrix2.mArray[2][0], - matrix1.mArray[0][0]*matrix2.mArray[0][1] + matrix1.mArray[0][1] * - matrix2.mArray[1][1] + matrix1.mArray[0][2]*matrix2.mArray[2][1], - matrix1.mArray[0][0]*matrix2.mArray[0][2] + matrix1.mArray[0][1] * - matrix2.mArray[1][2] + matrix1.mArray[0][2]*matrix2.mArray[2][2], - matrix1.mArray[1][0]*matrix2.mArray[0][0] + matrix1.mArray[1][1] * - matrix2.mArray[1][0] + matrix1.mArray[1][2]*matrix2.mArray[2][0], - matrix1.mArray[1][0]*matrix2.mArray[0][1] + matrix1.mArray[1][1] * - matrix2.mArray[1][1] + matrix1.mArray[1][2]*matrix2.mArray[2][1], - matrix1.mArray[1][0]*matrix2.mArray[0][2] + matrix1.mArray[1][1] * - matrix2.mArray[1][2] + matrix1.mArray[1][2]*matrix2.mArray[2][2], - matrix1.mArray[2][0]*matrix2.mArray[0][0] + matrix1.mArray[2][1] * - matrix2.mArray[1][0] + matrix1.mArray[2][2]*matrix2.mArray[2][0], - matrix1.mArray[2][0]*matrix2.mArray[0][1] + matrix1.mArray[2][1] * - matrix2.mArray[1][1] + matrix1.mArray[2][2]*matrix2.mArray[2][1], - matrix1.mArray[2][0]*matrix2.mArray[0][2] + matrix1.mArray[2][1] * - matrix2.mArray[1][2] + matrix1.mArray[2][2]*matrix2.mArray[2][2]); + return Matrix3x3(matrix1.mRows[0][0]*matrix2.mRows[0][0] + matrix1.mRows[0][1] * + matrix2.mRows[1][0] + matrix1.mRows[0][2]*matrix2.mRows[2][0], + matrix1.mRows[0][0]*matrix2.mRows[0][1] + matrix1.mRows[0][1] * + matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1], + matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] * + matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2], + matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] * + matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0], + matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] * + matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1], + matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] * + matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2], + matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] * + matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0], + matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] * + matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1], + matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] * + matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]); } // Overloaded operator for multiplication with a vector inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) { - return Vector3(matrix.mArray[0][0]*vector.x + matrix.mArray[0][1]*vector.y + - matrix.mArray[0][2]*vector.z, - matrix.mArray[1][0]*vector.x + matrix.mArray[1][1]*vector.y + - matrix.mArray[1][2]*vector.z, - matrix.mArray[2][0]*vector.x + matrix.mArray[2][1]*vector.y + - matrix.mArray[2][2]*vector.z); + return Vector3(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y + + matrix.mRows[0][2]*vector.z, + matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y + + matrix.mRows[1][2]*vector.z, + matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y + + matrix.mRows[2][2]*vector.z); } // Overloaded operator for equality condition inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const { - return (mArray[0][0] == matrix.mArray[0][0] && mArray[0][1] == matrix.mArray[0][1] && - mArray[0][2] == matrix.mArray[0][2] && - mArray[1][0] == matrix.mArray[1][0] && mArray[1][1] == matrix.mArray[1][1] && - mArray[1][2] == matrix.mArray[1][2] && - mArray[2][0] == matrix.mArray[2][0] && mArray[2][1] == matrix.mArray[2][1] && - mArray[2][2] == matrix.mArray[2][2]); + return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && + mRows[0][2] == matrix.mRows[0][2] && + mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1] && + mRows[1][2] == matrix.mRows[1][2] && + mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] && + mRows[2][2] == matrix.mRows[2][2]); } // Overloaded operator for the is different condition @@ -301,32 +311,46 @@ inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const { // Overloaded operator for addition with assignment inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) { - mArray[0][0] += matrix.mArray[0][0]; mArray[0][1] += matrix.mArray[0][1]; - mArray[0][2] += matrix.mArray[0][2]; mArray[1][0] += matrix.mArray[1][0]; - mArray[1][1] += matrix.mArray[1][1]; mArray[1][2] += matrix.mArray[1][2]; - mArray[2][0] += matrix.mArray[2][0]; mArray[2][1] += matrix.mArray[2][1]; - mArray[2][2] += matrix.mArray[2][2]; + mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; + mRows[0][2] += matrix.mRows[0][2]; mRows[1][0] += matrix.mRows[1][0]; + mRows[1][1] += matrix.mRows[1][1]; mRows[1][2] += matrix.mRows[1][2]; + mRows[2][0] += matrix.mRows[2][0]; mRows[2][1] += matrix.mRows[2][1]; + mRows[2][2] += matrix.mRows[2][2]; return *this; } // Overloaded operator for substraction with assignment inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { - mArray[0][0] -= matrix.mArray[0][0]; mArray[0][1] -= matrix.mArray[0][1]; - mArray[0][2] -= matrix.mArray[0][2]; mArray[1][0] -= matrix.mArray[1][0]; - mArray[1][1] -= matrix.mArray[1][1]; mArray[1][2] -= matrix.mArray[1][2]; - mArray[2][0] -= matrix.mArray[2][0]; mArray[2][1] -= matrix.mArray[2][1]; - mArray[2][2] -= matrix.mArray[2][2]; + mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; + mRows[0][2] -= matrix.mRows[0][2]; mRows[1][0] -= matrix.mRows[1][0]; + mRows[1][1] -= matrix.mRows[1][1]; mRows[1][2] -= matrix.mRows[1][2]; + mRows[2][0] -= matrix.mRows[2][0]; mRows[2][1] -= matrix.mRows[2][1]; + mRows[2][2] -= matrix.mRows[2][2]; return *this; } // Overloaded operator for multiplication with a number with assignment inline Matrix3x3& Matrix3x3::operator*=(decimal nb) { - mArray[0][0] *= nb; mArray[0][1] *= nb; mArray[0][2] *= nb; - mArray[1][0] *= nb; mArray[1][1] *= nb; mArray[1][2] *= nb; - mArray[2][0] *= nb; mArray[2][1] *= nb; mArray[2][2] *= nb; + mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[0][2] *= nb; + mRows[1][0] *= nb; mRows[1][1] *= nb; mRows[1][2] *= nb; + mRows[2][0] *= nb; mRows[2][1] *= nb; mRows[2][2] *= nb; return *this; } +// Overloaded operator to return a row of the matrix. +/// This operator is also used to access a matrix value using the syntax +/// matrix[row][col]. +inline const Vector3& Matrix3x3::operator[](int row) const { + return mRows[row]; +} + +// Overloaded operator to return a row of the matrix. +/// This operator is also used to access a matrix value using the syntax +/// matrix[row][col]. +inline Vector3& Matrix3x3::operator[](int row) { + return mRows[row]; +} + } #endif diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 8c5d0c81..4bb7e775 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -59,58 +59,51 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { // Get the trace of the matrix decimal trace = matrix.getTrace(); - decimal array[3][3]; - for (int i=0; i<3; i++) { - for (int j=0; j<3; j++) { - array[i][j] = matrix.getValue(i, j); - } - } - decimal r; decimal s; if (trace < 0.0) { - if (array[1][1] > array[0][0]) { - if(array[2][2] > array[1][1]) { - r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0); + if (matrix[1][1] > matrix[0][0]) { + if(matrix[2][2] > matrix[1][1]) { + r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + 1.0); s = 0.5 / r; // Compute the quaternion - x = (array[2][0] + array[0][2])*s; - y = (array[1][2] + array[2][1])*s; + x = (matrix[2][0] + matrix[0][2])*s; + y = (matrix[1][2] + matrix[2][1])*s; z = 0.5*r; - w = (array[1][0] - array[0][1])*s; + w = (matrix[1][0] - matrix[0][1])*s; } else { - r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0); + r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + 1.0); s = 0.5 / r; // Compute the quaternion - x = (array[0][1] + array[1][0])*s; + x = (matrix[0][1] + matrix[1][0])*s; y = 0.5 * r; - z = (array[1][2] + array[2][1])*s; - w = (array[0][2] - array[2][0])*s; + z = (matrix[1][2] + matrix[2][1])*s; + w = (matrix[0][2] - matrix[2][0])*s; } } - else if (array[2][2] > array[0][0]) { - r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0); + else if (matrix[2][2] > matrix[0][0]) { + r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + 1.0); s = 0.5 / r; // Compute the quaternion - x = (array[2][0] + array[0][2])*s; - y = (array[1][2] + array[2][1])*s; + x = (matrix[2][0] + matrix[0][2])*s; + y = (matrix[1][2] + matrix[2][1])*s; z = 0.5 * r; - w = (array[1][0] - array[0][1])*s; + w = (matrix[1][0] - matrix[0][1])*s; } else { - r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0); + r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + 1.0); s = 0.5 / r; // Compute the quaternion x = 0.5 * r; - y = (array[0][1] + array[1][0])*s; - z = (array[2][0] - array[0][2])*s; - w = (array[2][1] - array[1][2])*s; + y = (matrix[0][1] + matrix[1][0])*s; + z = (matrix[2][0] - matrix[0][2])*s; + w = (matrix[2][1] - matrix[1][2])*s; } } else { @@ -118,9 +111,9 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { s = 0.5/r; // Compute the quaternion - x = (array[2][1]-array[1][2])*s; - y = (array[0][2]-array[2][0])*s; - z = (array[1][0]-array[0][1])*s; + x = (matrix[2][1] - matrix[1][2]) * s; + y = (matrix[0][2] - matrix[2][0]) * s; + z = (matrix[1][0] - matrix[0][1]) * s; w = 0.5 * r; } } diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index c643b41f..52f58b05 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -70,12 +70,21 @@ struct Quaternion { // Destructor ~Quaternion(); + /// Set all the values + void setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW); + + /// Set the quaternion to zero + void setToZero(); + // Return the vector v=(x y z) of the quaternion - Vector3 vectorV() const; + Vector3 getVectorV() const; // Return the length of the quaternion decimal length() const; + /// Normalize the quaternion + void normalize(); + // Return the unit quaternion Quaternion getUnit() const; @@ -113,6 +122,9 @@ struct Quaternion { // Overloaded operator for the multiplication Quaternion operator*(const Quaternion& quaternion) const; + /// Overloaded operator for the multiplication with a vector + Vector3 operator*(const Vector3& point); + // Overloaded operator for assignment Quaternion& operator=(const Quaternion& quaternion); @@ -120,8 +132,24 @@ struct Quaternion { bool operator==(const Quaternion& quaternion) const; }; +/// Set all the values +inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { + x = newX; + y = newY; + z = newZ; + w = newW; +} + +/// Set the quaternion to zero +inline void Quaternion::setToZero() { + x = 0; + y = 0; + z = 0; + w = 0; +} + // Return the vector v=(x y z) of the quaternion -inline Vector3 Quaternion::vectorV() const { +inline Vector3 Quaternion::getVectorV() const { // Return the vector v return Vector3(x, y, z); @@ -132,6 +160,20 @@ inline decimal Quaternion::length() const { return sqrt(x*x + y*y + z*z + w*w); } +// Normalize the quaternion +inline void Quaternion::normalize() { + + decimal l = length(); + + // Check if the length is not equal to zero + assert (l > MACHINE_EPSILON); + + x /= l; + y /= l; + z /= l; + w /= l; +} + // Return the unit quaternion inline Quaternion Quaternion::getUnit() const { decimal lengthQuaternion = length(); @@ -193,9 +235,16 @@ inline Quaternion Quaternion::operator*(decimal nb) const { // Overloaded operator for the multiplication of two quaternions inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { - return Quaternion(w * quaternion.w - vectorV().dot(quaternion.vectorV()), - w * quaternion.vectorV() + quaternion.w * vectorV() + - vectorV().cross(quaternion.vectorV())); + return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), + w * quaternion.getVectorV() + quaternion.w * getVectorV() + + getVectorV().cross(quaternion.getVectorV())); +} + +// Overloaded operator for the multiplication with a vector. +/// This methods rotates a point given the rotation of a quaternion. +inline Vector3 Quaternion::operator*(const Vector3& point) { + Quaternion p(point.x, point.y, point.z, 0.0); + return (((*this) * p) * getConjugate()).getVectorV(); } // Overloaded operator for the assignment diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index b5fe9f3a..bc94c758 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -100,6 +100,9 @@ class Transform { const Transform& newTransform, decimal interpolationFactor); + /// Return the identity transform + static Transform identity(); + // Return the transformed vector Vector3 operator*(const Vector3& vector) const; @@ -154,12 +157,12 @@ inline void Transform::setFromOpenGL(decimal* openglMatrix) { // Get the OpenGL matrix of the transform inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { const Matrix3x3& matrix = mOrientation.getMatrix(); - openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); - openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 0.0; - openglMatrix[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1); - openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0; - openglMatrix[8] = matrix.getValue(0, 2); openglMatrix[9] = matrix.getValue(1, 2); - openglMatrix[10] = matrix.getValue(2, 2); openglMatrix[11] = 0.0; + openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0]; + openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0; + openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1]; + openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0; + openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2]; + openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0; openglMatrix[12] = mPosition.x; openglMatrix[13] = mPosition.y; openglMatrix[14] = mPosition.z; openglMatrix[15] = 1.0; } @@ -186,6 +189,11 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform, return Transform(interPosition, interOrientation); } +// Return the identity transform +inline Transform Transform::identity() { + return Transform(Vector3(0, 0, 0), Quaternion::identity()); +} + // Return the transformed vector inline Vector3 Transform::operator*(const Vector3& vector) const { return (mOrientation.getMatrix() * vector) + mPosition; diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 66ab9bd0..619cb346 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -67,6 +67,9 @@ struct Vector3 { // Set all the values of the vector void setAllValues(decimal newX, decimal newY, decimal newZ); + /// Set the vector to zero + void setToZero(); + // Return the lenght of the vector decimal length() const; @@ -140,6 +143,13 @@ struct Vector3 { friend Vector3 operator/(const Vector3& vector, decimal number); }; +// Set the vector to zero +inline void Vector3::setToZero() { + x = 0; + y = 0; + z = 0; +} + // Set all the values of the vector inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { x = newX;