Commit 9e97b77f authored by Ricardo Ortiz's avatar Ricardo Ortiz
Browse files

ENH: Integration of Assembler.

Implement missing methods and logic.
parent e5c39a98
Pipeline #4839 passed with stage
......@@ -22,63 +22,90 @@
//---------------------------------------------------------------------------
#include "Assembler/Assembler.h"
#include "Core/ContactHandling.h"
#include "Core/Model.h"
#include "TimeIntegrators/OdeSystem.h"
Assembler::Assembler()
{
}
Assembler::Assembler(std::shared_ptr<CollisionContext>& collContext)
Assembler::Assembler(std::shared_ptr<CollisionContext> collContext)
{
this->collisionContext = collContext;
initiateSystemOfEquations();
this->initSystem();
}
Assembler::~Assembler()
//---------------------------------------------------------------------------
void Assembler::type1Interactions()
{
}
if(!this->collisionContext)
{
// TODO: Log this
return;
}
void Assembler::consolidateType1Interactions()
{
auto ch = this->collisionContext->getContactHandlers();
for (auto it = ch.begin(); it != ch.end(); ++it)
for(auto &ch : this->collisionContext->getContactHandlers())
{
if ((*it)->getType() == core::ContactHandlingType::Penalty)
{
(*it)->getFirstSceneObject()->setCumulativeContactForces(
std::static_cast<std::shared_ptr<PenaltyContactHandling>>
(*it)->getContactForce());
}
const auto &forces = ch->getContactForces();
auto sceneModel = ch->getSecondSceneObject();
sceneModel->updateExternalForces(forces);
}
}
void Assembler::initiateSystemOfEquations()
//---------------------------------------------------------------------------
void Assembler::initSystem()
{
int dofSize;
auto islands = this->collisionContext->getIslands();
for (auto i = islands.begin(); i != islands.end(); ++i)
for(auto &rows : this->collisionContext->getIslands())
{
dofSize = 0;
for (auto j = i->begin(); j != i->end(); ++j)
size_t dofSize = 0;
size_t nnz = 0;
std::vector<const core::SparseMatrixd*> systemMatrices;
std::vector<const core::Vectord*> rhsVector;
for(auto &col : rows)
{
dofSize += this->collisionContext->getObjectWithIndex(*j)->getNumDof();
auto sceneModel = this->collisionContext->getSceneModel(col);
auto odeSystem = std::dynamic_pointer_cast<OdeSystem>(
this->collisionContext->getSceneModel(col));
if(sceneModel && odeSystem)
{
dofSize += sceneModel->getNumOfDOF();
systemMatrices.push_back(&odeSystem->getSystemMatrix());
rhsVector.push_back(&odeSystem->getRHS());
nnz += systemMatrices.back()->nonZeros();
}
}
}
}
void Assembler::updateSubsystems()
{
}
if(dofSize > 0)
{
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());
void Assembler::stackSparseMatrices(SparseMatrixd& parent, SparseMatrixd& addOn)
{
int expandedSize = parent.size() + addOn.size();
parent.resize(expandedSize, expandedSize);
this->systemOfEquationsList.push_back(system);
size_t size = 0;
for(size_t i = 0, end = systemMatrices.size(); i < end; ++i)
{
auto M = systemMatrices[i];
auto rhs = rhsVector[i];
this->concatenateMatrix(*M,this->A.back(),size,size);
this->b.back().segment(size,rhs->size());
for (int i = 0; i < )
size += rhs->size();
}
}
}
}
}
\ No newline at end of file
//---------------------------------------------------------------------------
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(core::SparseMatrixd::InnerIterator it(Q,k); it; ++it)
{
R.insert(k,it.col()+j) = it.value();
}
}
}
......@@ -47,61 +47,55 @@
///
class Assembler : public CoreClass
{
public:
using LinearSystemType = LinearSystem<core::SparseMatrixd>;
public:
///
/// \brief default constructor
/// \brief Default constructor
///
Assembler();
Assembler() = default;
///
/// \brief constructor
/// \brief Destructor
///
Assembler(std::shared_ptr<CollisionContext>& collContext);
~Assembler() = default;
///
/// \brief destructor
/// \brief Constructor. Takes a collision context.
///
~Assembler();
Assembler(std::shared_ptr<CollisionContext> collisionContext);
///
/// \brief consolidate the forces/projectors from type 1 interactions such as
/// forces from penalty based contact handling
///
void consolidateType1Interactions();
///
/// \brief consolidate the forces/projectors from type 2 interactions such as
/// lcp
///
void updateSubsystems();
void type1Interactions();
void stackSparseMatrices(SparseMatrixd& parent, SparseMatrixd& addOn);
///
/// \brief Get the number of system of equations
///
int getNumSystemOfEquations() const;
/// \brief
///
void initSystem();
///
/// \brief Get the number of linear system of equations
/// \brief Helper to concatenate the matrix Q into an block of R.
///
int getNumLinearSystemOfEquations() const;
///
/// \brief Get the number of Lcp system of equations
/// \param Q Submatrix
/// \param R Supermatrix
/// \param i row offset
/// \param j column offset
///
int getNumLcpSystemOfEquations() const;
///
/// \brief
///
void initiateSystemOfEquations();
void concatenateMatrix(const core::SparseMatrixd &Q, core::SparseMatrixd &R, size_t i, size_t j);
private:
// inputs
std::shared_ptr<CollisionContext> collisionContext;
// output
std::vector<std::shared_ptr<SystemOfEquations>> systemOfEquationsList;
std::vector<std::shared_ptr<LinearSystem<core::SparseMatrixd>>> systemOfEquationsList;
std::vector<core::SparseMatrixd> A;
std::vector<core::Vectord> b;
};
#endif // SM_ASSEMBLY
set(MODULE_NAME Assembler)
simmedtk_add_library(${MODULE_NAME}
SOURCES
Assembler.cpp
PUBLIC_HEADERS
Assembler.h
)
target_link_libraries(Assembler
PUBLIC
Core
PRIVATE
Event
ContactHandling
Simulators
)
......@@ -294,6 +294,7 @@ if(SimMedTK_USE_OCULUS)
list(APPEND SimMedTK_exports RenderingOculus)
endif()
add_subdirectory(Assembler)
add_subdirectory(Collision)
add_subdirectory(CollisionContext)
add_subdirectory(ContactHandling)
......
......@@ -337,7 +337,7 @@ std::vector<std::shared_ptr<ContactHandling>> CollisionContext::getContactHandle
}
}
return std::move(handlerList);
return handlerList;
}
//---------------------------------------------------------------------------
......@@ -386,7 +386,6 @@ void CollisionContext::formIslands()
}
//---------------------------------------------------------------------------
// \todo test
void CollisionContext::appendNeighbors(std::vector<bool>& visited,
std::vector<int>& memberList,
int row)
......@@ -396,12 +395,13 @@ void CollisionContext::appendNeighbors(std::vector<bool>& visited,
{
const auto &entry = interactionMatrix[row][col];
if (entry != 0 &&
std::find(std::begin(memberList), std::end(memberList), entry) != memberList.end())
std::find(std::begin(memberList), std::end(memberList), entry) != std::end(memberList))
{
memberList.push_back(interactionMatrix[row][col]);
if (!visited[interactionMatrix[row][col]])
memberList.push_back(col);
if (!visited[col])
{
this->appendNeighbors(visited, memberList, interactionMatrix[row][col]);
this->appendNeighbors(visited, memberList, col);
visited[col] = true;
}
}
}
......
......@@ -259,6 +259,25 @@ public:
void solveSimultaneously(std::shared_ptr<SceneObject> sceneObjectA,
std::shared_ptr<SceneObject> sceneObjectB);
///
/// \brief Get the scene model associated with ith index if exists.
///
/// \param index
///
std::shared_ptr<SceneObject> getSceneModel(int index)
{
auto fn = [=](const std::unordered_map<std::shared_ptr<SceneObject>,int>::value_type& vt)
{
return vt.second == index;
};
auto it = std::find_if(std::begin(this->objectIndexMap), std::end(this->objectIndexMap),fn);
if(it != std::end(this->objectIndexMap))
{
return it->first;
}
return nullptr;
}
private:
std::vector<std::vector<int>> interactionMatrix; ///> Adjacency matrix for the
///> assembly graph (undirected)
......
......@@ -69,6 +69,7 @@ void PenaltyContactFemToStatic::computeForces(std::shared_ptr< DeformableSceneOb
auto contactInfo = this->getCollisionPairs()->getContacts(model);
sceneObject->setContactForcesToZero();
this->clearContactForces();
core::Vec3d force;
core::Vec3d velocityProjection;
int nodeDofID;
......@@ -81,6 +82,7 @@ void PenaltyContactFemToStatic::computeForces(std::shared_ptr< DeformableSceneOb
force = -stiffness * contact->depth * contact->normal - damping * velocityProjection;
sceneObject->setContactForce(nodeDofID, contact->point, force);
this->setContactForce(contact->index, force);
}
}
}
......@@ -67,10 +67,10 @@ go_bandit([]() {
it("initializes properly ", []() {
auto handler = std::make_shared<PenaltyContactFemToStatic>(false);
AssertThat(handler != nullptr, IsTrue());
AssertThat(handler->getContactHandlingType() == ContactHandlingType::PenaltyFemToStatic, IsTrue());
AssertThat(handler->getContactHandlingType() == ContactHandling::PenaltyFemToStatic, IsTrue());
});
it("attches a collision pair ", []() {
it("attaches a collision pair ", []() {
auto handler = std::make_shared<PenaltyContactFemToStatic>(false);
auto collisionPair = createSampleCollisionPair();
handler->setCollisionPairs(collisionPair);
......
......@@ -39,6 +39,7 @@ ContactHandling::ContactHandling(const bool typeBilateral)
type = Unknown;
};
//---------------------------------------------------------------------------
ContactHandling::ContactHandling(const bool typeBilateral,
const std::shared_ptr< SceneObject > first,
const std::shared_ptr< SceneObject > second)
......@@ -57,6 +58,7 @@ ContactHandling::ContactHandling(const bool typeBilateral,
type = Unknown;
}
//---------------------------------------------------------------------------
ContactHandling::~ContactHandling()
{
}
......@@ -69,32 +71,62 @@ void ContactHandling::setSceneObjects(const std::shared_ptr< SceneObject > first
collidingSceneObjects.second = second;
}
//---------------------------------------------------------------------------
void ContactHandling::setCollisionPairs(const std::shared_ptr< CollisionManager > colPair)
{
collisionPair = colPair;
}
//---------------------------------------------------------------------------
std::shared_ptr<CollisionManager> ContactHandling::getCollisionPairs() const
{
return collisionPair;
}
//---------------------------------------------------------------------------
ContactHandling::MethodType ContactHandling::getContactHandlingType() const
{
return type;
}
//---------------------------------------------------------------------------
std::shared_ptr<SceneObject> ContactHandling::getFirstSceneObject() const
{
return this->collidingSceneObjects.first;
}
//---------------------------------------------------------------------------
std::shared_ptr<SceneObject> ContactHandling::getSecondSceneObject() const
{
return this->collidingSceneObjects.second;
}
//---------------------------------------------------------------------------
bool ContactHandling::isUnilateral() const
{
return !isBilateral;
}
//---------------------------------------------------------------------------
std::unordered_map< size_t, core::Vec3d> &ContactHandling::getContactForces()
{
return this->contactForces;
}
//---------------------------------------------------------------------------
const std::unordered_map< size_t,core::Vec3d> &ContactHandling::getContactForces() const
{
return this->contactForces;
}
//---------------------------------------------------------------------------
void ContactHandling::setContactForce(const size_t dofID, const core::Vec3d &force)
{
this->contactForces[dofID] = force;
}
//---------------------------------------------------------------------------
void ContactHandling::clearContactForces()
{
this->contactForces.clear();
}
......@@ -89,6 +89,28 @@ public:
/// objects is resolved
virtual void resolveContacts() = 0;
///
/// \brief Get contact forces vector
///
void setContactForce(const size_t dofID, const core::Vec3d &force);
///
/// \brief Get contact forces vector.
///
std::unordered_map<size_t,core::Vec3d> &getContactForces();
///
/// \brief Get the map of contact forces.
///
/// \return Map containing indices with contact points.
///
const std::unordered_map<size_t,core::Vec3d> &getContactForces() const;
///
/// \brief Set all contact forces to zero (if any)
///
void clearContactForces();
protected:
MethodType type;
......@@ -98,6 +120,9 @@ protected:
std::pair<std::shared_ptr<SceneObject>, std::shared_ptr<SceneObject>> collidingSceneObjects;
std::shared_ptr<CollisionManager> collisionPair;
// Map of contact forces to node indices in the mesh of the second scene object.
std::unordered_map<size_t,core::Vec3d> contactForces;
};
#endif //SMCONTACTHANDLING_H
......@@ -313,7 +313,7 @@ readWeights(std::shared_ptr<SurfaceMesh> surfaceMesh,
vertices.clear();
weigths.clear();
auto verticesPerElement = this->mesh->getNumElementVertices();
const auto verticesPerElement = this->mesh->getNumElementVertices();
int index;
int v[verticesPerElement];
......
......@@ -23,6 +23,7 @@ target_link_libraries(SceneModels
VirtualTools
Solvers
TimeIntegrators
Geometry
PUBLIC
VegaFEM::configFile
VegaFEM::vega-getopts
......
......@@ -37,8 +37,6 @@ DeformableSceneObject::DeformableSceneObject():
integrationScheme(TimeIntegrator::ImplicitEuler)
{
this->gravity = core::Vec3d::UnitY();
this->numOfDOF = 0;
this->numOfNodes = 0;
}
//---------------------------------------------------------------------------
......
......@@ -95,6 +95,19 @@ public:
///
const core::Vec3d &getGravity() const;
///
/// \brief Update cumulative forces
///
void updateExternalForces(const std::unordered_map<size_t,core::Vec3d> &forces)
{
auto externalForce = core::Matrixd::Map(this->f.data(),3,this->numOfNodes);
for(const auto &force : forces)
{
auto i = force.first;
externalForce.col(i) -= force.second;
}
}
private:
///////////////////////////////////////////////////////////////////////////////
//////////// TODO: These are pure virtual methods from superclass. ////////////
......@@ -133,10 +146,6 @@ protected:
// Gravity
core::Vec3d gravity;
// Total number of degrees of freedom
double numOfDOF;
double numOfNodes;
TimeIntegrator::IntegratorType integrationScheme; ///> Integration scheme used.
};
......
......@@ -36,6 +36,8 @@ SceneObject::SceneObject()
this->type = core::ClassType::Unknown;
this->objectSim = nullptr;
this->name = "SceneObject" + std::to_string(this->getUniqueId()->getId());
this->numOfDOF = 0;
this->numOfNodes = 0;
this->setRenderDelegate(
Factory<RenderDelegate>::createConcreteClassForGroup(
......@@ -225,3 +227,20 @@ std::shared_ptr< Model > SceneObject::getPhysicsModel()
void SceneObject::update(const double)
{
}
//---------------------------------------------------------------------------
void SceneObject::updateExternalForces(const std::unordered_map<size_t,core::Vec3d>&)
{
}
//---------------------------------------------------------------------------
std::size_t SceneObject::getNumOfDOF() const
{
return this->numOfDOF;
}
//---------------------------------------------------------------------------
std::size_t SceneObject::getNumOfNodes() const
{
return this->numOfNodes;
}
......@@ -253,6 +253,21 @@ public:
///
virtual void update(const double /*dt*/);
///
/// \brief Update cumulative forces
///
virtual void updateExternalForces(const std::unordered_map<size_t,core::Vec3d>&);
///
/// \brief Return the number of nodes for the underlying physics geometric model.
///
size_t getNumOfDOF() const;
///
/// \brief Return the number of nodes for the underlying physics geometric model.
///
size_t getNumOfNodes() const;
protected:
bool hasContactForces;
bool isActive;
......@@ -263,6 +278,10 @@ protected:
std::shared_ptr<ObjectSimulator> objectSim; //!< object simulator that will simulate the object
std::unordered_map<int,core::Vec3d> contactForces;
std::unordered_map<int,core::Vec3d> contactPoints;
// Total number of degrees of freedom
size_t numOfDOF;
size_t numOfNodes;
};
#endif
......@@ -13,6 +13,7 @@ simmedtk_add_library(SimulationManager
target_link_libraries(SimulationManager
PRIVATE
Core
Simulators
)
if(BUILD_TESTING)
......
......@@ -169,6 +169,7 @@ void SDK::shutDown()
/// \brief runs the simulator
void SDK::run()
{
// TODO: Remove this function call.
this->updateSceneListAll();
this->initRegisteredModules();
......@@ -179,6 +180,7 @@ void SDK::run()
}
// Tell framework threads to shutdown if the viewer returns
// TODO: Remove redundant function call.
this->shutDown();
this->terminateAll();
......
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