Naoto Morita / MatrixMath

add MatrixCross

MatrixMath.h@7:b22d56ac37aa, 2021-08-10 (annotated)

Committer:
NaotoMorita
Date:
Tue Aug 10 08:08:11 2021 +0000
Revision:
7:b22d56ac37aa
Parent:
6:aa5e94cddb3f 
add Vector3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yo_Robot 3:48754fe86e08 1 /**
Yo_Robot 5:93948a9bbde2 2 * @brief Version 0.9
Yo_Robot 3:48754fe86e08 3 * @file MatrixMath.h
Yo_Robot 3:48754fe86e08 4 * @author Ernesto Palacios
Yo_Robot 3:48754fe86e08 5 *
Yo_Robot 3:48754fe86e08 6 * Created on 15 de septiembre de 2011, 09:44 AM.
Yo_Robot 3:48754fe86e08 7 *
Yo_Robot 5:93948a9bbde2 8 * Develop Under GPL v3.0 License
Yo_Robot 5:93948a9bbde2 9 * http://www.gnu.org/licenses/gpl-3.0.html
Yo_Robot 3:48754fe86e08 10 *
Yo_Robot 3:48754fe86e08 11 */
Yo_Robot 3:48754fe86e08 12
Yo_Robot 5:93948a9bbde2 13 #ifndef MATRIXMATH_H
Yo_Robot 5:93948a9bbde2 14 #define MATRIXMATH_H
Yo_Robot 3:48754fe86e08 15
Yo_Robot 3:48754fe86e08 16 #include "mbed.h"
Yo_Robot 3:48754fe86e08 17 #include "Matrix.h"
NaotoMorita 7:b22d56ac37aa 18 #include "Vector3.hpp"
Yo_Robot 3:48754fe86e08 19
Yo_Robot 3:48754fe86e08 20 /**
Yo_Robot 5:93948a9bbde2 21 * @brief This class provides STATIC methods to perform operations
Yo_Robot 5:93948a9bbde2 22 * over Matrix Objects, version 0.9.
Yo_Robot 3:48754fe86e08 23 */
Yo_Robot 3:48754fe86e08 24 class MatrixMath{
Yo_Robot 3:48754fe86e08 25 public:
Yo_Robot 3:48754fe86e08 26
Yo_Robot 3:48754fe86e08 27
Yo_Robot 3:48754fe86e08 28 /**@brief
Yo_Robot 3:48754fe86e08 29 * Transposes Matrix, return new Object.
Yo_Robot 3:48754fe86e08 30 * @param Mat matrix to calculate
Yo_Robot 5:93948a9bbde2 31 * @return Transposed Matrix
Yo_Robot 3:48754fe86e08 32 */
Yo_Robot 3:48754fe86e08 33 static Matrix Transpose( const Matrix& Mat );
Yo_Robot 3:48754fe86e08 34
Yo_Robot 3:48754fe86e08 35
Yo_Robot 3:48754fe86e08 36 /**@brief
Yo_Robot 5:93948a9bbde2 37 * Calculate the inverse of a [n,n] Matrix BUT you check first if
Yo_Robot 5:93948a9bbde2 38 * the determinant is != 0, Same matrix will be return if Det( Mat ) == 0.
Yo_Robot 3:48754fe86e08 39 * @param Mat matrix to calcute inverse.
Yo_Robot 3:48754fe86e08 40 * @return Matrix Inverse
Yo_Robot 3:48754fe86e08 41 */
NaotoMorita 7:b22d56ac37aa 42
NaotoMorita 7:b22d56ac37aa 43 static Matrix Matrixcross(const float px, const float py, const float pz);
NaotoMorita 7:b22d56ac37aa 44 /**@brief
NaotoMorita 7:b22d56ac37aa 45 * 外積を計算するためのMatrixを計算する
NaotoMorita 7:b22d56ac37aa 46 */
NaotoMorita 7:b22d56ac37aa 47
NaotoMorita 7:b22d56ac37aa 48 static Matrix Vector2mat(const Vector3 vec);
NaotoMorita 7:b22d56ac37aa 49 /**@brief
NaotoMorita 7:b22d56ac37aa 50 * 外積を計算するためのMatrixを計算する
NaotoMorita 7:b22d56ac37aa 51 */
NaotoMorita 7:b22d56ac37aa 52
Yo_Robot 3:48754fe86e08 53 static Matrix Inv( const Matrix& Mat );
Yo_Robot 3:48754fe86e08 54
Yo_Robot 3:48754fe86e08 55
Yo_Robot 3:48754fe86e08 56 /**@brief
Yo_Robot 5:93948a9bbde2 57 * Creates an identity [n,n] Matrix
Yo_Robot 3:48754fe86e08 58 * @param Rows Number of Rowns and Columns
Yo_Robot 5:93948a9bbde2 59 * @return Identity Matrix of dimensions [Rows,Rows]
Yo_Robot 3:48754fe86e08 60 */
Yo_Robot 3:48754fe86e08 61 static Matrix Eye( int Rows );
Yo_Robot 5:93948a9bbde2 62
Yo_Robot 3:48754fe86e08 63
Yo_Robot 3:48754fe86e08 64 /**@brief
Yo_Robot 3:48754fe86e08 65 * Returns the dot Product of any two same leght vectors.
Yo_Robot 5:93948a9bbde2 66 * In this case a vector is defined as a [1,n] or [n,1] Matrix.
Yo_Robot 5:93948a9bbde2 67 * Very Flexible Function.
Yo_Robot 3:48754fe86e08 68 * @param leftM First Vector
Yo_Robot 3:48754fe86e08 69 * @param rightM Second Vector
Yo_Robot 3:48754fe86e08 70 * @return Dot Product or Scalar Product.
Yo_Robot 3:48754fe86e08 71 */
Yo_Robot 3:48754fe86e08 72 static float dot( const Matrix& leftM, const Matrix& rightM );
Yo_Robot 5:93948a9bbde2 73
Yo_Robot 3:48754fe86e08 74
Yo_Robot 3:48754fe86e08 75 /**@brief Calculates the determinant of a Matrix.
Yo_Robot 3:48754fe86e08 76 * @param Mat matrix to calculate.
Yo_Robot 3:48754fe86e08 77 * @return the determinant.
Yo_Robot 3:48754fe86e08 78 */
Yo_Robot 3:48754fe86e08 79 static float det( const Matrix& Mat );
Yo_Robot 3:48754fe86e08 80
mori3rti 6:aa5e94cddb3f 81 /**@brief Calculates Kronecker product of two Matrix.
mori3rti 6:aa5e94cddb3f 82 * Kronecker product is an operation on two matrices of arbitrary
mori3rti 6:aa5e94cddb3f 83 * size resulting in a block matrix. If A is an m × n matrix
mori3rti 6:aa5e94cddb3f 84 * and B is a p × q matrix, then the Kronecker product A ⊗ B is
mori3rti 6:aa5e94cddb3f 85 * the pm × qn block matrix:
mori3rti 6:aa5e94cddb3f 86 * - -
mori3rti 6:aa5e94cddb3f 87 * | a_11 B ... a_1n B |
mori3rti 6:aa5e94cddb3f 88 * A ⊗ B = | ... ... ... |
mori3rti 6:aa5e94cddb3f 89 * | a_m1 B ... a_mn B |
mori3rti 6:aa5e94cddb3f 90 * - -
mori3rti 6:aa5e94cddb3f 91 * @param Mat_A Matrix m × n
mori3rti 6:aa5e94cddb3f 92 * @param Mat_B Matrix p × q
mori3rti 6:aa5e94cddb3f 93 * @return Kron Matrix pm × qn.
mori3rti 6:aa5e94cddb3f 94 */
mori3rti 6:aa5e94cddb3f 95 static Matrix kron(const Matrix& Mat_A, const Matrix& Mat_B);
mori3rti 6:aa5e94cddb3f 96
Yo_Robot 3:48754fe86e08 97
Yo_Robot 3:48754fe86e08 98 //==== For Kinematics ====//
Yo_Robot 3:48754fe86e08 99
Yo_Robot 3:48754fe86e08 100 /**@brief
Yo_Robot 3:48754fe86e08 101 * Calculates the Rotation Matrix Transform along 'x' axis in radians.
Yo_Robot 3:48754fe86e08 102 * @param radians rotation angle.
Yo_Robot 3:48754fe86e08 103 * @return Rotation Matrix[4,4] along 'x' axis.
Yo_Robot 3:48754fe86e08 104 */
Yo_Robot 3:48754fe86e08 105 static Matrix RotX( float radians );
Yo_Robot 3:48754fe86e08 106
Yo_Robot 3:48754fe86e08 107
Yo_Robot 3:48754fe86e08 108 /**@brief
Yo_Robot 3:48754fe86e08 109 * Calculates the Rotation Matrix Transform along 'y' axis in radians.
Yo_Robot 3:48754fe86e08 110 * @param radians rotation angle.
Yo_Robot 3:48754fe86e08 111 * @return Rotation Matrix[4,4] along 'y' axis.
Yo_Robot 3:48754fe86e08 112 */
Yo_Robot 3:48754fe86e08 113 static Matrix RotY( float radians );
Yo_Robot 3:48754fe86e08 114
Yo_Robot 3:48754fe86e08 115
Yo_Robot 3:48754fe86e08 116 /**@brief
Yo_Robot 5:93948a9bbde2 117 * Calculates the Rotation Matrix Transform along 'z' axis in radians.
Yo_Robot 3:48754fe86e08 118 * @param radians rotation angle.
Yo_Robot 5:93948a9bbde2 119 * @return Rotation Matrix[4,4] along 'z' axis.
Yo_Robot 3:48754fe86e08 120 */
Yo_Robot 3:48754fe86e08 121 static Matrix RotZ( float radians );
Yo_Robot 3:48754fe86e08 122
Yo_Robot 3:48754fe86e08 123 /**@brief
Yo_Robot 5:93948a9bbde2 124 * Calculates the Translation Matrix to coordenates (x' y' z').
Yo_Robot 3:48754fe86e08 125 * @param x axis translation
Yo_Robot 3:48754fe86e08 126 * @param y axis translation
Yo_Robot 3:48754fe86e08 127 * @param z axis translation
Yo_Robot 3:48754fe86e08 128 * @return Translation Matrix[4,4] x'y'z'.
Yo_Robot 3:48754fe86e08 129 */
Yo_Robot 3:48754fe86e08 130 static Matrix Transl( float x, float y, float z );
Yo_Robot 3:48754fe86e08 131
Yo_Robot 3:48754fe86e08 132 private:
Yo_Robot 3:48754fe86e08 133
Yo_Robot 3:48754fe86e08 134 /**@brief
Yo_Robot 3:48754fe86e08 135 * Calculates the Determinant of a 3x3 Matrix
Yo_Robot 3:48754fe86e08 136 * @param Mat Already made sure is a 3 by 3
Yo_Robot 3:48754fe86e08 137 * @return Float, determinant.
Yo_Robot 3:48754fe86e08 138 */
Yo_Robot 3:48754fe86e08 139 float Det3x3( const Matrix& Mat );
Yo_Robot 3:48754fe86e08 140
Yo_Robot 3:48754fe86e08 141 };
Yo_Robot 3:48754fe86e08 142
Yo_Robot 3:48754fe86e08 143 #endif /* MATRIXMATH_H */