Andrew SaLoutos / MatrixMath

This class provides with operations for Matrix Objects + some changes

MatrixMath_Kinematics.cpp@6:aa5e94cddb3f, 2020-09-17 (annotated)

Committer:
mori3rti
Date:
Thu Sep 17 17:51:22 2020 +0700
Revision:
6:aa5e94cddb3f
Parent:
5:93948a9bbde2 
update kronecker product operation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yo_Robot 4:d360c068d55f 1 /**
Yo_Robot 5:93948a9bbde2 2 * @brief Simple Kinematics Operations
Yo_Robot 4:d360c068d55f 3 * @file MatrixMath_Kinematics.cpp
Yo_Robot 5:93948a9bbde2 4 * @author Ernesto Palacios
Yo_Robot 4:d360c068d55f 5 *
Yo_Robot 5:93948a9bbde2 6 * Created on september 2011, 09:44 AM.
Yo_Robot 5:93948a9bbde2 7 *
Yo_Robot 5:93948a9bbde2 8 * Develop Under GPL v3.0 License
Yo_Robot 4:d360c068d55f 9 * http://www.gnu.org/licenses/gpl-3.0.html
Yo_Robot 4:d360c068d55f 10 */
Yo_Robot 4:d360c068d55f 11
Yo_Robot 4:d360c068d55f 12 #include "mbed.h"
Yo_Robot 4:d360c068d55f 13 #include "Matrix.h"
Yo_Robot 4:d360c068d55f 14 #include "MatrixMath.h"
mori3rti 6:aa5e94cddb3f 15 #include "math.h"
Yo_Robot 4:d360c068d55f 16
Yo_Robot 4:d360c068d55f 17
Yo_Robot 4:d360c068d55f 18 Matrix MatrixMath::RotX( float radians )
Yo_Robot 4:d360c068d55f 19 {
Yo_Robot 4:d360c068d55f 20 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 21 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 22
Yo_Robot 4:d360c068d55f 23 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 24
Yo_Robot 4:d360c068d55f 25 rotate << 1 << 0 << 0 << 0
Yo_Robot 5:93948a9bbde2 26 << 0 << cs << -sn << 0
Yo_Robot 5:93948a9bbde2 27 << 0 << sn << cs << 0
Yo_Robot 5:93948a9bbde2 28 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 29
Yo_Robot 4:d360c068d55f 30 return rotate;
Yo_Robot 4:d360c068d55f 31
Yo_Robot 4:d360c068d55f 32 }
Yo_Robot 4:d360c068d55f 33
Yo_Robot 4:d360c068d55f 34 Matrix MatrixMath::RotY( float radians )
Yo_Robot 4:d360c068d55f 35 {
Yo_Robot 4:d360c068d55f 36 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 37 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 38
Yo_Robot 4:d360c068d55f 39 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 40
Yo_Robot 4:d360c068d55f 41 rotate << cs << 0 << sn << 0
Yo_Robot 5:93948a9bbde2 42 << 0 << 1 << 0 << 0
Yo_Robot 5:93948a9bbde2 43 << -sn << 0 << cs << 0
Yo_Robot 5:93948a9bbde2 44 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 45
Yo_Robot 4:d360c068d55f 46 return rotate;
Yo_Robot 4:d360c068d55f 47 }
Yo_Robot 4:d360c068d55f 48
Yo_Robot 4:d360c068d55f 49 Matrix MatrixMath::RotZ( float radians )
Yo_Robot 4:d360c068d55f 50 {
Yo_Robot 4:d360c068d55f 51 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 52 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 53
Yo_Robot 4:d360c068d55f 54 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 55
Yo_Robot 4:d360c068d55f 56 rotate << cs << -sn << 0 << 0
Yo_Robot 4:d360c068d55f 57 << sn << cs << 0 << 0
Yo_Robot 5:93948a9bbde2 58 << 0 << 0 << 1 << 0
Yo_Robot 5:93948a9bbde2 59 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 60
Yo_Robot 4:d360c068d55f 61 return rotate;
Yo_Robot 4:d360c068d55f 62 }
Yo_Robot 4:d360c068d55f 63
Yo_Robot 4:d360c068d55f 64
Yo_Robot 4:d360c068d55f 65 Matrix MatrixMath::Transl( float x, float y, float z )
Yo_Robot 4:d360c068d55f 66 {
Yo_Robot 4:d360c068d55f 67 Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix
Yo_Robot 4:d360c068d55f 68 Matrix Position( 4, 1 ); // Position Matrix
Yo_Robot 4:d360c068d55f 69
Yo_Robot 5:93948a9bbde2 70 Position << x << y << z << 1; // position @ x,y,z
Yo_Robot 4:d360c068d55f 71
Yo_Robot 4:d360c068d55f 72 Matrix::AddRow( Translation, 4 ); // Add Row
Yo_Robot 4:d360c068d55f 73 Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix
Yo_Robot 4:d360c068d55f 74
Yo_Robot 4:d360c068d55f 75 return Translation;
Yo_Robot 2:d487bb616ec1 76 }