distance.hpp

#ifndef SRC_DISTANCE_DISTANCE_HPP_
#define SRC_DISTANCE_DISTANCE_HPP_
 
#include <utility>
#include <memory>
 
#include "common/style.hpp"
#include "common/pipeline/stages.hpp"
#include "common/spatial/attitude-utils.hpp"
#include "common/spatial/camera.hpp"
 
namespace found {
 
class DistanceDeterminationAlgorithm : public FunctionStage<Points, PositionVector> {
 public:
    // Constructs this
    DistanceDeterminationAlgorithm() = default;
    // Destroys this
    virtual ~DistanceDeterminationAlgorithm() {}
};
 
class SphericalDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
 public:
    SphericalDistanceDeterminationAlgorithm(decimal radius, Camera &&cam) : cam_(cam), radius_(radius) {}
    ~SphericalDistanceDeterminationAlgorithm() {}
 
    PositionVector Run(const Points &p) override;
 
 protected:
    Vec3 getCenter(const Vec3 &a, const Vec3 &b, const Vec3 &c);
 
    PositionVector Run(const Vec3 &a, const Vec3 &b, const Vec3 &c);
 
    Camera cam_;
 
    decimal radius_;
 
    Vec3 center_;
 
    decimal r_;
};
 
class IterativeSphericalDistanceDeterminationAlgorithm : public SphericalDistanceDeterminationAlgorithm {
 public:
    IterativeSphericalDistanceDeterminationAlgorithm(decimal radius,
                                                     Camera &&cam,
                                                     size_t minimumIterations,
                                                     size_t maximumRefreshes,
                                                     decimal distanceRatio,
                                                     decimal discriminatorRatio,
                                                     int pdfOrder,
                                                     int radiusLossOrder);
    ~IterativeSphericalDistanceDeterminationAlgorithm() = default;
 
    PositionVector Run(const Points &p) override;
 
 private:
    decimal GenerateLoss(PositionVector &position,
                         decimal targetDistanceSq,
                         std::unique_ptr<Vec3[]> &projectedPoints,
                         size_t size);
 
    PositionVector ShuffledCall(std::unique_ptr<Vec3[]> &source, size_t n, std::unique_ptr<uint64_t[]> &logits);
 
    inline uint64_t Pow(uint64_t base, uint64_t power) {
        uint64_t result = 1;
        while (power > 0) {
           if (power % 2 == 1)
                 result *= base;
           base *= base;
           power /= 2;
        }
        return result;
    }
 
    size_t minimumIterations_;
    size_t maximumRefreshes_;
    decimal distanceRatioSq_;
    decimal discriminatorRatio_;
    uint64_t pdfOrder_;
    uint64_t radiusLossOrder_;
};
 
class EllipticDistanceDeterminationAlgorithm : public DistanceDeterminationAlgorithm {
 public:
    explicit EllipticDistanceDeterminationAlgorithm(PositionVector radius);
    ~EllipticDistanceDeterminationAlgorithm();
 
    PositionVector Run(const Points &p) override;
 private:
    // Fields specific to this algorithm, and helper methods
};
 
}  // namespace found
 
#endif  // SRC_DISTANCE_DISTANCE_HPP_