1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696 |
- /*
- Bullet Continuous Collision Detection and Physics Library
- Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
- This software is provided 'as-is', without any express or implied warranty.
- In no event will the authors be held liable for any damages arising from the use of this software.
- Permission is granted to anyone to use this software for any purpose,
- including commercial applications, and to alter it and redistribute it freely,
- subject to the following restrictions:
- 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.
- 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
- 3. This notice may not be removed or altered from any source distribution.
- */
- ///btSoftBodyHelpers.cpp by Nathanael Presson
- #include "btSoftBodyInternals.h"
- #include <stdio.h>
- #include <string>
- #include <iostream>
- #include <iomanip>
- #include <sstream>
- #include <string.h>
- #include <algorithm>
- #include "btSoftBodyHelpers.h"
- #include "LinearMath/btConvexHull.h"
- #include "LinearMath/btConvexHullComputer.h"
- #include <map>
- #include <vector>
- static void drawVertex(btIDebugDraw* idraw,
- const btVector3& x, btScalar s, const btVector3& c)
- {
- idraw->drawLine(x - btVector3(s, 0, 0), x + btVector3(s, 0, 0), c);
- idraw->drawLine(x - btVector3(0, s, 0), x + btVector3(0, s, 0), c);
- idraw->drawLine(x - btVector3(0, 0, s), x + btVector3(0, 0, s), c);
- }
- //
- static void drawBox(btIDebugDraw* idraw,
- const btVector3& mins,
- const btVector3& maxs,
- const btVector3& color)
- {
- const btVector3 c[] = {btVector3(mins.x(), mins.y(), mins.z()),
- btVector3(maxs.x(), mins.y(), mins.z()),
- btVector3(maxs.x(), maxs.y(), mins.z()),
- btVector3(mins.x(), maxs.y(), mins.z()),
- btVector3(mins.x(), mins.y(), maxs.z()),
- btVector3(maxs.x(), mins.y(), maxs.z()),
- btVector3(maxs.x(), maxs.y(), maxs.z()),
- btVector3(mins.x(), maxs.y(), maxs.z())};
- idraw->drawLine(c[0], c[1], color);
- idraw->drawLine(c[1], c[2], color);
- idraw->drawLine(c[2], c[3], color);
- idraw->drawLine(c[3], c[0], color);
- idraw->drawLine(c[4], c[5], color);
- idraw->drawLine(c[5], c[6], color);
- idraw->drawLine(c[6], c[7], color);
- idraw->drawLine(c[7], c[4], color);
- idraw->drawLine(c[0], c[4], color);
- idraw->drawLine(c[1], c[5], color);
- idraw->drawLine(c[2], c[6], color);
- idraw->drawLine(c[3], c[7], color);
- }
- //
- static void drawTree(btIDebugDraw* idraw,
- const btDbvtNode* node,
- int depth,
- const btVector3& ncolor,
- const btVector3& lcolor,
- int mindepth,
- int maxdepth)
- {
- if (node)
- {
- if (node->isinternal() && ((depth < maxdepth) || (maxdepth < 0)))
- {
- drawTree(idraw, node->childs[0], depth + 1, ncolor, lcolor, mindepth, maxdepth);
- drawTree(idraw, node->childs[1], depth + 1, ncolor, lcolor, mindepth, maxdepth);
- }
- if (depth >= mindepth)
- {
- const btScalar scl = (btScalar)(node->isinternal() ? 1 : 1);
- const btVector3 mi = node->volume.Center() - node->volume.Extents() * scl;
- const btVector3 mx = node->volume.Center() + node->volume.Extents() * scl;
- drawBox(idraw, mi, mx, node->isleaf() ? lcolor : ncolor);
- }
- }
- }
- //
- template <typename T>
- static inline T sum(const btAlignedObjectArray<T>& items)
- {
- T v;
- if (items.size())
- {
- v = items[0];
- for (int i = 1, ni = items.size(); i < ni; ++i)
- {
- v += items[i];
- }
- }
- return (v);
- }
- //
- template <typename T, typename Q>
- static inline void add(btAlignedObjectArray<T>& items, const Q& value)
- {
- for (int i = 0, ni = items.size(); i < ni; ++i)
- {
- items[i] += value;
- }
- }
- //
- template <typename T, typename Q>
- static inline void mul(btAlignedObjectArray<T>& items, const Q& value)
- {
- for (int i = 0, ni = items.size(); i < ni; ++i)
- {
- items[i] *= value;
- }
- }
- //
- template <typename T>
- static inline T average(const btAlignedObjectArray<T>& items)
- {
- const btScalar n = (btScalar)(items.size() > 0 ? items.size() : 1);
- return (sum(items) / n);
- }
- #if 0
- //
- inline static btScalar tetravolume(const btVector3& x0,
- const btVector3& x1,
- const btVector3& x2,
- const btVector3& x3)
- {
- const btVector3 a=x1-x0;
- const btVector3 b=x2-x0;
- const btVector3 c=x3-x0;
- return(btDot(a,btCross(b,c)));
- }
- #endif
- //
- #if 0
- static btVector3 stresscolor(btScalar stress)
- {
- static const btVector3 spectrum[]= { btVector3(1,0,1),
- btVector3(0,0,1),
- btVector3(0,1,1),
- btVector3(0,1,0),
- btVector3(1,1,0),
- btVector3(1,0,0),
- btVector3(1,0,0)};
- static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
- static const btScalar one=1;
- stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
- const int sel=(int)stress;
- const btScalar frc=stress-sel;
- return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
- }
- #endif
- //
- void btSoftBodyHelpers::Draw(btSoftBody* psb,
- btIDebugDraw* idraw,
- int drawflags)
- {
- const btScalar scl = (btScalar)0.1;
- const btScalar nscl = scl * 5;
- const btVector3 lcolor = btVector3(0, 0, 0);
- const btVector3 ncolor = btVector3(1, 1, 1);
- const btVector3 ccolor = btVector3(1, 0, 0);
- int i, j, nj;
- /* Clusters */
- if (0 != (drawflags & fDrawFlags::Clusters))
- {
- srand(1806);
- for (i = 0; i < psb->m_clusters.size(); ++i)
- {
- if (psb->m_clusters[i]->m_collide)
- {
- btVector3 color(rand() / (btScalar)RAND_MAX,
- rand() / (btScalar)RAND_MAX,
- rand() / (btScalar)RAND_MAX);
- color = color.normalized() * 0.75;
- btAlignedObjectArray<btVector3> vertices;
- vertices.resize(psb->m_clusters[i]->m_nodes.size());
- for (j = 0, nj = vertices.size(); j < nj; ++j)
- {
- vertices[j] = psb->m_clusters[i]->m_nodes[j]->m_x;
- }
- #define USE_NEW_CONVEX_HULL_COMPUTER
- #ifdef USE_NEW_CONVEX_HULL_COMPUTER
- btConvexHullComputer computer;
- int stride = sizeof(btVector3);
- int count = vertices.size();
- btScalar shrink = 0.f;
- btScalar shrinkClamp = 0.f;
- computer.compute(&vertices[0].getX(), stride, count, shrink, shrinkClamp);
- for (int i = 0; i < computer.faces.size(); i++)
- {
- int face = computer.faces[i];
- //printf("face=%d\n",face);
- const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
- const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
- int v0 = firstEdge->getSourceVertex();
- int v1 = firstEdge->getTargetVertex();
- while (edge != firstEdge)
- {
- int v2 = edge->getTargetVertex();
- idraw->drawTriangle(computer.vertices[v0], computer.vertices[v1], computer.vertices[v2], color, 1);
- edge = edge->getNextEdgeOfFace();
- v0 = v1;
- v1 = v2;
- };
- }
- #else
- HullDesc hdsc(QF_TRIANGLES, vertices.size(), &vertices[0]);
- HullResult hres;
- HullLibrary hlib;
- hdsc.mMaxVertices = vertices.size();
- hlib.CreateConvexHull(hdsc, hres);
- const btVector3 center = average(hres.m_OutputVertices);
- add(hres.m_OutputVertices, -center);
- mul(hres.m_OutputVertices, (btScalar)1);
- add(hres.m_OutputVertices, center);
- for (j = 0; j < (int)hres.mNumFaces; ++j)
- {
- const int idx[] = {hres.m_Indices[j * 3 + 0], hres.m_Indices[j * 3 + 1], hres.m_Indices[j * 3 + 2]};
- idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
- hres.m_OutputVertices[idx[1]],
- hres.m_OutputVertices[idx[2]],
- color, 1);
- }
- hlib.ReleaseResult(hres);
- #endif
- }
- /* Velocities */
- #if 0
- for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
- {
- const btSoftBody::Cluster& c=psb->m_clusters[i];
- const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
- const btVector3 v=c.m_lv+btCross(c.m_av,r);
- idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
- }
- #endif
- /* Frame */
- // btSoftBody::Cluster& c=*psb->m_clusters[i];
- // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
- // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
- // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
- }
- }
- else
- {
- /* Nodes */
- if (0 != (drawflags & fDrawFlags::Nodes))
- {
- for (i = 0; i < psb->m_nodes.size(); ++i)
- {
- const btSoftBody::Node& n = psb->m_nodes[i];
- if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- idraw->drawLine(n.m_x - btVector3(scl, 0, 0), n.m_x + btVector3(scl, 0, 0), btVector3(1, 0, 0));
- idraw->drawLine(n.m_x - btVector3(0, scl, 0), n.m_x + btVector3(0, scl, 0), btVector3(0, 1, 0));
- idraw->drawLine(n.m_x - btVector3(0, 0, scl), n.m_x + btVector3(0, 0, scl), btVector3(0, 0, 1));
- }
- }
- /* Links */
- if (0 != (drawflags & fDrawFlags::Links))
- {
- for (i = 0; i < psb->m_links.size(); ++i)
- {
- const btSoftBody::Link& l = psb->m_links[i];
- if (0 == (l.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- idraw->drawLine(l.m_n[0]->m_x, l.m_n[1]->m_x, lcolor);
- }
- }
- /* Normals */
- if (0 != (drawflags & fDrawFlags::Normals))
- {
- for (i = 0; i < psb->m_nodes.size(); ++i)
- {
- const btSoftBody::Node& n = psb->m_nodes[i];
- if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- const btVector3 d = n.m_n * nscl;
- idraw->drawLine(n.m_x, n.m_x + d, ncolor);
- idraw->drawLine(n.m_x, n.m_x - d, ncolor * 0.5);
- }
- }
- /* Contacts */
- if (0 != (drawflags & fDrawFlags::Contacts))
- {
- static const btVector3 axis[] = {btVector3(1, 0, 0),
- btVector3(0, 1, 0),
- btVector3(0, 0, 1)};
- for (i = 0; i < psb->m_rcontacts.size(); ++i)
- {
- const btSoftBody::RContact& c = psb->m_rcontacts[i];
- const btVector3 o = c.m_node->m_x - c.m_cti.m_normal *
- (btDot(c.m_node->m_x, c.m_cti.m_normal) + c.m_cti.m_offset);
- const btVector3 x = btCross(c.m_cti.m_normal, axis[c.m_cti.m_normal.minAxis()]).normalized();
- const btVector3 y = btCross(x, c.m_cti.m_normal).normalized();
- idraw->drawLine(o - x * nscl, o + x * nscl, ccolor);
- idraw->drawLine(o - y * nscl, o + y * nscl, ccolor);
- idraw->drawLine(o, o + c.m_cti.m_normal * nscl * 3, btVector3(1, 1, 0));
- }
- }
- /* Faces */
- if (0 != (drawflags & fDrawFlags::Faces))
- {
- const btScalar scl = (btScalar)0.8;
- const btScalar alp = (btScalar)1;
- const btVector3 col(0, (btScalar)0.7, 0);
- for (i = 0; i < psb->m_faces.size(); ++i)
- {
- const btSoftBody::Face& f = psb->m_faces[i];
- if (0 == (f.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- const btVector3 x[] = {f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x};
- const btVector3 c = (x[0] + x[1] + x[2]) / 3;
- idraw->drawTriangle((x[0] - c) * scl + c,
- (x[1] - c) * scl + c,
- (x[2] - c) * scl + c,
- col, alp);
- }
- }
- /* Tetras */
- if (0 != (drawflags & fDrawFlags::Tetras))
- {
- const btScalar scl = (btScalar)0.8;
- const btScalar alp = (btScalar)1;
- const btVector3 col((btScalar)0.3, (btScalar)0.3, (btScalar)0.7);
- for (int i = 0; i < psb->m_tetras.size(); ++i)
- {
- const btSoftBody::Tetra& t = psb->m_tetras[i];
- if (0 == (t.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- 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};
- const btVector3 c = (x[0] + x[1] + x[2] + x[3]) / 4;
- idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[2] - c) * scl + c, col, alp);
- idraw->drawTriangle((x[0] - c) * scl + c, (x[1] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
- idraw->drawTriangle((x[1] - c) * scl + c, (x[2] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
- idraw->drawTriangle((x[2] - c) * scl + c, (x[0] - c) * scl + c, (x[3] - c) * scl + c, col, alp);
- }
- }
- }
- /* Anchors */
- if (0 != (drawflags & fDrawFlags::Anchors))
- {
- for (i = 0; i < psb->m_anchors.size(); ++i)
- {
- const btSoftBody::Anchor& a = psb->m_anchors[i];
- const btVector3 q = a.m_body->getWorldTransform() * a.m_local;
- drawVertex(idraw, a.m_node->m_x, 0.25, btVector3(1, 0, 0));
- drawVertex(idraw, q, 0.25, btVector3(0, 1, 0));
- idraw->drawLine(a.m_node->m_x, q, btVector3(1, 1, 1));
- }
- for (i = 0; i < psb->m_nodes.size(); ++i)
- {
- const btSoftBody::Node& n = psb->m_nodes[i];
- if (0 == (n.m_material->m_flags & btSoftBody::fMaterial::DebugDraw)) continue;
- if (n.m_im <= 0)
- {
- drawVertex(idraw, n.m_x, 0.25, btVector3(1, 0, 0));
- }
- }
- }
- /* Notes */
- if (0 != (drawflags & fDrawFlags::Notes))
- {
- for (i = 0; i < psb->m_notes.size(); ++i)
- {
- const btSoftBody::Note& n = psb->m_notes[i];
- btVector3 p = n.m_offset;
- for (int j = 0; j < n.m_rank; ++j)
- {
- p += n.m_nodes[j]->m_x * n.m_coords[j];
- }
- idraw->draw3dText(p, n.m_text);
- }
- }
- /* Node tree */
- if (0 != (drawflags & fDrawFlags::NodeTree)) DrawNodeTree(psb, idraw);
- /* Face tree */
- if (0 != (drawflags & fDrawFlags::FaceTree)) DrawFaceTree(psb, idraw);
- /* Cluster tree */
- if (0 != (drawflags & fDrawFlags::ClusterTree)) DrawClusterTree(psb, idraw);
- /* Joints */
- if (0 != (drawflags & fDrawFlags::Joints))
- {
- for (i = 0; i < psb->m_joints.size(); ++i)
- {
- const btSoftBody::Joint* pj = psb->m_joints[i];
- switch (pj->Type())
- {
- case btSoftBody::Joint::eType::Linear:
- {
- const btSoftBody::LJoint* pjl = (const btSoftBody::LJoint*)pj;
- const btVector3 a0 = pj->m_bodies[0].xform() * pjl->m_refs[0];
- const btVector3 a1 = pj->m_bodies[1].xform() * pjl->m_refs[1];
- idraw->drawLine(pj->m_bodies[0].xform().getOrigin(), a0, btVector3(1, 1, 0));
- idraw->drawLine(pj->m_bodies[1].xform().getOrigin(), a1, btVector3(0, 1, 1));
- drawVertex(idraw, a0, 0.25, btVector3(1, 1, 0));
- drawVertex(idraw, a1, 0.25, btVector3(0, 1, 1));
- }
- break;
- case btSoftBody::Joint::eType::Angular:
- {
- //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
- const btVector3 o0 = pj->m_bodies[0].xform().getOrigin();
- const btVector3 o1 = pj->m_bodies[1].xform().getOrigin();
- const btVector3 a0 = pj->m_bodies[0].xform().getBasis() * pj->m_refs[0];
- const btVector3 a1 = pj->m_bodies[1].xform().getBasis() * pj->m_refs[1];
- idraw->drawLine(o0, o0 + a0 * 10, btVector3(1, 1, 0));
- idraw->drawLine(o0, o0 + a1 * 10, btVector3(1, 1, 0));
- idraw->drawLine(o1, o1 + a0 * 10, btVector3(0, 1, 1));
- idraw->drawLine(o1, o1 + a1 * 10, btVector3(0, 1, 1));
- break;
- }
- default:
- {
- }
- }
- }
- }
- }
- //
- void btSoftBodyHelpers::DrawInfos(btSoftBody* psb,
- btIDebugDraw* idraw,
- bool masses,
- bool areas,
- bool /*stress*/)
- {
- for (int i = 0; i < psb->m_nodes.size(); ++i)
- {
- const btSoftBody::Node& n = psb->m_nodes[i];
- char text[2048] = {0};
- char buff[1024];
- if (masses)
- {
- sprintf(buff, " M(%.2f)", 1 / n.m_im);
- strcat(text, buff);
- }
- if (areas)
- {
- sprintf(buff, " A(%.2f)", n.m_area);
- strcat(text, buff);
- }
- if (text[0]) idraw->draw3dText(n.m_x, text);
- }
- }
- //
- void btSoftBodyHelpers::DrawNodeTree(btSoftBody* psb,
- btIDebugDraw* idraw,
- int mindepth,
- int maxdepth)
- {
- drawTree(idraw, psb->m_ndbvt.m_root, 0, btVector3(1, 0, 1), btVector3(1, 1, 1), mindepth, maxdepth);
- }
- //
- void btSoftBodyHelpers::DrawFaceTree(btSoftBody* psb,
- btIDebugDraw* idraw,
- int mindepth,
- int maxdepth)
- {
- drawTree(idraw, psb->m_fdbvt.m_root, 0, btVector3(0, 1, 0), btVector3(1, 0, 0), mindepth, maxdepth);
- }
- //
- void btSoftBodyHelpers::DrawClusterTree(btSoftBody* psb,
- btIDebugDraw* idraw,
- int mindepth,
- int maxdepth)
- {
- drawTree(idraw, psb->m_cdbvt.m_root, 0, btVector3(0, 1, 1), btVector3(1, 0, 0), mindepth, maxdepth);
- }
- //The btSoftBody object from the BulletSDK includes an array of Nodes and Links. These links appear
- // to be first set up to connect a node to between 5 and 6 of its neighbors [480 links],
- //and then to the rest of the nodes after the execution of the Floyd-Warshall graph algorithm
- //[another 930 links].
- //The way the links are stored by default, we have a number of cases where adjacent links share a node in common
- // - this leads to the creation of a data dependency through memory.
- //The PSolve_Links() function reads and writes nodes as it iterates over each link.
- //So, we now have the possibility of a data dependency between iteration X
- //that processes link L with iteration X+1 that processes link L+1
- //because L and L+1 have one node in common, and iteration X updates the positions of that node,
- //and iteration X+1 reads in the position of that shared node.
- //
- //Such a memory dependency limits the ability of a modern CPU to speculate beyond
- //a certain point because it has to respect a possible dependency
- //- this prevents the CPU from making full use of its out-of-order resources.
- //If we re-order the links such that we minimize the cases where a link L and L+1 share a common node,
- //we create a temporal gap between when the node position is written,
- //and when it is subsequently read. This in turn allows the CPU to continue execution without
- //risking a dependency violation. Such a reordering would result in significant speedups on
- //modern CPUs with lots of execution resources.
- //In our testing, we see it have a tremendous impact not only on the A7,
- //but also on all x86 cores that ship with modern Macs.
- //The attached source file includes a single function (ReoptimizeLinkOrder) which can be called on a
- //btSoftBody object in the solveConstraints() function before the actual solver is invoked,
- //or right after generateBendingConstraints() once we have all 1410 links.
- //===================================================================
- //
- //
- // This function takes in a list of interdependent Links and tries
- // to maximize the distance between calculation
- // of dependent links. This increases the amount of parallelism that can
- // be exploited by out-of-order instruction processors with large but
- // (inevitably) finite instruction windows.
- //
- //===================================================================
- // A small structure to track lists of dependent link calculations
- class LinkDeps_t
- {
- public:
- int value; // A link calculation that is dependent on this one
- // Positive values = "input A" while negative values = "input B"
- LinkDeps_t* next; // Next dependence in the list
- };
- typedef LinkDeps_t* LinkDepsPtr_t;
- // Dependency list constants
- #define REOP_NOT_DEPENDENT -1
- #define REOP_NODE_COMPLETE -2 // Must be less than REOP_NOT_DEPENDENT
- void btSoftBodyHelpers::ReoptimizeLinkOrder(btSoftBody* psb /* This can be replaced by a btSoftBody pointer */)
- {
- int i, nLinks = psb->m_links.size(), nNodes = psb->m_nodes.size();
- btSoftBody::Link* lr;
- int ar, br;
- btSoftBody::Node* node0 = &(psb->m_nodes[0]);
- btSoftBody::Node* node1 = &(psb->m_nodes[1]);
- LinkDepsPtr_t linkDep;
- int readyListHead, readyListTail, linkNum, linkDepFrees, depLink;
- // Allocate temporary buffers
- int* nodeWrittenAt = new int[nNodes + 1]; // What link calculation produced this node's current values?
- int* linkDepA = new int[nLinks]; // Link calculation input is dependent upon prior calculation #N
- int* linkDepB = new int[nLinks];
- int* readyList = new int[nLinks]; // List of ready-to-process link calculations (# of links, maximum)
- LinkDeps_t* linkDepFreeList = new LinkDeps_t[2 * nLinks]; // Dependent-on-me list elements (2x# of links, maximum)
- LinkDepsPtr_t* linkDepListStarts = new LinkDepsPtr_t[nLinks]; // Start nodes of dependent-on-me lists, one for each link
- // Copy the original, unsorted links to a side buffer
- btSoftBody::Link* linkBuffer = new btSoftBody::Link[nLinks];
- memcpy(linkBuffer, &(psb->m_links[0]), sizeof(btSoftBody::Link) * nLinks);
- // Clear out the node setup and ready list
- for (i = 0; i < nNodes + 1; i++)
- {
- nodeWrittenAt[i] = REOP_NOT_DEPENDENT;
- }
- for (i = 0; i < nLinks; i++)
- {
- linkDepListStarts[i] = NULL;
- }
- readyListHead = readyListTail = linkDepFrees = 0;
- // Initial link analysis to set up data structures
- for (i = 0; i < nLinks; i++)
- {
- // Note which prior link calculations we are dependent upon & build up dependence lists
- lr = &(psb->m_links[i]);
- ar = (lr->m_n[0] - node0) / (node1 - node0);
- br = (lr->m_n[1] - node0) / (node1 - node0);
- if (nodeWrittenAt[ar] > REOP_NOT_DEPENDENT)
- {
- linkDepA[i] = nodeWrittenAt[ar];
- linkDep = &linkDepFreeList[linkDepFrees++];
- linkDep->value = i;
- linkDep->next = linkDepListStarts[nodeWrittenAt[ar]];
- linkDepListStarts[nodeWrittenAt[ar]] = linkDep;
- }
- else
- {
- linkDepA[i] = REOP_NOT_DEPENDENT;
- }
- if (nodeWrittenAt[br] > REOP_NOT_DEPENDENT)
- {
- linkDepB[i] = nodeWrittenAt[br];
- linkDep = &linkDepFreeList[linkDepFrees++];
- linkDep->value = -(i + 1);
- linkDep->next = linkDepListStarts[nodeWrittenAt[br]];
- linkDepListStarts[nodeWrittenAt[br]] = linkDep;
- }
- else
- {
- linkDepB[i] = REOP_NOT_DEPENDENT;
- }
- // Add this link to the initial ready list, if it is not dependent on any other links
- if ((linkDepA[i] == REOP_NOT_DEPENDENT) && (linkDepB[i] == REOP_NOT_DEPENDENT))
- {
- readyList[readyListTail++] = i;
- linkDepA[i] = linkDepB[i] = REOP_NODE_COMPLETE; // Probably not needed now
- }
- // Update the nodes to mark which ones are calculated by this link
- nodeWrittenAt[ar] = nodeWrittenAt[br] = i;
- }
- // Process the ready list and create the sorted list of links
- // -- By treating the ready list as a queue, we maximize the distance between any
- // inter-dependent node calculations
- // -- All other (non-related) nodes in the ready list will automatically be inserted
- // in between each set of inter-dependent link calculations by this loop
- i = 0;
- while (readyListHead != readyListTail)
- {
- // Use ready list to select the next link to process
- linkNum = readyList[readyListHead++];
- // Copy the next-to-calculate link back into the original link array
- psb->m_links[i++] = linkBuffer[linkNum];
- // Free up any link inputs that are dependent on this one
- linkDep = linkDepListStarts[linkNum];
- while (linkDep)
- {
- depLink = linkDep->value;
- if (depLink >= 0)
- {
- linkDepA[depLink] = REOP_NOT_DEPENDENT;
- }
- else
- {
- depLink = -depLink - 1;
- linkDepB[depLink] = REOP_NOT_DEPENDENT;
- }
- // Add this dependent link calculation to the ready list if *both* inputs are clear
- if ((linkDepA[depLink] == REOP_NOT_DEPENDENT) && (linkDepB[depLink] == REOP_NOT_DEPENDENT))
- {
- readyList[readyListTail++] = depLink;
- linkDepA[depLink] = linkDepB[depLink] = REOP_NODE_COMPLETE; // Probably not needed now
- }
- linkDep = linkDep->next;
- }
- }
- // Delete the temporary buffers
- delete[] nodeWrittenAt;
- delete[] linkDepA;
- delete[] linkDepB;
- delete[] readyList;
- delete[] linkDepFreeList;
- delete[] linkDepListStarts;
- delete[] linkBuffer;
- }
- //
- void btSoftBodyHelpers::DrawFrame(btSoftBody* psb,
- btIDebugDraw* idraw)
- {
- if (psb->m_pose.m_bframe)
- {
- static const btScalar ascl = 10;
- static const btScalar nscl = (btScalar)0.1;
- const btVector3 com = psb->m_pose.m_com;
- const btMatrix3x3 trs = psb->m_pose.m_rot * psb->m_pose.m_scl;
- const btVector3 Xaxis = (trs * btVector3(1, 0, 0)).normalized();
- const btVector3 Yaxis = (trs * btVector3(0, 1, 0)).normalized();
- const btVector3 Zaxis = (trs * btVector3(0, 0, 1)).normalized();
- idraw->drawLine(com, com + Xaxis * ascl, btVector3(1, 0, 0));
- idraw->drawLine(com, com + Yaxis * ascl, btVector3(0, 1, 0));
- idraw->drawLine(com, com + Zaxis * ascl, btVector3(0, 0, 1));
- for (int i = 0; i < psb->m_pose.m_pos.size(); ++i)
- {
- const btVector3 x = com + trs * psb->m_pose.m_pos[i];
- drawVertex(idraw, x, nscl, btVector3(1, 0, 1));
- }
- }
- }
- //
- btSoftBody* btSoftBodyHelpers::CreateRope(btSoftBodyWorldInfo& worldInfo, const btVector3& from,
- const btVector3& to,
- int res,
- int fixeds)
- {
- /* Create nodes */
- const int r = res + 2;
- btVector3* x = new btVector3[r];
- btScalar* m = new btScalar[r];
- int i;
- for (i = 0; i < r; ++i)
- {
- const btScalar t = i / (btScalar)(r - 1);
- x[i] = lerp(from, to, t);
- m[i] = 1;
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, r, x, m);
- if (fixeds & 1) psb->setMass(0, 0);
- if (fixeds & 2) psb->setMass(r - 1, 0);
- delete[] x;
- delete[] m;
- /* Create links */
- for (i = 1; i < r; ++i)
- {
- psb->appendLink(i - 1, i);
- }
- /* Finished */
- return (psb);
- }
- //
- btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00,
- const btVector3& corner10,
- const btVector3& corner01,
- const btVector3& corner11,
- int resx,
- int resy,
- int fixeds,
- bool gendiags,
- btScalar perturbation)
- {
- #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
- /* Create nodes */
- if ((resx < 2) || (resy < 2)) return (0);
- const int rx = resx;
- const int ry = resy;
- const int tot = rx * ry;
- btVector3* x = new btVector3[tot];
- btScalar* m = new btScalar[tot];
- int iy;
- for (iy = 0; iy < ry; ++iy)
- {
- const btScalar ty = iy / (btScalar)(ry - 1);
- const btVector3 py0 = lerp(corner00, corner01, ty);
- const btVector3 py1 = lerp(corner10, corner11, ty);
- for (int ix = 0; ix < rx; ++ix)
- {
- const btScalar tx = ix / (btScalar)(rx - 1);
- btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
- btVector3 temp1 = py1;
- temp1.setY(py1.getY() + pert);
- btVector3 temp = py0;
- pert = perturbation * btScalar(rand()) / RAND_MAX;
- temp.setY(py0.getY() + pert);
- x[IDX(ix, iy)] = lerp(temp, temp1, tx);
- m[IDX(ix, iy)] = 1;
- }
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
- if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
- if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
- if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
- if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
- delete[] x;
- delete[] m;
- /* Create links and faces */
- for (iy = 0; iy < ry; ++iy)
- {
- for (int ix = 0; ix < rx; ++ix)
- {
- const int idx = IDX(ix, iy);
- const bool mdx = (ix + 1) < rx;
- const bool mdy = (iy + 1) < ry;
- if (mdx) psb->appendLink(idx, IDX(ix + 1, iy));
- if (mdy) psb->appendLink(idx, IDX(ix, iy + 1));
- if (mdx && mdy)
- {
- if ((ix + iy) & 1)
- {
- psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
- psb->appendFace(IDX(ix, iy), IDX(ix + 1, iy + 1), IDX(ix, iy + 1));
- if (gendiags)
- {
- psb->appendLink(IDX(ix, iy), IDX(ix + 1, iy + 1));
- }
- }
- else
- {
- psb->appendFace(IDX(ix, iy + 1), IDX(ix, iy), IDX(ix + 1, iy));
- psb->appendFace(IDX(ix, iy + 1), IDX(ix + 1, iy), IDX(ix + 1, iy + 1));
- if (gendiags)
- {
- psb->appendLink(IDX(ix + 1, iy), IDX(ix, iy + 1));
- }
- }
- }
- }
- }
- /* Finished */
- #undef IDX
- return (psb);
- }
- //
- btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
- const btVector3& corner00,
- const btVector3& corner10,
- const btVector3& corner01,
- const btVector3& corner11,
- int resx,
- int resy,
- int fixeds,
- bool gendiags,
- float* tex_coords)
- {
- /*
- *
- * corners:
- *
- * [0][0] corner00 ------- corner01 [resx][0]
- * | |
- * | |
- * [0][resy] corner10 -------- corner11 [resx][resy]
- *
- *
- *
- *
- *
- *
- * "fixedgs" map:
- *
- * corner00 --> +1
- * corner01 --> +2
- * corner10 --> +4
- * corner11 --> +8
- * upper middle --> +16
- * left middle --> +32
- * right middle --> +64
- * lower middle --> +128
- * center --> +256
- *
- *
- * tex_coords size (resx-1)*(resy-1)*12
- *
- *
- *
- * SINGLE QUAD INTERNALS
- *
- * 1) btSoftBody's nodes and links,
- * diagonal link is optional ("gendiags")
- *
- *
- * node00 ------ node01
- * | .
- * | .
- * | .
- * | .
- * | .
- * node10 node11
- *
- *
- *
- * 2) Faces:
- * two triangles,
- * UV Coordinates (hier example for single quad)
- *
- * (0,1) (0,1) (1,1)
- * 1 |\ 3 \-----| 2
- * | \ \ |
- * | \ \ |
- * | \ \ |
- * | \ \ |
- * 2 |-----\ 3 \| 1
- * (0,0) (1,0) (1,0)
- *
- *
- *
- *
- *
- *
- */
- #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
- /* Create nodes */
- if ((resx < 2) || (resy < 2)) return (0);
- const int rx = resx;
- const int ry = resy;
- const int tot = rx * ry;
- btVector3* x = new btVector3[tot];
- btScalar* m = new btScalar[tot];
- int iy;
- for (iy = 0; iy < ry; ++iy)
- {
- const btScalar ty = iy / (btScalar)(ry - 1);
- const btVector3 py0 = lerp(corner00, corner01, ty);
- const btVector3 py1 = lerp(corner10, corner11, ty);
- for (int ix = 0; ix < rx; ++ix)
- {
- const btScalar tx = ix / (btScalar)(rx - 1);
- x[IDX(ix, iy)] = lerp(py0, py1, tx);
- m[IDX(ix, iy)] = 1;
- }
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, tot, x, m);
- if (fixeds & 1) psb->setMass(IDX(0, 0), 0);
- if (fixeds & 2) psb->setMass(IDX(rx - 1, 0), 0);
- if (fixeds & 4) psb->setMass(IDX(0, ry - 1), 0);
- if (fixeds & 8) psb->setMass(IDX(rx - 1, ry - 1), 0);
- if (fixeds & 16) psb->setMass(IDX((rx - 1) / 2, 0), 0);
- if (fixeds & 32) psb->setMass(IDX(0, (ry - 1) / 2), 0);
- if (fixeds & 64) psb->setMass(IDX(rx - 1, (ry - 1) / 2), 0);
- if (fixeds & 128) psb->setMass(IDX((rx - 1) / 2, ry - 1), 0);
- if (fixeds & 256) psb->setMass(IDX((rx - 1) / 2, (ry - 1) / 2), 0);
- delete[] x;
- delete[] m;
- int z = 0;
- /* Create links and faces */
- for (iy = 0; iy < ry; ++iy)
- {
- for (int ix = 0; ix < rx; ++ix)
- {
- const bool mdx = (ix + 1) < rx;
- const bool mdy = (iy + 1) < ry;
- int node00 = IDX(ix, iy);
- int node01 = IDX(ix + 1, iy);
- int node10 = IDX(ix, iy + 1);
- int node11 = IDX(ix + 1, iy + 1);
- if (mdx) psb->appendLink(node00, node01);
- if (mdy) psb->appendLink(node00, node10);
- if (mdx && mdy)
- {
- psb->appendFace(node00, node10, node11);
- if (tex_coords)
- {
- tex_coords[z + 0] = CalculateUV(resx, resy, ix, iy, 0);
- tex_coords[z + 1] = CalculateUV(resx, resy, ix, iy, 1);
- tex_coords[z + 2] = CalculateUV(resx, resy, ix, iy, 0);
- tex_coords[z + 3] = CalculateUV(resx, resy, ix, iy, 2);
- tex_coords[z + 4] = CalculateUV(resx, resy, ix, iy, 3);
- tex_coords[z + 5] = CalculateUV(resx, resy, ix, iy, 2);
- }
- psb->appendFace(node11, node01, node00);
- if (tex_coords)
- {
- tex_coords[z + 6] = CalculateUV(resx, resy, ix, iy, 3);
- tex_coords[z + 7] = CalculateUV(resx, resy, ix, iy, 2);
- tex_coords[z + 8] = CalculateUV(resx, resy, ix, iy, 3);
- tex_coords[z + 9] = CalculateUV(resx, resy, ix, iy, 1);
- tex_coords[z + 10] = CalculateUV(resx, resy, ix, iy, 0);
- tex_coords[z + 11] = CalculateUV(resx, resy, ix, iy, 1);
- }
- if (gendiags) psb->appendLink(node00, node11);
- z += 12;
- }
- }
- }
- /* Finished */
- #undef IDX
- return (psb);
- }
- float btSoftBodyHelpers::CalculateUV(int resx, int resy, int ix, int iy, int id)
- {
- /*
- *
- *
- * node00 --- node01
- * | |
- * node10 --- node11
- *
- *
- * ID map:
- *
- * node00 s --> 0
- * node00 t --> 1
- *
- * node01 s --> 3
- * node01 t --> 1
- *
- * node10 s --> 0
- * node10 t --> 2
- *
- * node11 s --> 3
- * node11 t --> 2
- *
- *
- */
- float tc = 0.0f;
- if (id == 0)
- {
- tc = (1.0f / ((resx - 1)) * ix);
- }
- else if (id == 1)
- {
- tc = (1.0f / ((resy - 1)) * (resy - 1 - iy));
- }
- else if (id == 2)
- {
- tc = (1.0f / ((resy - 1)) * (resy - 1 - iy - 1));
- }
- else if (id == 3)
- {
- tc = (1.0f / ((resx - 1)) * (ix + 1));
- }
- return tc;
- }
- //
- btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, const btVector3& center,
- const btVector3& radius,
- int res)
- {
- struct Hammersley
- {
- static void Generate(btVector3* x, int n)
- {
- for (int i = 0; i < n; i++)
- {
- btScalar p = 0.5, t = 0;
- for (int j = i; j; p *= 0.5, j >>= 1)
- if (j & 1) t += p;
- btScalar w = 2 * t - 1;
- btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n;
- btScalar s = btSqrt(1 - w * w);
- *x++ = btVector3(s * btCos(a), s * btSin(a), w);
- }
- }
- };
- btAlignedObjectArray<btVector3> vtx;
- vtx.resize(3 + res);
- Hammersley::Generate(&vtx[0], vtx.size());
- for (int i = 0; i < vtx.size(); ++i)
- {
- vtx[i] = vtx[i] * radius + center;
- }
- return (CreateFromConvexHull(worldInfo, &vtx[0], vtx.size()));
- }
- //
- btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices,
- const int* triangles,
- int ntriangles, bool randomizeConstraints)
- {
- int maxidx = 0;
- int i, j, ni;
- for (i = 0, ni = ntriangles * 3; i < ni; ++i)
- {
- maxidx = btMax(triangles[i], maxidx);
- }
- ++maxidx;
- btAlignedObjectArray<bool> chks;
- btAlignedObjectArray<btVector3> vtx;
- chks.resize(maxidx * maxidx, false);
- vtx.resize(maxidx);
- for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3)
- {
- vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]);
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0);
- for (i = 0, ni = ntriangles * 3; i < ni; i += 3)
- {
- const int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]};
- #define IDX(_x_, _y_) ((_y_)*maxidx + (_x_))
- for (int j = 2, k = 0; k < 3; j = k++)
- {
- if (!chks[IDX(idx[j], idx[k])])
- {
- chks[IDX(idx[j], idx[k])] = true;
- chks[IDX(idx[k], idx[j])] = true;
- psb->appendLink(idx[j], idx[k]);
- }
- }
- #undef IDX
- psb->appendFace(idx[0], idx[1], idx[2]);
- }
- if (randomizeConstraints)
- {
- psb->randomizeConstraints();
- }
- return (psb);
- }
- //
- btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
- int nvertices, bool randomizeConstraints)
- {
- HullDesc hdsc(QF_TRIANGLES, nvertices, vertices);
- HullResult hres;
- HullLibrary hlib; /*??*/
- hdsc.mMaxVertices = nvertices;
- hlib.CreateConvexHull(hdsc, hres);
- btSoftBody* psb = new btSoftBody(&worldInfo, (int)hres.mNumOutputVertices,
- &hres.m_OutputVertices[0], 0);
- for (int i = 0; i < (int)hres.mNumFaces; ++i)
- {
- const int idx[] = {static_cast<int>(hres.m_Indices[i * 3 + 0]),
- static_cast<int>(hres.m_Indices[i * 3 + 1]),
- static_cast<int>(hres.m_Indices[i * 3 + 2])};
- if (idx[0] < idx[1]) psb->appendLink(idx[0], idx[1]);
- if (idx[1] < idx[2]) psb->appendLink(idx[1], idx[2]);
- if (idx[2] < idx[0]) psb->appendLink(idx[2], idx[0]);
- psb->appendFace(idx[0], idx[1], idx[2]);
- }
- hlib.ReleaseResult(hres);
- if (randomizeConstraints)
- {
- psb->randomizeConstraints();
- }
- return (psb);
- }
- static int nextLine(const char* buffer)
- {
- int numBytesRead = 0;
- while (*buffer != '\n')
- {
- buffer++;
- numBytesRead++;
- }
- if (buffer[0] == 0x0a)
- {
- buffer++;
- numBytesRead++;
- }
- return numBytesRead;
- }
- /* Create from TetGen .ele, .face, .node data */
- btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
- const char* ele,
- const char* face,
- const char* node,
- bool bfacelinks,
- bool btetralinks,
- bool bfacesfromtetras)
- {
- btAlignedObjectArray<btVector3> pos;
- int nnode = 0;
- int ndims = 0;
- int nattrb = 0;
- int hasbounds = 0;
- int result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
- result = sscanf(node, "%d %d %d %d", &nnode, &ndims, &nattrb, &hasbounds);
- node += nextLine(node);
- pos.resize(nnode);
- for (int i = 0; i < pos.size(); ++i)
- {
- int index = 0;
- //int bound=0;
- float x, y, z;
- sscanf(node, "%d %f %f %f", &index, &x, &y, &z);
- // sn>>index;
- // sn>>x;sn>>y;sn>>z;
- node += nextLine(node);
- //for(int j=0;j<nattrb;++j)
- // sn>>a;
- //if(hasbounds)
- // sn>>bound;
- pos[index].setX(btScalar(x));
- pos[index].setY(btScalar(y));
- pos[index].setZ(btScalar(z));
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, nnode, &pos[0], 0);
- #if 0
- if(face&&face[0])
- {
- int nface=0;
- sf>>nface;sf>>hasbounds;
- for(int i=0;i<nface;++i)
- {
- int index=0;
- int bound=0;
- int ni[3];
- sf>>index;
- sf>>ni[0];sf>>ni[1];sf>>ni[2];
- sf>>bound;
- psb->appendFace(ni[0],ni[1],ni[2]);
- if(btetralinks)
- {
- psb->appendLink(ni[0],ni[1],0,true);
- psb->appendLink(ni[1],ni[2],0,true);
- psb->appendLink(ni[2],ni[0],0,true);
- }
- }
- }
- #endif
- if (ele && ele[0])
- {
- int ntetra = 0;
- int ncorner = 0;
- int neattrb = 0;
- sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb);
- ele += nextLine(ele);
- //se>>ntetra;se>>ncorner;se>>neattrb;
- for (int i = 0; i < ntetra; ++i)
- {
- int index = 0;
- int ni[4];
- //se>>index;
- //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
- sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
- ele += nextLine(ele);
- //for(int j=0;j<neattrb;++j)
- // se>>a;
- psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
- if (btetralinks)
- {
- psb->appendLink(ni[0], ni[1], 0, true);
- psb->appendLink(ni[1], ni[2], 0, true);
- psb->appendLink(ni[2], ni[0], 0, true);
- psb->appendLink(ni[0], ni[3], 0, true);
- psb->appendLink(ni[1], ni[3], 0, true);
- psb->appendLink(ni[2], ni[3], 0, true);
- }
- }
- }
- psb->initializeDmInverse();
- psb->m_tetraScratches.resize(psb->m_tetras.size());
- psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
- printf("Nodes: %u\r\n", psb->m_nodes.size());
- printf("Links: %u\r\n", psb->m_links.size());
- printf("Faces: %u\r\n", psb->m_faces.size());
- printf("Tetras: %u\r\n", psb->m_tetras.size());
- return (psb);
- }
- btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
- {
- std::ifstream fs;
- fs.open(vtk_file);
- btAssert(fs);
- typedef btAlignedObjectArray<int> Index;
- std::string line;
- btAlignedObjectArray<btVector3> X;
- btVector3 position;
- btAlignedObjectArray<Index> indices;
- bool reading_points = false;
- bool reading_tets = false;
- size_t n_points = 0;
- size_t n_tets = 0;
- size_t x_count = 0;
- size_t indices_count = 0;
- while (std::getline(fs, line))
- {
- std::stringstream ss(line);
- if (line.size() == (size_t)(0))
- {
- }
- else if (line.substr(0, 6) == "POINTS")
- {
- reading_points = true;
- reading_tets = false;
- ss.ignore(128, ' '); // ignore "POINTS"
- ss >> n_points;
- X.resize(n_points);
- }
- else if (line.substr(0, 5) == "CELLS")
- {
- reading_points = false;
- reading_tets = true;
- ss.ignore(128, ' '); // ignore "CELLS"
- ss >> n_tets;
- indices.resize(n_tets);
- }
- else if (line.substr(0, 10) == "CELL_TYPES")
- {
- reading_points = false;
- reading_tets = false;
- }
- else if (reading_points)
- {
- btScalar p;
- ss >> p;
- position.setX(p);
- ss >> p;
- position.setY(p);
- ss >> p;
- position.setZ(p);
- //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
- X[x_count++] = position;
- }
- else if (reading_tets)
- {
- int d;
- ss >> d;
- if (d != 4)
- {
- printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
- fs.close();
- return 0;
- }
- ss.ignore(128, ' '); // ignore "4"
- Index tet;
- tet.resize(4);
- for (size_t i = 0; i < 4; i++)
- {
- ss >> tet[i];
- //printf("%d ", tet[i]);
- }
- //printf("\n");
- indices[indices_count++] = tet;
- }
- }
- btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
- for (int i = 0; i < n_tets; ++i)
- {
- const Index& ni = indices[i];
- psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
- {
- psb->appendLink(ni[0], ni[1], 0, true);
- psb->appendLink(ni[1], ni[2], 0, true);
- psb->appendLink(ni[2], ni[0], 0, true);
- psb->appendLink(ni[0], ni[3], 0, true);
- psb->appendLink(ni[1], ni[3], 0, true);
- psb->appendLink(ni[2], ni[3], 0, true);
- }
- }
- generateBoundaryFaces(psb);
- psb->initializeDmInverse();
- psb->m_tetraScratches.resize(psb->m_tetras.size());
- psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
- printf("Nodes: %u\r\n", psb->m_nodes.size());
- printf("Links: %u\r\n", psb->m_links.size());
- printf("Faces: %u\r\n", psb->m_faces.size());
- printf("Tetras: %u\r\n", psb->m_tetras.size());
- fs.close();
- return psb;
- }
- void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
- {
- int counter = 0;
- for (int i = 0; i < psb->m_nodes.size(); ++i)
- {
- psb->m_nodes[i].index = counter++;
- }
- typedef btAlignedObjectArray<int> Index;
- btAlignedObjectArray<Index> indices;
- indices.resize(psb->m_tetras.size());
- for (int i = 0; i < indices.size(); ++i)
- {
- Index index;
- index.push_back(psb->m_tetras[i].m_n[0]->index);
- index.push_back(psb->m_tetras[i].m_n[1]->index);
- index.push_back(psb->m_tetras[i].m_n[2]->index);
- index.push_back(psb->m_tetras[i].m_n[3]->index);
- indices[i] = index;
- }
- std::map<std::vector<int>, std::vector<int> > dict;
- for (int i = 0; i < indices.size(); ++i)
- {
- for (int j = 0; j < 4; ++j)
- {
- std::vector<int> f;
- if (j == 0)
- {
- f.push_back(indices[i][1]);
- f.push_back(indices[i][0]);
- f.push_back(indices[i][2]);
- }
- if (j == 1)
- {
- f.push_back(indices[i][3]);
- f.push_back(indices[i][0]);
- f.push_back(indices[i][1]);
- }
- if (j == 2)
- {
- f.push_back(indices[i][3]);
- f.push_back(indices[i][1]);
- f.push_back(indices[i][2]);
- }
- if (j == 3)
- {
- f.push_back(indices[i][2]);
- f.push_back(indices[i][0]);
- f.push_back(indices[i][3]);
- }
- std::vector<int> f_sorted = f;
- std::sort(f_sorted.begin(), f_sorted.end());
- if (dict.find(f_sorted) != dict.end())
- {
- dict.erase(f_sorted);
- }
- else
- {
- dict.insert(std::make_pair(f_sorted, f));
- }
- }
- }
- for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
- {
- std::vector<int> f = it->second;
- psb->appendFace(f[0], f[1], f[2]);
- //printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1);
- }
- }
- //Write the surface mesh to an obj file.
- void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
- {
- std::ofstream fs;
- fs.open(filename);
- btAssert(fs);
- if (psb->m_tetras.size() > 0)
- {
- // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
- std::map<int, int> dict;
- for (int i = 0; i < psb->m_faces.size(); i++)
- {
- for (int d = 0; d < 3; d++)
- {
- int index = psb->m_faces[i].m_n[d]->index;
- if (dict.find(index) == dict.end())
- {
- int dict_size = dict.size();
- dict[index] = dict_size;
- fs << "v";
- for (int k = 0; k < 3; k++)
- {
- fs << " " << psb->m_nodes[index].m_x[k];
- }
- fs << "\n";
- }
- }
- }
- // Write surface mesh.
- for (int i = 0; i < psb->m_faces.size(); ++i)
- {
- fs << "f";
- for (int n = 0; n < 3; n++)
- {
- fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
- }
- fs << "\n";
- }
- }
- else
- {
- // For trimesh, directly write out all the nodes and faces.xs
- for (int i = 0; i < psb->m_nodes.size(); ++i)
- {
- fs << "v";
- for (int d = 0; d < 3; d++)
- {
- fs << " " << psb->m_nodes[i].m_x[d];
- }
- fs << "\n";
- }
- for (int i = 0; i < psb->m_faces.size(); ++i)
- {
- fs << "f";
- for (int n = 0; n < 3; n++)
- {
- fs << " " << psb->m_faces[i].m_n[n]->index + 1;
- }
- fs << "\n";
- }
- }
- fs.close();
- }
- void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
- {
- std::ofstream fs;
- fs.open(file);
- btAssert(fs);
- fs << std::scientific << std::setprecision(16);
- // Only write out for trimesh, directly write out all the nodes and faces.xs
- for (int i = 0; i < psb->m_nodes.size(); ++i)
- {
- fs << "q";
- for (int d = 0; d < 3; d++)
- {
- fs << " " << psb->m_nodes[i].m_q[d];
- }
- fs << "\n";
- }
- for (int i = 0; i < psb->m_nodes.size(); ++i)
- {
- fs << "v";
- for (int d = 0; d < 3; d++)
- {
- fs << " " << psb->m_nodes[i].m_v[d];
- }
- fs << "\n";
- }
- fs.close();
- }
- void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
- {
- std::ifstream fs_read;
- fs_read.open(filename);
- std::string line;
- btVector3 pos;
- btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
- while (std::getline(fs_read, line))
- {
- std::stringstream ss(line);
- if (line[0] == 'v')
- {
- }
- else if (line[0] == 'f')
- {
- ss.ignore();
- int id0, id1, id2;
- ss >> id0;
- ss >> id1;
- ss >> id2;
- btAlignedObjectArray<int> new_face;
- new_face.push_back(id1);
- new_face.push_back(id0);
- new_face.push_back(id2);
- additional_faces.push_back(new_face);
- }
- }
- fs_read.close();
- std::ofstream fs_write;
- fs_write.open(filename, std::ios_base::app);
- for (int i = 0; i < additional_faces.size(); ++i)
- {
- fs_write << "f";
- for (int n = 0; n < 3; n++)
- {
- fs_write << " " << additional_faces[i][n];
- }
- fs_write << "\n";
- }
- fs_write.close();
- }
- // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
- void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
- {
- btVector3 vap = p - a;
- btVector3 vbp = p - b;
- btVector3 vab = b - a;
- btVector3 vac = c - a;
- btVector3 vad = d - a;
- btVector3 vbc = c - b;
- btVector3 vbd = d - b;
- btScalar va6 = (vbp.cross(vbd)).dot(vbc);
- btScalar vb6 = (vap.cross(vac)).dot(vad);
- btScalar vc6 = (vap.cross(vad)).dot(vab);
- btScalar vd6 = (vap.cross(vab)).dot(vac);
- btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
- bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
- }
- // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
- void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
- {
- btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
- btScalar d00 = btDot(v0, v0);
- btScalar d01 = btDot(v0, v1);
- btScalar d11 = btDot(v1, v1);
- btScalar d20 = btDot(v2, v0);
- btScalar d21 = btDot(v2, v1);
- btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
- bary[1] = (d11 * d20 - d01 * d21) * invDenom;
- bary[2] = (d00 * d21 - d01 * d20) * invDenom;
- bary[0] = 1.0 - bary[1] - bary[2];
- bary[3] = 0;
- }
- // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
- // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
- void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
- {
- psb->m_z.resize(0);
- psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
- psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
- for (int i = 0; i < psb->m_renderNodes.size(); ++i)
- {
- const btVector3& p = psb->m_renderNodes[i].m_x;
- btVector4 bary;
- btVector4 optimal_bary;
- btScalar min_bary_weight = -1e3;
- btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
- for (int j = 0; j < psb->m_tetras.size(); ++j)
- {
- const btSoftBody::Tetra& t = psb->m_tetras[j];
- 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);
- btScalar new_min_bary_weight = bary[0];
- for (int k = 1; k < 4; ++k)
- {
- new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
- }
- if (new_min_bary_weight > min_bary_weight)
- {
- btAlignedObjectArray<const btSoftBody::Node*> parents;
- parents.push_back(t.m_n[0]);
- parents.push_back(t.m_n[1]);
- parents.push_back(t.m_n[2]);
- parents.push_back(t.m_n[3]);
- optimal_parents = parents;
- optimal_bary = bary;
- min_bary_weight = new_min_bary_weight;
- // stop searching if p is inside the tetrahedron at hand
- if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
- {
- break;
- }
- }
- }
- psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
- psb->m_renderNodesParents[i] = optimal_parents;
- }
- }
- // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
- void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
- {
- psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
- psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
- psb->m_z.resize(psb->m_renderNodes.size());
- for (int i = 0; i < psb->m_renderNodes.size(); ++i)
- {
- const btVector3& p = psb->m_renderNodes[i].m_x;
- btVector4 bary;
- btVector4 optimal_bary;
- btScalar min_bary_weight = -SIMD_INFINITY;
- btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
- btScalar dist = 0, optimal_dist = 0;
- for (int j = 0; j < psb->m_faces.size(); ++j)
- {
- const btSoftBody::Face& f = psb->m_faces[j];
- 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);
- btVector3 unit_n = n.normalized();
- dist = (p - f.m_n[0]->m_x).dot(unit_n);
- btVector3 proj_p = p - dist * unit_n;
- getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
- btScalar new_min_bary_weight = bary[0];
- for (int k = 1; k < 3; ++k)
- {
- new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
- }
- // p is out of the current best triangle, we found a traingle that's better
- bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
- // p is inside of the current best triangle, we found a triangle that's better
- bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
- if (better_than_closest_outisde || better_than_best_inside)
- {
- btAlignedObjectArray<const btSoftBody::Node*> parents;
- parents.push_back(f.m_n[0]);
- parents.push_back(f.m_n[1]);
- parents.push_back(f.m_n[2]);
- optimal_parents = parents;
- optimal_bary = bary;
- optimal_dist = dist;
- min_bary_weight = new_min_bary_weight;
- }
- }
- psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
- psb->m_renderNodesParents[i] = optimal_parents;
- psb->m_z[i] = optimal_dist;
- }
- }
|