Solution for Bluetooth SIG hands-on training course
Dependencies: BLE_API mbed-dev-bin nRF51822-bluetooth-mdw
Fork of microbit-dal-bluetooth-mdw_starter by
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 }
Generated on Tue Jul 12 2022 20:39:13 by 1.7.2