Program Listing for File CubicSDFCollisionDetection.cpp

↰ Return to documentation for file (Simulation/CubicSDFCollisionDetection.cpp)

#include "CubicSDFCollisionDetection.h"
#include "Simulation/IDFactory.h"
#include "Eigen/Dense"

using namespace PBD;

int CubicSDFCollisionDetection::CubicSDFCollisionObject::TYPE_ID = IDFactory::getId();

CubicSDFCollisionDetection::CubicSDFCollisionDetection() :
    DistanceFieldCollisionDetection()
{
}

CubicSDFCollisionDetection::~CubicSDFCollisionDetection()
{
}

bool CubicSDFCollisionDetection::isDistanceFieldCollisionObject(CollisionObject *co) const
{
    return DistanceFieldCollisionDetection::isDistanceFieldCollisionObject(co) ||
        (co->getTypeId() == CubicSDFCollisionDetection::CubicSDFCollisionObject::TYPE_ID);
}

void CubicSDFCollisionDetection::addCubicSDFCollisionObject(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const std::string &sdfFile, const Vector3r &scale, const bool testMesh, const bool invertSDF)
{
    CubicSDFCollisionDetection::CubicSDFCollisionObject *co = new CubicSDFCollisionDetection::CubicSDFCollisionObject();
    co->m_bodyIndex = bodyIndex;
    co->m_bodyType = bodyType;
    co->m_sdfFile = sdfFile;
    co->m_scale = scale;
    co->m_sdf = std::make_shared<Grid>(co->m_sdfFile);
    co->m_bvh.init(vertices, numVertices);
    co->m_bvh.construct();
    co->m_testMesh = testMesh;
    co->m_invertSDF = 1.0;
    if (invertSDF)
        co->m_invertSDF = -1.0;
    m_collisionObjects.push_back(co);
}

void PBD::CubicSDFCollisionDetection::addCubicSDFCollisionObject(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, GridPtr sdf, const Vector3r &scale, const bool testMesh /*= true*/, const bool invertSDF /*= false*/)
{
    CubicSDFCollisionDetection::CubicSDFCollisionObject *co = new CubicSDFCollisionDetection::CubicSDFCollisionObject();
    co->m_bodyIndex = bodyIndex;
    co->m_bodyType = bodyType;
    co->m_sdfFile = "";
    co->m_scale = scale;
    co->m_sdf = sdf;
    co->m_bvh.init(vertices, numVertices);
    co->m_bvh.construct();
    co->m_testMesh = testMesh;
    co->m_invertSDF = 1.0;
    if (invertSDF)
        co->m_invertSDF = -1.0;
    m_collisionObjects.push_back(co);
}

CubicSDFCollisionDetection::CubicSDFCollisionObject::CubicSDFCollisionObject()
{
}

CubicSDFCollisionDetection::CubicSDFCollisionObject::~CubicSDFCollisionObject()
{
}

double CubicSDFCollisionDetection::CubicSDFCollisionObject::distance(const Eigen::Vector3d &x, const Real tolerance)
{
    const Eigen::Vector3d scaled_x = x.cwiseProduct(m_scale.template cast<double>().cwiseInverse());
    const double dist = m_sdf->interpolate(0, scaled_x);
    if (dist == std::numeric_limits<double>::max())
        return dist;
    return m_invertSDF * m_scale[0]*dist - tolerance;
}

bool CubicSDFCollisionDetection::CubicSDFCollisionObject::collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist)
{
    const Vector3r scaled_x = x.cwiseProduct(m_scale.cwiseInverse());

    Eigen::Vector3d normal;
    double d = m_sdf->interpolate(0, scaled_x.template cast<double>(), &normal);
    if (d == std::numeric_limits<Real>::max())
        return false;
    dist = static_cast<Real>(m_invertSDF * d - tolerance);

    normal = m_invertSDF * normal;
    if (dist < maxDist)
    {
        normal.normalize();
        n = normal.template cast<Real>();

        cp = (scaled_x - dist * n);
        cp = cp.cwiseProduct(m_scale);

        return true;
    }
    return false;
}