PluginHertzianPhysics.hpp

Go to the documentation of this file.

#ifndef PLUGINBODYHERTZIANPHYSICS_HPP
#define PLUGINBODYHERTZIANPHYSICS_HPP
 
#include <vector>
#include <mecacell/mecacell.h>
#include <math.h>
#include <chrono>
#include "Tensor.hpp"
#include "PhysicsConstraints.hpp"
 
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
 
using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Triangulation_vertex_base_with_info_3<void *, K>;
using Tds = CGAL::Triangulation_data_structure_3<Vb>;
using Delaunay = CGAL::Delaunay_triangulation_3<K, Tds>;
using Point = Delaunay::Point;
 
namespace HertzianPhysics {
 
    template<typename cell_t>
    class PluginHertzianPhysics {
 
    private:
        Delaunay triangulation;  
        double coherenceCoeff = 0.2;  
        bool areForcesNegligible = false; 
        bool areMovementsNegligible = false; 
        MecaCell::Vec minConstraint; 
        MecaCell::Vec maxConstraint; 
        PhysicsConstraints constraints; 
        bool isDelaunayCoherent() {
            bool coherence = true;
 
            if (triangulation.number_of_vertices() > 0) {
 
                typename Delaunay::Finite_vertices_iterator vit;
 
                double tolerance;
                for (vit = triangulation.finite_vertices_begin();
                     vit != triangulation.finite_vertices_end(); ++vit) { // For each vertex
                    cell_t *c = static_cast<cell_t *>(vit->info());
                    tolerance = c->getBody().getBoundingBoxRadius() * coherenceCoeff;
                    tolerance *= tolerance;
                    if ((c->getBody().getPosition().x() * vit->point().x() +
                        c->getBody().getPosition().y() * vit->point().y() +
                        c->getBody().getPosition().z() * vit->point().z())
                        >= tolerance) {  // Verify the coherence between the real position of a cell and its position in the triangulation
                        coherence = false;
                        break;
                    }
                }
            } else coherence = false;
            return coherence;
        }
 
        void moveDelaunay() {
            typename Delaunay::Finite_vertices_iterator vit;
            vit = triangulation.finite_vertices_begin();
            double tolerance;
            for (int i = 0; i < triangulation.number_of_vertices(); ++i) { // For each vertex
                cell_t *c = static_cast<cell_t *>(vit->info());
                tolerance = c->getBody().getBoundingBoxRadius() * coherenceCoeff;
                tolerance *= tolerance;
                if ((c->getBody().getPosition().x() * vit->point().x() +
                    c->getBody().getPosition().y() * vit->point().y() +
                    c->getBody().getPosition().z() * vit->point().z())
                    >= tolerance) { // Verify the coherence between the real position of a cell and its position in the triangulation
                    Point p(c->getBody().getPosition().x(), c->getBody().getPosition().y(),
                            c->getBody().getPosition().z());
                    triangulation.move(vit, p);
                }
                ++vit;
            }
        }
 
        template<typename world_t>
        void computeDelaunay(world_t *w) {
            for (cell_t *c : w->cells) c->clearConnectedCells();
            if (!isDelaunayCoherent()) {
                std::vector<Point> points;
                points.reserve(w->cells.size());
                for (cell_t *c : w->cells) points.push_back(Point(c->getBody().getPosition().x(), c->getBody().getPosition().y(), c->getBody().getPosition().z()));
 
                triangulation.clear();
                triangulation = Delaunay(boost::make_zip_iterator(boost::make_tuple(points.begin(), w->cells.begin())),
                                         boost::make_zip_iterator(boost::make_tuple(points.end(), w->cells.end())));
            }
        }
 
        template<typename world_t>
        void addCells(world_t *w) {
            std::vector<Point> points;
            points.reserve(w->newCells.size());
            for (cell_t *c : w->newCells) points.push_back(Point(c->getBody().getPosition().x(), c->getBody().getPosition().y(), c->getBody().getPosition().z()));
 
            triangulation.insert(boost::make_zip_iterator(boost::make_tuple(points.begin(), w->newCells.begin())),
                                 boost::make_zip_iterator(boost::make_tuple(points.end(), w->newCells.end())));
        }
 
        void removeCells() {
            typename Delaunay::Finite_vertices_iterator vit;
 
            for (vit = triangulation.finite_vertices_begin();
                 vit != triangulation.finite_vertices_end(); ++vit) { // For each vertex
                auto *c1 = static_cast<cell_t *>(vit->info());
 
                if (c1->isDead()) {
                    triangulation.remove(vit);
                }
            }
        }
 
        template<typename world_t>
        void updateConnections(world_t *w) {
            if (!triangulation.is_valid()) {
                std::cout << "UNVALID DELAUNAY !!" << std::endl;
            }
            for (auto c : w->cells) c->clearConnectedCells();
 
            typename Delaunay::Finite_edges_iterator eit;
 
            for (eit = triangulation.finite_edges_begin();
                 eit != triangulation.finite_edges_end(); ++eit) { // For each connection
                cell_t *c1 = static_cast<cell_t *>(eit->first->vertex(eit->second)->info());
                cell_t *c2 = static_cast<cell_t *>(eit->first->vertex(eit->third)->info());
                if (c1 != nullptr && c2 != nullptr) {
                    double maxdist = c1->getBody().getBoundingBoxRadius() + c2->getBody().getBoundingBoxRadius();
                    if ((c1->getBody().getPosition() - c2->getBody().getPosition()).sqlength() <=
                        maxdist * maxdist) { // Max distance to consider 2 neighbour cells
                        c1->addConnectedCell(c2);
                        c2->addConnectedCell(c1);
                    }
                } else {
                    exit(41);
                }
            }
        }
 
        template<typename world_t>
        void customConstraint(world_t *w) {
            for (auto c : w->cells) {
                double minConstX = minConstraint.x() + c->getBody().getBoundingBoxRadius();
                double minConstY = minConstraint.y() + c->getBody().getBoundingBoxRadius();
                double minConstZ = minConstraint.z() + c->getBody().getBoundingBoxRadius();
                
                double maxConstX = maxConstraint.x() - c->getBody().getBoundingBoxRadius();
                double maxConstZ = maxConstraint.z() - c->getBody().getBoundingBoxRadius();
                double maxConstY = maxConstraint.y() - c->getBody().getBoundingBoxRadius();
 
                if (c->getBody().getPosition().x() < minConstX) {
                    c->getBody().setPosition(
                            MecaCell::Vec(minConstX, c->getBody().getPosition().y(), c->getBody().getPosition().z()));
                } else if (c->getBody().getPosition().x() > maxConstX) {
                    c->getBody().setPosition(
                            MecaCell::Vec(maxConstX, c->getBody().getPosition().y(), c->getBody().getPosition().z()));
                }
                if (c->getBody().getPosition().y() < minConstY) {
                    c->getBody().setPosition(
                            MecaCell::Vec(c->getBody().getPosition().x(), minConstY, c->getBody().getPosition().z()));
                } else if (c->getBody().getPosition().y() > maxConstY) {
                    c->getBody().setPosition(
                            MecaCell::Vec(c->getBody().getPosition().x(), maxConstY, c->getBody().getPosition().z()));
                }
                if (c->getBody().getPosition().z() < minConstZ) {
                    c->getBody().setPosition(
                            MecaCell::Vec(c->getBody().getPosition().x(), c->getBody().getPosition().y(), minConstZ));
                } else if (c->getBody().getPosition().z() > maxConstZ) {
                    c->getBody().setPosition(
                            MecaCell::Vec(c->getBody().getPosition().x(), c->getBody().getPosition().y(), maxConstZ));
                }
            }
        }
 
        template<typename world_t>
        double updateForces(world_t *w) {
            double dtMin2 = w->dt * w->dt;
 
            double currentDt2 = dtMin2; // s²
 
            for (auto c : w->cells) {
                updateInteractionForces(c);
                c->getBody().receiveForce(c->getBody().getPurposeVelocity() * c->getBody().getGamma());
                c->getBody().receiveForce(constraints.getConstraintVelocity(c) * c->getBody().getGamma());
            }
 
            for (auto c : w->cells) {
                c->getBody().setComputed(false);
                MecaCell::Vector3D vel = c->getBody().getForce() / c->getBody().getGamma();
                c->getBody().setVelocity(vel);
                currentDt2 = std::pow(coherenceCoeff * c->getBody().getBoundingBoxRadius(), 2) / vel.sqlength();
                if (currentDt2 < dtMin2) dtMin2 = currentDt2;
            }
            return sqrt(dtMin2);
        }
 
        void updateInteractionForces(cell_t *c1) {
            double delta, R, E, Frep, Fadh;
            areForcesNegligible = true;
            for (auto c2 : c1->getConnectedCells()) {
                if (c2 != nullptr && !c2->getBody().getComputed()) {
                    MecaCell::Vec rij(c2->getBody().getPosition() - c1->getBody().getPosition());
 
                    delta = c1->getBody().getBoundingBoxRadius() + c2->getBody().getBoundingBoxRadius() - rij.length();
                    if (delta > 0) {
                        R = 1.0 / (1.0 / c1->getBody().getBoundingBoxRadius() + 1.0 / c2->getBody().getBoundingBoxRadius());
                        E = 1.0 /
                            ((1.0 - c1->getBody().getPoissonNumber() * c1->getBody().getPoissonNumber()) / c1->getBody().getYoungModulus() +
                            (1.0 - c2->getBody().getPoissonNumber() * c2->getBody().getPoissonNumber()) / c2->getBody().getYoungModulus());
 
                        Frep = 4.0 / 3.0 * E * sqrt(R) * pow(delta, 3.0 / 2.0);
                        Fadh = -M_PI * R *
                                      (c1->getBody().getAdhesion() + c2->getBody().getAdhesion()) / 2.0 *
                                      M_PI * delta * R;
                        MecaCell::Vec direction = rij.normalized();
                        c1->getBody().receiveForce(-direction * (Frep + Fadh));
                        c2->getBody().receiveForce(direction * (Frep + Fadh));
                        c1->getBody().addStress(Tensor(-direction * (Frep + Fadh), rij));
                        c2->getBody().addStress(Tensor(direction * (Frep + Fadh), -rij));
                    }
                }
            }
            c1->getBody().setComputed(true);
            c1->getBody().divTensor(c1->getBody().getVolume());
        }
 
        template<typename world_t>
        void updatePositions(world_t *w, double dt) {
            double radius;
            areMovementsNegligible = true;
            for (cell_t *c : w->cells) {
                MecaCell::Vec vel = c->getBody().getVelocity();
                radius = c->getBody().getBoundingBoxRadius();
 
                c->getBody().setPosition(c->getBody().getPosition() + vel * dt);
                if (areMovementsNegligible && vel.sqlength() > coherenceCoeff * coherenceCoeff * radius * radius)
                    areMovementsNegligible = false;
                
                constraints.snapCellsToPlane(c);
            }
        }
 
        template<typename world_t>
        inline void resetForces(world_t *w) {
            for (cell_t *c : w->cells) {
                c->getBody().resetForce();
            }
        }
 
        template<typename world_t>
        void applyPhysics(world_t *w) {
            double time = 0.0;
            double dt;
            int nbStep = 0;
 
            do {
                resetForces(w);
                dt = updateForces(w);
                if (time + dt > w->dt) dt = w->dt - time;
                updatePositions(w, dt);
                time += dt;
 
                nbStep++;
            } while (time < w->dt);
        }
 
    public:
        
        inline PluginHertzianPhysics() {
            minConstraint = MecaCell::Vec(-99999.0, -99999.0, -99999.0);
            maxConstraint = MecaCell::Vec(99999.0, 99999.0, 99999.0);
        }
 
        inline void setCoherenceCoeff(double c) { coherenceCoeff = c; }
 
        template<typename world_t>
        void beginUpdate(world_t *w) {
            computeDelaunay(w);
            updateConnections(w);
        }
 
        template <typename world_t>
        void onAddCell(world_t *w) {
            addCells(w);
        }
 
        template<typename world_t>
        void preDeleteDeadCellsUpdate(world_t *w) {
            for (cell_t *c : w->cells) {
                if (c->isDead()) {
                    removeCells();
                    break;
                }
            }
        }
 
        template<typename world_t>
        void preBehaviorUpdate(world_t *w) {
            applyPhysics(w);
        }
 
        void setMinConstraint(MecaCell::Vec min) {
            minConstraint = min;
        }
 
        void setMaxConstraint(MecaCell::Vec max) {
            maxConstraint = max;
        }
 
        inline PhysicsConstraints getConstraints() { return constraints; }
 
        inline void addPlane(MecaCell::Vec _n = MecaCell::Vec(0, 1, 0), MecaCell::Vec _p = MecaCell::Vec(0, 0, 0), bool _sticky = false) { constraints.addPlane(_n, _p, _sticky); }
 
        inline void addSymAxis(std::function<double(double, MecaCell::Vec, MecaCell::Vec)> _radiusFunction, MecaCell::Vec _axis = MecaCell::Vec(0., 1., 0.), MecaCell::Vec _origin = MecaCell::Vec(0., 0., 0.), double _length = 100, double _maxEffectRadius = 100, bool _fitShape = true, double _velocity_attraction = 0.001) {
            constraints.addSymAxis(_radiusFunction, _axis, _origin, _length, _maxEffectRadius, _fitShape, _velocity_attraction);
        }
 
        inline void addVelocity(MecaCell::Vec _v = MecaCell::Vec(0, 1, 0)) { constraints.addVelocity(_v); }
 
        inline void setCurvedBox(double radiusBoxPlane, double curvedDistToMaxSpeed, double maxSpeed) { constraints.setCurvedBox(radiusBoxPlane, curvedDistToMaxSpeed, maxSpeed); }
 
    };
}
#endif