Diff: MatrixMath_Kinematics.cpp

Revision:

8:1774aa06ab99

Child:

9:f19d5d6c3468

diff -r 7a564cf28ab6 -r 1774aa06ab99 MatrixMath_Kinematics.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath_Kinematics.cpp	Sat Feb 11 13:07:13 2017 +0000
@@ -0,0 +1,88 @@
+/**
+ * @brief  Simple Kinematics Operations
+ * @file   MatrixMath_Kinematics.cpp
+ * @author Ernesto Palacios
+ *
+ * Created on september 2011, 09:44 AM.
+ *
+ * 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::Tz( float d )
+{
+    
+    Matrix z_d( 4, 4 );
+
+    z_d << 1  <<  0 <<  0  << 0
+           << 0  <<  1  <<  0  << 0
+           << 0  <<  0  <<  1  << d
+           << 0  <<  0  <<  0  << 1;
+
+    return z_d;
+}
+
+
+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