Commit 14ce80ff authored by Ricardo Ortiz's avatar Ricardo Ortiz
Browse files

ENH: Clean and refactor the penalty method.

Add an alias for a matrix map.
Create a templated method in the MeshModel and IOMesh to automatically
cast the underlying mesh.

Create an interpolate() method in the VegaVolumetricMesh class to
interpolate surface of the underlying volumetric mesh with an external
surface mesh and clean up the interface.

Create a computeGravity() method in the VegaVolumetricMesh class
to compute gravity forces.

Refactor getVelocity() method in DeformableSceneObject and add a
variable to hold the gravity vector.

Refactor SceneObject. Add SceneModelRenderDelegate render delegate to
take care of renderign operations for this model. Create visual,
physics and collision model variables.

Remove render delegates from the StaticSceneObject, this is now handled
by it base class.

Add storage for the gravity force in VegaFEMDeformableSceneObject and
initialized properly. Compute internal force by multiplying K*positions
instead of calling the vega function.

Refactor DefaultSimulator, VegaFEMModelSimulator. This class will be
marked obsolete soon.
parent 4cc20f40
......@@ -55,3 +55,32 @@ void PenaltyContactFemToStatic::computeUnilateralContactForces()
void PenaltyContactFemToStatic::computeBilateralContactForces()
{
}
//---------------------------------------------------------------------------
void PenaltyContactFemToStatic::computeForces(std::shared_ptr< SceneObjectDeformable > sceneObject)
{
if(sceneObject->computeContactForce())
{
auto model = sceneObject->getCollisionModel();
if(!model)
{
return;
}
auto contactInfo = this->getCollisionPairs()->getContacts(model);
sceneObject->setContactForcesToZero();
core::Vec3d force;
core::Vec3d velocityProjection;
int nodeDofID;
for(auto &contact : contactInfo)
{
nodeDofID = 3 * contact->index;
velocityProjection = sceneObject->getVelocity(nodeDofID);
velocityProjection = contact->normal.dot(velocityProjection) * contact->normal;
force = -stiffness * contact->depth * contact->normal - damping * velocityProjection;
sceneObject->setContactForce(nodeDofID, contact->point, force);
}
}
}
......@@ -48,33 +48,7 @@ public:
virtual void computeBilateralContactForces() override;
/// \brief Get the forces on both the scene objects using penalty method
virtual void computeForces(std::shared_ptr<SceneObjectDeformable> sceneObject)
{
if(sceneObject->computeContactForce())
{
auto model = sceneObject->getModel();
if(!model)
{
return;
}
auto contactInfo = this->getCollisionPairs()->getContacts(model);
sceneObject->setContactForcesToZero();
core::Vec3d force;
core::Vec3d velocityProjection;
int nodeDofID;
for(auto &contact : contactInfo)
{
nodeDofID = 3 * contact->index;
velocityProjection = sceneObject->getVelocity(nodeDofID);
velocityProjection = contact->normal.dot(velocityProjection) * contact->normal;
force = -stiffness * contact->depth * contact->normal - damping * velocityProjection;
sceneObject->setContactForce(nodeDofID, contact->point, force);
}
}
}
virtual void computeForces(std::shared_ptr<SceneObjectDeformable> sceneObject);
};
......
......@@ -27,8 +27,8 @@
PenaltyContactHandling::PenaltyContactHandling(bool typeBilateral) :
ContactHandling(typeBilateral),
stiffness(1e4),
damping(1e5)
stiffness(1e6),
damping(10000)
{
}
......@@ -38,8 +38,8 @@ PenaltyContactHandling::PenaltyContactHandling(bool typeBilateral,
const std::shared_ptr<SceneObject>& sceneObjFirst,
const std::shared_ptr<SceneObject>& sceneObjSecond) :
ContactHandling(typeBilateral,sceneObjFirst,sceneObjSecond),
stiffness(1e4),
damping(1e5)
stiffness(1e6),
damping(100)
{
}
......
......@@ -104,9 +104,9 @@ using StdVector3d = StdVector3<double>;
template<typename T>
using VectorMapType = Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>>;
// Nx3 matrix map
// kxN matrix map
template<typename T>
using Matrix3MapType = Eigen::Map<Eigen::Matrix<T,3,Eigen::Dynamic>>;
using MatrixMapType = Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>>;
} // core
......
......@@ -87,9 +87,18 @@ public:
///
std::shared_ptr<Core::BaseMesh> getMesh() override;
///
/// \brief Mesh accessors
///
template<typename MeshType>
std::shared_ptr<MeshType> getMeshAs()
{
return std::dynamic_pointer_cast<MeshType>(this->mesh);
}
protected:
std::shared_ptr<Core::BaseMesh> mesh; // Underlying mesh
};
#endif // SMMESHMODEL_H
......@@ -94,6 +94,15 @@ public:
std::shared_ptr<Core::BaseMesh> getMesh();
void setMesh(std::shared_ptr<Core::BaseMesh> newMesh);
///
/// \brief Mesh accessors
///
template<typename MeshType>
std::shared_ptr<MeshType> getMeshAs()
{
return std::dynamic_pointer_cast<MeshType>(this->mesh);
}
///
/// \brief Filename accessors
///
......
......@@ -61,7 +61,9 @@ size_t VegaVolumetricMesh::getNumberOfElements() const
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh, const double &radius)
void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh,
const double &radius,
bool useForRendering)
{
// Keep copy of the mesh pointer
this->attachedMeshes.push_back(surfaceMesh);
......@@ -72,10 +74,21 @@ void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceM
return;
}
this->generateWeigths(surfaceMesh,radius);
// If this surface mesh is the rendering mesh then pass along its delegate to this
// volumetric mesh.
if(useForRendering)
{
this->setRenderDelegate(surfaceMesh->getRenderDelegate());
}
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh, const std::string& fileName, const double &radius)
void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh,
const std::string& fileName,
const double &radius,
bool useForRendering
)
{
// Keep copy of the mesh pointer
this->attachedMeshes.push_back(surfaceMesh);
......@@ -86,6 +99,13 @@ void VegaVolumetricMesh::attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceM
return;
}
this->readWeights(surfaceMesh,fileName,radius);
// If this surface mesh is the rendering mesh then pass along its delegate to this
// volumetric mesh.
if(useForRendering)
{
this->setRenderDelegate(surfaceMesh->getRenderDelegate());
}
}
//---------------------------------------------------------------------------
......@@ -107,42 +127,74 @@ void VegaVolumetricMesh::setVegaMesh(std::shared_ptr<VolumetricMesh> newMesh)
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::updateAttachedMeshes(const core::Vectord &q)
void VegaVolumetricMesh::interpolate(const core::Vectord &x,
std::shared_ptr< SurfaceMesh > mesh)
{
auto renderingMesh = this->getRenderingMesh();
auto *data = const_cast<double*>(q.data());
if(renderingMesh)
if(size_t(x.size()) != 3*this->getNumberOfVertices())
{
std::vector<core::Vec3d> displacements(renderingMesh->getNumberOfVertices(),core::Vec3d::Zero());
VolumetricMesh::interpolate(data,
displacements.data()->data(),
renderingMesh->getNumberOfVertices(),
this->mesh->getNumElementVertices(),
this->attachedVertices.at(renderingMesh).data(),
this->attachedWeights.at(renderingMesh).data());
// TODO: Log this
return;
}
auto verticesPerElement = size_t(this->mesh->getNumElementVertices());
auto &restPositions = renderingMesh->getOrigVertices();
if(restPositions.size() != displacements.size())
{
std::cerr << "Error: rest positions not set!" << std::endl;
return;
}
// Create maps for the interpolation data.
auto vertices = core::Matrix<int>::Map(this->attachedVertices.at(mesh).data(),
verticesPerElement,
mesh->getNumberOfVertices());
auto &vertices = renderingMesh->getVertices();
for(size_t i = 0, end = displacements.size(); i < end; ++i)
auto weights = core::Matrix<double>::Map(this->attachedWeights.at(mesh).data(),
verticesPerElement,
mesh->getNumberOfVertices());
// Create maps to the mesh data. Each column represents a node on the mesh.
auto displacements = core::Matrix<double>::Map(x.data(),3,this->getNumberOfVertices());
auto &targets = mesh->getVertices();
const auto &initialTargets = mesh->getOrigVertices();
if(targets.size() != size_t(vertices.cols()) ||
targets.size() != size_t(weights.cols()))
{
// TODO: Log this error
return;
}
std::vector<core::Vec3d> interpolatedDisplacement(mesh->getNumberOfVertices(),
core::Vec3d::Zero());
for(size_t i = 0, end = targets.size(); i < end; ++i)
{
auto vertexIndices = vertices.col(i);
auto vertexWeights = weights.col(i);
for(size_t j = 0; j < verticesPerElement; ++j)
{
vertices[i] = restPositions[i] + displacements[i];
auto idx = vertexIndices(j);
interpolatedDisplacement[i] += displacements.col(idx) * vertexWeights(j);
}
}
for(size_t i = 0, end = targets.size(); i < end; ++i)
{
targets[i] = initialTargets[i] + interpolatedDisplacement[i];
}
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::updateAttachedMeshes(const core::Vectord &x)
{
auto renderingMesh = this->getRenderingMesh();
if(renderingMesh)
{
this->interpolate(x,renderingMesh);
renderingMesh->computeTriangleNormals();
renderingMesh->getRenderDelegate()->modified();
}
auto collisionMesh = this->getCollisionMesh();
if(collisionMesh)
{
core::Matrix3MapType<double> displacementMap(data,3,this->mesh->getNumVertices());
auto displacementMap = core::Matrix<double>::Map(x.data(),
3,this->mesh->getNumVertices());
auto &restPositions = collisionMesh->getOrigVertices();
auto &vertices = collisionMesh->getVertices();
......@@ -193,7 +245,8 @@ std::shared_ptr<SurfaceMesh> VegaVolumetricMesh::getAttachedMesh(const size_t i)
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::setRenderDetail(int i, std::shared_ptr< RenderDetail > newRenderDetail)
void VegaVolumetricMesh::
setRenderDetail(int i, std::shared_ptr< RenderDetail > newRenderDetail)
{
this->attachedMeshes.at(i)->setRenderDetail(newRenderDetail);
}
......@@ -215,17 +268,18 @@ std::shared_ptr<SurfaceMesh> VegaVolumetricMesh::getCollisionMesh()
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::saveWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, const std::string& filename) const
void VegaVolumetricMesh::
saveWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, const std::string& filename) const
{
auto vertices = this->attachedVertices.at(surfaceMesh);
auto weights = this->attachedWeights.at(surfaceMesh);
int numElementVertices = this->mesh->getNumElementVertices();
int verticesPerElement = this->mesh->getNumElementVertices();
std::ofstream fileStream(filename.c_str(), std::ofstream::out);
for(size_t i = 0, end = weights.size() / numElementVertices; i < end; ++i)
for(size_t i = 0, end = weights.size() / verticesPerElement; i < end; ++i)
{
auto index = i * numElementVertices;
auto index = i * verticesPerElement;
fileStream << i
<< " " << vertices[index] << " " << weights[index]
<< " " << vertices[index + 1] << " " << weights[index + 1]
......@@ -238,7 +292,10 @@ void VegaVolumetricMesh::saveWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, c
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::readWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, const std::string& filename, const double radius)
void VegaVolumetricMesh::
readWeights(std::shared_ptr<SurfaceMesh> surfaceMesh,
const std::string& filename,
const double radius)
{
std::ifstream fileStream(filename.c_str());
......@@ -256,9 +313,11 @@ void VegaVolumetricMesh::readWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, c
vertices.clear();
weigths.clear();
auto verticesPerElement = this->mesh->getNumElementVertices();
int index;
std::array<int, 4> v;
std::array<double, 4> w;
int v[verticesPerElement];
double w[verticesPerElement];
while(fileStream >> index
>> v[0] >> w[0]
......@@ -280,21 +339,25 @@ void VegaVolumetricMesh::readWeights(std::shared_ptr<SurfaceMesh> surfaceMesh, c
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMesh, double radius, const bool saveToDisk, const std::string& filename)
void VegaVolumetricMesh::
generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMesh,
double radius,
const bool saveToDisk,
const std::string& filename)
{
std::cerr << "Generating weights..." << std::endl;
const std::vector<core::Vec3d> &meshVertices = surfaceMesh->getVertices();
int numElementVertices = this->mesh->getNumElementVertices();
int verticesPerElement = this->mesh->getNumElementVertices();
size_t surfaceMeshSize = meshVertices.size();
std::vector<int> &vertices = this->attachedVertices[surfaceMesh];
std::vector<double> &weigths = this->attachedWeights[surfaceMesh];
std::vector<double> baryCentricWeights(numElementVertices);
std::vector<double> baryCentricWeights(verticesPerElement);
vertices.resize(numElementVertices * surfaceMeshSize);
weigths.resize(numElementVertices * surfaceMeshSize);
vertices.resize(verticesPerElement * surfaceMeshSize);
weigths.resize(verticesPerElement * surfaceMeshSize);
for(size_t i = 0; i < surfaceMeshSize; ++i)
{
......@@ -312,7 +375,7 @@ void VegaVolumetricMesh::generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMes
{
double minDistance = std::numeric_limits<double>::max();
for(int k = 0; k < numElementVertices; ++k)
for(int k = 0; k < verticesPerElement; ++k)
{
Vec3d &p = *this->mesh->getVertex(element, k);
double l = len(p - vegaPosition);
......@@ -325,7 +388,7 @@ void VegaVolumetricMesh::generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMes
if(minDistance > radius)
{
for(int k = 0; k < numElementVertices; ++k)
for(int k = 0; k < verticesPerElement; ++k)
{
baryCentricWeights[k] = 0.0;
}
......@@ -334,10 +397,10 @@ void VegaVolumetricMesh::generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMes
}
}
for(int k = 0; k < numElementVertices; ++k)
for(int k = 0; k < verticesPerElement; ++k)
{
vertices[numElementVertices * i + k] = this->mesh->getVertexIndex(element, k);
weigths[numElementVertices * i + k] = baryCentricWeights[k];
vertices[verticesPerElement * i + k] = this->mesh->getVertexIndex(element, k);
weigths[verticesPerElement * i + k] = baryCentricWeights[k];
}
}
......@@ -348,19 +411,22 @@ void VegaVolumetricMesh::generateWeigths(std::shared_ptr<SurfaceMesh> surfaceMes
}
//---------------------------------------------------------------------------
const std::vector< double >& VegaVolumetricMesh::getAttachedWeights(std::shared_ptr<SurfaceMesh> surfaceMesh) const
const std::vector< double >& VegaVolumetricMesh::
getAttachedWeights(std::shared_ptr<SurfaceMesh> surfaceMesh) const
{
return this->attachedWeights.at(surfaceMesh);
}
//---------------------------------------------------------------------------
const std::vector< int >& VegaVolumetricMesh::getAttachedVertices(std::shared_ptr<SurfaceMesh> surfaceMesh) const
const std::vector< int >& VegaVolumetricMesh::
getAttachedVertices(std::shared_ptr<SurfaceMesh> surfaceMesh) const
{
return this->attachedVertices.at(surfaceMesh);
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::translate(const Eigen::Translation3d& translation, bool setInitialPoints)
void VegaVolumetricMesh::
translate(const Eigen::Translation3d& translation, bool setInitialPoints)
{
for(auto meshes : this->attachedMeshes)
{
......@@ -368,3 +434,21 @@ void VegaVolumetricMesh::translate(const Eigen::Translation3d& translation, bool
}
}
//---------------------------------------------------------------------------
void VegaVolumetricMesh::computeGravity(const core::Vec3d& gravity,
core::Vectord& gravityForce)
{
auto verticesPerElement = this->mesh->getNumElementVertices();
auto invVerticesPerElement = 1.0/verticesPerElement;
auto mg = core::Matrixd::Map(gravityForce.data(),3,this->getNumberOfVertices());
for(size_t e = 0; e < this->getNumberOfElements(); ++e)
{
double mass = invVerticesPerElement * this->mesh->getElementDensity(e) *
this->mesh->getElementVolume(e);
for(int j = 0; j < verticesPerElement; ++j)
mg.col(this->mesh->getVertexIndex(e,j)) += mass * gravity;
}
}
......@@ -77,12 +77,17 @@ public:
///
/// \brief Attach surface mesh to the volume mesh and stores interpolation weights
///
void attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh, const double &radius = -1.0);
void attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh,
const double &radius = -1.0,
bool useForRendering = true);
///
/// \brief Get attached surface mesh
///
void attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh, const std::string &fileName, const double &radius = 5.0);
void attachSurfaceMesh(std::shared_ptr<SurfaceMesh> surfaceMesh,
const std::string &fileName,
const double &radius = 5.0,
bool useForRendering = true);
///
/// \brief Return underlying Vega mesh data structure.
......@@ -168,6 +173,20 @@ public:
///
void translate(const Eigen::Translation3d &translation, bool setInitialPoints = false);
///
/// \brief Interpolate the displacements on to the given mesh.
///
/// \param x Displacements.
/// \param mesh Mesh where the interpolation will be computed.
///
void interpolate(const core::Vectord &x,
std::shared_ptr< SurfaceMesh > mesh);
///
/// \brief Compute gravity force
///
void computeGravity(const core::Vec3d &gravity, core::Vectord &gravityForce);
private:
// Vega mesh base object
std::shared_ptr<VolumetricMesh> mesh;
......
......@@ -24,6 +24,10 @@
// SimMedTK includes
#include "SceneModels/DeformableSceneObject.h"
#include "TimeIntegrators/BackwarEuler.h"
#include "TimeIntegrators/ForwardEuler.h"
#include "Core/RenderDelegate.h"
#include "Core/Factory.h"
#include <cmath> // for std::isfinite()
......@@ -32,6 +36,9 @@ DeformableSceneObject::DeformableSceneObject():
OdeSystem(),
integrationScheme(TimeIntegrator::ImplicitEuler)
{
this->gravity = core::Vec3d::UnitY();
this->numOfDOF = 0;
this->numOfNodes = 0;
}
//---------------------------------------------------------------------------
......@@ -40,9 +47,9 @@ void DeformableSceneObject::applyContactForces()
for(const auto & cf : this->getContactForces())
{
auto i = cf.first;
this->f(i) += cf.second(0);
this->f(i + 1) += cf.second(1);
this->f(i + 2) += cf.second(2);
this->f(i) -= cf.second(0);
this->f(i + 1) -= cf.second(1);
this->f(i + 2) -= cf.second(2);
}
}
......@@ -90,9 +97,9 @@ void DeformableSceneObject::update(const double dt)
this->odeSolver->solve(*this->currentState, *this->newState, dt);
// TODO: Check state validity
// Check state validity
if(!std::isfinite(this->newState->getPositions().sum()) ||
!std::isfinite(this->newState->getVelocities().sum()) )
!std::isfinite(this->newState->getVelocities().sum()))
{
// TODO: log this and throw exception, this is a fatal error
return;
......@@ -122,3 +129,16 @@ std::shared_ptr< OdeSystemState > DeformableSceneObject::getPreviousState()
{
return this->previousState;
}
//---------------------------------------------------------------------------
Eigen::Map<core::Vec3d> DeformableSceneObject::getVelocity(const int index)
{
auto velocities = this->currentState->getVelocities();
return core::Vec3d::Map(&velocities(index));
}
//---------------------------------------------------------------------------
const core::Vec3d& DeformableSceneObject::getGravity() const
{
return this->gravity;
}
......@@ -26,9 +26,8 @@
// SimMedTK includes
#include "SceneModels/SceneObject.h"
#include "TimeIntegrators/OdeSystem.h"
#include "TimeIntegrators/TimeIntegrator.h"
#include "TimeIntegrators/BackwarEuler.h"
#include "TimeIntegrators/ForwardEuler.h"
///
/// \brief Base class for all deformable scene objects.
......@@ -89,11 +88,12 @@ public:
///
/// \brief Returns velocity of at a given location for the current state.
///
Eigen::Map<core::Vec3d> getVelocity(const int index)
{
auto velocities = this->currentState->getVelocities();
return Eigen::Map<core::Vec3d>(&velocities(index));
}
Eigen::Map<core::Vec3d> getVelocity(const int index);
///
/// \brief Returns velocity of at a given location for the current state.
///
const core::Vec3d &getGravity() const;
private:
///////////////////////////////////////////////////////////////////////////////
......@@ -130,6 +130,13 @@ protected:
core::SparseMatrixd K; ///> Stiffness matrix
core::Vectord f; ///> Accumulative forces vector
// Gravity
core::Vec3d gravity;
// Total number of degrees of freedom
double numOfDOF;
double numOfNodes;
TimeIntegrator::IntegratorType integrationScheme; ///> Integration scheme used.
};
......
......@@ -23,6 +23,9 @@
#include "SceneModels/SceneObject.h"
#include "Simulators/ObjectSimulator.h"
#include "Core/BaseMesh.h"
#include "Core/Model.h"
#include "Core/Factory.h"
SceneObject::SceneObject()
{
......@@ -33,6 +36,10 @@ SceneObject::SceneObject()