dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftBodyHelpers.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///btSoftBodyHelpers.cpp by Nathanael Presson
  16
  17 #include "btSoftBodyInternals.h"
  18 #include <stdio.h>
  19 #include <string>
  20 #include <iostream>
  21 #include <iomanip>
  22 #include <sstream>
  23 #include <string.h>
  24 #include <algorithm>
  25 #include "btSoftBodyHelpers.h"
  26 #include "LinearMath/btConvexHull.h"
  27 #include "LinearMath/btConvexHullComputer.h"
  28 #include <map>
  29 #include <vector>
  30
  31 static void drawVertex(btIDebugDraw* idraw,
  32                                            const btVector3& x, btScalar s, const btVector3& c)
  33 {
  34         idraw->drawLine(x - btVector3(s, 0, 0), x + btVector3(s, 0, 0), c);
  35         idraw->drawLine(x - btVector3(0, s, 0), x + btVector3(0, s, 0), c);
  36         idraw->drawLine(x - btVector3(0, 0, s), x + btVector3(0, 0, s), c);
  37 }
  38
  39 //
  40 static void drawBox(btIDebugDraw* idraw,
  41                                         const btVector3& mins,
  42                                         const btVector3& maxs,
  43                                         const btVector3& color)
  44 {
  45         const btVector3 c[] = {btVector3(mins.x(), mins.y(), mins.z()),
  46                                                    btVector3(maxs.x(), mins.y(), mins.z()),
  47                                                    btVector3(maxs.x(), maxs.y(), mins.z()),
  48                                                    btVector3(mins.x(), maxs.y(), mins.z()),
  49                                                    btVector3(mins.x(), mins.y(), maxs.z()),
  50                                                    btVector3(maxs.x(), mins.y(), maxs.z()),
  51                                                    btVector3(maxs.x(), maxs.y(), maxs.z()),
  52                                                    btVector3(mins.x(), maxs.y(), maxs.z())};
  53         idraw->drawLine(c[0], c[1], color);
  54         idraw->drawLine(c[1], c[2], color);
  55         idraw->drawLine(c[2], c[3], color);
  56         idraw->drawLine(c[3], c[0], color);
  57         idraw->drawLine(c[4], c[5], color);
  58         idraw->drawLine(c[5], c[6], color);
  59         idraw->drawLine(c[6], c[7], color);
  60         idraw->drawLine(c[7], c[4], color);
  61         idraw->drawLine(c[0], c[4], color);
  62         idraw->drawLine(c[1], c[5], color);
  63         idraw->drawLine(c[2], c[6], color);
  64         idraw->drawLine(c[3], c[7], color);
  65 }
  66
  67 //
  68 static void drawTree(btIDebugDraw* idraw,
  69                                          const btDbvtNode* node,
  70                                          int depth,
  71                                          const btVector3& ncolor,
  72                                          const btVector3& lcolor,
  73                                          int mindepth,
  74                                          int maxdepth)
  75 {
  76         if (node)
  77         {
  78                 if (node->isinternal() && ((depth < maxdepth) || (maxdepth < 0)))
  79                 {
  80                         drawTree(idraw, node->childs[0], depth + 1, ncolor, lcolor, mindepth, maxdepth);
  81                         drawTree(idraw, node->childs[1], depth + 1, ncolor, lcolor, mindepth, maxdepth);
  82                 }
  83                 if (depth >= mindepth)
  84                 {
  85                         const btScalar scl = (btScalar)(node->isinternal() ? 1 : 1);
  86                         const btVector3 mi = node->volume.Center() - node->volume.Extents() * scl;
  87                         const btVector3 mx = node->volume.Center() + node->volume.Extents() * scl;
  88                         drawBox(idraw, mi, mx, node->isleaf() ? lcolor : ncolor);
  89                 }
  90         }
  91 }
  92
  93 //
  94 template <typename T>
  95 static inline T sum(const btAlignedObjectArray<T>& items)
  96 {
  97         T v;
  98         if (items.size())
  99         {
 100                 v = items[0];
 101                 for (int i = 1, ni = items.size(); i < ni; ++i)
 102                 {
 103                         v += items[i];
 104                 }
 105         }
 106         return (v);
 107 }
 108
 109 //
 110 template <typename T, typename Q>
 111 static inline void add(btAlignedObjectArray<T>& items, const Q& value)
 112 {
 113         for (int i = 0, ni = items.size(); i < ni; ++i)
 114         {
 115                 items[i] += value;
 116         }
 117 }
 118
 119 //
 120 template <typename T, typename Q>
 121 static inline void mul(btAlignedObjectArray<T>& items, const Q& value)
 122 {
 123         for (int i = 0, ni = items.size(); i < ni; ++i)
 124         {
 125                 items[i] *= value;
 126         }
 127 }
 128
 129 //
 130 template <typename T>
 131 static inline T average(const btAlignedObjectArray<T>& items)
 132 {
 133         const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1);
 134         return (sum(items) / n);
 135 }
 136
 137 #if 0
 138 //
 139  inline static btScalar         tetravolume(const btVector3& x0,
 140                                                                                 const btVector3& x1,
 141                                                                                 const btVector3& x2,
 142                                                                                 const btVector3& x3)
 143 {
 144         const btVector3 a=x1-x0;
 145         const btVector3 b=x2-x0;
 146         const btVector3 c=x3-x0;
 147         return(btDot(a,btCross(b,c)));
 148 }
 149 #endif
 150
 151 //
 152 #if 0
 153 static btVector3                stresscolor(btScalar stress)
 154 {
 155         static const btVector3  spectrum[]=     {       btVector3(1,0,1),
 156                 btVector3(0,0,1),
 157                 btVector3(0,1,1),
 158                 btVector3(0,1,0),
 159                 btVector3(1,1,0),
 160                 btVector3(1,0,0),
 161                 btVector3(1,0,0)};
 162         static const int                ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
 163         static const btScalar   one=1;
 164         stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
 165         const int                               sel=(int)stress;
 166         const btScalar                  frc=stress-sel;
 167         return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
 168 }
 169 #endif
 170
 171 //
 172 void btSoftBodyHelpers::Draw(btSoftBody* psb,
 173                                                          btIDebugDraw* idraw,
 174                                                          int drawflags)
 175 {
 176         const btScalar scl = (btScalar)0.1;
 177         const btScalar nscl = scl * 5;
 178         const btVector3 lcolor = btVector3(0, 0, 0);
 179         const btVector3 ncolor = btVector3(1, 1, 1);
 180         const btVector3 ccolor = btVector3(1, 0, 0);
 181         int i, j, nj;
 182
 183         /* Clusters     */
 184         if (0 != (drawflags & fDrawFlags::Clusters))
 185         {
 186                 srand(1806);
 187                 for (i = 0; i < psb->m_clusters.size(); ++i)
 188                 {
 189                         if (psb->m_clusters[i]->m_collide)
 190                         {
 191                                 btVector3 color(rand() / (btScalar)RAND_MAX,
 192                                                                 rand() / (btScalar)RAND_MAX,
 193                                                                 rand() / (btScalar)RAND_MAX);
 194                                 color = color.normalized() * 0.75;
 195                                 btAlignedObjectArray<btVector3> vertices;
 196                                 vertices.resize(psb->m_clusters[i]->m_nodes.size());
 197                                 for (j = 0, nj = vertices.size(); j < nj; ++j)
 198                                 {
 199                                         vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x;
 200                                 }
 201 #define USE_NEW_CONVEX_HULL_COMPUTER
 202 #ifdef USE_NEW_CONVEX_HULL_COMPUTER
 203                                 btConvexHullComputer computer;
 204                                 int stride = sizeof(btVector3);
 205                                 int count = vertices.size();
 206                                 btScalar shrink = 0.f;
 207                                 btScalar shrinkClamp = 0.f;
 208                                 computer.compute(&vertices[0].getX(), stride, count, shrink, shrinkClamp);
 209                                 for (int i = 0; i < computer.faces.size(); i++)
 210                                 {
 211                                         int face = computer.faces[i];
 212                                         //printf("face=%d\n",face);
 213                                         const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
 214                                         const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
 215
 216                                         int v0 = firstEdge->getSourceVertex();
 217                                         int v1 = firstEdge->getTargetVertex();
 218                                         while (edge != firstEdge)
 219                                         {
 220                                                 int v2 = edge->getTargetVertex();
 221                                                 idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1);
 222                                                 edge = edge->getNextEdgeOfFace();
 223                                                 v0 = v1;
 224                                                 v1 = v2;
 225                                         };
 226                                 }
 227 #else
 228
 229                                 HullDesc hdsc(QF_TRIANGLES, vertices.size(), &vertices[0]);
 230                                 HullResult hres;
 231                                 HullLibrary hlib;
 232                                 hdsc.mMaxVertices = vertices.size();
 233                                 hlib.CreateConvexHull(hdsc, hres);
 234                                 const btVector3 center = average(hres.m_OutputVertices);
 235                                 add(hres.m_OutputVertices, -center);
 236                                 mul(hres.m_OutputVertices, (btScalar)1);
 237                                 add(hres.m_OutputVertices, center);
 238                                 for (j = 0; j < (int)hres.mNumFaces; ++j)
 239                                 {
 240                                         const int idx[] = {hres.m_Indices[j * 3 + 0], hres.m_Indices[j * 3 + 1], hres.m_Indices[j * 3 + 2]};
 241                                         idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
 242                                                                                 hres.m_OutputVertices[idx[1]],
 243                                                                                 hres.m_OutputVertices[idx[2]],
 244                                                                                 color, 1);
 245                                 }
 246                                 hlib.ReleaseResult(hres);
 247 #endif
 248                         }
 249                         /* Velocities   */
 250 #if 0
 251                         for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
 252                         {
 253                                 const btSoftBody::Cluster&      c=psb->m_clusters[i];
 254                                 const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
 255                                 const btVector3                         v=c.m_lv+btCross(c.m_av,r);
 256                                 idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
 257                         }
 258 #endif
 259                         /* Frame                */
 260                         //              btSoftBody::Cluster& c=*psb->m_clusters[i];
 261                         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
 262                         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
 263                         //              idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
 264                 }
 265         }
 266         else
 267         {
 268                 /* Nodes        */
 269                 if (0 != (drawflags & fDrawFlags::Nodes))
 270                 {
 271                         for (i = 0; i < psb->m_nodes.size(); ++i)
 272                         {
 273                                 const btSoftBody::Node& n = psb->m_nodes[i];
 274                                 if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 275                                 idraw->drawLine(n.m_x - btVector3(scl, 0, 0), n.m_x + btVector3(scl, 0, 0), btVector3(1, 0, 0));
 276                                 idraw->drawLine(n.m_x - btVector3(0, scl, 0), n.m_x + btVector3(0, scl, 0), btVector3(0, 1, 0));
 277                                 idraw->drawLine(n.m_x - btVector3(0, 0, scl), n.m_x + btVector3(0, 0, scl), btVector3(0, 0, 1));
 278                         }
 279                 }
 280                 /* Links        */
 281                 if (0 != (drawflags & fDrawFlags::Links))
 282                 {
 283                         for (i = 0; i < psb->m_links.size(); ++i)
 284                         {
 285                                 const btSoftBody::Link& l = psb->m_links[i];
 286                                 if (0 == (l.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 287                                 idraw->drawLine(l.m_n[0]->m_x, l.m_n[1]->m_x, lcolor);
 288                         }
 289                 }
 290                 /* Normals      */
 291                 if (0 != (drawflags & fDrawFlags::Normals))
 292                 {
 293                         for (i = 0; i < psb->m_nodes.size(); ++i)
 294                         {
 295                                 const btSoftBody::Node& n = psb->m_nodes[i];
 296                                 if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 297                                 const btVector3 d = n.m_n * nscl;
 298                                 idraw->drawLine(n.m_x, n.m_x + d, ncolor);
 299                                 idraw->drawLine(n.m_x, n.m_x - d, ncolor * 0.5);
 300                         }
 301                 }
 302                 /* Contacts     */
 303                 if (0 != (drawflags & fDrawFlags::Contacts))
 304                 {
 305                         static const btVector3 axis[] = {btVector3(1, 0, 0),
 306                                                                                          btVector3(0, 1, 0),
 307                                                                                          btVector3(0, 0, 1)};
 308                         for (i = 0; i < psb->m_rcontacts.size(); ++i)
 309                         {
 310                                 const btSoftBody::RContact& c = psb->m_rcontacts[i];
 311                                 const btVector3 o = c.m_node->m_x - c.m_cti.m_normal *
 312                                                                                                                 (btDot(c.m_node->m_x, c.m_cti.m_normal) + c.m_cti.m_offset);
 313                                 const btVector3 x = btCross(c.m_cti.m_normal, axis[c.m_cti.m_normal.minAxis()]).normalized();
 314                                 const btVector3 y = btCross(x, c.m_cti.m_normal).normalized();
 315                                 idraw->drawLine(o - x * nscl, o + x * nscl, ccolor);
 316                                 idraw->drawLine(o - y * nscl, o + y * nscl, ccolor);
 317                                 idraw->drawLine(o, o + c.m_cti.m_normal * nscl * 3, btVector3(1, 1, 0));
 318                         }
 319                 }
 320                 /* Faces        */
 321                 if (0 != (drawflags & fDrawFlags::Faces))
 322                 {
 323                         const btScalar scl = (btScalar)0.8;
 324                         const btScalar alp = (btScalar)1;
 325                         const btVector3 col(0, (btScalar)0.7, 0);
 326                         for (i = 0; i < psb->m_faces.size(); ++i)
 327                         {
 328                                 const btSoftBody::Face& f = psb->m_faces[i];
 329                                 if (0 == (f.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 330                                 const btVector3 x[] = {f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x};
 331                                 const btVector3 c = (x[0] + x[1] + x[2]) / 3;
 332                                 idraw->drawTriangle((x[0] - c) * scl + c,
 333                                                                         (x[1] - c) * scl + c,
 334                                                                         (x[2] - c) * scl + c,
 335                                                                         col, alp);
 336                         }
 337                 }
 338                 /* Tetras       */
 339                 if (0 != (drawflags & fDrawFlags::Tetras))
 340                 {
 341                         const btScalar scl = (btScalar)0.8;
 342                         const btScalar alp = (btScalar)1;
 343                         const btVector3 col((btScalar)0.3, (btScalar)0.3, (btScalar)0.7);
 344                         for (int i = 0; i < psb->m_tetras.size(); ++i)
 345                         {
 346                                 const btSoftBody::Tetra& t = psb->m_tetras[i];
 347                                 if (0 == (t.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 348                                 const btVector3 x[] = {t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x};
 349                                 const btVector3 c = (x[0] + x[1] + x[2] + x[3]) / 4;
 350                                 idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[2] - c) * scl + c, col, alp);
 351                                 idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
 352                                 idraw->drawTriangle((x[1] - c) * scl + c, (x[2] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
 353                                 idraw->drawTriangle((x[2] - c) * scl + c, (x[0] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
 354                         }
 355                 }
 356         }
 357         /* Anchors      */
 358         if (0 != (drawflags & fDrawFlags::Anchors))
 359         {
 360                 for (i = 0; i < psb->m_anchors.size(); ++i)
 361                 {
 362                         const btSoftBody::Anchor& a = psb->m_anchors[i];
 363                         const btVector3 q = a.m_body->getWorldTransform() * a.m_local;
 364                         drawVertex(idraw, a.m_node->m_x, 0.25, btVector3(1, 0, 0));
 365                         drawVertex(idraw, q, 0.25, btVector3(0, 1, 0));
 366                         idraw->drawLine(a.m_node->m_x, q, btVector3(1, 1, 1));
 367                 }
 368                 for (i = 0; i < psb->m_nodes.size(); ++i)
 369                 {
 370                         const btSoftBody::Node& n = psb->m_nodes[i];
 371                         if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
 372                         if (n.m_im <= 0)
 373                         {
 374                                 drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0));
 375                         }
 376                 }
 377         }
 378
 379         /* Notes        */
 380         if (0 != (drawflags & fDrawFlags::Notes))
 381         {
 382                 for (i = 0; i < psb->m_notes.size(); ++i)
 383                 {
 384                         const btSoftBody::Note& n = psb->m_notes[i];
 385                         btVector3 p = n.m_offset;
 386                         for (int j = 0; j < n.m_rank; ++j)
 387                         {
 388                                 p += n.m_nodes[j]->m_x * n.m_coords[j];
 389                         }
 390                         idraw->draw3dText(p, n.m_text);
 391                 }
 392         }
 393         /* Node tree    */
 394         if (0 != (drawflags & fDrawFlags::NodeTree)) DrawNodeTree(psb, idraw);
 395         /* Face tree    */
 396         if (0 != (drawflags & fDrawFlags::FaceTree)) DrawFaceTree(psb, idraw);
 397         /* Cluster tree */
 398         if (0 != (drawflags & fDrawFlags::ClusterTree)) DrawClusterTree(psb, idraw);
 399         /* Joints               */
 400         if (0 != (drawflags & fDrawFlags::Joints))
 401         {
 402                 for (i = 0; i < psb->m_joints.size(); ++i)
 403                 {
 404                         const btSoftBody::Joint* pj = psb->m_joints[i];
 405                         switch (pj->Type())
 406                         {
 407                                 case btSoftBody::Joint::eType::Linear:
 408                                 {
 409                                         const btSoftBody::LJoint* pjl = (const btSoftBody::LJoint*)pj;
 410                                         const btVector3 a0 = pj->m_bodies[0].xform() * pjl->m_refs[0];
 411                                         const btVector3 a1 = pj->m_bodies[1].xform() * pjl->m_refs[1];
 412                                         idraw->drawLine(pj->m_bodies[0].xform().getOrigin(), a0, btVector3(1, 1, 0));
 413                                         idraw->drawLine(pj->m_bodies[1].xform().getOrigin(), a1, btVector3(0, 1, 1));
 414                                         drawVertex(idraw, a0, 0.25, btVector3(1, 1, 0));
 415                                         drawVertex(idraw, a1, 0.25, btVector3(0, 1, 1));
 416                                 }
 417                                 break;
 418                                 case btSoftBody::Joint::eType::Angular:
 419                                 {
 420                                         //const btSoftBody::AJoint*     pja=(const btSoftBody::AJoint*)pj;
 421                                         const btVector3 o0 = pj->m_bodies[0].xform().getOrigin();
 422                                         const btVector3 o1 = pj->m_bodies[1].xform().getOrigin();
 423                                         const btVector3 a0 = pj->m_bodies[0].xform().getBasis() * pj->m_refs[0];
 424                                         const btVector3 a1 = pj->m_bodies[1].xform().getBasis() * pj->m_refs[1];
 425                                         idraw->drawLine(o0, o0 + a0 * 10, btVector3(1, 1, 0));
 426                                         idraw->drawLine(o0, o0 + a1 * 10, btVector3(1, 1, 0));
 427                                         idraw->drawLine(o1, o1 + a0 * 10, btVector3(0, 1, 1));
 428                                         idraw->drawLine(o1, o1 + a1 * 10, btVector3(0, 1, 1));
 429                                         break;
 430                                 }
 431                                 default:
 432                                 {
 433                                 }
 434                         }
 435                 }
 436         }
 437 }
 438
 439 //
 440 void btSoftBodyHelpers::DrawInfos(btSoftBody* psb,
 441                                                                   btIDebugDraw* idraw,
 442                                                                   bool masses,
 443                                                                   bool areas,
 444                                                                   bool /*stress*/)
 445 {
 446         for (int i = 0; i < psb->m_nodes.size(); ++i)
 447         {
 448                 const btSoftBody::Node& n = psb->m_nodes[i];
 449                 char text[2048] = {0};
 450                 char buff[1024];
 451                 if (masses)
 452                 {
 453                         sprintf(buff, " M(%.2f)", 1 / n.m_im);
 454                         strcat(text, buff);
 455                 }
 456                 if (areas)
 457                 {
 458                         sprintf(buff, " A(%.2f)", n.m_area);
 459                         strcat(text, buff);
 460                 }
 461                 if (text[0]) idraw->draw3dText(n.m_x, text);
 462         }
 463 }
 464
 465 //
 466 void btSoftBodyHelpers::DrawNodeTree(btSoftBody* psb,
 467                                                                          btIDebugDraw* idraw,
 468                                                                          int mindepth,
 469                                                                          int maxdepth)
 470 {
 471         drawTree(idraw, psb->m_ndbvt.m_root, 0, btVector3(1, 0, 1), btVector3(1, 1, 1), mindepth, maxdepth);
 472 }
 473
 474 //
 475 void btSoftBodyHelpers::DrawFaceTree(btSoftBody* psb,
 476                                                                          btIDebugDraw* idraw,
 477                                                                          int mindepth,
 478                                                                          int maxdepth)
 479 {
 480         drawTree(idraw, psb->m_fdbvt.m_root, 0, btVector3(0, 1, 0), btVector3(1, 0, 0), mindepth, maxdepth);
 481 }
 482
 483 //
 484 void btSoftBodyHelpers::DrawClusterTree(btSoftBody* psb,
 485                                                                                 btIDebugDraw* idraw,
 486                                                                                 int mindepth,
 487                                                                                 int maxdepth)
 488 {
 489         drawTree(idraw, psb->m_cdbvt.m_root, 0, btVector3(0, 1, 1), btVector3(1, 0, 0), mindepth, maxdepth);
 490 }
 491
 492 //The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear
 493 // to be first set up to connect a node to between 5 and 6 of its neighbors [480 links],
 494 //and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm
 495 //[another 930 links].
 496 //The way the links are stored by default, we have a number of cases where adjacent links share a node in common
 497 // - this leads to the creation of a data dependency through memory.
 498 //The PSolve_Links() function reads and writes nodes as it iterates over each link.
 499 //So, we now have the possibility of a data dependency between iteration X
 500 //that processes link L with iteration X+1 that processes link L+1
 501 //because L and L+1 have one node in common, and iteration X updates the positions of that node,
 502 //and iteration X+1 reads in the position of that shared node.
 503 //
 504 //Such a memory dependency limits the ability of a modern CPU to speculate beyond
 505 //a certain point because it has to respect a possible dependency
 506 //- this prevents the CPU from making full use of its out-of-order resources.
 507 //If we re-order the links such that we minimize the cases where a link L and L+1 share a common node,
 508 //we create a temporal gap between when the node position is written,
 509 //and when it is subsequently read. This in turn allows the CPU to continue execution without
 510 //risking a dependency violation. Such a reordering would result in significant speedups on
 511 //modern CPUs with lots of execution resources.
 512 //In our testing, we see it have a tremendous impact not only on the A7,
 513 //but also on all x86 cores that ship with modern Macs.
 514 //The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a
 515 //btSoftBody object in the solveConstraints() function before the actual solver is invoked,
 516 //or right after generateBendingConstraints() once we have all 1410 links.
 517
 518 //===================================================================
 519 //
 520 //
 521 // This function takes in a list of interdependent Links and tries
 522 // to maximize the distance between calculation
 523 // of dependent links.  This increases the amount of parallelism that can
 524 // be exploited by out-of-order instruction processors with large but
 525 // (inevitably) finite instruction windows.
 526 //
 527 //===================================================================
 528
 529 // A small structure to track lists of dependent link calculations
 530 class LinkDeps_t
 531 {
 532 public:
 533         int value;         // A link calculation that is dependent on this one
 534                                            // Positive values = "input A" while negative values = "input B"
 535         LinkDeps_t* next;  // Next dependence in the list
 536 };
 537 typedef LinkDeps_t* LinkDepsPtr_t;
 538
 539 // Dependency list constants
 540 #define REOP_NOT_DEPENDENT -1
 541 #define REOP_NODE_COMPLETE -2  // Must be less than REOP_NOT_DEPENDENT
 542
 543 void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody* psb /* This can be replaced by a btSoftBody pointer */)
 544 {
 545         int i, nLinks = psb->m_links.size(), nNodes = psb->m_nodes.size();
 546         btSoftBody::Link* lr;
 547         int ar, br;
 548         btSoftBody::Node* node0 = &(psb->m_nodes[0]);
 549         btSoftBody::Node* node1 = &(psb->m_nodes[1]);
 550         LinkDepsPtr_t linkDep;
 551         int readyListHead, readyListTail, linkNum, linkDepFrees, depLink;
 552
 553         // Allocate temporary buffers
 554         int* nodeWrittenAt = new int[nNodes + 1];  // What link calculation produced this node's current values?
 555         int* linkDepA = new int[nLinks];           // Link calculation input is dependent upon prior calculation #N
 556         int* linkDepB = new int[nLinks];
 557         int* readyList = new int[nLinks];                              // List of ready-to-process link calculations (# of links, maximum)
 558         LinkDeps_t* linkDepFreeList = new LinkDeps_t[2 * nLinks];      // Dependent-on-me list elements (2x# of links, maximum)
 559         LinkDepsPtr_t* linkDepListStarts = new LinkDepsPtr_t[nLinks];  // Start nodes of dependent-on-me lists, one for each link
 560
 561         // Copy the original, unsorted links to a side buffer
 562         btSoftBody::Link* linkBuffer = new btSoftBody::Link[nLinks];
 563         memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link) * nLinks);
 564
 565         // Clear out the node setup and ready list
 566         for (i = 0; i < nNodes + 1; i++)
 567         {
 568                 nodeWrittenAt[i] = REOP_NOT_DEPENDENT;
 569         }
 570         for (i = 0; i < nLinks; i++)
 571         {
 572                 linkDepListStarts[i] = NULL;
 573         }
 574         readyListHead = readyListTail = linkDepFrees = 0;
 575
 576         // Initial link analysis to set up data structures
 577         for (i = 0; i < nLinks; i++)
 578         {
 579                 // Note which prior link calculations we are dependent upon & build up dependence lists
 580                 lr = &(psb->m_links[i]);
 581                 ar = (lr->m_n[0] - node0) / (node1 - node0);
 582                 br = (lr->m_n[1] - node0) / (node1 - node0);
 583                 if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT)
 584                 {
 585                         linkDepA[i] = nodeWrittenAt[ar];
 586                         linkDep = &linkDepFreeList[linkDepFrees++];
 587                         linkDep->value = i;
 588                         linkDep->next = linkDepListStarts[nodeWrittenAt[ar]];
 589                         linkDepListStarts[nodeWrittenAt[ar]] = linkDep;
 590                 }
 591                 else
 592                 {
 593                         linkDepA[i] = REOP_NOT_DEPENDENT;
 594                 }
 595                 if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT)
 596                 {
 597                         linkDepB[i] = nodeWrittenAt[br];
 598                         linkDep = &linkDepFreeList[linkDepFrees++];
 599                         linkDep->value = -(i + 1);
 600                         linkDep->next = linkDepListStarts[nodeWrittenAt[br]];
 601                         linkDepListStarts[nodeWrittenAt[br]] = linkDep;
 602                 }
 603                 else
 604                 {
 605                         linkDepB[i] = REOP_NOT_DEPENDENT;
 606                 }
 607
 608                 // Add this link to the initial ready list, if it is not dependent on any other links
 609                 if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT))
 610                 {
 611                         readyList[readyListTail++] = i;
 612                         linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE;  // Probably not needed now
 613                 }
 614
 615                 // Update the nodes to mark which ones are calculated by this link
 616                 nodeWrittenAt[ar] = nodeWrittenAt[br] = i;
 617         }
 618
 619         // Process the ready list and create the sorted list of links
 620         // -- By treating the ready list as a queue, we maximize the distance between any
 621         //    inter-dependent node calculations
 622         // -- All other (non-related) nodes in the ready list will automatically be inserted
 623         //    in between each set of inter-dependent link calculations by this loop
 624         i = 0;
 625         while (readyListHead != readyListTail)
 626         {
 627                 // Use ready list to select the next link to process
 628                 linkNum = readyList[readyListHead++];
 629                 // Copy the next-to-calculate link back into the original link array
 630                 psb->m_links[i++] = linkBuffer[linkNum];
 631
 632                 // Free up any link inputs that are dependent on this one
 633                 linkDep = linkDepListStarts[linkNum];
 634                 while (linkDep)
 635                 {
 636                         depLink = linkDep->value;
 637                         if (depLink >= 0)
 638                         {
 639                                 linkDepA[depLink] = REOP_NOT_DEPENDENT;
 640                         }
 641                         else
 642                         {
 643                                 depLink = -depLink - 1;
 644                                 linkDepB[depLink] = REOP_NOT_DEPENDENT;
 645                         }
 646                         // Add this dependent link calculation to the ready list if *both* inputs are clear
 647                         if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT))
 648                         {
 649                                 readyList[readyListTail++] = depLink;
 650                                 linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE;  // Probably not needed now
 651                         }
 652                         linkDep = linkDep->next;
 653                 }
 654         }
 655
 656         // Delete the temporary buffers
 657         delete[] nodeWrittenAt;
 658         delete[] linkDepA;
 659         delete[] linkDepB;
 660         delete[] readyList;
 661         delete[] linkDepFreeList;
 662         delete[] linkDepListStarts;
 663         delete[] linkBuffer;
 664 }
 665
 666 //
 667 void btSoftBodyHelpers::DrawFrame(btSoftBody* psb,
 668                                                                   btIDebugDraw* idraw)
 669 {
 670         if (psb->m_pose.m_bframe)
 671         {
 672                 static const btScalar ascl = 10;
 673                 static const btScalar nscl = (btScalar)0.1;
 674                 const btVector3 com = psb->m_pose.m_com;
 675                 const btMatrix3x3 trs = psb->m_pose.m_rot * psb->m_pose.m_scl;
 676                 const btVector3 Xaxis = (trs * btVector3(1, 0, 0)).normalized();
 677                 const btVector3 Yaxis = (trs * btVector3(0, 1, 0)).normalized();
 678                 const btVector3 Zaxis = (trs * btVector3(0, 0, 1)).normalized();
 679                 idraw->drawLine(com, com + Xaxis * ascl, btVector3(1, 0, 0));
 680                 idraw->drawLine(com, com + Yaxis * ascl, btVector3(0, 1, 0));
 681                 idraw->drawLine(com, com + Zaxis * ascl, btVector3(0, 0, 1));
 682                 for (int i = 0; i < psb->m_pose.m_pos.size(); ++i)
 683                 {
 684                         const btVector3 x = com + trs * psb->m_pose.m_pos[i];
 685                         drawVertex(idraw, x, nscl, btVector3(1, 0, 1));
 686                 }
 687         }
 688 }
 689
 690 //
 691 btSoftBody* btSoftBodyHelpers::CreateRope(btSoftBodyWorldInfo& worldInfo, const btVector3& from,
 692                                                                                   const btVector3& to,
 693                                                                                   int res,
 694                                                                                   int fixeds)
 695 {
 696         /* Create nodes */
 697         const int r = res + 2;
 698         btVector3* x = new btVector3[r];
 699         btScalar* m = new btScalar[r];
 700         int i;
 701
 702         for (i = 0; i < r; ++i)
 703         {
 704                 const btScalar t = i / (btScalar)(r - 1);
 705                 x[i] = lerp(from, to, t);
 706                 m[i] = 1;
 707         }
 708         btSoftBody* psb = new btSoftBody(&worldInfo, r, x, m);
 709         if (fixeds & 1) psb->setMass(0, 0);
 710         if (fixeds & 2) psb->setMass(r - 1, 0);
 711         delete[] x;
 712         delete[] m;
 713         /* Create links */
 714         for (i = 1; i < r; ++i)
 715         {
 716                 psb->appendLink(i - 1, i);
 717         }
 718         /* Finished             */
 719         return (psb);
 720 }
 721
 722 //
 723 btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00,
 724                                                                                    const btVector3& corner10,
 725                                                                                    const btVector3& corner01,
 726                                                                                    const btVector3& corner11,
 727                                                                                    int resx,
 728                                                                                    int resy,
 729                                                                                    int fixeds,
 730                                                                                    bool gendiags,
 731                                                                                    btScalar perturbation)
 732 {
 733 #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
 734         /* Create nodes */
 735         if ((resx < 2) || (resy < 2)) return (0);
 736         const int rx = resx;
 737         const int ry = resy;
 738         const int tot = rx * ry;
 739         btVector3* x = new btVector3[tot];
 740         btScalar* m = new btScalar[tot];
 741         int iy;
 742
 743         for (iy = 0; iy < ry; ++iy)
 744         {
 745                 const btScalar ty = iy / (btScalar)(ry - 1);
 746                 const btVector3 py0 = lerp(corner00, corner01, ty);
 747                 const btVector3 py1 = lerp(corner10, corner11, ty);
 748                 for (int ix = 0; ix < rx; ++ix)
 749                 {
 750                         const btScalar tx = ix / (btScalar)(rx - 1);
 751                         btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
 752                         btVector3 temp1 = py1;
 753                         temp1.setY(py1.getY() + pert);
 754                         btVector3 temp = py0;
 755                         pert = perturbation * btScalar(rand()) / RAND_MAX;
 756                         temp.setY(py0.getY() + pert);
 757                         x[IDX(ix, iy)] = lerp(temp, temp1, tx);
 758                         m[IDX(ix, iy)] = 1;
 759                 }
 760         }
 761         btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
 762         if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
 763         if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
 764         if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
 765         if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
 766         delete[] x;
 767         delete[] m;
 768         /* Create links and faces */
 769         for (iy = 0; iy < ry; ++iy)
 770         {
 771                 for (int ix = 0; ix < rx; ++ix)
 772                 {
 773                         const int idx = IDX(ix, iy);
 774                         const bool mdx = (ix + 1) < rx;
 775                         const bool mdy = (iy + 1) < ry;
 776                         if (mdx) psb->appendLink(idx, IDX(ix + 1, iy));
 777                         if (mdy) psb->appendLink(idx, IDX(ix, iy + 1));
 778                         if (mdx && mdy)
 779                         {
 780                                 if ((ix + iy) & 1)
 781                                 {
 782                                         psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
 783                                         psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy + 1), IDX(ix, iy + 1));
 784                                         if (gendiags)
 785                                         {
 786                                                 psb->appendLink(IDX(ix, iy), IDX(ix + 1, iy + 1));
 787                                         }
 788                                 }
 789                                 else
 790                                 {
 791                                         psb->appendFace(IDX(ix, iy + 1), IDX(ix, iy), IDX(ix + 1, iy));
 792                                         psb->appendFace(IDX(ix, iy + 1), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
 793                                         if (gendiags)
 794                                         {
 795                                                 psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1));
 796                                         }
 797                                 }
 798                         }
 799                 }
 800         }
 801         /* Finished             */
 802 #undef IDX
 803         return (psb);
 804 }
 805
 806 //
 807 btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
 808                                                                                          const btVector3& corner00,
 809                                                                                          const btVector3& corner10,
 810                                                                                          const btVector3& corner01,
 811                                                                                          const btVector3& corner11,
 812                                                                                          int resx,
 813                                                                                          int resy,
 814                                                                                          int fixeds,
 815                                                                                          bool gendiags,
 816                                                                                          float* tex_coords)
 817 {
 818         /*
 819         *
 820         *  corners:
 821         *
 822         *  [0][0]     corner00 ------- corner01   [resx][0]
 823         *                |                |
 824         *                |                |
 825         *  [0][resy]  corner10 -------- corner11  [resx][resy]
 826         *
 827         *
 828         *
 829         *
 830         *
 831         *
 832         *   "fixedgs" map:
 833         *
 834         *  corner00     -->   +1
 835         *  corner01     -->   +2
 836         *  corner10     -->   +4
 837         *  corner11     -->   +8
 838         *  upper middle -->  +16
 839         *  left middle  -->  +32
 840         *  right middle -->  +64
 841         *  lower middle --> +128
 842         *  center       --> +256
 843         *
 844         *
 845         *   tex_coords size   (resx-1)*(resy-1)*12
 846         *
 847         *
 848         *
 849         *     SINGLE QUAD INTERNALS
 850         *
 851         *  1) btSoftBody's nodes and links,
 852         *     diagonal link is optional ("gendiags")
 853         *
 854         *
 855         *    node00 ------ node01
 856         *      | .
 857         *      |   .
 858         *      |     .
 859         *      |       .
 860         *      |         .
 861         *    node10        node11
 862         *
 863         *
 864         *
 865         *   2) Faces:
 866         *      two triangles,
 867         *      UV Coordinates (hier example for single quad)
 868         *
 869         *     (0,1)          (0,1)  (1,1)
 870         *     1 |\            3 \-----| 2
 871         *       | \              \    |
 872         *       |  \              \   |
 873         *       |   \              \  |
 874         *       |    \              \ |
 875         *     2 |-----\ 3            \| 1
 876         *     (0,0)    (1,0)       (1,0)
 877         *
 878         *
 879         *
 880         *
 881         *
 882         *
 883         */
 884
 885 #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
 886         /* Create nodes         */
 887         if ((resx < 2) || (resy < 2)) return (0);
 888         const int rx = resx;
 889         const int ry = resy;
 890         const int tot = rx * ry;
 891         btVector3* x = new btVector3[tot];
 892         btScalar* m = new btScalar[tot];
 893
 894         int iy;
 895
 896         for (iy = 0; iy < ry; ++iy)
 897         {
 898                 const btScalar ty = iy / (btScalar)(ry - 1);
 899                 const btVector3 py0 = lerp(corner00, corner01, ty);
 900                 const btVector3 py1 = lerp(corner10, corner11, ty);
 901                 for (int ix = 0; ix < rx; ++ix)
 902                 {
 903                         const btScalar tx = ix / (btScalar)(rx - 1);
 904                         x[IDX(ix, iy)] = lerp(py0, py1, tx);
 905                         m[IDX(ix, iy)] = 1;
 906                 }
 907         }
 908         btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
 909         if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
 910         if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
 911         if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
 912         if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
 913         if (fixeds & 16) psb->setMass(IDX((rx - 1) / 2, 0), 0);
 914         if (fixeds & 32) psb->setMass(IDX(0, (ry - 1) / 2), 0);
 915         if (fixeds & 64) psb->setMass(IDX(rx - 1, (ry - 1) / 2), 0);
 916         if (fixeds & 128) psb->setMass(IDX((rx - 1) / 2, ry - 1), 0);
 917         if (fixeds & 256) psb->setMass(IDX((rx - 1) / 2, (ry - 1) / 2), 0);
 918         delete[] x;
 919         delete[] m;
 920
 921         int z = 0;
 922         /* Create links and faces       */
 923         for (iy = 0; iy < ry; ++iy)
 924         {
 925                 for (int ix = 0; ix < rx; ++ix)
 926                 {
 927                         const bool mdx = (ix + 1) < rx;
 928                         const bool mdy = (iy + 1) < ry;
 929
 930                         int node00 = IDX(ix, iy);
 931                         int node01 = IDX(ix + 1, iy);
 932                         int node10 = IDX(ix, iy + 1);
 933                         int node11 = IDX(ix + 1, iy + 1);
 934
 935                         if (mdx) psb->appendLink(node00, node01);
 936                         if (mdy) psb->appendLink(node00, node10);
 937                         if (mdx && mdy)
 938                         {
 939                                 psb->appendFace(node00, node10, node11);
 940                                 if (tex_coords)
 941                                 {
 942                                         tex_coords[z + 0] = CalculateUV(resx, resy, ix, iy, 0);
 943                                         tex_coords[z + 1] = CalculateUV(resx, resy, ix, iy, 1);
 944                                         tex_coords[z + 2] = CalculateUV(resx, resy, ix, iy, 0);
 945                                         tex_coords[z + 3] = CalculateUV(resx, resy, ix, iy, 2);
 946                                         tex_coords[z + 4] = CalculateUV(resx, resy, ix, iy, 3);
 947                                         tex_coords[z + 5] = CalculateUV(resx, resy, ix, iy, 2);
 948                                 }
 949                                 psb->appendFace(node11, node01, node00);
 950                                 if (tex_coords)
 951                                 {
 952                                         tex_coords[z + 6] = CalculateUV(resx, resy, ix, iy, 3);
 953                                         tex_coords[z + 7] = CalculateUV(resx, resy, ix, iy, 2);
 954                                         tex_coords[z + 8] = CalculateUV(resx, resy, ix, iy, 3);
 955                                         tex_coords[z + 9] = CalculateUV(resx, resy, ix, iy, 1);
 956                                         tex_coords[z + 10] = CalculateUV(resx, resy, ix, iy, 0);
 957                                         tex_coords[z + 11] = CalculateUV(resx, resy, ix, iy, 1);
 958                                 }
 959                                 if (gendiags) psb->appendLink(node00, node11);
 960                                 z += 12;
 961                         }
 962                 }
 963         }
 964         /* Finished     */
 965 #undef IDX
 966         return (psb);
 967 }
 968
 969 float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id)
 970 {
 971         /*
 972         *
 973         *
 974         *    node00 --- node01
 975         *      |          |
 976         *    node10 --- node11
 977         *
 978         *
 979         *   ID map:
 980         *
 981         *   node00 s --> 0
 982         *   node00 t --> 1
 983         *
 984         *   node01 s --> 3
 985         *   node01 t --> 1
 986         *
 987         *   node10 s --> 0
 988         *   node10 t --> 2
 989         *
 990         *   node11 s --> 3
 991         *   node11 t --> 2
 992         *
 993         *
 994         */
 995
 996         float tc = 0.0f;
 997         if (id == 0)
 998         {
 999                 tc = (1.0f / ((resx - 1)) * ix);
1000         }
1001         else if (id == 1)
1002         {
1003                 tc = (1.0f / ((resy - 1)) * (resy - 1 - iy));
1004         }
1005         else if (id == 2)
1006         {
1007                 tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1));
1008         }
1009         else if (id == 3)
1010         {
1011                 tc = (1.0f / ((resx - 1)) * (ix + 1));
1012         }
1013         return tc;
1014 }
1015 //
1016 btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center,
1017                                                                                            const btVector3& radius,
1018                                                                                            int res)
1019 {
1020         struct Hammersley
1021         {
1022                 static void Generate(btVector3* x, int n)
1023                 {
1024                         for (int i = 0; i < n; i++)
1025                         {
1026                                 btScalar p = 0.5, t = 0;
1027                                 for (int j = i; j; p *= 0.5, j >>= 1)
1028                                         if (j & 1) t += p;
1029                                 btScalar w = 2 * t - 1;
1030                                 btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n;
1031                                 btScalar s = btSqrt(1 - w * w);
1032                                 *x++ = btVector3(s * btCos(a), s * btSin(a), w);
1033                         }
1034                 }
1035         };
1036         btAlignedObjectArray<btVector3> vtx;
1037         vtx.resize(3 + res);
1038         Hammersley::Generate(&vtx[0], vtx.size());
1039         for (int i = 0; i < vtx.size(); ++i)
1040         {
1041                 vtx[i] = vtx[i] * radius + center;
1042         }
1043         return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size()));
1044 }
1045
1046 //
1047 btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices,
1048                                                                                                  const int* triangles,
1049                                                                                                  int ntriangles, bool randomizeConstraints)
1050 {
1051         int maxidx = 0;
1052         int i, j, ni;
1053
1054         for (i = 0, ni = ntriangles * 3; i < ni; ++i)
1055         {
1056                 maxidx = btMax(triangles[i], maxidx);
1057         }
1058         ++maxidx;
1059         btAlignedObjectArray<bool> chks;
1060         btAlignedObjectArray<btVector3> vtx;
1061         chks.resize(maxidx * maxidx, false);
1062         vtx.resize(maxidx);
1063         for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3)
1064         {
1065                 vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]);
1066         }
1067         btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0);
1068         for (i = 0, ni = ntriangles * 3; i < ni; i += 3)
1069         {
1070                 const int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]};
1071 #define IDX(_x_, _y_) ((_y_)*maxidx + (_x_))
1072                 for (int j = 2, k = 0; k < 3; j = k++)
1073                 {
1074                         if (!chks[IDX(idx[j], idx[k])])
1075                         {
1076                                 chks[IDX(idx[j], idx[k])] = true;
1077                                 chks[IDX(idx[k], idx[j])] = true;
1078                                 psb->appendLink(idx[j], idx[k]);
1079                         }
1080                 }
1081 #undef IDX
1082                 psb->appendFace(idx[0], idx[1], idx[2]);
1083         }
1084
1085         if (randomizeConstraints)
1086         {
1087                 psb->randomizeConstraints();
1088         }
1089
1090         return (psb);
1091 }
1092
1093 //
1094 btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
1095                                                                                                         int nvertices, bool randomizeConstraints)
1096 {
1097         HullDesc hdsc(QF_TRIANGLES, nvertices, vertices);
1098         HullResult hres;
1099         HullLibrary hlib; /*??*/
1100         hdsc.mMaxVertices = nvertices;
1101         hlib.CreateConvexHull(hdsc, hres);
1102         btSoftBody* psb = new btSoftBody(&worldInfo, (int)hres.mNumOutputVertices,
1103                                                                          &hres.m_OutputVertices[0], 0);
1104         for (int i = 0; i < (int)hres.mNumFaces; ++i)
1105         {
1106                 const int idx[] = {static_cast<int>(hres.m_Indices[i * 3 + 0]),
1107                                                    static_cast<int>(hres.m_Indices[i * 3 + 1]),
1108                                                    static_cast<int>(hres.m_Indices[i * 3 + 2])};
1109                 if (idx[0] < idx[1]) psb->appendLink(idx[0], idx[1]);
1110                 if (idx[1] < idx[2]) psb->appendLink(idx[1], idx[2]);
1111                 if (idx[2] < idx[0]) psb->appendLink(idx[2], idx[0]);
1112                 psb->appendFace(idx[0], idx[1], idx[2]);
1113         }
1114         hlib.ReleaseResult(hres);
1115         if (randomizeConstraints)
1116         {
1117                 psb->randomizeConstraints();
1118         }
1119         return (psb);
1120 }
1121
1122 static int nextLine(const char* buffer)
1123 {
1124         int numBytesRead = 0;
1125
1126         while (*buffer != '\n')
1127         {
1128                 buffer++;
1129                 numBytesRead++;
1130         }
1131
1132         if (buffer[0] == 0x0a)
1133         {
1134                 buffer++;
1135                 numBytesRead++;
1136         }
1137         return numBytesRead;
1138 }
1139
1140 /* Create from TetGen .ele, .face, .node data                                                   */
1141 btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
1142                                                                                                         const char* ele,
1143                                                                                                         const char* face,
1144                                                                                                         const char* node,
1145                                                                                                         bool bfacelinks,
1146                                                                                                         bool btetralinks,
1147                                                                                                         bool bfacesfromtetras)
1148 {
1149         btAlignedObjectArray<btVector3> pos;
1150         int nnode = 0;
1151         int ndims = 0;
1152         int nattrb = 0;
1153         int hasbounds = 0;
1154         int result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
1155         result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
1156         node += nextLine(node);
1157
1158         pos.resize(nnode);
1159         for (int i = 0; i < pos.size(); ++i)
1160         {
1161                 int index = 0;
1162                 //int                   bound=0;
1163                 float x, y, z;
1164                 sscanf(node, "%d %f %f %f", &index, &x, &y, &z);
1165
1166                 //      sn>>index;
1167                 //      sn>>x;sn>>y;sn>>z;
1168                 node += nextLine(node);
1169
1170                 //for(int j=0;j<nattrb;++j)
1171                 //      sn>>a;
1172
1173                 //if(hasbounds)
1174                 //      sn>>bound;
1175
1176                 pos[index].setX(btScalar(x));
1177                 pos[index].setY(btScalar(y));
1178                 pos[index].setZ(btScalar(z));
1179         }
1180         btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0);
1181 #if 0
1182 if(face&&face[0])
1183         {
1184         int                                                             nface=0;
1185         sf>>nface;sf>>hasbounds;
1186         for(int i=0;i<nface;++i)
1187                 {
1188                 int                     index=0;
1189                 int                     bound=0;
1190                 int                     ni[3];
1191                 sf>>index;
1192                 sf>>ni[0];sf>>ni[1];sf>>ni[2];
1193                 sf>>bound;
1194                 psb->appendFace(ni[0],ni[1],ni[2]);
1195                 if(btetralinks)
1196                         {
1197                         psb->appendLink(ni[0],ni[1],0,true);
1198                         psb->appendLink(ni[1],ni[2],0,true);
1199                         psb->appendLink(ni[2],ni[0],0,true);
1200                         }
1201                 }
1202         }
1203 #endif
1204
1205         if (ele && ele[0])
1206         {
1207                 int ntetra = 0;
1208                 int ncorner = 0;
1209                 int neattrb = 0;
1210                 sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb);
1211                 ele += nextLine(ele);
1212
1213                 //se>>ntetra;se>>ncorner;se>>neattrb;
1214                 for (int i = 0; i < ntetra; ++i)
1215                 {
1216                         int index = 0;
1217                         int ni[4];
1218
1219                         //se>>index;
1220                         //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
1221                         sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
1222                         ele += nextLine(ele);
1223                         //for(int j=0;j<neattrb;++j)
1224                         //      se>>a;
1225                         psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
1226                         if (btetralinks)
1227                         {
1228                                 psb->appendLink(ni[0], ni[1], 0, true);
1229                                 psb->appendLink(ni[1], ni[2], 0, true);
1230                                 psb->appendLink(ni[2], ni[0], 0, true);
1231                                 psb->appendLink(ni[0], ni[3], 0, true);
1232                                 psb->appendLink(ni[1], ni[3], 0, true);
1233                                 psb->appendLink(ni[2], ni[3], 0, true);
1234                         }
1235                 }
1236         }
1237         psb->initializeDmInverse();
1238         psb->m_tetraScratches.resize(psb->m_tetras.size());
1239         psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
1240         printf("Nodes:  %u\r\n", psb->m_nodes.size());
1241         printf("Links:  %u\r\n", psb->m_links.size());
1242         printf("Faces:  %u\r\n", psb->m_faces.size());
1243         printf("Tetras: %u\r\n", psb->m_tetras.size());
1244         return (psb);
1245 }
1246
1247 btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
1248 {
1249         std::ifstream fs;
1250         fs.open(vtk_file);
1251         btAssert(fs);
1252
1253         typedef btAlignedObjectArray<int> Index;
1254         std::string line;
1255         btAlignedObjectArray<btVector3> X;
1256         btVector3 position;
1257         btAlignedObjectArray<Index> indices;
1258         bool reading_points = false;
1259         bool reading_tets = false;
1260         size_t n_points = 0;
1261         size_t n_tets = 0;
1262         size_t x_count = 0;
1263         size_t indices_count = 0;
1264         while (std::getline(fs, line))
1265         {
1266                 std::stringstream ss(line);
1267                 if (line.size() == (size_t)(0))
1268                 {
1269                 }
1270                 else if (line.substr(0, 6) == "POINTS")
1271                 {
1272                         reading_points = true;
1273                         reading_tets = false;
1274                         ss.ignore(128, ' ');  // ignore "POINTS"
1275                         ss >> n_points;
1276                         X.resize(n_points);
1277                 }
1278                 else if (line.substr(0, 5) == "CELLS")
1279                 {
1280                         reading_points = false;
1281                         reading_tets = true;
1282                         ss.ignore(128, ' ');  // ignore "CELLS"
1283                         ss >> n_tets;
1284                         indices.resize(n_tets);
1285                 }
1286                 else if (line.substr(0, 10) == "CELL_TYPES")
1287                 {
1288                         reading_points = false;
1289                         reading_tets = false;
1290                 }
1291                 else if (reading_points)
1292                 {
1293                         btScalar p;
1294                         ss >> p;
1295                         position.setX(p);
1296                         ss >> p;
1297                         position.setY(p);
1298                         ss >> p;
1299                         position.setZ(p);
1300                         //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
1301                         X[x_count++] = position;
1302                 }
1303                 else if (reading_tets)
1304                 {
1305                         int d;
1306                         ss >> d;
1307                         if (d != 4)
1308                         {
1309                                 printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
1310                                 fs.close();
1311                                 return 0;
1312                         }
1313                         ss.ignore(128, ' ');  // ignore "4"
1314                         Index tet;
1315                         tet.resize(4);
1316                         for (size_t i = 0; i < 4; i++)
1317                         {
1318                                 ss >> tet[i];
1319                                 //printf("%d ", tet[i]);
1320                         }
1321                         //printf("\n");
1322                         indices[indices_count++] = tet;
1323                 }
1324         }
1325         btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
1326
1327         for (int i = 0; i < n_tets; ++i)
1328         {
1329                 const Index& ni = indices[i];
1330                 psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
1331                 {
1332                         psb->appendLink(ni[0], ni[1], 0, true);
1333                         psb->appendLink(ni[1], ni[2], 0, true);
1334                         psb->appendLink(ni[2], ni[0], 0, true);
1335                         psb->appendLink(ni[0], ni[3], 0, true);
1336                         psb->appendLink(ni[1], ni[3], 0, true);
1337                         psb->appendLink(ni[2], ni[3], 0, true);
1338                 }
1339         }
1340
1341         generateBoundaryFaces(psb);
1342         psb->initializeDmInverse();
1343         psb->m_tetraScratches.resize(psb->m_tetras.size());
1344         psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
1345         printf("Nodes:  %u\r\n", psb->m_nodes.size());
1346         printf("Links:  %u\r\n", psb->m_links.size());
1347         printf("Faces:  %u\r\n", psb->m_faces.size());
1348         printf("Tetras: %u\r\n", psb->m_tetras.size());
1349
1350         fs.close();
1351         return psb;
1352 }
1353
1354 void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
1355 {
1356         int counter = 0;
1357         for (int i = 0; i < psb->m_nodes.size(); ++i)
1358         {
1359                 psb->m_nodes[i].index = counter++;
1360         }
1361         typedef btAlignedObjectArray<int> Index;
1362         btAlignedObjectArray<Index> indices;
1363         indices.resize(psb->m_tetras.size());
1364         for (int i = 0; i < indices.size(); ++i)
1365         {
1366                 Index index;
1367                 index.push_back(psb->m_tetras[i].m_n[0]->index);
1368                 index.push_back(psb->m_tetras[i].m_n[1]->index);
1369                 index.push_back(psb->m_tetras[i].m_n[2]->index);
1370                 index.push_back(psb->m_tetras[i].m_n[3]->index);
1371                 indices[i] = index;
1372         }
1373
1374         std::map<std::vector<int>, std::vector<int> > dict;
1375         for (int i = 0; i < indices.size(); ++i)
1376         {
1377                 for (int j = 0; j < 4; ++j)
1378                 {
1379                         std::vector<int> f;
1380                         if (j == 0)
1381                         {
1382                                 f.push_back(indices[i][1]);
1383                                 f.push_back(indices[i][0]);
1384                                 f.push_back(indices[i][2]);
1385                         }
1386                         if (j == 1)
1387                         {
1388                                 f.push_back(indices[i][3]);
1389                                 f.push_back(indices[i][0]);
1390                                 f.push_back(indices[i][1]);
1391                         }
1392                         if (j == 2)
1393                         {
1394                                 f.push_back(indices[i][3]);
1395                                 f.push_back(indices[i][1]);
1396                                 f.push_back(indices[i][2]);
1397                         }
1398                         if (j == 3)
1399                         {
1400                                 f.push_back(indices[i][2]);
1401                                 f.push_back(indices[i][0]);
1402                                 f.push_back(indices[i][3]);
1403                         }
1404                         std::vector<int> f_sorted = f;
1405                         std::sort(f_sorted.begin(), f_sorted.end());
1406                         if (dict.find(f_sorted) != dict.end())
1407                         {
1408                                 dict.erase(f_sorted);
1409                         }
1410                         else
1411                         {
1412                                 dict.insert(std::make_pair(f_sorted, f));
1413                         }
1414                 }
1415         }
1416
1417         for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
1418         {
1419                 std::vector<int> f = it->second;
1420                 psb->appendFace(f[0], f[1], f[2]);
1421                 //printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1);
1422         }
1423 }
1424
1425 //Write the surface mesh to an obj file.
1426 void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
1427 {
1428         std::ofstream fs;
1429         fs.open(filename);
1430         btAssert(fs);
1431
1432         if (psb->m_tetras.size() > 0)
1433         {
1434                 // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
1435                 std::map<int, int> dict;
1436                 for (int i = 0; i < psb->m_faces.size(); i++)
1437                 {
1438                         for (int d = 0; d < 3; d++)
1439                         {
1440                                 int index = psb->m_faces[i].m_n[d]->index;
1441                                 if (dict.find(index) == dict.end())
1442                                 {
1443                                         int dict_size = dict.size();
1444                                         dict[index] = dict_size;
1445                                         fs << "v";
1446                                         for (int k = 0; k < 3; k++)
1447                                         {
1448                                                 fs << " " << psb->m_nodes[index].m_x[k];
1449                                         }
1450                                         fs << "\n";
1451                                 }
1452                         }
1453                 }
1454                 // Write surface mesh.
1455                 for (int i = 0; i < psb->m_faces.size(); ++i)
1456                 {
1457                         fs << "f";
1458                         for (int n = 0; n < 3; n++)
1459                         {
1460                                 fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
1461                         }
1462                         fs << "\n";
1463                 }
1464         }
1465         else
1466         {
1467                 // For trimesh, directly write out all the nodes and faces.xs
1468                 for (int i = 0; i < psb->m_nodes.size(); ++i)
1469                 {
1470                         fs << "v";
1471                         for (int d = 0; d < 3; d++)
1472                         {
1473                                 fs << " " << psb->m_nodes[i].m_x[d];
1474                         }
1475                         fs << "\n";
1476                 }
1477
1478                 for (int i = 0; i < psb->m_faces.size(); ++i)
1479                 {
1480                         fs << "f";
1481                         for (int n = 0; n < 3; n++)
1482                         {
1483                                 fs << " " << psb->m_faces[i].m_n[n]->index + 1;
1484                         }
1485                         fs << "\n";
1486                 }
1487         }
1488         fs.close();
1489 }
1490
1491
1492 void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
1493 {
1494         std::ofstream fs;
1495         fs.open(file);
1496         btAssert(fs);
1497         fs << std::scientific << std::setprecision(16);
1498
1499         // Only write out for trimesh, directly write out all the nodes and faces.xs
1500         for (int i = 0; i < psb->m_nodes.size(); ++i)
1501         {
1502                 fs << "q";
1503                 for (int d = 0; d < 3; d++)
1504                 {
1505                         fs << " " << psb->m_nodes[i].m_q[d];
1506                 }
1507                 fs << "\n";
1508         }
1509
1510         for (int i = 0; i < psb->m_nodes.size(); ++i)
1511         {
1512                 fs << "v";
1513                 for (int d = 0; d < 3; d++)
1514                 {
1515                         fs << " " << psb->m_nodes[i].m_v[d];
1516                 }
1517                 fs << "\n";
1518         }
1519         fs.close();
1520 }
1521
1522 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
1523 {
1524         std::ifstream fs_read;
1525         fs_read.open(filename);
1526         std::string line;
1527         btVector3 pos;
1528         btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
1529         while (std::getline(fs_read, line))
1530         {
1531                 std::stringstream ss(line);
1532                 if (line[0] == 'v')
1533                 {
1534                 }
1535                 else if (line[0] == 'f')
1536                 {
1537                         ss.ignore();
1538                         int id0, id1, id2;
1539                         ss >> id0;
1540                         ss >> id1;
1541                         ss >> id2;
1542                         btAlignedObjectArray<int> new_face;
1543                         new_face.push_back(id1);
1544                         new_face.push_back(id0);
1545                         new_face.push_back(id2);
1546                         additional_faces.push_back(new_face);
1547                 }
1548         }
1549         fs_read.close();
1550
1551         std::ofstream fs_write;
1552         fs_write.open(filename, std::ios_base::app);
1553         for (int i = 0; i < additional_faces.size(); ++i)
1554         {
1555                 fs_write << "f";
1556                 for (int n = 0; n < 3; n++)
1557                 {
1558                         fs_write << " " << additional_faces[i][n];
1559                 }
1560                 fs_write << "\n";
1561         }
1562         fs_write.close();
1563 }
1564
1565 // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
1566 void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
1567 {
1568         btVector3 vap = p - a;
1569         btVector3 vbp = p - b;
1570
1571         btVector3 vab = b - a;
1572         btVector3 vac = c - a;
1573         btVector3 vad = d - a;
1574
1575         btVector3 vbc = c - b;
1576         btVector3 vbd = d - b;
1577         btScalar va6 = (vbp.cross(vbd)).dot(vbc);
1578         btScalar vb6 = (vap.cross(vac)).dot(vad);
1579         btScalar vc6 = (vap.cross(vad)).dot(vab);
1580         btScalar vd6 = (vap.cross(vab)).dot(vac);
1581         btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
1582         bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
1583 }
1584
1585 // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
1586 void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
1587 {
1588         btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
1589         btScalar d00 = btDot(v0, v0);
1590         btScalar d01 = btDot(v0, v1);
1591         btScalar d11 = btDot(v1, v1);
1592         btScalar d20 = btDot(v2, v0);
1593         btScalar d21 = btDot(v2, v1);
1594         btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
1595         bary[1] = (d11 * d20 - d01 * d21) * invDenom;
1596         bary[2] = (d00 * d21 - d01 * d20) * invDenom;
1597         bary[0] = 1.0 - bary[1] - bary[2];
1598         bary[3] = 0;
1599 }
1600
1601 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
1602 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
1603 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
1604 {
1605         psb->m_z.resize(0);
1606         psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
1607         psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
1608         for (int i = 0; i < psb->m_renderNodes.size(); ++i)
1609         {
1610                 const btVector3& p = psb->m_renderNodes[i].m_x;
1611                 btVector4 bary;
1612                 btVector4 optimal_bary;
1613                 btScalar min_bary_weight = -1e3;
1614                 btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
1615                 for (int j = 0; j < psb->m_tetras.size(); ++j)
1616                 {
1617                         const btSoftBody::Tetra& t = psb->m_tetras[j];
1618                         getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
1619                         btScalar new_min_bary_weight = bary[0];
1620                         for (int k = 1; k < 4; ++k)
1621                         {
1622                                 new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
1623                         }
1624                         if (new_min_bary_weight > min_bary_weight)
1625                         {
1626                                 btAlignedObjectArray<const btSoftBody::Node*> parents;
1627                                 parents.push_back(t.m_n[0]);
1628                                 parents.push_back(t.m_n[1]);
1629                                 parents.push_back(t.m_n[2]);
1630                                 parents.push_back(t.m_n[3]);
1631                                 optimal_parents = parents;
1632                                 optimal_bary = bary;
1633                                 min_bary_weight = new_min_bary_weight;
1634                                 // stop searching if p is inside the tetrahedron at hand
1635                                 if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
1636                                 {
1637                                         break;
1638                                 }
1639                         }
1640                 }
1641                 psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
1642                 psb->m_renderNodesParents[i] = optimal_parents;
1643         }
1644 }
1645
1646 // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
1647 void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
1648 {
1649         psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
1650         psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
1651         psb->m_z.resize(psb->m_renderNodes.size());
1652         for (int i = 0; i < psb->m_renderNodes.size(); ++i)
1653         {
1654                 const btVector3& p = psb->m_renderNodes[i].m_x;
1655                 btVector4 bary;
1656                 btVector4 optimal_bary;
1657                 btScalar min_bary_weight = -SIMD_INFINITY;
1658                 btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
1659                 btScalar dist = 0, optimal_dist = 0;
1660                 for (int j = 0; j < psb->m_faces.size(); ++j)
1661                 {
1662                         const btSoftBody::Face& f = psb->m_faces[j];
1663                         btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
1664                         btVector3 unit_n = n.normalized();
1665                         dist = (p - f.m_n[0]->m_x).dot(unit_n);
1666                         btVector3 proj_p = p - dist * unit_n;
1667                         getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
1668                         btScalar new_min_bary_weight = bary[0];
1669                         for (int k = 1; k < 3; ++k)
1670                         {
1671                                 new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
1672                         }
1673
1674                         // p is out of the current best triangle, we found a traingle that's better
1675                         bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
1676                         // p is inside of the current best triangle, we found a triangle that's better
1677                         bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
1678
1679                         if (better_than_closest_outisde || better_than_best_inside)
1680                         {
1681                                 btAlignedObjectArray<const btSoftBody::Node*> parents;
1682                                 parents.push_back(f.m_n[0]);
1683                                 parents.push_back(f.m_n[1]);
1684                                 parents.push_back(f.m_n[2]);
1685                                 optimal_parents = parents;
1686                                 optimal_bary = bary;
1687                                 optimal_dist = dist;
1688                                 min_bary_weight = new_min_bary_weight;
1689                         }
1690                 }
1691                 psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
1692                 psb->m_renderNodesParents[i] = optimal_parents;
1693                 psb->m_z[i] = optimal_dist;
1694         }
1695 }