Commit a7158781 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Merge branch 'addSupportForPointSet' into 'master'

Add support for point set

See merge request !202
parents 4098bd5e 4c47e8b4
......@@ -24,11 +24,11 @@
#include "imstkUnidirectionalPlaneToSphereCD.h"
#include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkSphereToSphereCD.h"
#include "imstkMeshToSphereCD.h"
#include "imstkMeshToPlaneCD.h"
#include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToPlaneCD.h"
#include "imstkMeshToMeshCD.h"
#include "imstkSphereCylinderCD.h"
#include "imstkMeshToSpherePickingCD.h"
#include "imstkPointSetToSpherePickingCD.h"
#include "imstkCollidingObject.h"
#include "imstkPlane.h"
......@@ -107,9 +107,9 @@ CollisionDetection::make_collision_detection(const Type& type,
return std::make_shared<SphereCylinderCD>(sphere, cylinder, colData);
}
break;
case Type::MeshToSphere:
case Type::PointSetToSphere:
{
auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
......@@ -119,12 +119,12 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<MeshToSphereCD>(mesh, sphere, colData);
return std::make_shared<PointSetToSphereCD>(mesh, sphere, colData);
}
break;
case Type::MeshToPlane:
case Type::PointSetToPlane:
{
auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
// Geometries check
......@@ -134,7 +134,7 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<MeshToPlaneCD>(mesh, plane, colData);
return std::make_shared<PointSetToPlaneCD>(mesh, plane, colData);
}
break;
case Type::MeshToMesh:
......@@ -152,9 +152,9 @@ CollisionDetection::make_collision_detection(const Type& type,
return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
}
break;
case Type::MeshToSpherePicking:
case Type::PointSetToSpherePicking:
{
auto mesh = std::dynamic_pointer_cast<Mesh>(objA->getCollidingGeometry());
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
......@@ -164,7 +164,7 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<MeshToSpherePickingCD>(mesh, sphere, colData);
return std::make_shared<PointSetToSpherePickingCD>(mesh, sphere, colData);
}
break;
default:
......
......@@ -47,11 +47,11 @@ public:
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
MeshToSphere,
MeshToPlane,
PointSetToSphere,
PointSetToPlane,
MeshToMesh,
MeshToCapsule,
MeshToSpherePicking
PointSetToCapsule,
PointSetToSpherePicking
};
///
......
......@@ -19,11 +19,11 @@
=========================================================================*/
#include "imstkMeshToCapsuleCD.h"
#include "imstkPointSetToCapsuleCD.h"
#include "imstkCollisionData.h"
#include "imstkCapsule.h"
#include "imstkMesh.h"
#include "imstkPointSet.h"
#include "imstkMath.h"
#include <g3log/g3log.hpp>
......@@ -31,7 +31,7 @@
namespace imstk
{
void
MeshToCapsuleCD::computeCollisionData()
PointSetToCapsuleCD::computeCollisionData()
{
// Clear collisionData
m_colData.clearAll();
......@@ -50,7 +50,7 @@ MeshToCapsuleCD::computeCollisionData()
auto pDotp0 = p.dot(p0);
size_t nodeId = 0;
for (const auto& q : m_mesh->getVertexPositions())
for (const auto& q : m_pointSet->getVertexPositions())
{
// First, check collision with bounding sphere
if ((mid - q).norm() > (radius + length*0.5))
......
......@@ -19,8 +19,8 @@
=========================================================================*/
#ifndef imstkMeshToCapsuleCD_h
#define imstkMeshToCapsuleCD_h
#ifndef imstkPointSetToCapsuleCD_h
#define imstkPointSetToCapsuleCD_h
#include <memory>
......@@ -28,34 +28,34 @@
namespace imstk
{
class Mesh;
class PointSet;
class Capsule;
class CollisionData;
///
/// \class MeshToCapsuleCD
/// \class PointSetToCapsuleCD
///
/// \brief Mesh to Capsule collision detection
/// \brief PointSet to Capsule collision detection
///
class MeshToCapsuleCD : public CollisionDetection
class PointSetToCapsuleCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
MeshToCapsuleCD(std::shared_ptr<Mesh> mesh,
std::shared_ptr<Capsule> capsule,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::MeshToCapsule,
PointSetToCapsuleCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Capsule> capsule,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PointSetToCapsule,
colData),
m_mesh(mesh),
m_pointSet(pointSet),
m_capsule(capsule){}
///
/// \brief Destructor
///
~MeshToCapsuleCD() = default;
~PointSetToCapsuleCD() = default;
///
/// \brief Detect collision and compute collision data
......@@ -64,9 +64,9 @@ public:
private:
std::shared_ptr<Mesh> m_mesh; ///> Mesh
std::shared_ptr<Capsule> m_capsule; ///> Capsule
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Capsule> m_capsule; ///> Capsule
};
}
#endif // ifndef imstkMeshToCapsuleCD_h
\ No newline at end of file
#endif // ifndef imstkPointSetToCapsuleCD_h
\ No newline at end of file
......@@ -19,19 +19,19 @@
=========================================================================*/
#include "imstkMeshToPlaneCD.h"
#include "imstkPointSetToPlaneCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkMesh.h"
#include "imstkPointSet.h"
#include <g3log/g3log.hpp>
namespace imstk
{
void
MeshToPlaneCD::computeCollisionData()
PointSetToPlaneCD::computeCollisionData()
{
// Clear collisionData
m_colData.clearAll();
......@@ -43,7 +43,7 @@ MeshToPlaneCD::computeCollisionData()
auto planeNormal = m_plane->getNormal();
size_t nodeId = 0;
for (const auto& p : m_mesh->getVertexPositions())
for (const auto& p : m_pointSet->getVertexPositions())
{
auto peneDistance = (planePos - p).dot(planeNormal);
if (peneDistance <= 0.0)
......
......@@ -19,8 +19,8 @@
=========================================================================*/
#ifndef imstkMeshToPlaneCD_h
#define imstkMeshToPlaneCD_h
#ifndef imstkPointSetToPlaneCD_h
#define imstkPointSetToPlaneCD_h
#include <memory>
......@@ -28,34 +28,34 @@
namespace imstk
{
class Mesh;
class PointSet;
class Plane;
class CollisionData;
///
/// \class MeshToPlaneCD
/// \class PointSetToPlaneCD
///
/// \brief Mesh to plane collision detection
/// \brief PointSet to plane collision detection
///
class MeshToPlaneCD : public CollisionDetection
class PointSetToPlaneCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
MeshToPlaneCD(std::shared_ptr<Mesh> mesh,
std::shared_ptr<Plane> plane,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::MeshToSphere,
PointSetToPlaneCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Plane> plane,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_mesh(mesh),
m_pointSet(pointSet),
m_plane(plane){}
///
/// \brief Destructor
///
~MeshToPlaneCD() = default;
~PointSetToPlaneCD() = default;
///
/// \brief Detect collision and compute collision data
......@@ -64,9 +64,9 @@ public:
private:
std::shared_ptr<Mesh> m_mesh; ///> Mesh
std::shared_ptr<Plane> m_plane; ///> Plane
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Plane> m_plane; ///> Plane
};
}
#endif // ifndef imstkMeshToPlaneCD_h
#endif // ifndef imstkPointSetToPlaneCD_h
......@@ -19,19 +19,19 @@
=========================================================================*/
#include "imstkMeshToSphereCD.h"
#include "imstkPointSetToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkSphere.h"
#include "imstkMesh.h"
#include "imstkPointSet.h"
#include <g3log/g3log.hpp>
namespace imstk
{
void
MeshToSphereCD::computeCollisionData()
PointSetToSphereCD::computeCollisionData()
{
// Clear collisionData
m_colData.clearAll();
......@@ -41,7 +41,7 @@ MeshToSphereCD::computeCollisionData()
auto radius = m_sphere->getRadius();
size_t nodeId = 0;
for (const auto& p : m_mesh->getVertexPositions())
for (const auto& p : m_pointSet->getVertexPositions())
{
auto dist = (spherePos - p).norm();
if (dist <= radius)
......
......@@ -19,8 +19,8 @@
=========================================================================*/
#ifndef imstkMeshToSphereCD_h
#define imstkMeshToSphereCD_h
#ifndef imstkPointSetToSphereCD_h
#define imstkPointSetToSphereCD_h
#include <memory>
......@@ -28,34 +28,34 @@
namespace imstk
{
class Mesh;
class PointSet;
class Sphere;
class CollisionData;
///
/// \class MeshToSphereCD
/// \class PointSetToSphereCD
///
/// \brief Mesh to sphere collision detection
/// \brief PointSet to sphere collision detection
///
class MeshToSphereCD : public CollisionDetection
class PointSetToSphereCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
MeshToSphereCD(std::shared_ptr<Mesh> mesh,
std::shared_ptr<Sphere> sphere,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::MeshToSphere,
PointSetToSphereCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_mesh(mesh),
m_pointSet(pointSet),
m_sphere(sphere){}
///
/// \brief Destructor
///
~MeshToSphereCD() = default;
~PointSetToSphereCD() = default;
///
/// \brief Detect collision and compute collision data
......@@ -64,9 +64,9 @@ public:
private:
std::shared_ptr<Mesh> m_mesh; ///> Mesh
std::shared_ptr<Sphere> m_sphere; ///> Sphere
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Sphere> m_sphere; ///> Sphere
};
}
#endif // ifndef imstkMeshToSphereCD_h
#endif // ifndef imstkPointSetToSphereCD_h
......@@ -19,15 +19,15 @@ limitations under the License.
=========================================================================*/
#include "imstkMeshToSpherePickingCD.h"
#include "imstkPointSetToSpherePickingCD.h"
#include "imstkCollisionData.h"
#include "imstkMesh.h"
#include "imstkPointSet.h"
#include "imstkSphere.h"
namespace imstk
{
void
MeshToSpherePickingCD::computeCollisionData()
PointSetToSpherePickingCD::computeCollisionData()
{
// Clear collisionData
m_colData.clearAll();
......@@ -42,7 +42,7 @@ MeshToSpherePickingCD::computeCollisionData()
auto radius = m_sphere->getRadius()*m_sphere->getScaling();
size_t nodeId = 0;
for (const auto& p : m_mesh->getVertexPositions())
for (const auto& p : m_pointSet->getVertexPositions())
{
auto dist = (spherePos - p).norm();
if (dist <= radius)
......
......@@ -19,8 +19,8 @@
=========================================================================*/
#ifndef imstkMeshToSpherePickingCD_h
#define imstkMeshToSpherePickingCD_h
#ifndef imstkPointSetToSpherePickingCD_h
#define imstkPointSetToSpherePickingCD_h
#include <memory>
......@@ -29,34 +29,34 @@
namespace imstk
{
class Mesh;
class PointSet;
class Sphere;
class CollisionData;
///
/// \class MeshToSpherePickingCD
/// \class PointSetToSpherePickingCD
///
/// \brief Mesh to sphere collision detection while picking
/// \brief PointSet to sphere collision detection while picking
///
class MeshToSpherePickingCD : public CollisionDetection
class PointSetToSpherePickingCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
MeshToSpherePickingCD(std::shared_ptr<Mesh> mesh,
std::shared_ptr<Sphere> sphere,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::MeshToSphere,
PointSetToSpherePickingCD(std::shared_ptr<PointSet> pointSet,
std::shared_ptr<Sphere> sphere,
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSphere,
colData),
m_mesh(mesh),
m_pointSet(pointSet),
m_sphere(sphere){}
///
/// \brief Destructor
///
~MeshToSpherePickingCD() = default;
~PointSetToSpherePickingCD() = default;
///
/// \brief Detect collision and compute collision data
......@@ -70,12 +70,12 @@ public:
const unsigned int buttonId = 0){ m_deviceTracker = devTracker; }
private:
std::shared_ptr<Mesh> m_mesh; ///> Mesh
std::shared_ptr<Sphere> m_sphere; ///> Sphere
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Sphere> m_sphere; ///> Sphere
std::shared_ptr<imstk::DeviceTracker> m_deviceTracker; ///> Device tracker to get the button status
unsigned int m_buttonId = 0; ///> button id
unsigned int m_buttonId = 0; ///> button id
};
}
#endif // ifndef imstkMeshToSphereCD_h
#endif // ifndef imstkPointSetToSphereCD_h
......@@ -58,7 +58,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
const auto& Uprev = deformableObj->getDisplacements();
const auto& Vprev = deformableObj->getVelocities();
auto PhysTetMesh = std::dynamic_pointer_cast<Mesh>(deformableObj->getPhysicsGeometry());
auto PhysTetMesh = std::dynamic_pointer_cast<PointSet>(deformableObj->getPhysicsGeometry());
auto dT = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_object->getDynamicalModel())->getTimeIntegrator()->getTimestepSize();
// If collision data, append LPC constraints
......
......@@ -32,8 +32,8 @@ PbdInteractionPair::doBroadPhaseCollision()
{
auto g1 = first->getCollidingGeometry();
auto g2 = second->getCollidingGeometry();
auto mesh1 = std::static_pointer_cast<Mesh>(g1);
auto mesh2 = std::static_pointer_cast<Mesh>(g2);
auto mesh1 = std::static_pointer_cast<PointSet>(g1);
auto mesh2 = std::static_pointer_cast<PointSet>(g2);
Vec3d min1, max1;
mesh1->computeBoundingBox(min1, max1);
......@@ -93,7 +93,7 @@ PbdInteractionPair::doNarrowPhaseCollision()
p2[0], p2[1], p2[2], prox1, prox2))
{
auto c = std::make_shared<PbdPointTriangleConstraint>();
c->initConstraint(dynaModel1, map1->getMapIdx(i),
c->initConstraint(dynaModel1, i,
dynaModel2, map2->getMapIdx(e[0]), map2->getMapIdx(e[1]), map2->getMapIdx(e[2]));
m_collisionConstraints.push_back(c);
}
......@@ -166,6 +166,38 @@ PbdInteractionPair::doNarrowPhaseCollision()
}
}
}
else if (g1->getType() == Geometry::Type::PointSet)
{
auto mesh1 = std::static_pointer_cast<PointSet>(g1);
// brute force, use BVH or spatial grid would be much better
// point
for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
{
const auto p = mesh1->getVertexPosition(i);
auto elements = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements.size(); ++j)
{
auto& e = elements[j];
const Vec3d p0 = mesh2->getVertexPosition(e[0]);
const Vec3d p1 = mesh2->getVertexPosition(e[1]);
const Vec3d p2 = mesh2->getVertexPosition(e[2]);
if (testPointToTriAABB(p[0], p[1], p[2],
p0[0], p0[1], p0[2],
p1[0], p1[1], p1[2],
p2[0], p2[1], p2[2], prox1, prox2))
{
auto c = std::make_shared<PbdPointTriangleConstraint>();
auto mappedIndex1 = (map1) ? map1->getMapIdx(i) : i;
c->initConstraint(dynaModel1, mappedIndex1, dynaModel2, map2->getMapIdx(e[0]), map2->getMapIdx(e[1]), map2->getMapIdx(e[2]));
m_collisionConstraints.push_back(c);
}
}
}
}
else
{
auto mesh1 = std::static_pointer_cast<SurfaceMesh>(g1);
......@@ -175,7 +207,7 @@ PbdInteractionPair::doNarrowPhaseCollision()
for (size_t i = 0; i < mesh1->getNumVertices(); ++i)
{
const Vec3d p = mesh1->getVertexPosition(i);
std::vector<SurfaceMesh::TriangleArray> elements = mesh2->getTrianglesVertices();
auto elements = mesh2->getTrianglesVertices();
for (size_t j = 0; j < elements.size(); ++j)
{
......
......@@ -41,10 +41,6 @@ PbdConstantDensityConstraint::initConstraint(PbdModel& model, const double k)
m_numNeighbors.resize(np);
m_deltaPositions.resize(np);
m_neighbors.resize(np * m_maxNumNeighbors);
m_xPosIndexes.resize(np);
m_yPosIndexes.resize(np);
m_zPosIndexes.resize(np);
}
bool
......@@ -101,7 +97,7 @@ PbdConstantDensityConstraint::gradSpiky(const Vec3d &pi, const Vec3d &pj)
}
inline void
PbdConstantDensityConstraint::clearNeighbors(const size_t &np)
PbdConstantDensityConstraint::clearNeighbors(const size_t np)
{
m_numNeighbors.clear();
m_neighbors.clear();
......@@ -111,7 +107,7 @@ PbdConstantDensityConstraint::clearNeighbors(const size_t &np)
inline void
PbdConstantDensityConstraint::updateNeighborsBruteForce(const Vec3d &pi,
const size_t &index,
const size_t index,
const StdVectorOfVec3d &positions)
{
const double neighborRadius = m_maxDist;
......@@ -141,7 +137,7 @@ PbdConstantDensityConstraint::updateNeighborsBruteForce(const Vec3d &pi,