39#include <ompl/multilevel/datastructures/projections/XRN_XRM_SO2.h>
40#include <ompl/base/spaces/SO2StateSpace.h>
41#include <ompl/base/spaces/RealVectorStateSpace.h>
43#include <ompl/util/Exception.h>
47Projection_SO2RN_SO2RM::Projection_SO2RN_SO2RM(ompl::base::StateSpacePtr
BundleSpace,
48 ompl::base::StateSpacePtr BaseSpace)
56 const auto *xBundle_SO2 = xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
57 const auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
59 auto *xBase_SO2 = xBase->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
60 auto *xBase_RM = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
62 xBase_SO2->value = xBundle_SO2->value;
64 for (
unsigned int k = 0; k < getBaseDimension() - 1; k++)
66 xBase_RM->values[k] = xBundle_RN->values[k];
70void Projection_SO2RN_SO2RM::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
71 ompl::base::State *xBundle)
const
73 auto *xBundle_SO2 = xBundle->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
74 auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
76 const auto *xBase_SO2 = xBase->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
77 const auto *xBase_RM = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
81 xBundle_SO2->value = xBase_SO2->value;
86 for (
unsigned int k = 0; k < M; k++)
88 xBundle_RN->values[k] = xBase_RM->values[k];
90 for (
unsigned int k = M; k < M + N; k++)
92 xBundle_RN->values[k] = xFiber_RJ->values[k - M];
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_SO2RN_SO2RM
SO2RN \rightarrow SO2RM, m < n.