diff --git a/src/widgets/map/gvf_trajectories/CMakeLists.txt b/src/widgets/map/gvf_trajectories/CMakeLists.txt index 3f35ff89..6ab153e7 100644 --- a/src/widgets/map/gvf_trajectories/CMakeLists.txt +++ b/src/widgets/map/gvf_trajectories/CMakeLists.txt @@ -8,6 +8,7 @@ set(SOURCE ${CMAKE_CURRENT_SOURCE_DIR}/gvf_traj_trefoil.cpp ${CMAKE_CURRENT_SOURCE_DIR}/gvf_traj_3D_ellipse.cpp ${CMAKE_CURRENT_SOURCE_DIR}/gvf_traj_3D_lissajous.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/gvf_traj_bezier.cpp ) set(INC_DIRS ${INC_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/widgets/map/gvf_trajectories/gvf_traj_bezier.cpp b/src/widgets/map/gvf_trajectories/gvf_traj_bezier.cpp new file mode 100644 index 00000000..4fc94715 --- /dev/null +++ b/src/widgets/map/gvf_trajectories/gvf_traj_bezier.cpp @@ -0,0 +1,222 @@ +#include "gvf_traj_bezier.h" +#include +#include + +GVF_traj_bezier::GVF_traj_bezier(QString id, QList param, QList _phi, + float wb, QVector *gvf_settings) : + GVF_trajectory(id, gvf_settings) +{ + set_param(param, _phi, wb); + generate_trajectory(); +} + +// 2D bezier trajectory (parametric representation) +void GVF_traj_bezier::genTraj() { + QList points; + + float res = 1e7; + float N; + + + // TODO: Change num_pts acrroding ttoo things + float max_t = n_seg; // NUMBER OF SPLINES max_t = NUMBER OF SPLINES.. TODO + float num_pts = max_t*100; // 100 per each segment + + float dt = max_t /num_pts; + for (float t = 0; t <= max_t + dt/2; t+=dt) { + points.append(traj_point(t)); + } + createTrajItem(points); +} + + +// 2D bezier GVF +void GVF_traj_bezier::genVField() { + + + QList vxy_mesh; + + float xmin, xmax, ymin, ymax; + xmin = xx[0]; xmax = xx[0]; ymin = yy[0]; ymax = yy[0]; + for(int k = 0; k < 3*n_seg + 1; k++) + { + if(xx[k] > xmax) + xmax = xx[k]; + if(xx[k] < xmin) + xmin = xx[k]; + if(yy[k] > ymax) + ymax = yy[k]; + if(yy[k] < ymin) + ymin = yy[k]; + } + //float bound_area = 2*(xmax-xmin)*(ymax-ymin); + float bound_area = 2*(xmax-xmin)*(ymax-ymin); + + // 40x40 is the number of arrows in x and in y + emit DispatcherUi::get()->gvf_defaultFieldSettings(ac_id, round(bound_area), 40, 40); + xy_mesh = meshGrid(); + + foreach (const QPointF &point, xy_mesh) { + + float phix = point.x() - traj_point(w).x(); // Normal component + float phiy = point.y() - traj_point(w).y(); // Normal Component + + float sigx = beta*traj_point_deriv(w).x(); // Tangential Component + float sigy = beta*traj_point_deriv(w).y(); // Tangential Component + + float vx = sigx - kx*phix; + float vy = sigy - ky*phiy; + + float norm = sqrt(pow(vx,2) + pow(vy,2)); + + vxy_mesh.append(QPointF(vx/norm, vy/norm)); + } + + createVFieldItem(xy_mesh, vxy_mesh); +} + +/////////////// PRIVATE FUNCTIONS /////////////// +void GVF_traj_bezier::set_param(QList param, QList _phi, float wb) { + if (param.size()>15) { // gvf_parametric_2D_bezier_wp() + auto ac = pprzApp()->toolbox()->aircraftManager()->getAircraft(ac_id); + Waypoint::WpFrame frame = ac->getFlightPlan()->getFrame(); + } + + + // Lest test writing on a file the parameters, and then take the information from that file... (.txt file) + // TODO: Remove files when the program ends... + // TODO: join in one file. x in first line, y in second line, ks in third line. getline (or seek \n)? And then the pointer is in the next line¿¿¿¿¿ + + // Number of P points (P \in R^2) : 3*N_SEGMENTS + 1 + // param[0] = N_SEGMENTS + FILE *file_x; FILE *file_y; FILE *file_ks; //FILE *file; + // TODO (ing) = Use only one file + char x_val[] = "/tmp/x_values.txt"; // ... + char y_val[] = "/tmp/y_values.txt"; + char ks_val[] = "/tmp/ks_values.txt"; + //char file_val[] = "/tmp/bez_values.txt"; + + int k = 1; + + // Write: + if(param[0] < 0){ + n_seg = -(int)param[0]; + file_x = fopen(x_val, "w+"); + for(k = 0; k < 3*n_seg+ 2; k++){ + fprintf(file_x, "%f ", param[k]); + } + fclose(file_x); + } + else if(param[0] > 0){ + n_seg = (int)param[0]; + file_y = fopen(y_val, "w+"); + for(k = 0; k < 3*n_seg + 2; k++){ + fprintf(file_y, "%f ", param[k]); + } + fclose(file_y); + } + else{ + file_ks = fopen(ks_val, "w+"); + fprintf(file_ks, "%f %f %f", param[1], param[2], param[3]); + fclose(file_ks); + } + + float N_SEG; + // Read: + if( (file_x = fopen(x_val, "r")) != NULL) + { + fscanf(file_x, "%f ", &N_SEG); + n_seg = -(int)N_SEG; + for(k = 0; k < 3*(int)n_seg + 1; k++){ + fscanf(file_x, "%f ", &xx[k]); + } + fclose(file_x); + } + if( (file_y = fopen(y_val, "r")) != NULL) + { + fscanf(file_x, "%f ", &N_SEG); + n_seg = (int)N_SEG; + for(k = 0; k < 3*(int)n_seg + 1; k++){ + fscanf(file_y, "%f ", &yy[k]); + } + fclose(file_y); + } + if( (file_ks = fopen(ks_val, "r")) != NULL) + { + fscanf(file_ks, "%f", &kx); + fscanf(file_ks, "%f", &ky); + fscanf(file_ks, "%f", &beta); + fclose(file_ks); + } + + /* + // Read + if(param[0] < 0){ + n_seg = -(int)param[0]; + file = fopen(file_val, "wr+"); + for(k = 0; k < 3*n_seg+ 2; k++){ + fprintf(file_val, "%f ", param[k]); + } + fprintf(file_val, "%c", '\n'); + fclose(file_val); + } + else if(param[0] > 0){ + n_seg = (int)param[0]; + file_y = fopen(file_val, "wr+"); + // One must write in the next line.. + // I can check if the file is already created by first reading and if the pointer is not null then it must exist..... + // with that on mind we can then write on the second line. The question is how + for(k = 0; k < 3*n_seg + 2; k++){ + fprintf(file_val, "%f ", param[k]); + } + fprintf(file_val, "%c", '\n'); + fclose(file_val); + } + else{ + file_ks = fopen(file_val, "wr+"); + fprintf(file_val, "%f %f %f", param[1], param[2], param[3]); + fprintf(file_val, "%c", '\n'); + fclose(file_val); + } + */ + phi = QPointF(_phi[0], _phi[1]); //TODO: Display error in GVF viewer?? + w = wb/beta; // gvf_parametric_w = wb/beta (wb = w*beta) +} + +QPointF GVF_traj_bezier::traj_point(float t) { + + int k = (int)t; // It works like a floor .. t >= k + /* Each Bézier spline must be evaluated between 0 and 1. + Remove integer part, get fractional + Choose which spline (0<=t<=1 -> Spline1 (coefs 0-3), + 1<=t<=2 -> Spline2 (coefs 3-6) ... */ + t = t - k; + k = 3*k; + float x = xx[k]*(1-t)*(1-t)*(1-t) + 3*(1-t)*(1-t)*t*xx[k+1] + + 3*(1-t)*t*t*xx[k+2] + t*t*t*xx[k+3]; + float y = yy[k]*(1-t)*(1-t)*(1-t) + 3*(1-t)*(1-t)*t*yy[k+1] + + 3*(1-t)*t*t*yy[k+2] + t*t*t*yy[k+3]; + return QPointF(x,y); +} + +QPointF GVF_traj_bezier::traj_point_deriv(float t) { + + int k = (int)t; // It works like a floor .. t >= k + /* Each Bézier spline must be evaluated between 0 and 1. + Remove integer part, get fractional + Choose which spline (0<=t<=1 -> Spline1 (coefs 0-3), + 1<=t<=2 -> Spline2 (coefs 3-6) ... */ + t = t - k; + k = 3*k; + float dx = (xx[k+1] - xx[k])*3*(1-t)*(1-t) + + 6*(1-t)*t*(xx[k+2]-xx[k+1]) + 3*t*t*(xx[k+3] - xx[k+2]); + float dy = (yy[k+1] - yy[k])*3*(1-t)*(1-t) + + 6*(1-t)*t*(yy[k+2]-yy[k+1]) + 3*t*t*(yy[k+3] - yy[k+2]); + return QPointF(dx,dy); +} + +float GVF_traj_bezier::gcd(int a, int b){ + if(b == 0) + return a; + return gcd(b, a % b); +} diff --git a/src/widgets/map/gvf_trajectories/gvf_traj_bezier.h b/src/widgets/map/gvf_trajectories/gvf_traj_bezier.h new file mode 100644 index 00000000..c9b1cad2 --- /dev/null +++ b/src/widgets/map/gvf_trajectories/gvf_traj_bezier.h @@ -0,0 +1,36 @@ +#ifndef GVF_TRAJ_BEZIER_H +#define GVF_TRAJ_BEZIER_H + +#include "gvf_trajectory.h" + +class GVF_traj_bezier : public GVF_trajectory +{ + Q_OBJECT +public: + explicit GVF_traj_bezier(QString id, QList param, QList _phi, + float wb, QVector *gvf_settings); + +protected: + virtual void genTraj() override; + virtual void genVField() override; + +private: + void set_param(QList param, QList _phi, float wb); // GVF PARAMETRIC + QPointF traj_point(float t); + QPointF traj_point_deriv(float t); + + // Maximum data size from gvf_parametric is 16 elements. + float xx[16]; + float yy[16]; + int n_seg; + float w; + float kx; + float ky; + float beta; + int which_spline = 0; + QPointF phi; + + float gcd(int a, int b); +}; + +#endif // GVF_TRAJ_bezier_H diff --git a/src/widgets/map/mapwidget.cpp b/src/widgets/map/mapwidget.cpp index a996f866..45f1682f 100644 --- a/src/widgets/map/mapwidget.cpp +++ b/src/widgets/map/mapwidget.cpp @@ -37,6 +37,7 @@ #include "gvf_traj_trefoil.h" #include "gvf_traj_3D_ellipse.h" #include "gvf_traj_3D_lissajous.h" +#include "gvf_traj_bezier.h" MapWidget::MapWidget(QWidget *parent) : Map2D(parent), @@ -1251,6 +1252,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) { // Common parser definitions uint8_t traj; + float wb; QList param = {0.0}; int8_t direction; GVF_trajectory* gvf_traj; @@ -1290,6 +1292,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) { QList phi = {0.0}; // Error signals msg.getField("traj", traj); + msg.getField("w", wb); msg.getField("p", param); msg.getField("phi", phi); @@ -1307,6 +1310,10 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) { gvf_traj = new GVF_traj_3D_lissajous(sender, param, phi, gvf_trajectories_config[sender]); break; } + case 3: { // Bezier 2D + gvf_traj = new GVF_traj_bezier(sender, param, phi, wb, gvf_trajectories_config[sender]); + break; + } default: qDebug() << "GVF: GVF_PARAMETRIC message parser received an unknown trajectory id."; return;