Martin Woolley / microbit-dal-eddystone

Eddystone test using modified DAL

Dependencies:   BLE_API mbed-dev-bin nRF51822

Dependents:   microbit-eddystone

Fork of microbit-dal by Lancaster University

source/types/Matrix4.cpp@74:a8f5674a0079, 2017-02-08 (annotated)

Committer:
bluetooth_mdw
Date:
Wed Feb 08 07:49:17 2017 +0000
Revision:
74:a8f5674a0079
Parent:
1:8aa5cdb4ab67 
Eddystone URL test using modified DAL

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan Austin 1:8aa5cdb4ab67 1 /*
Jonathan Austin 1:8aa5cdb4ab67 2 The MIT License (MIT)
Jonathan Austin 1:8aa5cdb4ab67 3
Jonathan Austin 1:8aa5cdb4ab67 4 Copyright (c) 2016 British Broadcasting Corporation.
Jonathan Austin 1:8aa5cdb4ab67 5 This software is provided by Lancaster University by arrangement with the BBC.
Jonathan Austin 1:8aa5cdb4ab67 6
Jonathan Austin 1:8aa5cdb4ab67 7 Permission is hereby granted, free of charge, to any person obtaining a
Jonathan Austin 1:8aa5cdb4ab67 8 copy of this software and associated documentation files (the "Software"),
Jonathan Austin 1:8aa5cdb4ab67 9 to deal in the Software without restriction, including without limitation
Jonathan Austin 1:8aa5cdb4ab67 10 the rights to use, copy, modify, merge, publish, distribute, sublicense,
Jonathan Austin 1:8aa5cdb4ab67 11 and/or sell copies of the Software, and to permit persons to whom the
Jonathan Austin 1:8aa5cdb4ab67 12 Software is furnished to do so, subject to the following conditions:
Jonathan Austin 1:8aa5cdb4ab67 13
Jonathan Austin 1:8aa5cdb4ab67 14 The above copyright notice and this permission notice shall be included in
Jonathan Austin 1:8aa5cdb4ab67 15 all copies or substantial portions of the Software.
Jonathan Austin 1:8aa5cdb4ab67 16
Jonathan Austin 1:8aa5cdb4ab67 17 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Jonathan Austin 1:8aa5cdb4ab67 18 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Jonathan Austin 1:8aa5cdb4ab67 19 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
Jonathan Austin 1:8aa5cdb4ab67 20 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Jonathan Austin 1:8aa5cdb4ab67 21 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
Jonathan Austin 1:8aa5cdb4ab67 22 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
Jonathan Austin 1:8aa5cdb4ab67 23 DEALINGS IN THE SOFTWARE.
Jonathan Austin 1:8aa5cdb4ab67 24 */
Jonathan Austin 1:8aa5cdb4ab67 25
Jonathan Austin 1:8aa5cdb4ab67 26 #include "MicroBitConfig.h"
Jonathan Austin 1:8aa5cdb4ab67 27 #include "Matrix4.h"
Jonathan Austin 1:8aa5cdb4ab67 28 #include "mbed.h"
Jonathan Austin 1:8aa5cdb4ab67 29
Jonathan Austin 1:8aa5cdb4ab67 30 /**
Jonathan Austin 1:8aa5cdb4ab67 31 * Class definition for a simple matrix, optimised for n x 4 or 4 x n matrices.
Jonathan Austin 1:8aa5cdb4ab67 32 *
Jonathan Austin 1:8aa5cdb4ab67 33 * This class is heavily optimised for these commonly used matrices as used in 3D geometry,
Jonathan Austin 1:8aa5cdb4ab67 34 * and is not intended as a general purpose matrix class. For programmers needing more flexible
Jonathan Austin 1:8aa5cdb4ab67 35 * Matrix support, the mbed Matrix and MatrixMath classes from Ernsesto Palacios provide a good basis:
Jonathan Austin 1:8aa5cdb4ab67 36 *
Jonathan Austin 1:8aa5cdb4ab67 37 * https://developer.mbed.org/cookbook/MatrixClass
Jonathan Austin 1:8aa5cdb4ab67 38 * https://developer.mbed.org/users/Yo_Robot/code/MatrixMath/
Jonathan Austin 1:8aa5cdb4ab67 39 */
Jonathan Austin 1:8aa5cdb4ab67 40
Jonathan Austin 1:8aa5cdb4ab67 41 /**
Jonathan Austin 1:8aa5cdb4ab67 42 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 43 * Create a matrix of the given size.
Jonathan Austin 1:8aa5cdb4ab67 44 *
Jonathan Austin 1:8aa5cdb4ab67 45 * @param rows the number of rows in the matrix to be created.
Jonathan Austin 1:8aa5cdb4ab67 46 *
Jonathan Austin 1:8aa5cdb4ab67 47 * @param cols the number of columns in the matrix to be created.
Jonathan Austin 1:8aa5cdb4ab67 48 *
Jonathan Austin 1:8aa5cdb4ab67 49 * @code
Jonathan Austin 1:8aa5cdb4ab67 50 * Matrix4(10, 4); // Creates a Matrix with 10 rows and 4 columns.
Jonathan Austin 1:8aa5cdb4ab67 51 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 52 */
Jonathan Austin 1:8aa5cdb4ab67 53 Matrix4::Matrix4(int rows, int cols)
Jonathan Austin 1:8aa5cdb4ab67 54 {
Jonathan Austin 1:8aa5cdb4ab67 55 this->rows = rows;
Jonathan Austin 1:8aa5cdb4ab67 56 this->cols = cols;
Jonathan Austin 1:8aa5cdb4ab67 57
Jonathan Austin 1:8aa5cdb4ab67 58 int size = rows * cols;
Jonathan Austin 1:8aa5cdb4ab67 59
Jonathan Austin 1:8aa5cdb4ab67 60 if (size > 0)
Jonathan Austin 1:8aa5cdb4ab67 61 data = new float[size];
Jonathan Austin 1:8aa5cdb4ab67 62 else
Jonathan Austin 1:8aa5cdb4ab67 63 data = NULL;
Jonathan Austin 1:8aa5cdb4ab67 64 }
Jonathan Austin 1:8aa5cdb4ab67 65
Jonathan Austin 1:8aa5cdb4ab67 66 /**
Jonathan Austin 1:8aa5cdb4ab67 67 * Constructor.
Jonathan Austin 1:8aa5cdb4ab67 68 * Create a matrix that is an identical copy of the given matrix.
Jonathan Austin 1:8aa5cdb4ab67 69 *
Jonathan Austin 1:8aa5cdb4ab67 70 * @param matrix The matrix to copy.
Jonathan Austin 1:8aa5cdb4ab67 71 *
Jonathan Austin 1:8aa5cdb4ab67 72 * @code
Jonathan Austin 1:8aa5cdb4ab67 73 * Matrix newMatrix(matrix); .
Jonathan Austin 1:8aa5cdb4ab67 74 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 75 */
Jonathan Austin 1:8aa5cdb4ab67 76 Matrix4::Matrix4(const Matrix4 &matrix)
Jonathan Austin 1:8aa5cdb4ab67 77 {
Jonathan Austin 1:8aa5cdb4ab67 78 this->rows = matrix.rows;
Jonathan Austin 1:8aa5cdb4ab67 79 this->cols = matrix.cols;
Jonathan Austin 1:8aa5cdb4ab67 80
Jonathan Austin 1:8aa5cdb4ab67 81 int size = rows * cols;
Jonathan Austin 1:8aa5cdb4ab67 82
Jonathan Austin 1:8aa5cdb4ab67 83 if (size > 0)
Jonathan Austin 1:8aa5cdb4ab67 84 {
Jonathan Austin 1:8aa5cdb4ab67 85 data = new float[size];
Jonathan Austin 1:8aa5cdb4ab67 86 for (int i = 0; i < size; i++)
Jonathan Austin 1:8aa5cdb4ab67 87 data[i] = matrix.data[i];
Jonathan Austin 1:8aa5cdb4ab67 88 }
Jonathan Austin 1:8aa5cdb4ab67 89 else
Jonathan Austin 1:8aa5cdb4ab67 90 {
Jonathan Austin 1:8aa5cdb4ab67 91 data = NULL;
Jonathan Austin 1:8aa5cdb4ab67 92 }
Jonathan Austin 1:8aa5cdb4ab67 93
Jonathan Austin 1:8aa5cdb4ab67 94 }
Jonathan Austin 1:8aa5cdb4ab67 95
Jonathan Austin 1:8aa5cdb4ab67 96 /**
Jonathan Austin 1:8aa5cdb4ab67 97 * Determines the number of columns in this matrix.
Jonathan Austin 1:8aa5cdb4ab67 98 *
Jonathan Austin 1:8aa5cdb4ab67 99 * @return The number of columns in the matrix.
Jonathan Austin 1:8aa5cdb4ab67 100 *
Jonathan Austin 1:8aa5cdb4ab67 101 * @code
Jonathan Austin 1:8aa5cdb4ab67 102 * int c = matrix.width();
Jonathan Austin 1:8aa5cdb4ab67 103 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 104 */
Jonathan Austin 1:8aa5cdb4ab67 105 int Matrix4::width()
Jonathan Austin 1:8aa5cdb4ab67 106 {
Jonathan Austin 1:8aa5cdb4ab67 107 return cols;
Jonathan Austin 1:8aa5cdb4ab67 108 }
Jonathan Austin 1:8aa5cdb4ab67 109
Jonathan Austin 1:8aa5cdb4ab67 110 /**
Jonathan Austin 1:8aa5cdb4ab67 111 * Determines the number of rows in this matrix.
Jonathan Austin 1:8aa5cdb4ab67 112 *
Jonathan Austin 1:8aa5cdb4ab67 113 * @return The number of rows in the matrix.
Jonathan Austin 1:8aa5cdb4ab67 114 *
Jonathan Austin 1:8aa5cdb4ab67 115 * @code
Jonathan Austin 1:8aa5cdb4ab67 116 * int r = matrix.height();
Jonathan Austin 1:8aa5cdb4ab67 117 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 118 */
Jonathan Austin 1:8aa5cdb4ab67 119 int Matrix4::height()
Jonathan Austin 1:8aa5cdb4ab67 120 {
Jonathan Austin 1:8aa5cdb4ab67 121 return rows;
Jonathan Austin 1:8aa5cdb4ab67 122 }
Jonathan Austin 1:8aa5cdb4ab67 123
Jonathan Austin 1:8aa5cdb4ab67 124 /**
Jonathan Austin 1:8aa5cdb4ab67 125 * Reads the matrix element at the given position.
Jonathan Austin 1:8aa5cdb4ab67 126 *
Jonathan Austin 1:8aa5cdb4ab67 127 * @param row The row of the element to read.
Jonathan Austin 1:8aa5cdb4ab67 128 *
Jonathan Austin 1:8aa5cdb4ab67 129 * @param col The column of the element to read.
Jonathan Austin 1:8aa5cdb4ab67 130 *
Jonathan Austin 1:8aa5cdb4ab67 131 * @return The value of the matrix element at the given position. 0 is returned if the given index is out of range.
Jonathan Austin 1:8aa5cdb4ab67 132 *
Jonathan Austin 1:8aa5cdb4ab67 133 * @code
Jonathan Austin 1:8aa5cdb4ab67 134 * float v = matrix.get(1,2);
Jonathan Austin 1:8aa5cdb4ab67 135 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 136 */
Jonathan Austin 1:8aa5cdb4ab67 137 float Matrix4::get(int row, int col)
Jonathan Austin 1:8aa5cdb4ab67 138 {
Jonathan Austin 1:8aa5cdb4ab67 139 if (row < 0 || col < 0 || row >= rows || col >= cols)
Jonathan Austin 1:8aa5cdb4ab67 140 return 0;
Jonathan Austin 1:8aa5cdb4ab67 141
Jonathan Austin 1:8aa5cdb4ab67 142 return data[width() * row + col];
Jonathan Austin 1:8aa5cdb4ab67 143 }
Jonathan Austin 1:8aa5cdb4ab67 144
Jonathan Austin 1:8aa5cdb4ab67 145 /**
Jonathan Austin 1:8aa5cdb4ab67 146 * Writes the matrix element at the given position.
Jonathan Austin 1:8aa5cdb4ab67 147 *
Jonathan Austin 1:8aa5cdb4ab67 148 * @param row The row of the element to write.
Jonathan Austin 1:8aa5cdb4ab67 149 *
Jonathan Austin 1:8aa5cdb4ab67 150 * @param col The column of the element to write.
Jonathan Austin 1:8aa5cdb4ab67 151 *
Jonathan Austin 1:8aa5cdb4ab67 152 * @param v The new value of the element.
Jonathan Austin 1:8aa5cdb4ab67 153 *
Jonathan Austin 1:8aa5cdb4ab67 154 * @code
Jonathan Austin 1:8aa5cdb4ab67 155 * matrix.set(1,2,42.0);
Jonathan Austin 1:8aa5cdb4ab67 156 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 157 */
Jonathan Austin 1:8aa5cdb4ab67 158 void Matrix4::set(int row, int col, float v)
Jonathan Austin 1:8aa5cdb4ab67 159 {
Jonathan Austin 1:8aa5cdb4ab67 160 if (row < 0 || col < 0 || row >= rows || col >= cols)
Jonathan Austin 1:8aa5cdb4ab67 161 return;
Jonathan Austin 1:8aa5cdb4ab67 162
Jonathan Austin 1:8aa5cdb4ab67 163 data[width() * row + col] = v;
Jonathan Austin 1:8aa5cdb4ab67 164 }
Jonathan Austin 1:8aa5cdb4ab67 165
Jonathan Austin 1:8aa5cdb4ab67 166 /**
Jonathan Austin 1:8aa5cdb4ab67 167 * Transposes this matrix.
Jonathan Austin 1:8aa5cdb4ab67 168 *
Jonathan Austin 1:8aa5cdb4ab67 169 * @return the resultant matrix.
Jonathan Austin 1:8aa5cdb4ab67 170 *
Jonathan Austin 1:8aa5cdb4ab67 171 * @code
Jonathan Austin 1:8aa5cdb4ab67 172 * matrix.transpose();
Jonathan Austin 1:8aa5cdb4ab67 173 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 174 */
Jonathan Austin 1:8aa5cdb4ab67 175 Matrix4 Matrix4::transpose()
Jonathan Austin 1:8aa5cdb4ab67 176 {
Jonathan Austin 1:8aa5cdb4ab67 177 Matrix4 result = Matrix4(cols, rows);
Jonathan Austin 1:8aa5cdb4ab67 178
Jonathan Austin 1:8aa5cdb4ab67 179 for (int i = 0; i < width(); i++)
Jonathan Austin 1:8aa5cdb4ab67 180 for (int j = 0; j < height(); j++)
Jonathan Austin 1:8aa5cdb4ab67 181 result.set(i, j, get(j, i));
Jonathan Austin 1:8aa5cdb4ab67 182
Jonathan Austin 1:8aa5cdb4ab67 183 return result;
Jonathan Austin 1:8aa5cdb4ab67 184 }
Jonathan Austin 1:8aa5cdb4ab67 185
Jonathan Austin 1:8aa5cdb4ab67 186 /**
Jonathan Austin 1:8aa5cdb4ab67 187 * Multiplies this matrix with the given matrix (if possible).
Jonathan Austin 1:8aa5cdb4ab67 188 *
Jonathan Austin 1:8aa5cdb4ab67 189 * @param matrix the matrix to multiply this matrix's values against.
Jonathan Austin 1:8aa5cdb4ab67 190 *
Jonathan Austin 1:8aa5cdb4ab67 191 * @param transpose Transpose the matrices before multiplication. Defaults to false.
Jonathan Austin 1:8aa5cdb4ab67 192 *
Jonathan Austin 1:8aa5cdb4ab67 193 * @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
Jonathan Austin 1:8aa5cdb4ab67 194 *
Jonathan Austin 1:8aa5cdb4ab67 195 * @code
Jonathan Austin 1:8aa5cdb4ab67 196 * Matrix result = matrixA.multiply(matrixB);
Jonathan Austin 1:8aa5cdb4ab67 197 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 198 */
Jonathan Austin 1:8aa5cdb4ab67 199 Matrix4 Matrix4::multiply(Matrix4 &matrix, bool transpose)
Jonathan Austin 1:8aa5cdb4ab67 200 {
Jonathan Austin 1:8aa5cdb4ab67 201 int w = transpose ? height() : width();
Jonathan Austin 1:8aa5cdb4ab67 202 int h = transpose ? width() : height();
Jonathan Austin 1:8aa5cdb4ab67 203
Jonathan Austin 1:8aa5cdb4ab67 204 if (w != matrix.height())
Jonathan Austin 1:8aa5cdb4ab67 205 return Matrix4(0, 0);
Jonathan Austin 1:8aa5cdb4ab67 206
Jonathan Austin 1:8aa5cdb4ab67 207 Matrix4 result(h, matrix.width());
Jonathan Austin 1:8aa5cdb4ab67 208
Jonathan Austin 1:8aa5cdb4ab67 209 for (int r = 0; r < result.height(); r++)
Jonathan Austin 1:8aa5cdb4ab67 210 {
Jonathan Austin 1:8aa5cdb4ab67 211 for (int c = 0; c < result.width(); c++)
Jonathan Austin 1:8aa5cdb4ab67 212 {
Jonathan Austin 1:8aa5cdb4ab67 213 float v = 0.0;
Jonathan Austin 1:8aa5cdb4ab67 214
Jonathan Austin 1:8aa5cdb4ab67 215 for (int i = 0; i < w; i++)
Jonathan Austin 1:8aa5cdb4ab67 216 v += (transpose ? get(i, r) : get(r, i)) * matrix.get(i, c);
Jonathan Austin 1:8aa5cdb4ab67 217
Jonathan Austin 1:8aa5cdb4ab67 218 result.set(r, c, v);
Jonathan Austin 1:8aa5cdb4ab67 219 }
Jonathan Austin 1:8aa5cdb4ab67 220 }
Jonathan Austin 1:8aa5cdb4ab67 221
Jonathan Austin 1:8aa5cdb4ab67 222 return result;
Jonathan Austin 1:8aa5cdb4ab67 223 }
Jonathan Austin 1:8aa5cdb4ab67 224
Jonathan Austin 1:8aa5cdb4ab67 225 /**
Jonathan Austin 1:8aa5cdb4ab67 226 * Performs an optimised inversion of a 4x4 matrix.
Jonathan Austin 1:8aa5cdb4ab67 227 * Only 4x4 matrices are supported by this operation.
Jonathan Austin 1:8aa5cdb4ab67 228 *
Jonathan Austin 1:8aa5cdb4ab67 229 * @return the resultant matrix. An empty matrix is returned if the operation canot be completed.
Jonathan Austin 1:8aa5cdb4ab67 230 *
Jonathan Austin 1:8aa5cdb4ab67 231 * @code
Jonathan Austin 1:8aa5cdb4ab67 232 * Matrix result = matrixA.invert();
Jonathan Austin 1:8aa5cdb4ab67 233 * @endcode
Jonathan Austin 1:8aa5cdb4ab67 234 */
Jonathan Austin 1:8aa5cdb4ab67 235 Matrix4 Matrix4::invert()
Jonathan Austin 1:8aa5cdb4ab67 236 {
Jonathan Austin 1:8aa5cdb4ab67 237 // We only support square matrices of size 4...
Jonathan Austin 1:8aa5cdb4ab67 238 if (width() != height() || width() != 4)
Jonathan Austin 1:8aa5cdb4ab67 239 return Matrix4(0, 0);
Jonathan Austin 1:8aa5cdb4ab67 240
Jonathan Austin 1:8aa5cdb4ab67 241 Matrix4 result(width(), height());
Jonathan Austin 1:8aa5cdb4ab67 242
Jonathan Austin 1:8aa5cdb4ab67 243 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];
Jonathan Austin 1:8aa5cdb4ab67 244 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];
Jonathan Austin 1:8aa5cdb4ab67 245 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];
Jonathan Austin 1:8aa5cdb4ab67 246 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];
Jonathan Austin 1:8aa5cdb4ab67 247 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];
Jonathan Austin 1:8aa5cdb4ab67 248 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];
Jonathan Austin 1:8aa5cdb4ab67 249 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];
Jonathan Austin 1:8aa5cdb4ab67 250 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];
Jonathan Austin 1:8aa5cdb4ab67 251 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];
Jonathan Austin 1:8aa5cdb4ab67 252 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];
Jonathan Austin 1:8aa5cdb4ab67 253 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];
Jonathan Austin 1:8aa5cdb4ab67 254 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];
Jonathan Austin 1:8aa5cdb4ab67 255 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];
Jonathan Austin 1:8aa5cdb4ab67 256 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];
Jonathan Austin 1:8aa5cdb4ab67 257 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];
Jonathan Austin 1:8aa5cdb4ab67 258 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];
Jonathan Austin 1:8aa5cdb4ab67 259
Jonathan Austin 1:8aa5cdb4ab67 260 float det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12];
Jonathan Austin 1:8aa5cdb4ab67 261
Jonathan Austin 1:8aa5cdb4ab67 262 if (det == 0)
Jonathan Austin 1:8aa5cdb4ab67 263 return Matrix4(0, 0);
Jonathan Austin 1:8aa5cdb4ab67 264
Jonathan Austin 1:8aa5cdb4ab67 265 det = 1.0f / det;
Jonathan Austin 1:8aa5cdb4ab67 266
Jonathan Austin 1:8aa5cdb4ab67 267 for (int i = 0; i < 16; i++)
Jonathan Austin 1:8aa5cdb4ab67 268 result.data[i] *= det;
Jonathan Austin 1:8aa5cdb4ab67 269
Jonathan Austin 1:8aa5cdb4ab67 270 return result;
Jonathan Austin 1:8aa5cdb4ab67 271 }
Jonathan Austin 1:8aa5cdb4ab67 272
Jonathan Austin 1:8aa5cdb4ab67 273 /**
Jonathan Austin 1:8aa5cdb4ab67 274 * Destructor.
Jonathan Austin 1:8aa5cdb4ab67 275 *
Jonathan Austin 1:8aa5cdb4ab67 276 * Frees any memory consumed by this Matrix4 instance.
Jonathan Austin 1:8aa5cdb4ab67 277 */
Jonathan Austin 1:8aa5cdb4ab67 278 Matrix4::~Matrix4()
Jonathan Austin 1:8aa5cdb4ab67 279 {
Jonathan Austin 1:8aa5cdb4ab67 280 if (data != NULL)
Jonathan Austin 1:8aa5cdb4ab67 281 {
Jonathan Austin 1:8aa5cdb4ab67 282 delete data;
Jonathan Austin 1:8aa5cdb4ab67 283 data = NULL;
Jonathan Austin 1:8aa5cdb4ab67 284 }
Jonathan Austin 1:8aa5cdb4ab67 285 }