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

ENH: Multiple enhancements all over.

Many are formating changes and others are
bug fixes as well as some refactoring.
parent 8d04948c
......@@ -18,16 +18,48 @@
// limitations under the License.
#include "Assembler/Assembler.h"
// iMSTK includes
#include "CollisionContext/CollisionContext.h"
#include "Core/ContactHandling.h"
#include "Core/Model.h"
#include "TimeIntegrators/OdeSystem.h"
#include "Solvers/SystemOfEquations.h"
Assembler::Assembler(std::shared_ptr<CollisionContext> collContext)
Assembler::Assembler(std::shared_ptr <CollisionContext> collContext)
{
this->collisionContext = collContext;
this->initSystem();
}
//---------------------------------------------------------------------------
void Assembler::
setCollisionContext(std::shared_ptr< CollisionContext > newCollisionContext)
{
this->collisionContext = newCollisionContext;
}
//---------------------------------------------------------------------------
std::shared_ptr<CollisionContext > Assembler::getCollisionContext() const
{
return this->collisionContext;
}
//---------------------------------------------------------------------------
void Assembler::
setSystemOfEquations(
std::vector<std::shared_ptr<SparseLinearSystem>> newSystemOfEquations)
{
this->equationList = newSystemOfEquations;
}
//---------------------------------------------------------------------------
std::vector<std::shared_ptr<Assembler::SparseLinearSystem>> Assembler::
getSystemOfEquations() const
{
return this->equationList;
}
//---------------------------------------------------------------------------
void Assembler::type1Interactions()
{
......@@ -39,7 +71,7 @@ void Assembler::type1Interactions()
auto ch = this->collisionContext->getContactHandlers();
for(auto &ch : this->collisionContext->getContactHandlers())
for(auto & ch : this->collisionContext->getContactHandlers())
{
const auto &forces = ch->getContactForces();
auto sceneModel = ch->getSecondSceneObject();
......@@ -50,58 +82,57 @@ void Assembler::type1Interactions()
//---------------------------------------------------------------------------
void Assembler::initSystem()
{
for(auto &rows : this->collisionContext->getIslands())
for(auto & rows : this->collisionContext->getIslands())
{
size_t dofSize = 0;
size_t nnz = 0;
std::vector<const core::SparseMatrixd*> systemMatrices;
std::vector<const core::Vectord*> rhsVector;
for(auto &col : rows)
std::vector<const core::SparseMatrixd *> systemMatrices;
std::vector<const core::Vectord *> rhsVector;
for(auto & col : rows)
{
// For the moment only DeformableSceneObject are SystemsOfEquations
auto sceneModel = this->collisionContext->getSceneModel(col);
auto odeSystem = std::dynamic_pointer_cast<OdeSystem>(
this->collisionContext->getSceneModel(col));
if(sceneModel && odeSystem)
auto linearSystem = std::dynamic_pointer_cast<SparseLinearSystem>(sceneModel);
if(sceneModel && linearSystem)
{
dofSize += sceneModel->getNumOfDOF();
systemMatrices.push_back(&odeSystem->getSystemMatrix());
rhsVector.push_back(&odeSystem->getRHS());
nnz += systemMatrices.back()->nonZeros();
this->equationList.push_back(linearSystem);
nnz += linearSystem->getMatrix().nonZeros();
dofSize += linearSystem->getRHSVector().size();
}
}
if(dofSize > 0)
{
this->A.emplace_back(dofSize,dofSize);
this->A.emplace_back(dofSize, dofSize);
this->A.back().reserve(nnz);
this->b.emplace_back(dofSize);
auto system = std::make_shared<LinearSystemType>(this->A.back(),this->b.back());
this->systemOfEquationsList.push_back(system);
size_t size = 0;
for(size_t i = 0, end = systemMatrices.size(); i < end; ++i)
for(auto &equation : this->equationList)
{
auto M = systemMatrices[i];
auto rhs = rhsVector[i];
this->concatenateMatrix(*M,this->A.back(),size,size);
this->b.back().segment(size,rhs->size());
auto &M = equation->getMatrix();
auto &rhs = equation->getRHSVector();
this->concatenateMatrix(M, this->A.back(), size, size);
this->b.back().segment(size, rhs.size());
size += rhs->size();
size += rhs.size();
}
}
}
}
//---------------------------------------------------------------------------
void Assembler::concatenateMatrix(const core::SparseMatrixd &Q, core::SparseMatrixd &R, std::size_t i, std::size_t j)
void Assembler::concatenateMatrix(const core::SparseMatrixd &Q,
core::SparseMatrixd &R,
std::size_t i,
std::size_t j)
{
for(size_t k = i; k < i+Q.outerSize(); ++k)
for(size_t k = i; k < i + Q.outerSize(); ++k)
{
for(core::SparseMatrixd::InnerIterator it(Q,k); it; ++it)
for(core::SparseMatrixd::InnerIterator it(Q, k); it; ++it)
{
R.insert(k,it.col()+j) = it.value();
R.insert(k, it.col() + j) = it.value();
}
}
}
......@@ -20,42 +20,35 @@
#ifndef ASSEMBLER_ASSEMBLER_H
#define ASSEMBLER_ASSEMBLER_H
// STL includes
#include <memory>
// iMSTK includes
#include "Core/CoreClass.h"
//#include "Core/SDK.h"
#include "Core/Config.h"
#include "Core/ContactHandling.h"
#include "CollisionContext/CollisionContext.h"
#include "Solvers/SystemOfEquations.h"
//#include "Core/SceneObject.h"
#include "Core/Matrix.h"
#include "Core/Vector.h"
// Forward declarations
class CollisionContext;
template<typename T>
class LinearSystem;
///
/// \class Assembler
///
/// \brief This class is responsible for using the
/// information in the collision context and the
/// internal, external forces from scene objects
/// to output systems of equations to be solved by
/// solver module.
/// \class Assembler This class is responsible for using the information in the
/// collision context, the internal and external forces from scene objects
/// to assemble an augmented systems of equations.
///
class Assembler : public CoreClass
{
public:
using LinearSystemType = LinearSystem<core::SparseMatrixd>;
using SparseLinearSystem = LinearSystem<core::SparseMatrixd>;
public:
///
/// \brief Default constructor
/// \brief Default constructor/destructor
///
Assembler() = default;
///
/// \brief Destructor
///
~Assembler() = default;
///
......@@ -63,36 +56,52 @@ public:
///
Assembler(std::shared_ptr<CollisionContext> collisionContext);
///
/// \brief Set/Get Collision context.
///
void setCollisionContext(std::shared_ptr<CollisionContext> newCollisionContext);
std::shared_ptr<CollisionContext> getCollisionContext() const;
///
/// \brief Set/Get System of equations.
///
void setSystemOfEquations(std::vector<std::shared_ptr<SparseLinearSystem>> newSystemOfEquations);
std::vector<std::shared_ptr<SparseLinearSystem>> getSystemOfEquations() const;
///
/// \brief consolidate the forces/projectors from type 1 interactions such as
/// forces from penalty based contact handling
/// \brief Consolidate the forces/projectors from type 1 interactions such as
/// forces from penalty based contact handling.
///
void type1Interactions();
///
/// \brief
/// \brief Initialize the system of equations from the scene models provided by the
/// interaction context.
///
void initSystem();
///
/// \brief Helper to concatenate the matrix Q into an block of R.
/// \brief Helper to concatenate the matrix Q into a block of R.
///
/// \param Q Submatrix
/// \param R Supermatrix
/// \param i row offset
/// \param j column offset
///
void concatenateMatrix(const core::SparseMatrixd &Q, core::SparseMatrixd &R, size_t i, size_t j);
void concatenateMatrix(const core::SparseMatrixd &Q,
core::SparseMatrixd &R,
size_t i,
size_t j);
private:
// inputs
std::shared_ptr<CollisionContext> collisionContext;
std::shared_ptr<CollisionContext> collisionContext; ///> Interaction context
// output
std::vector<std::shared_ptr<LinearSystem<core::SparseMatrixd>>> systemOfEquationsList;
///> List of systems to be solved, these can be linear, nonlinear or constrained.
///> Each system correspond to one type of interaction in the interaction graph.
std::vector<std::shared_ptr<SparseLinearSystem>> equationList;
std::vector<core::SparseMatrixd> A;
std::vector<core::Vectord> b;
std::vector<core::SparseMatrixd> A; ///> Matrices storage
std::vector<core::Vectord> b; ///> Right hand sides storage
};
#endif // ASSEMBLER_ASSEMBLER_H
......@@ -99,9 +99,16 @@ bool CollisionMoller::tri2tri( core::Vec3d &p_tri1Point1,
bool CollisionMoller::tri2tri( core::Vec3d &p_tri1Point1, core::Vec3d &p_tri1Point2, core::Vec3d &p_tri1Point3, core::Vec3d &p_tri2Point1, core::Vec3d &p_tri2Point2, core::Vec3d &p_tri2Point3 )
{
return ( tri_tri_intersect( p_tri1Point1.data(), p_tri1Point2.data(),
p_tri1Point3.data(), p_tri2Point1.data(),
p_tri2Point2.data(), p_tri2Point3.data() ) == 1 ? true : false );
if(tri_tri_intersect(p_tri1Point1.data(), p_tri1Point2.data(),
p_tri1Point3.data(), p_tri2Point1.data(),
p_tri2Point2.data(), p_tri2Point3.data()) == 1)
{
return true;
}
else
{
return false;
}
}
bool CollisionMoller::checkOverlapAABBAABB( AABB &aabbA, AABB &aabbB, AABB &result )
......
......@@ -386,12 +386,11 @@ bool SpatialHashCollision::findCandidates()
{
for(size_t j = i + 1; j < collisionModels.size(); j++)
{
if(collisionModels[i]->getCollisionGroup()->isCollisionPermitted(collisionModels[j]->getCollisionGroup()))
if(findCandidateTris(collisionModels[i], collisionModels[j]) == false &&
collisionModels[i]->getCollisionGroup()->isCollisionPermitted(collisionModels[j]->getCollisionGroup()) !=
NULL)
{
if(findCandidateTris(collisionModels[i], collisionModels[j]) == false)
{
continue;
}
continue;
}
}
}
......
......@@ -37,7 +37,8 @@ template<typename CellType>
class SurfaceTree : public CoreClass
{
protected:
typedef core::Matrix44d MatrixType;
using MatrixType = core::Matrix44d;
using CellPairType = std::pair<std::shared_ptr<CellType>,std::shared_ptr<CellType>>;
protected:
int minTreeRenderLevel; ///< !!
......@@ -101,10 +102,10 @@ public:
return root;
}
std::vector<std::pair<std::shared_ptr<CellType>,std::shared_ptr<CellType>>>
std::vector<CellPairType>
getIntersectingNodes(std::shared_ptr<SurfaceTree<CellType>> otherTree)
{
std::vector<std::pair<std::shared_ptr<CellType>,std::shared_ptr<CellType>>> intersectingNodes;
std::vector<CellPairType> intersectingNodes;
getIntersectingNodes(root, otherTree->getRoot(),intersectingNodes);
return intersectingNodes;
......@@ -112,7 +113,7 @@ public:
void getIntersectingNodes(const std::shared_ptr<CellType> left,
const std::shared_ptr<CellType> right,
std::vector<std::pair<std::shared_ptr<CellType>,std::shared_ptr<CellType>>> &result );
std::vector<CellPairType> &result );
};
#include "Collision/SurfaceTree.hpp"
......
......@@ -20,8 +20,10 @@
#ifndef COLLISION_SURFACETREE_HPP
#define COLLISION_SURFACETREE_HPP
#include "Collision/SurfaceTree.h"
// STD includes
#include <numeric> // for iota
#include<numeric> // for iota
// iMSTK includes
#include "Collision/SurfaceTreeIterator.h"
......@@ -30,17 +32,18 @@
#include "Collision/MeshCollisionModel.h"
/// \brief initialize the surface tree structure
template <typename CellType>
template<typename CellType>
void SurfaceTree<CellType>::initStructure()
{
if(!this->model->getMesh() || this->model->getMesh()->getNumberOfVertices() == 0)
{
std::cerr << "Error: Empty or unvalid mesh." << std::endl;
std::cerr << "Error: Empty or invalid mesh." << std::endl;
return;
}
core::Vec3d center;
std::vector<int> triangles(this->model->getMesh()->getTriangles().size());
std::iota(std::begin(triangles),std::end(triangles),0);
std::iota(std::begin(triangles), std::end(triangles), 0);
root = std::make_shared<CellType>();
......@@ -73,7 +76,7 @@ SurfaceTree<CellType>::SurfaceTree(std::shared_ptr<MeshCollisionModel> surfaceMo
currentLevel = maxLevel = maxLevels;
//compute the total cells
for (int i = 0; i < maxLevel; ++i)
for(int i = 0; i < maxLevel; ++i)
{
totalCells += std::pow(int(CellType::numberOfSubdivisions), i);
}
......@@ -85,13 +88,13 @@ SurfaceTree<CellType>::SurfaceTree(std::shared_ptr<MeshCollisionModel> surfaceMo
//compute the levels start and end
levelStartIndex[0][0] = 0;
for (int i = 1; i < maxLevel; ++i)
for(int i = 1; i < maxLevel; ++i)
{
levelStartIndex[i][0] = levelStartIndex[i-1][0] + std::pow(int(CellType::numberOfSubdivisions), i-1);
levelStartIndex[i-1][1] = levelStartIndex[i][0];
}
levelStartIndex[maxLevel-1][1] = totalCells;
levelStartIndex[maxLevel - 1][1] = totalCells;
minTreeRenderLevel = 0;
renderSurface = false;
......@@ -102,8 +105,8 @@ SurfaceTree<CellType>::SurfaceTree(std::shared_ptr<MeshCollisionModel> surfaceMo
this->model->computeBoundingBoxes();
this->setRenderDelegate(
Factory<RenderDelegate>::createConcreteClass(
"SurfaceTreeRenderDelegate"));
Factory<RenderDelegate>::createConcreteClass(
"SurfaceTreeRenderDelegate"));
}
/// \brief handle key press events
......@@ -114,10 +117,13 @@ void SurfaceTree<CellType>::handleEvent(std::shared_ptr<core::Event> event)
{
return;
}
auto keyBoardEvent = std::static_pointer_cast<event::KeyboardEvent>(event);
if(keyBoardEvent != nullptr)
{
event::Key keyPressed = keyBoardEvent->getKeyPressed();
switch(keyPressed)
{
case event::Key::Add:
......@@ -183,8 +189,8 @@ void SurfaceTree<CellType>::handleEvent(std::shared_ptr<core::Event> event)
/// \brief create the surface tree
template<typename CellType>
bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
const std::vector<int> &triangles,
int siblingIndex)
const std::vector<int> &triangles,
int siblingIndex)
{
std::array<CellType, CellType::numberOfSubdivisions> subDividedNodes;
std::array<std::vector<int>, CellType::numberOfSubdivisions> triangleMatrix;
......@@ -198,21 +204,22 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
return false;
}
if (level == maxLevel-1)
if(level == maxLevel - 1)
{
Node->setIsLeaf(true);
double totalDistance = 0.0;
for(auto &t : triangles)
for(auto & t : triangles)
{
Node->addTriangleData(this->model->getAabb(t),t);
Node->addTriangleData(this->model->getAabb(t), t);
Node->addVertexIndex(meshTriangles[t][0]);
Node->addVertexIndex(meshTriangles[t][1]);
Node->addVertexIndex(meshTriangles[t][2]);
}
Node->update();
for(const auto &i : Node->getVerticesIndices())
for(const auto & i : Node->getVerticesIndices())
{
totalDistance += (Node->getCenter() - vertices[i]).norm();
}
......@@ -221,15 +228,15 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
float weight;
float totalDistance2 = totalDistance * totalDistance;
for(const auto &i : Node->getVerticesIndices())
for(const auto & i : Node->getVerticesIndices())
{
// TODO: make sure this is what is meant: 1-d^2/D^2 and not (1-d^2)/D^2
weight = 1-(Node->getCenter()-vertices[i]).squaredNorm() / totalDistance2;
weight = 1 - (Node->getCenter() - vertices[i]).squaredNorm() / totalDistance2;
weightSum += weight;
Node->addWeight(weight);
}
for(auto &w : Node->getWeights())
for(auto & w : Node->getWeights())
{
w /= weightSum;
}
......@@ -239,17 +246,17 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
Node->subDivide(2, subDividedNodes);
for (int i = 0; i < CellType::numberOfSubdivisions; ++i)
for(int i = 0; i < CellType::numberOfSubdivisions; ++i)
{
//aabb[i].expand(0.2);
subDividedNodes[i].expand(0.01);
}
for (int i = 0; i < triangles.size(); ++i)
for(int i = 0; i < triangles.size(); ++i)
{
for (int j = 0; j < CellType::numberOfSubdivisions; ++j)
for(int j = 0; j < CellType::numberOfSubdivisions; ++j)
{
if (subDividedNodes[j].isCollidedWithTri(
if(subDividedNodes[j].isCollidedWithTri(
vertices[meshTriangles[triangles[i]][0]],
vertices[meshTriangles[triangles[i]][1]],
vertices[meshTriangles[triangles[i]][2]]))
......@@ -259,31 +266,32 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
}
}
int parentLevel = level == 0 ? 0 : level-1;
int offset = CellType::numberOfSubdivisions * (siblingIndex-levelStartIndex[parentLevel][0]);
int parentLevel = level == 0 ? 0 : level - 1;
int offset = CellType::numberOfSubdivisions * (siblingIndex - levelStartIndex[parentLevel][0]);
for (int j = 0; j < CellType::numberOfSubdivisions; ++j)
for(int j = 0; j < CellType::numberOfSubdivisions; ++j)
{
int childIndex = levelStartIndex[level][1] + offset + j;
if (triangleMatrix[j].size() > 0)
if(triangleMatrix[j].size() > 0)
{
std::shared_ptr<CellType> childNode = std::make_shared<CellType>();
childNode->copyShape(subDividedNodes[j]);
childNode->setLevel(level + 1);
childNode->setParentNode(Node);
Node->setChildNode(j,childNode);
Node->setChildNode(j, childNode);
treeAllLevels[childIndex] = *childNode.get();
// Set triangle and aabb data for the child node
if(childNode->getLevel() != maxLevel-1)
if(childNode->getLevel() != maxLevel - 1)
{
for(auto &t : triangleMatrix[j])
for(auto & t : triangleMatrix[j])
{
childNode->addTriangleData(this->model->getAabb(t),t);
childNode->addTriangleData(this->model->getAabb(t), t);
}
childNode->update();
}
......@@ -297,7 +305,7 @@ bool SurfaceTree<CellType>::createTree(std::shared_ptr<CellType> Node,
}
/// \brief !!
template <typename CellType>
template<typename CellType>
CollisionModelIterator<CellType> SurfaceTree<CellType>::getLevelIterator(int level)
{
SurfaceTreeIterator<CellType> iter(this);
......@@ -308,7 +316,7 @@ CollisionModelIterator<CellType> SurfaceTree<CellType>::getLevelIterator(int lev
}
/// \brief !!
template <typename CellType>
template<typename CellType>
CollisionModelIterator<CellType> SurfaceTree<CellType>::getLevelIterator()
{
SurfaceTreeIterator<CellType> iter(this);
......@@ -319,23 +327,24 @@ CollisionModelIterator<CellType> SurfaceTree<CellType>::getLevelIterator()
}
/// \brief update the surface tree
template <typename CellType>
template<typename CellType>
void SurfaceTree<CellType>::updateStructure()
{
CellType *current;
auto const &vertices = this->model->getVertices();
auto const &origVertices = this->model->getMesh()->getOrigVertices();
for (int i = levelStartIndex[maxLevel-1][0]; i < levelStartIndex[maxLevel-1][1]; ++i)
for(int i = levelStartIndex[maxLevel - 1][0]; i < levelStartIndex[maxLevel - 1][1]; ++i)
{
current = &treeAllLevels[i];
core::Vec3d tempCenter(0, 0, 0);
int counter = 0;
if (!current->isEmpty())
if(!current->isEmpty())
{
for(auto &i : current->getVerticesIndices())
for(auto & i : current->getVerticesIndices())
{
tempCenter = tempCenter + (vertices[i]-origVertices[i]) * current->getWeight(counter);
tempCenter = tempCenter + (vertices[i] - origVertices[i]) * current->getWeight(counter);
counter++;
}
......@@ -345,30 +354,31 @@ void SurfaceTree<CellType>::updateStructure()
}
/// \brief !!
template <typename CellType>
template<typename CellType>
void SurfaceTree<CellType>::translateRot()
{
CellType *current;
CellType *initial;
for (int i = levelStartIndex[maxLevel-1][0];
i < levelStartIndex[maxLevel-1][1]; ++i)
for(int i = levelStartIndex[maxLevel - 1][0];
i < levelStartIndex[maxLevel - 1][1]; ++i)
{
current = &treeAllLevels[i];
initial = &initialTreeAllLevels[i];
if (!current->isEmpty())
if(!current->isEmpty())
{
current->getCube().center = this->transRot.block(0,0,3,3) * initial->getCube().center + this->transRot.col(3).head(3);
current->getCube().center =
this->transRot.block(0, 0, 3, 3) * initial->getCube().center + this->transRot.col(3).head(3);
}
}
}
template <typename CellType>
template<typename CellType>
void SurfaceTree<CellType>::getIntersectingNodes(const std::shared_ptr<CellType> left,