hsu han-lin / Matrix

use from Ernesto Palacios

Dependents:   RoboticArm DXL_SDK_Porting_Test

Fork of Matrix by Ernesto Palacios

MatrixMath_Kinematics.cpp@9:f19d5d6c3468, 2017-03-31 (annotated)

Committer:
stanley1228
Date:
Fri Mar 31 09:10:27 2017 +0800
Revision:
9:f19d5d6c3468
Parent:
8:1774aa06ab99 
no

Who changed what in which revision?

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