39#include <ompl/multilevel/datastructures/projections/SO2N_SO2M.h>
40#include <ompl/base/spaces/SO2StateSpace.h>
44Projection_SO2N_SO2M::Projection_SO2N_SO2M(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
54 const auto *SO2bundle =
55 xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k + getBaseDimension());
56 auto *SO2fiber = xFiber->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
58 SO2fiber->value = SO2bundle->value;
62void Projection_SO2N_SO2M::project(
const ompl::base::State *xBundle, ompl::base::State *xBase)
const
64 for (
unsigned int k = 0; k < getBaseDimension(); k++)
66 const auto *SO2bundle = xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
70 if (getBaseDimension() <= 1)
76 SO2base = xBase->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
79 SO2base->
value = SO2bundle->value;
83void Projection_SO2N_SO2M::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
84 ompl::base::State *xBundle)
const
86 for (
unsigned int k = 0; k < getBaseDimension(); k++)
88 auto *SO2bundle = xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
91 if (getBaseDimension() <= 1)
97 SO2base = xBase->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
102 for (
unsigned int k = getBaseDimension(); k < getBaseDimension() +
getFiberDimension(); k++)
104 auto *SO2bundle = xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k);
105 const auto *SO2fiber =
106 xFiber->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(k - getBaseDimension());
108 SO2bundle->value = SO2fiber->value;
112ompl::base::StateSpacePtr Projection_SO2N_SO2M::computeFiberSpace()
114 unsigned int N = getDimension() - getBaseDimension();
115 auto fiberSpace = std::make_shared<base::CompoundStateSpace>();
116 for (
unsigned int k = 0; k < N; k++)
118 base::StateSpacePtr SO2Space = std::make_shared<base::SO2StateSpace>();
119 fiberSpace->addSubspace(SO2Space, 1.0);
double value
The value of the angle in the interval (-Pi, Pi].
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
unsigned int getFiberDimension() const
Dimension of Fiber Space.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SO2N_SO2M
SO2N \rightarrow SO2M (N copies of SO2 onto M copies of SO2)