Edge randomSteer(const State &start, double dt) const {
fcl::Vec3f translation;// = math::randomPointInSphere();
translation[3] = -0.01;
fcl::Quaternion3f rotation;// = math::getRandomUnitQuaternion();
State end = doSteps(start, translation, rotation, dt);
return Edge(start, end, translation, rotation, dt);
}
void draw() const {
double dt = 0.1;
state = doSteps(state, fcl::Vec3f(), fcl::Quaternion3f(), dt);
auto transform = state.toOpenGLTransform();
mesh.draw(color, transform);
}
Edge steerWithControl(const State &start, const std::vector<double> controls, double dt) const {
/* Be careful about the order these are being passed in */
fcl::Vec3f translation(controls[0], controls[1], controls[2]);
fcl::Quaternion3f rotation(controls[3], controls[4], controls[5], controls[6]);
rotation = math::normalize(rotation);
State end = doSteps(start, translation, rotation, dt);
return Edge(start, end, translation, rotation, dt);
}
void drawSolution(const std::vector<const Edge *> &solution, double dt = std::numeric_limits<double>::infinity()) const {
for(const Edge *edge : solution) {
unsigned int steps = std::isinf(dt) ? 1 : edge->dt / dt;
State state = edge->start;
edge->draw(OpenGLWrapper::Color::Green());
for(unsigned int step = 0; step < steps; ++step) {
auto transform = state.toOpenGLTransform();
mesh.draw(color, transform);
state.draw(OpenGLWrapper::Color::Green());
state = doSteps(state, edge->translation, edge->rotation, dt);
}
}
}
std::vector<std::vector<fcl::Transform3f> > getPoses(const Edge &edge, double dt) const {
std::vector<std::vector<fcl::Transform3f> > retPoses;
unsigned int steps = std::isinf(dt) ? 0 : edge.dt / dt;
State state = edge.start;
retPoses.emplace_back();
retPoses.back().push_back(state.toFCLTransform());
for(unsigned int step = 0; step < steps; ++step) {
state = doSteps(edge.start, edge.translation, edge.rotation, dt * (double)step);
retPoses.emplace_back();
retPoses.back().push_back(state.toFCLTransform());
}
retPoses.emplace_back();
retPoses.back().push_back(edge.end.toFCLTransform());
return retPoses;
}
Edge steerWithControl(const State &start, const Edge &getControlsFromThisEdge, double dt) const {
State end = doSteps(start, getControlsFromThisEdge.translation, getControlsFromThisEdge.rotation, dt);
return Edge(start, end, getControlsFromThisEdge.translation, getControlsFromThisEdge.rotation, dt);
}
void VisionStepper::doRotationInAngle(float angle)
{
doSteps(angle * degreeStepRatio);
}
void VisionStepper::doDistanceInCm(float distance)
{
doSteps(distance * stepCmRatio);
}
