Commit e5c39a98 authored by Ricardo Ortiz's avatar Ricardo Ortiz
Browse files

ENH: Merge collision context branch.

Revert some changes and make it build, run FEMSimulator example

CollisionContext/CMakeLists.txt was contributed by Sreekanth.
parent d2f84191
......@@ -295,6 +295,7 @@ if(SimMedTK_USE_OCULUS)
endif()
add_subdirectory(Collision)
add_subdirectory(CollisionContext)
add_subdirectory(ContactHandling)
add_subdirectory(Core)
add_subdirectory(Devices)
......
......@@ -24,7 +24,7 @@
#include "MeshToMeshCollision.h"
// SimMedTK includes
#include "Core/CollisionPair.h"
#include "Core/CollisionManager.h"
#include "Collision/CollisionMoller.h"
#include "Collision/MeshCollisionModel.h"
#include "Collision/SurfaceTree.h"
......@@ -32,7 +32,7 @@
// STL includes
#include <vector>
void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionPair> pairs)
void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> pairs)
{
auto meshA = std::static_pointer_cast<MeshCollisionModel>(pairs->getFirst());
auto meshB = std::static_pointer_cast<MeshCollisionModel>(pairs->getSecond());
......
......@@ -28,7 +28,7 @@
// SimMedTK includes
#include "Core/CollisionDetection.h"
class CollisionPair;
class CollisionManager;
///
/// @brief COllision detection based on BVH queries and
......@@ -43,7 +43,7 @@ public:
MeshToMeshCollision(const MeshToMeshCollision &) = delete;
private:
virtual void doComputeCollision(std::shared_ptr<CollisionPair> pairs) override;
virtual void doComputeCollision(std::shared_ptr<CollisionManager> pairs) override;
};
......
......@@ -25,14 +25,14 @@
// SimMedTK includes
#include "Collision/CollisionMoller.h"
#include "Core/CollisionPair.h"
#include "Core/CollisionManager.h"
#include "Collision/MeshCollisionModel.h"
#include "Collision/PlaneCollisionModel.h"
// STL includes
#include <limits>
void PlaneToMeshCollision::doComputeCollision(std::shared_ptr<CollisionPair> pair)
void PlaneToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> pair)
{
auto meshModel = std::static_pointer_cast<MeshCollisionModel>(pair->getFirst());
auto planeModel = std::static_pointer_cast<PlaneCollisionModel>(pair->getSecond());
......
......@@ -28,7 +28,7 @@
// SimMedTK includes
#include "Core/CollisionDetection.h"
class CollisionPair;
class CollisionManager;
///
/// @brief Brute force collision detection between a mesh and a static plane
......@@ -42,7 +42,7 @@ public:
PlaneToMeshCollision(const PlaneToMeshCollision &) = delete;
private:
virtual void doComputeCollision(std::shared_ptr<CollisionPair> pairs) override;
virtual void doComputeCollision(std::shared_ptr<CollisionManager> pairs) override;
};
......
......@@ -113,7 +113,7 @@ protected:
void updateBVH();
private:
void doComputeCollision(std::shared_ptr<CollisionPair> /*pairs*/)
void doComputeCollision(std::shared_ptr<CollisionManager> /*pairs*/)
{
reset();
updateBVH();
......
......@@ -23,7 +23,7 @@
#include <bandit/bandit.h>
#include "Core/CollisionPair.h"
#include "Core/CollisionManager.h"
#include "Collision/MeshCollisionModel.h"
#include "Core/Model.h"
#include "Core/MakeUnique.h"
......@@ -33,10 +33,10 @@ using namespace bandit;
go_bandit([](){
describe("Collision pair", []() {
it("constructs ", []() {
std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
});
it("attaches models ", []() {
std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
std::shared_ptr<Model> modelA = std::make_shared<MeshCollisionModel>();
std::shared_ptr<Model> modelB = std::make_shared<MeshCollisionModel>();
......@@ -48,14 +48,14 @@ go_bandit([](){
});
it("attaches contacts ", []() {
std::unique_ptr<CollisionPair> collisionPair(Core::make_unique<CollisionPair>());
std::unique_ptr<CollisionManager> collisionPair(Core::make_unique<CollisionManager>());
float depth = 1.0;
core::Vec3d contactPoint(0,0,1);
core::Vec3d normal(1,0,0);
collisionPair->addContact(depth,contactPoint,1,normal);
std::shared_ptr<Contact> contact = collisionPair->getContacts().back();
std::shared_ptr<PenetrationDepthCollisionData> contact = collisionPair->getContacts().back();
AssertThat(collisionPair->hasContacts(), IsTrue());
AssertThat(contact->normal, Equals(normal));
......
......@@ -25,7 +25,7 @@
#include "Collision/MeshToMeshCollision.h"
#include "Collision/MeshCollisionModel.h"
#include "Core/CollisionPair.h"
#include "Core/CollisionManager.h"
#include "Core/MakeUnique.h"
#include "Mesh/SurfaceMesh.h"
......@@ -73,7 +73,7 @@ go_bandit([](){
std::shared_ptr<Model> modelB = getModel(verticesB);
std::shared_ptr<CollisionPair> collisionPair = std::make_shared<CollisionPair>();
std::shared_ptr<CollisionManager> collisionPair = std::make_shared<CollisionManager>();
collisionPair->setModels(modelA,modelB);
......
simmedtk_add_library(CollisionContext
SOURCES
CollisionContext.cpp
PUBLIC_HEADERS
CollisionContext.h
)
target_link_libraries(CollisionContext
PUBLIC
Core
PRIVATE
Event
Collision
Simulators
ContactHandling
)
......@@ -23,39 +23,26 @@
#include "CollisionContext/CollisionContext.h"
InteractionDataElement::InteractionDataElement(
int so1,
int so2,
core::CollisionDetectionType cd,
core::ContactHandlingType ch1,
core::ContactHandlingType ch2,
core::CollisionPairType cp)
{
sceneObj1Id = so1;
sceneObj2Id = so2;
collDetectionType = cd;
contHandlingType1 = ch1;
contHandlingType2 = ch2;
collPairType = cp;
}
//---------------------------------------------------------------------------
bool CollisionContext::addInteraction(
void CollisionContext::addInteraction(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< CollisionDetection > collisionDetection,
std::shared_ptr< ContactHandling > contactHandlingA,
std::shared_ptr< ContactHandling > contactHandlingB,
std::shared_ptr< CollisionDataBase > contactType, bool active)
std::shared_ptr< CollisionManager > contactType, bool active)
{
if (!sceneObjectA ||
!sceneObjectB ||
!collisionDetection ||
!contactHandlingA ||
!contactHandlingB ||
!contactType)
if (!sceneObjectA || !sceneObjectB ||
!collisionDetection || !contactHandlingA ||
!contactHandlingB || !contactType)
{
// TODO: log this
return;
}
if(sceneObjectA != sceneObjectB)
{
return false;
this->totalNumberOfSceneModels++;
}
auto scenePair = std::make_tuple(sceneObjectA, sceneObjectB);
......@@ -64,26 +51,49 @@ bool CollisionContext::addInteraction(
contactHandlingB,
contactType,
active);
this->interactionMap.emplace(scenePair, scenePairData);
}
//---------------------------------------------------------------------------
bool CollisionContext::addInteraction(
void CollisionContext::addInteraction(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< CollisionDetection > collisionDetection)
{
if (!sceneObjectA || !sceneObjectB || !collisionDetection)
{
// TODO: log this
return;
}
if(sceneObjectA != sceneObjectB)
{
this->totalNumberOfSceneModels++;
}
auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
std::get<Detection>(this->interactionMap[pair]) = collisionDetection;
std::get<Enabled>(this->interactionMap[pair]) = true;
}
//---------------------------------------------------------------------------
bool CollisionContext::addInteraction(
void CollisionContext::addInteraction(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< ContactHandling > contactHandler)
{
if (!sceneObjectA || !sceneObjectB)
{
// TODO: log this
return;
}
if(sceneObjectA != sceneObjectB)
{
this->totalNumberOfSceneModels++;
}
auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
std::get<HandlingA>(this->interactionMap[pair]) = contactHandler;
std::get<HandlingB>(this->interactionMap[pair]) = contactHandler;
......@@ -91,18 +101,28 @@ bool CollisionContext::addInteraction(
}
//---------------------------------------------------------------------------
bool CollisionContext::addInteraction(
void CollisionContext::addInteraction(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB)
{
if (!sceneObjectA || !sceneObjectB)
{
// TODO: log this
return;
}
if(sceneObjectA != sceneObjectB)
{
this->totalNumberOfSceneModels++;
}
auto pair = std::make_tuple(sceneObjectA, sceneObjectB);
this->interactionMap[pair] = std::make_tuple(nullptr, nullptr, nullptr, nullptr, false);
}
//---------------------------------------------------------------------------
void CollisionContext::disableInteraction(
std::shared_ptr< SceneObject > sceneObject1,
std::shared_ptr< SceneObject > sceneObject2)
void CollisionContext::disableInteraction(std::shared_ptr< SceneObject > sceneObject1,
std::shared_ptr< SceneObject > sceneObject2)
{
auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
......@@ -120,46 +140,58 @@ void CollisionContext::disableInteraction(
//---------------------------------------------------------------------------
void CollisionContext::removeInteraction(
std::shared_ptr< SceneObject > sceneObject1,
std::shared_ptr< SceneObject > sceneObject2)
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB)
{
auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
if (it == std::end(this->interactionMap))
if (!sceneObjectA || !sceneObjectB)
{
std::cout
<< "Warning: The interaction to be disabled is not found!"
<< std::endl;
// TODO: log this
return;
}
else
auto it = this->interactionMap.find(std::make_tuple(sceneObjectA, sceneObjectB));
if (it == std::end(this->interactionMap))
{
this->interactionMap.erase(it);
// TODO: Log this
return;
}
this->interactionMap.erase(it);
this->totalNumberOfSceneModels--;
}
//---------------------------------------------------------------------------
bool CollisionContext::setCollisionDetection(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< CollisionDetection > collisionDetection)
void CollisionContext::
setCollisionDetection(std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< CollisionDetection > collisionDetection)
{
if (!sceneObjectA || !sceneObjectB || !collisionDetection)
{
// TODO: log this
return;
}
this->addInteraction(sceneObjectA, sceneObjectB, collisionDetection);
}
//---------------------------------------------------------------------------
bool CollisionContext::setContactHandling(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< ContactHandling > contactHandler)
void CollisionContext::
setContactHandling(std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB,
std::shared_ptr< ContactHandling > contactHandler)
{
if (!sceneObjectA || !sceneObjectB || !contactHandler)
{
// TODO: log this
return;
}
this->addInteraction(sceneObjectA, sceneObjectB, contactHandler);
}
//---------------------------------------------------------------------------
bool CollisionContext::exist(
std::shared_ptr< SceneObject > sceneObject1,
std::shared_ptr< SceneObject > sceneObject2)
bool CollisionContext::exist(std::shared_ptr< SceneObject > sceneObject1,
std::shared_ptr< SceneObject > sceneObject2)
{
auto it = this->interactionMap.find(std::make_tuple(sceneObject1, sceneObject2));
return it != std::end(this->interactionMap);
......@@ -194,16 +226,13 @@ void CollisionContext::createAssemblerAdjacencyMatrix()
// set the size of the adjacency matrix
this->interactionMatrix.resize(numSceneObjects);
for (int i = 0; i < numSceneObjects; ++i)
for (size_t i = 0; i < numSceneObjects; ++i)
{
this->interactionMatrix[i].resize(numSceneObjects);
}
std::vector<Eigen::Triplet<int>> triplets;
// Populate the assembly adjacency matrix
int i = 0, j = 0;
for (auto & interaction : this->interactionMap)
{
if (!std::get<Enabled>(interaction.second))
......@@ -224,31 +253,31 @@ void CollisionContext::createAssemblerAdjacencyMatrix()
}
else
{
this->interactionMatrix[i.second][j.second] = 1;
this->interactionMatrix[i->second][j->second] = 1;
}
}
for (auto & st : this->solveTogether)//
for (auto & st : this->modelPairs)//
{
auto objA = this->objectIndexMap.find(st.first);
auto objB = this->objectIndexMap.find(st.second);
auto i = this->objectIndexMap.find(st.first);
auto j = this->objectIndexMap.find(st.second);
if (objA == std::end(this->objectIndexMap) || objB == std::end(this->objectIndexMap))
if (i == std::end(this->objectIndexMap) || j == std::end(this->objectIndexMap))
{
std::cout << "Warning: scene object does not have an index" << std::endl;
continue;
}
else
{
this->interactionMatrix[objA.second][objB.second] = 1;
this->interactionMatrix[i->second][j->second] = 1;
}
}
}
//---------------------------------------------------------------------------
void CollisionContext::solveTogether(
std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB)
void CollisionContext::solveSimultaneously(std::shared_ptr< SceneObject > sceneObjectA,
std::shared_ptr< SceneObject > sceneObjectB)
{
auto i = this->objectIndexMap.find(sceneObjectA);
auto j = this->objectIndexMap.find(sceneObjectB);
......@@ -259,8 +288,7 @@ void CollisionContext::solveTogether(
}
else
{
auto objPair = std::make_pair(sceneObjectA, sceneObjectB);
this->solveTogether.push_back(objPair);
this->modelPairs.emplace_back(sceneObjectA, sceneObjectB);
}
}
......@@ -272,19 +300,19 @@ bool CollisionContext::configure()
}
//---------------------------------------------------------------------------
std::size_t CollisionContext::getNumberOfInterations() const
std::size_t CollisionContext::getNumberOfInteractions() const
{
return this->interactionMap.size();
}
//---------------------------------------------------------------------------
CollisionContext::InteractionMapType &CollisionContext::getInteractions() const
const CollisionContext::InteractionMapType &CollisionContext::getInteractions() const
{
return this->interactionMap;
}
//---------------------------------------------------------------------------
std::vector< std::shared_ptr< ContactHandling > > CollisionContext::getContactHandlers()
std::vector<std::shared_ptr<ContactHandling>> CollisionContext::getContactHandlers()
{
std::vector<std::shared_ptr<ContactHandling>> handlerList;
......@@ -318,62 +346,39 @@ void CollisionContext::findIslands()
}
int CollisionContext::getNumOfIslands()
//---------------------------------------------------------------------------
size_t CollisionContext::getNumOfIslands()
{
return this->islands->size();
return this->islands.size();
}
//---------------------------------------------------------------------------
std::vector<std::vector<int>>& CollisionContext::getIslands()
{
return this->islands;
}
std::shared_ptr<SceneObject> CollisionContext::getObjectWithIndex(const int objIndex)
{
for (int i = 0; i < this->objectIndexMap.size(); ++i)
{
if (this->objectIndexMap.at(i).second == objIndex)
{
return this->objectIndexMap.at(i).second;
}
}
}
//---------------------------------------------------------------------------
template<typename T>
int CollisionContext::numberOfInteractions() const
void CollisionContext::formIslands()
{
int count = 0;
auto counter = [&](const InteractionPairDataType &data)
if(this->totalNumberOfSceneModels == 0)
{
if (std::dynamic_pointer_cast<T>(std::get<HandlingA>(data)) ||
std::dynamic_pointer_cast<T>(std::get<HandlingB>(data)))
{
++count;
}
};
std::for_each(this->interactionMap.begin(), interactionMap.end(), counter);
return count;
}
// TODO: Log this
return;
}
//---------------------------------------------------------------------------
// \todo test
void CollisionContext::formIslands()
{
int nSceneObj, nIslands;
std::vector<bool> visited;
visited.resize(nSceneObj);
std::vector<bool> visited(this->totalNumberOfSceneModels,false);
nIslands = 0;
for (int i = 0; i < nSceneObj; i++)
int nIslands = 0;
for (int i = 0; i < this->totalNumberOfSceneModels; i++)
{
if (!visited[i])
{
nIslands++;
std::vector<int> memberList;
appendNeighbors(visited, memberList, i);
visited[i] = 1;
memberList.push_back(i);
visited[i] = true;
nIslands++;
this->appendNeighbors(visited, memberList, i);
this->islands.push_back(memberList);
}
......@@ -382,19 +387,22 @@ void CollisionContext::formIslands()
//---------------------------------------------------------------------------
// \todo test
void CollisionContext::appendNeighbors(
std::vector<bool>& visited, std::vector<int>& memberList, int row)
void CollisionContext::appendNeighbors(std::vector<bool>& visited,
std::vector<int>& memberList,
int row)
{
for (int i = row; i < interactionMatrix.size(); ++i)// only check the upper triangular matrix for now
// NOTE: only check the upper triangular matrix for now
for (size_t col = row, end = interactionMatrix.size(); col < end; ++col)
{
if (interactionMatrix[row][i] != 0
&& std::find(memberList.begin(), memberList.end(), interactionMatrix[row][i]) != memberList.end())
const auto &entry = interactionMatrix[row][col];
if (entry != 0 &&
std::find(std::begin(memberList), std::end(memberList), entry) != memberList.end())
{
memberList.push_back(interactionMatrix[row][i]);
if (!visited[interactionMatrix[row][i]])
memberList.push_back(interactionMatrix[row][col]);
if (!visited[interactionMatrix[row][col]])
{
appendNeighbors(visited, memberList, interactionMatrix[row][i]);
this->appendNeighbors(visited, memberList, interactionMatrix[row][col]);
}
}
}
}
\ No newline at end of file
}
......@@ -27,14 +27,13 @@
// SimMedTK includes
#include "Core/CoreClass.h"
#include "Core/SDK.h"
#include "Core/Matrix.h"
#include "Core/Config.h"
#include "Core/SceneObject.h"
#include "SceneModels/SceneObject.h"
// collision detection includes
#include "Core/CollisionDetection.h"
#include "Core/CollisionDataManager.h"
#include "Core/CollisionManager.h"
#include "Collision/PlaneToMeshCollision.h"
#include "Collision/SpatialHashCollision.h"
......@@ -42,27 +41,6 @@
#include "Core/ContactHandling.h"
#include "ContactHandling/PenaltyContactFemToStatic.h"
///
/// \brief Contains the information required to define an interaction
///