This class provides with operations for Matrix Objects
Dependents: Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more
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?
User | Revision | Line number | New 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 | } |