38#include "ompl/util/ProlateHyperspheroid.h"
40#include "ompl/util/Exception.h"
44#include "ompl/util/GeometricEquations.h"
56struct ompl::ProlateHyperspheroid::PhsData
61 bool isTransformUpToDate_;
63 double minTransverseDiameter_;
65 double transverseDiameter_;
70 Eigen::VectorXd xFocus1_;
73 Eigen::VectorXd xFocus2_;
76 Eigen::VectorXd xCentre_;
79 Eigen::MatrixXd rotationWorldFromEllipse_;
82 Eigen::MatrixXd transformationWorldFromEllipse_;
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88 : dataPtr_(
std::make_shared<PhsData>())
92 dataPtr_->transverseDiameter_ = 0.0;
93 dataPtr_->isTransformUpToDate_ =
false;
96 dataPtr_->xFocus1_ = Eigen::Map<const Eigen::VectorXd>(focus1, dataPtr_->dim_);
97 dataPtr_->xFocus2_ = Eigen::Map<const Eigen::VectorXd>(focus2, dataPtr_->dim_);
100 dataPtr_->minTransverseDiameter_ = (dataPtr_->xFocus1_ - dataPtr_->xFocus2_).norm();
103 dataPtr_->xCentre_ = 0.5 * (dataPtr_->xFocus1_ + dataPtr_->xFocus2_);
111 if (transverseDiameter < dataPtr_->minTransverseDiameter_)
113 OMPL_ERROR(
"%g < %g", transverseDiameter, dataPtr_->minTransverseDiameter_);
114 throw Exception(
"Transverse diameter cannot be less than the distance between the foci.");
118 if (dataPtr_->transverseDiameter_ != transverseDiameter)
121 dataPtr_->isTransformUpToDate_ =
false;
124 dataPtr_->transverseDiameter_ = transverseDiameter;
127 updateTransformation();
134 if (!dataPtr_->isTransformUpToDate_)
136 throw Exception(
"The transformation is not up to date in the PHS class. Has the transverse diameter been set?");
140 Eigen::Map<Eigen::VectorXd>(phs, dataPtr_->dim_) =
141 dataPtr_->transformationWorldFromEllipse_ * Eigen::Map<const Eigen::VectorXd>(sphere, dataPtr_->dim_);
142 Eigen::Map<Eigen::VectorXd>(phs, dataPtr_->dim_) += dataPtr_->xCentre_;
147 if (!dataPtr_->isTransformUpToDate_)
150 throw Exception(
"The transverse diameter has not been set");
153 return (getPathLength(point) < dataPtr_->transverseDiameter_);
158 if (!dataPtr_->isTransformUpToDate_)
161 throw Exception(
"The transverse diameter has not been set");
164 return (getPathLength(point) == dataPtr_->transverseDiameter_);
169 return dataPtr_->dim_;
174 if (!dataPtr_->isTransformUpToDate_)
178 return std::numeric_limits<double>::infinity();
182 return dataPtr_->phsMeasure_;
192 return dataPtr_->minTransverseDiameter_;
197 return (dataPtr_->xFocus1_ - Eigen::Map<const Eigen::VectorXd>(point, dataPtr_->dim_)).norm() +
198 (Eigen::Map<const Eigen::VectorXd>(point, dataPtr_->dim_) - dataPtr_->xFocus2_).norm();
203 return dataPtr_->dim_;
206void ompl::ProlateHyperspheroid::updateRotation()
209 dataPtr_->isTransformUpToDate_ =
false;
212 double circleTol = 1E-9;
213 if (dataPtr_->minTransverseDiameter_ < circleTol)
215 dataPtr_->rotationWorldFromEllipse_.setIdentity(dataPtr_->dim_, dataPtr_->dim_);
221 Eigen::VectorXd transverseAxis(dataPtr_->dim_);
223 Eigen::MatrixXd wahbaProb(dataPtr_->dim_, dataPtr_->dim_);
225 Eigen::VectorXd middleM(dataPtr_->dim_);
228 transverseAxis = (dataPtr_->xFocus2_ - dataPtr_->xFocus1_) / dataPtr_->minTransverseDiameter_;
233 wahbaProb = transverseAxis * Eigen::MatrixXd::Identity(dataPtr_->dim_, dataPtr_->dim_).col(0).transpose();
236 Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::NoQRPreconditioner> svd(wahbaProb,
237 Eigen::ComputeFullV | Eigen::ComputeFullU);
241 middleM = Eigen::VectorXd::Ones(dataPtr_->dim_);
243 middleM(dataPtr_->dim_ - 1) = svd.matrixU().determinant() * svd.matrixV().determinant();
246 dataPtr_->rotationWorldFromEllipse_ = svd.matrixU() * middleM.asDiagonal() * svd.matrixV().transpose();
250void ompl::ProlateHyperspheroid::updateTransformation()
254 Eigen::VectorXd diagAsVector(dataPtr_->dim_);
256 double conjugateDiamater;
259 conjugateDiamater = std::sqrt(dataPtr_->transverseDiameter_ * dataPtr_->transverseDiameter_ -
260 dataPtr_->minTransverseDiameter_ * dataPtr_->minTransverseDiameter_);
264 diagAsVector.fill(conjugateDiamater / 2.0);
267 diagAsVector(0) = 0.5 * dataPtr_->transverseDiameter_;
270 dataPtr_->transformationWorldFromEllipse_ = dataPtr_->rotationWorldFromEllipse_ * diagAsVector.asDiagonal();
273 dataPtr_->phsMeasure_ =
277 dataPtr_->isTransformUpToDate_ =
true;
The exception type for ompl.
bool isInPhs(const double point[]) const
Check if the given point lies in the PHS.
bool isOnPhs(const double point[]) const
Check if the given point lies on the PHS.
double getMinTransverseDiameter() const
The minimum transverse diameter of the PHS, i.e., the distance between the foci.
void setTransverseDiameter(double transverseDiameter)
Set the transverse diameter of the PHS.
unsigned int getPhsDimension() const
The dimension of the PHS.
double getPathLength(const double point[]) const
Calculate length of a line that originates from one focus, passes through the given point,...
double getPhsMeasure() const
The measure of the PHS.
void transform(const double sphere[], double phs[]) const
Transform a point from a sphere to PHS. The return variable phs is expected to already exist.
ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
The description of an n-dimensional prolate hyperspheroid.
unsigned int getDimension() const
The state dimension of the PHS.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
double prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
The Lebesgue measure (i.e., "volume") of an n-dimensional prolate hyperspheroid (a symmetric hyperell...