ProlateHyperspheroid.cpp
87 ompl::ProlateHyperspheroid::ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
136 throw Exception("The transformation is not up to date in the PHS class. Has the transverse diameter been set?");
141 dataPtr_->transformationWorldFromEllipse_ * Eigen::Map<const Eigen::VectorXd>(sphere, dataPtr_->dim_);
176 // The transform is not up to date until the transverse diameter has been set, therefore we have no transverse
231 // Formulate as a Wahba problem, first forming the matrix a_j*a_i' where a_j is the transverse axis if the
232 // ellipse in the world frame, and a_i is the first basis vector of the world frame (i.e., [1 0 .... 0])
233 wahbaProb = transverseAxis * Eigen::MatrixXd::Identity(dataPtr_->dim_, dataPtr_->dim_).col(0).transpose();
246 dataPtr_->rotationWorldFromEllipse_ = svd.matrixU() * middleM.asDiagonal() * svd.matrixV().transpose();
270 dataPtr_->transformationWorldFromEllipse_ = dataPtr_->rotationWorldFromEllipse_ * diagAsVector.asDiagonal();
274 prolateHyperspheroidMeasure(dataPtr_->dim_, dataPtr_->minTransverseDiameter_, dataPtr_->transverseDiameter_);
ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
The description of an n-dimensional prolate hyperspheroid.
Definition: ProlateHyperspheroid.cpp:87
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.
Definition: ProlateHyperspheroid.cpp:132
bool isInPhs(const double point[]) const
Check if the given point lies in the PHS.
Definition: ProlateHyperspheroid.cpp:145
double getPathLength(const double point[]) const
Calculate length of a line that originates from one focus, passes through the given point,...
Definition: ProlateHyperspheroid.cpp:195
void setTransverseDiameter(double transverseDiameter)
Set the transverse diameter of the PHS.
Definition: ProlateHyperspheroid.cpp:109
unsigned int getDimension() const
The state dimension of the PHS.
Definition: ProlateHyperspheroid.cpp:201
double prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
The Lebesgue measure (i.e., "volume") of an n-dimensional prolate hyperspheroid (a symmetric hyperell...
Definition: GeometricEquations.cpp:62
unsigned int getPhsDimension() const
The dimension of the PHS.
Definition: ProlateHyperspheroid.cpp:167
bool isOnPhs(const double point[]) const
Check if the given point lies on the PHS.
Definition: ProlateHyperspheroid.cpp:156
double getMinTransverseDiameter() const
The minimum transverse diameter of the PHS, i.e., the distance between the foci.
Definition: ProlateHyperspheroid.cpp:190