DistanceGridCollisionModel.h

Go to the documentation of this file.

/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
*                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_H
#define SOFA_COMPONENT_COLLISION_DISTANCEGRIDCOLLISIONMODEL_H

#include <sofa/core/CollisionModel.h>
#include <sofa/component/MechanicalObject.h>
#include <sofa/defaulttype/Vec3Types.h>
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/helper/io/Mesh.h>
#include <sofa/component/topology/RegularGridTopology.h>


namespace sofa
{

namespace component
{

namespace collision
{

using namespace sofa::defaulttype;
using namespace sofa::helper;

class DistanceGrid
{
public:
    static SReal maxDist() { return (SReal)1e10; }
    typedef Vector3 Coord;
    typedef defaulttype::ExtVector<SReal> VecSReal;
    typedef defaulttype::ExtVector<Coord> VecCoord;

    DistanceGrid(int nx, int ny, int nz, Coord pmin, Coord pmax);

    DistanceGrid(int nx, int ny, int nz, Coord pmin, Coord pmax, defaulttype::ExtVectorAllocator<SReal>* alloc);

protected:
    ~DistanceGrid();

public:
    static DistanceGrid* load(const std::string& filename, double scale=1.0, int nx=64, int ny=64, int nz=64, Coord pmin = Coord(), Coord pmax = Coord());

    static DistanceGrid* loadShared(const std::string& filename, double scale=1.0, int nx=64, int ny=64, int nz=64, Coord pmin = Coord(), Coord pmax = Coord());

    DistanceGrid* addRef();

    bool release();

    bool save(const std::string& filename);

    void calcDistance(sofa::helper::io::Mesh* mesh, double scale=1.0);

    void calcCubeDistance(SReal dim=1, int np=5);

    void computeBBox();

    int getNx() const { return nx; }
    int getNy() const { return ny; }
    int getNz() const { return nz; }
    const Coord& getCellWidth() const { return cellWidth; }

    int size() const { return nxnynz; }

    const Coord& getBBMin() const { return bbmin; }
    const Coord& getBBMax() const { return bbmax; }
    void setBBMin(const Coord& val) { bbmin = val; }
    void setBBMax(const Coord& val) { bbmax = val; }
    Coord getBBCorner(int i) const { return Coord((i&1)?bbmax[0]:bbmin[0],(i&2)?bbmax[1]:bbmin[1],(i&4)?bbmax[2]:bbmin[2]); }
    bool inBBox(const Coord& p, SReal margin=0.0f) const
    {
    for (int c=0;c<3;++c)
        if (p[c] < bbmin[c]-margin || p[c] > bbmax[c]+margin) return false;
    return true;
    }

    const Coord& getPMin() const { return pmin; }
    const Coord& getPMax() const { return pmax; }
    Coord getCorner(int i) const { return Coord((i&1)?pmax[0]:pmin[0],(i&2)?pmax[1]:pmin[1],(i&4)?pmax[2]:pmin[2]); }

    bool isCube() const { return cubeDim != 0; }
    SReal getCubeDim() const { return cubeDim; }

    bool inGrid(const Coord& p) const
    {
        Coord epsilon = cellWidth*0.1;
    for (int c=0;c<3;++c)
        if (p[c] < pmin[c]+epsilon[c] || p[c] > pmax[c]-epsilon[c]) return false;
    return true;
    }

    Coord clamp(Coord p) const
    {
    for (int c=0;c<3;++c)
        if (p[c] < pmin[c]) p[c] = pmin[c];
        else if (p[c] > pmax[c]) p[c] = pmax[c];
    return p;
    }

    int ix(const Coord& p) const
    {
    return rfloor((p[0]-pmin[0])*invCellWidth[0]);
    }

    int iy(const Coord& p) const
    {
    return rfloor((p[1]-pmin[1])*invCellWidth[1]);
    }

    int iz(const Coord& p) const
    {
    return rfloor((p[2]-pmin[2])*invCellWidth[2]);
    }

    int index(const Coord& p, Coord& coefs) const
    {
    coefs[0] = (p[0]-pmin[0])*invCellWidth[0];
    coefs[1] = (p[1]-pmin[1])*invCellWidth[1];
    coefs[2] = (p[2]-pmin[2])*invCellWidth[2];
    int x = rfloor(coefs[0]);
    if (x<0) x=0; else if (x>=nx-1) x=nx-2;
    coefs[0] -= x;
    int y = rfloor(coefs[1]);
    if (y<0) y=0; else if (y>=ny-1) y=ny-2;
    coefs[1] -= y;
    int z = rfloor(coefs[2]);
    if (z<0) z=0; else if (z>=nz-1) z=nz-2;
    coefs[2] -= z;
    return x+nx*(y+ny*(z));
    }

    int index(const Coord& p) const
    {
    Coord coefs;
    return index(p, coefs);
    }

    int index(int x, int y, int z)
    {
    return x+nx*(y+ny*(z));
    }

    Coord coord(int x, int y, int z)
    {
    return pmin+Coord(x*cellWidth[0], y*cellWidth[1], z*cellWidth[2]);
    }

    SReal operator[](int index) const { return dists[index]; }
    SReal& operator[](int index) { return dists[index]; }

    static SReal interp(SReal coef, SReal a, SReal b)
    {
    return a+coef*(b-a);
    }

    SReal interp(int index, const Coord& coefs) const
    {
        return interp(coefs[2],interp(coefs[1],interp(coefs[0],dists[index          ],dists[index+1        ]),
                                               interp(coefs[0],dists[index  +nx     ],dists[index+1+nx     ])),
                               interp(coefs[1],interp(coefs[0],dists[index     +nxny],dists[index+1   +nxny]),
                                               interp(coefs[0],dists[index  +nx+nxny],dists[index+1+nx+nxny])));
    }

    SReal interp(const Coord& p) const
    {
    Coord coefs;
    int i = index(p, coefs);
    return interp(i, coefs);
    }
    
    Coord grad(int index, const Coord& coefs) const
    {
        // val = dist[0][0][0] * (1-x) * (1-y) * (1-z)
        //     + dist[1][0][0] * (  x) * (1-y) * (1-z)
        //     + dist[0][1][0] * (1-x) * (  y) * (1-z)
        //     + dist[1][1][0] * (  x) * (  y) * (1-z)
        //     + dist[0][0][1] * (1-x) * (1-y) * (  z)
        //     + dist[1][0][1] * (  x) * (1-y) * (  z)
        //     + dist[0][1][1] * (1-x) * (  y) * (  z)
        //     + dist[1][1][1] * (  x) * (  y) * (  z)
        // dval / dx = (dist[1][0][0]-dist[0][0][0]) * (1-y) * (1-z)
        //           + (dist[1][1][0]-dist[0][1][0]) * (  y) * (1-z)
        //           + (dist[1][0][1]-dist[0][0][1]) * (1-y) * (  z)
        //           + (dist[1][1][1]-dist[0][1][1]) * (  y) * (  z)
        const SReal dist000 = dists[index          ];
        const SReal dist100 = dists[index+1        ];
        const SReal dist010 = dists[index  +nx     ];
        const SReal dist110 = dists[index+1+nx     ];
        const SReal dist001 = dists[index     +nxny];
        const SReal dist101 = dists[index+1   +nxny];
        const SReal dist011 = dists[index  +nx+nxny];
        const SReal dist111 = dists[index+1+nx+nxny];
        return Coord(
            interp(coefs[2],interp(coefs[1],dist100-dist000,dist110-dist010),interp(coefs[1],dist101-dist001,dist111-dist011)), //*invCellWidth[0],
            interp(coefs[2],interp(coefs[0],dist010-dist000,dist110-dist100),interp(coefs[0],dist011-dist001,dist111-dist101)), //*invCellWidth[1],
            interp(coefs[1],interp(coefs[0],dist001-dist000,dist101-dist100),interp(coefs[0],dist011-dist010,dist111-dist110))); //*invCellWidth[2]);
    }

    Coord grad(const Coord& p) const
    {
    Coord coefs;
    int i = index(p, coefs);
    return grad(i, coefs);
    }

    SReal eval(const Coord& x) const
    {
    SReal d;
    if (inGrid(x))
    {
        d = interp(x);
    }
    else
    {
        Coord xclamp = clamp(x);
        d = interp(xclamp);
        d = rsqrt((x-xclamp).norm2() + d*d); // we underestimate the distance
    }
    return d;
    }

    SReal quickeval(const Coord& x) const
    {
    SReal d;
    if (inGrid(x))
    {
        d = dists[index(x)] - cellWidth[0]; // we underestimate the distance
    }
    else
    {
        Coord xclamp = clamp(x);
        d = dists[index(xclamp)] - cellWidth[0]; // we underestimate the distance
        d = rsqrt((x-xclamp).norm2() + d*d);
    }
    return d;
    }

    SReal eval2(const Coord& x) const
    {
    SReal d2;
    if (inGrid(x))
    {
        SReal d = interp(x);
        d2 = d*d;
    }
    else
    {
        Coord xclamp = clamp(x);
        SReal d = interp(xclamp);
        d2 = ((x-xclamp).norm2() + d*d); // we underestimate the distance
    }
    return d2;
    }

    SReal quickeval2(const Coord& x) const
    {
    SReal d2;
    if (inGrid(x))
    {
        SReal d = dists[index(x)] - cellWidth[0]; // we underestimate the distance
        d2 = d*d;
    }
    else
    {
        Coord xclamp = clamp(x);
        SReal d = dists[index(xclamp)] - cellWidth[0]; // we underestimate the distance
        d2 = ((x-xclamp).norm2() + d*d);
    }
    return d2;
    }

    VecCoord meshPts;

protected:
    int nbRef;
    VecSReal dists;
    const int nx,ny,nz, nxny, nxnynz;
    const Coord pmin, pmax;
    const Coord cellWidth, invCellWidth;
    Coord bbmin, bbmax; 

    SReal cubeDim; 

    // Fast Marching Method Update
    enum Status { FMM_FRONT0 = 0, FMM_FAR = -1, FMM_KNOWN_OUT = -2, FMM_KNOWN_IN = -3 };
    helper::vector<int> fmm_status;
    helper::vector<int> fmm_heap;
    int fmm_heap_size;

    int fmm_pop();
    void fmm_push(int index);
    void fmm_swap(int entry1, int entry2);

    // Grid shared resources

    struct DistanceGridParams
    {
        std::string filename;
        double scale;
    int nx,ny,nz;
    Coord pmin,pmax;
    bool operator==(const DistanceGridParams& v) const
    {
        if (!(filename == v.filename)) return false;
        if (!(scale    == v.scale   )) return false;
        if (!(nx       == v.nx      )) return false;
        if (!(ny       == v.ny      )) return false;
        if (!(nz       == v.nz      )) return false;
        if (!(pmin[0]  == v.pmin[0] )) return false;
        if (!(pmin[1]  == v.pmin[1] )) return false;
        if (!(pmin[2]  == v.pmin[2] )) return false;
        if (!(pmax[0]  == v.pmax[0] )) return false;
        if (!(pmax[1]  == v.pmax[1] )) return false;
        if (!(pmax[2]  == v.pmax[2] )) return false;
        return true;
    }
    bool operator<(const DistanceGridParams& v) const
    {
        if (filename < v.filename) return true;
        if (filename > v.filename) return false;
        if (scale    < v.scale   ) return true;
        if (scale    > v.scale   ) return false;
        if (nx       < v.nx      ) return false;
        if (nx       > v.nx      ) return true;
        if (ny       < v.ny      ) return false;
        if (ny       > v.ny      ) return true;
        if (nz       < v.nz      ) return false;
        if (nz       > v.nz      ) return true;
        if (pmin[0]  < v.pmin[0] ) return false;
        if (pmin[0]  > v.pmin[0] ) return true;
        if (pmin[1]  < v.pmin[1] ) return false;
        if (pmin[1]  > v.pmin[1] ) return true;
        if (pmin[2]  < v.pmin[2] ) return false;
        if (pmin[2]  > v.pmin[2] ) return true;
        if (pmax[0]  < v.pmax[0] ) return false;
        if (pmax[0]  > v.pmax[0] ) return true;
        if (pmax[1]  < v.pmax[1] ) return false;
        if (pmax[1]  > v.pmax[1] ) return true;
        if (pmax[2]  < v.pmax[2] ) return false;
        if (pmax[2]  > v.pmax[2] ) return true;
        return false;
    }
    bool operator>(const DistanceGridParams& v) const
    {
        if (filename > v.filename) return true;
        if (filename < v.filename) return false;
        if (scale    > v.scale   ) return true;
        if (scale    < v.scale   ) return false;
        if (nx       > v.nx      ) return false;
        if (nx       < v.nx      ) return true;
        if (ny       > v.ny      ) return false;
        if (ny       < v.ny      ) return true;
        if (nz       > v.nz      ) return false;
        if (nz       < v.nz      ) return true;
        if (pmin[0]  > v.pmin[0] ) return false;
        if (pmin[0]  < v.pmin[0] ) return true;
        if (pmin[1]  > v.pmin[1] ) return false;
        if (pmin[1]  < v.pmin[1] ) return true;
        if (pmin[2]  > v.pmin[2] ) return false;
        if (pmin[2]  < v.pmin[2] ) return true;
        if (pmax[0]  > v.pmax[0] ) return false;
        if (pmax[0]  < v.pmax[0] ) return true;
        if (pmax[1]  > v.pmax[1] ) return false;
        if (pmax[1]  < v.pmax[1] ) return true;
        if (pmax[2]  > v.pmax[2] ) return false;
        if (pmax[2]  < v.pmax[2] ) return true;
        return false;
    }
    };
    static std::map<DistanceGridParams, DistanceGrid*>& getShared();

};


class RigidDistanceGridCollisionModel;

class RigidDistanceGridCollisionElement : public core::TCollisionElementIterator<RigidDistanceGridCollisionModel>
{
public:
  
    RigidDistanceGridCollisionElement(RigidDistanceGridCollisionModel* model, int index);

    explicit RigidDistanceGridCollisionElement(core::CollisionElementIterator& i);

    DistanceGrid* getGrid();

    bool isTransformed();
    const Matrix3& getRotation();
    const Vector3& getTranslation();

    void setGrid(DistanceGrid* surf);

    DistanceGrid* getPrevGrid();
    const Matrix3& getPrevRotation();
    const Vector3& getPrevTranslation();
    double getPrevDt();
    
    void setNewState(double dt, DistanceGrid* grid, const Matrix3& rotation, const Vector3& translation);
};

class RigidDistanceGridCollisionModel : public core::CollisionModel
{
protected:

    class ElementData
    {
    public:
        Matrix3 rotation;
        Vector3 translation;
        DistanceGrid* grid;
        
        DistanceGrid* prevGrid; 
        Matrix3 prevRotation; 
        Vector3 prevTranslation; 
        double prevDt; 

        
        bool isTransformed; 
        ElementData() : grid(NULL), prevGrid(NULL), prevDt(0.0), isTransformed(false) { rotation.identity(); prevRotation.identity(); }
    };

    sofa::helper::vector<ElementData> elems;
    bool modified;

    // Input data parameters
    Data< std::string > filename;
    Data< double > scale;
    Data< helper::fixed_array<DistanceGrid::Coord,2> > box;
    Data< int > nx;
    Data< int > ny;
    Data< int > nz;
    Data< std::string > dumpfilename;

    core::componentmodel::behavior::MechanicalState<RigidTypes>* rigid;

    void updateGrid();
public:
    typedef Rigid3Types InDataTypes;
    typedef Vec3Types DataTypes;
    typedef RigidDistanceGridCollisionElement Element;

    Data< bool > usePoints;

    RigidDistanceGridCollisionModel();

    ~RigidDistanceGridCollisionModel();

    core::componentmodel::behavior::MechanicalState<InDataTypes>* getRigidModel() { return rigid; }
    core::componentmodel::behavior::MechanicalState<InDataTypes>* getMechanicalState() { return rigid; }

    void init();

    DistanceGrid* getGrid(int index=0)
    {
        return elems[index].grid;
    }
    bool isTransformed(int index=0)
    {
        return elems[index].isTransformed;
    }
    const Matrix3& getRotation(int index=0)
    {
        return elems[index].rotation;
    }
    const Vector3& getTranslation(int index=0)
    {
        return elems[index].translation;
    }

    void setGrid(DistanceGrid* surf, int index=0);

    DistanceGrid* getPrevGrid(int index=0)
    {
        return elems[index].prevGrid;
    }
    const Matrix3& getPrevRotation(int index=0)
    {
        return elems[index].prevRotation;
    }
    const Vector3& getPrevTranslation(int index=0)
    {
        return elems[index].prevTranslation;
    }
    double getPrevDt(int index=0)
    {
        return elems[index].prevDt;
    }
    
    void setNewState(int index, double dt, DistanceGrid* grid, const Matrix3& rotation, const Vector3& translation);
    
    
    void setNewState(double dt, DistanceGrid* grid, const Matrix3& rotation, const Vector3& translation);

    // -- CollisionModel interface

    void resize(int size);

    void computeBoundingTree(int maxDepth=0);

    void draw(int index);

    void draw();
};

inline RigidDistanceGridCollisionElement::RigidDistanceGridCollisionElement(RigidDistanceGridCollisionModel* model, int index)
: core::TCollisionElementIterator<RigidDistanceGridCollisionModel>(model, index)
{}

inline RigidDistanceGridCollisionElement::RigidDistanceGridCollisionElement(core::CollisionElementIterator& i)
: core::TCollisionElementIterator<RigidDistanceGridCollisionModel>(static_cast<RigidDistanceGridCollisionModel*>(i.getCollisionModel()), i.getIndex())
{
}

inline DistanceGrid* RigidDistanceGridCollisionElement::getGrid() { return model->getGrid(index); }
inline void RigidDistanceGridCollisionElement::setGrid(DistanceGrid* surf) { return model->setGrid(surf, index); }

inline bool RigidDistanceGridCollisionElement::isTransformed() { return model->isTransformed(index); }
inline const Matrix3& RigidDistanceGridCollisionElement::getRotation() { return model->getRotation(index); }
inline const Vector3& RigidDistanceGridCollisionElement::getTranslation() { return model->getTranslation(index); }

inline DistanceGrid* RigidDistanceGridCollisionElement::getPrevGrid() { return model->getPrevGrid(index); }
inline const Matrix3& RigidDistanceGridCollisionElement::getPrevRotation() { return model->getPrevRotation(index); }
inline const Vector3& RigidDistanceGridCollisionElement::getPrevTranslation() { return model->getPrevTranslation(index); }
inline double RigidDistanceGridCollisionElement::getPrevDt() { return model->getPrevDt(index); }

inline void RigidDistanceGridCollisionElement::setNewState(double dt, DistanceGrid* grid, const Matrix3& rotation, const Vector3& translation)
{
    return model->setNewState(dt, grid, rotation, translation);
}


class FFDDistanceGridCollisionModel;

class FFDDistanceGridCollisionElement : public core::TCollisionElementIterator<FFDDistanceGridCollisionModel>
{
public:
  
    FFDDistanceGridCollisionElement(FFDDistanceGridCollisionModel* model, int index);

    explicit FFDDistanceGridCollisionElement(core::CollisionElementIterator& i);

    DistanceGrid* getGrid();

    void setGrid(DistanceGrid* surf);
};

class FFDDistanceGridCollisionModel : public core::CollisionModel
{
public:
    typedef SReal GSReal;
    typedef DistanceGrid::Coord GCoord;
    class DeformedCube
    {
    public:
        DistanceGrid* grid;
        DeformedCube() : grid(NULL) {}
        int elem; 
        struct Point
        {
            GCoord bary; 
            int index; 
        };
        vector<Point> points; 
        GCoord initP0,initDP,invDP; 
        GCoord corners[8]; 
        enum {C000 = 0+0+0,
              C100 = 1+0+0,
              C010 = 0+2+0,
              C110 = 1+2+0,
              C001 = 0+0+4,
              C101 = 1+0+4,
              C011 = 0+2+4,
              C111 = 1+2+4};
        typedef Vec<4,GSReal> Plane; 
        Plane faces[6]; 
        enum {FX0 = 0+0,
              FX1 = 0+1,
              FY0 = 2+0,
              FY1 = 2+1,
              FZ0 = 4+0,
              FZ1 = 4+1};
        GCoord Dx;   
        GCoord Dy;   
        GCoord Dz;   
        GCoord Dxy;  
        GCoord Dxz;  
        GCoord Dyz;  
        GCoord Dxyz; 


        void updateDeform();
        
        GCoord center; 
        GSReal radius; 
        vector<GCoord> deformedPoints; 
        bool pointsUpdated; 
        void updatePoints(); 
        bool facesUpdated; 
        void updateFaces(); 

        DistanceGrid::Coord baryCoords(const GCoord& c) const
        {
            return GCoord( (c[0]-initP0[0])*invDP[0],
                           (c[1]-initP0[1])*invDP[1],
                           (c[2]-initP0[2])*invDP[2]);
        }
        GCoord initpos(const GCoord& b) const
        {
            return GCoord( initP0[0]+initDP[0]*b[0],
                           initP0[1]+initDP[1]*b[1],
                           initP0[2]+initDP[2]*b[2]);
        }
        GCoord deform(const GCoord& b) const
        {
            return corners[C000] + Dx*b[0] + (Dy + Dxy*b[0])*b[1] + (Dz + Dxz*b[0] + (Dyz + Dxyz*b[0])*b[1])*b[2];
        }

        static GSReal interp(GSReal coef, GSReal a, GSReal b)
        {
            return a+coef*(b-a);
        }

        GCoord deformDir(const GCoord& b, const GCoord& dir) const
        {
            GCoord r;
            // dp/dx = Dx + Dxy*y + Dxz*z + Dxyz*y*z
            r  = (Dx + Dxy*b[1] + (Dxz + Dxyz*b[1])*b[2])*dir[0];
            // dp/dy = Dy + Dxy*x + Dyz*z + Dxyz*x*z
            r += (Dy + Dxy*b[0] + (Dyz + Dxyz*b[0])*b[2])*dir[1];
            // dp/dz = Dz + Dxz*x + Dyz*y + Dxyz*x*y
            r += (Dz + Dxz*b[0] + (Dyz + Dxyz*b[0])*b[1])*dir[2];
            return r;
        }

        Mat<3,3,double> Jdeform(const GCoord& b) const
        {
            Mat<3,3,double> J;
            for (int i=0;i<3;i++)
            {
                // dp/dx = Dx + Dxy*y + Dxz*z + Dxyz*y*z
                J[i][0] = (Dx[i] + Dxy[i]*b[1] + (Dxz[i] + Dxyz[i]*b[1])*b[2]);
                // dp/dy = Dy + Dxy*x + Dyz*z + Dxyz*x*z
                J[i][1] = (Dy[i] + Dxy[i]*b[0] + (Dyz[i] + Dxyz[i]*b[0])*b[2]);
                // dp/dz = Dz + Dxz*x + Dyz*y + Dxyz*x*y
                J[i][2] = (Dz[i] + Dxz[i]*b[0] + (Dyz[i] + Dxyz[i]*b[0])*b[1]);
            }
            return J;
        }

        GCoord undeform0(const GCoord& p) const
        {
            GCoord b;
            for (int i=0;i<3;i++)
            {
                GSReal b0 = faces[2*i+0]*Plane(p,1);
                GSReal b1 = faces[2*i+1]*Plane(p,1);
                b[i] = b0 / (b0 + b1);
            }
            return b;
        }
        GCoord undeformDir(const GCoord& b, const GCoord& dir) const
        {
            // we want to find b2 so that deform(b2)-deform(b) = dir
            // we can use Newton's method using the jacobian of the deformation.
            Mat<3,3,double> m = Jdeform(b);
            Mat<3,3,double> minv;
            minv.invert(m);
            return minv*dir;
        }
        
        Plane computePlane(int c00, int c10, int c01, int c11);
    };

protected:

    sofa::helper::vector<DeformedCube> elems;

    // Input data parameters
    Data< std::string > filename;
    Data< double > scale;
    Data< helper::fixed_array<DistanceGrid::Coord,2> > box;
    Data< int > nx;
    Data< int > ny;
    Data< int > nz;
    Data< std::string > dumpfilename;

    core::componentmodel::behavior::MechanicalState<Vec3Types>* ffd;
    topology::RegularGridTopology* ffdGrid;

    void updateGrid();
public:
    typedef Vec3Types InDataTypes;
    typedef Vec3Types DataTypes;
    typedef topology::RegularGridTopology Topology;
    typedef FFDDistanceGridCollisionElement Element;

    Data< bool > usePoints;

    FFDDistanceGridCollisionModel();

    ~FFDDistanceGridCollisionModel();

    core::componentmodel::behavior::MechanicalState<DataTypes>* getDeformModel() { return ffd; }
    topology::RegularGridTopology* getDeformGrid() { return ffdGrid; }

    // alias used by ContactMapper

    core::componentmodel::behavior::MechanicalState<DataTypes>* getMechanicalState() { return ffd; }
    topology::RegularGridTopology* getMeshTopology() { return ffdGrid; }

    void init();

    DistanceGrid* getGrid(int index=0)
    {
        return elems[index].grid;
    }

    DeformedCube& getDeformCube(int index=0)
    {
        return elems[index];
    }

    void setGrid(DistanceGrid* surf, int index=0);

    // -- CollisionModel interface

    void resize(int size);

    void computeBoundingTree(int maxDepth=0);

    void draw(int index);

    void draw();
};

inline FFDDistanceGridCollisionElement::FFDDistanceGridCollisionElement(FFDDistanceGridCollisionModel* model, int index)
: core::TCollisionElementIterator<FFDDistanceGridCollisionModel>(model, index)
{}

inline FFDDistanceGridCollisionElement::FFDDistanceGridCollisionElement(core::CollisionElementIterator& i)
: core::TCollisionElementIterator<FFDDistanceGridCollisionModel>(static_cast<FFDDistanceGridCollisionModel*>(i.getCollisionModel()), i.getIndex())
{
}

inline DistanceGrid* FFDDistanceGridCollisionElement::getGrid() { return model->getGrid(index); }
inline void FFDDistanceGridCollisionElement::setGrid(DistanceGrid* surf) { return model->setGrid(surf, index); }

} // namespace collision

} // namespace component

} // namespace sofa

#endif