pathPlanning.c

00001 #include "pathPlanning.h"
00002 
00003 void GeneratePath(matrix* p0, matrix* pf, matrix* path)
00004 {
00005     float delta_x = (pf->element[0][0] - p0->element[0][0]) / path->col;
00006     float delta_y = (pf->element[1][0] - p0->element[1][0]) / path->col;
00007     float delta_z = (pf->element[2][0] - p0->element[2][0]) / path->col;
00008     float delta_theta_x = (pf->element[3][0] - p0->element[3][0]) / path->col;
00009     float delta_theta_y = (pf->element[4][0] - p0->element[4][0]) / path->col;
00010     float delta_theta_z = (pf->element[5][0] - p0->element[5][0]) / path->col;
00011     
00012     for(int i = 0; i < path->col; i++)
00013     {
00014         path->element[0][i] = p0->element[0][0] + i*delta_x;
00015         path->element[1][i] = p0->element[1][0] + i*delta_y;
00016         path->element[2][i] = p0->element[2][0] + i*delta_z;
00017         path->element[3][i] = p0->element[3][0] + i*delta_theta_x;
00018         path->element[4][i] = p0->element[4][0] + i*delta_theta_y;
00019         path->element[5][i] = p0->element[5][0] + i*delta_theta_z;
00020         
00021     } 
00022     
00023     return;
00024 }