TetrahedronModel.cpp

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                             *
******************************************************************************/
#include <sofa/component/collision/TetrahedronModel.h>
#include <sofa/component/collision/CubeModel.h>
#include <sofa/helper/gl/template.h>
#include <sofa/simulation/tree/GNode.h>
#include <sofa/component/topology/RegularGridTopology.h>
#include <sofa/core/CollisionElement.h>
#include <sofa/core/ObjectFactory.h>
#include <vector>
#include <sofa/helper/system/gl.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace sofa
{

namespace component
{

namespace collision
{

SOFA_DECL_CLASS(TetrahedronModel)
    
int TetrahedronModelClass = core::RegisterObject("collision model using a tetrahedral mesh, as described in BaseMeshTopology")
.add< TetrahedronModel >()
.addAlias("Tetrahedron")
;

TetrahedronModel::TetrahedronModel()
: tetra(NULL), mstate(NULL)
{
}
    
void TetrahedronModel::resize(int size)
{
    this->core::CollisionModel::resize(size);
    elems.resize(size);
    if (getPrevious() != NULL) getPrevious()->resize(0); // force recomputation of bounding tree
}

void TetrahedronModel::init()
{
    _topology = this->getContext()->getMeshTopology();

    this->CollisionModel::init();
    mstate = dynamic_cast< core::componentmodel::behavior::MechanicalState<Vec3Types>* > (getContext()->getMechanicalState());

    if (mstate==NULL)
    {
        std::cerr << "ERROR: TetrahedronModel requires a Vec3 Mechanical Model.\n";
        return;
    }
       
    if (!_topology) {
        std::cerr << "ERROR: TetrahedronModel requires a BaseMeshTopology.\n";
        return;
    }
    
    tetra = &_topology->getTetras();
    resize(tetra->size());
    
}
    
void TetrahedronModel::handleTopologyChange()
{
    resize(_topology->getNbTetras());
}

void TetrahedronModel::draw(int index)
{
    Tetrahedron t(this,index);
    glBegin(GL_TRIANGLES);
    Coord p1 = t.p1();
    Coord p2 = t.p2();
    Coord p3 = t.p3();
    Coord p4 = t.p4();
    Coord c = (p1+p2+p3+p4)*0.25f;
    p1 += (c-p1)*0.1f;
    p2 += (c-p2)*0.1f;
    p3 += (c-p3)*0.1f;
    p4 += (c-p4)*0.1f;
    Coord n1,n2,n3,n4;
    n1 = cross(p3-p1,p2-p1); n1.normalize();
    helper::gl::glNormalT(n1);
    helper::gl::glVertexT(p1);
    helper::gl::glVertexT(p3);
    helper::gl::glVertexT(p2);
    
    n2 = cross(p4-p1,p3-p1); n2.normalize();
    helper::gl::glNormalT(n2);
    helper::gl::glVertexT(p1);
    helper::gl::glVertexT(p4);
    helper::gl::glVertexT(p3);
    
    n3 = cross(p2-p1,p4-p1); n3.normalize();
    helper::gl::glNormalT(n3);
    helper::gl::glVertexT(p1);
    helper::gl::glVertexT(p2);
    helper::gl::glVertexT(p4);
    
    n4 = cross(p3-p2,p4-p2); n4.normalize();
    helper::gl::glNormalT(n4);
    helper::gl::glVertexT(p2);
    helper::gl::glVertexT(p3);
    helper::gl::glVertexT(p4);
    glEnd();
    if (getContext()->getShowNormals())
    {
    Coord p;
    glBegin(GL_LINES);
    p = (p1+p2+p3)*(1.0/3.0);
    helper::gl::glVertexT(p);
    helper::gl::glVertexT(p+n1*0.1);
    p = (p1+p3+p4)*(1.0/3.0);
    helper::gl::glVertexT(p);
    helper::gl::glVertexT(p+n2*0.1);
    p = (p1+p4+p2)*(1.0/3.0);
    helper::gl::glVertexT(p);
    helper::gl::glVertexT(p+n3*0.1);
    p = (p2+p3+p4)*(1.0/3.0);
    helper::gl::glVertexT(p);
    helper::gl::glVertexT(p+n4*0.1);
    glEnd();
    }
}

void TetrahedronModel::draw()
{
    if (mstate && _topology && getContext()->getShowCollisionModels())
    {
        if (getContext()->getShowWireFrame())
            glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

        glEnable(GL_LIGHTING);
        //Enable<GL_BLEND> blending;
        //glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);

        glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, getColor4f());
        static const float emissive[4] = { 0.0f, 0.0f, 0.0f, 0.0f};
        static const float specular[4] = { 1.0f, 1.0f, 1.0f, 1.0f};
        glMaterialfv (GL_FRONT_AND_BACK, GL_EMISSION, emissive);
        glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular);
        glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 20);

        for (int i=0;i<size;i++)
        {
            draw(i);
        }

        glColor3f(1.0f, 1.0f, 1.0f);
        glDisable(GL_LIGHTING);
        if (getContext()->getShowWireFrame())
            glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    }
    if (getPrevious()!=NULL && getContext()->getShowBoundingCollisionModels())
        getPrevious()->draw();
}

void TetrahedronModel::computeBoundingTree(int maxDepth)
{
    CubeModel* cubeModel = createPrevious<CubeModel>();
    if (!mstate || !_topology) return;
    if (!isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile

    Vector3 minElem, maxElem;
    const VecCoord& x = *this->mstate->getX();

    for (int i=0;i<size;i++)
    {
    Tetrahedron t(this,i);
    const Vector3& pt1 = x[t.p1Index()];
    const Vector3& pt2 = x[t.p2Index()];
    const Vector3& pt3 = x[t.p3Index()];
    const Vector3& pt4 = x[t.p4Index()];
    Matrix3 m, minv;
    m[0] = pt2-pt1;
    m[1] = pt3-pt1;
    m[2] = pt4-pt1;
    m.transpose();
    minv.invert(m);
    elems[i].coord0 = pt1;
    elems[i].bary2coord = m;
    elems[i].coord2bary = minv;
    }
    
    if (maxDepth == 0)
    { // no hierarchy
        if (empty())
            cubeModel->resize(0);
        else
        {
            cubeModel->resize(1);
            minElem = x[0];
            maxElem = x[0];
            for (unsigned i=1;i<x.size();i++)
            {
                const Vector3& pt1 = x[i];
                if (pt1[0] > maxElem[0]) maxElem[0] = pt1[0];
                else if (pt1[0] < minElem[0]) minElem[0] = pt1[0];
                if (pt1[1] > maxElem[1]) maxElem[1] = pt1[1];
                else if (pt1[1] < minElem[1]) minElem[1] = pt1[1];
                if (pt1[2] > maxElem[2]) maxElem[2] = pt1[2];
                else if (pt1[2] < minElem[2]) minElem[2] = pt1[2];
            }
            cubeModel->setLeafCube(0, std::make_pair(this->begin(),this->end()), minElem, maxElem); // define the bounding box of the current Tetrahedron
        }
    }
    else
    {
        cubeModel->resize(size);  // size = number of Tetrahedrons
        if (!empty())
        {
            for (int i=0;i<size;i++)
            {
                Tetrahedron t(this,i);
                const Vector3& pt1 = x[t.p1Index()];
                const Vector3& pt2 = x[t.p2Index()];
                const Vector3& pt3 = x[t.p3Index()];
                const Vector3& pt4 = x[t.p4Index()];
                for (int c = 0; c < 3; c++)
                {
                                                  minElem[c] = pt1[c];
                                                  maxElem[c] = pt1[c];
                         if (pt2[c] > maxElem[c]) maxElem[c] = pt2[c];
                    else if (pt2[c] < minElem[c]) minElem[c] = pt2[c];
                         if (pt3[c] > maxElem[c]) maxElem[c] = pt3[c];
                    else if (pt3[c] < minElem[c]) minElem[c] = pt3[c];
                         if (pt4[c] > maxElem[c]) maxElem[c] = pt4[c];
                    else if (pt4[c] < minElem[c]) minElem[c] = pt4[c];
                }
                cubeModel->setParentOf(i, minElem, maxElem); // define the bounding box of the current Tetrahedron
            }
            cubeModel->computeBoundingTree(maxDepth);
        }
    }
}

} // namespace collision

} // namespace component

} // namespace sofa