Matrix4.cpp

00001 /*
00002 The MIT License (MIT)
00003 
00004 Copyright (c) 2016 British Broadcasting Corporation.
00005 This software is provided by Lancaster University by arrangement with the BBC.
00006 
00007 Permission is hereby granted, free of charge, to any person obtaining a
00008 copy of this software and associated documentation files (the "Software"),
00009 to deal in the Software without restriction, including without limitation
00010 the rights to use, copy, modify, merge, publish, distribute, sublicense,
00011 and/or sell copies of the Software, and to permit persons to whom the
00012 Software is furnished to do so, subject to the following conditions:
00013 
00014 The above copyright notice and this permission notice shall be included in
00015 all copies or substantial portions of the Software.
00016 
00017 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00020 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00022 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
00023 DEALINGS IN THE SOFTWARE.
00024 */
00025 
00026 #include "MicroBitConfig.h"
00027 #include "Matrix4.h"
00028 #include "mbed.h"
00029 
00030 /**
00031 * Class definition for a simple matrix, optimised for n x 4 or 4 x n matrices.
00032 *
00033 * This class is heavily optimised for these commonly used matrices as used in 3D geometry,
00034 * and is not intended as a general purpose matrix class. For programmers needing more flexible
00035 * Matrix support, the mbed Matrix and MatrixMath classes from Ernsesto Palacios provide a good basis:
00036 *
00037 * https://developer.mbed.org/cookbook/MatrixClass
00038 * https://developer.mbed.org/users/Yo_Robot/code/MatrixMath/
00039 */
00040 
00041 /**
00042   * Constructor.
00043   * Create a matrix of the given size.
00044   *
00045   * @param rows the number of rows in the matrix to be created.
00046   *
00047   * @param cols the number of columns in the matrix to be created.
00048   *
00049   * @code
00050   * Matrix4(10, 4);        // Creates a Matrix with 10 rows and 4 columns.
00051   * @endcode
00052   */
00053 Matrix4::Matrix4(int rows, int cols)
00054 {
00055     this->rows = rows;
00056     this->cols = cols;
00057 
00058     int size = rows * cols;
00059 
00060     if (size > 0)
00061         data = new float[size];
00062     else
00063         data = NULL;
00064 }
00065 
00066 /**
00067   * Constructor.
00068   * Create a matrix that is an identical copy of the given matrix.
00069   *
00070   * @param matrix The matrix to copy.
00071   *
00072   * @code
00073   * Matrix newMatrix(matrix);        .
00074   * @endcode
00075   */
00076 Matrix4::Matrix4(const Matrix4 &matrix)
00077 {
00078     this->rows = matrix.rows;
00079     this->cols = matrix.cols;
00080 
00081     int size = rows * cols;
00082 
00083     if (size > 0)
00084     {
00085         data = new float[size];
00086         for (int i = 0; i < size; i++)
00087             data[i] = matrix.data[i];
00088     }
00089     else
00090     {
00091         data = NULL;
00092     }
00093 
00094 }
00095 
00096 /**
00097   * Determines the number of columns in this matrix.
00098   *
00099   * @return The number of columns in the matrix.
00100   *
00101   * @code
00102   * int c = matrix.width();
00103   * @endcode
00104   */
00105 int Matrix4::width()
00106 {
00107     return cols;
00108 }
00109 
00110 /**
00111   * Determines the number of rows in this matrix.
00112   *
00113   * @return The number of rows in the matrix.
00114   *
00115   * @code
00116   * int r = matrix.height();
00117   * @endcode
00118   */
00119 int Matrix4::height()
00120 {
00121     return rows;
00122 }
00123 
00124 /**
00125   * Reads the matrix element at the given position.
00126   *
00127   * @param row The row of the element to read.
00128   *
00129   * @param col The column of the element to read.
00130   *
00131   * @return The value of the matrix element at the given position. 0 is returned if the given index is out of range.
00132   *
00133   * @code
00134   * float v = matrix.get(1,2);
00135   * @endcode
00136   */
00137 float Matrix4::get(int row, int col)
00138 {
00139     if (row < 0 || col < 0 || row >= rows || col >= cols)
00140         return 0;
00141 
00142     return data[width() * row + col];
00143 }
00144 
00145 /**
00146   * Writes the matrix element at the given position.
00147   *
00148   * @param row The row of the element to write.
00149   *
00150   * @param col The column of the element to write.
00151   *
00152   * @param v The new value of the element.
00153   *
00154   * @code
00155   * matrix.set(1,2,42.0);
00156   * @endcode
00157   */
00158 void Matrix4::set(int row, int col, float v)
00159 {
00160     if (row < 0 || col < 0 || row >= rows || col >= cols)
00161         return;
00162 
00163     data[width() * row + col] = v;
00164 }
00165 
00166 /**
00167   * Transposes this matrix.
00168   *
00169   * @return the resultant matrix.
00170   *
00171   * @code
00172   * matrix.transpose();
00173   * @endcode
00174   */
00175 Matrix4 Matrix4::transpose()
00176 {
00177     Matrix4 result = Matrix4(cols, rows);
00178 
00179     for (int i = 0; i < width(); i++)
00180         for (int j = 0; j < height(); j++)
00181             result.set(i, j, get(j, i));
00182 
00183     return result;
00184 }
00185 
00186 /**
00187   * Multiplies this matrix with the given matrix (if possible).
00188   *
00189   * @param matrix the matrix to multiply this matrix's values against.
00190   *
00191   * @param transpose Transpose the matrices before multiplication. Defaults to false.
00192   *
00193   * @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
00194   *
00195   * @code
00196   * Matrix result = matrixA.multiply(matrixB);
00197   * @endcode
00198   */
00199 Matrix4 Matrix4::multiply(Matrix4 &matrix, bool transpose)
00200 {
00201     int w = transpose ? height() : width();
00202     int h = transpose ? width() : height();
00203 
00204     if (w != matrix.height())
00205         return Matrix4(0, 0);
00206 
00207     Matrix4 result(h, matrix.width());
00208 
00209     for (int r = 0; r < result.height(); r++)
00210     {
00211         for (int c = 0; c < result.width(); c++)
00212         {
00213             float v = 0.0;
00214 
00215             for (int i = 0; i < w; i++)
00216                 v += (transpose ? get(i, r) : get(r, i)) * matrix.get(i, c);
00217 
00218             result.set(r, c, v);
00219         }
00220     }
00221 
00222     return result;
00223 }
00224 
00225 /**
00226   * Performs an optimised inversion of a 4x4 matrix.
00227   * Only 4x4 matrices are supported by this operation.
00228   *
00229   * @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
00230   *
00231   * @code
00232   * Matrix result = matrixA.invert();
00233   * @endcode
00234   */
00235 Matrix4 Matrix4::invert()
00236 {
00237     // We only support square matrices of size 4...
00238     if (width() != height() || width() != 4)
00239         return Matrix4(0, 0);
00240 
00241     Matrix4 result(width(), height());
00242 
00243     result.data[0] = data[5] * data[10] * data[15] - data[5] * data[11] * data[14] - data[9] * data[6] * data[15] + data[9] * data[7] * data[14] + data[13] * data[6] * data[11] - data[13] * data[7] * data[10];
00244     result.data[1] = -data[1] * data[10] * data[15] + data[1] * data[11] * data[14] + data[9] * data[2] * data[15] - data[9] * data[3] * data[14] - data[13] * data[2] * data[11] + data[13] * data[3] * data[10];
00245     result.data[2] = data[1] * data[6] * data[15] - data[1] * data[7] * data[14] - data[5] * data[2] * data[15] + data[5] * data[3] * data[14] + data[13] * data[2] * data[7] - data[13] * data[3] * data[6];
00246     result.data[3] = -data[1] * data[6] * data[11] + data[1] * data[7] * data[10] + data[5] * data[2] * data[11] - data[5] * data[3] * data[10] - data[9] * data[2] * data[7] + data[9] * data[3] * data[6];
00247     result.data[4] = -data[4] * data[10] * data[15] + data[4] * data[11] * data[14] + data[8] * data[6] * data[15] - data[8] * data[7] * data[14] - data[12] * data[6] * data[11] + data[12] * data[7] * data[10];
00248     result.data[5] = data[0] * data[10] * data[15] - data[0] * data[11] * data[14] - data[8] * data[2] * data[15] + data[8] * data[3] * data[14] + data[12] * data[2] * data[11] - data[12] * data[3] * data[10];
00249     result.data[6] = -data[0] * data[6] * data[15] + data[0] * data[7] * data[14] + data[4] * data[2] * data[15] - data[4] * data[3] * data[14] - data[12] * data[2] * data[7] + data[12] * data[3] * data[6];
00250     result.data[7] = data[0] * data[6] * data[11] - data[0] * data[7] * data[10] - data[4] * data[2] * data[11] + data[4] * data[3] * data[10] + data[8] * data[2] * data[7] - data[8] * data[3] * data[6];
00251     result.data[8] = data[4] * data[9] * data[15] - data[4] * data[11] * data[13] - data[8] * data[5] * data[15] + data[8] * data[7] * data[13] + data[12] * data[5] * data[11] - data[12] * data[7] * data[9];
00252     result.data[9] = -data[0] * data[9] * data[15] + data[0] * data[11] * data[13] + data[8] * data[1] * data[15] - data[8] * data[3] * data[13] - data[12] * data[1] * data[11] + data[12] * data[3] * data[9];
00253     result.data[10] = data[0] * data[5] * data[15] - data[0] * data[7] * data[13] - data[4] * data[1] * data[15] + data[4] * data[3] * data[13] + data[12] * data[1] * data[7] - data[12] * data[3] * data[5];
00254     result.data[11] = -data[0] * data[5] * data[11] + data[0] * data[7] * data[9] + data[4] * data[1] * data[11] - data[4] * data[3] * data[9] - data[8] * data[1] * data[7] + data[8] * data[3] * data[5];
00255     result.data[12] = -data[4] * data[9] * data[14] + data[4] * data[10] * data[13] + data[8] * data[5] * data[14] - data[8] * data[6] * data[13] - data[12] * data[5] * data[10] + data[12] * data[6] * data[9];
00256     result.data[13] = data[0] * data[9] * data[14] - data[0] * data[10] * data[13] - data[8] * data[1] * data[14] + data[8] * data[2] * data[13] + data[12] * data[1] * data[10] - data[12] * data[2] * data[9];
00257     result.data[14] = -data[0] * data[5] * data[14] + data[0] * data[6] * data[13] + data[4] * data[1] * data[14] - data[4] * data[2] * data[13] - data[12] * data[1] * data[6] + data[12] * data[2] * data[5];
00258     result.data[15] = data[0] * data[5] * data[10] - data[0] * data[6] * data[9] - data[4] * data[1] * data[10] + data[4] * data[2] * data[9] + data[8] * data[1] * data[6] - data[8] * data[2] * data[5];
00259 
00260     float det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12];
00261 
00262     if (det == 0)
00263         return Matrix4(0, 0);
00264 
00265     det = 1.0f / det;
00266 
00267     for (int i = 0; i < 16; i++)
00268         result.data[i] *= det;
00269 
00270     return result;
00271 }
00272 
00273 /**
00274   * Destructor.
00275   *
00276   * Frees any memory consumed by this Matrix4 instance.
00277   */
00278 Matrix4::~Matrix4()
00279 {
00280     if (data != NULL)
00281     {
00282         delete data;
00283         data = NULL;
00284     }
00285 }