Naoto Morita / MatrixMath

add MatrixCross

MatrixMath_Kinematics.cpp@4:d360c068d55f, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 04:48:15 2011 +0000
Revision:
4:d360c068d55f
Parent:
2:d487bb616ec1
Child:
5:93948a9bbde2
Basic Kinematics & Basic Matrix, version 0.9

Who changed what in which revision?

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