Extras/HACD/hacdICHull.cpp

   1 /* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)\r
   2  All rights reserved.\r
   3  \r
   4  \r
   5  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:\r
   6  \r
   7  1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.\r
   8  \r
   9  2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.\r
  10  \r
  11  3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission.\r
  12  \r
  13  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
  14  */\r
  15 #include "hacdICHull.h"\r
  16 #include <limits>\r
  17 namespace HACD\r
  18 {   \r
  19         const long ICHull::sc_dummyIndex = std::numeric_limits<long>::max();\r
  20         ICHull::ICHull(void)\r
  21     {\r
  22                 m_distPoints = 0;\r
  23                 m_isFlat = false;\r
  24                 m_dummyVertex = 0;\r
  25     }\r
  26         bool ICHull::AddPoints(const Vec3<Real> * points, size_t nPoints)\r
  27         {\r
  28                 if (!points)\r
  29                 {\r
  30                         return false;\r
  31                 }\r
  32                 CircularListElement<TMMVertex> * vertex = NULL;\r
  33                 for (size_t i = 0; i < nPoints; i++)\r
  34                 {\r
  35                         vertex = m_mesh.AddVertex();\r
  36                         vertex->GetData().m_pos.X() = points[i].X();\r
  37                         vertex->GetData().m_pos.Y() = points[i].Y();\r
  38                         vertex->GetData().m_pos.Z() = points[i].Z();\r
  39             vertex->GetData().m_name = static_cast<long>(i);\r
  40                 }     \r
  41                 return true;\r
  42         }\r
  43         bool ICHull::AddPoints(std::vector< Vec3<Real> > points)\r
  44         {\r
  45                 CircularListElement<TMMVertex> * vertex = NULL;\r
  46                 for (size_t i = 0; i < points.size(); i++)\r
  47                 {\r
  48                         vertex = m_mesh.AddVertex();\r
  49                         vertex->GetData().m_pos.X() = points[i].X();\r
  50                         vertex->GetData().m_pos.Y() = points[i].Y();\r
  51                         vertex->GetData().m_pos.Z() = points[i].Z();\r
  52                 }     \r
  53                 return true;\r
  54         }\r
  55 \r
  56     bool ICHull::AddPoint(const Vec3<Real> & point, long id)\r
  57         {\r
  58                 if (AddPoints(&point, 1))\r
  59                 {\r
  60                         m_mesh.m_vertices.GetData().m_name = id;\r
  61                         return true;\r
  62                 }\r
  63                 return false;\r
  64         }\r
  65 \r
  66         ICHullError ICHull::Process()\r
  67         {\r
  68         unsigned long addedPoints = 0;  \r
  69                 if (m_mesh.GetNVertices() < 3)\r
  70                 {\r
  71                         return ICHullErrorNotEnoughPoints;\r
  72                 }\r
  73                 if (m_mesh.GetNVertices() == 3)\r
  74                 {\r
  75                         m_isFlat = true;\r
  76                         CircularListElement<TMMTriangle> * t1 = m_mesh.AddTriangle();\r
  77                         CircularListElement<TMMTriangle> * t2 = m_mesh.AddTriangle();\r
  78                         CircularListElement<TMMVertex> * v0 = m_mesh.m_vertices.GetHead();\r
  79                 CircularListElement<TMMVertex> * v1 = v0->GetNext();\r
  80                 CircularListElement<TMMVertex> * v2 = v1->GetNext();\r
  81                         // Compute the normal to the plane\r
  82             Vec3<Real> p0 = v0->GetData().m_pos;\r
  83             Vec3<Real> p1 = v1->GetData().m_pos;            \r
  84             Vec3<Real> p2 = v2->GetData().m_pos;                        \r
  85             m_normal = (p1-p0) ^ (p2-p0);\r
  86             m_normal.Normalize();\r
  87                         t1->GetData().m_vertices[0] = v0;\r
  88                         t1->GetData().m_vertices[1] = v1;\r
  89                         t1->GetData().m_vertices[2] = v2;\r
  90                         t2->GetData().m_vertices[0] = v1;\r
  91                         t2->GetData().m_vertices[1] = v2;\r
  92                         t2->GetData().m_vertices[2] = v2;\r
  93                         return ICHullErrorOK;\r
  94                 }\r
  95                 if (m_isFlat)\r
  96                 {\r
  97             m_mesh.m_edges.Clear();\r
  98                         m_mesh.m_triangles.Clear();\r
  99                         m_isFlat = false;\r
 100                 }\r
 101         if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron\r
 102         {\r
 103                         ICHullError res = DoubleTriangle();\r
 104             if (res != ICHullErrorOK)\r
 105             {\r
 106                 return res;\r
 107             }\r
 108             else\r
 109             {\r
 110                 addedPoints += 3;\r
 111             }\r
 112         }\r
 113         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 114         // go to the first added and not processed vertex\r
 115         while (!(vertices.GetHead()->GetPrev()->GetData().m_tag))\r
 116         {\r
 117             vertices.Prev();\r
 118         }\r
 119         while (!vertices.GetData().m_tag) // not processed\r
 120         {\r
 121             vertices.GetData().m_tag = true;\r
 122             if (ProcessPoint())\r
 123             {\r
 124                 addedPoints++;\r
 125                 CleanUp(addedPoints);\r
 126                     vertices.Next();\r
 127                                 if (!GetMesh().CheckConsistancy())\r
 128                                 {\r
 129                                         return ICHullErrorInconsistent;\r
 130                                 }\r
 131             }\r
 132         }\r
 133                 if (m_isFlat)\r
 134                 {\r
 135                         std::vector< CircularListElement<TMMTriangle> * > trianglesToDuplicate;\r
 136                         size_t nT = m_mesh.GetNTriangles();\r
 137                         for(size_t f = 0; f < nT; f++)\r
 138                         {\r
 139                                 TMMTriangle & currentTriangle = m_mesh.m_triangles.GetHead()->GetData();\r
 140                                 if( currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex ||\r
 141                    currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex ||\r
 142                    currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex )\r
 143                                 {\r
 144                                         m_trianglesToDelete.push_back(m_mesh.m_triangles.GetHead());\r
 145                     for(int k = 0; k < 3; k++)\r
 146                     {\r
 147                         for(int h = 0; h < 2; h++)\r
 148                         {\r
 149                             if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead())\r
 150                             {\r
 151                                 currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0;\r
 152                                 break;\r
 153                             }\r
 154                         }\r
 155                     }\r
 156                                 }\r
 157                                 else\r
 158                                 {\r
 159                                         trianglesToDuplicate.push_back(m_mesh.m_triangles.GetHead());\r
 160                                 }\r
 161                                 m_mesh.m_triangles.Next();\r
 162                         }\r
 163             size_t nE = m_mesh.GetNEdges();\r
 164             for(size_t e = 0; e < nE; e++)\r
 165                         {\r
 166                                 TMMEdge & currentEdge = m_mesh.m_edges.GetHead()->GetData();\r
 167                                 if( currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) \r
 168                                 {\r
 169                                         m_edgesToDelete.push_back(m_mesh.m_edges.GetHead());\r
 170                                 }\r
 171                                 m_mesh.m_edges.Next();\r
 172                         }\r
 173                         m_mesh.m_vertices.Delete(m_dummyVertex);\r
 174                         m_dummyVertex = 0;\r
 175             size_t nV = m_mesh.GetNVertices();\r
 176             CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 177                         for(size_t v = 0; v < nV; ++v)\r
 178                         {\r
 179                 vertices.GetData().m_tag = false;\r
 180                 vertices.Next();\r
 181             }\r
 182             CleanEdges();\r
 183                         CleanTriangles();\r
 184                         CircularListElement<TMMTriangle> * newTriangle;\r
 185                         for(size_t t = 0; t < trianglesToDuplicate.size(); t++)\r
 186                         {\r
 187                                 newTriangle = m_mesh.AddTriangle();\r
 188                                 newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1];\r
 189                                 newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0];\r
 190                                 newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2];\r
 191                         }\r
 192                 }\r
 193                 return ICHullErrorOK;\r
 194         }\r
 195     ICHullError ICHull::Process(unsigned long nPointsCH)\r
 196         {\r
 197         unsigned long addedPoints = 0;  \r
 198         if (nPointsCH < 3 || m_mesh.GetNVertices() < 3)\r
 199                 {\r
 200                         return ICHullErrorNotEnoughPoints;\r
 201                 }\r
 202                 if (m_mesh.GetNVertices() == 3)\r
 203                 {\r
 204                         m_isFlat = true;\r
 205                         CircularListElement<TMMTriangle> * t1 = m_mesh.AddTriangle();\r
 206                         CircularListElement<TMMTriangle> * t2 = m_mesh.AddTriangle();\r
 207                         CircularListElement<TMMVertex> * v0 = m_mesh.m_vertices.GetHead();\r
 208                 CircularListElement<TMMVertex> * v1 = v0->GetNext();\r
 209                 CircularListElement<TMMVertex> * v2 = v1->GetNext();\r
 210                         // Compute the normal to the plane\r
 211             Vec3<Real> p0 = v0->GetData().m_pos;\r
 212             Vec3<Real> p1 = v1->GetData().m_pos;            \r
 213             Vec3<Real> p2 = v2->GetData().m_pos;                        \r
 214             m_normal = (p1-p0) ^ (p2-p0);\r
 215             m_normal.Normalize();\r
 216                         t1->GetData().m_vertices[0] = v0;\r
 217                         t1->GetData().m_vertices[1] = v1;\r
 218                         t1->GetData().m_vertices[2] = v2;\r
 219                         t2->GetData().m_vertices[0] = v1;\r
 220                         t2->GetData().m_vertices[1] = v0;\r
 221                         t2->GetData().m_vertices[2] = v2;\r
 222                         return ICHullErrorOK;\r
 223                 }\r
 224 \r
 225                 if (m_isFlat)\r
 226                 {\r
 227                         m_mesh.m_triangles.Clear();\r
 228             m_mesh.m_edges.Clear();\r
 229             m_isFlat = false;\r
 230                 }\r
 231         \r
 232         if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron\r
 233         {\r
 234                         ICHullError res = DoubleTriangle();\r
 235             if (res != ICHullErrorOK)\r
 236             {\r
 237                 return res;\r
 238             }\r
 239             else\r
 240             {\r
 241                 addedPoints += 3;\r
 242             }\r
 243         }\r
 244         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 245         while (!vertices.GetData().m_tag && addedPoints < nPointsCH) // not processed\r
 246         {\r
 247             if (!FindMaxVolumePoint())\r
 248             {\r
 249                 break;\r
 250             }                  \r
 251             vertices.GetData().m_tag = true;                      \r
 252             if (ProcessPoint())\r
 253             {\r
 254                 addedPoints++;\r
 255                 CleanUp(addedPoints);\r
 256                                 if (!GetMesh().CheckConsistancy())\r
 257                                 {\r
 258                                         return ICHullErrorInconsistent;\r
 259                                 }\r
 260                 vertices.Next();\r
 261             }\r
 262         }\r
 263         // delete remaining points\r
 264         while (!vertices.GetData().m_tag)\r
 265         {\r
 266             vertices.Delete();\r
 267         }\r
 268                 if (m_isFlat)\r
 269                 {\r
 270                         std::vector< CircularListElement<TMMTriangle> * > trianglesToDuplicate;\r
 271                         size_t nT = m_mesh.GetNTriangles();\r
 272                         for(size_t f = 0; f < nT; f++)\r
 273                         {\r
 274                                 TMMTriangle & currentTriangle = m_mesh.m_triangles.GetHead()->GetData();\r
 275                                 if( currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex ||\r
 276                    currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex ||\r
 277                    currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex )\r
 278                                 {\r
 279                                         m_trianglesToDelete.push_back(m_mesh.m_triangles.GetHead());\r
 280                     for(int k = 0; k < 3; k++)\r
 281                     {\r
 282                         for(int h = 0; h < 2; h++)\r
 283                         {\r
 284                             if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead())\r
 285                             {\r
 286                                 currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0;\r
 287                                 break;\r
 288                             }\r
 289                         }\r
 290                     }\r
 291                                 }\r
 292                                 else\r
 293                                 {\r
 294                                         trianglesToDuplicate.push_back(m_mesh.m_triangles.GetHead());\r
 295                                 }\r
 296                                 m_mesh.m_triangles.Next();\r
 297                         }\r
 298             size_t nE = m_mesh.GetNEdges();\r
 299             for(size_t e = 0; e < nE; e++)\r
 300                         {\r
 301                                 TMMEdge & currentEdge = m_mesh.m_edges.GetHead()->GetData();\r
 302                                 if( currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) \r
 303                                 {\r
 304                                         m_edgesToDelete.push_back(m_mesh.m_edges.GetHead());\r
 305                                 }\r
 306                                 m_mesh.m_edges.Next();\r
 307                         }\r
 308                         m_mesh.m_vertices.Delete(m_dummyVertex);\r
 309                         m_dummyVertex = 0;\r
 310             size_t nV = m_mesh.GetNVertices();\r
 311             CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 312                         for(size_t v = 0; v < nV; ++v)\r
 313                         {\r
 314                 vertices.GetData().m_tag = false;\r
 315                 vertices.Next();\r
 316             }\r
 317             CleanEdges();\r
 318                         CleanTriangles();\r
 319                         CircularListElement<TMMTriangle> * newTriangle;\r
 320                         for(size_t t = 0; t < trianglesToDuplicate.size(); t++)\r
 321                         {\r
 322                                 newTriangle = m_mesh.AddTriangle();\r
 323                                 newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1];\r
 324                                 newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0];\r
 325                                 newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2];\r
 326                         }\r
 327                 }\r
 328                 return ICHullErrorOK;\r
 329         }\r
 330     bool ICHull::FindMaxVolumePoint()\r
 331         {\r
 332         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 333         CircularListElement<TMMVertex> * vMaxVolume = 0;\r
 334         CircularListElement<TMMVertex> * vHeadPrev = vertices.GetHead()->GetPrev();\r
 335         \r
 336         double maxVolume = 0.0;\r
 337         double volume = 0.0;\r
 338         \r
 339         while (!vertices.GetData().m_tag) // not processed\r
 340         {\r
 341             if (ComputePointVolume(volume, false))\r
 342             {\r
 343                 if ( maxVolume < volume)\r
 344                 {\r
 345                     maxVolume = volume;\r
 346                     vMaxVolume = vertices.GetHead();\r
 347                 }\r
 348                 vertices.Next();\r
 349             }\r
 350         }\r
 351         CircularListElement<TMMVertex> * vHead = vHeadPrev->GetNext();\r
 352         vertices.GetHead() = vHead;  \r
 353         \r
 354         if (!vMaxVolume)\r
 355         {\r
 356             return false;\r
 357         }\r
 358         \r
 359         if (vMaxVolume != vHead)\r
 360         {\r
 361             Vec3<Real> pos = vHead->GetData().m_pos;\r
 362             long id = vHead->GetData().m_name;\r
 363             vHead->GetData().m_pos = vMaxVolume->GetData().m_pos;\r
 364             vHead->GetData().m_name = vMaxVolume->GetData().m_name;\r
 365             vMaxVolume->GetData().m_pos = pos;\r
 366             vHead->GetData().m_name = id;\r
 367         }\r
 368         \r
 369  \r
 370         return true;\r
 371     }\r
 372         ICHullError ICHull::DoubleTriangle()\r
 373         {\r
 374         // find three non colinear points\r
 375                 m_isFlat = false;\r
 376         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 377         CircularListElement<TMMVertex> * v0 = vertices.GetHead();\r
 378         while( Colinear(v0->GetData().m_pos, \r
 379                         v0->GetNext()->GetData().m_pos, \r
 380                         v0->GetNext()->GetNext()->GetData().m_pos))\r
 381         {\r
 382             if ( (v0 = v0->GetNext()) == vertices.GetHead())\r
 383             {\r
 384                 return ICHullErrorCoplanarPoints;\r
 385             }\r
 386         }\r
 387         CircularListElement<TMMVertex> * v1 = v0->GetNext();\r
 388         CircularListElement<TMMVertex> * v2 = v1->GetNext();\r
 389         // mark points as processed\r
 390         v0->GetData().m_tag = v1->GetData().m_tag = v2->GetData().m_tag = true;\r
 391         \r
 392         // create two triangles\r
 393         CircularListElement<TMMTriangle> * f0 = MakeFace(v0, v1, v2, 0);\r
 394         MakeFace(v2, v1, v0, f0);\r
 395 \r
 396         // find a fourth non-coplanar point to form tetrahedron\r
 397         CircularListElement<TMMVertex> * v3 = v2->GetNext();\r
 398         vertices.GetHead() = v3;\r
 399 \r
 400                 double vol = Volume(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos);\r
 401                 while (vol == 0.0 && !v3->GetNext()->GetData().m_tag)\r
 402                 {\r
 403                         v3 = v3->GetNext();\r
 404                         vol = Volume(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos);\r
 405                 }                       \r
 406                 if (vol == 0.0)\r
 407                 {\r
 408                         // compute the barycenter\r
 409                         Vec3<Real> bary(0.0,0.0,0.0);\r
 410                         CircularListElement<TMMVertex> * vBary = v0;\r
 411                         do\r
 412                         {\r
 413                                 bary += vBary->GetData().m_pos;\r
 414                         }\r
 415                         while ( (vBary = vBary->GetNext()) != v0);\r
 416                         bary /= static_cast<Real>(vertices.GetSize());\r
 417 \r
 418                         // Compute the normal to the plane\r
 419             Vec3<Real> p0 = v0->GetData().m_pos;\r
 420             Vec3<Real> p1 = v1->GetData().m_pos;            \r
 421             Vec3<Real> p2 = v2->GetData().m_pos;                        \r
 422             m_normal = (p1-p0) ^ (p2-p0);\r
 423             m_normal.Normalize();\r
 424                         // add dummy vertex placed at (bary + normal)\r
 425                         vertices.GetHead() = v2;\r
 426                         Vec3<Real> newPt = bary + m_normal;\r
 427                         AddPoint(newPt, sc_dummyIndex); \r
 428                         m_dummyVertex = vertices.GetHead();\r
 429                         m_isFlat = true;\r
 430             v3 = v2->GetNext();\r
 431             vol = Volume(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos);\r
 432                         return ICHullErrorOK;\r
 433                 }\r
 434                 else if (v3 != vertices.GetHead())\r
 435                 {\r
 436                         TMMVertex temp;\r
 437                         temp.m_name = v3->GetData().m_name;\r
 438                         temp.m_pos = v3->GetData().m_pos;\r
 439                         v3->GetData().m_name = vertices.GetHead()->GetData().m_name;\r
 440                         v3->GetData().m_pos = vertices.GetHead()->GetData().m_pos;\r
 441                         vertices.GetHead()->GetData().m_name = temp.m_name;\r
 442                         vertices.GetHead()->GetData().m_pos = temp.m_pos;\r
 443                 }\r
 444                 return ICHullErrorOK;\r
 445         }\r
 446     CircularListElement<TMMTriangle> *  ICHull::MakeFace(CircularListElement<TMMVertex> * v0,  \r
 447                                                          CircularListElement<TMMVertex> * v1,\r
 448                                                          CircularListElement<TMMVertex> * v2,\r
 449                                                          CircularListElement<TMMTriangle> * fold)\r
 450         {        \r
 451         CircularListElement<TMMEdge> * e0;\r
 452         CircularListElement<TMMEdge> * e1;\r
 453         CircularListElement<TMMEdge> * e2;\r
 454         long index = 0;\r
 455         if (!fold) // if first face to be created\r
 456         {\r
 457             e0 = m_mesh.AddEdge(); // create the three edges\r
 458             e1 = m_mesh.AddEdge();\r
 459             e2 = m_mesh.AddEdge();            \r
 460         }\r
 461         else // otherwise re-use existing edges (in reverse order)\r
 462         {\r
 463             e0 = fold->GetData().m_edges[2];\r
 464             e1 = fold->GetData().m_edges[1];\r
 465             e2 = fold->GetData().m_edges[0];\r
 466             index = 1;\r
 467         }\r
 468         e0->GetData().m_vertices[0] = v0; e0->GetData().m_vertices[1] = v1;\r
 469         e1->GetData().m_vertices[0] = v1; e1->GetData().m_vertices[1] = v2;\r
 470         e2->GetData().m_vertices[0] = v2; e2->GetData().m_vertices[1] = v0;\r
 471         // create the new face\r
 472         CircularListElement<TMMTriangle> * f = m_mesh.AddTriangle();   \r
 473         f->GetData().m_edges[0]    = e0; f->GetData().m_edges[1]    = e1; f->GetData().m_edges[2]    = e2;\r
 474         f->GetData().m_vertices[0] = v0; f->GetData().m_vertices[1] = v1; f->GetData().m_vertices[2] = v2;     \r
 475         // link edges to face f\r
 476         e0->GetData().m_triangles[index] = e1->GetData().m_triangles[index] = e2->GetData().m_triangles[index] = f;\r
 477                 return f;\r
 478         }\r
 479         CircularListElement<TMMTriangle> * ICHull::MakeConeFace(CircularListElement<TMMEdge> * e, CircularListElement<TMMVertex> * p)\r
 480         {\r
 481         // create two new edges if they don't already exist\r
 482         CircularListElement<TMMEdge> * newEdges[2];\r
 483         for(int i = 0; i < 2; ++i)\r
 484         {\r
 485             if ( !( newEdges[i] = e->GetData().m_vertices[i]->GetData().m_duplicate ) )  \r
 486             { // if the edge doesn't exits add it and mark the vertex as duplicated\r
 487                 newEdges[i] = m_mesh.AddEdge();\r
 488                 newEdges[i]->GetData().m_vertices[0] = e->GetData().m_vertices[i];\r
 489                 newEdges[i]->GetData().m_vertices[1] = p;\r
 490                 e->GetData().m_vertices[i]->GetData().m_duplicate = newEdges[i];\r
 491             }\r
 492         }\r
 493         // make the new face\r
 494         CircularListElement<TMMTriangle> * newFace = m_mesh.AddTriangle();\r
 495         newFace->GetData().m_edges[0] = e;\r
 496         newFace->GetData().m_edges[1] = newEdges[0];\r
 497         newFace->GetData().m_edges[2] = newEdges[1];\r
 498         MakeCCW(newFace, e, p);\r
 499         for(int i=0; i < 2; ++i)\r
 500         {\r
 501             for(int j=0; j < 2; ++j)\r
 502             {\r
 503                 if ( ! newEdges[i]->GetData().m_triangles[j] )\r
 504                 {\r
 505                     newEdges[i]->GetData().m_triangles[j] = newFace;\r
 506                     break;\r
 507                 }\r
 508             }\r
 509         }\r
 510                 return newFace;\r
 511         }\r
 512     bool ICHull::ComputePointVolume(double &totalVolume, bool markVisibleFaces)\r
 513     {\r
 514         // mark visible faces\r
 515         CircularListElement<TMMTriangle> * fHead = m_mesh.GetTriangles().GetHead();\r
 516         CircularListElement<TMMTriangle> * f = fHead;\r
 517         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 518         CircularListElement<TMMVertex> * vertex0 = vertices.GetHead();\r
 519         bool visible = false;\r
 520         Vec3<double> pos0 = Vec3<double>(vertex0->GetData().m_pos.X(),\r
 521                                                                                  vertex0->GetData().m_pos.Y(),\r
 522                                                                                  vertex0->GetData().m_pos.Z());\r
 523         double vol = 0.0;\r
 524         totalVolume = 0.0;\r
 525                 Vec3<double> ver0, ver1, ver2;\r
 526         do \r
 527         {\r
 528                         ver0.X() = f->GetData().m_vertices[0]->GetData().m_pos.X();\r
 529                         ver0.Y() = f->GetData().m_vertices[0]->GetData().m_pos.Y();\r
 530                         ver0.Z() = f->GetData().m_vertices[0]->GetData().m_pos.Z();\r
 531                         ver1.X() = f->GetData().m_vertices[1]->GetData().m_pos.X();\r
 532                         ver1.Y() = f->GetData().m_vertices[1]->GetData().m_pos.Y();\r
 533                         ver1.Z() = f->GetData().m_vertices[1]->GetData().m_pos.Z();\r
 534                         ver2.X() = f->GetData().m_vertices[2]->GetData().m_pos.X();\r
 535                         ver2.Y() = f->GetData().m_vertices[2]->GetData().m_pos.Y();\r
 536                         ver2.Z() = f->GetData().m_vertices[2]->GetData().m_pos.Z();\r
 537                         vol = Volume(ver0, ver1, ver2, pos0);\r
 538                         if ( vol < 0.0 )\r
 539                         {\r
 540                                 vol = fabs(vol);\r
 541                                 totalVolume += vol;\r
 542                                 if (markVisibleFaces)\r
 543                                 {\r
 544                                         f->GetData().m_visible = true;\r
 545                                         m_trianglesToDelete.push_back(f);\r
 546                                 }\r
 547                                 visible = true;\r
 548                         }\r
 549                         f = f->GetNext();\r
 550         } \r
 551         while (f != fHead);\r
 552 \r
 553                 if (m_trianglesToDelete.size() == m_mesh.m_triangles.GetSize())\r
 554                 {\r
 555                         for(size_t i = 0; i < m_trianglesToDelete.size(); i++)\r
 556                         {\r
 557                                 m_trianglesToDelete[i]->GetData().m_visible = false;\r
 558                         }\r
 559                         visible = false;\r
 560                 }\r
 561         // if no faces visible from p then p is inside the hull\r
 562         if (!visible && markVisibleFaces)\r
 563         {\r
 564             vertices.Delete();\r
 565                         m_trianglesToDelete.clear();\r
 566             return false;\r
 567         }\r
 568         return true;\r
 569     }\r
 570         bool ICHull::ProcessPoint()\r
 571         {\r
 572         double totalVolume = 0.0;\r
 573         if (!ComputePointVolume(totalVolume, true))\r
 574         {\r
 575             return false;\r
 576         }\r
 577         // Mark edges in interior of visible region for deletion.\r
 578         // Create a new face based on each border edge\r
 579         CircularListElement<TMMVertex> * v0 = m_mesh.GetVertices().GetHead();\r
 580         CircularListElement<TMMEdge> * eHead = m_mesh.GetEdges().GetHead();\r
 581         CircularListElement<TMMEdge> * e = eHead;    \r
 582         CircularListElement<TMMEdge> * tmp = 0;\r
 583         long nvisible = 0;\r
 584         m_edgesToDelete.clear();\r
 585         m_edgesToUpdate.clear();\r
 586         do \r
 587         {\r
 588             tmp = e->GetNext();\r
 589             nvisible = 0;\r
 590             for(int k = 0; k < 2; k++)\r
 591             {\r
 592                 if ( e->GetData().m_triangles[k]->GetData().m_visible )\r
 593                 {\r
 594                     nvisible++;\r
 595                 }\r
 596             }\r
 597             if ( nvisible == 2)\r
 598             {\r
 599                 m_edgesToDelete.push_back(e);\r
 600             }\r
 601             else if ( nvisible == 1)\r
 602             {\r
 603                 e->GetData().m_newFace = MakeConeFace(e, v0);\r
 604                 m_edgesToUpdate.push_back(e);\r
 605             }\r
 606             e = tmp;\r
 607         }\r
 608         while (e != eHead);        \r
 609                 return true;\r
 610         }\r
 611     bool ICHull::MakeCCW(CircularListElement<TMMTriangle> * f,\r
 612                          CircularListElement<TMMEdge> * e, \r
 613                          CircularListElement<TMMVertex> * v)\r
 614     {\r
 615         // the visible face adjacent to e\r
 616         CircularListElement<TMMTriangle> * fv; \r
 617         if (e->GetData().m_triangles[0]->GetData().m_visible)\r
 618         {\r
 619             fv = e->GetData().m_triangles[0];\r
 620         }\r
 621         else\r
 622         {\r
 623             fv = e->GetData().m_triangles[1];\r
 624         }\r
 625         \r
 626         //  set vertex[0] and vertex[1] to have the same orientation as the corresponding vertices of fv.\r
 627         long i;                                 // index of e->m_vertices[0] in fv\r
 628         CircularListElement<TMMVertex> * v0 = e->GetData().m_vertices[0];\r
 629         CircularListElement<TMMVertex> * v1 = e->GetData().m_vertices[1];\r
 630         for(i = 0; fv->GetData().m_vertices[i] !=  v0; i++);\r
 631         \r
 632         if ( fv->GetData().m_vertices[(i+1) % 3] != e->GetData().m_vertices[1] )\r
 633         {\r
 634             f->GetData().m_vertices[0] = v1;\r
 635             f->GetData().m_vertices[1] = v0;\r
 636         }\r
 637         else\r
 638         {\r
 639             f->GetData().m_vertices[0] = v0;\r
 640             f->GetData().m_vertices[1] = v1;  \r
 641             // swap edges\r
 642             CircularListElement<TMMEdge> * tmp = f->GetData().m_edges[0];\r
 643             f->GetData().m_edges[0] = f->GetData().m_edges[1];\r
 644             f->GetData().m_edges[1] = tmp;\r
 645         }\r
 646         f->GetData().m_vertices[2] = v;\r
 647         return true;\r
 648     }\r
 649     bool ICHull::CleanUp(unsigned long & addedPoints)\r
 650     {\r
 651         bool r0 = CleanEdges();\r
 652         bool r1 = CleanTriangles();\r
 653         bool r2 = CleanVertices(addedPoints);\r
 654         return  r0 && r1 && r2;\r
 655     }\r
 656     bool ICHull::CleanEdges()\r
 657     {\r
 658         // integrate the new faces into the data structure\r
 659         CircularListElement<TMMEdge> * e;\r
 660         const std::vector<CircularListElement<TMMEdge> *>::iterator itEndUpdate = m_edgesToUpdate.end();\r
 661         for(std::vector<CircularListElement<TMMEdge> *>::iterator it = m_edgesToUpdate.begin(); it != itEndUpdate; ++it)\r
 662         {\r
 663             e = *it;\r
 664             if ( e->GetData().m_newFace )\r
 665             {\r
 666                 if ( e->GetData().m_triangles[0]->GetData().m_visible)\r
 667                 {\r
 668                     e->GetData().m_triangles[0] = e->GetData().m_newFace;\r
 669                 }\r
 670                 else\r
 671                 {\r
 672                     e->GetData().m_triangles[1] = e->GetData().m_newFace;\r
 673                 }\r
 674                 e->GetData().m_newFace = 0;\r
 675             }           \r
 676         }\r
 677         // delete edges maked for deletion\r
 678         CircularList<TMMEdge> & edges = m_mesh.GetEdges();\r
 679         const std::vector<CircularListElement<TMMEdge> *>::iterator itEndDelete = m_edgesToDelete.end();\r
 680         for(std::vector<CircularListElement<TMMEdge> *>::iterator it = m_edgesToDelete.begin(); it != itEndDelete; ++it)\r
 681         {\r
 682             edges.Delete(*it);         \r
 683         }\r
 684                 m_edgesToDelete.clear();\r
 685         m_edgesToUpdate.clear();\r
 686         return true;\r
 687     }\r
 688     bool ICHull::CleanTriangles()\r
 689     {\r
 690         CircularList<TMMTriangle> & triangles = m_mesh.GetTriangles();\r
 691         const std::vector<CircularListElement<TMMTriangle> *>::iterator itEndDelete = m_trianglesToDelete.end();\r
 692         for(std::vector<CircularListElement<TMMTriangle> *>::iterator it = m_trianglesToDelete.begin(); it != itEndDelete; ++it)\r
 693         {\r
 694                         if (m_distPoints)\r
 695                         {\r
 696                                 if (m_isFlat)\r
 697                                 {\r
 698                                         // to be updated\r
 699                                 }\r
 700                                 else\r
 701                                 {\r
 702                                         std::set<long>::const_iterator itPEnd((*it)->GetData().m_incidentPoints.end());\r
 703                                         std::set<long>::const_iterator itP((*it)->GetData().m_incidentPoints.begin());\r
 704                                         std::map<long, DPoint>::iterator itPoint;\r
 705                                         for(; itP != itPEnd; ++itP) \r
 706                                         {\r
 707                                                 itPoint = m_distPoints->find(*itP);\r
 708                                                 if (itPoint != m_distPoints->end())\r
 709                                                 {\r
 710                                                         itPoint->second.m_computed = false;\r
 711                                                 }\r
 712                                         }\r
 713                                 }\r
 714                         }\r
 715             triangles.Delete(*it);         \r
 716         }\r
 717                 m_trianglesToDelete.clear();\r
 718         return true;\r
 719     }\r
 720     bool ICHull::CleanVertices(unsigned long & addedPoints)\r
 721     {\r
 722         // mark all vertices incident to some undeleted edge as on the hull\r
 723         CircularList<TMMEdge> & edges = m_mesh.GetEdges();\r
 724         CircularListElement<TMMEdge> * e = edges.GetHead();\r
 725         size_t nE = edges.GetSize();\r
 726         for(size_t i = 0; i < nE; i++)\r
 727         {\r
 728             e->GetData().m_vertices[0]->GetData().m_onHull = true;\r
 729             e->GetData().m_vertices[1]->GetData().m_onHull = true;\r
 730             e = e->GetNext();\r
 731         }\r
 732         // delete all the vertices that have been processed but are not on the hull\r
 733         CircularList<TMMVertex> & vertices = m_mesh.GetVertices();\r
 734         CircularListElement<TMMVertex> * vHead = vertices.GetHead();\r
 735         CircularListElement<TMMVertex> * v = vHead;\r
 736         v = v->GetPrev();\r
 737         do \r
 738         {\r
 739             if (v->GetData().m_tag && !v->GetData().m_onHull)\r
 740             {\r
 741                 CircularListElement<TMMVertex> * tmp = v->GetPrev();\r
 742                 vertices.Delete(v);\r
 743                 v = tmp;\r
 744                 addedPoints--;\r
 745             }\r
 746             else\r
 747             {\r
 748                 v->GetData().m_duplicate = 0;\r
 749                 v->GetData().m_onHull = false;\r
 750                 v = v->GetPrev();\r
 751             }\r
 752         } \r
 753         while (v->GetData().m_tag && v != vHead);\r
 754         return true;\r
 755     }\r
 756         void ICHull::Clear()\r
 757         {       \r
 758                 m_mesh.Clear();\r
 759                 m_edgesToDelete = std::vector<CircularListElement<TMMEdge> *>();\r
 760                 m_edgesToUpdate = std::vector<CircularListElement<TMMEdge> *>();\r
 761                 m_trianglesToDelete= std::vector<CircularListElement<TMMTriangle> *>();\r
 762                 m_isFlat = false;\r
 763         }\r
 764     const ICHull & ICHull::operator=(ICHull & rhs)\r
 765     {\r
 766         if (&rhs != this)\r
 767         {\r
 768             m_mesh.Copy(rhs.m_mesh);\r
 769             m_edgesToDelete = rhs.m_edgesToDelete;\r
 770             m_edgesToUpdate = rhs.m_edgesToUpdate;\r
 771             m_trianglesToDelete = rhs.m_trianglesToDelete;\r
 772                         m_isFlat = rhs.m_isFlat;\r
 773         }\r
 774         return (*this);\r
 775     }   \r
 776     double ICHull::ComputeVolume()\r
 777     {\r
 778         size_t nV = m_mesh.m_vertices.GetSize();\r
 779                 if (nV == 0 || m_isFlat)\r
 780                 {\r
 781                         return 0.0;\r
 782                 }       \r
 783         Vec3<double> bary(0.0, 0.0, 0.0);\r
 784         for(size_t v = 0; v < nV; v++)\r
 785         {\r
 786                         bary.X() +=  m_mesh.m_vertices.GetHead()->GetData().m_pos.X();\r
 787                         bary.Y() +=  m_mesh.m_vertices.GetHead()->GetData().m_pos.Y();\r
 788                         bary.Z() +=  m_mesh.m_vertices.GetHead()->GetData().m_pos.Z();\r
 789                         m_mesh.m_vertices.Next();\r
 790         }\r
 791         bary /= static_cast<double>(nV);\r
 792         \r
 793         size_t nT = m_mesh.m_triangles.GetSize();\r
 794         Vec3<double> ver0, ver1, ver2;\r
 795         double totalVolume = 0.0;\r
 796         for(size_t t = 0; t < nT; t++)\r
 797         {\r
 798             ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X();\r
 799             ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y();\r
 800             ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z();\r
 801             ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X();\r
 802             ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y();\r
 803             ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z();\r
 804             ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X();\r
 805             ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y();\r
 806             ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z();\r
 807                         totalVolume += Volume(ver0, ver1, ver2, bary);\r
 808                         m_mesh.m_triangles.Next();\r
 809         }\r
 810         return totalVolume;\r
 811     }\r
 812     bool ICHull::IsInside(const Vec3<Real> & pt0)\r
 813     {\r
 814                 const Vec3<double> pt(pt0.X(), pt0.Y(), pt0.Z());\r
 815                 if (m_isFlat)\r
 816                 {\r
 817                         size_t nT = m_mesh.m_triangles.GetSize();\r
 818                         Vec3<double> ver0, ver1, ver2, a, b, c;\r
 819                         double u,v;\r
 820                         for(size_t t = 0; t < nT; t++)\r
 821                         {\r
 822                                 ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X();\r
 823                                 ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y();\r
 824                                 ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z();\r
 825                                 ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X();\r
 826                                 ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y();\r
 827                                 ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z();\r
 828                                 ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X();\r
 829                                 ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y();\r
 830                                 ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z();\r
 831                                 a = ver1 - ver0;\r
 832                                 b = ver2 - ver0;\r
 833                                 c = pt - ver0;\r
 834                                 u = c * a;\r
 835                                 v = c * b;\r
 836                                 if ( u >= 0.0 && u <= 1.0 && v >= 0.0 && u+v <= 1.0)\r
 837                                 {\r
 838                                         return true;\r
 839                                 }\r
 840                                 m_mesh.m_triangles.Next();\r
 841                         }\r
 842                         return false;\r
 843                 }\r
 844                 else\r
 845                 {\r
 846                         size_t nT = m_mesh.m_triangles.GetSize();\r
 847                         Vec3<double> ver0, ver1, ver2;\r
 848                         for(size_t t = 0; t < nT; t++)\r
 849                         {\r
 850                                 ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X();\r
 851                                 ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y();\r
 852                                 ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z();\r
 853                                 ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X();\r
 854                                 ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y();\r
 855                                 ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z();\r
 856                                 ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X();\r
 857                                 ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y();\r
 858                                 ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z();\r
 859                                 if (Volume(ver0, ver1, ver2, pt) < 0.0)\r
 860                                 {\r
 861                                         return false;\r
 862                                 }\r
 863                                 m_mesh.m_triangles.Next();\r
 864                         }\r
 865                         return true;\r
 866                 }\r
 867     }\r
 868         double ICHull::ComputeDistance(long name, const Vec3<Real> & pt, const Vec3<Real> & normal, bool & insideHull, bool updateIncidentPoints)\r
 869         {\r
 870                 Vec3<double> ptNormal(static_cast<double>(normal.X()), \r
 871                                                           static_cast<double>(normal.Y()), \r
 872                                                           static_cast<double>(normal.Z()));\r
 873                 Vec3<double> p0( static_cast<double>(pt.X()), \r
 874                                                  static_cast<double>(pt.Y()), \r
 875                                                  static_cast<double>(pt.Z()));\r
 876 \r
 877                 if (m_isFlat)\r
 878                 {\r
 879                         double distance = 0.0;\r
 880                         Vec3<double> chNormal(static_cast<double>(m_normal.X()), \r
 881                                                                   static_cast<double>(m_normal.Y()), \r
 882                                                               static_cast<double>(m_normal.Z()));\r
 883                         ptNormal -= (ptNormal * chNormal) * chNormal;\r
 884                         if (ptNormal.GetNorm() > 0.0)\r
 885                         {\r
 886                                 ptNormal.Normalize();\r
 887                                 long nameVE1;\r
 888                                 long nameVE2;\r
 889                                 Vec3<double> pa, pb, d0, d1, d2, d3;                            \r
 890                                 Vec3<double> p1 = p0 + ptNormal;\r
 891                                 Vec3<double> p2, p3;\r
 892                                 double mua, mub, s;\r
 893                                 const double EPS = 0.00000000001;\r
 894                                 size_t nE = m_mesh.GetNEdges();\r
 895                                 for(size_t e = 0; e < nE; e++)\r
 896                                 {\r
 897                                         TMMEdge & currentEdge = m_mesh.m_edges.GetHead()->GetData();\r
 898                     nameVE1 = currentEdge.m_vertices[0]->GetData().m_name;\r
 899                     nameVE2 = currentEdge.m_vertices[1]->GetData().m_name;\r
 900                     if (currentEdge.m_triangles[0] == 0 || currentEdge.m_triangles[1] == 0)\r
 901                     {\r
 902                         if ( nameVE1==name || nameVE2==name )\r
 903                         {\r
 904                             return 0.0;\r
 905                         }\r
 906                         /*\r
 907                         if (debug) std::cout << "V" << name \r
 908                                              << " E "  << nameVE1 << " " << nameVE2 << std::endl;\r
 909                          */\r
 910 \r
 911                         p2.X() = currentEdge.m_vertices[0]->GetData().m_pos.X();\r
 912                         p2.Y() = currentEdge.m_vertices[0]->GetData().m_pos.Y();\r
 913                         p2.Z() = currentEdge.m_vertices[0]->GetData().m_pos.Z();\r
 914                         p3.X() = currentEdge.m_vertices[1]->GetData().m_pos.X();\r
 915                         p3.Y() = currentEdge.m_vertices[1]->GetData().m_pos.Y();\r
 916                         p3.Z() = currentEdge.m_vertices[1]->GetData().m_pos.Z();\r
 917                         d0 = p3 - p2;\r
 918                         if (d0.GetNorm() > 0.0)\r
 919                         {\r
 920                             if (IntersectLineLine(p0, p1, p2, p3, pa, pb, mua, mub))\r
 921                             {\r
 922                                 d1 = pa - p2;\r
 923                                 d2 = pa - pb;\r
 924                                 d3 = pa - p0;\r
 925                                 mua = d1.GetNorm()/d0.GetNorm();\r
 926                                 mub = d1*d0;\r
 927                                 s = d3*ptNormal;\r
 928                                 if (d2.GetNorm() < EPS &&  mua <= 1.0 && mub>=0.0 && s>0.0)\r
 929                                 {\r
 930                                     distance = std::max<double>(distance, d3.GetNorm());\r
 931                                 }\r
 932                             }\r
 933                         }\r
 934                     }\r
 935                                         m_mesh.m_edges.Next();\r
 936                                 }\r
 937                         }\r
 938                         return distance;\r
 939                 }\r
 940                 else\r
 941                 {\r
 942                         Vec3<double> ptNormal(static_cast<double>(normal.X()), \r
 943                                                                   static_cast<double>(normal.Y()), \r
 944                                                                   static_cast<double>(normal.Z()));\r
 945 \r
 946                         Vec3<double> impact;\r
 947                         long nhit;\r
 948                         double dist;\r
 949                         double distance = 0.0; \r
 950                         size_t nT = m_mesh.GetNTriangles();\r
 951                         insideHull = false;\r
 952                         CircularListElement<TMMTriangle> * face = 0;\r
 953                         Vec3<double> ver0, ver1, ver2;\r
 954                         for(size_t f = 0; f < nT; f++)\r
 955                         {\r
 956                                 TMMTriangle & currentTriangle = m_mesh.m_triangles.GetHead()->GetData();\r
 957         /*\r
 958                                 if (debug) std::cout << "T " << currentTriangle.m_vertices[0]->GetData().m_name << " "\r
 959                                                                                                                   << currentTriangle.m_vertices[1]->GetData().m_name << " "\r
 960                                                                                                                   << currentTriangle.m_vertices[2]->GetData().m_name << std::endl;\r
 961      */\r
 962                                 if (currentTriangle.m_vertices[0]->GetData().m_name == name ||\r
 963                                         currentTriangle.m_vertices[1]->GetData().m_name == name ||\r
 964                                         currentTriangle.m_vertices[2]->GetData().m_name == name)\r
 965                                 {\r
 966                                         nhit = 1;\r
 967                                         dist = 0.0;\r
 968                                 }\r
 969                                 else\r
 970                                 {\r
 971                                         ver0.X() = currentTriangle.m_vertices[0]->GetData().m_pos.X();\r
 972                                         ver0.Y() = currentTriangle.m_vertices[0]->GetData().m_pos.Y();\r
 973                                         ver0.Z() = currentTriangle.m_vertices[0]->GetData().m_pos.Z();\r
 974                                         ver1.X() = currentTriangle.m_vertices[1]->GetData().m_pos.X();\r
 975                                         ver1.Y() = currentTriangle.m_vertices[1]->GetData().m_pos.Y();\r
 976                                         ver1.Z() = currentTriangle.m_vertices[1]->GetData().m_pos.Z();\r
 977                                         ver2.X() = currentTriangle.m_vertices[2]->GetData().m_pos.X();\r
 978                                         ver2.Y() = currentTriangle.m_vertices[2]->GetData().m_pos.Y();\r
 979                                         ver2.Z() = currentTriangle.m_vertices[2]->GetData().m_pos.Z();\r
 980                                         nhit = IntersectRayTriangle(p0, ptNormal, ver0, ver1, ver2, dist);\r
 981                                 }\r
 982 \r
 983                                 if (nhit == 1 && distance <= dist)\r
 984                                 {\r
 985                                         distance = dist;\r
 986                                         insideHull = true;\r
 987                                         face = m_mesh.m_triangles.GetHead();    \r
 988 /*\r
 989                                         std::cout << name << " -> T " << currentTriangle.m_vertices[0]->GetData().m_name << " "\r
 990                                                                                                   << currentTriangle.m_vertices[1]->GetData().m_name << " "\r
 991                                                                                                   << currentTriangle.m_vertices[2]->GetData().m_name << " Dist "\r
 992                                                                                                   << dist << " P " << currentTriangle.m_normal * normal << std::endl;\r
 993 */\r
 994                                         if (dist > 0.1)\r
 995                                         {\r
 996                                                 break;\r
 997                                         }\r
 998                                 }\r
 999                                 m_mesh.m_triangles.Next();\r
1000                         }\r
1001                         if (updateIncidentPoints && face && m_distPoints)\r
1002                         {\r
1003                                 (*m_distPoints)[name].m_dist = static_cast<Real>(distance);\r
1004                                 face->GetData().m_incidentPoints.insert(name);\r
1005                         }\r
1006                         return distance;\r
1007                 }\r
1008         }\r
1009 }\r
1010 \r