[SofaGeneralMeshCollision] Introduce RayTraceNarrowPhase

examples/Components/collision/RayTraceCollision.scn

@@ -1,10 +1,11 @@
 <Node name="root" dt="0.03333">
     <RequiredPlugin name="SofaOpenglVisual"/>
     <RequiredPlugin name="SofaGeneralMeshCollision"/>
-    <RequiredPlugin pluginName='SofaGeneralDeformable'/>
-    <RequiredPlugin pluginName='SofaImplicitOdeSolver'/>
-    <RequiredPlugin pluginName='SofaLoader'/>
-    <RequiredPlugin pluginName='SofaMiscCollision'/> 
+    <RequiredPlugin name="SofaGeneralDeformable"/>
+    <RequiredPlugin name="SofaImplicitOdeSolver"/>
+    <RequiredPlugin name="SofaLoader"/>
+    <RequiredPlugin name="SofaMiscCollision"/>
+    <RequiredPlugin name="SofaUserInteraction"/>
 
     <DefaultPipeline verbose="0" depth="10" draw="0" />
     <RayTraceDetection name="N2" />

modules/SofaGeneralMeshCollision/CMakeLists.txt

@@ -17,15 +17,10 @@ list(APPEND HEADER_FILES
     ${SOFAGENERALMESHCOLLISION_SRC}/DirectSAPNarrowPhase.h
     ${SOFAGENERALMESHCOLLISION_SRC}/DSAPBox.h
     ${SOFAGENERALMESHCOLLISION_SRC}/IncrSAP.h
-    # IntrTriangleOBB.h
-    # IntrTriangleOBB.inl
     ${SOFAGENERALMESHCOLLISION_SRC}/MeshDiscreteIntersection.h
     ${SOFAGENERALMESHCOLLISION_SRC}/MeshDiscreteIntersection.inl
     ${SOFAGENERALMESHCOLLISION_SRC}/MeshMinProximityIntersection.h
-    # RigidContactMapper.h
-    # RigidContactMapper.inl
-    # SubsetContactMapper.h
-    # SubsetContactMapper.inl
+    ${SOFAGENERALMESHCOLLISION_SRC}/RayTraceNarrowPhase.h
     ${SOFAGENERALMESHCOLLISION_SRC}/TriangleOctree.h
     ${SOFAGENERALMESHCOLLISION_SRC}/TriangleOctreeModel.h
     )
@@ -34,11 +29,9 @@ list(APPEND SOURCE_FILES
     ${SOFAGENERALMESHCOLLISION_SRC}/DirectSAPNarrowPhase.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/DSAPBox.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/IncrSAP.cpp
-    # IntrTriangleOBB.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/MeshDiscreteIntersection.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/MeshMinProximityIntersection.cpp
-    # RigidContactMapper.cpp
-    # SubsetContactMapper.cpp
+    ${SOFAGENERALMESHCOLLISION_SRC}/RayTraceNarrowPhase.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/TriangleOctree.cpp
     ${SOFAGENERALMESHCOLLISION_SRC}/TriangleOctreeModel.cpp
     )

modules/SofaGeneralMeshCollision/src/SofaGeneralMeshCollision/RayTraceNarrowPhase.cpp

@@ -0,0 +1,266 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program 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 program 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 program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#include <SofaGeneralMeshCollision/RayTraceNarrowPhase.h>
+#include <sofa/core/ObjectFactory.h>
+#include <SofaBaseCollision/CubeModel.h>
+#include <SofaGeneralMeshCollision/TriangleOctreeModel.h>
+#include <sofa/core/visual/VisualParams.h>
+
+namespace sofa::component::collision
+{
+
+using sofa::core::collision::TDetectionOutputVector;
+
+int RayTraceNarrowPhaseClass = core::RegisterObject("Collision detection using TriangleOctreeModel").add < RayTraceNarrowPhase > ();
+
+RayTraceNarrowPhase::RayTraceNarrowPhase()
+:bDraw (initData(&bDraw, false, "draw","enable/disable display of results"))
+{}
+
+void RayTraceNarrowPhase::findPairsVolume (CubeCollisionModel * cm1, CubeCollisionModel * cm2)
+{
+    /*Obtain the CollisionModel at the lowest level, in this case it must be a TriangleOctreeModel */
+
+    TriangleOctreeModel *tm1 =
+            dynamic_cast < TriangleOctreeModel * >(cm1->getLast ());
+    TriangleOctreeModel *tm2 =
+            dynamic_cast < TriangleOctreeModel * >(cm2->getLast ());
+    if (!tm1 || !tm2)
+        return;
+
+
+    /*construct the octree of both models, when it still doesn't exisits */
+    if (!tm1->octreeRoot)
+    {
+
+        tm1->buildOctree ();
+    }
+
+    if (!tm2->octreeRoot)
+    {
+
+        tm2->buildOctree ();
+
+    }
+
+    /* get the output vector for a TriangleOctreeModel, TriangleOctreeModel Collision*/
+    /*Get the cube representing the bounding box of both Models */
+    // sofa::core::collision::DetectionOutputVector *& contacts=outputsMap[std::make_pair(tm1, tm2)];
+    core::collision::DetectionOutputVector*& contacts = this->getDetectionOutputs(tm1, tm2);
+
+
+    if (contacts == nullptr)
+    {
+        contacts = new
+                sofa::core::collision::TDetectionOutputVector <
+                        TriangleOctreeModel, TriangleOctreeModel >;
+
+    }
+
+
+
+
+    TDetectionOutputVector < TriangleOctreeModel,
+    TriangleOctreeModel > *outputs =
+            static_cast < TDetectionOutputVector < TriangleOctreeModel,
+            TriangleOctreeModel > *>(contacts);
+
+    Cube cube1 (cm1, 0);
+    Cube cube2 (cm2, 0);
+
+
+    const sofa::defaulttype::Vector3 & minVect2 = cube2.minVect ();
+    const sofa::defaulttype::Vector3 & maxVect2 = cube2.maxVect ();
+    int size = tm1->getSize ();
+
+    for (int j = 0; j < size; j++)
+    {
+
+        /*creates a Triangle for each object being tested */
+        Triangle tri1 (tm1, j);
+
+
+        /*cosAngle will store the angle between the triangle from t1 and his corresponding in t2 */
+        double cosAngle;
+        /*cosAngle will store the angle between the triangle from t1 and another triangle on t1 that is crossed by the -normal of tri1*/
+        double cosAngle2;
+        /*resTriangle and resTriangle2 will store the triangle result from the trace method */
+        int resTriangle = -1;
+        int resTriangle2 = -1;
+        sofa::defaulttype::Vector3 trianglePoints[4];
+        //bool found = false;
+        int nPoints = 0;
+        sofa::defaulttype::Vector3 normau[3];
+        /*if it fails to find a correspondence between the triangles it tries the raytracing procedure */
+        /*test if this triangle was tested before */
+
+        /*set the triangle as tested */
+        int flags = tri1.flags();
+
+        /*test only the points related to this triangle */
+        if (flags & TriangleCollisionModel<sofa::defaulttype::Vec3Types>::FLAG_P1)
+        {
+            normau[nPoints] = tm1->pNorms[tri1.p1Index ()];
+            trianglePoints[nPoints++] = tri1.p1 ();
+
+        }
+        if (flags & TriangleCollisionModel<sofa::defaulttype::Vec3Types>::FLAG_P2)
+        {
+            normau[nPoints] = tm1->pNorms[tri1.p2Index ()];
+            trianglePoints[nPoints++] = tri1.p2 ();
+        }
+        if (flags & TriangleCollisionModel<sofa::defaulttype::Vec3Types>::FLAG_P3)
+        {
+            normau[nPoints] = tm1->pNorms[tri1.p3Index ()];
+            trianglePoints[nPoints++] = tri1.p3 ();
+        }
+
+        for (int t = 0; t < nPoints; t++)
+        {
+
+            sofa::defaulttype::Vector3 point = trianglePoints[t];
+
+            if ((point[0] < (minVect2[0]))
+                || (point[0] > maxVect2[0] )
+                || (point[1] < minVect2[1] )
+                || (point[1] > maxVect2[1] )
+                || (point[2] < minVect2[2] )
+                || (point[2] > maxVect2[2] ))
+                continue;
+            /*res and res2 will store the point of intercection and the distance from the point */
+            TriangleOctree::traceResult res, res2;
+            /*search a triangle on t2 */
+            resTriangle =
+                    tm2->octreeRoot->
+                            trace (point /*+ normau[t] * (contactDistance / 2) */ ,
+                                   -normau[t], res);
+            if (resTriangle == -1)
+                continue;
+            Triangle triang2 (tm2, resTriangle);
+            cosAngle = dot (tri1.n (), triang2.n ());
+            if (cosAngle > 0)
+                continue;
+            /*search a triangle on t1, to be sure that the triangle found on t2 isn't outside the t1 object */
+
+            /*if there is no triangle in t1  that is crossed by the tri1 normal (resTriangle2==-1), it means that t1 is not an object with a closed volume, so we can't continue.
+              If the distance from the  point to the triangle on t1 is less than the distance to the triangle on t2 it means that the corresponding point is outside t1, and is not a good point */
+            resTriangle2 =
+                    tm1->octreeRoot->trace (point, -normau[t], res2);
+
+
+
+            if (resTriangle2 == -1 || res2.t < res.t)
+            {
+
+                continue;
+            }
+
+
+            Triangle tri3 (tm1, resTriangle2);
+            cosAngle2 = dot (tri1.n (), tri3.n ());
+            if (cosAngle2 > 0)
+                continue;
+
+            sofa::defaulttype::Vector3 Q =
+                    (triang2.p1 () * (1.0 - res.u - res.v)) +
+                    (triang2.p2 () * res.u) + (triang2.p3 () * res.v);
+
+            outputs->resize (outputs->size () + 1);
+            sofa::core::collision::DetectionOutput *detection = &*(outputs->end () - 1);
+
+
+            detection->elem =
+                    std::pair <
+                            core::CollisionElementIterator,
+                            core::CollisionElementIterator > (tri1, triang2);
+            detection->point[0] = point;
+
+            detection->point[1] = Q;
+
+            detection->normal = normau[t];
+
+            detection->value = -(res.t);
+
+            detection->id = tri1.getIndex()*3+t;
+            //found = true;
+
+        }
+
+    }
+
+}
+
+
+void RayTraceNarrowPhase::addCollisionPair (const std::pair <
+        core::CollisionModel *,
+        core::CollisionModel * >&cmPair)
+{
+    CubeCollisionModel *cm1 = dynamic_cast < CubeCollisionModel * >(cmPair.first);
+    CubeCollisionModel *cm2 = dynamic_cast < CubeCollisionModel * >(cmPair.second);
+    if (cm1 && cm2)
+    {
+        findPairsVolume (cm1, cm2);
+        findPairsVolume (cm2, cm1);
+    }
+}
+
+
+void RayTraceNarrowPhase::draw (const core::visual::VisualParams* vparams)
+{
+    if (!bDraw.getValue ())
+        return;
+
+    vparams->drawTool()->saveLastState();
+    vparams->drawTool()->disableLighting();
+
+    sofa::helper::types::RGBAColor color(1.0, 0.0, 1.0, 1.0);
+    vparams->drawTool()->setPolygonMode(0, true);
+    std::vector<sofa::defaulttype::Vector3> vertices;
+
+    const DetectionOutputMap& outputsMap = this->getDetectionOutputs();
+
+    for (DetectionOutputMap::const_iterator it = outputsMap.begin ();
+         it != outputsMap.end (); ++it)
+    {
+        TDetectionOutputVector < TriangleOctreeModel,
+        TriangleOctreeModel > *outputs =
+                static_cast <
+                        sofa::core::collision::TDetectionOutputVector <
+                                TriangleOctreeModel, TriangleOctreeModel > *>(it->second);
+        for (TDetectionOutputVector < TriangleOctreeModel,
+                TriangleOctreeModel >::iterator it2 = (outputs)->begin ();
+             it2 != outputs->end (); ++it2)
+        {
+            vertices.push_back(sofa::defaulttype::Vector3(it2->point[0][0], it2->point[0][1],it2->point[0][2]));
+            vertices.push_back(sofa::defaulttype::Vector3(it2->point[1][0], it2->point[1][1],it2->point[1][2]));
+
+            msg_error() << it2->point[0] << " " << it2->point[0];
+
+            it2->elem.first.draw(vparams);
+            it2->elem.second.draw(vparams);
+        }
+    }
+    vparams->drawTool()->drawLines(vertices,3,color);
+    vparams->drawTool()->restoreLastState();
+}
+
+}

modules/SofaGeneralMeshCollision/src/SofaGeneralMeshCollision/RayTraceNarrowPhase.h

@@ -0,0 +1,64 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program 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 program 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 program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#pragma once
+
+#include <SofaGeneralMeshCollision/config.h>
+
+#include <sofa/core/collision/NarrowPhaseDetection.h>
+
+namespace sofa::component::collision
+{
+
+class CubeCollisionModel;
+
+/**
+ *  \brief It is a Ray Trace based collision detection algorithm
+ *
+ *   For each point in one object, we trace a ray following the oposite of the point's normal
+ *   up to find a triangle in the other object. Both triangles are tested to evaluate if they are in
+ *   colliding state. It must be used with a TriangleOctreeModel,as an octree is used to traverse the object.
+ */
+class SOFA_SOFAGENERALMESHCOLLISION_API RayTraceNarrowPhase : public core::collision::NarrowPhaseDetection
+{
+public:
+    SOFA_CLASS(RayTraceNarrowPhase, core::collision::NarrowPhaseDetection);
+
+private:
+    Data < bool > bDraw;
+
+protected:
+    RayTraceNarrowPhase();
+
+public:
+    void addCollisionPair (const std::pair < core::CollisionModel *,
+            core::CollisionModel * >&cmPair) override;
+
+    void findPairsVolume (CubeCollisionModel * cm1, CubeCollisionModel * cm2);
+
+    void draw (const core::visual::VisualParams* vparams) override;
+    void setDraw (bool val)
+    {
+        bDraw.setValue (val);
+    }
+};
+
+}