Program Listing for File math_user_utils.hpp

↰ Return to documentation for file (src/tap/algorithms/math_user_utils.hpp)

/*
 * Copyright (c) 2020-2023 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef TAPROOT_MATH_USER_UTILS_HPP_
#define TAPROOT_MATH_USER_UTILS_HPP_

#include <array>
#include <cinttypes>
#include <cmath>
#include <cstring>

#include "modm/architecture/interface/assert.hpp"
#include "modm/math/geometry/angle.hpp"
#include "modm/math/geometry/quaternion.hpp"
#include "modm/math/geometry/vector3.hpp"

#include "cmsis_mat.hpp"

namespace tap
{
namespace algorithms
{
static constexpr float ACCELERATION_GRAVITY = 9.80665f;

inline bool compareFloatClose(float val1, float val2, float epsilon)
{
    return fabsf(val1 - val2) <= epsilon;
}

template <typename T>
inline T limitVal(T val, T min, T max)
{
    if (min > max)
    {
        return val;
    }
    if (val < min)
    {
        return min;
    }
    else if (val > max)
    {
        return max;
    }
    else
    {
        return val;
    }
}

inline float lowPassFilter(float prevValue, float newValue, float alpha)
{
    if (alpha < 0.0f || alpha > 1.0f)
    {
        return newValue;
    }
    return alpha * newValue + (1.0f - alpha) * prevValue;
}

template <typename From, typename To>
To reinterpretCopy(From from)
{
    static_assert(sizeof(From) == sizeof(To), "can only reinterpret-copy types of the same size");
    To result;
    memcpy(static_cast<void*>(&result), static_cast<void*>(&from), sizeof(To));
    return result;
}

float fastInvSqrt(float x);

CMSISMat<3, 1> cross(const CMSISMat<3, 1>& a, const CMSISMat<3, 1>& b);

CMSISMat<3, 3> fromEulerAngles(const float roll, const float pitch, const float yaw);

void rotateVector(float* x, float* y, float radians);

constexpr int32_t ceil(float num)
{
    return (static_cast<float>(static_cast<int32_t>(num)) == num)
               ? static_cast<int32_t>(num)
               : static_cast<int32_t>(num) + ((num > 0) ? 1 : 0);
}

modm::Vector3f eulerAnglesFromQuaternion(modm::Quaternion<float>& q);

template <typename T>
int getSign(T val)
{
    return (T(0) < val) - (val < T(0));
}

template <typename T, size_t xSize, size_t ySize>
float interpolateLinear2D(
    const std::array<std::array<T, ySize>, xSize>& values,
    const float xMin,
    const float xMax,
    const float dx,
    const float yMin,
    const float yMax,
    const float dy,
    float xDes,
    float yDes)
{
    modm_assert((xMax - xMin) / dx == xSize - 1, "Bilinear Interpolator", "x range error");
    modm_assert((yMax - yMin) / dy == ySize - 1, "Bilinear Interpolator", "y range error");
    float xDesBounded = limitVal(xDes, xMin, xMax);  // no extrapolation allowed
    float yDesBounded = limitVal(yDes, yMin, yMax);  // no extrapolation allowed

    // In each dimension, find the index of the closest point in the LUT below the desired point,
    // then use that index to find the value of the two points which the desired point lies between
    int xIndex = floor((xDesBounded - xMin) / dx);
    xIndex = limitVal(xIndex, 0, static_cast<int>(xSize) - 2);  // Prevent OOB errors
    float x1 = xMin + xIndex * dx;                              // gets value from index
    float x2 = xMin + (xIndex + 1) * dx;

    int yIndex = floor((yDesBounded - yMin) / dy);
    yIndex = limitVal(yIndex, 0, static_cast<int>(ySize) - 2);
    float y1 = yMin + yIndex * dy;
    float y2 = yMin + (yIndex + 1) * dy;

    float q11, q12, q21, q22;  // values of x1y1, x1y2, x2y1, x2y2
    q11 = static_cast<float>(values.at(xIndex).at(yIndex));
    q12 = static_cast<float>(values.at(xIndex).at(yIndex + 1));
    q21 = static_cast<float>(values.at(xIndex + 1).at(yIndex));
    q22 = static_cast<float>(values.at(xIndex + 1).at(yIndex + 1));

    float x2x, y2y, yy1, xx1;  // deltas from each pt to sample pt
    x2x = x2 - xDesBounded;
    y2y = y2 - yDesBounded;
    yy1 = yDesBounded - y1;
    xx1 = xDesBounded - x1;
    // it's essentially a weighted average
    return 1.0f / (dx * dy) *
           (q11 * x2x * y2y + q21 * xx1 * y2y + q12 * x2x * yy1 + q22 * xx1 * yy1);
}

}  // namespace algorithms

}  // namespace tap

#endif  // TAPROOT_MATH_USER_UTILS_HPP_