compass.hpp

Go to the documentation of this file.

#ifndef SGDK_MATH_GEOM_COMPASS_HPP
#define SGDK_MATH_GEOM_COMPASS_HPP

#include <vector>  // std::vector
#include <swiss_gd_knife/math/zero_checker.hpp>  // sgdk::ZeroChecker
#include <swiss_gd_knife/zref/tangent.hpp>  // sgdk::Tangent
#include <swiss_gd_knife/zref/pi.hpp>  // sgdk::Pi

namespace sgdk {


class ZeroDirection
{
 public:
    static const ZeroDirection NEGATIVE_X_AXIS;

    static const ZeroDirection NEGATIVE_Y_AXIS;

    static const ZeroDirection POSITIVE_X_AXIS;

    static const ZeroDirection POSITIVE_Y_AXIS;

 private:
    bool _is_y_axis;
    bool _is_positive;

    ZeroDirection(const bool is_y_axis, const bool is_positive)
      : _is_y_axis(is_y_axis)
      , _is_positive(is_positive)
    {
    }

 public:

    ZeroDirection(const ZeroDirection& copy)
      : _is_y_axis(copy._is_y_axis)
      , _is_positive(copy._is_positive)
    {
    }


    ZeroDirection& operator=(const ZeroDirection& copy)
    {
        _is_y_axis   = copy._is_y_axis;
        _is_positive = copy._is_positive;
        return *this;
    }

    inline bool isParallelToXAxis() const
    {
        return !_is_y_axis;
    }

    inline bool isParallelToYAxis() const
    {
        return _is_y_axis;
    }

    inline bool isNegative() const
    {
        return !_is_positive;
    }

    inline bool isPositive() const
    {
        return _is_positive;
    }
};

const ZeroDirection
      ZeroDirection::NEGATIVE_X_AXIS
    = ZeroDirection(false, false);
const ZeroDirection
      ZeroDirection::NEGATIVE_Y_AXIS
    = ZeroDirection(true, false);
const ZeroDirection
      ZeroDirection::POSITIVE_X_AXIS
    = ZeroDirection(false, true);
const ZeroDirection
      ZeroDirection::POSITIVE_Y_AXIS
    = ZeroDirection(true, true);


class PositiveRotation
{
 public:
    static const PositiveRotation CLOCKWISE;

    static const PositiveRotation COUNTERCLOCKWISE;

 private:
    bool _is_clockwise;

    explicit PositiveRotation(const bool is_clockwise)
      : _is_clockwise(is_clockwise)
    {
    }

 public:

    PositiveRotation(const PositiveRotation& copy)
      : _is_clockwise(copy._is_clockwise)
    {
    }


    PositiveRotation& operator=(const PositiveRotation& copy)
    {
        _is_clockwise = copy._is_clockwise;
        return *this;
    }

    inline bool isClockwise() const
    {
        return _is_clockwise;
    }

    inline bool isCounterclockwise() const
    {
        return !_is_clockwise;
    }
};

const PositiveRotation
      PositiveRotation::CLOCKWISE
    = PositiveRotation(true);
const PositiveRotation
      PositiveRotation::COUNTERCLOCKWISE
    = PositiveRotation(false);


template <
#ifndef SGDK_DOX
    typename Direction = unsigned int
  , typename DirectionChange = int
  , typename PiConstant = sgdk::Pi
  , typename TangentFunction = sgdk::Tangent
#endif  /* SGDK_DOX */
>
class Compass
{
 public:
#ifdef SGDK_DOX
    typedef implementation_defined RealNumber;
#else
    typedef typename TangentFunction::result_type
            RealNumber;
#endif  /* SGDK_DOX */

 private:
    typedef std::vector<RealNumber>
            RealNumberContainer;

    static const TangentFunction  _tan;

    Direction                     _direction_count;
    RealNumberContainer           _neg_cot_thetas;
    ZeroDirection                 _zero_direction;
    PositiveRotation              _positive_rotation;
    const ZeroChecker<RealNumber> _zero_checker;

 public:

    explicit Compass(
        const Direction direction_count = 4
      , const ZeroDirection& zero_direction = ZeroDirection::POSITIVE_Y_AXIS
      , const PositiveRotation& positive_rotation = PositiveRotation::CLOCKWISE
      , const RealNumber zero_tolerance = 0.00001f
    )
      : _direction_count(direction_count)
      , _neg_cot_thetas(
          (
              (direction_count & 1)
            ? (direction_count - 1)
            : (direction_count >> 1)
          )
          , RealNumber()
        )
      , _zero_direction(zero_direction)
      , _positive_rotation(positive_rotation)
      , _zero_checker(zero_tolerance)
    {
        _validate();
    }


    Compass(const Compass& copy)
      : _direction_count(copy._direction_count)
      , _neg_cot_thetas(copy._neg_cot_thetas)
      , _zero_direction(copy._zero_direction)
      , _positive_rotation(copy._positive_rotation)
      , _zero_checker(copy._zero_checker)
    {
    }


    Compass& operator=(const Compass& copy)
    {
        _direction_count   = copy._direction_count;
        _neg_cot_thetas    = copy._neg_cot_thetas;
        _zero_direction    = copy._zero_direction;
        _positive_rotation = copy._positive_rotation;
        _zero_checker      = copy._zero_checker;
        return *this;
    }

    inline Direction getDirectionCount() const
    {
        return _direction_count;
    }

    Direction getDirection(const RealNumber dx, const RealNumber dy) const;

    DirectionChange
        getDirectionChange(
            const RealNumber dx1
          , const RealNumber dy1
          , const RealNumber dx2
          , const RealNumber dy2
        ) const;

 private:
    void _validate();

 public:
    inline void setDirectionCount(const Direction direction_count)
    {
        _direction_count = direction_count;
        _validate();
    }

    inline void setZeroDirection(const ZeroDirection& zero_direction)
    {
        _zero_direction = zero_direction;
    }

    inline void setPositiveRotation(const PositiveRotation& positive_rotation)
    {
        _positive_rotation = positive_rotation;
    }
};

template <
    typename Direction
  , typename DirectionChange
  , typename PiConstant
  , typename TangentFunction
>
const TangentFunction
    Compass<
        Direction
      , DirectionChange
      , PiConstant
      , TangentFunction
    >::_tan
      = TangentFunction();

#ifndef SGDK_DOX
template <
    typename Direction
  , typename DirectionChange
  , typename PiConstant
  , typename TangentFunction
>
#endif  /* SGDK_DOX */
Direction
    Compass<
#ifndef SGDK_DOX
        Direction
      , DirectionChange
      , PiConstant
      , TangentFunction
#endif  /* SGDK_DOX */
    >::getDirection(
        const RealNumber dx
      , const RealNumber dy
    ) const
{
    if (_direction_count < 2)
    {
        return 0;
    }

    if (_zero_direction.isParallelToYAxis())
    {
        if (_zero_checker.isZero(dx))
        {
            if ((dy < 0) == _zero_direction.isPositive())
            {
                if (_direction_count & 1)
                {
                    if (
                        (dx < 0)
                     == (
                            _zero_direction.isPositive()
                         == _positive_rotation.isClockwise()
                        )
                    )
                    {
                        return (_direction_count + 1) >> 1;
                    }
                    else
                    {
                        return (_direction_count - 1) >> 1;
                    }
                }
                else
                {
                    return _neg_cot_thetas.size();
                }
            }
            else
            {
                return 0;
            }
        }
    }
    else
    {
        if (_zero_checker.isZero(dy))
        {
            if ((dx < 0) == _zero_direction.isPositive())
            {
                if (_direction_count & 1)
                {
                    if (
                        (dy < 0)
                     == (
                            _zero_direction.isPositive()
                         == _positive_rotation.isClockwise()
                        )
                    )
                    {
                        return (_direction_count - 1) >> 1;
                    }
                    else
                    {
                        return (_direction_count + 1) >> 1;
                    }
                }
                else
                {
                    return _neg_cot_thetas.size();
                }
            }
            else
            {
                return 0;
            }
        }
    }

    const RealNumber
        neg_cot_theta
          = _zero_direction.isParallelToYAxis()
          ? (
                (
                    (
                        _positive_rotation.isClockwise()
                     == _zero_direction.isPositive()
                    )
                  ? dy
                  : -dy
                )
              / dx
            )
          : (
                (
                    (
                        _positive_rotation.isClockwise()
                     != _zero_direction.isPositive()
                    )
                  ? dx
                  : -dx
                )
              / dy
            );

    if (_neg_cot_thetas[0] < neg_cot_theta)
    {
        if (
            _zero_direction.isParallelToYAxis()
         && ((dy < 0) == _zero_direction.isPositive())
         || _zero_direction.isParallelToXAxis()
         && ((dx < 0) == _zero_direction.isPositive())
        )
        {
            if (_direction_count & 1)
            {
                if (_zero_direction.isPositive())
                {
                    return (_direction_count + 1) >> 1;
                }
                else
                {
                    return (_direction_count - 1) >> 1;
                }
            }
            else
            {
                return _neg_cot_thetas.size();
            }
        }
        else
        {
            return 0;
        }
    }

    Direction low  = 0;
    Direction high = _neg_cot_thetas.size();
    Direction mid  = ((low + high) & ~1) >> 1;

    while (low < high - 1)
    {
        if (_neg_cot_thetas[mid] < neg_cot_theta)
        {
                high = mid;
        }
        else
        {
           low = mid;
        }

        mid = ((low + high) & ~1) >> 1;
    }

    // We could use mid the rest of the way, at the cost of readability.
    Direction direction = mid;

    if (neg_cot_theta < _neg_cot_thetas[direction])
    {
        ++direction;
    }

    if (_direction_count & 1)
    {
        if (direction & 1)
        {
            ++direction;
            direction >>= 1;

            if (
                _zero_direction.isParallelToYAxis()
             && ((dx < 0) == _positive_rotation.isClockwise())
             || _zero_direction.isParallelToXAxis()
             && ((dy < 0) == _positive_rotation.isCounterclockwise())
            )
            {
                direction += (_direction_count - 1) >> 1;
            }
        }
        else
        {
            direction >>= 1;

            if (
                _zero_direction.isParallelToYAxis()
             && ((dx < 0) == _positive_rotation.isClockwise())
             || _zero_direction.isParallelToXAxis()
             && ((dy < 0) == _positive_rotation.isCounterclockwise())
            )
            {
                direction += (_direction_count + 1) >> 1;

                if (direction == _direction_count)
                {
                    direction = 0;
                }
            }
        }
    }
    else
    {
        if (
            _zero_direction.isParallelToYAxis()
         && ((dx < 0) == _positive_rotation.isClockwise())
         || _zero_direction.isParallelToXAxis()
         && ((dy < 0) == _positive_rotation.isCounterclockwise())
        )
        {
            direction += _neg_cot_thetas.size();

            if (direction == _direction_count)
            {
                direction = 0;
            }
        }
    }

    if (direction && !_zero_direction.isPositive())
    {
        direction = _direction_count - direction;
    }

    return direction;
}

#ifndef SGDK_DOX
template <
    typename Direction
  , typename DirectionChange
  , typename PiConstant
  , typename TangentFunction
>
#endif  /* SGDK_DOX */
DirectionChange
    Compass<
#ifndef SGDK_DOX
        Direction
      , DirectionChange
      , PiConstant
      , TangentFunction
#endif  /* SGDK_DOX */
    >::getDirectionChange(
        const RealNumber dx1
      , const RealNumber dy1
      , const RealNumber dx2
      , const RealNumber dy2
    ) const
{
    const DirectionChange
        back_dir1
          = _direction_count >> 1;
    const DirectionChange
        back_dir2
          = back_dir1 - _direction_count;
    const DirectionChange
        yaw
          = _zero_direction.isParallelToYAxis()
          ? (getDirection(dx2, dy2) - getDirection(dx1, dy1))
          : (getDirection(dy2, dx2) - getDirection(dy1, dx1));

    return
        (back_dir1 < yaw)
      ? (yaw - _direction_count)
      : (back_dir2 < yaw)
      ? yaw
      : (yaw + _direction_count);
}

#ifndef SGDK_DOX
template <
    typename Direction
  , typename DirectionChange
  , typename PiConstant
  , typename TangentFunction
>
#endif  /* SGDK_DOX */
void
    Compass<
#ifndef SGDK_DOX
        Direction
      , DirectionChange
      , PiConstant
      , TangentFunction
#endif  /* SGDK_DOX */
    >::_validate()
{
    if (_direction_count < 2)
    {
        return;
    }
    else if (_direction_count & 1)
    {
        Direction direction = _direction_count - 1;
        const Direction max_direction = direction << 1;
        DirectionChange factor = 2 - _direction_count;

        _neg_cot_thetas.resize(direction);

        while (direction > 0)
        {
            _neg_cot_thetas[--direction]
              = _tan(factor * PiConstant() / max_direction);
            factor += 2;
        }
    }
    else
    {
        const Direction neg_cot_thetas_size = _direction_count >> 1;

        _neg_cot_thetas.resize(neg_cot_thetas_size);

        if (neg_cot_thetas_size & 1)
        {
            DirectionChange factor = -((neg_cot_thetas_size + 1) >> 1);
            Direction direction = neg_cot_thetas_size;

            while (direction > 0)
            {
                _neg_cot_thetas[--direction]
                  = _tan(++factor * PiConstant() / neg_cot_thetas_size);
            }
        }
        else
        {
            Direction direction = neg_cot_thetas_size;
            DirectionChange factor = 1 - neg_cot_thetas_size;

            while (direction > 0)
            {
                _neg_cot_thetas[--direction]
                  = _tan(factor * PiConstant() / _direction_count);
                factor += 2;
            }
        }
    }
}
}  // namespace sgdk

#endif  /* SGDK_MATH_GEOM_COMPASS_HPP */

---

Swiss GD Knife is hosted by .