Method evaluateEDTWithGrad

src/planner/plan_env/src/edt_environment.cpp:46–56

}

void EDTEnvironment::evaluateEDTWithGrad(
    const Eigen::Vector3d& pos, double time, double& dist, Eigen::Vector3d& grad)
{
  // Eigen::Vector3d diff;
  // Eigen::Vector3d sur_pts[2][2][2];
  // sdf_map_->getSurroundPts(pos, sur_pts, diff);
  // double dists[2][2][2];
  // getSurroundDistance(sur_pts, dists);
  // interpolateTrilinear(dists, diff, dist, grad);
  dist = sdf_map_->getDistWithGrad(pos, grad);
}

// EDTEnvironment::
}  // namespace hetero_planner