Revised to disable BLE for radio communication as needed.
Dependencies: BLE_API nRF51822 mbed-dev-bin
Diff: source/types/Matrix4.cpp
- Revision:
- 1:8aa5cdb4ab67
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/source/types/Matrix4.cpp Thu Apr 07 01:33:22 2016 +0100 @@ -0,0 +1,285 @@ +/* +The MIT License (MIT) + +Copyright (c) 2016 British Broadcasting Corporation. +This software is provided by Lancaster University by arrangement with the BBC. + +Permission is hereby granted, free of charge, to any person obtaining a +copy of this software and associated documentation files (the "Software"), +to deal in the Software without restriction, including without limitation +the rights to use, copy, modify, merge, publish, distribute, sublicense, +and/or sell copies of the Software, and to permit persons to whom the +Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. +*/ + +#include "MicroBitConfig.h" +#include "Matrix4.h" +#include "mbed.h" + +/** +* Class definition for a simple matrix, optimised for n x 4 or 4 x n matrices. +* +* This class is heavily optimised for these commonly used matrices as used in 3D geometry, +* and is not intended as a general purpose matrix class. For programmers needing more flexible +* Matrix support, the mbed Matrix and MatrixMath classes from Ernsesto Palacios provide a good basis: +* +* https://developer.mbed.org/cookbook/MatrixClass +* https://developer.mbed.org/users/Yo_Robot/code/MatrixMath/ +*/ + +/** + * Constructor. + * Create a matrix of the given size. + * + * @param rows the number of rows in the matrix to be created. + * + * @param cols the number of columns in the matrix to be created. + * + * @code + * Matrix4(10, 4); // Creates a Matrix with 10 rows and 4 columns. + * @endcode + */ +Matrix4::Matrix4(int rows, int cols) +{ + this->rows = rows; + this->cols = cols; + + int size = rows * cols; + + if (size > 0) + data = new float[size]; + else + data = NULL; +} + +/** + * Constructor. + * Create a matrix that is an identical copy of the given matrix. + * + * @param matrix The matrix to copy. + * + * @code + * Matrix newMatrix(matrix); . + * @endcode + */ +Matrix4::Matrix4(const Matrix4 &matrix) +{ + this->rows = matrix.rows; + this->cols = matrix.cols; + + int size = rows * cols; + + if (size > 0) + { + data = new float[size]; + for (int i = 0; i < size; i++) + data[i] = matrix.data[i]; + } + else + { + data = NULL; + } + +} + +/** + * Determines the number of columns in this matrix. + * + * @return The number of columns in the matrix. + * + * @code + * int c = matrix.width(); + * @endcode + */ +int Matrix4::width() +{ + return cols; +} + +/** + * Determines the number of rows in this matrix. + * + * @return The number of rows in the matrix. + * + * @code + * int r = matrix.height(); + * @endcode + */ +int Matrix4::height() +{ + return rows; +} + +/** + * Reads the matrix element at the given position. + * + * @param row The row of the element to read. + * + * @param col The column of the element to read. + * + * @return The value of the matrix element at the given position. 0 is returned if the given index is out of range. + * + * @code + * float v = matrix.get(1,2); + * @endcode + */ +float Matrix4::get(int row, int col) +{ + if (row < 0 || col < 0 || row >= rows || col >= cols) + return 0; + + return data[width() * row + col]; +} + +/** + * Writes the matrix element at the given position. + * + * @param row The row of the element to write. + * + * @param col The column of the element to write. + * + * @param v The new value of the element. + * + * @code + * matrix.set(1,2,42.0); + * @endcode + */ +void Matrix4::set(int row, int col, float v) +{ + if (row < 0 || col < 0 || row >= rows || col >= cols) + return; + + data[width() * row + col] = v; +} + +/** + * Transposes this matrix. + * + * @return the resultant matrix. + * + * @code + * matrix.transpose(); + * @endcode + */ +Matrix4 Matrix4::transpose() +{ + Matrix4 result = Matrix4(cols, rows); + + for (int i = 0; i < width(); i++) + for (int j = 0; j < height(); j++) + result.set(i, j, get(j, i)); + + return result; +} + +/** + * Multiplies this matrix with the given matrix (if possible). + * + * @param matrix the matrix to multiply this matrix's values against. + * + * @param transpose Transpose the matrices before multiplication. Defaults to false. + * + * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. + * + * @code + * Matrix result = matrixA.multiply(matrixB); + * @endcode + */ +Matrix4 Matrix4::multiply(Matrix4 &matrix, bool transpose) +{ + int w = transpose ? height() : width(); + int h = transpose ? width() : height(); + + if (w != matrix.height()) + return Matrix4(0, 0); + + Matrix4 result(h, matrix.width()); + + for (int r = 0; r < result.height(); r++) + { + for (int c = 0; c < result.width(); c++) + { + float v = 0.0; + + for (int i = 0; i < w; i++) + v += (transpose ? get(i, r) : get(r, i)) * matrix.get(i, c); + + result.set(r, c, v); + } + } + + return result; +} + +/** + * Performs an optimised inversion of a 4x4 matrix. + * Only 4x4 matrices are supported by this operation. + * + * @return the resultant matrix. An empty matrix is returned if the operation canot be completed. + * + * @code + * Matrix result = matrixA.invert(); + * @endcode + */ +Matrix4 Matrix4::invert() +{ + // We only support square matrices of size 4... + if (width() != height() || width() != 4) + return Matrix4(0, 0); + + Matrix4 result(width(), height()); + + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + 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]; + + float det = data[0] * result.data[0] + data[1] * result.data[4] + data[2] * result.data[8] + data[3] * result.data[12]; + + if (det == 0) + return Matrix4(0, 0); + + det = 1.0f / det; + + for (int i = 0; i < 16; i++) + result.data[i] *= det; + + return result; +} + +/** + * Destructor. + * + * Frees any memory consumed by this Matrix4 instance. + */ +Matrix4::~Matrix4() +{ + if (data != NULL) + { + delete data; + data = NULL; + } +}