imstkMath.h

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#pragma once

#define NOMINMAX

#include <vector>
#include <memory>
#include <utility>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <Eigen/Sparse>

#if !defined(_MSC_VER) && __cplusplus <= 201103L
namespace std
{
template<typename T, typename ... Args>
std::unique_ptr<T>
make_unique(Args&& ... args)
{
    return std::unique_ptr<T>(new T(std::forward<Args>(args) ...));
}
} // namespace std
#endif

namespace imstk
{
// 2D vector
// using Vec2f = Eigen::Vector2f;
// using Vec2d = Eigen::Vector2d;
using Vec2f = Eigen::Matrix<float, 2, 1>;
using Vec2d = Eigen::Matrix<double, 2, 1>;
using Vec2i = Eigen::Matrix<int, 2, 1>;
using StdVectorOfVec2f = std::vector<Vec2f, Eigen::aligned_allocator<Vec2f>>;
using StdVectorOfVec2d = std::vector<Vec2d, Eigen::aligned_allocator<Vec2d>>;

// 3D vector
// using Vec3f = Eigen::Vector3f;
// using Vec3d = Eigen::Vector3d;
using Vec3f = Eigen::Matrix<float, 3, 1>;
using Vec3d = Eigen::Matrix<double, 3, 1>;
using Vec3i = Eigen::Matrix<int, 3, 1>;
using StdVectorOfVec3f = std::vector<Vec3f, Eigen::aligned_allocator<Vec3f>>;
using StdVectorOfVec3d = std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>>;

// 4D vector
// using Vec4f = Eigen::Vector4f;
// using Vec4d = Eigen::Vector4d;
using Vec4f = Eigen::Matrix<float, 4, 1>;
using Vec4d = Eigen::Matrix<double, 4, 1>;
using Vec4i = Eigen::Matrix<int, 4, 1>;
using StdVectorOfVec4f = std::vector<Vec4f, Eigen::aligned_allocator<Vec4f>>;
using StdVectorOfVec4d = std::vector<Vec4d, Eigen::aligned_allocator<Vec4d>>;

// 6D vector
using Vec6f = Eigen::Matrix<float, 6, 1>;
using Vec6d = Eigen::Matrix<double, 6, 1>;
using Vec6i = Eigen::Matrix<int, 6, 1>;

// 8D vector
using Vec8i = Eigen::Matrix<int, 8, 1>;

// Dynamic size vector
using Vectorf = Eigen::VectorXf;
using Vectord = Eigen::VectorXd;
using StdVectorOfVectorf = std::vector<Vectorf, Eigen::aligned_allocator<Vectorf>>;
using StdVectorOfVectord = std::vector<Vectord, Eigen::aligned_allocator<Vectord>>;

// Quaternion
using Quatf = Eigen::Quaternion<float>;
using Quatd = Eigen::Quaternion<double>;
using StdVectorOfQuatf = std::vector<Quatf, Eigen::aligned_allocator<Quatf>>;
using StdVectorOfQuatd = std::vector<Quatd, Eigen::aligned_allocator<Quatd>>;

// Angle-Axis
using Rotf = Eigen::AngleAxisf;
using Rotd = Eigen::AngleAxisd;

// 3x3 Matrix
using Mat3f = Eigen::Matrix<float, 3, 3>;
using Mat3d = Eigen::Matrix<double, 3, 3>;
using StdVectorOfMat3d = std::vector<Mat3d, Eigen::aligned_allocator<Mat3d>>;

// 4x4 Matrix
using Mat4f = Eigen::Matrix<float, 4, 4>;
using Mat4d = Eigen::Matrix<double, 4, 4>;

using Matrixf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;

using Matrixd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;

// A dynamic size sparse matrix of doubles
using SparseMatrixf = Eigen::SparseMatrix<float, Eigen::RowMajor>;

// A dynamic size sparse matrix of doubles
using SparseMatrixd = Eigen::SparseMatrix<double, Eigen::RowMajor>;

// Rigid transform (translation and rotation)
using RigidTransform3f = Eigen::Isometry3f;
using RigidTransform3d = Eigen::Isometry3d;

// Affine transform (translation, rotation, scaling and shearing)
using AffineTransform3f = Eigen::Affine3f;
using AffineTransform3d = Eigen::Affine3d;

#define PI               3.14159265358979323846
#define PI_2             1.57079632679489661923
#define PI_4             0.785398163397448309616
#define INV_1_PI         0.318309886183790671538
#define INV_2_PI         0.636619772367581343076
#define TWO_OVER_SQRT_PI 1.12837916709551257390
#define SQRT2            1.41421356237309504880
#define SQRT1_2          0.707106781186547524401
#define NLOG_E           2.71828182845904523536
#define LOG2E            1.44269504088896340736
#define LOG10E           0.434294481903251827651
#define LN2              0.693147180559945309417
#define LN10             2.30258509299404568402

#define MAX_D                std::numeric_limits<double>::max()
#define MIN_D                std::numeric_limits<double>::min()
#define VERY_SMALL_EPSILON_D std::numeric_limits<double>::epsilon()

#define MAX_F                std::numeric_limits<float>::max()
#define MIN_F                std::numeric_limits<float>::min()
#define VERY_SMALL_EPSILON_F std::numeric_limits<float>::epsilon()

static inline Mat4d
mat4dTranslate(const Vec3d& translate)
{
    return AffineTransform3d(Eigen::Translation3d(translate)).matrix();
}

static inline Mat4d
mat4dScale(const Vec3d& scale)
{
    Mat4d mScale = Mat4d::Identity();
    mScale(0, 0) = scale[0];
    mScale(1, 1) = scale[1];
    mScale(2, 2) = scale[2];
    return mScale;
}

static inline Mat4d
mat4dRotation(const Quatd& rotation)
{
    Mat4d mRot = Mat4d::Identity();
    mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
    return mRot;
}

static inline Mat4d
mat4dRotation(const Rotd& rotation)
{
    Mat4d mRot = Mat4d::Identity();
    mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
    return mRot;
}

static inline Mat4d
mat4dRotation(const Mat3d& rotation)
{
    Mat4d mRot = Mat4d::Identity();
    mRot.block<3, 3>(0, 0) = rotation;
    return mRot;
}

static inline void
mat4dTRS(const Mat4d& m, Vec3d& t, Mat3d& r, Vec3d& s)
{
    // Assumes affine, no shear
    const Vec3d& x = m.block<3, 1>(0, 0);
    const Vec3d& y = m.block<3, 1>(0, 1);
    const Vec3d& z = m.block<3, 1>(0, 2);

    s = Vec3d(m.block<3, 1>(0, 0).norm(),
        m.block<3, 1>(0, 1).norm(),
        m.block<3, 1>(0, 2).norm());

    r = Mat3d::Identity();
    r.block<3, 1>(0, 0) = x.normalized();
    r.block<3, 1>(0, 1) = y.normalized();
    r.block<3, 1>(0, 2) = z.normalized();

    t = m.block<3, 1>(0, 3);
}

static double
tetVolume(Vec3d p0, Vec3d p1, Vec3d p2, Vec3d p3)
{
    Mat4d m;
    m.block<1, 3>(0, 0) = p0;
    m.block<1, 3>(1, 0) = p1;
    m.block<1, 3>(2, 0) = p2;
    m.block<1, 3>(3, 0) = p3;
    m(0, 3) = m(1, 3) = m(2, 3) = m(3, 3) = 1.0;
    return m.determinant() / 6.0;
}

static Vec2d
baryCentric(const Vec3d& pt, const Vec3d& p, const Vec3d& q)
{
    Vec3d        dir    = q - p;
    const double length = dir.norm();
    dir /= length;
    const double t = (pt - p).dot(dir) / length;
    return Vec2d(1.0 - t, t);
}

static Vec3d
baryCentric(const Vec2d& p, const Vec2d& a, const Vec2d& b, const Vec2d& c)
{
    const Vec2d  v0  = b - a;
    const Vec2d  v1  = c - a;
    const Vec2d  v2  = p - a;
    const double den = v0[0] * v1[1] - v1[0] * v0[1];
    const double v   = (v2[0] * v1[1] - v1[0] * v2[1]) / den;
    const double w   = (v0[0] * v2[1] - v2[0] * v0[1]) / den;
    const double u   = 1.0 - v - w;
    return Vec3d(u, v, w);
}

static Vec3d
baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
{
    const Vec3d  v0    = b - a;
    const Vec3d  v1    = c - a;
    const Vec3d  v2    = p - a;
    const double d00   = v0.dot(v0);
    const double d01   = v0.dot(v1);
    const double d11   = v1.dot(v1);
    const double d20   = v2.dot(v0);
    const double d21   = v2.dot(v1);
    const double denom = d00 * d11 - d01 * d01;
    const double v     = (d11 * d20 - d01 * d21) / denom;
    const double w     = (d00 * d21 - d01 * d20) / denom;
    const double u     = 1.0 - v - w;
    return Vec3d(u, v, w);
}

static Vec4d
baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c, const Vec3d& d)
{
    Mat4d A;
    A << a[0], a[1], a[2], 1.0,
        b[0], b[1], b[2], 1.0,
        c[0], c[1], c[2], 1.0,
        d[0], d[1], d[2], 1.0;

    Vec4d  weights;
    double det = A.determinant(); // Signed volume
    for (int i = 0; i < 4; ++i)
    {
        Mat4d B = A;
        B(i, 0)    = p[0];
        B(i, 1)    = p[1];
        B(i, 2)    = p[2];
        weights[i] = B.determinant() / det;
    }
    return weights;
}

template<typename T>
static T
cantor(const T a, const T b)
{
    return (a + b) * (a + b + 1) / 2 + b;
}

template<typename T>
static T
symCantor(const T a, const T b)
{
    const T max = std::max<T>(a, b);
    const T min = std::min<T>(a, b);
    return max * (max + 1) / 2 + min;
}
} // namespace imstk