//-------------------------------------------------------------------------------
// 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;
}
