odomMath.hpp

Go to the documentation of this file.

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
#pragma once
 
#include "okapi/api/odometry/odomState.hpp"
#include "okapi/api/odometry/point.hpp"
#include "okapi/api/util/logging.hpp"
#include <tuple>
 
namespace okapi {
class OdomMath {
  public:
  static QLength computeDistanceToPoint(const Point &ipoint, const OdomState &istate);
 
  static QAngle computeAngleToPoint(const Point &ipoint, const OdomState &istate);
 
  static std::pair<QLength, QAngle> computeDistanceAndAngleToPoint(const Point &ipoint,
                                                                   const OdomState &istate);
 
  static QAngle constrainAngle360(const QAngle &angle);
 
  static QAngle constrainAngle180(const QAngle &angle);
 
  private:
  OdomMath();
  ~OdomMath();
 
  static std::pair<double, double> computeDiffs(const Point &ipoint, const OdomState &istate);
 
  static double computeDistance(double xDiff, double yDiff);
 
  static double computeAngle(double xDiff, double yDiff, double theta);
};
} // namespace okapi