Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Oct 20, 2008, 5:40:38 PM (16 years ago)
Author:
rgrieder
Message:

Downgraded Bullet to latest tagged version: 2.72
That should give us more stability.

Location:
code/branches/physics/src/bullet/BulletSoftBody
Files:
15 edited

Legend:

Unmodified
Added
Removed

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp

    r1963 r1972  
    151151btSoftBody::Material*           btSoftBody::appendMaterial()
    152152{
    153         Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
    154         if(m_materials.size()>0)
    155                 *pm=*m_materials[0];
     153Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
     154if(m_materials.size()>0)
     155        *pm=*m_materials[0];
    156156        else
    157                 ZeroInitialize(*pm);
    158         m_materials.push_back(pm);
    159         return(pm);
     157        ZeroInitialize(*pm);
     158m_materials.push_back(pm);
     159return(pm);
    160160}
    161161
    162162//
    163163void                    btSoftBody::appendNote( const char* text,
    164                                                                            const btVector3& o,
    165                                                                            const btVector4& c,
    166                                                                            Node* n0,
    167                                                                            Node* n1,
    168                                                                            Node* n2,
    169                                                                            Node* n3)
    170 {
    171         Note    n;
    172         ZeroInitialize(n);
    173         n.m_rank                =       0;
    174         n.m_text                =       text;
    175         n.m_offset              =       o;
    176         n.m_coords[0]   =       c.x();
    177         n.m_coords[1]   =       c.y();
    178         n.m_coords[2]   =       c.z();
    179         n.m_coords[3]   =       c.w();
    180         n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
    181         n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
    182         n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
    183         n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
    184         m_notes.push_back(n);
     164                                                                                const btVector3& o,
     165                                                                                const btVector4& c,
     166                                                                                Node* n0,
     167                                                                                Node* n1,
     168                                                                                Node* n2,
     169                                                                                Node* n3)
     170{
     171Note    n;
     172ZeroInitialize(n);
     173n.m_rank                =       0;
     174n.m_text                =       text;
     175n.m_offset              =       o;
     176n.m_coords[0]   =       c.x();
     177n.m_coords[1]   =       c.y();
     178n.m_coords[2]   =       c.z();
     179n.m_coords[3]   =       c.w();
     180n.m_nodes[0]    =       n0;n.m_rank+=n0?1:0;
     181n.m_nodes[1]    =       n1;n.m_rank+=n1?1:0;
     182n.m_nodes[2]    =       n2;n.m_rank+=n2?1:0;
     183n.m_nodes[3]    =       n3;n.m_rank+=n3?1:0;
     184m_notes.push_back(n);
    185185}
    186186
    187187//
    188188void                    btSoftBody::appendNote( const char* text,
    189                                                                            const btVector3& o,
    190                                                                            Node* feature)
    191 {
    192         appendNote(text,o,btVector4(1,0,0,0),feature);
     189                                                                                const btVector3& o,
     190                                                                                Node* feature)
     191{
     192appendNote(text,o,btVector4(1,0,0,0),feature);
    193193}
    194194
    195195//
    196196void                    btSoftBody::appendNote( const char* text,
    197                                                                            const btVector3& o,
    198                                                                            Link* feature)
    199 {
    200         static const btScalar   w=1/(btScalar)2;
    201         appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
    202                 feature->m_n[1]);
    203 }
    204 
     197                                                                                const btVector3& o,
     198                                                                                Link* feature)
     199{
     200static const btScalar   w=1/(btScalar)2;
     201appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
     202                                                                                feature->m_n[1]);
     203}
     204                                                               
    205205//
    206206void                    btSoftBody::appendNote( const char* text,
    207                                                                            const btVector3& o,
    208                                                                            Face* feature)
    209 {
    210         static const btScalar   w=1/(btScalar)3;
    211         appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
    212                 feature->m_n[1],
    213                 feature->m_n[2]);
     207                                                                                const btVector3& o,
     208                                                                                Face* feature)
     209{
     210static const btScalar   w=1/(btScalar)3;
     211appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
     212                                                                                feature->m_n[1],
     213                                                                                feature->m_n[2]);
    214214}
    215215
    … …  
    217217void                    btSoftBody::appendNode( const btVector3& x,btScalar m)
    218218{
    219         if(m_nodes.capacity()==m_nodes.size())
    220         {
    221                 pointersToIndices();
    222                 m_nodes.reserve(m_nodes.size()*2+1);
    223                 indicesToPointers();
    224         }
    225         const btScalar  margin=getCollisionShape()->getMargin();
    226         m_nodes.push_back(Node());
    227         Node&                   n=m_nodes[m_nodes.size()-1];
    228         ZeroInitialize(n);
    229         n.m_x                   =       x;
    230         n.m_q                   =       n.m_x;
    231         n.m_im                  =       m>0?1/m:0;
    232         n.m_material    =       m_materials[0];
    233         n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
     219if(m_nodes.capacity()==m_nodes.size())
     220        {
     221        pointersToIndices();
     222        m_nodes.reserve(m_nodes.size()*2+1);
     223        indicesToPointers();
     224        }
     225const btScalar  margin=getCollisionShape()->getMargin();
     226m_nodes.push_back(Node());
     227Node&                   n=m_nodes[m_nodes.size()-1];
     228ZeroInitialize(n);
     229n.m_x                   =       x;
     230n.m_q                   =       n.m_x;
     231n.m_im                  =       m>0?1/m:0;
     232n.m_material    =       m_materials[0];
     233n.m_leaf                =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
    234234}
    235235
    … …  
    237237void                    btSoftBody::appendLink(int model,Material* mat)
    238238{
    239         Link    l;
    240         if(model>=0)
    241                 l=m_links[model];
     239Link    l;
     240if(model>=0)
     241        l=m_links[model];
    242242        else
    243243        { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
    244         m_links.push_back(l);
     244m_links.push_back(l);
    245245}
    246246
    … …  
    274274void                    btSoftBody::appendFace(int model,Material* mat)
    275275{
    276         Face    f;
    277         if(model>=0)
     276Face    f;
     277if(model>=0)
    278278        { f=m_faces[model]; }
    279279        else
    280280        { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
    281         m_faces.push_back(f);
     281m_faces.push_back(f);
    282282}
    283283
    … …  
    320320void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
    321321{
    322         LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
    323         pj->m_bodies[0] =       body0;
    324         pj->m_bodies[1] =       body1;
    325         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
    326         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
    327         pj->m_cfm               =       specs.cfm;
    328         pj->m_erp               =       specs.erp;
    329         pj->m_split             =       specs.split;
    330         m_joints.push_back(pj);
     322LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
     323pj->m_bodies[0] =       body0;
     324pj->m_bodies[1] =       body1;
     325pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
     326pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
     327pj->m_cfm               =       specs.cfm;
     328pj->m_erp               =       specs.erp;
     329pj->m_split             =       specs.split;
     330m_joints.push_back(pj);
    331331}
    332332
    … …  
    334334void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
    335335{
    336         appendLinearJoint(specs,m_clusters[0],body);
     336appendLinearJoint(specs,m_clusters[0],body);
    337337}
    338338
    … …  
    340340void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
    341341{
    342         appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
     342appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
    343343}
    344344
    … …  
    346346void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
    347347{
    348         AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
    349         pj->m_bodies[0] =       body0;
    350         pj->m_bodies[1] =       body1;
    351         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
    352         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
    353         pj->m_cfm               =       specs.cfm;
    354         pj->m_erp               =       specs.erp;
    355         pj->m_split             =       specs.split;
    356         pj->m_icontrol  =       specs.icontrol;
    357         m_joints.push_back(pj);
     348AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
     349pj->m_bodies[0] =       body0;
     350pj->m_bodies[1] =       body1;
     351pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
     352pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
     353pj->m_cfm               =       specs.cfm;
     354pj->m_erp               =       specs.erp;
     355pj->m_split             =       specs.split;
     356pj->m_icontrol  =       specs.icontrol;
     357m_joints.push_back(pj);
    358358}
    359359
    … …  
    361361void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
    362362{
    363         appendAngularJoint(specs,m_clusters[0],body);
     363appendAngularJoint(specs,m_clusters[0],body);
    364364}
    365365
    … …  
    367367void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
    368368{
    369         appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
     369appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
    370370}
    371371
    … …  
    456456                        const Face&             f=m_faces[i];
    457457                        const btScalar  twicearea=AreaOf(       f.m_n[0]->m_x,
    458                                 f.m_n[1]->m_x,
    459                                 f.m_n[2]->m_x);
     458                                                                                                f.m_n[1]->m_x,
     459                                                                                                f.m_n[2]->m_x);
    460460                        for(int j=0;j<3;++j)
    461461                        {
    … …  
    503503void                    btSoftBody::translate(const btVector3& trs)
    504504{
    505         btTransform     t;
    506         t.setIdentity();
    507         t.setOrigin(trs);
    508         transform(t);
     505btTransform     t;
     506t.setIdentity();
     507t.setOrigin(trs);
     508transform(t);
    509509}
    510510
    … …  
    512512void                    btSoftBody::rotate(     const btQuaternion& rot)
    513513{
    514         btTransform     t;
    515         t.setIdentity();
    516         t.setRotation(rot);
    517         transform(t);
     514btTransform     t;
     515t.setIdentity();
     516t.setRotation(rot);
     517transform(t);
    518518}
    519519
    … …  
    554554                Node&   n=m_nodes[i];
    555555                m_pose.m_wgh[i]=        n.m_im>0                                        ?
    556                         1/(m_nodes[i].m_im*tmass)       :
    557                 kmass/tmass;
     556                                                        1/(m_nodes[i].m_im*tmass)       :
     557                                                        kmass/tmass;
    558558        }
    559559        /* Pos          */
    … …  
    570570        /* Aqq          */
    571571        m_pose.m_aqq[0] =
    572                 m_pose.m_aqq[1] =
    573                 m_pose.m_aqq[2] =       btVector3(0,0,0);
     572        m_pose.m_aqq[1] =
     573        m_pose.m_aqq[2] =       btVector3(0,0,0);
    574574        for( i=0,ni=m_nodes.size();i<ni;++i)
    575         {
     575                {
    576576                const btVector3&        q=m_pose.m_pos[i];
    577577                const btVector3         mq=m_pose.m_wgh[i]*q;
    … …  
    579579                m_pose.m_aqq[1]+=mq.y()*q;
    580580                m_pose.m_aqq[2]+=mq.z()*q;
    581         }
     581                }
    582582        m_pose.m_aqq=m_pose.m_aqq.inverse();
    583583        updateConstants();
    … …  
    606606int                             btSoftBody::clusterCount() const
    607607{
    608         return(m_clusters.size());
     608return(m_clusters.size());
    609609}
    610610
    … …  
    612612btVector3               btSoftBody::clusterCom(const Cluster* cluster)
    613613{
    614         btVector3               com(0,0,0);
    615         for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
    616         {
    617                 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
    618         }
    619         return(com*cluster->m_imass);
     614btVector3               com(0,0,0);
     615for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
     616        {
     617        com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
     618        }
     619return(com*cluster->m_imass);
    620620}
    621621
    … …  
    623623btVector3               btSoftBody::clusterCom(int cluster) const
    624624{
    625         return(clusterCom(m_clusters[cluster]));
     625return(clusterCom(m_clusters[cluster]));
    626626}
    627627
    … …  
    629629btVector3               btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
    630630{
    631         return(cluster->m_lv+cross(cluster->m_av,rpos));
     631return(cluster->m_lv+cross(cluster->m_av,rpos));
    632632}
    633633
    … …  
    635635void                    btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    636636{
    637         const btVector3 li=cluster->m_imass*impulse;
    638         const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
    639         cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
    640         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
    641         cluster->m_nvimpulses++;
     637const btVector3 li=cluster->m_imass*impulse;
     638const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
     639cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
     640cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
     641cluster->m_nvimpulses++;
    642642}
    643643
    … …  
    645645void                    btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    646646{
    647         const btVector3 li=cluster->m_imass*impulse;
    648         const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
    649         cluster->m_dimpulses[0]+=li;
    650         cluster->m_dimpulses[1]+=ai;
    651         cluster->m_ndimpulses++;
     647const btVector3 li=cluster->m_imass*impulse;
     648const btVector3 ai=cluster->m_invwi*cross(rpos,impulse);
     649cluster->m_dimpulses[0]+=li;
     650cluster->m_dimpulses[1]+=ai;
     651cluster->m_ndimpulses++;
    652652}
    653653
    … …  
    655655void                    btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
    656656{
    657         if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
    658         if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
     657if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
     658if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
    659659}
    660660
    … …  
    662662void                    btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
    663663{
    664         const btVector3 ai=cluster->m_invwi*impulse;
    665         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
    666         cluster->m_nvimpulses++;
     664const btVector3 ai=cluster->m_invwi*impulse;
     665cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
     666cluster->m_nvimpulses++;
    667667}
    668668
    … …  
    670670void                    btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
    671671{
    672         const btVector3 ai=cluster->m_invwi*impulse;
    673         cluster->m_dimpulses[1]+=ai;
    674         cluster->m_ndimpulses++;
     672const btVector3 ai=cluster->m_invwi*impulse;
     673cluster->m_dimpulses[1]+=ai;
     674cluster->m_ndimpulses++;
    675675}
    676676
    … …  
    678678void                    btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
    679679{
    680         if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
    681         if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
     680if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
     681if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
    682682}
    683683
    … …  
    685685void                    btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
    686686{
    687         cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
    688         cluster->m_ndimpulses++;
     687cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
     688cluster->m_ndimpulses++;
    689689}
    690690
    … …  
    754754void                    btSoftBody::randomizeConstraints()
    755755{
    756         unsigned long   seed=243703;
     756unsigned long   seed=243703;
    757757#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
    758         int i,ni;
     758int i,ni;
    759759
    760760        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    772772void                    btSoftBody::releaseCluster(int index)
    773773{
    774         Cluster*        c=m_clusters[index];
    775         if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
    776         c->~Cluster();
    777         btAlignedFree(c);
    778         m_clusters.remove(c);
     774Cluster*        c=m_clusters[index];
     775if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
     776c->~Cluster();
     777btAlignedFree(c);
     778m_clusters.remove(c);
    779779}
    780780
    … …  
    782782void                    btSoftBody::releaseClusters()
    783783{
    784         while(m_clusters.size()>0) releaseCluster(0);
     784while(m_clusters.size()>0) releaseCluster(0);
    785785}
    786786
    … …  
    788788int                             btSoftBody::generateClusters(int k,int maxiterations)
    789789{
    790         int i;
    791         releaseClusters();
    792         m_clusters.resize(btMin(k,m_nodes.size()));
    793         for(i=0;i<m_clusters.size();++i)
    794         {
    795                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
    796                 m_clusters[i]->m_collide=       true;
    797         }
    798         k=m_clusters.size();
    799         if(k>0)
    800         {
    801                 /* Initialize           */
    802                 btAlignedObjectArray<btVector3> centers;
    803                 btVector3                                               cog(0,0,0);
    804                 int                                                             i;
     790int i;
     791releaseClusters();
     792m_clusters.resize(btMin(k,m_nodes.size()));
     793for(i=0;i<m_clusters.size();++i)
     794        {
     795        m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
     796        m_clusters[i]->m_collide=       true;
     797        }
     798k=m_clusters.size();
     799if(k>0)
     800        {
     801        /* Initialize           */
     802        btAlignedObjectArray<btVector3> centers;
     803        btVector3                                               cog(0,0,0);
     804        int                                                             i;
     805        for(i=0;i<m_nodes.size();++i)
     806                {
     807                cog+=m_nodes[i].m_x;
     808                m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
     809                }
     810        cog/=(btScalar)m_nodes.size();
     811        centers.resize(k,cog);
     812        /* Iterate                      */
     813        const btScalar  slope=16;
     814        bool                    changed;
     815        int                             iterations=0;
     816        do      {
     817                const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
     818                changed=false;
     819                iterations++;   
     820                int i;
     821
     822                for(i=0;i<k;++i)
     823                        {
     824                        btVector3       c(0,0,0);
     825                        for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     826                                {
     827                                c+=m_clusters[i]->m_nodes[j]->m_x;
     828                                }
     829                        if(m_clusters[i]->m_nodes.size())
     830                                {
     831                                c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
     832                                c                       =       centers[i]+(c-centers[i])*w;
     833                                changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
     834                                centers[i]      =       c;
     835                                m_clusters[i]->m_nodes.resize(0);
     836                                }                       
     837                        }
    805838                for(i=0;i<m_nodes.size();++i)
    806                 {
    807                         cog+=m_nodes[i].m_x;
    808                         m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
    809                 }
    810                 cog/=(btScalar)m_nodes.size();
    811                 centers.resize(k,cog);
    812                 /* Iterate                      */
    813                 const btScalar  slope=16;
    814                 bool                    changed;
    815                 int                             iterations=0;
    816                 do      {
    817                         const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
    818                         changed=false;
    819                         iterations++;   
    820                         int i;
    821 
    822                         for(i=0;i<k;++i)
    823                         {
    824                                 btVector3       c(0,0,0);
    825                                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     839                        {
     840                        const btVector3 nx=m_nodes[i].m_x;
     841                        int                             kbest=0;
     842                        btScalar                kdist=ClusterMetric(centers[0],nx);
     843                        for(int j=1;j<k;++j)
    826844                                {
    827                                         c+=m_clusters[i]->m_nodes[j]->m_x;
    828                                 }
    829                                 if(m_clusters[i]->m_nodes.size())
    830                                 {
    831                                         c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
    832                                         c                       =       centers[i]+(c-centers[i])*w;
    833                                         changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
    834                                         centers[i]      =       c;
    835                                         m_clusters[i]->m_nodes.resize(0);
    836                                 }                       
    837                         }
    838                         for(i=0;i<m_nodes.size();++i)
    839                         {
    840                                 const btVector3 nx=m_nodes[i].m_x;
    841                                 int                             kbest=0;
    842                                 btScalar                kdist=ClusterMetric(centers[0],nx);
    843                                 for(int j=1;j<k;++j)
    844                                 {
    845                                         const btScalar  d=ClusterMetric(centers[j],nx);
    846                                         if(d<kdist)
     845                                const btScalar  d=ClusterMetric(centers[j],nx);
     846                                if(d<kdist)
    847847                                        {
    848                                                 kbest=j;
    849                                                 kdist=d;
     848                                        kbest=j;
     849                                        kdist=d;
    850850                                        }
    851851                                }
    852                                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
     852                        m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
    853853                        }               
    854854                } while(changed&&(iterations<maxiterations));
    855                 /* Merge                */
    856                 btAlignedObjectArray<int>       cids;
    857                 cids.resize(m_nodes.size(),-1);
    858                 for(i=0;i<m_clusters.size();++i)
    859                 {
    860                         for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
    861                         {
    862                                 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
    863                         }
    864                 }
    865                 for(i=0;i<m_faces.size();++i)
    866                 {
    867                         const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
    868                                 int(m_faces[i].m_n[1]-&m_nodes[0]),
    869                                 int(m_faces[i].m_n[2]-&m_nodes[0])};
    870                         for(int j=0;j<3;++j)
    871                         {
    872                                 const int cid=cids[idx[j]];
    873                                 for(int q=1;q<3;++q)
     855        /* Merge                */
     856        btAlignedObjectArray<int>       cids;
     857        cids.resize(m_nodes.size(),-1);
     858        for(i=0;i<m_clusters.size();++i)
     859                {
     860                for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
     861                        {
     862                        cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
     863                        }
     864                }
     865        for(i=0;i<m_faces.size();++i)
     866                {
     867                const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
     868                                                        int(m_faces[i].m_n[1]-&m_nodes[0]),
     869                                                        int(m_faces[i].m_n[2]-&m_nodes[0])};
     870                for(int j=0;j<3;++j)
     871                        {
     872                        const int cid=cids[idx[j]];
     873                        for(int q=1;q<3;++q)
    874874                                {
    875                                         const int kid=idx[(j+q)%3];
    876                                         if(cids[kid]!=cid)
     875                                const int kid=idx[(j+q)%3];
     876                                if(cids[kid]!=cid)
    877877                                        {
    878                                                 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
     878                                        if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
    879879                                                {
    880                                                         m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
     880                                                m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
    881881                                                }
    882882                                        }
    … …  
    884884                        }
    885885                }
    886                 /* Master               */
    887                 if(m_clusters.size()>1)
    888                 {
    889                         Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
    890                         pmaster->m_collide      =       false;
    891                         pmaster->m_nodes.reserve(m_nodes.size());
    892                         for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
    893                         m_clusters.push_back(pmaster);
    894                         btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
    895                 }
    896                 /* Terminate    */
    897                 for(i=0;i<m_clusters.size();++i)
    898                 {
    899                         if(m_clusters[i]->m_nodes.size()==0)
    900                         {
    901                                 releaseCluster(i--);
    902                         }
    903                 }
    904 
    905                 initializeClusters();
    906                 updateClusters();
    907                 return(m_clusters.size());
    908         }
    909         return(0);
     886        /* Master               */
     887        if(m_clusters.size()>1)
     888                {
     889                Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
     890                pmaster->m_collide      =       false;
     891                pmaster->m_nodes.reserve(m_nodes.size());
     892                for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
     893                m_clusters.push_back(pmaster);
     894                btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
     895                }
     896        /* Terminate    */
     897        for(i=0;i<m_clusters.size();++i)
     898                {
     899                if(m_clusters[i]->m_nodes.size()==0)
     900                        {
     901                        releaseCluster(i--);
     902                        }
     903                }
     904               
     905        initializeClusters();
     906        updateClusters();
     907        return(m_clusters.size());
     908        }
     909return(0);
    910910}
    911911
    … …  
    913913void                    btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
    914914{
    915         const Node*                     nbase = &m_nodes[0];
    916         int                                     ncount = m_nodes.size();
    917         btSymMatrix<int>        edges(ncount,-2);
    918         int                                     newnodes=0;
    919         int i,j,k,ni;
    920 
    921         /* Filter out           */
    922         for(i=0;i<m_links.size();++i)
    923         {
    924                 Link&   l=m_links[i];
    925                 if(l.m_bbending)
    926                 {
    927                         if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
    928                         {
    929                                 btSwap(m_links[i],m_links[m_links.size()-1]);
    930                                 m_links.pop_back();--i;
     915const Node*                     nbase = &m_nodes[0];
     916int                                     ncount = m_nodes.size();
     917btSymMatrix<int>        edges(ncount,-2);
     918int                                     newnodes=0;
     919int i,j,k,ni;
     920
     921/* Filter out           */
     922for(i=0;i<m_links.size();++i)
     923        {
     924        Link&   l=m_links[i];
     925        if(l.m_bbending)
     926                {
     927                if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
     928                        {
     929                        btSwap(m_links[i],m_links[m_links.size()-1]);
     930                        m_links.pop_back();--i;
    931931                        }
    932932                }       
    933933        }
    934         /* Fill edges           */
    935         for(i=0;i<m_links.size();++i)
    936         {
    937                 Link&   l=m_links[i];
    938                 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
    939         }
    940         for(i=0;i<m_faces.size();++i)
     934/* Fill edges           */
     935for(i=0;i<m_links.size();++i)
     936        {
     937        Link&   l=m_links[i];
     938        edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
     939        }
     940for(i=0;i<m_faces.size();++i)
    941941        {       
    942                 Face&   f=m_faces[i];
    943                 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
    944                 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
    945                 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
    946         }
    947         /* Intersect            */
    948         for(i=0;i<ncount;++i)
    949         {
    950                 for(j=i+1;j<ncount;++j)
    951                 {
    952                         if(edges(i,j)==-1)
    953                         {
    954                                 Node&                   a=m_nodes[i];
    955                                 Node&                   b=m_nodes[j];
    956                                 const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
    957                                 if(t>0)
     942        Face&   f=m_faces[i];
     943        edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
     944        edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
     945        edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
     946        }
     947/* Intersect            */
     948for(i=0;i<ncount;++i)
     949        {
     950        for(j=i+1;j<ncount;++j)
     951                {
     952                if(edges(i,j)==-1)
     953                        {
     954                        Node&                   a=m_nodes[i];
     955                        Node&                   b=m_nodes[j];
     956                        const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
     957                        if(t>0)
    958958                                {
    959                                         const btVector3 x=Lerp(a.m_x,b.m_x,t);
    960                                         const btVector3 v=Lerp(a.m_v,b.m_v,t);
    961                                         btScalar                m=0;
    962                                         if(a.m_im>0)
     959                                const btVector3 x=Lerp(a.m_x,b.m_x,t);
     960                                const btVector3 v=Lerp(a.m_v,b.m_v,t);
     961                                btScalar                m=0;
     962                                if(a.m_im>0)
    963963                                        {
    964                                                 if(b.m_im>0)
     964                                        if(b.m_im>0)
    965965                                                {
    966                                                         const btScalar  ma=1/a.m_im;
    967                                                         const btScalar  mb=1/b.m_im;
    968                                                         const btScalar  mc=Lerp(ma,mb,t);
    969                                                         const btScalar  f=(ma+mb)/(ma+mb+mc);
    970                                                         a.m_im=1/(ma*f);
    971                                                         b.m_im=1/(mb*f);
    972                                                         m=mc*f;
     966                                                const btScalar  ma=1/a.m_im;
     967                                                const btScalar  mb=1/b.m_im;
     968                                                const btScalar  mc=Lerp(ma,mb,t);
     969                                                const btScalar  f=(ma+mb)/(ma+mb+mc);
     970                                                a.m_im=1/(ma*f);
     971                                                b.m_im=1/(mb*f);
     972                                                m=mc*f;
    973973                                                }
    974974                                                else
    … …  
    977977                                        else
    978978                                        {
    979                                                 if(b.m_im>0)
     979                                        if(b.m_im>0)
    980980                                                { b.m_im/=0.5;m=1/b.m_im; }
    981981                                                else
    982                                                         m=0;
     982                                                m=0;
    983983                                        }
    984                                         appendNode(x,m);
    985                                         edges(i,j)=m_nodes.size()-1;
    986                                         m_nodes[edges(i,j)].m_v=v;
    987                                         ++newnodes;
     984                                appendNode(x,m);
     985                                edges(i,j)=m_nodes.size()-1;
     986                                m_nodes[edges(i,j)].m_v=v;
     987                                ++newnodes;
    988988                                }
    989989                        }
    990990                }
    991991        }
     992nbase=&m_nodes[0];
     993/* Refine links         */
     994for(i=0,ni=m_links.size();i<ni;++i)
     995        {
     996        Link&           feat=m_links[i];
     997        const int       idx[]={ int(feat.m_n[0]-nbase),
     998                                                int(feat.m_n[1]-nbase)};
     999        if((idx[0]<ncount)&&(idx[1]<ncount))
     1000                {
     1001                const int ni=edges(idx[0],idx[1]);
     1002                if(ni>0)
     1003                        {
     1004                        appendLink(i);
     1005                        Link*           pft[]={ &m_links[i],
     1006                                                                &m_links[m_links.size()-1]};                   
     1007                        pft[0]->m_n[0]=&m_nodes[idx[0]];
     1008                        pft[0]->m_n[1]=&m_nodes[ni];
     1009                        pft[1]->m_n[0]=&m_nodes[ni];
     1010                        pft[1]->m_n[1]=&m_nodes[idx[1]];
     1011                        }
     1012                }
     1013        }
     1014/* Refine faces         */
     1015for(i=0;i<m_faces.size();++i)
     1016        {
     1017        const Face&     feat=m_faces[i];
     1018        const int       idx[]={ int(feat.m_n[0]-nbase),
     1019                                                int(feat.m_n[1]-nbase),
     1020                                                int(feat.m_n[2]-nbase)};
     1021        for(j=2,k=0;k<3;j=k++)
     1022                {
     1023                if((idx[j]<ncount)&&(idx[k]<ncount))
     1024                        {
     1025                        const int ni=edges(idx[j],idx[k]);
     1026                        if(ni>0)
     1027                                {
     1028                                appendFace(i);
     1029                                const int       l=(k+1)%3;
     1030                                Face*           pft[]={ &m_faces[i],
     1031                                                                        &m_faces[m_faces.size()-1]};
     1032                                pft[0]->m_n[0]=&m_nodes[idx[l]];
     1033                                pft[0]->m_n[1]=&m_nodes[idx[j]];
     1034                                pft[0]->m_n[2]=&m_nodes[ni];
     1035                                pft[1]->m_n[0]=&m_nodes[ni];
     1036                                pft[1]->m_n[1]=&m_nodes[idx[k]];
     1037                                pft[1]->m_n[2]=&m_nodes[idx[l]];
     1038                                appendLink(ni,idx[l],pft[0]->m_material);
     1039                                --i;break;
     1040                                }
     1041                        }
     1042                }
     1043        }
     1044/* Cut                          */
     1045if(cut)
     1046        {       
     1047        btAlignedObjectArray<int>       cnodes;
     1048        const int                                       pcount=ncount;
     1049        int                                                     i;
     1050        ncount=m_nodes.size();
     1051        cnodes.resize(ncount,0);
     1052        /* Nodes                */
     1053        for(i=0;i<ncount;++i)
     1054                {
     1055                const btVector3 x=m_nodes[i].m_x;
     1056                if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
     1057                        {
     1058                        const btVector3 v=m_nodes[i].m_v;
     1059                        btScalar                m=getMass(i);
     1060                        if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
     1061                        appendNode(x,m);
     1062                        cnodes[i]=m_nodes.size()-1;
     1063                        m_nodes[cnodes[i]].m_v=v;
     1064                        }
     1065                }
    9921066        nbase=&m_nodes[0];
    993         /* Refine links         */
     1067        /* Links                */
    9941068        for(i=0,ni=m_links.size();i<ni;++i)
    995         {
    996                 Link&           feat=m_links[i];
    997                 const int       idx[]={ int(feat.m_n[0]-nbase),
    998                         int(feat.m_n[1]-nbase)};
    999                 if((idx[0]<ncount)&&(idx[1]<ncount))
    1000                 {
    1001                         const int ni=edges(idx[0],idx[1]);
    1002                         if(ni>0)
    1003                         {
    1004                                 appendLink(i);
    1005                                 Link*           pft[]={ &m_links[i],
    1006                                         &m_links[m_links.size()-1]};                   
    1007                                 pft[0]->m_n[0]=&m_nodes[idx[0]];
    1008                                 pft[0]->m_n[1]=&m_nodes[ni];
    1009                                 pft[1]->m_n[0]=&m_nodes[ni];
    1010                                 pft[1]->m_n[1]=&m_nodes[idx[1]];
    1011                         }
    1012                 }
    1013         }
    1014         /* Refine faces         */
    1015         for(i=0;i<m_faces.size();++i)
    1016         {
    1017                 const Face&     feat=m_faces[i];
    1018                 const int       idx[]={ int(feat.m_n[0]-nbase),
    1019                         int(feat.m_n[1]-nbase),
    1020                         int(feat.m_n[2]-nbase)};
    1021                 for(j=2,k=0;k<3;j=k++)
    1022                 {
    1023                         if((idx[j]<ncount)&&(idx[k]<ncount))
    1024                         {
    1025                                 const int ni=edges(idx[j],idx[k]);
    1026                                 if(ni>0)
     1069                {
     1070                const int               id[]={  int(m_links[i].m_n[0]-nbase),
     1071                                                                int(m_links[i].m_n[1]-nbase)};
     1072                int                             todetach=0;
     1073                if(cnodes[id[0]]&&cnodes[id[1]])
     1074                        {
     1075                        appendLink(i);
     1076                        todetach=m_links.size()-1;
     1077                        }
     1078                else
     1079                        {
     1080                        if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
     1081                                        (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
     1082                                todetach=i;
     1083                        }
     1084                if(todetach)
     1085                        {
     1086                        Link&   l=m_links[todetach];
     1087                        for(int j=0;j<2;++j)
    10271088                                {
    1028                                         appendFace(i);
    1029                                         const int       l=(k+1)%3;
    1030                                         Face*           pft[]={ &m_faces[i],
    1031                                                 &m_faces[m_faces.size()-1]};
    1032                                         pft[0]->m_n[0]=&m_nodes[idx[l]];
    1033                                         pft[0]->m_n[1]=&m_nodes[idx[j]];
    1034                                         pft[0]->m_n[2]=&m_nodes[ni];
    1035                                         pft[1]->m_n[0]=&m_nodes[ni];
    1036                                         pft[1]->m_n[1]=&m_nodes[idx[k]];
    1037                                         pft[1]->m_n[2]=&m_nodes[idx[l]];
    1038                                         appendLink(ni,idx[l],pft[0]->m_material);
    1039                                         --i;break;
     1089                                int cn=cnodes[int(l.m_n[j]-nbase)];
     1090                                if(cn) l.m_n[j]=&m_nodes[cn];
     1091                                }                       
     1092                        }
     1093                }
     1094        /* Faces                */
     1095        for(i=0,ni=m_faces.size();i<ni;++i)
     1096                {
     1097                Node**                  n=      m_faces[i].m_n;
     1098                if(     (ifn->Eval(n[0]->m_x)<accurary)&&
     1099                        (ifn->Eval(n[1]->m_x)<accurary)&&
     1100                        (ifn->Eval(n[2]->m_x)<accurary))
     1101                        {
     1102                        for(int j=0;j<3;++j)
     1103                                {
     1104                                int cn=cnodes[int(n[j]-nbase)];
     1105                                if(cn) n[j]=&m_nodes[cn];
    10401106                                }
    10411107                        }
    10421108                }
    1043         }
    1044         /* Cut                          */
    1045         if(cut)
    1046         {       
    1047                 btAlignedObjectArray<int>       cnodes;
    1048                 const int                                       pcount=ncount;
    1049                 int                                                     i;
    1050                 ncount=m_nodes.size();
    1051                 cnodes.resize(ncount,0);
    1052                 /* Nodes                */
    1053                 for(i=0;i<ncount;++i)
    1054                 {
    1055                         const btVector3 x=m_nodes[i].m_x;
    1056                         if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
    1057                         {
    1058                                 const btVector3 v=m_nodes[i].m_v;
    1059                                 btScalar                m=getMass(i);
    1060                                 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
    1061                                 appendNode(x,m);
    1062                                 cnodes[i]=m_nodes.size()-1;
    1063                                 m_nodes[cnodes[i]].m_v=v;
    1064                         }
    1065                 }
    1066                 nbase=&m_nodes[0];
    1067                 /* Links                */
    1068                 for(i=0,ni=m_links.size();i<ni;++i)
    1069                 {
    1070                         const int               id[]={  int(m_links[i].m_n[0]-nbase),
    1071                                 int(m_links[i].m_n[1]-nbase)};
    1072                         int                             todetach=0;
    1073                         if(cnodes[id[0]]&&cnodes[id[1]])
    1074                         {
    1075                                 appendLink(i);
    1076                                 todetach=m_links.size()-1;
    1077                         }
    1078                         else
    1079                         {
    1080                                 if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
    1081                                         (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
    1082                                         todetach=i;
    1083                         }
    1084                         if(todetach)
    1085                         {
    1086                                 Link&   l=m_links[todetach];
    1087                                 for(int j=0;j<2;++j)
    1088                                 {
    1089                                         int cn=cnodes[int(l.m_n[j]-nbase)];
    1090                                         if(cn) l.m_n[j]=&m_nodes[cn];
    1091                                 }                       
    1092                         }
    1093                 }
    1094                 /* Faces                */
    1095                 for(i=0,ni=m_faces.size();i<ni;++i)
    1096                 {
    1097                         Node**                  n=      m_faces[i].m_n;
    1098                         if(     (ifn->Eval(n[0]->m_x)<accurary)&&
    1099                                 (ifn->Eval(n[1]->m_x)<accurary)&&
    1100                                 (ifn->Eval(n[2]->m_x)<accurary))
    1101                         {
    1102                                 for(int j=0;j<3;++j)
    1103                                 {
    1104                                         int cn=cnodes[int(n[j]-nbase)];
    1105                                         if(cn) n[j]=&m_nodes[cn];
    1106                                 }
    1107                         }
    1108                 }
    1109                 /* Clean orphans        */
    1110                 int                                                     nnodes=m_nodes.size();
    1111                 btAlignedObjectArray<int>       ranks;
    1112                 btAlignedObjectArray<int>       todelete;
    1113                 ranks.resize(nnodes,0);
    1114                 for(i=0,ni=m_links.size();i<ni;++i)
    1115                 {
    1116                         for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
    1117                 }
    1118                 for(i=0,ni=m_faces.size();i<ni;++i)
    1119                 {
    1120                         for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
    1121                 }
    1122                 for(i=0;i<m_links.size();++i)
    1123                 {
    1124                         const int       id[]={  int(m_links[i].m_n[0]-nbase),
    1125                                 int(m_links[i].m_n[1]-nbase)};
    1126                         const bool      sg[]={  ranks[id[0]]==1,
    1127                                 ranks[id[1]]==1};
    1128                         if(sg[0]||sg[1])
    1129                         {
    1130                                 --ranks[id[0]];
    1131                                 --ranks[id[1]];
    1132                                 btSwap(m_links[i],m_links[m_links.size()-1]);
    1133                                 m_links.pop_back();--i;
    1134                         }
    1135                 }
    1136 #if 0   
    1137                 for(i=nnodes-1;i>=0;--i)
    1138                 {
    1139                         if(!ranks[i]) todelete.push_back(i);
     1109        /* Clean orphans        */
     1110        int                                                     nnodes=m_nodes.size();
     1111        btAlignedObjectArray<int>       ranks;
     1112        btAlignedObjectArray<int>       todelete;
     1113        ranks.resize(nnodes,0);
     1114        for(i=0,ni=m_links.size();i<ni;++i)
     1115                {
     1116                for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
     1117                }
     1118        for(i=0,ni=m_faces.size();i<ni;++i)
     1119                {
     1120                for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
     1121                }
     1122        for(i=0;i<m_links.size();++i)
     1123                {
     1124                const int       id[]={  int(m_links[i].m_n[0]-nbase),
     1125                                                        int(m_links[i].m_n[1]-nbase)};
     1126                const bool      sg[]={  ranks[id[0]]==1,
     1127                                                        ranks[id[1]]==1};
     1128                if(sg[0]||sg[1])
     1129                        {
     1130                        --ranks[id[0]];
     1131                        --ranks[id[1]];
     1132                        btSwap(m_links[i],m_links[m_links.size()-1]);
     1133                        m_links.pop_back();--i;
     1134                        }
     1135                }
     1136        #if 0   
     1137        for(i=nnodes-1;i>=0;--i)
     1138                {
     1139                if(!ranks[i]) todelete.push_back(i);
    11401140                }       
    1141                 if(todelete.size())
     1141        if(todelete.size())
    11421142                {               
    1143                         btAlignedObjectArray<int>&      map=ranks;
    1144                         for(int i=0;i<nnodes;++i) map[i]=i;
    1145                         PointersToIndices(this);
    1146                         for(int i=0,ni=todelete.size();i<ni;++i)
    1147                         {
    1148                                 int             j=todelete[i];
    1149                                 int&    a=map[j];
    1150                                 int&    b=map[--nnodes];
    1151                                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
    1152                                 btSwap(m_nodes[a],m_nodes[b]);
    1153                                 j=a;a=b;b=j;                   
    1154                         }
    1155                         IndicesToPointers(this,&map[0]);
    1156                         m_nodes.resize(nnodes);
    1157                 }
    1158 #endif
    1159         }
    1160         m_bUpdateRtCst=true;
     1143                btAlignedObjectArray<int>&      map=ranks;
     1144                for(int i=0;i<nnodes;++i) map[i]=i;
     1145                PointersToIndices(this);
     1146                for(int i=0,ni=todelete.size();i<ni;++i)
     1147                        {
     1148                        int             j=todelete[i];
     1149                        int&    a=map[j];
     1150                        int&    b=map[--nnodes];
     1151                        m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
     1152                        btSwap(m_nodes[a],m_nodes[b]);
     1153                        j=a;a=b;b=j;                   
     1154                        }
     1155                IndicesToPointers(this,&map[0]);
     1156                m_nodes.resize(nnodes);
     1157                }
     1158        #endif
     1159        }
     1160m_bUpdateRtCst=true;
    11611161}
    11621162
    … …  
    11641164bool                    btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
    11651165{
    1166         return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
     1166return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
    11671167}
    11681168
    … …  
    11701170bool                    btSoftBody::cutLink(int node0,int node1,btScalar position)
    11711171{
    1172         bool                    done=false;
    1173         int i,ni;
    1174         const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
    1175         const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
    1176         const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
    1177         const btScalar  m=1;
    1178         appendNode(x,m);
    1179         appendNode(x,m);
    1180         Node*                   pa=&m_nodes[node0];
    1181         Node*                   pb=&m_nodes[node1];
    1182         Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
    1183                 &m_nodes[m_nodes.size()-1]};
    1184         pn[0]->m_v=v;
    1185         pn[1]->m_v=v;
    1186         for(i=0,ni=m_links.size();i<ni;++i)
    1187         {
    1188                 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
     1172bool                    done=false;
     1173int i,ni;
     1174const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
     1175const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
     1176const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
     1177const btScalar  m=1;
     1178appendNode(x,m);
     1179appendNode(x,m);
     1180Node*                   pa=&m_nodes[node0];
     1181Node*                   pb=&m_nodes[node1];
     1182Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
     1183                                                &m_nodes[m_nodes.size()-1]};
     1184pn[0]->m_v=v;
     1185pn[1]->m_v=v;
     1186for(i=0,ni=m_links.size();i<ni;++i)
     1187        {
     1188        const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
     1189        if(mtch!=-1)
     1190                {
     1191                appendLink(i);
     1192                Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
     1193                pft[0]->m_n[1]=pn[mtch];
     1194                pft[1]->m_n[0]=pn[1-mtch];
     1195                done=true;
     1196                }
     1197        }
     1198for(i=0,ni=m_faces.size();i<ni;++i)
     1199        {
     1200        for(int k=2,l=0;l<3;k=l++)
     1201                {
     1202                const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
    11891203                if(mtch!=-1)
    1190                 {
    1191                         appendLink(i);
    1192                         Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
    1193                         pft[0]->m_n[1]=pn[mtch];
    1194                         pft[1]->m_n[0]=pn[1-mtch];
    1195                         done=true;
    1196                 }
    1197         }
    1198         for(i=0,ni=m_faces.size();i<ni;++i)
    1199         {
    1200                 for(int k=2,l=0;l<3;k=l++)
    1201                 {
    1202                         const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
    1203                         if(mtch!=-1)
    1204                         {
    1205                                 appendFace(i);
    1206                                 Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
    1207                                 pft[0]->m_n[l]=pn[mtch];
    1208                                 pft[1]->m_n[k]=pn[1-mtch];
    1209                                 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
    1210                                 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
    1211                         }
    1212                 }
    1213         }
    1214         if(!done)
    1215         {
    1216                 m_ndbvt.remove(pn[0]->m_leaf);
    1217                 m_ndbvt.remove(pn[1]->m_leaf);
    1218                 m_nodes.pop_back();
    1219                 m_nodes.pop_back();
    1220         }
    1221         return(done);
    1222 }
    1223 
    1224 //
    1225 bool                    btSoftBody::rayTest(const btVector3& rayFrom,
    1226                                                                         const btVector3& rayTo,
    1227                                                                         sRayCast& results)
    1228 {
    1229         if(m_faces.size()&&m_fdbvt.empty())
    1230                 initializeFaceTree();
    1231 
    1232         results.body    =       this;
    1233         results.fraction = 1.f;
    1234         results.feature =       eFeature::None;
    1235         results.index   =       -1;
    1236 
    1237         return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
     1204                        {
     1205                        appendFace(i);
     1206                        Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
     1207                        pft[0]->m_n[l]=pn[mtch];
     1208                        pft[1]->m_n[k]=pn[1-mtch];
     1209                        appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
     1210                        appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
     1211                        }
     1212                }
     1213        }
     1214if(!done)
     1215        {
     1216        m_ndbvt.remove(pn[0]->m_leaf);
     1217        m_ndbvt.remove(pn[1]->m_leaf);
     1218        m_nodes.pop_back();
     1219        m_nodes.pop_back();
     1220        }
     1221return(done);
     1222}
     1223
     1224//
     1225bool                    btSoftBody::rayCast(const btVector3& org,
     1226                                                                        const btVector3& dir,
     1227                                                                        sRayCast& results,
     1228                                                                        btScalar maxtime)
     1229{
     1230if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree();
     1231results.body    =       this;
     1232results.time    =       maxtime;
     1233results.feature =       eFeature::None;
     1234results.index   =       -1;
     1235return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0);
    12381236}
    12391237
    … …  
    12411239void                    btSoftBody::setSolver(eSolverPresets::_ preset)
    12421240{
    1243         m_cfg.m_vsequence.clear();
    1244         m_cfg.m_psequence.clear();
    1245         m_cfg.m_dsequence.clear();
    1246         switch(preset)
     1241m_cfg.m_vsequence.clear();
     1242m_cfg.m_psequence.clear();
     1243m_cfg.m_dsequence.clear();
     1244switch(preset)
    12471245        {
    12481246        case    eSolverPresets::Positions:
    1249                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
    1250                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
    1251                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
    1252                 m_cfg.m_psequence.push_back(ePSolver::Linear); 
    1253                 break; 
     1247        m_cfg.m_psequence.push_back(ePSolver::Anchors);
     1248        m_cfg.m_psequence.push_back(ePSolver::RContacts);
     1249        m_cfg.m_psequence.push_back(ePSolver::SContacts);
     1250        m_cfg.m_psequence.push_back(ePSolver::Linear); 
     1251        break; 
    12541252        case    eSolverPresets::Velocities:
    1255                 m_cfg.m_vsequence.push_back(eVSolver::Linear);
    1256 
    1257                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
    1258                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
    1259                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
    1260 
    1261                 m_cfg.m_dsequence.push_back(ePSolver::Linear);
    1262                 break;
     1253        m_cfg.m_vsequence.push_back(eVSolver::Linear);
     1254       
     1255        m_cfg.m_psequence.push_back(ePSolver::Anchors);
     1256        m_cfg.m_psequence.push_back(ePSolver::RContacts);
     1257        m_cfg.m_psequence.push_back(ePSolver::SContacts);
     1258       
     1259        m_cfg.m_dsequence.push_back(ePSolver::Linear);
     1260        break;
    12631261        }
    12641262}
    … …  
    12761274                m_fdbvt.clear();
    12771275                if(m_cfg.collisions&fCollision::VF_SS)
    1278                 {
     1276                        {
    12791277                        initializeFaceTree();                   
    1280                 }
    1281         }
    1282 
     1278                        }
     1279        }
     1280               
    12831281        /* Prepare                              */
    12841282        m_sst.sdt               =       dt*m_cfg.timescale;
    … …  
    13081306                Node&   n=m_nodes[i];
    13091307                m_ndbvt.update( n.m_leaf,
    1310                         btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
    1311                         n.m_v*m_sst.velmrg,
    1312                         m_sst.updmrg);
     1308                                                btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
     1309                                                n.m_v*m_sst.velmrg,
     1310                                                m_sst.updmrg);
    13131311        }
    13141312        /* Faces                                */
    13151313        if(!m_fdbvt.empty())
    1316         {
     1314                {
    13171315                for(int i=0;i<m_faces.size();++i)
    1318                 {
     1316                        {
    13191317                        Face&                   f=m_faces[i];
    13201318                        const btVector3 v=(     f.m_n[0]->m_v+
    1321                                 f.m_n[1]->m_v+
    1322                                 f.m_n[2]->m_v)/3;
     1319                                                                f.m_n[1]->m_v+
     1320                                                                f.m_n[2]->m_v)/3;
    13231321                        m_fdbvt.update( f.m_leaf,
    1324                                 VolumeOf(f,m_sst.radmrg),
    1325                                 v*m_sst.velmrg,
    1326                                 m_sst.updmrg);
    1327                 }
    1328         }
     1322                                                        VolumeOf(f,m_sst.radmrg),
     1323                                                        v*m_sst.velmrg,
     1324                                                        m_sst.updmrg);
     1325                        }
     1326                }
    13291327        /* Pose                                 */
    13301328        updatePose();
    13311329        /* Match                                */
    13321330        if(m_pose.m_bframe&&(m_cfg.kMT>0))
    1333         {
     1331                {
    13341332                const btMatrix3x3       posetrs=m_pose.m_rot;
    13351333                for(int i=0,ni=m_nodes.size();i<ni;++i)
    1336                 {
     1334                        {
    13371335                        Node&   n=m_nodes[i];
    13381336                        if(n.m_im>0)
    1339                         {
     1337                                {
    13401338                                const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
    13411339                                n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
    1342                         }
    1343                 }
    1344         }
     1340                                }
     1341                        }
     1342                }
    13451343        /* Clear contacts               */
    13461344        m_rcontacts.resize(0);
    … …  
    13551353void                    btSoftBody::solveConstraints()
    13561354{
    1357         /* Apply clusters               */
    1358         applyClusters(false);
    1359         /* Prepare links                */
    1360 
    1361         int i,ni;
    1362 
    1363         for(i=0,ni=m_links.size();i<ni;++i)
    1364         {
    1365                 Link&   l=m_links[i];
    1366                 l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
    1367                 l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
    1368         }
    1369         /* Prepare anchors              */
    1370         for(i=0,ni=m_anchors.size();i<ni;++i)
    1371         {
    1372                 Anchor&                 a=m_anchors[i];
    1373                 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
    1374                 a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
    1375                         a.m_node->m_im,
    1376                         a.m_body->getInvMass(),
    1377                         a.m_body->getInvInertiaTensorWorld(),
    1378                         ra);
    1379                 a.m_c1  =       ra;
    1380                 a.m_c2  =       m_sst.sdt*a.m_node->m_im;
    1381                 a.m_body->activate();
    1382         }
    1383         /* Solve velocities             */
    1384         if(m_cfg.viterations>0)
    1385         {
    1386                 /* Solve                        */
    1387                 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
    1388                 {
    1389                         for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
    1390                         {
    1391                                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
    1392                         }
    1393                 }
    1394                 /* Update                       */
    1395                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1396                 {
    1397                         Node&   n=m_nodes[i];
    1398                         n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
    1399                 }
    1400         }
    1401         /* Solve positions              */
    1402         if(m_cfg.piterations>0)
    1403         {
    1404                 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
    1405                 {
    1406                         const btScalar ti=isolve/(btScalar)m_cfg.piterations;
    1407                         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
    1408                         {
    1409                                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
    1410                         }
    1411                 }
    1412                 const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
    1413                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1414                 {
    1415                         Node&   n=m_nodes[i];
    1416                         n.m_v   =       (n.m_x-n.m_q)*vc;
    1417                         n.m_f   =       btVector3(0,0,0);               
    1418                 }
    1419         }
    1420         /* Solve drift                  */
    1421         if(m_cfg.diterations>0)
    1422         {
    1423                 const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
    1424                 for(i=0,ni=m_nodes.size();i<ni;++i)
    1425                 {
    1426                         Node&   n=m_nodes[i];
    1427                         n.m_q   =       n.m_x;
    1428                 }
    1429                 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
    1430                 {
    1431                         for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
    1432                         {
    1433                                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
    1434                         }
    1435                 }
    1436                 for(int i=0,ni=m_nodes.size();i<ni;++i)
    1437                 {
    1438                         Node&   n=m_nodes[i];
    1439                         n.m_v   +=      (n.m_x-n.m_q)*vcf;
    1440                 }
    1441         }
    1442         /* Apply clusters               */
    1443         dampClusters();
    1444         applyClusters(true);
     1355/* Apply clusters               */
     1356applyClusters(false);
     1357/* Prepare links                */
     1358
     1359int i,ni;
     1360
     1361for(i=0,ni=m_links.size();i<ni;++i)
     1362        {
     1363        Link&   l=m_links[i];
     1364        l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
     1365        l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
     1366        }
     1367/* Prepare anchors              */
     1368for(i=0,ni=m_anchors.size();i<ni;++i)
     1369        {
     1370        Anchor&                 a=m_anchors[i];
     1371        const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
     1372        a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
     1373                                                                a.m_node->m_im,
     1374                                                                a.m_body->getInvMass(),
     1375                                                                a.m_body->getInvInertiaTensorWorld(),
     1376                                                                ra);
     1377        a.m_c1  =       ra;
     1378        a.m_c2  =       m_sst.sdt*a.m_node->m_im;
     1379        a.m_body->activate();
     1380        }
     1381/* Solve velocities             */
     1382if(m_cfg.viterations>0)
     1383        {
     1384        /* Solve                        */
     1385        for(int isolve=0;isolve<m_cfg.viterations;++isolve)
     1386                {
     1387                for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
     1388                        {
     1389                        getSolver(m_cfg.m_vsequence[iseq])(this,1);
     1390                        }
     1391                }
     1392        /* Update                       */
     1393        for(i=0,ni=m_nodes.size();i<ni;++i)
     1394                {
     1395                Node&   n=m_nodes[i];
     1396                n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
     1397                }
     1398        }
     1399/* Solve positions              */
     1400if(m_cfg.piterations>0)
     1401        {
     1402        for(int isolve=0;isolve<m_cfg.piterations;++isolve)
     1403                {
     1404                const btScalar ti=isolve/(btScalar)m_cfg.piterations;
     1405                for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
     1406                        {
     1407                        getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
     1408                        }
     1409                }
     1410        const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
     1411        for(i=0,ni=m_nodes.size();i<ni;++i)
     1412                {
     1413                Node&   n=m_nodes[i];
     1414                n.m_v   =       (n.m_x-n.m_q)*vc;
     1415                n.m_f   =       btVector3(0,0,0);               
     1416                }
     1417        }
     1418/* Solve drift                  */
     1419if(m_cfg.diterations>0)
     1420        {
     1421        const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
     1422        for(i=0,ni=m_nodes.size();i<ni;++i)
     1423                {
     1424                Node&   n=m_nodes[i];
     1425                n.m_q   =       n.m_x;
     1426                }
     1427        for(int idrift=0;idrift<m_cfg.diterations;++idrift)
     1428                {
     1429                for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
     1430                        {
     1431                        getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
     1432                        }
     1433                }
     1434        for(int i=0,ni=m_nodes.size();i<ni;++i)
     1435                {
     1436                Node&   n=m_nodes[i];
     1437                n.m_v   +=      (n.m_x-n.m_q)*vcf;
     1438                }
     1439        }
     1440/* Apply clusters               */
     1441dampClusters();
     1442applyClusters(true);
    14451443}
    14461444
    … …  
    14481446void                    btSoftBody::staticSolve(int iterations)
    14491447{
    1450         for(int isolve=0;isolve<iterations;++isolve)
    1451         {
    1452                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
    1453                 {
    1454                         getSolver(m_cfg.m_psequence[iseq])(this,1,0);
     1448for(int isolve=0;isolve<iterations;++isolve)
     1449        {
     1450        for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
     1451                {
     1452                getSolver(m_cfg.m_psequence[iseq])(this,1,0);
    14551453                }
    14561454        }
    … …  
    14601458void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
    14611459{
    1462         /// placeholder
     1460/// placeholder
    14631461}
    14641462
    … …  
    14661464void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
    14671465{
    1468         const int       nb=bodies.size();
    1469         int                     iterations=0;
    1470         int i;
    1471 
    1472         for(i=0;i<nb;++i)
    1473         {
    1474                 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
    1475         }
    1476         for(i=0;i<nb;++i)
    1477         {
    1478                 bodies[i]->prepareClusters(iterations);
    1479         }
    1480         for(i=0;i<iterations;++i)
    1481         {
    1482                 const btScalar sor=1;
    1483                 for(int j=0;j<nb;++j)
    1484                 {
    1485                         bodies[j]->solveClusters(sor);
    1486                 }
    1487         }
    1488         for(i=0;i<nb;++i)
    1489         {
    1490                 bodies[i]->cleanupClusters();
     1466const int       nb=bodies.size();
     1467int                     iterations=0;
     1468int i;
     1469
     1470for(i=0;i<nb;++i)
     1471        {
     1472        iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
     1473        }
     1474for(i=0;i<nb;++i)
     1475        {
     1476        bodies[i]->prepareClusters(iterations);
     1477        }
     1478for(i=0;i<iterations;++i)
     1479        {
     1480        const btScalar sor=1;
     1481        for(int j=0;j<nb;++j)
     1482                {
     1483                bodies[j]->solveClusters(sor);
     1484                }
     1485        }
     1486for(i=0;i<nb;++i)
     1487        {
     1488        bodies[i]->cleanupClusters();
    14911489        }
    14921490}
    … …  
    15001498
    15011499//
    1502 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
    1503 {
    1504         m_rayFrom = rayFrom;
    1505         m_rayNormalizedDirection = (rayTo-rayFrom);
    1506         m_rayTo = rayTo;
    1507         m_mint  =       mxt;
    1508         m_face  =       0;
    1509         m_tests =       0;
    1510 }
    1511 
    1512 //
    1513 void                            btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
    1514 {
    1515         btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
    1516         const btScalar          t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
    1517                 f.m_n[0]->m_x,
    1518                 f.m_n[1]->m_x,
    1519                 f.m_n[2]->m_x,
    1520                 m_mint);
    1521         if((t>0)&&(t<m_mint))
    1522         {
    1523                 m_mint=t;m_face=&f;
    1524         }
    1525         ++m_tests;
    1526 }
    1527 
    1528 //
    1529 btScalar                        btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
    1530                                                                                                                                    const btVector3& rayTo,
    1531                                                                                                                                    const btVector3& rayNormalizedDirection,
    1532                                                                                                                                    const btVector3& a,
    1533                                                                                                                                    const btVector3& b,
    1534                                                                                                                                    const btVector3& c,
    1535                                                                                                                                    btScalar maxt)
     1500                                        btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
     1501{
     1502o               =       org;
     1503d               =       dir;
     1504mint    =       mxt;
     1505face    =       0;
     1506tests   =       0;
     1507}
     1508
     1509//
     1510void                            btSoftBody::RayCaster::Process(const btDbvtNode* leaf)
     1511{
     1512btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
     1513const btScalar          t=rayTriangle(  o,d,
     1514                                                                        f.m_n[0]->m_x,
     1515                                                                        f.m_n[1]->m_x,
     1516                                                                        f.m_n[2]->m_x,
     1517                                                                        mint);
     1518if((t>0)&&(t<mint)) { mint=t;face=&f; }
     1519++tests;
     1520}
     1521
     1522//
     1523btScalar                        btSoftBody::RayCaster::rayTriangle(     const btVector3& org,
     1524                                                                                                                const btVector3& dir,
     1525                                                                                                                const btVector3& a,
     1526                                                                                                                const btVector3& b,
     1527                                                                                                                const btVector3& c,
     1528                                                                                                                btScalar maxt)
    15361529{
    15371530        static const btScalar   ceps=-SIMD_EPSILON*10;
    15381531        static const btScalar   teps=SIMD_EPSILON*10;
    1539 
    15401532        const btVector3                 n=cross(b-a,c-a);
    15411533        const btScalar                  d=dot(a,n);
    1542         const btScalar                  den=dot(rayNormalizedDirection,n);
     1534        const btScalar                  den=dot(dir,n);
    15431535        if(!btFuzzyZero(den))
    15441536        {
    1545                 const btScalar          num=dot(rayFrom,n)-d;
     1537                const btScalar          num=dot(org,n)-d;
    15461538                const btScalar          t=-num/den;
    15471539                if((t>teps)&&(t<maxt))
    15481540                {
    1549                         const btVector3 hit=rayFrom+rayNormalizedDirection*t;
     1541                        const btVector3 hit=org+dir*t;
    15501542                        if(     (dot(n,cross(a-hit,b-hit))>ceps)        &&                     
    15511543                                (dot(n,cross(b-hit,c-hit))>ceps)        &&
    … …  
    15691561        {
    15701562                if(m_nodes[i].m_leaf)
    1571                 {
     1563                        {
    15721564                        m_nodes[i].m_leaf->data=*(void**)&i;
    1573                 }
     1565                        }
    15741566        }
    15751567        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    15841576                m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
    15851577                if(m_faces[i].m_leaf)
    1586                 {
     1578                        {
    15871579                        m_faces[i].m_leaf->data=*(void**)&i;
    1588                 }
     1580                        }
    15891581        }
    15901582        for(i=0,ni=m_anchors.size();i<ni;++i)
    1591         {
     1583                {
    15921584                m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
    1593         }
     1585                }
    15941586        for(i=0,ni=m_notes.size();i<ni;++i)
    1595         {
     1587                {
    15961588                for(int j=0;j<m_notes[i].m_rank;++j)
    1597                 {
     1589                        {
    15981590                        m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
    1599                 }
    1600         }
     1591                        }
     1592                }
    16011593#undef  PTR2IDX
    16021594}
    … …  
    16061598{
    16071599#define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
    1608         (&(_b_)[(((char*)_p_)-(char*)0)])
     1600                                                                (&(_b_)[(((char*)_p_)-(char*)0)])
    16091601        btSoftBody::Node*       base=&m_nodes[0];
    16101602        int i,ni;
    … …  
    16131605        {
    16141606                if(m_nodes[i].m_leaf)
    1615                 {
     1607                        {
    16161608                        m_nodes[i].m_leaf->data=&m_nodes[i];
    1617                 }
     1609                        }
    16181610        }
    16191611        for(i=0,ni=m_links.size();i<ni;++i)
    … …  
    16281620                m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
    16291621                if(m_faces[i].m_leaf)
    1630                 {
     1622                        {
    16311623                        m_faces[i].m_leaf->data=&m_faces[i];
    1632                 }
     1624                        }
    16331625        }
    16341626        for(i=0,ni=m_anchors.size();i<ni;++i)
    1635         {
     1627                {
    16361628                m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
    1637         }
     1629                }
    16381630        for(i=0,ni=m_notes.size();i<ni;++i)
    1639         {
     1631                {
    16401632                for(int j=0;j<m_notes[i].m_rank;++j)
    1641                 {
     1633                        {
    16421634                        m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
    1643                 }
    1644         }
     1635                        }
     1636                }
    16451637#undef  IDX2PTR
    16461638}
    16471639
    16481640//
    1649 int                                     btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
     1641int                                     btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
    16501642                                                                                btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
    16511643{
    16521644        int     cnt=0;
    16531645        if(bcountonly||m_fdbvt.empty())
    1654         {/* Full search */
    1655                 btVector3 dir = rayTo-rayFrom;
    1656                 dir.normalize();
    1657 
     1646                {/* Full search */
    16581647                for(int i=0,ni=m_faces.size();i<ni;++i)
    1659                 {
     1648                        {
    16601649                        const btSoftBody::Face& f=m_faces[i];
    1661 
    1662                         const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
    1663                                 f.m_n[0]->m_x,
    1664                                 f.m_n[1]->m_x,
    1665                                 f.m_n[2]->m_x,
    1666                                 mint);
     1650                        const btScalar                  t=RayCaster::rayTriangle(       org,dir,
     1651                                                                                                                                f.m_n[0]->m_x,
     1652                                                                                                                                f.m_n[1]->m_x,
     1653                                                                                                                                f.m_n[2]->m_x,
     1654                                                                                                                                mint);
    16671655                        if(t>0)
    1668                         {
     1656                                {
    16691657                                ++cnt;
    16701658                                if(!bcountonly)
    1671                                 {
     1659                                        {
    16721660                                        feature=btSoftBody::eFeature::Face;
    16731661                                        index=i;
    16741662                                        mint=t;
     1663                                        }
    16751664                                }
    16761665                        }
    16771666                }
    1678         }
    1679         else
    1680         {/* Use dbvt    */
    1681                 RayFromToCaster collider(rayFrom,rayTo,mint);
    1682 
    1683                 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
    1684                 if(collider.m_face)
    1685                 {
    1686                         mint=collider.m_mint;
     1667                else
     1668                {/* Use dbvt    */
     1669                RayCaster       collider(org,dir,mint);
     1670                btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider);
     1671                if(collider.face)
     1672                        {
     1673                        mint=collider.mint;
    16871674                        feature=btSoftBody::eFeature::Face;
    1688                         index=(int)(collider.m_face-&m_faces[0]);
     1675                        index=(int)(collider.face-&m_faces[0]);
    16891676                        cnt=1;
    1690                 }
    1691         }
     1677                        }
     1678                }
    16921679        return(cnt);
    16931680}
    … …  
    16961683void                    btSoftBody::initializeFaceTree()
    16971684{
    1698         m_fdbvt.clear();
    1699         for(int i=0;i<m_faces.size();++i)
    1700         {
    1701                 Face&   f=m_faces[i];
    1702                 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
     1685m_fdbvt.clear();
     1686for(int i=0;i<m_faces.size();++i)
     1687        {
     1688        Face&   f=m_faces[i];
     1689        f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
    17031690        }
    17041691}
    … …  
    17071694btVector3               btSoftBody::evaluateCom() const
    17081695{
    1709         btVector3       com(0,0,0);
    1710         if(m_pose.m_bframe)
    1711         {
    1712                 for(int i=0,ni=m_nodes.size();i<ni;++i)
    1713                 {
    1714                         com+=m_nodes[i].m_x*m_pose.m_wgh[i];
     1696btVector3       com(0,0,0);
     1697if(m_pose.m_bframe)
     1698        {
     1699        for(int i=0,ni=m_nodes.size();i<ni;++i)
     1700                {
     1701                com+=m_nodes[i].m_x*m_pose.m_wgh[i];
    17151702                }
    17161703        }
    17171704        return(com);
    17181705}
    1719 
     1706       
    17201707//
    17211708bool                            btSoftBody::checkContact(       btRigidBody* prb,
    1722                                                                                          const btVector3& x,
    1723                                                                                          btScalar margin,
    1724                                                                                          btSoftBody::sCti& cti) const
     1709                                                                                                const btVector3& x,
     1710                                                                                                btScalar margin,
     1711                                                                                                btSoftBody::sCti& cti) const
    17251712{
    17261713        btVector3                       nrm;
    … …  
    17281715        const btTransform&      wtr=prb->getInterpolationWorldTransform();
    17291716        btScalar                        dst=m_worldInfo->m_sparsesdf.Evaluate(  wtr.invXform(x),
    1730                 shp,
    1731                 nrm,
    1732                 margin);
     1717                                                                                                                                shp,
     1718                                                                                                                                nrm,
     1719                                                                                                                                margin);
    17331720        if(dst<0)
    17341721        {
    … …  
    17361723                cti.m_normal    =       wtr.getBasis()*nrm;
    17371724                cti.m_offset    =       -dot(   cti.m_normal,
    1738                         x-cti.m_normal*dst);
     1725                                                                        x-cti.m_normal*dst);
    17391726                return(true);
    17401727        }
    … …  
    17561743                btSoftBody::Face&       f=m_faces[i];
    17571744                const btVector3         n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
    1758                         f.m_n[2]->m_x-f.m_n[0]->m_x);
     1745                                                                        f.m_n[2]->m_x-f.m_n[0]->m_x);
    17591746                f.m_normal=n.normalized();
    17601747                f.m_n[0]->m_n+=n;
    … …  
    17741761{
    17751762        if(m_ndbvt.m_root)
    1776         {
     1763                {
    17771764                const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
    17781765                const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
    17791766                const btScalar          csm=getCollisionShape()->getMargin();
    17801767                const btVector3         mrg=btVector3(  csm,
    1781                         csm,
    1782                         csm)*1; // ??? to investigate...
     1768                                                                                        csm,
     1769                                                                                        csm)*1; // ??? to investigate...
    17831770                m_bounds[0]=mins-mrg;
    17841771                m_bounds[1]=maxs+mrg;
    1785                 if(0!=getBroadphaseHandle())
    1786                 {                                       
    1787                         m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
    1788                                 m_bounds[0],
    1789                                 m_bounds[1],
    1790                                 m_worldInfo->m_dispatcher);
    1791                 }
    1792         }
    1793         else
    1794         {
     1772                        if(0!=getBroadphaseHandle())
     1773                        {                                       
     1774                                m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
     1775                                                                                                        m_bounds[0],
     1776                                                                                                        m_bounds[1],
     1777                                                                                                        m_worldInfo->m_dispatcher);
     1778                        }
     1779                }
     1780                else
     1781                {
    17951782                m_bounds[0]=
    1796                         m_bounds[1]=btVector3(0,0,0);
    1797         }               
     1783                m_bounds[1]=btVector3(0,0,0);
     1784                }               
    17981785}
    17991786
    … …  
    18261813                pose.m_scl=pose.m_aqq*r.transpose()*Apq;
    18271814                if(m_cfg.maxvolume>1)
    1828                 {
     1815                        {
    18291816                        const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
    1830                                 1,m_cfg.maxvolume);
     1817                                                                                                        1,m_cfg.maxvolume);
    18311818                        pose.m_scl=Mul(pose.m_scl,idet);
    1832                 }
    1833 
     1819                        }
     1820               
    18341821        }
    18351822}
    … …  
    18861873        int i;
    18871874
    1888         for( i=0;i<m_clusters.size();++i)
    1889         {
    1890                 Cluster&        c=*m_clusters[i];
    1891                 c.m_imass=0;
    1892                 c.m_masses.resize(c.m_nodes.size());
    1893                 for(int j=0;j<c.m_nodes.size();++j)
    1894                 {
    1895                         c.m_masses[j]   =       c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
    1896                         c.m_imass               +=      c.m_masses[j];
    1897                 }
    1898                 c.m_imass               =       1/c.m_imass;
    1899                 c.m_com                 =       btSoftBody::clusterCom(&c);
    1900                 c.m_lv                  =       btVector3(0,0,0);
    1901                 c.m_av                  =       btVector3(0,0,0);
    1902                 c.m_leaf                =       0;
    1903                 /* Inertia      */
    1904                 btMatrix3x3&    ii=c.m_locii;
    1905                 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
    1906                 {
    1907                         int i,ni;
    1908 
    1909                         for(i=0,ni=c.m_nodes.size();i<ni;++i)
    1910                         {
    1911                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
    1912                                 const btVector3 q=k*k;
    1913                                 const btScalar  m=c.m_masses[i];
    1914                                 ii[0][0]        +=      m*(q[1]+q[2]);
    1915                                 ii[1][1]        +=      m*(q[0]+q[2]);
    1916                                 ii[2][2]        +=      m*(q[0]+q[1]);
    1917                                 ii[0][1]        -=      m*k[0]*k[1];
    1918                                 ii[0][2]        -=      m*k[0]*k[2];
    1919                                 ii[1][2]        -=      m*k[1]*k[2];
    1920                         }
    1921                 }
    1922                 ii[1][0]=ii[0][1];
    1923                 ii[2][0]=ii[0][2];
    1924                 ii[2][1]=ii[1][2];
    1925                 ii=ii.inverse();
    1926                 /* Frame        */
    1927                 c.m_framexform.setIdentity();
     1875for( i=0;i<m_clusters.size();++i)
     1876        {
     1877        Cluster&        c=*m_clusters[i];
     1878        c.m_imass=0;
     1879        c.m_masses.resize(c.m_nodes.size());
     1880        for(int j=0;j<c.m_nodes.size();++j)
     1881                {
     1882                c.m_masses[j]   =       c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
     1883                c.m_imass               +=      c.m_masses[j];
     1884                }
     1885        c.m_imass               =       1/c.m_imass;
     1886        c.m_com                 =       btSoftBody::clusterCom(&c);
     1887        c.m_lv                  =       btVector3(0,0,0);
     1888        c.m_av                  =       btVector3(0,0,0);
     1889        c.m_leaf                =       0;
     1890        /* Inertia      */
     1891        btMatrix3x3&    ii=c.m_locii;
     1892        ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
     1893        {
     1894                int i,ni;
     1895
     1896                for(i=0,ni=c.m_nodes.size();i<ni;++i)
     1897                {
     1898                        const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
     1899                        const btVector3 q=k*k;
     1900                        const btScalar  m=c.m_masses[i];
     1901                        ii[0][0]        +=      m*(q[1]+q[2]);
     1902                        ii[1][1]        +=      m*(q[0]+q[2]);
     1903                        ii[2][2]        +=      m*(q[0]+q[1]);
     1904                        ii[0][1]        -=      m*k[0]*k[1];
     1905                        ii[0][2]        -=      m*k[0]*k[2];
     1906                        ii[1][2]        -=      m*k[1]*k[2];
     1907                }
     1908        }
     1909        ii[1][0]=ii[0][1];
     1910        ii[2][0]=ii[0][2];
     1911        ii[2][1]=ii[1][2];
     1912        ii=ii.inverse();
     1913        /* Frame        */
     1914        c.m_framexform.setIdentity();
     1915        c.m_framexform.setOrigin(c.m_com);
     1916        c.m_framerefs.resize(c.m_nodes.size());
     1917        {
     1918                int i;
     1919                for(i=0;i<c.m_framerefs.size();++i)
     1920                        {
     1921                        c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
     1922                        }
     1923                }
     1924        }
     1925}
     1926
     1927//
     1928void                                    btSoftBody::updateClusters()
     1929{
     1930BT_PROFILE("UpdateClusters");
     1931int i;
     1932
     1933for(i=0;i<m_clusters.size();++i)
     1934        {
     1935        btSoftBody::Cluster&    c=*m_clusters[i];
     1936        const int                               n=c.m_nodes.size();
     1937        const btScalar                  invn=1/(btScalar)n;
     1938        if(n)
     1939                {
     1940                /* Frame                                */
     1941                const btScalar  eps=btScalar(0.0001);
     1942                btMatrix3x3             m,r,s;
     1943                m[0]=m[1]=m[2]=btVector3(0,0,0);
     1944                m[0][0]=eps*1;
     1945                m[1][1]=eps*2;
     1946                m[2][2]=eps*3;
     1947                c.m_com=clusterCom(&c);
     1948                for(int i=0;i<c.m_nodes.size();++i)
     1949                        {
     1950                        const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
     1951                        const btVector3&        b=c.m_framerefs[i];
     1952                        m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
     1953                        }
     1954                PolarDecompose(m,r,s);
    19281955                c.m_framexform.setOrigin(c.m_com);
    1929                 c.m_framerefs.resize(c.m_nodes.size());
    1930                 {
    1931                         int i;
    1932                         for(i=0;i<c.m_framerefs.size();++i)
    1933                         {
    1934                                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
    1935                         }
    1936                 }
    1937         }
    1938 }
    1939 
    1940 //
    1941 void                                    btSoftBody::updateClusters()
    1942 {
    1943         BT_PROFILE("UpdateClusters");
    1944         int i;
    1945 
    1946         for(i=0;i<m_clusters.size();++i)
    1947         {
    1948                 btSoftBody::Cluster&    c=*m_clusters[i];
    1949                 const int                               n=c.m_nodes.size();
    1950                 const btScalar                  invn=1/(btScalar)n;
    1951                 if(n)
    1952                 {
    1953                         /* Frame                                */
    1954                         const btScalar  eps=btScalar(0.0001);
    1955                         btMatrix3x3             m,r,s;
    1956                         m[0]=m[1]=m[2]=btVector3(0,0,0);
    1957                         m[0][0]=eps*1;
    1958                         m[1][1]=eps*2;
    1959                         m[2][2]=eps*3;
    1960                         c.m_com=clusterCom(&c);
    1961                         for(int i=0;i<c.m_nodes.size();++i)
    1962                         {
    1963                                 const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
    1964                                 const btVector3&        b=c.m_framerefs[i];
    1965                                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
    1966                         }
    1967                         PolarDecompose(m,r,s);
    1968                         c.m_framexform.setOrigin(c.m_com);
    1969                         c.m_framexform.setBasis(r);             
    1970                         /* Inertia                      */
    1971 #if 1/* Constant        */
    1972                         c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
    1973 #else
    1974 #if 0/* Sphere  */
     1956                c.m_framexform.setBasis(r);             
     1957                /* Inertia                      */
     1958                #if 1/* Constant        */
     1959                c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
     1960                #else
     1961                        #if 0/* Sphere  */
    19751962                        const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
    19761963                        const btVector3 inertia(rk,rk,rk);
    19771964                        const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
    1978                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
    1979                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
    1980 
     1965                                                                btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
     1966                                                                btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
     1967                       
    19811968                        c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
    1982 #else/* Actual  */             
     1969                        #else/* Actual  */             
    19831970                        c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
    19841971                        for(int i=0;i<n;++i)
    1985                         {
     1972                                {
    19861973                                const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
    19871974                                const btVector3         q=k*k;
    … …  
    19931980                                c.m_invwi[0][2] -=      m*k[0]*k[2];
    19941981                                c.m_invwi[1][2] -=      m*k[1]*k[2];
    1995                         }
     1982                                }
    19961983                        c.m_invwi[1][0]=c.m_invwi[0][1];
    19971984                        c.m_invwi[2][0]=c.m_invwi[0][2];
    19981985                        c.m_invwi[2][1]=c.m_invwi[1][2];
    19991986                        c.m_invwi=c.m_invwi.inverse();
    2000 #endif
    2001 #endif
    2002                         /* Velocities                   */
    2003                         c.m_lv=btVector3(0,0,0);
    2004                         c.m_av=btVector3(0,0,0);
    2005                         {
    2006                                 int i;
    2007 
    2008                                 for(i=0;i<n;++i)
     1987                        #endif
     1988                #endif
     1989                /* Velocities                   */
     1990                c.m_lv=btVector3(0,0,0);
     1991                c.m_av=btVector3(0,0,0);
     1992                {
     1993                        int i;
     1994
     1995                        for(i=0;i<n;++i)
     1996                        {
     1997                                const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
     1998                                c.m_lv  +=      v;
     1999                                c.m_av  +=      cross(c.m_nodes[i]->m_x-c.m_com,v);
     2000                        }
     2001                }
     2002                c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
     2003                c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
     2004                c.m_vimpulses[0]        =
     2005                c.m_vimpulses[1]        = btVector3(0,0,0);
     2006                c.m_dimpulses[0]        =
     2007                c.m_dimpulses[1]        = btVector3(0,0,0);
     2008                c.m_nvimpulses          = 0;
     2009                c.m_ndimpulses          = 0;
     2010                /* Matching                             */
     2011                if(c.m_matching>0)
     2012                        {
     2013                        for(int j=0;j<c.m_nodes.size();++j)
    20092014                                {
    2010                                         const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
    2011                                         c.m_lv  +=      v;
    2012                                         c.m_av  +=      cross(c.m_nodes[i]->m_x-c.m_com,v);
     2015                                Node&                   n=*c.m_nodes[j];
     2016                                const btVector3 x=c.m_framexform*c.m_framerefs[j];
     2017                                n.m_x=Lerp(n.m_x,x,c.m_matching);
    20132018                                }
    20142019                        }
    2015                         c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
    2016                         c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
    2017                         c.m_vimpulses[0]        =
    2018                                 c.m_vimpulses[1]        = btVector3(0,0,0);
    2019                         c.m_dimpulses[0]        =
    2020                                 c.m_dimpulses[1]        = btVector3(0,0,0);
    2021                         c.m_nvimpulses          = 0;
    2022                         c.m_ndimpulses          = 0;
    2023                         /* Matching                             */
    2024                         if(c.m_matching>0)
    2025                         {
    2026                                 for(int j=0;j<c.m_nodes.size();++j)
     2020                /* Dbvt                                 */
     2021                if(c.m_collide)
     2022                        {
     2023                        btVector3       mi=c.m_nodes[0]->m_x;
     2024                        btVector3       mx=mi;
     2025                        for(int j=1;j<n;++j)
    20272026                                {
    2028                                         Node&                   n=*c.m_nodes[j];
    2029                                         const btVector3 x=c.m_framexform*c.m_framerefs[j];
    2030                                         n.m_x=Lerp(n.m_x,x,c.m_matching);
     2027                                mi.setMin(c.m_nodes[j]->m_x);
     2028                                mx.setMax(c.m_nodes[j]->m_x);
     2029                                }                       
     2030                        const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
     2031                        if(c.m_leaf)
     2032                                m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
     2033                                else
     2034                                c.m_leaf=m_cdbvt.insert(bounds,&c);
     2035                        }
     2036                }
     2037        }
     2038}
     2039
     2040//
     2041void                                    btSoftBody::cleanupClusters()
     2042{
     2043for(int i=0;i<m_joints.size();++i)
     2044        {
     2045        m_joints[i]->Terminate(m_sst.sdt);
     2046        if(m_joints[i]->m_delete)
     2047                {
     2048                btAlignedFree(m_joints[i]);
     2049                m_joints.remove(m_joints[i--]);
     2050                }       
     2051        }
     2052}
     2053
     2054//
     2055void                                    btSoftBody::prepareClusters(int iterations)
     2056{
     2057for(int i=0;i<m_joints.size();++i)
     2058        {
     2059        m_joints[i]->Prepare(m_sst.sdt,iterations);
     2060        }
     2061}
     2062
     2063
     2064//
     2065void                                    btSoftBody::solveClusters(btScalar sor)
     2066{
     2067for(int i=0,ni=m_joints.size();i<ni;++i)
     2068        {
     2069        m_joints[i]->Solve(m_sst.sdt,sor);
     2070        }
     2071}
     2072
     2073//
     2074void                                    btSoftBody::applyClusters(bool drift)
     2075{
     2076BT_PROFILE("ApplyClusters");
     2077const btScalar                                  f0=m_sst.sdt;
     2078const btScalar                                  f1=f0/2;
     2079btAlignedObjectArray<btVector3> deltas;
     2080btAlignedObjectArray<btScalar>  weights;
     2081deltas.resize(m_nodes.size(),btVector3(0,0,0));
     2082weights.resize(m_nodes.size(),0);
     2083int i;
     2084
     2085if(drift)
     2086        {
     2087        for(i=0;i<m_clusters.size();++i)
     2088                {
     2089                Cluster&        c=*m_clusters[i];
     2090                if(c.m_ndimpulses)
     2091                        {
     2092                        c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
     2093                        c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
     2094                        }
     2095                }
     2096        }
     2097for(i=0;i<m_clusters.size();++i)
     2098        {
     2099        Cluster&        c=*m_clusters[i];       
     2100        if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
     2101                {
     2102                const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
     2103                const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
     2104                for(int j=0;j<c.m_nodes.size();++j)
     2105                        {
     2106                        const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
     2107                        const btVector3&        x=c.m_nodes[j]->m_x;
     2108                        const btScalar          q=c.m_masses[j];
     2109                        deltas[idx]             +=      (v+cross(w,x-c.m_com))*q;
     2110                        weights[idx]    +=      q;
     2111                        }
     2112                }
     2113        }
     2114        for(i=0;i<deltas.size();++i)
     2115        {
     2116                if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
     2117        }
     2118}
     2119
     2120//
     2121void                                    btSoftBody::dampClusters()
     2122{
     2123        int i;
     2124
     2125for(i=0;i<m_clusters.size();++i)
     2126        {
     2127        Cluster&        c=*m_clusters[i];       
     2128        if(c.m_ndamping>0)
     2129                {
     2130                for(int j=0;j<c.m_nodes.size();++j)
     2131                        {
     2132                        Node&                   n=*c.m_nodes[j];
     2133                        if(n.m_im>0)
     2134                                {
     2135                                const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
     2136                                n.m_v   +=      c.m_ndamping*(vx-n.m_v);
    20312137                                }
    20322138                        }
    2033                         /* Dbvt                                 */
    2034                         if(c.m_collide)
    2035                         {
    2036                                 btVector3       mi=c.m_nodes[0]->m_x;
    2037                                 btVector3       mx=mi;
    2038                                 for(int j=1;j<n;++j)
    2039                                 {
    2040                                         mi.setMin(c.m_nodes[j]->m_x);
    2041                                         mx.setMax(c.m_nodes[j]->m_x);
    2042                                 }                       
    2043                                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
    2044                                 if(c.m_leaf)
    2045                                         m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
    2046                                 else
    2047                                         c.m_leaf=m_cdbvt.insert(bounds,&c);
    2048                         }
    2049                 }
    2050         }
    2051 }
    2052 
    2053 //
    2054 void                                    btSoftBody::cleanupClusters()
    2055 {
    2056         for(int i=0;i<m_joints.size();++i)
    2057         {
    2058                 m_joints[i]->Terminate(m_sst.sdt);
    2059                 if(m_joints[i]->m_delete)
    2060                 {
    2061                         btAlignedFree(m_joints[i]);
    2062                         m_joints.remove(m_joints[i--]);
    2063                 }       
    2064         }
    2065 }
    2066 
    2067 //
    2068 void                                    btSoftBody::prepareClusters(int iterations)
    2069 {
    2070         for(int i=0;i<m_joints.size();++i)
    2071         {
    2072                 m_joints[i]->Prepare(m_sst.sdt,iterations);
    2073         }
    2074 }
    2075 
    2076 
    2077 //
    2078 void                                    btSoftBody::solveClusters(btScalar sor)
    2079 {
    2080         for(int i=0,ni=m_joints.size();i<ni;++i)
    2081         {
    2082                 m_joints[i]->Solve(m_sst.sdt,sor);
    2083         }
    2084 }
    2085 
    2086 //
    2087 void                                    btSoftBody::applyClusters(bool drift)
    2088 {
    2089         BT_PROFILE("ApplyClusters");
    2090         const btScalar                                  f0=m_sst.sdt;
    2091         const btScalar                                  f1=f0/2;
    2092         btAlignedObjectArray<btVector3> deltas;
    2093         btAlignedObjectArray<btScalar>  weights;
    2094         deltas.resize(m_nodes.size(),btVector3(0,0,0));
    2095         weights.resize(m_nodes.size(),0);
    2096         int i;
    2097 
    2098         if(drift)
    2099         {
    2100                 for(i=0;i<m_clusters.size();++i)
    2101                 {
    2102                         Cluster&        c=*m_clusters[i];
    2103                         if(c.m_ndimpulses)
    2104                         {
    2105                                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
    2106                                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
    2107                         }
    2108                 }
    2109         }
    2110         for(i=0;i<m_clusters.size();++i)
    2111         {
    2112                 Cluster&        c=*m_clusters[i];       
    2113                 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
    2114                 {
    2115                         const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
    2116                         const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
    2117                         for(int j=0;j<c.m_nodes.size();++j)
    2118                         {
    2119                                 const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
    2120                                 const btVector3&        x=c.m_nodes[j]->m_x;
    2121                                 const btScalar          q=c.m_masses[j];
    2122                                 deltas[idx]             +=      (v+cross(w,x-c.m_com))*q;
    2123                                 weights[idx]    +=      q;
    2124                         }
    2125                 }
    2126         }
    2127         for(i=0;i<deltas.size();++i)
    2128         {
    2129                 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
    2130         }
    2131 }
    2132 
    2133 //
    2134 void                                    btSoftBody::dampClusters()
    2135 {
    2136         int i;
    2137 
    2138         for(i=0;i<m_clusters.size();++i)
    2139         {
    2140                 Cluster&        c=*m_clusters[i];       
    2141                 if(c.m_ndamping>0)
    2142                 {
    2143                         for(int j=0;j<c.m_nodes.size();++j)
    2144                         {
    2145                                 Node&                   n=*c.m_nodes[j];
    2146                                 if(n.m_im>0)
    2147                                 {
    2148                                         const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
    2149                                         n.m_v   +=      c.m_ndamping*(vx-n.m_v);
    2150                                 }
    2151                         }
    21522139                }
    21532140        }
    … …  
    21572144void                            btSoftBody::Joint::Prepare(btScalar dt,int)
    21582145{
    2159         m_bodies[0].activate();
    2160         m_bodies[1].activate();
     2146m_bodies[0].activate();
     2147m_bodies[1].activate();
    21612148}
    21622149
    … …  
    21642151void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
    21652152{
    2166         static const btScalar   maxdrift=4;
    2167         Joint::Prepare(dt,iterations);
    2168         m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
    2169         m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
    2170         m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
    2171         m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
    2172         m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
    2173         m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
    2174                 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
     2153static const btScalar   maxdrift=4;
     2154Joint::Prepare(dt,iterations);
     2155m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
     2156m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
     2157m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
     2158m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
     2159m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
     2160m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
     2161                                                                        m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
     2162if(m_split>0)
     2163        {
     2164        m_sdrift        =       m_massmatrix*(m_drift*m_split);
     2165        m_drift         *=      1-m_split;
     2166        }
     2167m_drift /=(btScalar)iterations;
     2168}
     2169
     2170//
     2171void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
     2172{
     2173const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
     2174const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
     2175const btVector3         vr=va-vb;
     2176btSoftBody::Impulse     impulse;
     2177impulse.m_asVelocity    =       1;
     2178impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
     2179m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
     2180m_bodies[1].applyImpulse( impulse,m_rpos[1]);
     2181}
     2182
     2183//
     2184void                            btSoftBody::LJoint::Terminate(btScalar dt)
     2185{
     2186if(m_split>0)
     2187        {
     2188        m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
     2189        m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
     2190        }
     2191}
     2192
     2193//
     2194void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
     2195{
     2196static const btScalar   maxdrift=SIMD_PI/16;
     2197m_icontrol->Prepare(this);
     2198Joint::Prepare(dt,iterations);
     2199m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
     2200m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
     2201m_drift         =       NormalizeAny(cross(m_axis[1],m_axis[0]));
     2202m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
     2203m_drift         *=      m_erp/dt;
     2204m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
     2205if(m_split>0)
     2206        {
     2207        m_sdrift        =       m_massmatrix*(m_drift*m_split);
     2208        m_drift         *=      1-m_split;
     2209        }
     2210m_drift /=(btScalar)iterations;
     2211}
     2212
     2213//
     2214void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
     2215{
     2216const btVector3         va=m_bodies[0].angularVelocity();
     2217const btVector3         vb=m_bodies[1].angularVelocity();
     2218const btVector3         vr=va-vb;
     2219const btScalar          sp=dot(vr,m_axis[0]);
     2220const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
     2221btSoftBody::Impulse     impulse;
     2222impulse.m_asVelocity    =       1;
     2223impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
     2224m_bodies[0].applyAImpulse(-impulse);
     2225m_bodies[1].applyAImpulse( impulse);
     2226}
     2227
     2228//
     2229void                            btSoftBody::AJoint::Terminate(btScalar dt)
     2230{
     2231if(m_split>0)
     2232        {
     2233        m_bodies[0].applyDAImpulse(-m_sdrift);
     2234        m_bodies[1].applyDAImpulse( m_sdrift);
     2235        }
     2236}
     2237
     2238//
     2239void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
     2240{
     2241Joint::Prepare(dt,iterations);
     2242const bool      dodrift=(m_life==0);
     2243m_delete=(++m_life)>m_maxlife;
     2244if(dodrift)
     2245        {
     2246        m_drift=m_drift*m_erp/dt;
    21752247        if(m_split>0)
    2176         {
     2248                {
    21772249                m_sdrift        =       m_massmatrix*(m_drift*m_split);
    21782250                m_drift         *=      1-m_split;
    2179         }
    2180         m_drift /=(btScalar)iterations;
    2181 }
    2182 
    2183 //
    2184 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
    2185 {
    2186         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
    2187         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
    2188         const btVector3         vr=va-vb;
    2189         btSoftBody::Impulse     impulse;
    2190         impulse.m_asVelocity    =       1;
    2191         impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
    2192         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
    2193         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
    2194 }
    2195 
    2196 //
    2197 void                            btSoftBody::LJoint::Terminate(btScalar dt)
    2198 {
    2199         if(m_split>0)
    2200         {
    2201                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
    2202                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
    2203         }
    2204 }
    2205 
    2206 //
    2207 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
    2208 {
    2209         static const btScalar   maxdrift=SIMD_PI/16;
    2210         m_icontrol->Prepare(this);
    2211         Joint::Prepare(dt,iterations);
    2212         m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
    2213         m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
    2214         m_drift         =       NormalizeAny(cross(m_axis[1],m_axis[0]));
    2215         m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
    2216         m_drift         *=      m_erp/dt;
    2217         m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
    2218         if(m_split>0)
    2219         {
    2220                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
    2221                 m_drift         *=      1-m_split;
    2222         }
    2223         m_drift /=(btScalar)iterations;
    2224 }
    2225 
    2226 //
    2227 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
    2228 {
    2229         const btVector3         va=m_bodies[0].angularVelocity();
    2230         const btVector3         vb=m_bodies[1].angularVelocity();
    2231         const btVector3         vr=va-vb;
    2232         const btScalar          sp=dot(vr,m_axis[0]);
    2233         const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
    2234         btSoftBody::Impulse     impulse;
    2235         impulse.m_asVelocity    =       1;
    2236         impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
    2237         m_bodies[0].applyAImpulse(-impulse);
    2238         m_bodies[1].applyAImpulse( impulse);
    2239 }
    2240 
    2241 //
    2242 void                            btSoftBody::AJoint::Terminate(btScalar dt)
    2243 {
    2244         if(m_split>0)
    2245         {
    2246                 m_bodies[0].applyDAImpulse(-m_sdrift);
    2247                 m_bodies[1].applyDAImpulse( m_sdrift);
    2248         }
    2249 }
    2250 
    2251 //
    2252 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
    2253 {
    2254         Joint::Prepare(dt,iterations);
    2255         const bool      dodrift=(m_life==0);
    2256         m_delete=(++m_life)>m_maxlife;
    2257         if(dodrift)
    2258         {
    2259                 m_drift=m_drift*m_erp/dt;
    2260                 if(m_split>0)
    2261                 {
    2262                         m_sdrift        =       m_massmatrix*(m_drift*m_split);
    2263                         m_drift         *=      1-m_split;
    2264                 }
    2265                 m_drift/=(btScalar)iterations;
     2251                }
     2252        m_drift/=(btScalar)iterations;
    22662253        }
    22672254        else
    22682255        {
    2269                 m_drift=m_sdrift=btVector3(0,0,0);
     2256        m_drift=m_sdrift=btVector3(0,0,0);
    22702257        }
    22712258}
    … …  
    22742261void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
    22752262{
    2276         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
    2277         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
    2278         const btVector3         vrel=va-vb;
    2279         const btScalar          rvac=dot(vrel,m_normal);
    2280         btSoftBody::Impulse     impulse;
    2281         impulse.m_asVelocity    =       1;
    2282         impulse.m_velocity              =       m_drift;
    2283         if(rvac<0)
    2284         {
    2285                 const btVector3 iv=m_normal*rvac;
    2286                 const btVector3 fv=vrel-iv;
    2287                 impulse.m_velocity      +=      iv+fv*m_friction;
    2288         }
    2289         impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
    2290         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
    2291         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
     2263const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
     2264const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
     2265const btVector3         vrel=va-vb;
     2266const btScalar          rvac=dot(vrel,m_normal);
     2267btSoftBody::Impulse     impulse;
     2268impulse.m_asVelocity    =       1;
     2269impulse.m_velocity              =       m_drift;
     2270if(rvac<0)
     2271        {
     2272        const btVector3 iv=m_normal*rvac;
     2273        const btVector3 fv=vrel-iv;
     2274        impulse.m_velocity      +=      iv+fv*m_friction;
     2275        }
     2276impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
     2277m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
     2278m_bodies[1].applyImpulse( impulse,m_rpos[1]);
    22922279}
    22932280
    … …  
    22952282void                            btSoftBody::CJoint::Terminate(btScalar dt)
    22962283{
    2297         if(m_split>0)
    2298         {
    2299                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
    2300                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
     2284if(m_split>0)
     2285        {
     2286        m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
     2287        m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
    23012288        }
    23022289}
    … …  
    23162303        const bool                                              as_volume=kVC>0;
    23172304        const bool                                              as_aero=        as_lift         ||
    2318                 as_drag         ;
     2305                                                                                                as_drag         ;
    23192306        const bool                                              as_vaero=       as_aero         &&
    2320                 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
     2307                                                                                                (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
    23212308        const bool                                              as_faero=       as_aero         &&
    2322                 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
     2309                                                                                                (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
    23232310        const bool                                              use_medium=     as_aero;
    23242311        const bool                                              use_volume=     as_pressure     ||
    2325                 as_volume       ;
     2312                                                                                                as_volume       ;
    23262313        btScalar                                                volume=0;
    23272314        btScalar                                                ivolumetp=0;
    … …  
    24712458void                            btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
    24722459{
    2473         for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
    2474         {
    2475                 const SContact&         c=psb->m_scontacts[i];
    2476                 const btVector3&        nr=c.m_normal;
    2477                 Node&                           n=*c.m_node;
    2478                 Face&                           f=*c.m_face;
    2479                 const btVector3         p=BaryEval(     f.m_n[0]->m_x,
    2480                         f.m_n[1]->m_x,
    2481                         f.m_n[2]->m_x,
    2482                         c.m_weights);
    2483                 const btVector3         q=BaryEval(     f.m_n[0]->m_q,
    2484                         f.m_n[1]->m_q,
    2485                         f.m_n[2]->m_q,
    2486                         c.m_weights);                                                                                   
    2487                 const btVector3         vr=(n.m_x-n.m_q)-(p-q);
    2488                 btVector3                       corr(0,0,0);
    2489                 if(dot(vr,nr)<0)
    2490                 {
    2491                         const btScalar  j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
    2492                         corr+=c.m_normal*j;
    2493                 }
    2494                 corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
    2495                 n.m_x                   +=      corr*c.m_cfm[0];
    2496                 f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
    2497                 f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
    2498                 f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
     2460for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
     2461        {
     2462        const SContact&         c=psb->m_scontacts[i];
     2463        const btVector3&        nr=c.m_normal;
     2464        Node&                           n=*c.m_node;
     2465        Face&                           f=*c.m_face;
     2466        const btVector3         p=BaryEval(     f.m_n[0]->m_x,
     2467                                                                        f.m_n[1]->m_x,
     2468                                                                        f.m_n[2]->m_x,
     2469                                                                        c.m_weights);
     2470        const btVector3         q=BaryEval(     f.m_n[0]->m_q,
     2471                                                                        f.m_n[1]->m_q,
     2472                                                                        f.m_n[2]->m_q,
     2473                                                                        c.m_weights);                                                                                   
     2474        const btVector3         vr=(n.m_x-n.m_q)-(p-q);
     2475        btVector3                       corr(0,0,0);
     2476        if(dot(vr,nr)<0)
     2477                {
     2478                const btScalar  j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
     2479                corr+=c.m_normal*j;
     2480                }
     2481        corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
     2482        n.m_x                   +=      corr*c.m_cfm[0];
     2483        f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
     2484        f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
     2485        f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
    24992486        }
    25002487}
    … …  
    25232510void                            btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
    25242511{
    2525         for(int i=0,ni=psb->m_links.size();i<ni;++i)
     2512for(int i=0,ni=psb->m_links.size();i<ni;++i)
    25262513        {                       
    2527                 Link&                   l=psb->m_links[i];
    2528                 Node**                  n=l.m_n;
    2529                 const btScalar  j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
    2530                 n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
    2531                 n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
     2514        Link&                   l=psb->m_links[i];
     2515        Node**                  n=l.m_n;
     2516        const btScalar  j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
     2517        n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
     2518        n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
    25322519        }
    25332520}
    … …  
    25362523btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
    25372524{
    2538         switch(solver)
     2525switch(solver)
    25392526        {
    25402527        case    ePSolver::Anchors:              return(&btSoftBody::PSolve_Anchors);
    … …  
    25432530        case    ePSolver::SContacts:    return(&btSoftBody::PSolve_SContacts); 
    25442531        }
    2545         return(0);
     2532return(0);
    25462533}
    25472534
    … …  
    25492536btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
    25502537{
    2551         switch(solver)
     2538switch(solver)
    25522539        {
    25532540        case    eVSolver::Linear:               return(&btSoftBody::VSolve_Links);
    25542541        }
    2555         return(0);
     2542return(0);
    25562543}
    25572544
    … …  
    25592546void                    btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
    25602547{
    2561         switch(m_cfg.collisions&fCollision::RVSmask)
     2548switch(m_cfg.collisions&fCollision::RVSmask)
    25622549        {
    25632550        case    fCollision::SDF_RS:
    25642551                {
    2565                         btSoftColliders::CollideSDF_RS  docollide;             
    2566                         btRigidBody*            prb=btRigidBody::upcast(pco);
    2567                         const btTransform       wtr=prb->getInterpolationWorldTransform();
    2568                         const btTransform       ctr=prb->getWorldTransform();
    2569                         const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
    2570                         const btScalar          basemargin=getCollisionShape()->getMargin();
    2571                         btVector3                       mins;
    2572                         btVector3                       maxs;
    2573                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
    2574                         pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
    2575                                 mins,
    2576                                 maxs);
    2577                         volume=btDbvtVolume::FromMM(mins,maxs);
    2578                         volume.Expand(btVector3(basemargin,basemargin,basemargin));             
    2579                         docollide.psb           =       this;
    2580                         docollide.prb           =       prb;
    2581                         docollide.dynmargin     =       basemargin+timemargin;
    2582                         docollide.stamargin     =       basemargin;
    2583                         btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
    2584                 }
    2585                 break;
     2552                btSoftColliders::CollideSDF_RS  docollide;             
     2553                btRigidBody*            prb=btRigidBody::upcast(pco);
     2554                const btTransform       wtr=prb->getInterpolationWorldTransform();
     2555                const btTransform       ctr=prb->getWorldTransform();
     2556                const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
     2557                const btScalar          basemargin=getCollisionShape()->getMargin();
     2558                btVector3                       mins;
     2559                btVector3                       maxs;
     2560                ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
     2561                pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
     2562                                                                                        mins,
     2563                                                                                        maxs);
     2564                volume=btDbvtVolume::FromMM(mins,maxs);
     2565                volume.Expand(btVector3(basemargin,basemargin,basemargin));             
     2566                docollide.psb           =       this;
     2567                docollide.prb           =       prb;
     2568                docollide.dynmargin     =       basemargin+timemargin;
     2569                docollide.stamargin     =       basemargin;
     2570                btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
     2571                }
     2572        break;
    25862573        case    fCollision::CL_RS:
    25872574                {
    2588                         btSoftColliders::CollideCL_RS   collider;
    2589                         collider.Process(this,btRigidBody::upcast(pco));
    2590                 }
    2591                 break;
     2575                btSoftColliders::CollideCL_RS   collider;
     2576                collider.Process(this,btRigidBody::upcast(pco));
     2577                }
     2578        break;
    25922579        }
    25932580}
    … …  
    25962583void                    btSoftBody::defaultCollisionHandler(btSoftBody* psb)
    25972584{
    2598         const int cf=m_cfg.collisions&psb->m_cfg.collisions;
    2599         switch(cf&fCollision::SVSmask)
     2585const int cf=m_cfg.collisions&psb->m_cfg.collisions;
     2586switch(cf&fCollision::SVSmask)
    26002587        {
    26012588        case    fCollision::CL_SS:
    26022589                {
    2603                         btSoftColliders::CollideCL_SS   docollide;
    2604                         docollide.Process(this,psb);
    2605                 }
    2606                 break;
     2590                btSoftColliders::CollideCL_SS   docollide;
     2591                docollide.Process(this,psb);
     2592                }
     2593        break;
    26072594        case    fCollision::VF_SS:
    26082595                {
    2609                         btSoftColliders::CollideVF_SS   docollide;
    2610                         /* common                                       */
    2611                         docollide.mrg=  getCollisionShape()->getMargin()+
    2612                                 psb->getCollisionShape()->getMargin();
    2613                         /* psb0 nodes vs psb1 faces     */
    2614                         docollide.psb[0]=this;
    2615                         docollide.psb[1]=psb;
    2616                         btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
    2617                                 docollide.psb[1]->m_fdbvt.m_root,
    2618                                 docollide);
    2619                         /* psb1 nodes vs psb0 faces     */
    2620                         docollide.psb[0]=psb;
    2621                         docollide.psb[1]=this;
    2622                         btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
    2623                                 docollide.psb[1]->m_fdbvt.m_root,
    2624                                 docollide);
    2625                 }
    2626                 break;
    2627         }
    2628 }
     2596                btSoftColliders::CollideVF_SS   docollide;
     2597                /* common                                       */
     2598                docollide.mrg=  getCollisionShape()->getMargin()+
     2599                                                psb->getCollisionShape()->getMargin();
     2600                /* psb0 nodes vs psb1 faces     */
     2601                docollide.psb[0]=this;
     2602                docollide.psb[1]=psb;
     2603                btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
     2604                                                        docollide.psb[1]->m_fdbvt.m_root,
     2605                                                        docollide);
     2606                /* psb1 nodes vs psb0 faces     */
     2607                docollide.psb[0]=psb;
     2608                docollide.psb[1]=this;
     2609                btDbvt::collideTT(      docollide.psb[0]->m_ndbvt.m_root,
     2610                                                        docollide.psb[1]->m_fdbvt.m_root,
     2611                                                        docollide);
     2612                }
     2613        break;
     2614        }
     2615}

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.h

    r1963 r1972  
    6363                END
    6464        };};
    65 
     65       
    6666        ///eVSolver : velocities solvers
    6767        struct  eVSolver { enum _ {
    … …  
    6969                END
    7070        };};
    71 
     71       
    7272        ///ePSolver : positions solvers
    7373        struct  ePSolver { enum _ {
    … …  
    7878                END
    7979        };};
    80 
     80       
    8181        ///eSolverPresets
    8282        struct  eSolverPresets { enum _ {
    … …  
    8686                END
    8787        };};
    88 
     88       
    8989        ///eFeature
    9090        struct  eFeature { enum _ {
    … …  
    9595                END
    9696        };};
    97 
     97       
    9898        typedef btAlignedObjectArray<eVSolver::_>       tVSolverArray;
    9999        typedef btAlignedObjectArray<ePSolver::_>       tPSolverArray;
    100 
     100       
    101101        //
    102102        // Flags
    103103        //
    104 
     104       
    105105        ///fCollision
    106106        struct fCollision { enum _ {
    … …  
    108108                SDF_RS  =       0x0001, ///SDF based rigid vs soft
    109109                CL_RS   =       0x0002, ///Cluster vs convex rigid vs soft
    110 
     110               
    111111                SVSmask =       0x00f0, ///Rigid versus soft mask               
    112112                VF_SS   =       0x0010, ///Vertex vs face soft vs soft handling
    … …  
    116116                END
    117117        };};
    118 
     118       
    119119        ///fMaterial
    120120        struct fMaterial { enum _ {
    … …  
    124124                END
    125125        };};
    126 
     126               
    127127        //
    128128        // API Types
    129129        //
    130 
     130       
    131131        /* sRayCast             */
    132132        struct sRayCast
    … …  
    135135                eFeature::_     feature;        /// feature type
    136136                int                     index;          /// feature index
    137                 btScalar        fraction;               /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
    138         };
    139 
     137                btScalar        time;           /// time of impact (rayorg+raydir*time)
     138        };
     139       
    140140        /* ImplicitFn   */
    141141        struct  ImplicitFn
    … …  
    143143                virtual btScalar        Eval(const btVector3& x)=0;
    144144        };
    145 
     145       
    146146        //
    147147        // Internal types
    … …  
    181181                int                                             m_flags;                // Flags
    182182        };
    183 
     183               
    184184        /* Feature              */
    185185        struct  Feature : Element
    … …  
    297297                btScalar                                        m_matching;
    298298                bool                                            m_collide;
    299                 Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
     299                                                                        Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
    300300        };
    301301        /* Impulse              */
    … …  
    308308                Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0)       {}
    309309                Impulse                                         operator -() const
    310                 {
     310                        {
    311311                        Impulse i=*this;
    312312                        i.m_velocity=-i.m_velocity;
    313313                        i.m_drift=-i.m_drift;
    314314                        return(i);
    315                 }
     315                        }
    316316                Impulse                                         operator*(btScalar x) const
    317                 {
     317                        {
    318318                        Impulse i=*this;
    319319                        i.m_velocity*=x;
    320320                        i.m_drift*=x;
    321321                        return(i);
    322                 }
     322                        }
    323323        };
    324324        /* Body                 */
    … …  
    327327                Cluster*                m_soft;
    328328                btRigidBody*                            m_rigid;
    329                 Body() : m_soft(0),m_rigid(0)                           {}
    330                 Body(Cluster* p) : m_soft(p),m_rigid(0) {}
    331                 Body(btRigidBody* p) : m_soft(0),m_rigid(p)     {}
     329                                                                        Body() : m_soft(0),m_rigid(0)                           {}
     330                                                                        Body(Cluster* p) : m_soft(p),m_rigid(0) {}
     331                                                                        Body(btRigidBody* p) : m_soft(0),m_rigid(p)     {}
    332332                void                                            activate() const
    333                 {
     333                        {
    334334                        if(m_rigid) m_rigid->activate();
    335                 }
     335                        }
    336336                const btMatrix3x3&                      invWorldInertia() const
    337                 {
     337                        {
    338338                        static const btMatrix3x3        iwi(0,0,0,0,0,0,0,0,0);
    339339                        if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
    340340                        if(m_soft)      return(m_soft->m_invwi);
    341341                        return(iwi);
    342                 }
     342                        }
    343343                btScalar                                        invMass() const
    344                 {
     344                        {
    345345                        if(m_rigid) return(m_rigid->getInvMass());
    346346                        if(m_soft)      return(m_soft->m_imass);
    347347                        return(0);
    348                 }
     348                        }
    349349                const btTransform&                      xform() const
    350                 {
     350                        {
    351351                        static const btTransform        identity=btTransform::getIdentity();           
    352352                        if(m_rigid) return(m_rigid->getInterpolationWorldTransform());
    353353                        if(m_soft)      return(m_soft->m_framexform);
    354354                        return(identity);
    355                 }
     355                        }
    356356                btVector3                                       linearVelocity() const
    357                 {
     357                        {
    358358                        if(m_rigid) return(m_rigid->getLinearVelocity());
    359359                        if(m_soft)      return(m_soft->m_lv);
    360360                        return(btVector3(0,0,0));
    361                 }
     361                        }
    362362                btVector3                                       angularVelocity(const btVector3& rpos) const
    363                 {                       
     363                        {                       
    364364                        if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos));
    365365                        if(m_soft)      return(cross(m_soft->m_av,rpos));
    366366                        return(btVector3(0,0,0));
    367                 }
     367                        }
    368368                btVector3                                       angularVelocity() const
    369                 {                       
     369                        {                       
    370370                        if(m_rigid) return(m_rigid->getAngularVelocity());
    371371                        if(m_soft)      return(m_soft->m_av);
    372372                        return(btVector3(0,0,0));
    373                 }
     373                        }
    374374                btVector3                                       velocity(const btVector3& rpos) const
    375                 {
     375                        {
    376376                        return(linearVelocity()+angularVelocity(rpos));
    377                 }
     377                        }
    378378                void                                            applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
    379                 {
     379                        {
    380380                        if(m_rigid)     m_rigid->applyImpulse(impulse,rpos);
    381381                        if(m_soft)      btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
    382                 }
     382                        }
    383383                void                                            applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
    384                 {
     384                        {
    385385                        if(m_rigid)     m_rigid->applyImpulse(impulse,rpos);
    386386                        if(m_soft)      btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
    387                 }               
     387                        }               
    388388                void                                            applyImpulse(const Impulse& impulse,const btVector3& rpos) const
    389                 {
     389                        {
    390390                        if(impulse.m_asVelocity)        applyVImpulse(impulse.m_velocity,rpos);
    391391                        if(impulse.m_asDrift)           applyDImpulse(impulse.m_drift,rpos);
    392                 }
     392                        }
    393393                void                                            applyVAImpulse(const btVector3& impulse) const
    394                 {
     394                        {
    395395                        if(m_rigid)     m_rigid->applyTorqueImpulse(impulse);
    396396                        if(m_soft)      btSoftBody::clusterVAImpulse(m_soft,impulse);
    397                 }
     397                        }
    398398                void                                            applyDAImpulse(const btVector3& impulse) const
    399                 {
     399                        {
    400400                        if(m_rigid)     m_rigid->applyTorqueImpulse(impulse);
    401401                        if(m_soft)      btSoftBody::clusterDAImpulse(m_soft,impulse);
    402                 }
     402                        }
    403403                void                                            applyAImpulse(const Impulse& impulse) const
    404                 {
     404                        {
    405405                        if(impulse.m_asVelocity)        applyVAImpulse(impulse.m_velocity);
    406406                        if(impulse.m_asDrift)           applyDAImpulse(impulse.m_drift);
    407                 }
     407                        }
    408408                void                                            applyDCImpulse(const btVector3& impulse) const
    409                 {
     409                        {
    410410                        if(m_rigid)     m_rigid->applyCentralImpulse(impulse);
    411411                        if(m_soft)      btSoftBody::clusterDCImpulse(m_soft,impulse);
    412                 }
     412                        }
    413413        };
    414414        /* Joint                */
    … …  
    421421                };};
    422422                struct Specs
    423                 {
    424                         Specs() : erp(1),cfm(1),split(1) {}
     423                        {
     424                                                Specs() : erp(1),cfm(1),split(1) {}
    425425                        btScalar        erp;
    426426                        btScalar        cfm;
    427427                        btScalar        split;
    428                 };
     428                        };
    429429                Body                                            m_bodies[2];
    430430                btVector3                                       m_refs[2];
    … …  
    437437                bool                                            m_delete;
    438438                virtual                                         ~Joint() {}
    439                 Joint() : m_delete(false) {}
     439                                                                        Joint() : m_delete(false) {}
    440440                virtual void                            Prepare(btScalar dt,int iterations);
    441441                virtual void                            Solve(btScalar dt,btScalar sor)=0;
    … …  
    447447        {
    448448                struct Specs : Joint::Specs
    449                 {
     449                        {
    450450                        btVector3       position;
    451                 };             
     451                        };             
    452452                btVector3                                       m_rpos[2];
    453453                void                                            Prepare(btScalar dt,int iterations);
    … …  
    460460        {
    461461                struct IControl
    462                 {
     462                        {
    463463                        virtual void                    Prepare(AJoint*)                                {}
    464464                        virtual btScalar                Speed(AJoint*,btScalar current) { return(current); }
    465465                        static IControl*                Default()                                               { static IControl def;return(&def); }
    466                 };
     466                        };
    467467                struct Specs : Joint::Specs
    468                 {
    469                         Specs() : icontrol(IControl::Default()) {}
     468                        {
     469                                                Specs() : icontrol(IControl::Default()) {}
    470470                        btVector3       axis;
    471471                        IControl*       icontrol;
    472                 };             
     472                        };             
    473473                btVector3                                       m_axis[2];
    474474                IControl*                                       m_icontrol;
    … …  
    533533                btScalar                                updmrg;                 // Update margin
    534534        };     
    535         /// RayFromToCaster takes a ray from, ray to (instead of direction!)
    536         struct  RayFromToCaster : btDbvt::ICollide
    537         {
    538                 btVector3                       m_rayFrom;
    539                 btVector3                       m_rayTo;
    540                 btVector3                       m_rayNormalizedDirection;
    541                 btScalar                        m_mint;
    542                 Face*                           m_face;
    543                 int                                     m_tests;
    544                 RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
     535        /* RayCaster    */
     536        struct  RayCaster : btDbvt::ICollide
     537                {
     538                btVector3                       o;
     539                btVector3                       d;
     540                btScalar                        mint;
     541                Face*   face;
     542                int                                     tests;
     543                                                                RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt);
    545544                void                                    Process(const btDbvtNode* leaf);
    546 
    547                 static inline btScalar  rayFromToTriangle(const btVector3& rayFrom,
    548                         const btVector3& rayTo,
    549                         const btVector3& rayNormalizedDirection,
    550                         const btVector3& a,
    551                         const btVector3& b,
    552                         const btVector3& c,
    553                         btScalar maxt=SIMD_INFINITY);
    554         };
     545                static inline btScalar  rayTriangle(const btVector3& org,
     546                                                                                        const btVector3& dir,
     547                                                                                        const btVector3& a,
     548                                                                                        const btVector3& b,
     549                                                                                        const btVector3& c,
     550                                                                                        btScalar maxt=SIMD_INFINITY);
     551                };
    555552
    556553        //
    … …  
    598595        btDbvt                                  m_cdbvt;                // Clusters tree
    599596        tClusterArray                   m_clusters;             // Clusters
    600 
     597               
    601598        //
    602599        // Api
    603600        //
    604 
     601       
    605602        /* ctor                                                                                                                                 */
    606603        btSoftBody(     btSoftBodyWorldInfo* worldInfo,int node_count,
    607                 const btVector3* x,
    608                 const btScalar* m);
     604                                const btVector3* x,
     605                                const btScalar* m);
    609606        /* dtor                                                                                                                                 */
    610607        virtual ~btSoftBody();
    … …  
    626623                int node1) const;
    627624        bool                            checkLink(      const Node* node0,
    628                 const Node* node1) const;
     625                                                                        const Node* node1) const;
    629626        /* Check for existring face                                                                                             */
    630627        bool                            checkFace(      int node0,
    631                 int node1,
    632                 int node2) const;
     628                                                                        int node1,
     629                                                                        int node2) const;
    633630        /* Append material                                                                                                              */
    634631        Material*                       appendMaterial();
    635632        /* Append note                                                                                                                  */
    636633        void                            appendNote(     const char* text,
    637                 const btVector3& o,
    638                 const btVector4& c=btVector4(1,0,0,0),
    639                 Node* n0=0,
    640                 Node* n1=0,
    641                 Node* n2=0,
    642                 Node* n3=0);
     634                                                                        const btVector3& o,
     635                                                                        const btVector4& c=btVector4(1,0,0,0),
     636                                                                        Node* n0=0,
     637                                                                        Node* n1=0,
     638                                                                        Node* n2=0,
     639                                                                        Node* n3=0);
    643640        void                            appendNote(     const char* text,
    644                 const btVector3& o,
    645                 Node* feature);
     641                                                                        const btVector3& o,
     642                                                                        Node* feature);
    646643        void                            appendNote(     const char* text,
    647                 const btVector3& o,
    648                 Link* feature);
     644                                                                        const btVector3& o,
     645                                                                        Link* feature);
    649646        void                            appendNote(     const char* text,
    650                 const btVector3& o,
    651                 Face* feature);
     647                                                                        const btVector3& o,
     648                                                                        Face* feature);
    652649        /* Append node                                                                                                                  */
    653650        void                            appendNode(     const btVector3& x,btScalar m);
    … …  
    655652        void                            appendLink(int model=-1,Material* mat=0);
    656653        void                            appendLink(     int node0,
    657                 int node1,
    658                 Material* mat=0,
    659                 bool bcheckexist=false);
     654                                                                        int node1,
     655                                                                        Material* mat=0,
     656                                                                        bool bcheckexist=false);
    660657        void                            appendLink(     Node* node0,
    661                 Node* node1,
    662                 Material* mat=0,
    663                 bool bcheckexist=false);
     658                                                                        Node* node1,
     659                                                                        Material* mat=0,
     660                                                                        bool bcheckexist=false);
    664661        /* Append face                                                                                                                  */
    665662        void                            appendFace(int model=-1,Material* mat=0);
    666663        void                            appendFace(     int node0,
    667                 int node1,
    668                 int node2,
    669                 Material* mat=0);
     664                                                                        int node1,
     665                                                                        int node2,
     666                                                                        Material* mat=0);
    670667        /* Append anchor                                                                                                                */
    671668        void                            appendAnchor(   int node,
    672                 btRigidBody* body);
     669                                                                                btRigidBody* body);
    673670        /* Append linear joint                                                                                                  */
    674671        void                            appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
    … …  
    683680        /* Add force (or gravity) to a node of the body                                                 */
    684681        void                            addForce(               const btVector3& force,
    685                 int node);
     682                                                                                int node);
    686683        /* Add velocity to the entire body                                                                              */
    687684        void                            addVelocity(    const btVector3& velocity);
    … …  
    692689        /* Add velocity to a node of the body                                                                   */
    693690        void                            addVelocity(    const btVector3& velocity,
    694                 int node);
     691                                                                                int node);
    695692        /* Set mass                                                                                                                             */
    696693        void                            setMass(                int node,
    697                 btScalar mass);
     694                                                                                btScalar mass);
    698695        /* Get mass                                                                                                                             */
    699696        btScalar                        getMass(                int node) const;
    … …  
    702699        /* Set total mass (weighted by previous masses)                                                 */
    703700        void                            setTotalMass(   btScalar mass,
    704                 bool fromfaces=false);
     701                                                                                bool fromfaces=false);
    705702        /* Set total density                                                                                                    */
    706703        void                            setTotalDensity(btScalar density);
    … …  
    715712        /* Set current state as pose                                                                                    */
    716713        void                            setPose(                bool bvolume,
    717                 bool bframe);
     714                                                                                bool bframe);
    718715        /* Return the volume                                                                                                    */
    719716        btScalar                        getVolume() const;
    … …  
    735732        /* Generate bending constraints based on distance in the adjency graph  */
    736733        int                                     generateBendingConstraints(     int distance,
    737                 Material* mat=0);
     734                                                                                                        Material* mat=0);
    738735        /* Randomize constraints to reduce solver bias                                                  */
    739736        void                            randomizeConstraints();
    … …  
    748745        bool                            cutLink(int node0,int node1,btScalar position);
    749746        bool                            cutLink(const Node* node0,const Node* node1,btScalar position);
    750 
    751         ///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
    752         bool                            rayTest(const btVector3& rayFrom,
    753                 const btVector3& rayTo,
    754                 sRayCast& results);
     747        /* Ray casting                                                                                                                  */
     748        bool                            rayCast(const btVector3& org,
     749                                                                const btVector3& dir,
     750                                                                sRayCast& results,
     751                                                                btScalar maxtime=SIMD_INFINITY);
    755752        /* Solver presets                                                                                                               */
    756753        void                            setSolver(eSolverPresets::_ preset);
    … …  
    770767        void                            defaultCollisionHandler(btCollisionObject* pco);
    771768        void                            defaultCollisionHandler(btSoftBody* psb);
    772 
     769               
    773770        //
    774771        // Cast
    775772        //
    776 
     773               
    777774        static const btSoftBody*        upcast(const btCollisionObject* colObj)
    778775        {
    … …  
    802799        void                            pointersToIndices();
    803800        void                            indicesToPointers(const int* map=0);
    804 
    805         int                                     rayTest(const btVector3& rayFrom,const btVector3& rayTo,
    806                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
     801        int                                     rayCast(const btVector3& org,const btVector3& dir,
     802                                                                btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
    807803        void                            initializeFaceTree();
    808804        btVector3                       evaluateCom() const;
    … …  
    827823        static psolver_t        getSolver(ePSolver::_ solver);
    828824        static vsolver_t        getSolver(eVSolver::_ solver);
    829 
     825       
    830826};
    831827

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

    r1963 r1972  
    5151
    5252btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
    53 m_dispatcher(dispatcher),
    54 m_dispatchInfoPtr(0)
     53          m_dispatcher(dispatcher),
     54        m_dispatchInfoPtr(0)
    5555{
    5656        m_softBody = (btSoftBody*) (isSwapped? body1:body0);
    5757        m_triBody = isSwapped? body0:body1;
    58 
    59         //
    60         // create the manifold from the dispatcher 'manifold pool'
    61         //
    62         //        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
    63 
     58       
     59          //
     60          // create the manifold from the dispatcher 'manifold pool'
     61          //
     62//        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
     63
     64          clearCache();
     65}
     66
     67btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
     68{
    6469        clearCache();
    65 }
    66 
    67 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
    68 {
    69         clearCache();
    70         //      m_dispatcher->releaseManifold( m_manifoldPtr );
    71 
    72 }
    73 
     70//      m_dispatcher->releaseManifold( m_manifoldPtr );
     71 
     72}
     73 
    7474
    7575void    btSoftBodyTriangleCallback::clearCache()
    … …  
    8989void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
    9090{
    91         //just for debugging purposes
     91        //just for debugging purposes
    9292        //printf("triangle %d",m_triangleCount++);
    9393        btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
    … …  
    9595        ci.m_dispatcher1 = m_dispatcher;
    9696
    97         ///debug drawing of the overlapping triangles
     97                ///debug drawing of the overlapping triangles
    9898        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
    9999        {
    … …  
    108108        btHashKey<btTriIndex> triKey(triIndex.getUid());
    109109
    110 
     110       
    111111        btTriIndex* shapeIndex = m_shapeCache[triKey];
    112112        if (shapeIndex)
    … …  
    117117                //copy over user pointers to temporary shape
    118118                tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
    119 
     119               
    120120                btCollisionShape* tmpShape = ob->getCollisionShape();
    121121                ob->internalSetTemporaryCollisionShape( tm );
    122 
     122               
    123123
    124124                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
    125 
     125               
    126126                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    127127                colAlgo->~btCollisionAlgorithm();
    … …  
    134134
    135135        //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
    136 
    137         //      if (m_softBody->getCollisionShape()->getShapeType()==
    138         {
    139                 //              btVector3 other;
     136       
     137//      if (m_softBody->getCollisionShape()->getShapeType()==
     138        {
     139//              btVector3 other;
    140140                btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
    141141                normal.normalize();
    142142                normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
    143                 //              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
    144                 //              other+=normal*22.f;
     143//              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
     144//              other+=normal*22.f;
    145145                btVector3       pts[6] = {triangle[0]+normal,
    146146                        triangle[1]+normal,
    147                         triangle[2]+normal,
    148                         triangle[0]-normal,
    149                         triangle[1]-normal,
    150                         triangle[2]-normal};
     147                triangle[2]+normal,
     148                triangle[0]-normal,
     149                triangle[1]-normal,
     150                triangle[2]-normal};
    151151
    152152                btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
    153153
    154154
    155                 //              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
    156 
     155//              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
     156               
    157157                //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);     
    158                 //      tm.setMargin(m_collisionMarginTriangle);
    159 
     158        //      tm.setMargin(m_collisionMarginTriangle);
     159               
    160160                //copy over user pointers to temporary shape
    161161                tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
    162 
     162               
    163163                btCollisionShape* tmpShape = ob->getCollisionShape();
    164164                ob->internalSetTemporaryCollisionShape( tm );
    165 
     165               
    166166
    167167                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
    … …  
    170170
    171171                //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
    172                 //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
    173                 //              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
     172        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
     173//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    174174                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
    175175                colAlgo->~btCollisionAlgorithm();
    176176                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
    177 
    178 
     177               
     178               
    179179                ob->internalSetTemporaryCollisionShape( tmpShape );
    180180                triIndex.m_childShape = tm;
    … …  
    183183        }
    184184
    185 
     185       
    186186
    187187}
    … …  
    195195        m_resultOut = resultOut;
    196196
    197 
     197       
    198198        btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
    199199        m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
    … …  
    218218void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
    219219{
    220 
    221 
     220       
     221       
    222222        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
    223223        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
    … …  
    229229                btCollisionObject*      triOb = triBody;
    230230                btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
    231 
    232                 //      if (convexBody->getCollisionShape()->isConvex())
     231               
     232        //      if (convexBody->getCollisionShape()->isConvex())
    233233                {
    234234                        btScalar collisionMarginTriangle = concaveShape->getMargin();
    235 
    236                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
     235                                       
     236//                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
    237237                        m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
    238238
    … …  
    240240                        //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
    241241
    242                         //                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
     242//                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
    243243
    244244
    245245                        concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
    246 
    247                         //      resultOut->refreshContactPoints();
    248 
    249                 }
    250 
     246                       
     247                //      resultOut->refreshContactPoints();
     248       
     249                }
     250       
    251251        }
    252252
    … …  
    288288                btScalar        m_ccdSphereRadius;
    289289                btScalar        m_hitFraction;
    290 
     290       
    291291
    292292                LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
    … …  
    297297                {                       
    298298                }
    299 
    300 
     299               
     300               
    301301                virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
    302302                {
    … …  
    328328
    329329
    330 
    331 
    332 
     330       
     331
     332       
    333333        if (triBody->getCollisionShape()->isConcave())
    334334        {
    … …  
    350350
    351351                btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
    352 
     352               
    353353                if (triangleMesh)
    354354                {
    355355                        triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
    356356                }
    357 
     357       
    358358
    359359

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h

    r1963 r1972  
    3636        int m_PartIdTriangleIndex;
    3737        class btCollisionShape* m_childShape;
    38 
     38       
    3939        btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
    4040        {
    … …  
    7676
    7777        btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
    78 
     78       
    7979public:
    80         int     m_triangleCount;
    81 
    82         //      btPersistentManifold*   m_manifoldPtr;
     80int     m_triangleCount;
     81       
     82//      btPersistentManifold*   m_manifoldPtr;
    8383
    8484        btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
    … …  
    8989
    9090        virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
    91 
     91       
    9292        void clearCache();
    9393

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

    r1963 r1972  
    2323//
    2424static void                             drawVertex(     btIDebugDraw* idraw,
    25                                                                    const btVector3& x,btScalar s,const btVector3& c)
    26 {
    27         idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
    28         idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
    29         idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
    30 }
     25                                                                        const btVector3& x,btScalar s,const btVector3& c)
     26        {
     27                idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
     28                idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
     29                idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
     30        }
    3131
    3232//
    3333static void                             drawBox(        btIDebugDraw* idraw,
    34                                                                 const btVector3& mins,
    35                                                                 const btVector3& maxs,
    36                                                                 const btVector3& color)
    37 {
    38         const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
    39                 btVector3(maxs.x(),mins.y(),mins.z()),
    40                 btVector3(maxs.x(),maxs.y(),mins.z()),
    41                 btVector3(mins.x(),maxs.y(),mins.z()),
    42                 btVector3(mins.x(),mins.y(),maxs.z()),
    43                 btVector3(maxs.x(),mins.y(),maxs.z()),
    44                 btVector3(maxs.x(),maxs.y(),maxs.z()),
    45                 btVector3(mins.x(),maxs.y(),maxs.z())};
    46         idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
    47         idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
    48         idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
    49         idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
    50         idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
    51         idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
     34                                                                        const btVector3& mins,
     35                                                                        const btVector3& maxs,
     36                                                                        const btVector3& color)
     37{
     38const btVector3 c[]={   btVector3(mins.x(),mins.y(),mins.z()),
     39                                                btVector3(maxs.x(),mins.y(),mins.z()),
     40                                                btVector3(maxs.x(),maxs.y(),mins.z()),
     41                                                btVector3(mins.x(),maxs.y(),mins.z()),
     42                                                btVector3(mins.x(),mins.y(),maxs.z()),
     43                                                btVector3(maxs.x(),mins.y(),maxs.z()),
     44                                                btVector3(maxs.x(),maxs.y(),maxs.z()),
     45                                                btVector3(mins.x(),maxs.y(),maxs.z())};
     46idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
     47idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
     48idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
     49idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
     50idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
     51idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
    5252}
    5353
    5454//
    5555static void                             drawTree(       btIDebugDraw* idraw,
    56                                                                  const btDbvtNode* node,
    57                                                                  int depth,
    58                                                                  const btVector3& ncolor,
    59                                                                  const btVector3& lcolor,
    60                                                                  int mindepth,
    61                                                                  int maxdepth)
    62 {
    63         if(node)
    64         {
    65                 if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
    66                 {
    67                         drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
    68                         drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
    69                 }
    70                 if(depth>=mindepth)
    71                 {
    72                         const btScalar  scl=(btScalar)(node->isinternal()?1:1);
    73                         const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
    74                         const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
    75                         drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
     56                                                                        const btDbvtNode* node,
     57                                                                        int depth,
     58                                                                        const btVector3& ncolor,
     59                                                                        const btVector3& lcolor,
     60                                                                        int mindepth,
     61                                                                        int maxdepth)
     62{
     63if(node)
     64        {
     65        if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
     66                {
     67                drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
     68                drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
     69                }
     70        if(depth>=mindepth)
     71                {
     72                const btScalar  scl=(btScalar)(node->isinternal()?1:1);
     73                const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
     74                const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
     75                drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
    7676                }
    7777        }
    … …  
    8282static inline T                         sum(const btAlignedObjectArray<T>& items)
    8383{
    84         T       v;
    85         if(items.size())
    86         {
    87                 v=items[0];
    88                 for(int i=1,ni=items.size();i<ni;++i)
    89                 {
    90                         v+=items[i];
    91                 }
    92         }
    93         return(v);
     84T       v;
     85if(items.size())
     86        {
     87        v=items[0];
     88        for(int i=1,ni=items.size();i<ni;++i)
     89                {
     90                v+=items[i];
     91                }
     92        }
     93return(v);
    9494}
    9595
    … …  
    9898static inline void                      add(btAlignedObjectArray<T>& items,const Q& value)
    9999{
    100         for(int i=0,ni=items.size();i<ni;++i)
    101         {
    102                 items[i]+=value;
     100for(int i=0,ni=items.size();i<ni;++i)
     101        {
     102        items[i]+=value;
    103103        }
    104104}
    … …  
    108108static inline void                      mul(btAlignedObjectArray<T>& items,const Q& value)
    109109{
    110         for(int i=0,ni=items.size();i<ni;++i)
    111         {
    112                 items[i]*=value;
     110for(int i=0,ni=items.size();i<ni;++i)
     111        {
     112        items[i]*=value;
    113113        }
    114114}
    … …  
    118118static inline T                         average(const btAlignedObjectArray<T>& items)
    119119{
    120         const btScalar  n=(btScalar)(items.size()>0?items.size():1);
    121         return(sum(items)/n);
     120const btScalar  n=(btScalar)(items.size()>0?items.size():1);
     121return(sum(items)/n);
    122122}
    123123
    … …  
    137137#if 0
    138138static btVector3                stresscolor(btScalar stress)
    139 {
     139        {
    140140        static const btVector3  spectrum[]=     {       btVector3(1,0,1),
    141                 btVector3(0,0,1),
    142                 btVector3(0,1,1),
    143                 btVector3(0,1,0),
    144                 btVector3(1,1,0),
    145                 btVector3(1,0,0),
    146                 btVector3(1,0,0)};
     141                                                                                        btVector3(0,0,1),
     142                                                                                        btVector3(0,1,1),
     143                                                                                        btVector3(0,1,0),
     144                                                                                        btVector3(1,1,0),
     145                                                                                        btVector3(1,0,0),
     146                                                                                        btVector3(1,0,0)};
    147147        static const int                ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
    148148        static const btScalar   one=1;
    … …  
    151151        const btScalar                  frc=stress-sel;
    152152        return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
    153 }
     153        }
    154154#endif
    155155
    156156//
    157157void                    btSoftBodyHelpers::Draw(        btSoftBody* psb,
    158                                                                                 btIDebugDraw* idraw,
    159                                                                                 int drawflags)
     158                                         btIDebugDraw* idraw,
     159                                         int drawflags)
    160160{
    161161        const btScalar          scl=(btScalar)0.1;
    … …  
    252252                        const btVector3                 c=(x[0]+x[1]+x[2])/3;
    253253                        idraw->drawTriangle((x[0]-c)*scl+c,
    254                                 (x[1]-c)*scl+c,
    255                                 (x[2]-c)*scl+c,
    256                                 col,alp);
     254                                                                (x[1]-c)*scl+c,
     255                                                                (x[2]-c)*scl+c,
     256                                                                col,alp);
    257257                }       
    258258        }
    259259        /* Clusters     */
    260260        if(0!=(drawflags&fDrawFlags::Clusters))
    261         {
     261                {
    262262                srand(1806);
    263263                for(i=0;i<psb->m_clusters.size();++i)
    264                 {
     264                        {
    265265                        if(psb->m_clusters[i]->m_collide)
    266                         {
     266                                {
    267267                                btVector3                                               color(  rand()/(btScalar)RAND_MAX,
    268                                         rand()/(btScalar)RAND_MAX,
    269                                         rand()/(btScalar)RAND_MAX);
     268                                                                                                                rand()/(btScalar)RAND_MAX,
     269                                                                                                                rand()/(btScalar)RAND_MAX);
    270270                                color=color.normalized()*0.75;
    271271                                btAlignedObjectArray<btVector3> vertices;
    272272                                vertices.resize(psb->m_clusters[i]->m_nodes.size());
    273273                                for(j=0,nj=vertices.size();j<nj;++j)
    274                                 {                               
     274                                        {                               
    275275                                        vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
    276                                 }
     276                                        }
    277277                                HullDesc                hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
    278278                                HullResult              hres;
    … …  
    285285                                add(hres.m_OutputVertices,center);
    286286                                for(j=0;j<(int)hres.mNumFaces;++j)
    287                                 {
     287                                        {
    288288                                        const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
    289289                                        idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
    290                                                 hres.m_OutputVertices[idx[1]],
    291                                                 hres.m_OutputVertices[idx[2]],
    292                                                 color,1);
     290                                                                                hres.m_OutputVertices[idx[1]],
     291                                                                                hres.m_OutputVertices[idx[2]],
     292                                                                                color,1);
     293                                        }
     294                                hlib.ReleaseResult(hres);
    293295                                }
    294                                 hlib.ReleaseResult(hres);
    295                         }
    296296                        /* Velocities   */
    297 #if 0
     297                        #if 0
    298298                        for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
    299                         {
     299                                {
    300300                                const btSoftBody::Cluster&      c=psb->m_clusters[i];
    301301                                const btVector3                         r=c.m_nodes[j]->m_x-c.m_com;
    302302                                const btVector3                         v=c.m_lv+cross(c.m_av,r);
    303303                                idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
    304                         }
    305 #endif
     304                                }
     305                        #endif
    306306                        /* Frame                */
    307307                        btSoftBody::Cluster& c=*psb->m_clusters[i];
    … …  
    309309                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
    310310                        idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
    311                 }
    312         }
     311                        }
     312                }
    313313        /* Notes        */
    314314        if(0!=(drawflags&fDrawFlags::Notes))
    … …  
    319319                        btVector3                               p=n.m_offset;
    320320                        for(int j=0;j<n.m_rank;++j)
    321                         {
     321                                {
    322322                                p+=n.m_nodes[j]->m_x*n.m_coords[j];
    323                         }
     323                                }
    324324                        idraw->draw3dText(p,n.m_text);
    325325                }
    … …  
    334334        if(0!=(drawflags&fDrawFlags::Joints))
    335335        {
    336                 for(i=0;i<psb->m_joints.size();++i)
    337                 {
    338                         const btSoftBody::Joint*        pj=psb->m_joints[i];
    339                         switch(pj->Type())
     336        for(i=0;i<psb->m_joints.size();++i)
     337                {
     338                const btSoftBody::Joint*        pj=psb->m_joints[i];
     339                switch(pj->Type())
    340340                        {
    341341                        case    btSoftBody::Joint::eType::Linear:
    342342                                {
    343                                         const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
    344                                         const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
    345                                         const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
    346                                         idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
    347                                         idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
    348                                         drawVertex(idraw,a0,0.25,btVector3(1,1,0));
    349                                         drawVertex(idraw,a1,0.25,btVector3(0,1,1));
     343                                const btSoftBody::LJoint*       pjl=(const btSoftBody::LJoint*)pj;
     344                                const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
     345                                const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
     346                                idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
     347                                idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
     348                                drawVertex(idraw,a0,0.25,btVector3(1,1,0));
     349                                drawVertex(idraw,a1,0.25,btVector3(0,1,1));
    350350                                }
    351                                 break;
     351                        break;
    352352                        case    btSoftBody::Joint::eType::Angular:
    353353                                {
    354                                         const btSoftBody::AJoint*       pja=(const btSoftBody::AJoint*)pj;
    355                                         const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
    356                                         const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
    357                                         const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
    358                                         const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
    359                                         idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
    360                                         idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
    361                                         idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
    362                                         idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
     354                                const btSoftBody::AJoint*       pja=(const btSoftBody::AJoint*)pj;
     355                                const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
     356                                const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
     357                                const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
     358                                const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
     359                                idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
     360                                idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
     361                                idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
     362                                idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
    363363                                }
    364364                        }               
    … …  
    369369//
    370370void                    btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
    371                                                                                         btIDebugDraw* idraw,
    372                                                                                         bool masses,
    373                                                                                         bool areas,
    374                                                                                         bool /*stress*/)
     371                                                  btIDebugDraw* idraw,
     372                                                  bool masses,
     373                                                  bool areas,
     374                                                  bool /*stress*/)
    375375{
    376376        for(int i=0;i<psb->m_nodes.size();++i)
    … …  
    395395//
    396396void                    btSoftBodyHelpers::DrawNodeTree(        btSoftBody* psb,
    397                                                                                                 btIDebugDraw* idraw,
    398                                                                                                 int mindepth,
    399                                                                                                 int maxdepth)
    400 {
    401         drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
     397                                                                                                        btIDebugDraw* idraw,
     398                                                                                                        int mindepth,
     399                                                                                                        int maxdepth)
     400{
     401drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
    402402}
    403403
    404404//
    405405void                    btSoftBodyHelpers::DrawFaceTree(        btSoftBody* psb,
    406                                                                                                 btIDebugDraw* idraw,
    407                                                                                                 int mindepth,
    408                                                                                                 int maxdepth)
    409 {
    410         drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
     406                                                                                                        btIDebugDraw* idraw,
     407                                                                                                        int mindepth,
     408                                                                                                        int maxdepth)
     409{
     410drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
    411411}
    412412
    413413//
    414414void                    btSoftBodyHelpers::DrawClusterTree(     btSoftBody* psb,
    415                                                                                                    btIDebugDraw* idraw,
    416                                                                                                    int mindepth,
    417                                                                                                    int maxdepth)
    418 {
    419         drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
     415                                                                                                        btIDebugDraw* idraw,
     416                                                                                                        int mindepth,
     417                                                                                                        int maxdepth)
     418{
     419drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
    420420}
    421421
    422422//
    423423void                    btSoftBodyHelpers::DrawFrame(           btSoftBody* psb,
    424                                                                                         btIDebugDraw* idraw)
     424                                                  btIDebugDraw* idraw)
    425425{
    426426        if(psb->m_pose.m_bframe)
    … …  
    446446//
    447447btSoftBody*             btSoftBodyHelpers::CreateRope(  btSoftBodyWorldInfo& worldInfo, const btVector3& from,
    448                                                                                           const btVector3& to,
    449                                                                                           int res,
    450                                                                                           int fixeds)
     448                                                   const btVector3& to,
     449                                                   int res,
     450                                                   int fixeds)
    451451{
    452452        /* Create nodes */
    … …  
    478478//
    479479btSoftBody*             btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
    480                                                                                            const btVector3& corner10,
    481                                                                                            const btVector3& corner01,
    482                                                                                            const btVector3& corner11,
    483                                                                                            int resx,
    484                                                                                            int resy,
    485                                                                                            int fixeds,
    486                                                                                            bool gendiags)
     480                                                        const btVector3& corner10,
     481                                                        const btVector3& corner01,
     482                                                        const btVector3& corner11,
     483                                                        int resx,
     484                                                        int resy,
     485                                                        int fixeds,
     486                                                        bool gendiags)
    487487{
    488488#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    … …  
    554554
    555555//
    556 btSoftBody*             btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
    557                                                                                                  const btVector3& corner00,
    558                                                                                                  const btVector3& corner10,
    559                                                                                                  const btVector3& corner01,
    560                                                                                                  const btVector3& corner11,
    561                                                                                                  int resx,
    562                                                                                                  int resy,
    563                                                                                                  int fixeds,
    564                                                                                                  bool gendiags,
    565                                                                                                  float* tex_coords)
    566 {
    567 
    568         /*
    569         *
    570         *  corners:
    571         *
    572         *  [0][0]     corner00 ------- corner01   [resx][0]
    573         *                |                |
    574         *                |                |
    575         *  [0][resy]  corner10 -------- corner11  [resx][resy]
    576         *
    577         *
    578         *
    579         *
    580         *
    581         *
    582         *   "fixedgs" map:
    583         *
    584         *  corner00     -->   +1
    585         *  corner01     -->   +2
    586         *  corner10     -->   +4
    587         *  corner11     -->   +8
    588         *  upper middle -->  +16
    589         *  left middle  -->  +32
    590         *  right middle -->  +64
    591         *  lower middle --> +128
    592         *  center       --> +256
    593         *
    594         *
    595         *   tex_coords size   (resx-1)*(resy-1)*12
    596         *
    597         *
    598         *
    599         *     SINGLE QUAD INTERNALS
    600         *
    601         *  1) btSoftBody's nodes and links,
    602         *     diagonal link is optional ("gendiags")
    603         *
    604         *
    605         *    node00 ------ node01
    606         *      | .             
    607         *      |   .           
    608         *      |     .         
    609         *      |       .       
    610         *      |         .     
    611         *    node10        node11
    612         *
    613         *
    614         *
    615         *   2) Faces:
    616         *      two triangles,
    617         *      UV Coordinates (hier example for single quad)
    618         *     
    619         *     (0,1)          (0,1)  (1,1)
    620         *     1 |\            3 \-----| 2
    621         *       | \              \    |
    622         *       |  \              \   |
    623         *       |   \              \  |
    624         *       |    \              \ |
    625         *     2 |-----\ 3            \| 1
    626         *     (0,0)    (1,0)       (1,0)
    627         *
    628         *
    629         *
    630         *
    631         *
    632         *
    633         */
    634 
    635 #define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    636         /* Create nodes         */
    637         if((resx<2)||(resy<2)) return(0);
    638         const int       rx=resx;
    639         const int       ry=resy;
    640         const int       tot=rx*ry;
    641         btVector3*      x=new btVector3[tot];
    642         btScalar*       m=new btScalar[tot];
    643 
    644         for(int iy=0;iy<ry;++iy)
    645         {
    646                 const btScalar  ty=iy/(btScalar)(ry-1);
    647                 const btVector3 py0=lerp(corner00,corner01,ty);
    648                 const btVector3 py1=lerp(corner10,corner11,ty);
    649                 for(int ix=0;ix<rx;++ix)
    650                 {
    651                         const btScalar  tx=ix/(btScalar)(rx-1);
    652                         x[IDX(ix,iy)]=lerp(py0,py1,tx);
    653                         m[IDX(ix,iy)]=1;
    654                 }
    655         }
    656         btSoftBody*     psb=new btSoftBody(&worldInfo,tot,x,m);
    657         if(fixeds&1)            psb->setMass(IDX(0,0),0);
    658         if(fixeds&2)            psb->setMass(IDX(rx-1,0),0);
    659         if(fixeds&4)            psb->setMass(IDX(0,ry-1),0);
    660         if(fixeds&8)            psb->setMass(IDX(rx-1,ry-1),0);
    661         if(fixeds&16)           psb->setMass(IDX((rx-1)/2,0),0);
    662         if(fixeds&32)           psb->setMass(IDX(0,(ry-1)/2),0);
    663         if(fixeds&64)           psb->setMass(IDX(rx-1,(ry-1)/2),0);
    664         if(fixeds&128)          psb->setMass(IDX((rx-1)/2,ry-1),0);
    665         if(fixeds&256)          psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
    666         delete[] x;
    667         delete[] m;
    668 
    669 
    670         int z = 0;
    671         /* Create links and faces       */
    672         for(int iy=0;iy<ry;++iy)
    673         {
    674                 for(int ix=0;ix<rx;++ix)
    675                 {
    676                         const bool      mdx=(ix+1)<rx;
    677                         const bool      mdy=(iy+1)<ry;
    678 
    679                         int node00=IDX(ix,iy);
    680                         int node01=IDX(ix+1,iy);
    681                         int node10=IDX(ix,iy+1);
    682                         int node11=IDX(ix+1,iy+1);
    683 
    684                         if(mdx) psb->appendLink(node00,node01);
    685                         if(mdy) psb->appendLink(node00,node10);
    686                         if(mdx&&mdy)
    687                         {
    688                                 psb->appendFace(node00,node10,node11);
    689                                 if (tex_coords) {
    690                                         tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
    691                                         tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
    692                                         tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
    693                                         tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
    694                                         tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
    695                                         tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
    696                                 }
    697                                 psb->appendFace(node11,node01,node00);
    698                                 if (tex_coords) {
    699                                         tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
    700                                         tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
    701                                         tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
    702                                         tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
    703                                         tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
    704                                         tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
    705                                 }
    706                                 if (gendiags) psb->appendLink(node00,node11);
    707                                 z += 12;
    708                         }
    709                 }
    710         }
    711         /* Finished     */
    712 #undef IDX
    713         return(psb);
    714 }
    715 
    716 float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
    717 {
    718 
    719         /*
    720         *
    721         *
    722         *    node00 --- node01
    723         *      |          |
    724         *    node10 --- node11
    725         *
    726         *
    727         *   ID map:
    728         *
    729         *   node00 s --> 0
    730         *   node00 t --> 1
    731         *
    732         *   node01 s --> 3
    733         *   node01 t --> 1
    734         *
    735         *   node10 s --> 0
    736         *   node10 t --> 2
    737         *
    738         *   node11 s --> 3
    739         *   node11 t --> 2
    740         *
    741         *
    742         */
    743 
    744         float tc=0.0f;
    745         if (id == 0) {
    746                 tc = (1.0f/((resx-1))*ix);
    747         }
    748         else if (id==1) {
    749                 tc = (1.0f/((resy-1))*(resy-1-iy));
    750         }
    751         else if (id==2) {
    752                 tc = (1.0f/((resy-1))*(resy-1-iy-1));
    753         }
    754         else if (id==3) {
    755                 tc = (1.0f/((resx-1))*(ix+1));
    756         }
    757         return tc;
    758 }
    759 //
    760556btSoftBody*             btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
    761                                                                                                    const btVector3& radius,
    762                                                                                                    int res)
     557                                                                const btVector3& radius,
     558                                                                int res)
    763559{
    764560        struct  Hammersley
    … …  
    791587//
    792588btSoftBody*             btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*     vertices,
    793                                                                                                         const int* triangles,
    794                                                                                                         int ntriangles)
     589                                                                  const int* triangles,
     590                                                                  int ntriangles)
    795591{
    796592        int             maxidx=0;
    … …  
    833629//
    834630btSoftBody*             btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
    835                                                                                                                 int nvertices)
     631                                                                         int nvertices)
    836632{
    837633        HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.h

    r1963 r1972  
    4747        /* Draw body                                                                                                                    */
    4848        static void                             Draw(           btSoftBody* psb,
    49                 btIDebugDraw* idraw,
    50                 int drawflags=fDrawFlags::Std);
     49                                                                                btIDebugDraw* idraw,
     50                                                                                int drawflags=fDrawFlags::Std);
    5151        /* Draw body infos                                                                                                              */
    5252        static  void                    DrawInfos(      btSoftBody* psb,
    53                 btIDebugDraw* idraw,
    54                 bool masses,
    55                 bool areas,
    56                 bool stress);
     53                                                                                btIDebugDraw* idraw,
     54                                                                                bool masses,
     55                                                                                bool areas,
     56                                                                                bool stress);
    5757        /* Draw node tree                                                                                                               */
    5858        static void                             DrawNodeTree(   btSoftBody* psb,
    59                 btIDebugDraw* idraw,
    60                 int mindepth=0,
    61                 int maxdepth=-1);
     59                                                                                        btIDebugDraw* idraw,
     60                                                                                        int mindepth=0,
     61                                                                                        int maxdepth=-1);
    6262        /* Draw face tree                                                                                                               */
    6363        static void                             DrawFaceTree(   btSoftBody* psb,
    64                 btIDebugDraw* idraw,
    65                 int mindepth=0,
    66                 int maxdepth=-1);
     64                                                                                        btIDebugDraw* idraw,
     65                                                                                        int mindepth=0,
     66                                                                                        int maxdepth=-1);
    6767        /* Draw cluster tree                                                                                                    */
    6868        static void                             DrawClusterTree(btSoftBody* psb,
    69                 btIDebugDraw* idraw,
    70                 int mindepth=0,
    71                 int maxdepth=-1);
     69                                                                                        btIDebugDraw* idraw,
     70                                                                                        int mindepth=0,
     71                                                                                        int maxdepth=-1);
    7272        /* Draw rigid frame                                                                                                             */
    7373        static  void                    DrawFrame(              btSoftBody* psb,
    74                 btIDebugDraw* idraw);
     74                                                                                        btIDebugDraw* idraw);
    7575        /* Create a rope                                                                                                                */
    7676        static  btSoftBody*             CreateRope( btSoftBodyWorldInfo& worldInfo,
    77                 const btVector3& from,
    78                 const btVector3& to,
    79                 int res,
    80                 int fixeds);
     77                                                                                const btVector3& from,
     78                                                                                const btVector3& to,
     79                                                                                int res,
     80                                                                                int fixeds);
    8181        /* Create a patch                                                                                                               */
    8282        static  btSoftBody*             CreatePatch(btSoftBodyWorldInfo& worldInfo,
    83                 const btVector3& corner00,
    84                 const btVector3& corner10,
    85                 const btVector3& corner01,
    86                 const btVector3& corner11,
    87                 int resx,
    88                 int resy,
    89                 int fixeds,
    90                 bool gendiags);
    91         /* Create a patch with UV Texture Coordinates   */
    92         static  btSoftBody*             CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
    93                 const btVector3& corner00,
    94                 const btVector3& corner10,
    95                 const btVector3& corner01,
    96                 const btVector3& corner11,
    97                 int resx,
    98                 int resy,
    99                 int fixeds,
    100                 bool gendiags,
    101                 float* tex_coords=0);
    102         static  float   CalculateUV(int resx,int resy,int ix,int iy,int id);
     83                                                                                const btVector3& corner00,
     84                                                                                const btVector3& corner10,
     85                                                                                const btVector3& corner01,
     86                                                                                const btVector3& corner11,
     87                                                                                int resx,
     88                                                                                int resy,
     89                                                                                int fixeds,
     90                                                                                bool gendiags);
    10391        /* Create an ellipsoid                                                                                                  */
    10492        static  btSoftBody*             CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
    105                 const btVector3& center,
    106                 const btVector3& radius,
    107                 int res);       
     93                                                                                        const btVector3& center,
     94                                                                                        const btVector3& radius,
     95                                                                                        int res);       
    10896        /* Create from trimesh                                                                                                  */
    10997        static  btSoftBody*             CreateFromTriMesh(      btSoftBodyWorldInfo& worldInfo,
    110                 const btScalar* vertices,
    111                 const int* triangles,
    112                 int ntriangles);
     98                                                                                                const btScalar* vertices,
     99                                                                                                const int* triangles,
     100                                                                                                int ntriangles);
    113101        /* Create from convex-hull                                                                                              */
    114102        static  btSoftBody*             CreateFromConvexHull(   btSoftBodyWorldInfo& worldInfo,
    115                 const btVector3* vertices,
    116                 int nvertices);
     103                                                                                                        const btVector3* vertices,
     104                                                                                                        int nvertices);
    117105};
    118106

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h

    r1963 r1972  
    3232struct btSymMatrix
    3333{
    34         btSymMatrix() : dim(0)                                  {}
    35         btSymMatrix(int n,const T& init=T())    { resize(n,init); }
    36         void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
    37         int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
    38         T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
    39         const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
    40         btAlignedObjectArray<T> store;
    41         int                                             dim;
     34                                                btSymMatrix() : dim(0)                                  {}
     35                                                btSymMatrix(int n,const T& init=T())    { resize(n,init); }
     36void                                    resize(int n,const T& init=T())                 { dim=n;store.resize((n*(n+1))/2,init); }
     37int                                             index(int c,int r) const                                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
     38T&                                              operator()(int c,int r)                                 { return(store[index(c,r)]); }
     39const T&                                operator()(int c,int r) const                   { return(store[index(c,r)]); }
     40btAlignedObjectArray<T> store;
     41int                                             dim;
    4242};     
    4343
    … …  
    4949public:
    5050        btSoftBody*                                             m_body;
    51 
     51       
    5252        btSoftBodyCollisionShape(btSoftBody* backptr)
    5353        {
    … …  
    7070        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
    7171        {
    72                 /* t should be identity, but better be safe than...fast? */
    73                 const btVector3 mins=m_body->m_bounds[0];
    74                 const btVector3 maxs=m_body->m_bounds[1];
    75                 const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
    76                         t*btVector3(maxs.x(),mins.y(),mins.z()),
    77                         t*btVector3(maxs.x(),maxs.y(),mins.z()),
    78                         t*btVector3(mins.x(),maxs.y(),mins.z()),
    79                         t*btVector3(mins.x(),mins.y(),maxs.z()),
    80                         t*btVector3(maxs.x(),mins.y(),maxs.z()),
    81                         t*btVector3(maxs.x(),maxs.y(),maxs.z()),
    82                         t*btVector3(mins.x(),maxs.y(),maxs.z())};
    83                 aabbMin=aabbMax=crns[0];
    84                 for(int i=1;i<8;++i)
    85                 {
    86                         aabbMin.setMin(crns[i]);
    87                         aabbMax.setMax(crns[i]);
    88                 }
    89         }
    90 
    91 
     72        /* t should be identity, but better be safe than...fast? */
     73        const btVector3 mins=m_body->m_bounds[0];
     74        const btVector3 maxs=m_body->m_bounds[1];
     75        const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
     76                                                        t*btVector3(maxs.x(),mins.y(),mins.z()),
     77                                                        t*btVector3(maxs.x(),maxs.y(),mins.z()),
     78                                                        t*btVector3(mins.x(),maxs.y(),mins.z()),
     79                                                        t*btVector3(mins.x(),mins.y(),maxs.z()),
     80                                                        t*btVector3(maxs.x(),mins.y(),maxs.z()),
     81                                                        t*btVector3(maxs.x(),maxs.y(),maxs.z()),
     82                                                        t*btVector3(mins.x(),maxs.y(),maxs.z())};
     83        aabbMin=aabbMax=crns[0];
     84        for(int i=1;i<8;++i)
     85                {
     86                aabbMin.setMin(crns[i]);
     87                aabbMax.setMax(crns[i]);
     88                }
     89        }
     90
     91       
    9292        virtual void    setLocalScaling(const btVector3& /*scaling*/)
    9393        {               
    … …  
    9696        virtual const btVector3& getLocalScaling() const
    9797        {
    98                 static const btVector3 dummy(1,1,1);
    99                 return dummy;
     98        static const btVector3 dummy(1,1,1);
     99        return dummy;
    100100        }
    101101        virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
    … …  
    120120
    121121        btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
    122 
    123 
     122       
     123       
    124124        virtual btVector3       localGetSupportingVertex(const btVector3& vec) const
    125         {
     125                {
    126126                btSoftBody::Node* const *                                               n=&m_cluster->m_nodes[0];
    127127                btScalar                                                                                d=dot(vec,n[0]->m_x);
    128128                int                                                                                             j=0;
    129129                for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
    130                 {
     130                        {
    131131                        const btScalar  k=dot(vec,n[i]->m_x);
    132132                        if(k>d) { d=k;j=i; }
    133                 }
     133                        }
    134134                return(n[j]->m_x);
    135         }
     135                }
    136136        virtual btVector3       localGetSupportingVertexWithoutMargin(const btVector3& vec)const
    137         {
     137                {
    138138                return(localGetSupportingVertex(vec));
    139         }
     139                }
    140140        //notice that the vectors should be unit length
    141141        virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
    … …  
    150150
    151151        virtual int     getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
    152 
     152       
    153153        //debugging
    154154        virtual const char*     getName()const {return "SOFTCLUSTER";}
    … …  
    172172static inline void                      ZeroInitialize(T& value)
    173173{
    174         static const T  zerodummy;
    175         value=zerodummy;
     174static const T  zerodummy;
     175value=zerodummy;
    176176}
    177177//
    … …  
    193193//
    194194static inline btMatrix3x3       Lerp(   const btMatrix3x3& a,
    195                                                                  const btMatrix3x3& b,
    196                                                                  btScalar t)
    197 {
    198         btMatrix3x3     r;
    199         r[0]=Lerp(a[0],b[0],t);
    200         r[1]=Lerp(a[1],b[1],t);
    201         r[2]=Lerp(a[2],b[2],t);
    202         return(r);
     195                                                                        const btMatrix3x3& b,
     196                                                                        btScalar t)
     197{
     198btMatrix3x3     r;
     199r[0]=Lerp(a[0],b[0],t);
     200r[1]=Lerp(a[1],b[1],t);
     201r[2]=Lerp(a[2],b[2],t);
     202return(r);
    203203}
    204204//
    205205static inline btVector3         Clamp(const btVector3& v,btScalar maxlength)
    206206{
    207         const btScalar sql=v.length2();
    208         if(sql>(maxlength*maxlength))
    209                 return((v*maxlength)/btSqrt(sql));
     207const btScalar sql=v.length2();
     208if(sql>(maxlength*maxlength))
     209        return((v*maxlength)/btSqrt(sql));
    210210        else
    211                 return(v);
     211        return(v);
    212212}
    213213//
    … …  
    234234static inline btScalar          ClusterMetric(const btVector3& x,const btVector3& y)
    235235{
    236         const btVector3 d=x-y;
    237         return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
     236const btVector3 d=x-y;
     237return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
    238238}
    239239//
    … …  
    272272//
    273273static inline btMatrix3x3       Add(const btMatrix3x3& a,
    274                                                                 const btMatrix3x3& b)
     274        const btMatrix3x3& b)
    275275{
    276276        btMatrix3x3     r;
    … …  
    280280//
    281281static inline btMatrix3x3       Sub(const btMatrix3x3& a,
    282                                                                 const btMatrix3x3& b)
     282        const btMatrix3x3& b)
    283283{
    284284        btMatrix3x3     r;
    … …  
    288288//
    289289static inline btMatrix3x3       Mul(const btMatrix3x3& a,
    290                                                                 btScalar b)
     290        btScalar b)
    291291{
    292292        btMatrix3x3     r;
    … …  
    297297static inline void                      Orthogonalize(btMatrix3x3& m)
    298298{
    299         m[2]=cross(m[0],m[1]).normalized();
    300         m[1]=cross(m[2],m[0]).normalized();
    301         m[0]=cross(m[1],m[2]).normalized();
     299m[2]=cross(m[0],m[1]).normalized();
     300m[1]=cross(m[2],m[0]).normalized();
     301m[0]=cross(m[1],m[2]).normalized();
    302302}
    303303//
    … …  
    310310//
    311311static inline btMatrix3x3       ImpulseMatrix(  btScalar dt,
    312                                                                                   btScalar ima,
    313                                                                                   btScalar imb,
    314                                                                                   const btMatrix3x3& iwi,
    315                                                                                   const btVector3& r)
     312        btScalar ima,
     313        btScalar imb,
     314        const btMatrix3x3& iwi,
     315        const btVector3& r)
    316316{
    317317        return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
    … …  
    320320//
    321321static inline btMatrix3x3       ImpulseMatrix(  btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
    322                                                                                   btScalar imb,const btMatrix3x3& iib,const btVector3& rb)     
    323 {
    324         return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
     322                                                                                        btScalar imb,const btMatrix3x3& iib,const btVector3& rb)       
     323{
     324return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
    325325}
    326326
    327327//
    328328static inline btMatrix3x3       AngularImpulseMatrix(   const btMatrix3x3& iia,
    329                                                                                                  const btMatrix3x3& iib)
    330 {
    331         return(Add(iia,iib).inverse());
     329                                                                                                        const btMatrix3x3& iib)
     330{
     331return(Add(iia,iib).inverse());
    332332}
    333333
    334334//
    335335static inline btVector3         ProjectOnAxis(  const btVector3& v,
    336                                                                                   const btVector3& a)
     336        const btVector3& a)
    337337{
    338338        return(a*dot(v,a));
    … …  
    340340//
    341341static inline btVector3         ProjectOnPlane( const btVector3& v,
    342                                                                                    const btVector3& a)
     342        const btVector3& a)
    343343{
    344344        return(v-ProjectOnAxis(v,a));
    … …  
    347347//
    348348static inline void                      ProjectOrigin(  const btVector3& a,
    349                                                                                   const btVector3& b,
    350                                                                                   btVector3& prj,
    351                                                                                   btScalar& sqd)
    352 {
    353         const btVector3 d=b-a;
    354         const btScalar  m2=d.length2();
    355         if(m2>SIMD_EPSILON)
     349                                                                                        const btVector3& b,
     350                                                                                        btVector3& prj,
     351                                                                                        btScalar& sqd)
     352{
     353const btVector3 d=b-a;
     354const btScalar  m2=d.length2();
     355if(m2>SIMD_EPSILON)
    356356        {       
    357                 const btScalar  t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
    358                 const btVector3 p=a+d*t;
    359                 const btScalar  l2=p.length2();
    360                 if(l2<sqd)
    361                 {
     357        const btScalar  t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
     358        const btVector3 p=a+d*t;
     359        const btScalar  l2=p.length2();
     360        if(l2<sqd)
     361                {
     362                prj=p;
     363                sqd=l2;
     364                }
     365        }
     366}
     367//
     368static inline void                      ProjectOrigin(  const btVector3& a,
     369                                                                                        const btVector3& b,
     370                                                                                        const btVector3& c,
     371                                                                                        btVector3& prj,
     372                                                                                        btScalar& sqd)
     373{
     374const btVector3&        q=cross(b-a,c-a);
     375const btScalar          m2=q.length2();
     376if(m2>SIMD_EPSILON)
     377        {
     378        const btVector3 n=q/btSqrt(m2);
     379        const btScalar  k=dot(a,n);
     380        const btScalar  k2=k*k;
     381        if(k2<sqd)
     382                {
     383                const btVector3 p=n*k;
     384                if(     (dot(cross(a-p,b-p),q)>0)&&
     385                        (dot(cross(b-p,c-p),q)>0)&&
     386                        (dot(cross(c-p,a-p),q)>0))
     387                        {                       
    362388                        prj=p;
    363                         sqd=l2;
    364                 }
    365         }
    366 }
    367 //
    368 static inline void                      ProjectOrigin(  const btVector3& a,
    369                                                                                   const btVector3& b,
    370                                                                                   const btVector3& c,
    371                                                                                   btVector3& prj,
    372                                                                                   btScalar& sqd)
    373 {
    374         const btVector3&        q=cross(b-a,c-a);
    375         const btScalar          m2=q.length2();
    376         if(m2>SIMD_EPSILON)
    377         {
    378                 const btVector3 n=q/btSqrt(m2);
    379                 const btScalar  k=dot(a,n);
    380                 const btScalar  k2=k*k;
    381                 if(k2<sqd)
    382                 {
    383                         const btVector3 p=n*k;
    384                         if(     (dot(cross(a-p,b-p),q)>0)&&
    385                                 (dot(cross(b-p,c-p),q)>0)&&
    386                                 (dot(cross(c-p,a-p),q)>0))
    387                         {                       
    388                                 prj=p;
    389                                 sqd=k2;
     389                        sqd=k2;
    390390                        }
    391391                        else
    392392                        {
    393                                 ProjectOrigin(a,b,prj,sqd);
    394                                 ProjectOrigin(b,c,prj,sqd);
    395                                 ProjectOrigin(c,a,prj,sqd);
     393                        ProjectOrigin(a,b,prj,sqd);
     394                        ProjectOrigin(b,c,prj,sqd);
     395                        ProjectOrigin(c,a,prj,sqd);
    396396                        }
    397397                }
    … …  
    402402template <typename T>
    403403static inline T                         BaryEval(               const T& a,
    404                                                                          const T& b,
    405                                                                          const T& c,
    406                                                                          const btVector3& coord)
     404                                                                                        const T& b,
     405                                                                                        const T& c,
     406                                                                                        const btVector3& coord)
    407407{
    408408        return(a*coord.x()+b*coord.y()+c*coord.z());
    … …  
    410410//
    411411static inline btVector3         BaryCoord(      const btVector3& a,
    412                                                                           const btVector3& b,
    413                                                                           const btVector3& c,
    414                                                                           const btVector3& p)
    415 {
    416         const btScalar  w[]={   cross(a-p,b-p).length(),
    417                 cross(b-p,c-p).length(),
    418                 cross(c-p,a-p).length()};
    419         const btScalar  isum=1/(w[0]+w[1]+w[2]);
    420         return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
     412                                                                                const btVector3& b,
     413                                                                                const btVector3& c,
     414                                                                                const btVector3& p)
     415{
     416const btScalar  w[]={   cross(a-p,b-p).length(),
     417                                                cross(b-p,c-p).length(),
     418                                                cross(c-p,a-p).length()};
     419const btScalar  isum=1/(w[0]+w[1]+w[2]);
     420return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
    421421}
    422422
    423423//
    424424static btScalar                         ImplicitSolve(  btSoftBody::ImplicitFn* fn,
    425                                                                                   const btVector3& a,
    426                                                                                   const btVector3& b,
    427                                                                                   const btScalar accuracy,
    428                                                                                   const int maxiterations=256)
    429 {
    430         btScalar        span[2]={0,1};
    431         btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
    432         if(values[0]>values[1])
    433         {
    434                 btSwap(span[0],span[1]);
    435                 btSwap(values[0],values[1]);
    436         }
    437         if(values[0]>-accuracy) return(-1);
    438         if(values[1]<+accuracy) return(-1);
    439         for(int i=0;i<maxiterations;++i)
    440         {
    441                 const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
    442                 const btScalar  v=fn->Eval(Lerp(a,b,t));
    443                 if((t<=0)||(t>=1))              break;
    444                 if(btFabs(v)<accuracy)  return(t);
    445                 if(v<0)
     425                                                                                        const btVector3& a,
     426                                                                                        const btVector3& b,
     427                                                                                        const btScalar accuracy,
     428                                                                                        const int maxiterations=256)
     429{
     430btScalar        span[2]={0,1};
     431btScalar        values[2]={fn->Eval(a),fn->Eval(b)};
     432if(values[0]>values[1])
     433        {
     434        btSwap(span[0],span[1]);
     435        btSwap(values[0],values[1]);
     436        }
     437if(values[0]>-accuracy) return(-1);
     438if(values[1]<+accuracy) return(-1);
     439for(int i=0;i<maxiterations;++i)
     440        {
     441        const btScalar  t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
     442        const btScalar  v=fn->Eval(Lerp(a,b,t));
     443        if((t<=0)||(t>=1))              break;
     444        if(btFabs(v)<accuracy)  return(t);
     445        if(v<0)
    446446                { span[0]=t;values[0]=v; }
    447447                else
    448448                { span[1]=t;values[1]=v; }
    449449        }
    450         return(-1);
     450return(-1);
    451451}
    452452
    … …  
    463463//
    464464static inline btDbvtVolume      VolumeOf(       const btSoftBody::Face& f,
    465                                                                          btScalar margin)
    466 {
    467         const btVector3*        pts[]={ &f.m_n[0]->m_x,
    468                 &f.m_n[1]->m_x,
    469                 &f.m_n[2]->m_x};
    470         btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
    471         vol.Expand(btVector3(margin,margin,margin));
    472         return(vol);
     465                                                                                btScalar margin)
     466{
     467const btVector3*        pts[]={ &f.m_n[0]->m_x,
     468                                                        &f.m_n[1]->m_x,
     469                                                        &f.m_n[2]->m_x};
     470btDbvtVolume            vol=btDbvtVolume::FromPoints(pts,3);
     471vol.Expand(btVector3(margin,margin,margin));
     472return(vol);
    473473}
    474474
    … …  
    476476static inline btVector3                 CenterOf(       const btSoftBody::Face& f)
    477477{
    478         return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
     478return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
    479479}
    480480
    481481//
    482482static inline btScalar                  AreaOf(         const btVector3& x0,
    483                                                                            const btVector3& x1,
    484                                                                            const btVector3& x2)
     483                                                                                        const btVector3& x1,
     484                                                                                        const btVector3& x2)
    485485{
    486486        const btVector3 a=x1-x0;
    … …  
    493493//
    494494static inline btScalar          VolumeOf(       const btVector3& x0,
    495                                                                          const btVector3& x1,
    496                                                                          const btVector3& x2,
    497                                                                          const btVector3& x3)
     495                                                                                const btVector3& x1,
     496                                                                                const btVector3& x2,
     497                                                                                const btVector3& x3)
    498498{
    499499        const btVector3 a=x1-x0;
    … …  
    505505//
    506506static void                                     EvaluateMedium( const btSoftBodyWorldInfo* wfi,
    507                                                                                    const btVector3& x,
    508                                                                                    btSoftBody::sMedium& medium)
     507                                                                                        const btVector3& x,
     508                                                                                        btSoftBody::sMedium& medium)
    509509{
    510510        medium.m_velocity       =       btVector3(0,0,0);
    … …  
    524524//
    525525static inline void                      ApplyClampedForce(      btSoftBody::Node& n,
    526                                                                                           const btVector3& f,
    527                                                                                           btScalar dt)
     526                                                                                                const btVector3& f,
     527                                                                                                btScalar dt)
    528528{
    529529        const btScalar  dtim=dt*n.m_im;
    … …  
    540540//
    541541static inline int               MatchEdge(      const btSoftBody::Node* a,
    542                                                                   const btSoftBody::Node* b,
    543                                                                   const btSoftBody::Node* ma,
    544                                                                   const btSoftBody::Node* mb)
    545 {
    546         if((a==ma)&&(b==mb)) return(0);
    547         if((a==mb)&&(b==ma)) return(1);
    548         return(-1);
     542                                                                        const btSoftBody::Node* b,
     543                                                                        const btSoftBody::Node* ma,
     544                                                                        const btSoftBody::Node* mb)
     545{
     546if((a==ma)&&(b==mb)) return(0);
     547if((a==mb)&&(b==ma)) return(1);
     548return(-1);
    549549}
    550550
    … …  
    556556struct  btEigen
    557557{
    558         static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
    559         {
    560                 static const int                maxiterations=16;
    561                 static const btScalar   accuracy=(btScalar)0.0001;
    562                 btMatrix3x3&                    v=*vectors;
    563                 int                                             iterations=0;
    564                 vectors->setIdentity();
    565                 do      {
    566                         int                             p=0,q=1;
    567                         if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
    568                         if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
    569                         if(btFabs(a[p][q])>accuracy)
     558static int                      system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
     559        {
     560        static const int                maxiterations=16;
     561        static const btScalar   accuracy=(btScalar)0.0001;
     562        btMatrix3x3&                    v=*vectors;
     563        int                                             iterations=0;
     564        vectors->setIdentity();
     565        do      {
     566                int                             p=0,q=1;
     567                if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
     568                if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
     569                if(btFabs(a[p][q])>accuracy)
    570570                        {
    571                                 const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
    572                                 const btScalar  z=btFabs(w);
    573                                 const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
    574                                 if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
     571                        const btScalar  w=(a[q][q]-a[p][p])/(2*a[p][q]);
     572                        const btScalar  z=btFabs(w);
     573                        const btScalar  t=w/(z*(btSqrt(1+w*w)+z));
     574                        if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
    575575                                {
    576                                         const btScalar  c=1/btSqrt(t*t+1);
    577                                         const btScalar  s=c*t;
    578                                         mulPQ(a,c,s,p,q);
    579                                         mulTPQ(a,c,s,p,q);
    580                                         mulPQ(v,c,s,p,q);
     576                                const btScalar  c=1/btSqrt(t*t+1);
     577                                const btScalar  s=c*t;
     578                                mulPQ(a,c,s,p,q);
     579                                mulTPQ(a,c,s,p,q);
     580                                mulPQ(v,c,s,p,q);
    581581                                } else break;
    582582                        } else break;
    583583                } while((++iterations)<maxiterations);
    584                 if(values)
    585                 {
    586                         *values=btVector3(a[0][0],a[1][1],a[2][2]);
    587                 }
    588                 return(iterations);
     584        if(values)
     585                {
     586                *values=btVector3(a[0][0],a[1][1],a[2][2]);
     587                }
     588        return(iterations);
    589589        }
    590590private:
    591         static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    592         {
    593                 const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
    594                 {a[q][0],a[q][1],a[q][2]}};
    595                 int i;
    596 
    597                 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
    598                 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
    599         }
    600         static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    601         {
    602                 const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
    603                 {a[0][q],a[1][q],a[2][q]}};
    604                 int i;
    605 
    606                 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
    607                 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
     591static inline void      mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
     592        {
     593        const btScalar  m[2][3]={       {a[p][0],a[p][1],a[p][2]},
     594                                                                {a[q][0],a[q][1],a[q][2]}};
     595        int i;
     596
     597        for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
     598        for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
     599        }
     600static inline void      mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
     601        {
     602        const btScalar  m[2][3]={       {a[0][p],a[1][p],a[2][p]},
     603                                                                {a[0][q],a[1][q],a[2][q]}};
     604        int i;
     605
     606        for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
     607        for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
    608608        }
    609609};
    … …  
    640640                s.setIdentity();
    641641        }
    642         return(i);
     642return(i);
    643643}
    644644
    … …  
    653653        struct  ClusterBase : btDbvt::ICollide
    654654        {
    655                 btScalar                        erp;
    656                 btScalar                        idt;
    657                 btScalar                        margin;
    658                 btScalar                        friction;
    659                 btScalar                        threshold;
    660                 ClusterBase()
    661                 {
    662                         erp                     =(btScalar)1;
    663                         idt                     =0;
    664                         margin          =0;
    665                         friction        =0;
    666                         threshold       =(btScalar)0;
    667                 }
    668                 bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
    669                         btSoftBody::Body ba,btSoftBody::Body bb,
    670                         btSoftBody::CJoint& joint)
    671                 {
    672                         if(res.distance<margin)
     655        btScalar                        erp;
     656        btScalar                        idt;
     657        btScalar                        margin;
     658        btScalar                        friction;
     659        btScalar                        threshold;
     660                                        ClusterBase()
     661                {
     662                erp                     =(btScalar)1;
     663                idt                     =0;
     664                margin          =0;
     665                friction        =0;
     666                threshold       =(btScalar)0;
     667                }
     668        bool                            SolveContact(   const btGjkEpaSolver2::sResults& res,
     669                                                                                btSoftBody::Body ba,btSoftBody::Body bb,
     670                                                                                btSoftBody::CJoint& joint)
     671                {
     672                if(res.distance<margin)
    673673                        {
    674                                 const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
    675                                 const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
    676                                 const btVector3         va=ba.velocity(ra);
    677                                 const btVector3         vb=bb.velocity(rb);
    678                                 const btVector3         vrel=va-vb;
    679                                 const btScalar          rvac=dot(vrel,res.normal);
    680                                 const btScalar          depth=res.distance-margin;
    681                                 const btVector3         iv=res.normal*rvac;
    682                                 const btVector3         fv=vrel-iv;
    683                                 joint.m_bodies[0]       =       ba;
    684                                 joint.m_bodies[1]       =       bb;
    685                                 joint.m_refs[0]         =       ra*ba.xform().getBasis();
    686                                 joint.m_refs[1]         =       rb*bb.xform().getBasis();
    687                                 joint.m_rpos[0]         =       ra;
    688                                 joint.m_rpos[1]         =       rb;
    689                                 joint.m_cfm                     =       1;
    690                                 joint.m_erp                     =       1;
    691                                 joint.m_life            =       0;
    692                                 joint.m_maxlife         =       0;
    693                                 joint.m_split           =       1;
    694                                 joint.m_drift           =       depth*res.normal;
    695                                 joint.m_normal          =       res.normal;
    696                                 joint.m_delete          =       false;
    697                                 joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
    698                                 joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
    699                                         bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
    700                                 return(true);
     674                        const btVector3         ra=res.witnesses[0]-ba.xform().getOrigin();
     675                        const btVector3         rb=res.witnesses[1]-bb.xform().getOrigin();
     676                        const btVector3         va=ba.velocity(ra);
     677                        const btVector3         vb=bb.velocity(rb);
     678                        const btVector3         vrel=va-vb;
     679                        const btScalar          rvac=dot(vrel,res.normal);
     680                        const btScalar          depth=res.distance-margin;
     681                        const btVector3         iv=res.normal*rvac;
     682                        const btVector3         fv=vrel-iv;
     683                        joint.m_bodies[0]       =       ba;
     684                        joint.m_bodies[1]       =       bb;
     685                        joint.m_refs[0]         =       ra*ba.xform().getBasis();
     686                        joint.m_refs[1]         =       rb*bb.xform().getBasis();
     687                        joint.m_rpos[0]         =       ra;
     688                        joint.m_rpos[1]         =       rb;
     689                        joint.m_cfm                     =       1;
     690                        joint.m_erp                     =       1;
     691                        joint.m_life            =       0;
     692                        joint.m_maxlife         =       0;
     693                        joint.m_split           =       1;
     694                        joint.m_drift           =       depth*res.normal;
     695                        joint.m_normal          =       res.normal;
     696                        joint.m_delete          =       false;
     697                        joint.m_friction        =       fv.length2()<(-rvac*friction)?1:friction;
     698                        joint.m_massmatrix      =       ImpulseMatrix(  ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
     699                                                                                                        bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
     700                        return(true);
    701701                        }
    702                         return(false);
     702                return(false);
    703703                }
    704704        };
    … …  
    708708        struct  CollideCL_RS : ClusterBase
    709709        {
    710                 btSoftBody*             psb;
    711                 btRigidBody*    prb;
    712                 void            Process(const btDbvtNode* leaf)
    713                 {
    714                         btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
    715                         btSoftClusterCollisionShape     cshape(cluster);
    716                         const btConvexShape*            rshape=(const btConvexShape*)prb->getCollisionShape();
    717                         btGjkEpaSolver2::sResults       res;           
    718                         if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
    719                                 rshape,prb->getInterpolationWorldTransform(),
    720                                 btVector3(1,0,0),res))
     710        btSoftBody*             psb;
     711        btRigidBody*    prb;
     712        void            Process(const btDbvtNode* leaf)
     713                {
     714                btSoftBody::Cluster*            cluster=(btSoftBody::Cluster*)leaf->data;
     715                btSoftClusterCollisionShape     cshape(cluster);
     716                const btConvexShape*            rshape=(const btConvexShape*)prb->getCollisionShape();
     717                btGjkEpaSolver2::sResults       res;           
     718                if(btGjkEpaSolver2::SignedDistance(     &cshape,btTransform::getIdentity(),
     719                                                                                        rshape,prb->getInterpolationWorldTransform(),
     720                                                                                        btVector3(1,0,0),res))
    721721                        {
    722                                 btSoftBody::CJoint      joint;
    723                                 if(SolveContact(res,cluster,prb,joint))
     722                        btSoftBody::CJoint      joint;
     723                        if(SolveContact(res,cluster,prb,joint))
    724724                                {
    725                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
    726                                         *pj=joint;psb->m_joints.push_back(pj);
    727                                         if(prb->isStaticOrKinematicObject())
     725                                btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
     726                                *pj=joint;psb->m_joints.push_back(pj);
     727                                if(prb->isStaticOrKinematicObject())
    728728                                        {
    729                                                 pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
    730                                                 pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
     729                                        pj->m_erp       *=      psb->m_cfg.kSKHR_CL;
     730                                        pj->m_split     *=      psb->m_cfg.kSK_SPLT_CL;
    731731                                        }
    732732                                        else
    733733                                        {
    734                                                 pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
    735                                                 pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
     734                                        pj->m_erp       *=      psb->m_cfg.kSRHR_CL;
     735                                        pj->m_split     *=      psb->m_cfg.kSR_SPLT_CL;
    736736                                        }
    737737                                }
    738738                        }
    739739                }
    740                 void            Process(btSoftBody* ps,btRigidBody* pr)
    741                 {
    742                         psb                     =       ps;
    743                         prb                     =       pr;
    744                         idt                     =       ps->m_sst.isdt;
    745                         margin          =       ps->getCollisionShape()->getMargin()+
    746                                 pr->getCollisionShape()->getMargin();
    747                         friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
    748                         btVector3                       mins;
    749                         btVector3                       maxs;
    750 
    751                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
    752                         pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
    753                         volume=btDbvtVolume::FromMM(mins,maxs);
    754                         volume.Expand(btVector3(1,1,1)*margin);
    755                         btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
     740        void            Process(btSoftBody* ps,btRigidBody* pr)
     741                {
     742                psb                     =       ps;
     743                prb                     =       pr;
     744                idt                     =       ps->m_sst.isdt;
     745                margin          =       ps->getCollisionShape()->getMargin()+
     746                                                pr->getCollisionShape()->getMargin();
     747                friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
     748                btVector3                       mins;
     749                btVector3                       maxs;
     750               
     751                ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
     752                pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
     753                volume=btDbvtVolume::FromMM(mins,maxs);
     754                volume.Expand(btVector3(1,1,1)*margin);
     755                btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
    756756                }       
    757757        };
    … …  
    761761        struct  CollideCL_SS : ClusterBase
    762762        {
    763                 btSoftBody*     bodies[2];
    764                 void            Process(const btDbvtNode* la,const btDbvtNode* lb)
    765                 {
    766                         btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
    767                         btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
    768                         btSoftClusterCollisionShape     csa(cla);
    769                         btSoftClusterCollisionShape     csb(clb);
    770                         btGjkEpaSolver2::sResults       res;           
    771                         if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
    772                                 &csb,btTransform::getIdentity(),
    773                                 cla->m_com-clb->m_com,res))
     763        btSoftBody*     bodies[2];
     764        void            Process(const btDbvtNode* la,const btDbvtNode* lb)
     765                {
     766                btSoftBody::Cluster*            cla=(btSoftBody::Cluster*)la->data;
     767                btSoftBody::Cluster*            clb=(btSoftBody::Cluster*)lb->data;
     768                btSoftClusterCollisionShape     csa(cla);
     769                btSoftClusterCollisionShape     csb(clb);
     770                btGjkEpaSolver2::sResults       res;           
     771                if(btGjkEpaSolver2::SignedDistance(     &csa,btTransform::getIdentity(),
     772                                                                                        &csb,btTransform::getIdentity(),
     773                                                                                        cla->m_com-clb->m_com,res))
    774774                        {
    775                                 btSoftBody::CJoint      joint;
    776                                 if(SolveContact(res,cla,clb,joint))
     775                        btSoftBody::CJoint      joint;
     776                        if(SolveContact(res,cla,clb,joint))
    777777                                {
    778                                         btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
    779                                         *pj=joint;bodies[0]->m_joints.push_back(pj);
    780                                         pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
    781                                         pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
     778                                btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
     779                                *pj=joint;bodies[0]->m_joints.push_back(pj);
     780                                pj->m_erp       *=      btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
     781                                pj->m_split     *=      (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
    782782                                }
    783783                        }
    784784                }
    785                 void            Process(btSoftBody* psa,btSoftBody* psb)
    786                 {
    787                         idt                     =       psa->m_sst.isdt;
    788                         margin          =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
    789                         friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
    790                         bodies[0]       =       psa;
    791                         bodies[1]       =       psb;
    792                         btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
     785        void            Process(btSoftBody* psa,btSoftBody* psb)
     786                {
     787                idt                     =       psa->m_sst.isdt;
     788                margin          =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
     789                friction        =       btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
     790                bodies[0]       =       psa;
     791                bodies[1]       =       psb;
     792                btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
    793793                }       
    794794        };
    … …  
    798798        struct  CollideSDF_RS : btDbvt::ICollide
    799799        {
    800                 void            Process(const btDbvtNode* leaf)
    801                 {
    802                         btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
    803                         DoNode(*node);
    804                 }
    805                 void            DoNode(btSoftBody::Node& n) const
    806                 {
    807                         const btScalar                  m=n.m_im>0?dynmargin:stamargin;
    808                         btSoftBody::RContact    c;
    809                         if(     (!n.m_battach)&&
    810                                 psb->checkContact(prb,n.m_x,m,c.m_cti))
     800        void            Process(const btDbvtNode* leaf)
     801                {
     802                btSoftBody::Node*       node=(btSoftBody::Node*)leaf->data;
     803                DoNode(*node);
     804                }
     805        void            DoNode(btSoftBody::Node& n) const
     806                {
     807                const btScalar                  m=n.m_im>0?dynmargin:stamargin;
     808                btSoftBody::RContact    c;
     809                if(     (!n.m_battach)&&
     810                        psb->checkContact(prb,n.m_x,m,c.m_cti))
    811811                        {
    812                                 const btScalar  ima=n.m_im;
    813                                 const btScalar  imb=prb->getInvMass();
    814                                 const btScalar  ms=ima+imb;
    815                                 if(ms>0)
     812                        const btScalar  ima=n.m_im;
     813                        const btScalar  imb=prb->getInvMass();
     814                        const btScalar  ms=ima+imb;
     815                        if(ms>0)
    816816                                {
    817                                         const btTransform&      wtr=prb->getInterpolationWorldTransform();
    818                                         const btMatrix3x3&      iwi=prb->getInvInertiaTensorWorld();
    819                                         const btVector3         ra=n.m_x-wtr.getOrigin();
    820                                         const btVector3         va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
    821                                         const btVector3         vb=n.m_x-n.m_q;
    822                                         const btVector3         vr=vb-va;
    823                                         const btScalar          dn=dot(vr,c.m_cti.m_normal);
    824                                         const btVector3         fv=vr-c.m_cti.m_normal*dn;
    825                                         const btScalar          fc=psb->m_cfg.kDF*prb->getFriction();
    826                                         c.m_node        =       &n;
    827                                         c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
    828                                         c.m_c1          =       ra;
    829                                         c.m_c2          =       ima*psb->m_sst.sdt;
    830                                         c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
    831                                         c.m_c4          =       prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
    832                                         psb->m_rcontacts.push_back(c);
    833                                         prb->activate();
     817                                const btTransform&      wtr=prb->getInterpolationWorldTransform();
     818                                const btMatrix3x3&      iwi=prb->getInvInertiaTensorWorld();
     819                                const btVector3         ra=n.m_x-wtr.getOrigin();
     820                                const btVector3         va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
     821                                const btVector3         vb=n.m_x-n.m_q;
     822                                const btVector3         vr=vb-va;
     823                                const btScalar          dn=dot(vr,c.m_cti.m_normal);
     824                                const btVector3         fv=vr-c.m_cti.m_normal*dn;
     825                                const btScalar          fc=psb->m_cfg.kDF*prb->getFriction();
     826                                c.m_node        =       &n;
     827                                c.m_c0          =       ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
     828                                c.m_c1          =       ra;
     829                                c.m_c2          =       ima*psb->m_sst.sdt;
     830                                c.m_c3          =       fv.length2()<(btFabs(dn)*fc)?0:1-fc;
     831                                c.m_c4          =       prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
     832                                psb->m_rcontacts.push_back(c);
     833                                prb->activate();
    834834                                }
    835835                        }
    836836                }
    837                 btSoftBody*             psb;
    838                 btRigidBody*    prb;
    839                 btScalar                dynmargin;
    840                 btScalar                stamargin;
     837        btSoftBody*             psb;
     838        btRigidBody*    prb;
     839        btScalar                dynmargin;
     840        btScalar                stamargin;
    841841        };
    842842        //
    … …  
    845845        struct  CollideVF_SS : btDbvt::ICollide
    846846        {
    847                 void            Process(const btDbvtNode* lnode,
    848                         const btDbvtNode* lface)
    849                 {
    850                         btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
    851                         btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
    852                         btVector3                       o=node->m_x;
    853                         btVector3                       p;
    854                         btScalar                        d=SIMD_INFINITY;
    855                         ProjectOrigin(  face->m_n[0]->m_x-o,
    856                                 face->m_n[1]->m_x-o,
    857                                 face->m_n[2]->m_x-o,
    858                                 p,d);
    859                         const btScalar  m=mrg+(o-node->m_q).length()*2;
    860                         if(d<(m*m))
     847        void            Process(const btDbvtNode* lnode,
     848                                                const btDbvtNode* lface)
     849                {
     850                btSoftBody::Node*       node=(btSoftBody::Node*)lnode->data;
     851                btSoftBody::Face*       face=(btSoftBody::Face*)lface->data;
     852                btVector3                       o=node->m_x;
     853                btVector3                       p;
     854                btScalar                        d=SIMD_INFINITY;
     855                ProjectOrigin(  face->m_n[0]->m_x-o,
     856                                                face->m_n[1]->m_x-o,
     857                                                face->m_n[2]->m_x-o,
     858                                                p,d);
     859                const btScalar  m=mrg+(o-node->m_q).length()*2;
     860                if(d<(m*m))
    861861                        {
    862                                 const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
    863                                 const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
    864                                 const btScalar                  ma=node->m_im;
    865                                 btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
    866                                 if(     (n[0]->m_im<=0)||
    867                                         (n[1]->m_im<=0)||
    868                                         (n[2]->m_im<=0))
     862                        const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
     863                        const btVector3                 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
     864                        const btScalar                  ma=node->m_im;
     865                        btScalar                                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
     866                        if(     (n[0]->m_im<=0)||
     867                                (n[1]->m_im<=0)||
     868                                (n[2]->m_im<=0))
    869869                                {
    870                                         mb=0;
     870                                mb=0;
    871871                                }
    872                                 const btScalar  ms=ma+mb;
    873                                 if(ms>0)
     872                        const btScalar  ms=ma+mb;
     873                        if(ms>0)
    874874                                {
    875                                         btSoftBody::SContact    c;
    876                                         c.m_normal              =       p/-btSqrt(d);
    877                                         c.m_margin              =       m;
    878                                         c.m_node                =       node;
    879                                         c.m_face                =       face;
    880                                         c.m_weights             =       w;
    881                                         c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
    882                                         c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
    883                                         c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
    884                                         psb[0]->m_scontacts.push_back(c);
     875                                btSoftBody::SContact    c;
     876                                c.m_normal              =       p/-btSqrt(d);
     877                                c.m_margin              =       m;
     878                                c.m_node                =       node;
     879                                c.m_face                =       face;
     880                                c.m_weights             =       w;
     881                                c.m_friction    =       btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
     882                                c.m_cfm[0]              =       ma/ms*psb[0]->m_cfg.kSHR;
     883                                c.m_cfm[1]              =       mb/ms*psb[1]->m_cfg.kSHR;
     884                                psb[0]->m_scontacts.push_back(c);
    885885                                }
    886886                        }       
    887887                }
    888                 btSoftBody*             psb[2];
    889                 btScalar                mrg;
     888        btSoftBody*             psb[2];
     889        btScalar                mrg;
    890890        };
    891891};

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp

    r1963 r1972  
    3030        mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
    3131        m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
    32 
     32       
    3333        mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
    3434        m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
    35 
     35       
    3636        mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
    3737        m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
    … …  
    4141        mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
    4242        m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
    43 
     43       
    4444        mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
    4545        m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
    … …  
    4848
    4949        //replace pool by a new one, with potential larger size
    50 
     50       
    5151        if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool)
    5252        {
    5353                int curElemSize = m_collisionAlgorithmPool->getElementSize();
    5454                ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
    55 
    56 
     55               
     56               
    5757                int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
    5858                int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
    … …  
    9393#endif
    9494}
    95 
     95       
    9696///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
    9797btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h

    r1963 r1972  
    3333        btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc;
    3434        btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc;
    35 
     35       
    3636public:
    3737

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp

    r1963 r1972  
    3636btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
    3737{
    38 
     38       
    3939        //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
    4040
    4141        /*if (m_ownManifold)
    4242        {
    43         if (m_manifoldPtr)
    44         m_dispatcher->releaseManifold(m_manifoldPtr);
     43                if (m_manifoldPtr)
     44                        m_dispatcher->releaseManifold(m_manifoldPtr);
    4545        }
    4646        */
    … …  
    5959        btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
    6060        btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
    61 
     61       
    6262        softBody->defaultCollisionHandler(rigidCollisionObject);
    6363

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.h

    r1963 r1972  
    2929class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
    3030{
    31         //      bool    m_ownManifold;
    32         //      btPersistentManifold*   m_manifoldPtr;
     31//      bool    m_ownManifold;
     32//      btPersistentManifold*   m_manifoldPtr;
    3333
    3434        btSoftBody*                             m_softBody;
    … …  
    3737        ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
    3838        bool    m_isSwapped;
    39 
     39       
    4040public:
    4141
    … …  
    5353        }
    5454
    55 
     55       
    5656        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
    5757        {

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.cpp

    r1963 r1972  
    2929:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
    3030{
    31         m_drawFlags                     =       fDrawFlags::Std;
    32         m_drawNodeTree          =       true;
    33         m_drawFaceTree          =       false;
    34         m_drawClusterTree       =       false;
    35         m_sbi.m_broadphase = pairCache;
    36         m_sbi.m_dispatcher = dispatcher;
    37         m_sbi.m_sparsesdf.Initialize();
    38         m_sbi.m_sparsesdf.Reset();
     31m_drawFlags                     =       fDrawFlags::Std;
     32m_drawNodeTree          =       true;
     33m_drawFaceTree          =       false;
     34m_drawClusterTree       =       false;
     35m_sbi.m_broadphase = pairCache;
     36m_sbi.m_dispatcher = dispatcher;
     37m_sbi.m_sparsesdf.Initialize();
     38m_sbi.m_sparsesdf.Reset();
    3939
    4040}
    41 
     41               
    4242btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
    4343{
    … …  
    5656        }
    5757}
    58 
     58               
    5959void    btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
    6060{
    … …  
    7272{
    7373        BT_PROFILE("updateSoftBodies");
    74 
     74       
    7575        for ( int i=0;i<m_softBodies.size();i++)
    7676        {
    … …  
    8383{
    8484        BT_PROFILE("solveSoftConstraints");
    85 
     85       
    8686        if(m_softBodies.size())
    87         {
     87                {
    8888                btSoftBody::solveClusters(m_softBodies);
    89         }
    90 
     89                }
     90       
    9191        for(int i=0;i<m_softBodies.size();++i)
    9292        {
    … …  
    101101
    102102        btCollisionWorld::addCollisionObject(body,
    103                 btBroadphaseProxy::DefaultFilter,
    104                 btBroadphaseProxy::AllFilter);
     103                                        btBroadphaseProxy::DefaultFilter,
     104                                        btBroadphaseProxy::AllFilter);
    105105
    106106}

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.h

    r1963 r1972  
    2424class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
    2525{
    26 
     26       
    2727        btSoftBodyArray m_softBodies;
    2828        int                             m_drawFlags;
    … …  
    3333
    3434protected:
    35 
     35       
    3636        virtual void    predictUnconstraintMotion(btScalar timeStep);
    37 
     37       
    3838        virtual void    internalSingleStepSimulation( btScalar timeStep);
    3939
    … …  
    4444
    4545public:
    46 
     46       
    4747        btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
    4848
    4949        virtual ~btSoftRigidDynamicsWorld();
    50 
     50       
    5151        virtual void    debugDrawWorld();
    52 
     52                       
    5353        void    addSoftBody(btSoftBody* body);
    5454
    5555        void    removeSoftBody(btSoftBody* body);
    56 
     56       
    5757        int             getDrawFlags() const { return(m_drawFlags); }
    5858        void    setDrawFlags(int f)     { m_drawFlags=f; }
    … …  
    6767        }
    6868
    69 
     69                       
    7070        btSoftBodyArray& getSoftBodyArray()
    7171        {
    … …  
    7777                return m_softBodies;
    7878        }
    79 
     79               
    8080};
    8181

  • code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.h

    r1963 r1972  
    3434        btSoftBody*     m_softBody1;
    3535
    36 
     36       
    3737public:
    3838        btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)

  • code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h

    r1963 r1972  
    2424template <const int DWORDLEN>
    2525unsigned int HsiehHash(const void* pdata)
    26 {
     26        {
    2727        const unsigned short*   data=(const unsigned short*)pdata;
    2828        unsigned                                hash=DWORDLEN<<2,tmp;
    2929        for(int i=0;i<DWORDLEN;++i)
    30         {
     30                {
    3131                hash    +=      data[0];
    3232                tmp             =       (data[1]<<11)^hash;
    … …  
    3434                data    +=      2;
    3535                hash    +=      hash>>11;
    36         }
     36                }
    3737        hash^=hash<<3;hash+=hash>>5;
    3838        hash^=hash<<4;hash+=hash>>17;
    3939        hash^=hash<<25;hash+=hash>>6;
    4040        return(hash);
    41 }
     41        }
    4242
    4343template <const int CELLSIZE>
    4444struct  btSparseSdf
    45 {
     45        {
    4646        //
    4747        // Inner types
    4848        //
    4949        struct IntFrac
    50         {
     50                {
    5151                int                                     b;
    5252                int                                     i;
    5353                btScalar                        f;
    54         };
     54                };
    5555        struct  Cell
    56         {
     56                {
    5757                btScalar                        d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
    5858                int                                     c[3];
    … …  
    6161                btCollisionShape*       pclient;
    6262                Cell*                           next;
    63         };
     63                };
    6464        //
    6565        // Fields
    6666        //
    67 
     67       
    6868        btAlignedObjectArray<Cell*>             cells; 
    6969        btScalar                                                voxelsz;
    … …  
    7272        int                                                             nprobes;
    7373        int                                                             nqueries;       
    74 
     74       
    7575        //
    7676        // Methods
    7777        //
    78 
     78       
    7979        //
    8080        void                                    Initialize(int hashsize=2383)
    81         {
     81                {
    8282                cells.resize(hashsize,0);
    8383                Reset();               
    84         }
     84                }
    8585        //
    8686        void                                    Reset()
    87         {
     87                {
    8888                for(int i=0,ni=cells.size();i<ni;++i)
    89                 {
     89                        {
    9090                        Cell*   pc=cells[i];
    9191                        cells[i]=0;
    9292                        while(pc)
    93                         {
     93                                {
    9494                                Cell*   pn=pc->next;
    9595                                delete pc;
    9696                                pc=pn;
    97                         }
    98                 }
     97                                }
     98                        }
    9999                voxelsz         =0.25;
    100100                puid            =0;
    … …  
    102102                nprobes         =1;
    103103                nqueries        =1;
    104         }
     104                }
    105105        //
    106106        void                                    GarbageCollect(int lifetime=256)
    107         {
     107                {
    108108                const int life=puid-lifetime;
    109109                for(int i=0;i<cells.size();++i)
    110                 {
     110                        {
    111111                        Cell*&  root=cells[i];
    112112                        Cell*   pp=0;
    113113                        Cell*   pc=root;
    114114                        while(pc)
    115                         {
     115                                {
    116116                                Cell*   pn=pc->next;
    117117                                if(pc->puid<life)
    118                                 {
     118                                        {
    119119                                        if(pp) pp->next=pn; else root=pn;
    120120                                        delete pc;pc=pp;--ncells;
     121                                        }
     122                                pp=pc;pc=pn;
    121123                                }
    122                                 pp=pc;pc=pn;
    123                         }
    124                 }
     124                        }
    125125                //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
    126126                nqueries=1;
    127127                nprobes=1;
    128128                ++puid; /* TODO: Reset puid's when int range limit is reached   */
    129                 /* else setup a priority list...                                                */
    130         }
     129                                /* else setup a priority list...                                                */
     130                }
    131131        //
    132132        int                                             RemoveReferences(btCollisionShape* pcs)
    133         {
     133                {
    134134                int     refcount=0;
    135135                for(int i=0;i<cells.size();++i)
    136                 {
     136                        {
    137137                        Cell*&  root=cells[i];
    138138                        Cell*   pp=0;
    139139                        Cell*   pc=root;
    140140                        while(pc)
    141                         {
     141                                {
    142142                                Cell*   pn=pc->next;
    143143                                if(pc->pclient==pcs)
    144                                 {
     144                                        {
    145145                                        if(pp) pp->next=pn; else root=pn;
    146146                                        delete pc;pc=pp;++refcount;
     147                                        }
     148                                pp=pc;pc=pn;
    147149                                }
    148                                 pp=pc;pc=pn;
    149                         }
    150                 }
     150                        }
    151151                return(refcount);
    152         }
     152                }
    153153        //
    154154        btScalar                                Evaluate(       const btVector3& x,
    155                 btCollisionShape* shape,
    156                 btVector3& normal,
    157                 btScalar margin)
    158         {
     155                                                                                btCollisionShape* shape,
     156                                                                                btVector3& normal,
     157                                                                                btScalar margin)
     158                {
    159159                /* Lookup cell                  */
    160160                const btVector3 scx=x/voxelsz;
    … …  
    167167                ++nqueries;
    168168                while(c)
    169                 {
     169                        {
    170170                        ++nprobes;
    171171                        if(     (c->hash==h)    &&
    … …  
    174174                                (c->c[2]==iz.b) &&
    175175                                (c->pclient==shape))
    176                         { break; }
    177                         else
    178                         { c=c->next; }
    179                 }
     176                                { break; }
     177                                else
     178                                { c=c->next; }
     179                        }
    180180                if(!c)
    181                 {
     181                        {
    182182                        ++nprobes;             
    183183                        ++ncells;
    … …  
    188188                        c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
    189189                        BuildCell(*c);
    190                 }
     190                        }
    191191                c->puid=puid;
    192192                /* Extract infos                */
    193193                const int               o[]={   ix.i,iy.i,iz.i};
    194194                const btScalar  d[]={   c->d[o[0]+0][o[1]+0][o[2]+0],
    195                         c->d[o[0]+1][o[1]+0][o[2]+0],
    196                         c->d[o[0]+1][o[1]+1][o[2]+0],
    197                         c->d[o[0]+0][o[1]+1][o[2]+0],
    198                         c->d[o[0]+0][o[1]+0][o[2]+1],
    199                         c->d[o[0]+1][o[1]+0][o[2]+1],
    200                         c->d[o[0]+1][o[1]+1][o[2]+1],
    201                         c->d[o[0]+0][o[1]+1][o[2]+1]};
     195                                                                c->d[o[0]+1][o[1]+0][o[2]+0],
     196                                                                c->d[o[0]+1][o[1]+1][o[2]+0],
     197                                                                c->d[o[0]+0][o[1]+1][o[2]+0],
     198                                                                c->d[o[0]+0][o[1]+0][o[2]+1],
     199                                                                c->d[o[0]+1][o[1]+0][o[2]+1],
     200                                                                c->d[o[0]+1][o[1]+1][o[2]+1],
     201                                                                c->d[o[0]+0][o[1]+1][o[2]+1]};
    202202                /* Normal       */
    203 #if 1
     203                #if 1
    204204                const btScalar  gx[]={  d[1]-d[0],d[2]-d[3],
    205                         d[5]-d[4],d[6]-d[7]};
     205                                                                d[5]-d[4],d[6]-d[7]};
    206206                const btScalar  gy[]={  d[3]-d[0],d[2]-d[1],
    207                         d[7]-d[4],d[6]-d[5]};
     207                                                                d[7]-d[4],d[6]-d[5]};
    208208                const btScalar  gz[]={  d[4]-d[0],d[5]-d[1],
    209                         d[7]-d[3],d[6]-d[2]};
     209                                                                d[7]-d[3],d[6]-d[2]};
    210210                normal.setX(Lerp(       Lerp(gx[0],gx[1],iy.f),
    211                         Lerp(gx[2],gx[3],iy.f),iz.f));
     211                                                        Lerp(gx[2],gx[3],iy.f),iz.f));
    212212                normal.setY(Lerp(       Lerp(gy[0],gy[1],ix.f),
    213                         Lerp(gy[2],gy[3],ix.f),iz.f));
     213                                                        Lerp(gy[2],gy[3],ix.f),iz.f));
    214214                normal.setZ(Lerp(       Lerp(gz[0],gz[1],ix.f),
    215                         Lerp(gz[2],gz[3],ix.f),iy.f));
     215                                                        Lerp(gz[2],gz[3],ix.f),iy.f));
    216216                normal          =       normal.normalized();
    217 #else
     217                #else
    218218                normal          =       btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
    219 #endif
     219                #endif
    220220                /* Distance     */
    221221                const btScalar  d0=Lerp(Lerp(d[0],d[1],ix.f),
    222                         Lerp(d[3],d[2],ix.f),iy.f);
     222                                                                Lerp(d[3],d[2],ix.f),iy.f);
    223223                const btScalar  d1=Lerp(Lerp(d[4],d[5],ix.f),
    224                         Lerp(d[7],d[6],ix.f),iy.f);
     224                                                                Lerp(d[7],d[6],ix.f),iy.f);
    225225                return(Lerp(d0,d1,iz.f)-margin);
    226         }
     226                }
    227227        //
    228228        void                                    BuildCell(Cell& c)
    229         {
     229                {
    230230                const btVector3 org=btVector3(  (btScalar)c.c[0],
    231                         (btScalar)c.c[1],
    232                         (btScalar)c.c[2])       *
    233                         CELLSIZE*voxelsz;
     231                                                                                (btScalar)c.c[1],
     232                                                                                (btScalar)c.c[2])       *
     233                                                                                CELLSIZE*voxelsz;
    234234                for(int k=0;k<=CELLSIZE;++k)
    235                 {
     235                        {
    236236                        const btScalar  z=voxelsz*k+org.z();
    237237                        for(int j=0;j<=CELLSIZE;++j)
    238                         {
     238                                {
    239239                                const btScalar  y=voxelsz*j+org.y();
    240240                                for(int i=0;i<=CELLSIZE;++i)
    241                                 {
     241                                        {
    242242                                        const btScalar  x=voxelsz*i+org.x();
    243243                                        c.d[i][j][k]=DistanceToShape(   btVector3(x,y,z),
    244                                                 c.pclient);
     244                                                                                                        c.pclient);
     245                                        }
    245246                                }
    246247                        }
    247248                }
    248         }
    249249        //
    250250        static inline btScalar  DistanceToShape(const btVector3& x,
    251                 btCollisionShape* shape)
    252         {
     251                                                                                        btCollisionShape* shape)
     252                {
    253253                btTransform     unit;
    254254                unit.setIdentity();
    255255                if(shape->isConvex())
    256                 {
     256                        {
    257257                        btGjkEpaSolver2::sResults       res;
    258258                        btConvexShape*                          csh=static_cast<btConvexShape*>(shape);
    259259                        return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
    260                 }
     260                        }
    261261                return(0);
    262         }
     262                }
    263263        //
    264264        static inline IntFrac   Decompose(btScalar x)
    265         {
     265                {
    266266                /* That one need a lot of improvements...       */
    267267                /* Remove test, faster floor...                         */
    … …  
    273273                r.i=(int)k;r.f=k-r.i;r.b-=o;
    274274                return(r);
    275         }
     275                }
    276276        //
    277277        static inline btScalar  Lerp(btScalar a,btScalar b,btScalar t)
    278         {
     278                {
    279279                return(a+(b-a)*t);
    280         }
    281 
    282 
     280                }
     281
     282       
    283283
    284284        //
    285285        static inline unsigned int      Hash(int x,int y,int z,btCollisionShape* shape)
    286         {
     286                {
    287287                struct btS
    288288                {
    … …  
    292292
    293293                btS myset;
    294 
     294               
    295295                myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
    296296                const void* ptr = &myset;
    297297
    298298                unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
    299 
     299               
    300300
    301301                return result;
    302         }
     302                }
    303303};
    304 
     304       
    305305
    306306#endif
Note: See TracChangeset for help on using the changeset viewer.