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@2:d487bb616ec1, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 04:41:00 2011 +0000
Revision:
2:d487bb616ec1
Child:
4:d360c068d55f
Kinematic and Dot functions version 0.9

Who changed what in which revision?

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