Matrix class included
Dependencies: Matrix
MatrixMath_Kinematics.cpp@5:93948a9bbde2, 2011-10-30 (annotated)
- Committer:
- Yo_Robot
- Date:
- Sun Oct 30 19:21:30 2011 +0000
- Revision:
- 5:93948a9bbde2
- Parent:
- 4:d360c068d55f
- Child:
- 6:aa5e94cddb3f
Version 0.9 Simple Kinematic Operations. View Log.c on Matrix Class for details.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yo_Robot | 4:d360c068d55f | 1 | /** |
Yo_Robot | 5:93948a9bbde2 | 2 | * @brief Simple Kinematics Operations |
Yo_Robot | 4:d360c068d55f | 3 | * @file MatrixMath_Kinematics.cpp |
Yo_Robot | 5:93948a9bbde2 | 4 | * @author Ernesto Palacios |
Yo_Robot | 4:d360c068d55f | 5 | * |
Yo_Robot | 5:93948a9bbde2 | 6 | * Created on september 2011, 09:44 AM. |
Yo_Robot | 5:93948a9bbde2 | 7 | * |
Yo_Robot | 5:93948a9bbde2 | 8 | * Develop Under GPL v3.0 License |
Yo_Robot | 4:d360c068d55f | 9 | * http://www.gnu.org/licenses/gpl-3.0.html |
Yo_Robot | 4:d360c068d55f | 10 | */ |
Yo_Robot | 4:d360c068d55f | 11 | |
Yo_Robot | 4:d360c068d55f | 12 | #include "mbed.h" |
Yo_Robot | 4:d360c068d55f | 13 | #include "Matrix.h" |
Yo_Robot | 4:d360c068d55f | 14 | #include "MatrixMath.h" |
Yo_Robot | 4:d360c068d55f | 15 | |
Yo_Robot | 4:d360c068d55f | 16 | |
Yo_Robot | 4:d360c068d55f | 17 | Matrix MatrixMath::RotX( float radians ) |
Yo_Robot | 4:d360c068d55f | 18 | { |
Yo_Robot | 4:d360c068d55f | 19 | float cs = cos( radians ); |
Yo_Robot | 4:d360c068d55f | 20 | float sn = sin( radians ); |
Yo_Robot | 4:d360c068d55f | 21 | |
Yo_Robot | 4:d360c068d55f | 22 | Matrix rotate( 4, 4 ); |
Yo_Robot | 4:d360c068d55f | 23 | |
Yo_Robot | 4:d360c068d55f | 24 | rotate << 1 << 0 << 0 << 0 |
Yo_Robot | 5:93948a9bbde2 | 25 | << 0 << cs << -sn << 0 |
Yo_Robot | 5:93948a9bbde2 | 26 | << 0 << sn << cs << 0 |
Yo_Robot | 5:93948a9bbde2 | 27 | << 0 << 0 << 0 << 1; |
Yo_Robot | 4:d360c068d55f | 28 | |
Yo_Robot | 4:d360c068d55f | 29 | return rotate; |
Yo_Robot | 4:d360c068d55f | 30 | |
Yo_Robot | 4:d360c068d55f | 31 | } |
Yo_Robot | 4:d360c068d55f | 32 | |
Yo_Robot | 4:d360c068d55f | 33 | Matrix MatrixMath::RotY( float radians ) |
Yo_Robot | 4:d360c068d55f | 34 | { |
Yo_Robot | 4:d360c068d55f | 35 | float cs = cos( radians ); |
Yo_Robot | 4:d360c068d55f | 36 | float sn = sin( radians ); |
Yo_Robot | 4:d360c068d55f | 37 | |
Yo_Robot | 4:d360c068d55f | 38 | Matrix rotate( 4, 4 ); |
Yo_Robot | 4:d360c068d55f | 39 | |
Yo_Robot | 4:d360c068d55f | 40 | rotate << cs << 0 << sn << 0 |
Yo_Robot | 5:93948a9bbde2 | 41 | << 0 << 1 << 0 << 0 |
Yo_Robot | 5:93948a9bbde2 | 42 | << -sn << 0 << cs << 0 |
Yo_Robot | 5:93948a9bbde2 | 43 | << 0 << 0 << 0 << 1; |
Yo_Robot | 4:d360c068d55f | 44 | |
Yo_Robot | 4:d360c068d55f | 45 | return rotate; |
Yo_Robot | 4:d360c068d55f | 46 | } |
Yo_Robot | 4:d360c068d55f | 47 | |
Yo_Robot | 4:d360c068d55f | 48 | Matrix MatrixMath::RotZ( float radians ) |
Yo_Robot | 4:d360c068d55f | 49 | { |
Yo_Robot | 4:d360c068d55f | 50 | float cs = cos( radians ); |
Yo_Robot | 4:d360c068d55f | 51 | float sn = sin( radians ); |
Yo_Robot | 4:d360c068d55f | 52 | |
Yo_Robot | 4:d360c068d55f | 53 | Matrix rotate( 4, 4 ); |
Yo_Robot | 4:d360c068d55f | 54 | |
Yo_Robot | 4:d360c068d55f | 55 | rotate << cs << -sn << 0 << 0 |
Yo_Robot | 4:d360c068d55f | 56 | << sn << cs << 0 << 0 |
Yo_Robot | 5:93948a9bbde2 | 57 | << 0 << 0 << 1 << 0 |
Yo_Robot | 5:93948a9bbde2 | 58 | << 0 << 0 << 0 << 1; |
Yo_Robot | 4:d360c068d55f | 59 | |
Yo_Robot | 4:d360c068d55f | 60 | return rotate; |
Yo_Robot | 4:d360c068d55f | 61 | } |
Yo_Robot | 4:d360c068d55f | 62 | |
Yo_Robot | 4:d360c068d55f | 63 | |
Yo_Robot | 4:d360c068d55f | 64 | Matrix MatrixMath::Transl( float x, float y, float z ) |
Yo_Robot | 4:d360c068d55f | 65 | { |
Yo_Robot | 4:d360c068d55f | 66 | Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix |
Yo_Robot | 4:d360c068d55f | 67 | Matrix Position( 4, 1 ); // Position Matrix |
Yo_Robot | 4:d360c068d55f | 68 | |
Yo_Robot | 5:93948a9bbde2 | 69 | Position << x << y << z << 1; // position @ x,y,z |
Yo_Robot | 4:d360c068d55f | 70 | |
Yo_Robot | 4:d360c068d55f | 71 | Matrix::AddRow( Translation, 4 ); // Add Row |
Yo_Robot | 4:d360c068d55f | 72 | Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix |
Yo_Robot | 4:d360c068d55f | 73 | |
Yo_Robot | 4:d360c068d55f | 74 | return Translation; |
Yo_Robot | 2:d487bb616ec1 | 75 | } |