Diff: MatrixMath_Kinematics.cpp

Revision:

2:d487bb616ec1

Child:

4:d360c068d55f

diff -r c74cdf14aea2 -r d487bb616ec1 MatrixMath_Kinematics.cpp
--- /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