math.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>
#include <iomanip>

#include <se2ez/core/math.h>
#include <se2ez/core/log.h>

using namespace se2ez;

Eigen::Isometry2d tf::toIsometry(double x, double y, double theta)
{
    Eigen::Isometry2d iso;
    iso.translation() = Eigen::Vector2d(x, y);
    iso.linear() = Eigen::Rotation2D<double>(theta).toRotationMatrix();

    return iso;
}

Eigen::Isometry2d tf::toIsometry(const Eigen::Vector3d &vec)
{
    return toIsometry(vec[0], vec[1], vec[2]);
}

double tf::getRotation(const Eigen::Isometry2d &frame)
{
    return Eigen::Rotation2D<double>(frame.linear()).angle();
}

Eigen::Vector3d tf::flattenIsometry(const Eigen::Isometry2d &frame)
{
    const auto &t = frame.translation();
    return {t[0], t[1], getRotation(frame)};
}

double tf::transformDistance(const Eigen::Isometry2d &t1, const Eigen::Isometry2d &t2, double alpha)
{
    if (alpha > 1 || alpha < 0)
    {
        SE2EZ_WARN("Parameter alpha must be in [0,1] defaulting to 0.75");
        alpha = 0.75;
    }

    return alpha * (t1.translation() - t2.translation()).norm()  //
           + (1 - alpha) * math::angleDistance(getRotation(t1), getRotation(t2));
}

KDL::Frame tf::EigenToKDLFrame(const Eigen::Isometry2d &iso)

{
    const auto &v = flattenIsometry(iso);
    return KDL::Frame(KDL::Rotation::RPY(0., 0., v[2]), KDL::Vector(v[0], v[1], 0));
}

Eigen::Isometry2d tf::KDLToEigenFrame(const KDL::Frame &frame)
{
    Eigen::Isometry2d iso;
    auto pos = frame.p;
    auto rot = frame.M;

    double r, p, y;
    rot.GetRPY(r, p, y);
    return toIsometry(pos.x(), pos.y(), y);
}

KDL::JntArray tf::EigenToKDLVec(const Eigen::VectorXd &vec)
{
    KDL::JntArray jnt(vec.size());
    for (unsigned int i = 0; i < vec.size(); ++i)
        jnt(i) = vec[i];
    return jnt;
}

Eigen::VectorXd tf::KDLToEigenVec(const KDL::JntArray &frame)
{
    return frame.data;
}

std::string tf::printFrame(const Eigen::Isometry2d &frame)
{
    const auto &v = flattenIsometry(frame);
    return printVector(v);
}

std::string tf::printFrame(const KDL::Frame &frame)
{
    return printFrame(KDLToEigenFrame(frame));
}

std::string tf::printVector(const Eigen::VectorXd &v, unsigned int precision)
{
    std::stringstream ss;
    ss.setf(std::ios::fixed, std::ios::floatfield);

    ss.precision(precision);

    ss << "[ ";
    for (unsigned int i = 0; i < v.size(); ++i)
    {
        ss << std::setw(precision + 4) << v[i];
        if (i < v.size() - 1)
            ss << ", ";
    }
    ss << " ]";

    return ss.str();
}

std::string tf::printMatrix(const Eigen::MatrixXd &v, unsigned int precision)
{
    std::stringstream ss;
    ss.setf(std::ios::fixed, std::ios::floatfield);
    ss.precision(precision);

    for (unsigned int j = 0; j < v.rows(); ++j)
    {
        ss << ((j) ? "  " : "[ ");
        for (unsigned int i = 0; i < v.cols(); ++i)
        {
            ss << std::setw(precision + 4) << v(j, i);
            if (i < v.cols() - 1)
                ss << ", ";
        }

        if (j < v.rows() - 1)
            ss << std::endl;
        else
            ss << " ]";
    }

    return ss.str();
}

Eigen::VectorXd math::toVector(double x)
{
    return Eigen::VectorXd::Constant(1, x);
}

Eigen::VectorXd math::toVector(const std::initializer_list<double> &x)
{
    Eigen::VectorXd r(x.size());

    auto it = x.begin();
    for (unsigned int i = 0; i < x.size(); ++i)
        r[i] = *it++;

    return r;
}

Eigen::MatrixXd math::toMatrix(const std::vector<Eigen::VectorXd> &vlist)
{
    Eigen::MatrixXd M(vlist.size(), vlist[0].size());

    for (unsigned int i = 0; i < M.rows(); i++)
        M.row(i) = vlist[i];

    return M;
}

double math::remap(double a1, double a2, double av, double b1, double b2)
{
    double dat = fabs(a2 - a1);
    double dav = fabs(av - a1);

    double dbt = fabs(b2 - b1);
    double dbv = dbt * (dav / dat);

    double bv = b1 + dbv;

    // SE2EZ_DEBUG("%1% - %2% - %3% (%6%/%7%) -> %4% - %10% - %5% (%8%/%9%)",  //
    //             a1, av, a2,                                                 //
    //             b1, b2,                                                     //
    //             dav, dat,                                                   //
    //             dbv, dbt, bv);

    return bv;
}

double math::clamp(double v, double a, double b)
{
     return (v > b) ? b : ((v < a) ? a : v);
}

double math::angleDistance(double v1, double v2)
{
    return angleNormalize(std::fabs(v1 - v2));
}

double math::angleNormalize(double v)
{
    return (v > math::pi) ? math::pipi - v : v;
}

double math::toDegrees(double v)
{
    double n = math::angleNormalize(v);
    double d = n * 180. / math::pi;
    if (n >= 0)
        return d;
    else
        return 360. + d;
}

double math::toRadians(double v)
{
    if (v < 0)
        v += 360.;
    if (v >= 360.)
        v -= 360.;

    if (v <= 180.)
        return v * math::pi / 180.;
    else
        return -(360. - v) * math::pi / 180.;
}

double math::uniformReal(double lower, double upper)
{
    if (lower > upper)
    {
        SE2EZ_ERROR("Lower bound must be lower than upper bound");
        return 0;
    }

    return rng.uniformReal(lower, upper);
}

int math::uniformInteger(int lower, int upper)
{
    if (lower > upper)
        SE2EZ_ERROR("Lower bound must be lower than upper bound");

    return rng.uniformInt(lower, upper);
}

double math::gaussianReal(double mean, double std)
{
    return rng.gaussian(mean, std);
}