45 template<
class RealType,
class CollisionChecker,
class ConfigType>
46 ConfigType
dcdSteer(
const ConfigType& from,
const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree)
50 const auto size =
static_cast<long int>(from.size());
52 Eigen::Map<const EigenVector> eFrom {from.data(), size};
53 Eigen::Map<const EigenVector> eTo {to.data(), size};
55 EigenVector step = eTo - eFrom;
56 const auto length = step.norm();
57 step *= dcdStepSize / length;
62 const std::size_t stepsToTake =
static_cast<std::size_t
>(length / dcdStepSize);
64 ConfigType stepToCheck(from);
66 Eigen::Map<EigenVector> eStepToCheck {stepToCheck.data(), size};
67 std::size_t stepsTaken = 0;
69 for (; stepsTaken < stepsToTake; ++stepsTaken)
73 if (!isCollisionFree(stepToCheck))
90 if (isCollisionFree(to))
113 template<
class RealType,
class CollisionChecker,
class ConfigType,
class Distance = std::function<
float(Eigen::VectorXf, Eigen::VectorXf)>,
114 class Interpolate = std::function<Eigen::VectorXf(Eigen::VectorXf, Eigen::VectorXf,
float)>>
115 bool dcdIsPathCollisionFree(
const ConfigType& from,
const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree,
bool toIsCollisionFree =
true,
116 const std::optional<Distance>& distanceLambda = std::optional<Distance>(),
117 const std::optional<Interpolate>& interpolationLambda = std::optional<Interpolate>(), std::vector<ConfigType>* resultPath = NULL)
119 if (!(toIsCollisionFree || isCollisionFree(to)))
126 const auto size =
static_cast<long int>(from.size());
128 Eigen::Map<const EigenVector> eFrom {from.data(), size};
129 Eigen::Map<const EigenVector> eTo {to.data(), size};
130 EigenVector connecting = eTo - eFrom;
131 std::vector<float> tmp(size);
132 Eigen::Map<EigenVector> eTmp {tmp.data(), size};
133 float distanceFromTo;
136 distanceFromTo = (*distanceLambda)(eFrom, eTo);
140 distanceFromTo = connecting.norm();
143 if (distanceFromTo <= dcdStepSize)
147 resultPath->push_back(to);
153 float dcdAsDistanceFactor = dcdStepSize / distanceFromTo;
155 float checkedStepFactor;
156 float betweenStepFactor;
157 std::size_t stepsToCheck = 1;
161 betweenStepFactor = 1.f / stepsToCheck;
162 checkedStepFactor = betweenStepFactor / 2.f;
164 float factor = checkedStepFactor;
166 for (std::size_t step = 0; step < stepsToCheck; ++step, factor += betweenStepFactor)
168 if (interpolationLambda)
170 eTmp = (*interpolationLambda)(eFrom, eTo, factor);
174 eTmp = eFrom + factor * connecting;
176 if (!isCollisionFree(tmp))
183 resultPath->push_back(tmp);
186 stepsToCheck = stepsToCheck << 1;
188 while (checkedStepFactor > dcdAsDistanceFactor);