Transform.h

Go to the documentation of this file.

#ifndef PBAT_GEOMETRY_SDF_TRANSFORM_H
#define PBAT_GEOMETRY_SDF_TRANSFORM_H
 
#include "TypeDefs.h"
#include "pbat/HostDevice.h"
#include "pbat/common/Concepts.h"
#include "pbat/math/linalg/mini/BinaryOperations.h"
#include "pbat/math/linalg/mini/Eigen.h"
#include "pbat/math/linalg/mini/Product.h"
 
#include <Eigen/SVD>
 
namespace pbat::geometry::sdf {

template <common::CArithmetic TScalar>
struct Transform
{
    using ScalarType = TScalar; 
    Mat3<ScalarType> R;         
    Vec3<ScalarType> t;         
    Transform() = default;
    Transform(Mat3<ScalarType> const& _R, Vec3<ScalarType> const& _t) : R(_R), t(_t) {}
    static Transform Identity()
    {
        return Transform{
            pbat::math::linalg::mini::Identity<ScalarType, 3, 3>(),
            Zero3<ScalarType>()};
    }

    PBAT_HOST_DEVICE Vec3<ScalarType> operator()(Vec3<ScalarType> const& p) const
    {
        return R * p + t;
    }

    PBAT_HOST_DEVICE Vec3<ScalarType> operator/(Vec3<ScalarType> const& p) const
    {
        return R.Transpose() * (p - t);
    }

    void CleanRotation()
    {
        Eigen::Matrix<ScalarType, 3, 3> Reig = math::linalg::mini::ToEigen(R);
        Eigen::JacobiSVD<decltype(Reig)> svd{Reig, Eigen::ComputeFullU | Eigen::ComputeFullV};
        Reig = svd.matrixU() * svd.matrixV().transpose();
        R    = math::linalg::mini::FromEigen(Reig);
    }
};
 
} // namespace pbat::geometry::sdf
 
#endif // PBAT_GEOMETRY_SDF_TRANSFORM_H