Diff: source/pathPlanning.c

Revision:

0:fb6e494a7656

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/pathPlanning.c	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,24 @@
+#include "pathPlanning.h"
+
+void GeneratePath(matrix* p0, matrix* pf, matrix* path)
+{
+    float delta_x = (pf->element[0][0] - p0->element[0][0]) / path->col;
+    float delta_y = (pf->element[1][0] - p0->element[1][0]) / path->col;
+    float delta_z = (pf->element[2][0] - p0->element[2][0]) / path->col;
+    float delta_theta_x = (pf->element[3][0] - p0->element[3][0]) / path->col;
+    float delta_theta_y = (pf->element[4][0] - p0->element[4][0]) / path->col;
+    float delta_theta_z = (pf->element[5][0] - p0->element[5][0]) / path->col;
+    
+    for(int i = 0; i < path->col; i++)
+    {
+        path->element[0][i] = p0->element[0][0] + i*delta_x;
+        path->element[1][i] = p0->element[1][0] + i*delta_y;
+        path->element[2][i] = p0->element[2][0] + i*delta_z;
+        path->element[3][i] = p0->element[3][0] + i*delta_theta_x;
+        path->element[4][i] = p0->element[4][0] + i*delta_theta_y;
+        path->element[5][i] = p0->element[5][0] + i*delta_theta_z;
+        
+    } 
+    
+    return;
+}
\ No newline at end of file