NumberOperations.hpp

Go to the documentation of this file.

 
#ifndef NUMBER_OPERATIONS_HPP
#define NUMBER_OPERATIONS_HPP
 
#include <algorithm>
#include <cmath>
#include <iostream>
 
 
 
namespace numops
{
 
    template<typename T>
    struct CoordinatePoint
    {
        public:
            // Declare public struct member variables.
            T tX;
            T tY;
            T tZ;
 
 
            CoordinatePoint(const T tX = 0.0, const T tY = 0.0, const T tZ = 0.0)
            {
                // Initialize member variables.
                this->tX = tX;
                this->tY = tY;
                this->tZ = tZ;
            }
    };
 
 
    template<typename T>
    inline constexpr T Clamp(T tValue, T tMin, T tMax)
 
    {
        return std::max(std::min(tMax, tValue), tMin);
    }
 
 
    template<typename T>
    inline bool Bounded(T tValue, T tMin, T tMax, const bool bInclusive = true)
    {
        // Check if value is inclusive or not.
        if (bInclusive)
        {
            // Return true if the given value is valid.
            return (tValue >= tMin) && (tValue <= tMax);
        }
        else
        {
            // Return true if the given value is valid.
            return (tValue > tMin) && (tValue < tMax);
        }
    }
 
 
    template<typename T>
    inline constexpr T MapRange(const T tValue, const T tOldMinimum, const T tOldMaximum, const T tNewMinimum, const T tNewMaximum)
    {
        // Check if the ranges are valid.
        if (tOldMinimum == tOldMaximum || tNewMinimum == tNewMaximum)
        {
            // Submit logger message.
            std::cerr << "MAPRANGE: The old/new range is not valid." << std::endl;
 
            // Return old, given value.
            return tValue;
        }
 
        // Perform the mapping using linear interpolation.
        T tOldValueRange = tOldMaximum - tOldMinimum;
        T tNewValueRange = tNewMaximum - tNewMinimum;
        T tScaledValue   = (tValue - tOldMinimum) / tOldValueRange;
 
        // Return new mapped value.
        return tNewMinimum + tScaledValue * tNewValueRange;
    }
 
 
    template<typename T>
    inline constexpr T InputAngleModulus(T tValue, T tMinValue, T tMaxValue)
    {
        // Determine the correct modulus number.
        T tModulus = tMaxValue - tMinValue;
 
        // Wrap input if it's above the maximum input.
        int nNumMax = (tValue - tMinValue) / tModulus;
        tValue -= nNumMax * tModulus;
        // Wrap input if it's below the minimum input.
        int nNumMin = (tValue - tMaxValue) / tModulus;
        tValue -= nNumMin * tModulus;
 
        // Check if the final value is the max value, set to min.
        if (tValue == tMaxValue)
        {
            tValue = tMinValue;
        }
 
        // Return wrapped number.
        return tValue;
    }
 
 
    template<typename T>
    inline constexpr T AngularDifference(T tFirstValue, T tSecondValue)
    {
        // Check input.
        if (!Bounded<T>(tFirstValue, 0, 360) && !Bounded<T>(tSecondValue, 0, 360))
        {
            // Submit logger message.
            std::cerr << "ANGULARDIFFERENCE: An input value is not valid must be between 0-360. The result difference will not be accurate!" << std::endl;
        }
 
        // Find absolute difference between the two values.
        T tDifference = std::abs(tFirstValue - tSecondValue);
        // If greater than 180 degrees, subtract 360/
        if (tDifference > 180)
        {
            // Wrap value.
            tDifference -= 360;
 
            // Check if first values is bigger than second value. If it is, flip sign so that clockwise is positive.
            if (tFirstValue > tSecondValue)
            {
                tDifference *= -1;
            }
        }
 
        // Return value.
        return tDifference;
    }
 
 
    template<typename T>
    inline constexpr void CoordinateFrameRotate3D(std::vector<CoordinatePoint<T>>& vPointCloud,
                                                  const double dXRotationDegrees,
                                                  const double dYRotationDegrees,
                                                  const double dZRotationDegrees)
    {
        // Convert input degrees to radians.
        double dXRotationRadians = (dXRotationDegrees * M_PI) / 180.0;
        double dYRotationRadians = (dYRotationDegrees * M_PI) / 180.0;
        double dZRotationRadians = (dZRotationDegrees * M_PI) / 180.0;
 
        // Find the cosine and sine for the X-axis rotation matrix.
        double dCosA = cos(dXRotationRadians);
        double dSinA = sin(dXRotationRadians);
        // Find the cosine and sine for the Y-axis rotation matrix.
        double dCosB = cos(dYRotationRadians);
        double dSinB = sin(dYRotationRadians);
        // Find the cosine and sine for the Z-axis rotation matrix.
        double dCosC = cos(dZRotationRadians);
        double dSinC = sin(dZRotationRadians);
 
        // Calculate the 3D rotation matrix using matrix multiplication with proper Euler angles Z, Y, X. A = (Rz * Ry * Rx).
        // First row of A matrix.
        double dAXX = dCosC * dCosB;
        double dAXY = dCosC * dSinB * dSinA - dSinC * dCosA;
        double dAXZ = dCosC * dSinB * dCosA + dSinC * dSinA;
        // Second row of A matrix.
        double dAYX = dSinC * dCosB;
        double dAYY = dSinC * dSinB * dSinA + dCosC * dCosA;
        double dAYZ = dSinC * dSinB * dCosA - dCosC * dSinA;
        // Third row of A matrix.
        double dAZX = -dSinB;
        double dAZY = dCosB * dSinA;
        double dAZZ = dCosB * dCosA;
 
        // Loop through each point in the given point cloud.
        for (CoordinatePoint<T>& stPoint : vPointCloud)
        {
            // Rotate the points in X, Y, Z order using the rotation matrix A.
            T tX = static_cast<T>((dAXX * stPoint.tX) + (dAXY * stPoint.tY) + (dAXZ * stPoint.tZ));
            T tY = static_cast<T>((dAYX * stPoint.tX) + (dAYY * stPoint.tY) + (dAYZ * stPoint.tZ));
            T tZ = static_cast<T>((dAZX * stPoint.tX) + (dAZY * stPoint.tY) + (dAZZ * stPoint.tZ));
            // Update point cloud.
            stPoint.tX = tX;
            stPoint.tY = tY;
            stPoint.tZ = tZ;
        }
    }
}    // namespace numops
#endif