add kronecker product
MatrixMath_Kinematics.cpp
- Committer:
- Yo_Robot
- Date:
- 2011-10-30
- Revision:
- 2:d487bb616ec1
- Child:
- 4:d360c068d55f
File content as of revision 2:d487bb616ec1:
/**@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; }