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 tOldRange = tOldMaximum - tOldMinimum;
        T tNewRange = tNewMaximum - tNewMinimum;
        return (((tValue - tOldMinimum) * tNewRange) / tOldRange) + tNewMinimum;
    }
 
 
    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)
    {
        // Calculate the difference between the two angles.
        T tDifference = tSecondValue - tFirstValue;
        // Wrap the difference around the 0-360 degree range.
        tDifference = InputAngleModulus(tDifference, -180.0, 180.0);
 
        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