Commit 3186990d authored by Ricardo Ortiz's avatar Ricardo Ortiz
Browse files

ENH: Refactor the contact handling module.

parent d6136d80
Pipeline #4816 passed with stage
......@@ -29,25 +29,29 @@ PenaltyContactFemToStatic::PenaltyContactFemToStatic(bool typeBilateral) : Penal
type = ContactHandlingType::PenaltyFemToStatic;
}
//---------------------------------------------------------------------------
PenaltyContactFemToStatic::PenaltyContactFemToStatic(
bool typeBilateral,
const std::shared_ptr<SceneObject>& sceneObjFirst,
const std::shared_ptr<SceneObject>& sceneObjSecond)
const std::shared_ptr<SceneObjectDeformable>& sceneObjSecond)
: PenaltyContactHandling(typeBilateral, sceneObjFirst, sceneObjSecond)
{
type = ContactHandlingType::PenaltyFemToStatic;
}
//---------------------------------------------------------------------------
PenaltyContactFemToStatic::~PenaltyContactFemToStatic()
{
}
//---------------------------------------------------------------------------
void PenaltyContactFemToStatic::computeUnilateralContactForces()
{
auto femSceneObject = std::static_pointer_cast<VegaFemSceneObject>(this->getSecondSceneObject());
auto femSceneObject = std::static_pointer_cast<SceneObjectDeformable>(this->getSecondSceneObject());
this->computeForces(femSceneObject);
}
//---------------------------------------------------------------------------
void PenaltyContactFemToStatic::computeBilateralContactForces()
{
}
......@@ -37,7 +37,7 @@ public:
PenaltyContactFemToStatic(bool typeBilateral,
const std::shared_ptr<SceneObject>& sceneObjFirst,
const std::shared_ptr<SceneObject>& sceneObjSecond);
const std::shared_ptr<SceneObjectDeformable>& sceneObjSecond);
virtual ~PenaltyContactFemToStatic();
......@@ -47,6 +47,35 @@ public:
/// \brief Get the forces on both the scene objects using penalty method
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);
}
}
}
};
#endif // SMPENALTY_FEM_TO_STATIC_SCENE_OBJECT
......@@ -33,6 +33,7 @@ PenaltyContactHandling::PenaltyContactHandling(bool typeBilateral) :
{
}
//---------------------------------------------------------------------------
PenaltyContactHandling::PenaltyContactHandling(bool typeBilateral,
const std::shared_ptr<SceneObject>& sceneObjFirst,
const std::shared_ptr<SceneObject>& sceneObjSecond) :
......@@ -42,10 +43,12 @@ PenaltyContactHandling::PenaltyContactHandling(bool typeBilateral,
{
}
//---------------------------------------------------------------------------
PenaltyContactHandling::~PenaltyContactHandling()
{
};
//---------------------------------------------------------------------------
void PenaltyContactHandling::resolveContacts()
{
if (!isBilateral)
......
......@@ -25,7 +25,7 @@
// SimMedTK includes
#include "Core/ContactHandling.h"
#include "Simulators/SceneObjectDeformable.h"
#include "Core/CollisionPair.h"
/// \brief Penalty based for contact handling
......@@ -48,35 +48,6 @@ public:
/// \brief Get the forces on both the scene objects using penalty method
virtual void computeBilateralContactForces() = 0;
/// \brief Get the forces on both the scene objects using penalty method
virtual void computeForces(std::shared_ptr<SceneObject> 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);
}
}
}
///
/// \brief Set stiffness coefficient
///
......@@ -109,7 +80,7 @@ public:
return this->damping;
}
private:
protected:
double stiffness;
double damping;
};
......
Supports Markdown
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