Ernesto Palacios / MatrixMath

This class provides with operations for Matrix Objects

Dependents:   Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more

MatrixMath_Kinematics.cpp@5:93948a9bbde2, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 19:21:30 2011 +0000
Revision:
5:93948a9bbde2
Parent:
4:d360c068d55f
Child:
6:aa5e94cddb3f
Version 0.9 Simple Kinematic Operations. View Log.c on Matrix Class for details.

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"
Yo_Robot 4:d360c068d55f 15
Yo_Robot 4:d360c068d55f 16
Yo_Robot 4:d360c068d55f 17 Matrix MatrixMath::RotX( float radians )
Yo_Robot 4:d360c068d55f 18 {
Yo_Robot 4:d360c068d55f 19 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 20 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 21
Yo_Robot 4:d360c068d55f 22 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 23
Yo_Robot 4:d360c068d55f 24 rotate << 1 << 0 << 0 << 0
Yo_Robot 5:93948a9bbde2 25 << 0 << cs << -sn << 0
Yo_Robot 5:93948a9bbde2 26 << 0 << sn << cs << 0
Yo_Robot 5:93948a9bbde2 27 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 28
Yo_Robot 4:d360c068d55f 29 return rotate;
Yo_Robot 4:d360c068d55f 30
Yo_Robot 4:d360c068d55f 31 }
Yo_Robot 4:d360c068d55f 32
Yo_Robot 4:d360c068d55f 33 Matrix MatrixMath::RotY( float radians )
Yo_Robot 4:d360c068d55f 34 {
Yo_Robot 4:d360c068d55f 35 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 36 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 37
Yo_Robot 4:d360c068d55f 38 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 39
Yo_Robot 4:d360c068d55f 40 rotate << cs << 0 << sn << 0
Yo_Robot 5:93948a9bbde2 41 << 0 << 1 << 0 << 0
Yo_Robot 5:93948a9bbde2 42 << -sn << 0 << cs << 0
Yo_Robot 5:93948a9bbde2 43 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 44
Yo_Robot 4:d360c068d55f 45 return rotate;
Yo_Robot 4:d360c068d55f 46 }
Yo_Robot 4:d360c068d55f 47
Yo_Robot 4:d360c068d55f 48 Matrix MatrixMath::RotZ( float radians )
Yo_Robot 4:d360c068d55f 49 {
Yo_Robot 4:d360c068d55f 50 float cs = cos( radians );
Yo_Robot 4:d360c068d55f 51 float sn = sin( radians );
Yo_Robot 4:d360c068d55f 52
Yo_Robot 4:d360c068d55f 53 Matrix rotate( 4, 4 );
Yo_Robot 4:d360c068d55f 54
Yo_Robot 4:d360c068d55f 55 rotate << cs << -sn << 0 << 0
Yo_Robot 4:d360c068d55f 56 << sn << cs << 0 << 0
Yo_Robot 5:93948a9bbde2 57 << 0 << 0 << 1 << 0
Yo_Robot 5:93948a9bbde2 58 << 0 << 0 << 0 << 1;
Yo_Robot 4:d360c068d55f 59
Yo_Robot 4:d360c068d55f 60 return rotate;
Yo_Robot 4:d360c068d55f 61 }
Yo_Robot 4:d360c068d55f 62
Yo_Robot 4:d360c068d55f 63
Yo_Robot 4:d360c068d55f 64 Matrix MatrixMath::Transl( float x, float y, float z )
Yo_Robot 4:d360c068d55f 65 {
Yo_Robot 4:d360c068d55f 66 Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix
Yo_Robot 4:d360c068d55f 67 Matrix Position( 4, 1 ); // Position Matrix
Yo_Robot 4:d360c068d55f 68
Yo_Robot 5:93948a9bbde2 69 Position << x << y << z << 1; // position @ x,y,z
Yo_Robot 4:d360c068d55f 70
Yo_Robot 4:d360c068d55f 71 Matrix::AddRow( Translation, 4 ); // Add Row
Yo_Robot 4:d360c068d55f 72 Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix
Yo_Robot 4:d360c068d55f 73
Yo_Robot 4:d360c068d55f 74 return Translation;
Yo_Robot 2:d487bb616ec1 75 }