Commit 1233c057 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

WIP: Change SceneManager to straighten PBD pipeline

parent fa95e986
......@@ -25,6 +25,8 @@
#include "imstkTetrahedralMesh.h"
#include "imstkMeshIO.h"
#include "imstkOneToOneMap.h"
#include "imstkMeshToMeshBruteForceCD.h"
#include "imstkPBDCollisionHandling.h"
using namespace imstk;
......@@ -184,6 +186,7 @@ int main()
pbdSolverfloor->setPbdObject(floor);
scene->addNonlinearSolver(pbdSolverfloor);
<<<<<<< HEAD
scene->addSceneObject(floor);
// Collisions
......@@ -192,6 +195,153 @@ int main()
pair->setNumberOfInterations(2);
colGraph->addInteractionPair(pair);
=======
auto vs = volTetMesh1->getInitialVertexPositions();
Vec3d tmpPos;
for (int i = 0; i < volTetMesh1->getNumVertices(); ++i)
{
tmpPos = volTetMesh1->getVertexPosition(i);
tmpPos[1] -= 6;
volTetMesh1->setVertexPosition(i, tmpPos);
}
volTetMesh1->setInitialVertexPositions(volTetMesh1->getVertexPositions());
volTetMesh1->extractSurfaceMesh(surfMesh1);
volTetMesh1->extractSurfaceMesh(surfMeshVisual1);
auto deformMapP2V1 = std::make_shared<OneToOneMap>();
deformMapP2V1->setMaster(volTetMesh1);
deformMapP2V1->setSlave(surfMeshVisual1);
deformMapP2V1->compute();
auto deformMapC2V1 = std::make_shared<OneToOneMap>();
deformMapC2V1->setMaster(surfMesh1);
deformMapC2V1->setSlave(surfMeshVisual1);
deformMapC2V1->compute();
auto deformMapP2C1 = std::make_shared<OneToOneMap>();
deformMapP2C1->setMaster(volTetMesh1);
deformMapP2C1->setSlave(surfMesh1);
deformMapP2C1->compute();
auto deformableObj1 = std::make_shared<PbdObject>("Dragon2");
deformableObj1->setVisualGeometry(surfMeshVisual1);
deformableObj1->setCollidingGeometry(surfMesh1);
deformableObj1->setPhysicsGeometry(volTetMesh1);
deformableObj1->setPhysicsToCollidingMap(deformMapP2C1);
deformableObj1->setPhysicsToVisualMap(deformMapP2V1);
deformableObj1->setCollidingToVisualMap(deformMapC2V1);
//deformableObj1->initialize(/*Number of Constraints*/ 1,
// /*Constraint configuration*/ "FEM NeoHookean 10.0 0.5",
// /*Mass*/ 0.0,
// /*Gravity*/ "0 -9.8 0",
// /*TimeStep*/ 0.002,
// /*FixedPoint*/ "",
// /*NumberOfIterationInConstraintSolver*/ 2,
// /*Proximity*/ 0.1,
// /*Contact stiffness*/ 0.01);
scene->addSceneObject(deformableObj1);
// Collisions
auto colGraph = scene->getCollisionGraph();
auto pair = std::make_shared<PbdInteractionPair>(PbdInteractionPair(deformableObj, deformableObj1));
pair->setNumberOfInterations(2);
colGraph->addInteractionPair(pair);
}
else
{
// Build floor geometry
StdVectorOfVec3d vertList;
const double width = 100.0;
const double height = 100.0;
const int nRows = 2;
const int nCols = 2;
vertList.resize(nRows*nCols);
const double dy = width / (double)(nCols - 1);
const double dx = height / (double)(nRows - 1);
for (int i = 0; i < nRows; ++i)
{
for (int j = 0; j < nCols; j++)
{
const double y = (double)dy*j;
const double x = (double)dx*i;
vertList[i*nCols + j] = Vec3d(x - 50, -10.0, y - 50);
}
}
// c. Add connectivity data
std::vector<SurfaceMesh::TriangleArray> triangles;
for (std::size_t i = 0; i < nRows - 1; ++i)
{
for (std::size_t j = 0; j < nCols - 1; j++)
{
SurfaceMesh::TriangleArray tri[2];
tri[0] = { { i*nCols + j, i*nCols + j + 1, (i + 1)*nCols + j } };
tri[1] = { { (i + 1)*nCols + j + 1, (i + 1)*nCols + j, i*nCols + j + 1 } };
triangles.push_back(tri[0]);
triangles.push_back(tri[1]);
}
}
auto floorMesh = std::make_shared<SurfaceMesh>();
floorMesh->initialize(vertList, triangles);
auto materialFloor = std::make_shared<RenderMaterial>();
materialFloor->setDisplayMode(RenderMaterial::DisplayMode::WIREFRAME_SURFACE);
floorMesh->setRenderMaterial(materialFloor);
auto floorMapP2V = std::make_shared<OneToOneMap>();
floorMapP2V->setMaster(floorMesh);
floorMapP2V->setSlave(floorMesh);
floorMapP2V->compute();
auto floorMapP2C = std::make_shared<OneToOneMap>();
floorMapP2C->setMaster(floorMesh);
floorMapP2C->setSlave(floorMesh);
floorMapP2C->compute();
auto floorMapC2V = std::make_shared<OneToOneMap>();
floorMapC2V->setMaster(floorMesh);
floorMapC2V->setSlave(floorMesh);
floorMapC2V->compute();
auto floor = std::make_shared<PbdObject>("Floor");
floor->setCollidingGeometry(floorMesh);
floor->setVisualGeometry(floorMesh);
floor->setPhysicsGeometry(floorMesh);
floor->setPhysicsToCollidingMap(floorMapP2C);
floor->setPhysicsToVisualMap(floorMapP2V);
floor->setCollidingToVisualMap(floorMapC2V);
auto pbdModel2 = std::make_shared<PbdModel>();
pbdModel2->setModelGeometry(floorMesh);
pbdModel2->configure(/*Number of Constraints*/ 0,
/*Mass*/ 0.0,
/*Proximity*/ 0.1,
/*Contact stiffness*/ 1.0);
floor->setDynamicalModel(pbdModel2);
auto pbdSolverfloor = std::make_shared<PbdSolver>();
pbdSolverfloor->setPbdObject(floor);
scene->addNonlinearSolver(pbdSolverfloor);
scene->addSceneObject(floor);
auto colData = std::make_shared<CollisionData>();
auto CD = std::make_shared<MeshToMeshBruteForceCD>(floor->getCollidingGeometry(),
surfMesh,
*colData.get());
auto CH = std::make_shared<PBDCollisionHandling>(CollisionHandling::Side::B,
CD->getCollisionData(),
floor,
deformableObj);
//scene->getCollisionGraph()->addInteractionPair(floor, deformableObj, CD, CH, nullptr);
}
>>>>>>> WIP: Change SceneManager to straighten PBD pipeline
// Light
auto light = std::make_shared<DirectionalLight>("light");
......@@ -200,7 +350,7 @@ int main()
scene->addLight(light);
sdk->setActiveScene(scene);
sdk->startSimulation(SimulationStatus::PAUSED);
sdk->startSimulation(SimulationStatus::RUNNING);
return 0;
}
......@@ -25,4 +25,4 @@ if( BUILD_TESTING )
imstk_add_test(Collision)
imstk_add_data(Collision ${FILE_LIST_COL_TEST})
endif()
\ No newline at end of file
endif()
......@@ -29,6 +29,7 @@
#include "imstkMeshToMeshCD.h"
#include "imstkSphereCylinderCD.h"
#include "imstkPointSetToSpherePickingCD.h"
#include "imstkMeshToMeshBruteForceCD.h"
#include "imstkCollidingObject.h"
#include "imstkPlane.h"
......@@ -167,6 +168,21 @@ CollisionDetection::makeCollisionDetectionObject(const Type& type,
return std::make_shared<PointSetToSpherePickingCD>(mesh, sphere, colData);
}
break;
case Type::MeshToMeshBruteForce:
{
auto geometry1 = std::dynamic_pointer_cast<Geometry>(objA->getCollidingGeometry());
auto surfaceGeo = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
// Geometries check
if (geometry1 == nullptr || surfaceGeo == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for MeshToMeshBruteForce collision detection.";
return nullptr;
}
return std::make_shared<MeshToMeshBruteForceCD>(geometry1, surfaceGeo, colData);
}
break;
default:
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
......
......@@ -23,6 +23,7 @@
#define imstkMeshToMeshBruteforceCD_h
#include <memory>
#include <iostream>
#include "imstkCollisionDetection.h"
......@@ -50,7 +51,8 @@ public:
CollisionDetection(CollisionDetection::Type::MeshToMeshBruteForce,
colData),
m_object1(obj1),
m_object2(obj2){}
m_object2(obj2)
{}
///
/// \brief Destructor
......
......@@ -51,7 +51,8 @@ public:
VirtualCoupling,
NodalPicking,
BoneDrilling,
SPH
SPH,
PBD
};
///
......
......@@ -49,11 +49,15 @@ PBDCollisionHandling::processCollisionData()
<< "no penalty collision handling available for " << m_object->getName()
<< " (rigid mesh not yet supported).";
}*/
this->generatePBDConstraints();
}
void
PBDCollisionHandling::generatePBDConstraints()
{
// clear the constraints before populating wit new ones
m_PBDConstraints.clear();
const auto dynaModel1 = std::static_pointer_cast<PbdModel>(m_pbdObject1->getDynamicalModel());
const auto dynaModel2 = std::static_pointer_cast<PbdModel>(m_pbdObject2->getDynamicalModel());
......@@ -62,6 +66,8 @@ PBDCollisionHandling::generatePBDConstraints()
const auto map1 = m_pbdObject1->getPhysicsToCollidingMap();
const auto map2 = m_pbdObject2->getPhysicsToCollidingMap();
std::cout << "EE: " << m_colData.EEColData.size() << "TV: " << m_colData.TVColData.size() << std::endl;
// Generate edge-edge pbd constraints
for (auto& colData : m_colData.EEColData)
{
......
......@@ -41,11 +41,11 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
}
// Check if objects are different
if (A == B)
/*if (A == B)
{
LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
return;
}
}*/
// Collision Detection
std::shared_ptr<CollisionDetection> CD = CollisionDetection::makeCollisionDetectionObject(CDType, A, B, m_colData);
......@@ -105,11 +105,11 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
}
// Check if objects are different
if (A == B)
/*if (A == B)
{
LOG(WARNING) << "InteractionPair error: object cannot interact with itself.";
return;
}
}*/
m_objects = ObjectsPair(A, B);
m_colDetect = CD;
......
......@@ -43,9 +43,9 @@ namespace imstk
class CollisionGraph
{
public:
using CollidingObjectPtr = std::shared_ptr < CollidingObject >;
using CollisionHandlingPtr = std::shared_ptr < CollisionHandling >;
using CollisionDetectionPtr = std::shared_ptr < CollisionDetection >;
using CollidingObjectPtr = std::shared_ptr<CollidingObject>;
using CollisionHandlingPtr = std::shared_ptr<CollisionHandling>;
using CollisionDetectionPtr = std::shared_ptr<CollisionDetection>;
using InteractionPairPtr = std::shared_ptr<InteractionPair>;
///
......
......@@ -26,7 +26,6 @@
#include "imstkDynamicObject.h"
#include "imstkPbdObject.h"
#include "imstkDeformableObject.h"
#include "imstkVirtualCouplingPBDObject.h"
#include "imstkGeometryMap.h"
#include "imstkTimer.h"
#include "imstkPbdSolver.h"
......@@ -49,15 +48,6 @@ SceneManager::initModule()
{
this->startModuleInNewThread(camController);
}
// Init virtual coupling objects offsets
for (auto obj : m_scene->getSceneObjects())
{
if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
virtualCouplingPBD->initOffsets();
}
}
}
void
......@@ -76,13 +66,7 @@ SceneManager::runModule()
else if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
{
collidingObj->resetForce();
}
// todo: refactor pbd
// description: so that the transform obtained from device can be applied
if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
virtualCouplingPBD->resetCollidingGeometry();
}
}
}
// Update objects controlled by the device controllers
......@@ -94,55 +78,31 @@ SceneManager::runModule()
// Compute collision data per interaction pair
for (auto intPair : m_scene->getCollisionGraph()->getInteractionPairList())
{
intPair->computeCollisionData();
intPair->processCollisionData();
intPair->computeCollisionData();
}
// Apply forces on device
for (auto controller : m_scene->getSceneObjectControllers())
{
controller->applyForces();
// Process collision data per interaction pair
for (auto intPair : m_scene->getCollisionGraph()->getInteractionPairList())
{
intPair->processCollisionData();
}
// Update the solvers
// Run the solvers
for (auto solvers : m_scene->getSolvers())
{
solvers->solve();
}
// Apply the geometry and apply maps to all the objects
for (auto obj : m_scene->getSceneObjects())
{
obj->updateGeometries();
}
// Do collision detection and response for pbd objects
for (auto intPair : m_scene->getCollisionGraph()->getPbdPairList())
{
intPair->resetConstraints();
if (intPair->doBroadPhaseCollision())
{
intPair->doNarrowPhaseCollision();
}
intPair->resolveCollision();
}
// Update velocity of PBD objects
for (auto obj : m_scene->getSceneObjects())
// Apply updated forces on device
for (auto controller : m_scene->getSceneObjectControllers())
{
if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
{
pbdObj->updateVelocity();
}
controller->applyForces();
}
// Set the trackers of virtual coupling PBD objects to out-of-date
// Apply the geometry and apply maps to all the objects
for (auto obj : m_scene->getSceneObjects())
{
if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
virtualCouplingPBD->setTrackerToOutOfDate();
}
obj->updateGeometries();
}
// Set the trackers of the scene object controllers to out-of-date
......
......@@ -31,7 +31,7 @@ namespace imstk
///
/// \class PbdSolver
///
/// \brief Pbd solver
/// \brief Position Based Dynamics solver
///
class PbdSolver : public SolverBase
{
......@@ -76,6 +76,7 @@ public:
{
m_pbdObject->integratePosition();
m_pbdObject->solveConstraints();
m_pbdObject->updateVelocity();
}
///
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment