ProlateHyperspheroid.cpp
85 ompl::ProlateHyperspheroid::ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
134 throw Exception("The transformation is not up to date in the PHS class. Has the transverse diameter been set?");
139 dataPtr_->transformationWorldFromEllipse_ * Eigen::Map<const Eigen::VectorXd>(sphere, dataPtr_->dim_);
174 // The transform is not up to date until the transverse diameter has been set, therefore we have no transverse
229 // Formulate as a Wahba problem, first forming the matrix a_j*a_i' where a_j is the transverse axis if the
230 // ellipse in the world frame, and a_i is the first basis vector of the world frame (i.e., [1 0 .... 0])
231 wahbaProb = transverseAxis * Eigen::MatrixXd::Identity(dataPtr_->dim_, dataPtr_->dim_).col(0).transpose();
244 dataPtr_->rotationWorldFromEllipse_ = svd.matrixU() * middleM.asDiagonal() * svd.matrixV().transpose();
268 dataPtr_->transformationWorldFromEllipse_ = dataPtr_->rotationWorldFromEllipse_ * diagAsVector.asDiagonal();
272 prolateHyperspheroidMeasure(dataPtr_->dim_, dataPtr_->minTransverseDiameter_, dataPtr_->transverseDiameter_);
bool isOnPhs(const double point[]) const
Check if the given point lies on the PHS.
Definition: ProlateHyperspheroid.cpp:154
double getPathLength(const double point[]) const
Calculate length of a line that originates from one focus, passes through the given point...
Definition: ProlateHyperspheroid.cpp:193
unsigned int getPhsDimension() const
The dimension of the PHS.
Definition: ProlateHyperspheroid.cpp:165
STL namespace.
void setTransverseDiameter(double transverseDiameter)
Set the transverse diameter of the PHS.
Definition: ProlateHyperspheroid.cpp:107
double getMinTransverseDiameter() const
The minimum transverse diameter of the PHS, i.e., the distance between the foci.
Definition: ProlateHyperspheroid.cpp:188
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
bool isInPhs(const double point[]) const
Check if the given point lies in the PHS.
Definition: ProlateHyperspheroid.cpp:143
ProlateHyperspheroid(unsigned int n, const double focus1[], const double focus2[])
The description of an n-dimensional prolate hyperspheroid.
Definition: ProlateHyperspheroid.cpp:85
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:130
unsigned int getDimension() const
The state dimension of the PHS.
Definition: ProlateHyperspheroid.cpp:199