Matrix class included
Dependencies: Matrix
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?
User | Revision | Line number | New 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 | } |