Matrix class included
Dependencies: Matrix
Diff: MatrixMath_Kinematics.cpp
- Revision:
- 2:d487bb616ec1
- Child:
- 4:d360c068d55f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath_Kinematics.cpp Sun Oct 30 04:41:00 2011 +0000 @@ -0,0 +1,74 @@ +/**@file Kinematic_Operations + * @author Ernesto Palacios + * + * @brief Definition for Kinematic Operations. + * + * Develop Under GPL v3.0 License + * http://www.gnu.org/licenses/gpl-3.0.html + * + */ + +#include "mbed.h" +#include "Matrix.h" +#include "MatrixMath.h" + + +Matrix MatrixMath::RotX( float radians ) +{ + float cs = cos( radians ); + float sn = sin( radians ); + + Matrix rotate( 4, 4 ); + + rotate << 1 << 0 << 0 << 0 + << 0 << cs << -sn << 0 + << 0 << sn << cs << 0 + << 0 << 0 << 0 << 1; + + return rotate; + +} + +Matrix MatrixMath::RotY( float radians ) +{ + float cs = cos( radians ); + float sn = sin( radians ); + + Matrix rotate( 4, 4 ); + + rotate << cs << 0 << sn << 0 + << 0 << 1 << 0 << 0 + << -sn << 0 << cs << 0 + << 0 << 0 << 0 << 1; + + return rotate; +} + +Matrix MatrixMath::RotZ( float radians ) +{ + float cs = cos( radians ); + float sn = sin( radians ); + + Matrix rotate( 4, 4 ); + + rotate << cs << -sn << 0 << 0 + << sn << cs << 0 << 0 + << 0 << 0 << 1 << 0 + << 0 << 0 << 0 << 1; + + return rotate; +} + + +Matrix MatrixMath::Transl( float x, float y, float z ) +{ + Matrix Translation = MatrixMath::Eye( 3 ); //Identity Matrix + Matrix Position( 4, 1 ); // Position Matrix + + Position << x << y << z << 1; // position @ x,y,z + + Matrix::AddRow( Translation, 4 ); // Add Row + Matrix::AddCol( Translation, Position, 4 ); // Add Position Matrix + + return Translation; +} \ No newline at end of file