//------------------------------------------------------------------------------- // Constructeur de interpolate //------------------------------------------------------------------------------- GribRecord * GribRecord::InterpolatedRecord(const GribRecord &rec1, const GribRecord &rec2, double d, bool dir) { double La1, Lo1, La2, Lo2, Di, Dj; int im1, jm1, im2, jm2; int Ni, Nj, rec1offi, rec1offj, rec2offi, rec2offj; if(!GetInterpolatedParameters(rec1, rec2, La1, Lo1, La2, Lo2, Di, Dj, im1, jm1, im2, jm2, Ni, Nj, rec1offi, rec1offj, rec2offi, rec2offj)) return NULL; // recopie les champs de bits int size = Ni*Nj; double *data = new double[size]; zuchar *BMSbits = NULL; if (rec1.BMSbits != NULL && rec2.BMSbits != NULL) BMSbits = new zuchar[(Ni*Nj-1)/8+1](); for (int i=0; i<Ni; i++) for (int j=0; j<Nj; j++) { int in=j*Ni+i; int i1 = (j*jm1+rec1offj)*rec1.Ni + i*im1+rec1offi; int i2 = (j*jm2+rec2offj)*rec2.Ni + i*im2+rec2offi; double data1 = rec1.data[i1], data2 = rec2.data[i2]; if(data1 == GRIB_NOTDEF || data2 == GRIB_NOTDEF) data[in] = GRIB_NOTDEF; else { if( !dir ) data[in] = (1-d)*data1 + d*data2; else data[in] = interp_angle(data1, data2, d, 180.); } if(BMSbits) { int b1 = rec1.BMSbits[i1>>3] & 1<<(i1&7); int b2 = rec2.BMSbits[i2>>3] & 1<<(i2&7); if(b1 && b2) BMSbits[in>>3] |= 1<<(in&7); else BMSbits[in>>3] &= ~(1<<(in&7)); } }
std::vector<Pose2_cont> create_interpolated_motion(const Pose2_cont& start, const Pose2_cont& goal) { auto sqrd = [](double d) { return d * d; }; std::vector<Pose2_cont> motion; const double res = 0.01; double dist = sqrt(sqrd(goal.x - start.x) + sqrd(goal.y - start.y)); const int num_samples = (int)std::ceil(dist / res); motion.resize(num_samples); for (int i = 0; i < num_samples; ++i) { double alpha = (double)i / (num_samples - 1); double interm_x = interp(start.x, goal.x, alpha); double interm_y = interp(start.y, goal.y, alpha); double interm_yaw = interp_angle(start.yaw, goal.yaw, alpha); motion.emplace_back(interm_x, interm_y, interm_yaw); } return motion; }