== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletSoftBody / btSoftBody.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 ///btSoftBody implementation by Nathanael Presson
16
17 #include "btSoftBodyInternals.h"
18
19 //
20 btSoftBody::btSoftBody(btSoftBodyWorldInfo*     worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
21 :m_worldInfo(worldInfo)
22 {       
23         /* Init         */ 
24         m_internalType          =       CO_SOFT_BODY;
25         m_cfg.aeromodel         =       eAeroModel::V_Point;
26         m_cfg.kVCF                      =       1;
27         m_cfg.kDG                       =       0;
28         m_cfg.kLF                       =       0;
29         m_cfg.kDP                       =       0;
30         m_cfg.kPR                       =       0;
31         m_cfg.kVC                       =       0;
32         m_cfg.kDF                       =       (btScalar)0.2;
33         m_cfg.kMT                       =       0;
34         m_cfg.kCHR                      =       (btScalar)1.0;
35         m_cfg.kKHR                      =       (btScalar)0.1;
36         m_cfg.kSHR                      =       (btScalar)1.0;
37         m_cfg.kAHR                      =       (btScalar)0.7;
38         m_cfg.kSRHR_CL          =       (btScalar)0.1;
39         m_cfg.kSKHR_CL          =       (btScalar)1;
40         m_cfg.kSSHR_CL          =       (btScalar)0.5;
41         m_cfg.kSR_SPLT_CL       =       (btScalar)0.5;
42         m_cfg.kSK_SPLT_CL       =       (btScalar)0.5;
43         m_cfg.kSS_SPLT_CL       =       (btScalar)0.5;
44         m_cfg.maxvolume         =       (btScalar)1;
45         m_cfg.timescale         =       1;
46         m_cfg.viterations       =       0;
47         m_cfg.piterations       =       1;      
48         m_cfg.diterations       =       0;
49         m_cfg.citerations       =       4;
50         m_cfg.collisions        =       fCollision::Default;
51         m_pose.m_bvolume        =       false;
52         m_pose.m_bframe         =       false;
53         m_pose.m_volume         =       0;
54         m_pose.m_com            =       btVector3(0,0,0);
55         m_pose.m_rot.setIdentity();
56         m_pose.m_scl.setIdentity();
57         m_tag                           =       0;
58         m_timeacc                       =       0;
59         m_bUpdateRtCst          =       true;
60         m_bounds[0]                     =       btVector3(0,0,0);
61         m_bounds[1]                     =       btVector3(0,0,0);
62         m_worldTransform.setIdentity();
63         setSolver(eSolverPresets::Positions);
64         /* Default material     */ 
65         Material*       pm=appendMaterial();
66         pm->m_kLST      =       1;
67         pm->m_kAST      =       1;
68         pm->m_kVST      =       1;
69         pm->m_flags     =       fMaterial::Default;
70         /* Collision shape      */ 
71         ///for now, create a collision shape internally
72         m_collisionShape = new btSoftBodyCollisionShape(this);
73         m_collisionShape->setMargin(0.25);
74         /* Nodes                        */ 
75         const btScalar          margin=getCollisionShape()->getMargin();
76         m_nodes.resize(node_count);
77         for(int i=0,ni=node_count;i<ni;++i)
78         {       
79                 Node&   n=m_nodes[i];
80                 ZeroInitialize(n);
81                 n.m_x           =       x?*x++:btVector3(0,0,0);
82                 n.m_q           =       n.m_x;
83                 n.m_im          =       m?*m++:1;
84                 n.m_im          =       n.m_im>0?1/n.m_im:0;
85                 n.m_leaf        =       m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
86                 n.m_material=   pm;
87         }
88         updateBounds(); 
89
90         m_initialWorldTransform.setIdentity();
91 }
92
93 //
94 btSoftBody::~btSoftBody()
95 {
96         //for now, delete the internal shape
97         delete m_collisionShape;        
98         int i;
99
100         releaseClusters();
101         for(i=0;i<m_materials.size();++i) 
102                 btAlignedFree(m_materials[i]);
103         for(i=0;i<m_joints.size();++i) 
104                 btAlignedFree(m_joints[i]);
105 }
106
107 //
108 bool                    btSoftBody::checkLink(int node0,int node1) const
109 {
110         return(checkLink(&m_nodes[node0],&m_nodes[node1]));
111 }
112
113 //
114 bool                    btSoftBody::checkLink(const Node* node0,const Node* node1) const
115 {
116         const Node*     n[]={node0,node1};
117         for(int i=0,ni=m_links.size();i<ni;++i)
118         {
119                 const Link&     l=m_links[i];
120                 if(     (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
121                         (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
122                 {
123                         return(true);
124                 }
125         }
126         return(false);
127 }
128
129 //
130 bool                    btSoftBody::checkFace(int node0,int node1,int node2) const
131 {
132         const Node*     n[]={   &m_nodes[node0],
133                 &m_nodes[node1],
134                 &m_nodes[node2]};
135         for(int i=0,ni=m_faces.size();i<ni;++i)
136         {
137                 const Face&     f=m_faces[i];
138                 int                     c=0;
139                 for(int j=0;j<3;++j)
140                 {
141                         if(     (f.m_n[j]==n[0])||
142                                 (f.m_n[j]==n[1])||
143                                 (f.m_n[j]==n[2])) c|=1<<j; else break;
144                 }
145                 if(c==7) return(true);
146         }
147         return(false);
148 }
149
150 //
151 btSoftBody::Material*           btSoftBody::appendMaterial()
152 {
153         Material*       pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
154         if(m_materials.size()>0)
155                 *pm=*m_materials[0];
156         else
157                 ZeroInitialize(*pm);
158         m_materials.push_back(pm);
159         return(pm);
160 }
161
162 //
163 void                    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);
185 }
186
187 //
188 void                    btSoftBody::appendNote( const char* text,
189                                                                            const btVector3& o,
190                                                                            Node* feature)
191 {
192         appendNote(text,o,btVector4(1,0,0,0),feature);
193 }
194
195 //
196 void                    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
205 //
206 void                    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]);
214 }
215
216 //
217 void                    btSoftBody::appendNode( const btVector3& x,btScalar m)
218 {
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);
234 }
235
236 //
237 void                    btSoftBody::appendLink(int model,Material* mat)
238 {
239         Link    l;
240         if(model>=0)
241                 l=m_links[model];
242         else
243         { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
244         m_links.push_back(l);
245 }
246
247 //
248 void                    btSoftBody::appendLink( int node0,
249                                                                            int node1,
250                                                                            Material* mat,
251                                                                            bool bcheckexist)
252 {
253         appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
254 }
255
256 //
257 void                    btSoftBody::appendLink( Node* node0,
258                                                                            Node* node1,
259                                                                            Material* mat,
260                                                                            bool bcheckexist)
261 {
262         if((!bcheckexist)||(!checkLink(node0,node1)))
263         {
264                 appendLink(-1,mat);
265                 Link&   l=m_links[m_links.size()-1];
266                 l.m_n[0]                =       node0;
267                 l.m_n[1]                =       node1;
268                 l.m_rl                  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
269                 m_bUpdateRtCst=true;
270         }
271 }
272
273 //
274 void                    btSoftBody::appendFace(int model,Material* mat)
275 {
276         Face    f;
277         if(model>=0)
278         { f=m_faces[model]; }
279         else
280         { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
281         m_faces.push_back(f);
282 }
283
284 //
285 void                    btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
286 {
287         if (node0==node1)
288                 return;
289         if (node1==node2)
290                 return;
291         if (node2==node0)
292                 return;
293
294         appendFace(-1,mat);
295         Face&   f=m_faces[m_faces.size()-1];
296         btAssert(node0!=node1);
297         btAssert(node1!=node2);
298         btAssert(node2!=node0);
299         f.m_n[0]        =       &m_nodes[node0];
300         f.m_n[1]        =       &m_nodes[node1];
301         f.m_n[2]        =       &m_nodes[node2];
302         f.m_ra          =       AreaOf( f.m_n[0]->m_x,
303                 f.m_n[1]->m_x,
304                 f.m_n[2]->m_x); 
305         m_bUpdateRtCst=true;
306 }
307
308 //
309 void                    btSoftBody::appendTetra(int model,Material* mat)
310 {
311 Tetra   t;
312 if(model>=0)
313         t=m_tetras[model];
314         else
315         { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
316 m_tetras.push_back(t);
317 }
318
319 //
320 void                    btSoftBody::appendTetra(int node0,
321                                                                                 int node1,
322                                                                                 int node2,
323                                                                                 int node3,
324                                                                                 Material* mat)
325 {
326         appendTetra(-1,mat);
327         Tetra&  t=m_tetras[m_tetras.size()-1];
328         t.m_n[0]        =       &m_nodes[node0];
329         t.m_n[1]        =       &m_nodes[node1];
330         t.m_n[2]        =       &m_nodes[node2];
331         t.m_n[3]        =       &m_nodes[node3];
332         t.m_rv          =       VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
333         m_bUpdateRtCst=true;
334 }
335
336 //
337 void                    btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies)
338 {
339         if (disableCollisionBetweenLinkedBodies)
340         {
341                 if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
342                 {
343                         m_collisionDisabledObjects.push_back(body);
344                 }
345         }
346
347         Anchor  a;
348         a.m_node                        =       &m_nodes[node];
349         a.m_body                        =       body;
350         a.m_local                       =       body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
351         a.m_node->m_battach     =       1;
352         m_anchors.push_back(a);
353 }
354
355 //
356 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
357 {
358         LJoint*         pj      =       new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
359         pj->m_bodies[0] =       body0;
360         pj->m_bodies[1] =       body1;
361         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse()*specs.position;
362         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse()*specs.position;
363         pj->m_cfm               =       specs.cfm;
364         pj->m_erp               =       specs.erp;
365         pj->m_split             =       specs.split;
366         m_joints.push_back(pj);
367 }
368
369 //
370 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
371 {
372         appendLinearJoint(specs,m_clusters[0],body);
373 }
374
375 //
376 void                    btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
377 {
378         appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
379 }
380
381 //
382 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
383 {
384         AJoint*         pj      =       new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
385         pj->m_bodies[0] =       body0;
386         pj->m_bodies[1] =       body1;
387         pj->m_refs[0]   =       pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
388         pj->m_refs[1]   =       pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
389         pj->m_cfm               =       specs.cfm;
390         pj->m_erp               =       specs.erp;
391         pj->m_split             =       specs.split;
392         pj->m_icontrol  =       specs.icontrol;
393         m_joints.push_back(pj);
394 }
395
396 //
397 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
398 {
399         appendAngularJoint(specs,m_clusters[0],body);
400 }
401
402 //
403 void                    btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
404 {
405         appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
406 }
407
408 //
409 void                    btSoftBody::addForce(const btVector3& force)
410 {
411         for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
412 }
413
414 //
415 void                    btSoftBody::addForce(const btVector3& force,int node)
416 {
417         Node&   n=m_nodes[node];
418         if(n.m_im>0)
419         {
420                 n.m_f   +=      force;
421         }
422 }
423
424 //
425 void                    btSoftBody::addVelocity(const btVector3& velocity)
426 {
427         for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
428 }
429
430 /* Set velocity for the entire body                                                                             */ 
431 void                            btSoftBody::setVelocity(        const btVector3& velocity)
432 {
433         for(int i=0,ni=m_nodes.size();i<ni;++i) 
434         {
435                 Node&   n=m_nodes[i];
436                 if(n.m_im>0)
437                 {
438                         n.m_v   =       velocity;
439                 }
440         }
441 }
442
443
444 //
445 void                    btSoftBody::addVelocity(const btVector3& velocity,int node)
446 {
447         Node&   n=m_nodes[node];
448         if(n.m_im>0)
449         {
450                 n.m_v   +=      velocity;
451         }
452 }
453
454 //
455 void                    btSoftBody::setMass(int node,btScalar mass)
456 {
457         m_nodes[node].m_im=mass>0?1/mass:0;
458         m_bUpdateRtCst=true;
459 }
460
461 //
462 btScalar                btSoftBody::getMass(int node) const
463 {
464         return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
465 }
466
467 //
468 btScalar                btSoftBody::getTotalMass() const
469 {
470         btScalar        mass=0;
471         for(int i=0;i<m_nodes.size();++i)
472         {
473                 mass+=getMass(i);
474         }
475         return(mass);
476 }
477
478 //
479 void                    btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
480 {
481         int i;
482
483         if(fromfaces)
484         {
485
486                 for(i=0;i<m_nodes.size();++i)
487                 {
488                         m_nodes[i].m_im=0;
489                 }
490                 for(i=0;i<m_faces.size();++i)
491                 {
492                         const Face&             f=m_faces[i];
493                         const btScalar  twicearea=AreaOf(       f.m_n[0]->m_x,
494                                 f.m_n[1]->m_x,
495                                 f.m_n[2]->m_x);
496                         for(int j=0;j<3;++j)
497                         {
498                                 f.m_n[j]->m_im+=twicearea;
499                         }
500                 }
501                 for( i=0;i<m_nodes.size();++i)
502                 {
503                         m_nodes[i].m_im=1/m_nodes[i].m_im;
504                 }
505         }
506         const btScalar  tm=getTotalMass();
507         const btScalar  itm=1/tm;
508         for( i=0;i<m_nodes.size();++i)
509         {
510                 m_nodes[i].m_im/=itm*mass;
511         }
512         m_bUpdateRtCst=true;
513 }
514
515 //
516 void                    btSoftBody::setTotalDensity(btScalar density)
517 {
518         setTotalMass(getVolume()*density,true);
519 }
520
521 //
522 void                    btSoftBody::setVolumeMass(btScalar mass)
523 {
524 btAlignedObjectArray<btScalar>  ranks;
525 ranks.resize(m_nodes.size(),0);
526 int i;
527
528 for(i=0;i<m_nodes.size();++i)
529         {
530         m_nodes[i].m_im=0;
531         }
532 for(i=0;i<m_tetras.size();++i)
533         {
534         const Tetra& t=m_tetras[i];
535         for(int j=0;j<4;++j)
536                 {
537                 t.m_n[j]->m_im+=btFabs(t.m_rv);
538                 ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
539                 }
540         }
541 for( i=0;i<m_nodes.size();++i)
542         {
543         if(m_nodes[i].m_im>0)
544                 {
545                 m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
546                 }
547         }
548 setTotalMass(mass,false);
549 }
550
551 //
552 void                    btSoftBody::setVolumeDensity(btScalar density)
553 {
554 btScalar        volume=0;
555 for(int i=0;i<m_tetras.size();++i)
556         {
557         const Tetra& t=m_tetras[i];
558         for(int j=0;j<4;++j)
559                 {
560                 volume+=btFabs(t.m_rv);
561                 }
562         }
563 setVolumeMass(volume*density/6);
564 }
565
566 //
567 void                    btSoftBody::transform(const btTransform& trs)
568 {
569         const btScalar  margin=getCollisionShape()->getMargin();
570         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
571         
572         for(int i=0,ni=m_nodes.size();i<ni;++i)
573         {
574                 Node&   n=m_nodes[i];
575                 n.m_x=trs*n.m_x;
576                 n.m_q=trs*n.m_q;
577                 n.m_n=trs.getBasis()*n.m_n;
578                 vol = btDbvtVolume::FromCR(n.m_x,margin);
579                 
580                 m_ndbvt.update(n.m_leaf,vol);
581         }
582         updateNormals();
583         updateBounds();
584         updateConstants();
585         m_initialWorldTransform = trs;
586 }
587
588 //
589 void                    btSoftBody::translate(const btVector3& trs)
590 {
591         btTransform     t;
592         t.setIdentity();
593         t.setOrigin(trs);
594         transform(t);
595 }
596
597 //
598 void                    btSoftBody::rotate(     const btQuaternion& rot)
599 {
600         btTransform     t;
601         t.setIdentity();
602         t.setRotation(rot);
603         transform(t);
604 }
605
606 //
607 void                    btSoftBody::scale(const btVector3& scl)
608 {
609         const btScalar  margin=getCollisionShape()->getMargin();
610         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
611         
612         for(int i=0,ni=m_nodes.size();i<ni;++i)
613         {
614                 Node&   n=m_nodes[i];
615                 n.m_x*=scl;
616                 n.m_q*=scl;
617                 vol = btDbvtVolume::FromCR(n.m_x,margin);
618                 m_ndbvt.update(n.m_leaf,vol);
619         }
620         updateNormals();
621         updateBounds();
622         updateConstants();
623 }
624
625 //
626 void                    btSoftBody::setPose(bool bvolume,bool bframe)
627 {
628         m_pose.m_bvolume        =       bvolume;
629         m_pose.m_bframe         =       bframe;
630         int i,ni;
631
632         /* Weights              */ 
633         const btScalar  omass=getTotalMass();
634         const btScalar  kmass=omass*m_nodes.size()*1000;
635         btScalar                tmass=omass;
636         m_pose.m_wgh.resize(m_nodes.size());
637         for(i=0,ni=m_nodes.size();i<ni;++i)
638         {
639                 if(m_nodes[i].m_im<=0) tmass+=kmass;
640         }
641         for( i=0,ni=m_nodes.size();i<ni;++i)
642         {
643                 Node&   n=m_nodes[i];
644                 m_pose.m_wgh[i]=        n.m_im>0                                        ?
645                         1/(m_nodes[i].m_im*tmass)       :
646                 kmass/tmass;
647         }
648         /* Pos          */ 
649         const btVector3 com=evaluateCom();
650         m_pose.m_pos.resize(m_nodes.size());
651         for( i=0,ni=m_nodes.size();i<ni;++i)
652         {
653                 m_pose.m_pos[i]=m_nodes[i].m_x-com;
654         }
655         m_pose.m_volume =       bvolume?getVolume():0;
656         m_pose.m_com    =       com;
657         m_pose.m_rot.setIdentity();
658         m_pose.m_scl.setIdentity();
659         /* Aqq          */ 
660         m_pose.m_aqq[0] =
661                 m_pose.m_aqq[1] =
662                 m_pose.m_aqq[2] =       btVector3(0,0,0);
663         for( i=0,ni=m_nodes.size();i<ni;++i)
664         {
665                 const btVector3&        q=m_pose.m_pos[i];
666                 const btVector3         mq=m_pose.m_wgh[i]*q;
667                 m_pose.m_aqq[0]+=mq.x()*q;
668                 m_pose.m_aqq[1]+=mq.y()*q;
669                 m_pose.m_aqq[2]+=mq.z()*q;
670         }
671         m_pose.m_aqq=m_pose.m_aqq.inverse();
672         updateConstants();
673 }
674
675 //
676 btScalar                btSoftBody::getVolume() const
677 {
678         btScalar        vol=0;
679         if(m_nodes.size()>0)
680         {
681                 int i,ni;
682
683                 const btVector3 org=m_nodes[0].m_x;
684                 for(i=0,ni=m_faces.size();i<ni;++i)
685                 {
686                         const Face&     f=m_faces[i];
687                         vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
688                 }
689                 vol/=(btScalar)6;
690         }
691         return(vol);
692 }
693
694 //
695 int                             btSoftBody::clusterCount() const
696 {
697         return(m_clusters.size());
698 }
699
700 //
701 btVector3               btSoftBody::clusterCom(const Cluster* cluster)
702 {
703         btVector3               com(0,0,0);
704         for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
705         {
706                 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
707         }
708         return(com*cluster->m_imass);
709 }
710
711 //
712 btVector3               btSoftBody::clusterCom(int cluster) const
713 {
714         return(clusterCom(m_clusters[cluster]));
715 }
716
717 //
718 btVector3               btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
719 {
720         return(cluster->m_lv+btCross(cluster->m_av,rpos));
721 }
722
723 //
724 void                    btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
725 {
726         const btVector3 li=cluster->m_imass*impulse;
727         const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
728         cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
729         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
730         cluster->m_nvimpulses++;
731 }
732
733 //
734 void                    btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
735 {
736         const btVector3 li=cluster->m_imass*impulse;
737         const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
738         cluster->m_dimpulses[0]+=li;
739         cluster->m_dimpulses[1]+=ai;
740         cluster->m_ndimpulses++;
741 }
742
743 //
744 void                    btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
745 {
746         if(impulse.m_asVelocity)        clusterVImpulse(cluster,rpos,impulse.m_velocity);
747         if(impulse.m_asDrift)           clusterDImpulse(cluster,rpos,impulse.m_drift);
748 }
749
750 //
751 void                    btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
752 {
753         const btVector3 ai=cluster->m_invwi*impulse;
754         cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
755         cluster->m_nvimpulses++;
756 }
757
758 //
759 void                    btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
760 {
761         const btVector3 ai=cluster->m_invwi*impulse;
762         cluster->m_dimpulses[1]+=ai;
763         cluster->m_ndimpulses++;
764 }
765
766 //
767 void                    btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
768 {
769         if(impulse.m_asVelocity)        clusterVAImpulse(cluster,impulse.m_velocity);
770         if(impulse.m_asDrift)           clusterDAImpulse(cluster,impulse.m_drift);
771 }
772
773 //
774 void                    btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
775 {
776         cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
777         cluster->m_ndimpulses++;
778 }
779
780 struct NodeLinks
781 {
782     btAlignedObjectArray<int> m_links;
783 };
784
785
786
787 //
788 int                             btSoftBody::generateBendingConstraints(int distance,Material* mat)
789 {
790         int i,j;
791
792         if(distance>1)
793         {
794                 /* Build graph  */ 
795                 const int               n=m_nodes.size();
796                 const unsigned  inf=(~(unsigned)0)>>1;
797                 unsigned*               adj=new unsigned[n*n];
798                 
799
800 #define IDX(_x_,_y_)    ((_y_)*n+(_x_))
801                 for(j=0;j<n;++j)
802                 {
803                         for(i=0;i<n;++i)
804                         {
805                                 if(i!=j)
806                                 {
807                                         adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
808                                 }
809                                 else
810                                 {
811                                         adj[IDX(i,j)]=adj[IDX(j,i)]=0;
812                                 }
813                         }
814                 }
815                 for( i=0;i<m_links.size();++i)
816                 {
817                         const int       ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
818                         const int       ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
819                         adj[IDX(ia,ib)]=1;
820                         adj[IDX(ib,ia)]=1;
821                 }
822
823
824                 //special optimized case for distance == 2
825                 if (distance == 2)
826                 {
827
828                         btAlignedObjectArray<NodeLinks> nodeLinks;
829
830
831                         /* Build node links */
832                         nodeLinks.resize(m_nodes.size());
833
834                         for( i=0;i<m_links.size();++i)
835                         {
836                                 const int       ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
837                                 const int       ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
838                                 if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
839                                         nodeLinks[ia].m_links.push_back(ib);
840
841                                 if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
842                                         nodeLinks[ib].m_links.push_back(ia);
843                         }
844                         for (int ii=0;ii<nodeLinks.size();ii++)
845                         {
846                                 int i=ii;
847
848                                 for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
849                                 {
850                                         int k = nodeLinks[ii].m_links[jj];
851                                         for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
852                                         {
853                                                 int j = nodeLinks[k].m_links[kk];
854                                                 if (i!=j)
855                                                 {
856                                                         const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
857                                                         btAssert(sum==2);
858                                                         if(adj[IDX(i,j)]>sum)
859                                                         {
860                                                                 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
861                                                         }
862                                                 }
863
864                                         }
865                                 }
866                         }
867                 } else
868                 {
869                         ///generic Floyd's algorithm
870                         for(int k=0;k<n;++k)
871                         {
872                                 for(j=0;j<n;++j)
873                                 {
874                                         for(i=j+1;i<n;++i)
875                                         {
876                                                 const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
877                                                 if(adj[IDX(i,j)]>sum)
878                                                 {
879                                                         adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
880                                                 }
881                                         }
882                                 }
883                         }
884                 }
885
886
887                 /* Build links  */ 
888                 int     nlinks=0;
889                 for(j=0;j<n;++j)
890                 {
891                         for(i=j+1;i<n;++i)
892                         {
893                                 if(adj[IDX(i,j)]==(unsigned)distance)
894                                 {
895                                         appendLink(i,j,mat);
896                                         m_links[m_links.size()-1].m_bbending=1;
897                                         ++nlinks;
898                                 }
899                         }
900                 }
901                 delete[] adj;           
902                 return(nlinks);
903         }
904         return(0);
905 }
906
907 //
908 void                    btSoftBody::randomizeConstraints()
909 {
910         unsigned long   seed=243703;
911 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
912         int i,ni;
913
914         for(i=0,ni=m_links.size();i<ni;++i)
915         {
916                 btSwap(m_links[i],m_links[NEXTRAND%ni]);
917         }
918         for(i=0,ni=m_faces.size();i<ni;++i)
919         {
920                 btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
921         }
922 #undef NEXTRAND
923 }
924
925 //
926 void                    btSoftBody::releaseCluster(int index)
927 {
928         Cluster*        c=m_clusters[index];
929         if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
930         c->~Cluster();
931         btAlignedFree(c);
932         m_clusters.remove(c);
933 }
934
935 //
936 void                    btSoftBody::releaseClusters()
937 {
938         while(m_clusters.size()>0) releaseCluster(0);
939 }
940
941 //
942 int                             btSoftBody::generateClusters(int k,int maxiterations)
943 {
944         int i;
945         releaseClusters();
946         m_clusters.resize(btMin(k,m_nodes.size()));
947         for(i=0;i<m_clusters.size();++i)
948         {
949                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
950                 m_clusters[i]->m_collide=       true;
951         }
952         k=m_clusters.size();
953         if(k>0)
954         {
955                 /* Initialize           */ 
956                 btAlignedObjectArray<btVector3> centers;
957                 btVector3                                               cog(0,0,0);
958                 int                                                             i;
959                 for(i=0;i<m_nodes.size();++i)
960                 {
961                         cog+=m_nodes[i].m_x;
962                         m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
963                 }
964                 cog/=(btScalar)m_nodes.size();
965                 centers.resize(k,cog);
966                 /* Iterate                      */ 
967                 const btScalar  slope=16;
968                 bool                    changed;
969                 int                             iterations=0;
970                 do      {
971                         const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
972                         changed=false;
973                         iterations++;   
974                         int i;
975
976                         for(i=0;i<k;++i)
977                         {
978                                 btVector3       c(0,0,0);
979                                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
980                                 {
981                                         c+=m_clusters[i]->m_nodes[j]->m_x;
982                                 }
983                                 if(m_clusters[i]->m_nodes.size())
984                                 {
985                                         c                       /=      (btScalar)m_clusters[i]->m_nodes.size();
986                                         c                       =       centers[i]+(c-centers[i])*w;
987                                         changed         |=      ((c-centers[i]).length2()>SIMD_EPSILON);
988                                         centers[i]      =       c;
989                                         m_clusters[i]->m_nodes.resize(0);
990                                 }                       
991                         }
992                         for(i=0;i<m_nodes.size();++i)
993                         {
994                                 const btVector3 nx=m_nodes[i].m_x;
995                                 int                             kbest=0;
996                                 btScalar                kdist=ClusterMetric(centers[0],nx);
997                                 for(int j=1;j<k;++j)
998                                 {
999                                         const btScalar  d=ClusterMetric(centers[j],nx);
1000                                         if(d<kdist)
1001                                         {
1002                                                 kbest=j;
1003                                                 kdist=d;
1004                                         }
1005                                 }
1006                                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1007                         }               
1008                 } while(changed&&(iterations<maxiterations));
1009                 /* Merge                */ 
1010                 btAlignedObjectArray<int>       cids;
1011                 cids.resize(m_nodes.size(),-1);
1012                 for(i=0;i<m_clusters.size();++i)
1013                 {
1014                         for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1015                         {
1016                                 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
1017                         }
1018                 }
1019                 for(i=0;i<m_faces.size();++i)
1020                 {
1021                         const int idx[]={       int(m_faces[i].m_n[0]-&m_nodes[0]),
1022                                 int(m_faces[i].m_n[1]-&m_nodes[0]),
1023                                 int(m_faces[i].m_n[2]-&m_nodes[0])};
1024                         for(int j=0;j<3;++j)
1025                         {
1026                                 const int cid=cids[idx[j]];
1027                                 for(int q=1;q<3;++q)
1028                                 {
1029                                         const int kid=idx[(j+q)%3];
1030                                         if(cids[kid]!=cid)
1031                                         {
1032                                                 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
1033                                                 {
1034                                                         m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1035                                                 }
1036                                         }
1037                                 }
1038                         }
1039                 }
1040                 /* Master               */ 
1041                 if(m_clusters.size()>1)
1042                 {
1043                         Cluster*        pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1044                         pmaster->m_collide      =       false;
1045                         pmaster->m_nodes.reserve(m_nodes.size());
1046                         for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1047                         m_clusters.push_back(pmaster);
1048                         btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
1049                 }
1050                 /* Terminate    */ 
1051                 for(i=0;i<m_clusters.size();++i)
1052                 {
1053                         if(m_clusters[i]->m_nodes.size()==0)
1054                         {
1055                                 releaseCluster(i--);
1056                         }
1057                 }
1058         } else
1059         {
1060                 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1061                 if (m_tetras.size())
1062                 {
1063                         m_clusters.resize(m_tetras.size());
1064                         for(i=0;i<m_clusters.size();++i)
1065                         {
1066                                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1067                                 m_clusters[i]->m_collide=       true;
1068                         }
1069                         for (i=0;i<m_tetras.size();i++)
1070                         {
1071                                 for (int j=0;j<4;j++)
1072                                 {
1073                                         m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1074                                 }
1075                         }
1076
1077                 } else
1078                 {
1079                         m_clusters.resize(m_faces.size());
1080                         for(i=0;i<m_clusters.size();++i)
1081                         {
1082                                 m_clusters[i]                   =       new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1083                                 m_clusters[i]->m_collide=       true;
1084                         }
1085
1086                         for(i=0;i<m_faces.size();++i)
1087                         {
1088                                 for(int j=0;j<3;++j)
1089                                 {
1090                                         m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1091                                 }
1092                         }
1093                 }
1094         }
1095
1096         if (m_clusters.size())
1097         {
1098                 initializeClusters();
1099                 updateClusters();
1100
1101
1102                 //for self-collision
1103                 m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
1104                 {
1105                         for (int c0=0;c0<m_clusters.size();c0++)
1106                         {
1107                                 m_clusters[c0]->m_clusterIndex=c0;
1108                                 for (int c1=0;c1<m_clusters.size();c1++)
1109                                 {
1110                                         
1111                                         bool connected=false;
1112                                         Cluster* cla = m_clusters[c0];
1113                                         Cluster* clb = m_clusters[c1];
1114                                         for (int i=0;!connected&&i<cla->m_nodes.size();i++)
1115                                         {
1116                                                 for (int j=0;j<clb->m_nodes.size();j++)
1117                                                 {
1118                                                         if (cla->m_nodes[i] == clb->m_nodes[j])
1119                                                         {
1120                                                                 connected=true;
1121                                                                 break;
1122                                                         }
1123                                                 }
1124                                         }
1125                                         m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
1126                                 }
1127                         }
1128                 }
1129         }
1130
1131         return(m_clusters.size());
1132 }
1133
1134 //
1135 void                    btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
1136 {
1137         const Node*                     nbase = &m_nodes[0];
1138         int                                     ncount = m_nodes.size();
1139         btSymMatrix<int>        edges(ncount,-2);
1140         int                                     newnodes=0;
1141         int i,j,k,ni;
1142
1143         /* Filter out           */ 
1144         for(i=0;i<m_links.size();++i)
1145         {
1146                 Link&   l=m_links[i];
1147                 if(l.m_bbending)
1148                 {
1149                         if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
1150                         {
1151                                 btSwap(m_links[i],m_links[m_links.size()-1]);
1152                                 m_links.pop_back();--i;
1153                         }
1154                 }       
1155         }
1156         /* Fill edges           */ 
1157         for(i=0;i<m_links.size();++i)
1158         {
1159                 Link&   l=m_links[i];
1160                 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
1161         }
1162         for(i=0;i<m_faces.size();++i)
1163         {       
1164                 Face&   f=m_faces[i];
1165                 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
1166                 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
1167                 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
1168         }
1169         /* Intersect            */ 
1170         for(i=0;i<ncount;++i)
1171         {
1172                 for(j=i+1;j<ncount;++j)
1173                 {
1174                         if(edges(i,j)==-1)
1175                         {
1176                                 Node&                   a=m_nodes[i];
1177                                 Node&                   b=m_nodes[j];
1178                                 const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
1179                                 if(t>0)
1180                                 {
1181                                         const btVector3 x=Lerp(a.m_x,b.m_x,t);
1182                                         const btVector3 v=Lerp(a.m_v,b.m_v,t);
1183                                         btScalar                m=0;
1184                                         if(a.m_im>0)
1185                                         {
1186                                                 if(b.m_im>0)
1187                                                 {
1188                                                         const btScalar  ma=1/a.m_im;
1189                                                         const btScalar  mb=1/b.m_im;
1190                                                         const btScalar  mc=Lerp(ma,mb,t);
1191                                                         const btScalar  f=(ma+mb)/(ma+mb+mc);
1192                                                         a.m_im=1/(ma*f);
1193                                                         b.m_im=1/(mb*f);
1194                                                         m=mc*f;
1195                                                 }
1196                                                 else
1197                                                 { a.m_im/=0.5;m=1/a.m_im; }
1198                                         }
1199                                         else
1200                                         {
1201                                                 if(b.m_im>0)
1202                                                 { b.m_im/=0.5;m=1/b.m_im; }
1203                                                 else
1204                                                         m=0;
1205                                         }
1206                                         appendNode(x,m);
1207                                         edges(i,j)=m_nodes.size()-1;
1208                                         m_nodes[edges(i,j)].m_v=v;
1209                                         ++newnodes;
1210                                 }
1211                         }
1212                 }
1213         }
1214         nbase=&m_nodes[0];
1215         /* Refine links         */ 
1216         for(i=0,ni=m_links.size();i<ni;++i)
1217         {
1218                 Link&           feat=m_links[i];
1219                 const int       idx[]={ int(feat.m_n[0]-nbase),
1220                         int(feat.m_n[1]-nbase)};
1221                 if((idx[0]<ncount)&&(idx[1]<ncount))
1222                 {
1223                         const int ni=edges(idx[0],idx[1]);
1224                         if(ni>0)
1225                         {
1226                                 appendLink(i);
1227                                 Link*           pft[]={ &m_links[i],
1228                                         &m_links[m_links.size()-1]};                    
1229                                 pft[0]->m_n[0]=&m_nodes[idx[0]];
1230                                 pft[0]->m_n[1]=&m_nodes[ni];
1231                                 pft[1]->m_n[0]=&m_nodes[ni];
1232                                 pft[1]->m_n[1]=&m_nodes[idx[1]];
1233                         }
1234                 }
1235         }
1236         /* Refine faces         */ 
1237         for(i=0;i<m_faces.size();++i)
1238         {
1239                 const Face&     feat=m_faces[i];
1240                 const int       idx[]={ int(feat.m_n[0]-nbase),
1241                         int(feat.m_n[1]-nbase),
1242                         int(feat.m_n[2]-nbase)};
1243                 for(j=2,k=0;k<3;j=k++)
1244                 {
1245                         if((idx[j]<ncount)&&(idx[k]<ncount))
1246                         {
1247                                 const int ni=edges(idx[j],idx[k]);
1248                                 if(ni>0)
1249                                 {
1250                                         appendFace(i);
1251                                         const int       l=(k+1)%3;
1252                                         Face*           pft[]={ &m_faces[i],
1253                                                 &m_faces[m_faces.size()-1]};
1254                                         pft[0]->m_n[0]=&m_nodes[idx[l]];
1255                                         pft[0]->m_n[1]=&m_nodes[idx[j]];
1256                                         pft[0]->m_n[2]=&m_nodes[ni];
1257                                         pft[1]->m_n[0]=&m_nodes[ni];
1258                                         pft[1]->m_n[1]=&m_nodes[idx[k]];
1259                                         pft[1]->m_n[2]=&m_nodes[idx[l]];
1260                                         appendLink(ni,idx[l],pft[0]->m_material);
1261                                         --i;break;
1262                                 }
1263                         }
1264                 }
1265         }
1266         /* Cut                          */ 
1267         if(cut)
1268         {       
1269                 btAlignedObjectArray<int>       cnodes;
1270                 const int                                       pcount=ncount;
1271                 int                                                     i;
1272                 ncount=m_nodes.size();
1273                 cnodes.resize(ncount,0);
1274                 /* Nodes                */ 
1275                 for(i=0;i<ncount;++i)
1276                 {
1277                         const btVector3 x=m_nodes[i].m_x;
1278                         if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
1279                         {
1280                                 const btVector3 v=m_nodes[i].m_v;
1281                                 btScalar                m=getMass(i);
1282                                 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
1283                                 appendNode(x,m);
1284                                 cnodes[i]=m_nodes.size()-1;
1285                                 m_nodes[cnodes[i]].m_v=v;
1286                         }
1287                 }
1288                 nbase=&m_nodes[0];
1289                 /* Links                */ 
1290                 for(i=0,ni=m_links.size();i<ni;++i)
1291                 {
1292                         const int               id[]={  int(m_links[i].m_n[0]-nbase),
1293                                 int(m_links[i].m_n[1]-nbase)};
1294                         int                             todetach=0;
1295                         if(cnodes[id[0]]&&cnodes[id[1]])
1296                         {
1297                                 appendLink(i);
1298                                 todetach=m_links.size()-1;
1299                         }
1300                         else
1301                         {
1302                                 if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
1303                                         (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
1304                                         todetach=i;
1305                         }
1306                         if(todetach)
1307                         {
1308                                 Link&   l=m_links[todetach];
1309                                 for(int j=0;j<2;++j)
1310                                 {
1311                                         int cn=cnodes[int(l.m_n[j]-nbase)];
1312                                         if(cn) l.m_n[j]=&m_nodes[cn];
1313                                 }                       
1314                         }
1315                 }
1316                 /* Faces                */ 
1317                 for(i=0,ni=m_faces.size();i<ni;++i)
1318                 {
1319                         Node**                  n=      m_faces[i].m_n;
1320                         if(     (ifn->Eval(n[0]->m_x)<accurary)&&
1321                                 (ifn->Eval(n[1]->m_x)<accurary)&&
1322                                 (ifn->Eval(n[2]->m_x)<accurary))
1323                         {
1324                                 for(int j=0;j<3;++j)
1325                                 {
1326                                         int cn=cnodes[int(n[j]-nbase)];
1327                                         if(cn) n[j]=&m_nodes[cn];
1328                                 }
1329                         }
1330                 }
1331                 /* Clean orphans        */ 
1332                 int                                                     nnodes=m_nodes.size();
1333                 btAlignedObjectArray<int>       ranks;
1334                 btAlignedObjectArray<int>       todelete;
1335                 ranks.resize(nnodes,0);
1336                 for(i=0,ni=m_links.size();i<ni;++i)
1337                 {
1338                         for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
1339                 }
1340                 for(i=0,ni=m_faces.size();i<ni;++i)
1341                 {
1342                         for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
1343                 }
1344                 for(i=0;i<m_links.size();++i)
1345                 {
1346                         const int       id[]={  int(m_links[i].m_n[0]-nbase),
1347                                 int(m_links[i].m_n[1]-nbase)};
1348                         const bool      sg[]={  ranks[id[0]]==1,
1349                                 ranks[id[1]]==1};
1350                         if(sg[0]||sg[1])
1351                         {
1352                                 --ranks[id[0]];
1353                                 --ranks[id[1]];
1354                                 btSwap(m_links[i],m_links[m_links.size()-1]);
1355                                 m_links.pop_back();--i;
1356                         }
1357                 }
1358 #if 0   
1359                 for(i=nnodes-1;i>=0;--i)
1360                 {
1361                         if(!ranks[i]) todelete.push_back(i);
1362                 }       
1363                 if(todelete.size())
1364                 {               
1365                         btAlignedObjectArray<int>&      map=ranks;
1366                         for(int i=0;i<nnodes;++i) map[i]=i;
1367                         PointersToIndices(this);
1368                         for(int i=0,ni=todelete.size();i<ni;++i)
1369                         {
1370                                 int             j=todelete[i];
1371                                 int&    a=map[j];
1372                                 int&    b=map[--nnodes];
1373                                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1374                                 btSwap(m_nodes[a],m_nodes[b]);
1375                                 j=a;a=b;b=j;                    
1376                         }
1377                         IndicesToPointers(this,&map[0]);
1378                         m_nodes.resize(nnodes);
1379                 }
1380 #endif
1381         }
1382         m_bUpdateRtCst=true;
1383 }
1384
1385 //
1386 bool                    btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
1387 {
1388         return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
1389 }
1390
1391 //
1392 bool                    btSoftBody::cutLink(int node0,int node1,btScalar position)
1393 {
1394         bool                    done=false;
1395         int i,ni;
1396         const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1397         const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
1398         const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
1399         const btScalar  m=1;
1400         appendNode(x,m);
1401         appendNode(x,m);
1402         Node*                   pa=&m_nodes[node0];
1403         Node*                   pb=&m_nodes[node1];
1404         Node*                   pn[2]={ &m_nodes[m_nodes.size()-2],
1405                 &m_nodes[m_nodes.size()-1]};
1406         pn[0]->m_v=v;
1407         pn[1]->m_v=v;
1408         for(i=0,ni=m_links.size();i<ni;++i)
1409         {
1410                 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
1411                 if(mtch!=-1)
1412                 {
1413                         appendLink(i);
1414                         Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
1415                         pft[0]->m_n[1]=pn[mtch];
1416                         pft[1]->m_n[0]=pn[1-mtch];
1417                         done=true;
1418                 }
1419         }
1420         for(i=0,ni=m_faces.size();i<ni;++i)
1421         {
1422                 for(int k=2,l=0;l<3;k=l++)
1423                 {
1424                         const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
1425                         if(mtch!=-1)
1426                         {
1427                                 appendFace(i);
1428                                 Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
1429                                 pft[0]->m_n[l]=pn[mtch];
1430                                 pft[1]->m_n[k]=pn[1-mtch];
1431                                 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1432                                 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1433                         }
1434                 }
1435         }
1436         if(!done)
1437         {
1438                 m_ndbvt.remove(pn[0]->m_leaf);
1439                 m_ndbvt.remove(pn[1]->m_leaf);
1440                 m_nodes.pop_back();
1441                 m_nodes.pop_back();
1442         }
1443         return(done);
1444 }
1445
1446 //
1447 bool                    btSoftBody::rayTest(const btVector3& rayFrom,
1448                                                                         const btVector3& rayTo,
1449                                                                         sRayCast& results)
1450 {
1451         if(m_faces.size()&&m_fdbvt.empty()) 
1452                 initializeFaceTree();
1453
1454         results.body    =       this;
1455         results.fraction = 1.f;
1456         results.feature =       eFeature::None;
1457         results.index   =       -1;
1458
1459         return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
1460 }
1461
1462 //
1463 void                    btSoftBody::setSolver(eSolverPresets::_ preset)
1464 {
1465         m_cfg.m_vsequence.clear();
1466         m_cfg.m_psequence.clear();
1467         m_cfg.m_dsequence.clear();
1468         switch(preset)
1469         {
1470         case    eSolverPresets::Positions:
1471                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1472                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1473                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1474                 m_cfg.m_psequence.push_back(ePSolver::Linear);  
1475                 break;  
1476         case    eSolverPresets::Velocities:
1477                 m_cfg.m_vsequence.push_back(eVSolver::Linear);
1478
1479                 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1480                 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1481                 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1482
1483                 m_cfg.m_dsequence.push_back(ePSolver::Linear);
1484                 break;
1485         }
1486 }
1487
1488 //
1489 void                    btSoftBody::predictMotion(btScalar dt)
1490 {
1491         int i,ni;
1492
1493         /* Update                               */ 
1494         if(m_bUpdateRtCst)
1495         {
1496                 m_bUpdateRtCst=false;
1497                 updateConstants();
1498                 m_fdbvt.clear();
1499                 if(m_cfg.collisions&fCollision::VF_SS)
1500                 {
1501                         initializeFaceTree();                   
1502                 }
1503         }
1504
1505         /* Prepare                              */ 
1506         m_sst.sdt               =       dt*m_cfg.timescale;
1507         m_sst.isdt              =       1/m_sst.sdt;
1508         m_sst.velmrg    =       m_sst.sdt*3;
1509         m_sst.radmrg    =       getCollisionShape()->getMargin();
1510         m_sst.updmrg    =       m_sst.radmrg*(btScalar)0.25;
1511         /* Forces                               */ 
1512         addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1513         applyForces();
1514         /* Integrate                    */ 
1515         for(i=0,ni=m_nodes.size();i<ni;++i)
1516         {
1517                 Node&   n=m_nodes[i];
1518                 n.m_q   =       n.m_x;
1519                 n.m_v   +=      n.m_f*n.m_im*m_sst.sdt;
1520                 n.m_x   +=      n.m_v*m_sst.sdt;
1521                 n.m_f   =       btVector3(0,0,0);
1522         }
1523         /* Clusters                             */ 
1524         updateClusters();
1525         /* Bounds                               */ 
1526         updateBounds(); 
1527         /* Nodes                                */ 
1528         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
1529         for(i=0,ni=m_nodes.size();i<ni;++i)
1530         {
1531                 Node&   n=m_nodes[i];
1532                 vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1533                 m_ndbvt.update( n.m_leaf,
1534                         vol,
1535                         n.m_v*m_sst.velmrg,
1536                         m_sst.updmrg);
1537         }
1538         /* Faces                                */ 
1539         if(!m_fdbvt.empty())
1540         {
1541                 for(int i=0;i<m_faces.size();++i)
1542                 {
1543                         Face&                   f=m_faces[i];
1544                         const btVector3 v=(     f.m_n[0]->m_v+
1545                                 f.m_n[1]->m_v+
1546                                 f.m_n[2]->m_v)/3;
1547                         vol = VolumeOf(f,m_sst.radmrg);
1548                         m_fdbvt.update( f.m_leaf,
1549                                 vol,
1550                                 v*m_sst.velmrg,
1551                                 m_sst.updmrg);
1552                 }
1553         }
1554         /* Pose                                 */ 
1555         updatePose();
1556         /* Match                                */ 
1557         if(m_pose.m_bframe&&(m_cfg.kMT>0))
1558         {
1559                 const btMatrix3x3       posetrs=m_pose.m_rot;
1560                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1561                 {
1562                         Node&   n=m_nodes[i];
1563                         if(n.m_im>0)
1564                         {
1565                                 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1566                                 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1567                         }
1568                 }
1569         }
1570         /* Clear contacts               */ 
1571         m_rcontacts.resize(0);
1572         m_scontacts.resize(0);
1573         /* Optimize dbvt's              */ 
1574         m_ndbvt.optimizeIncremental(1);
1575         m_fdbvt.optimizeIncremental(1);
1576         m_cdbvt.optimizeIncremental(1);
1577 }
1578
1579 //
1580 void                    btSoftBody::solveConstraints()
1581 {
1582         /* Apply clusters               */ 
1583         applyClusters(false);
1584         /* Prepare links                */ 
1585
1586         int i,ni;
1587
1588         for(i=0,ni=m_links.size();i<ni;++i)
1589         {
1590                 Link&   l=m_links[i];
1591                 l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
1592                 l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
1593         }
1594         /* Prepare anchors              */ 
1595         for(i=0,ni=m_anchors.size();i<ni;++i)
1596         {
1597                 Anchor&                 a=m_anchors[i];
1598                 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1599                 a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
1600                         a.m_node->m_im,
1601                         a.m_body->getInvMass(),
1602                         a.m_body->getInvInertiaTensorWorld(),
1603                         ra);
1604                 a.m_c1  =       ra;
1605                 a.m_c2  =       m_sst.sdt*a.m_node->m_im;
1606                 a.m_body->activate();
1607         }
1608         /* Solve velocities             */ 
1609         if(m_cfg.viterations>0)
1610         {
1611                 /* Solve                        */ 
1612                 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1613                 {
1614                         for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1615                         {
1616                                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
1617                         }
1618                 }
1619                 /* Update                       */ 
1620                 for(i=0,ni=m_nodes.size();i<ni;++i)
1621                 {
1622                         Node&   n=m_nodes[i];
1623                         n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
1624                 }
1625         }
1626         /* Solve positions              */ 
1627         if(m_cfg.piterations>0)
1628         {
1629                 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1630                 {
1631                         const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1632                         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1633                         {
1634                                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1635                         }
1636                 }
1637                 const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
1638                 for(i=0,ni=m_nodes.size();i<ni;++i)
1639                 {
1640                         Node&   n=m_nodes[i];
1641                         n.m_v   =       (n.m_x-n.m_q)*vc;
1642                         n.m_f   =       btVector3(0,0,0);               
1643                 }
1644         }
1645         /* Solve drift                  */ 
1646         if(m_cfg.diterations>0)
1647         {
1648                 const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
1649                 for(i=0,ni=m_nodes.size();i<ni;++i)
1650                 {
1651                         Node&   n=m_nodes[i];
1652                         n.m_q   =       n.m_x;
1653                 }
1654                 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1655                 {
1656                         for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1657                         {
1658                                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1659                         }
1660                 }
1661                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1662                 {
1663                         Node&   n=m_nodes[i];
1664                         n.m_v   +=      (n.m_x-n.m_q)*vcf;
1665                 }
1666         }
1667         /* Apply clusters               */ 
1668         dampClusters();
1669         applyClusters(true);
1670 }
1671
1672 //
1673 void                    btSoftBody::staticSolve(int iterations)
1674 {
1675         for(int isolve=0;isolve<iterations;++isolve)
1676         {
1677                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1678                 {
1679                         getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1680                 }
1681         }
1682 }
1683
1684 //
1685 void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1686 {
1687         /// placeholder
1688 }
1689
1690 //
1691 void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1692 {
1693         const int       nb=bodies.size();
1694         int                     iterations=0;
1695         int i;
1696
1697         for(i=0;i<nb;++i)
1698         {
1699                 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1700         }
1701         for(i=0;i<nb;++i)
1702         {
1703                 bodies[i]->prepareClusters(iterations);
1704         }
1705         for(i=0;i<iterations;++i)
1706         {
1707                 const btScalar sor=1;
1708                 for(int j=0;j<nb;++j)
1709                 {
1710                         bodies[j]->solveClusters(sor);
1711                 }
1712         }
1713         for(i=0;i<nb;++i)
1714         {
1715                 bodies[i]->cleanupClusters();
1716         }
1717 }
1718
1719 //
1720 void                    btSoftBody::integrateMotion()
1721 {
1722         /* Update                       */ 
1723         updateNormals();
1724 }
1725
1726 //
1727 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1728 {
1729         m_rayFrom = rayFrom;
1730         m_rayNormalizedDirection = (rayTo-rayFrom);
1731         m_rayTo = rayTo;
1732         m_mint  =       mxt;
1733         m_face  =       0;
1734         m_tests =       0;
1735 }
1736
1737 //
1738 void                            btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
1739 {
1740         btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
1741         const btScalar          t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
1742                 f.m_n[0]->m_x,
1743                 f.m_n[1]->m_x,
1744                 f.m_n[2]->m_x,
1745                 m_mint);
1746         if((t>0)&&(t<m_mint)) 
1747         { 
1748                 m_mint=t;m_face=&f; 
1749         }
1750         ++m_tests;
1751 }
1752
1753 //
1754 btScalar                        btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
1755                                                                                                                                    const btVector3& rayTo,
1756                                                                                                                                    const btVector3& rayNormalizedDirection,
1757                                                                                                                                    const btVector3& a,
1758                                                                                                                                    const btVector3& b,
1759                                                                                                                                    const btVector3& c,
1760                                                                                                                                    btScalar maxt)
1761 {
1762         static const btScalar   ceps=-SIMD_EPSILON*10;
1763         static const btScalar   teps=SIMD_EPSILON*10;
1764
1765         const btVector3                 n=btCross(b-a,c-a);
1766         const btScalar                  d=btDot(a,n);
1767         const btScalar                  den=btDot(rayNormalizedDirection,n);
1768         if(!btFuzzyZero(den))
1769         {
1770                 const btScalar          num=btDot(rayFrom,n)-d;
1771                 const btScalar          t=-num/den;
1772                 if((t>teps)&&(t<maxt))
1773                 {
1774                         const btVector3 hit=rayFrom+rayNormalizedDirection*t;
1775                         if(     (btDot(n,btCross(a-hit,b-hit))>ceps)    &&                      
1776                                 (btDot(n,btCross(b-hit,c-hit))>ceps)    &&
1777                                 (btDot(n,btCross(c-hit,a-hit))>ceps))
1778                         {
1779                                 return(t);
1780                         }
1781                 }
1782         }
1783         return(-1);
1784 }
1785
1786 //
1787 void                            btSoftBody::pointersToIndices()
1788 {
1789 #define PTR2IDX(_p_,_b_)        reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
1790         btSoftBody::Node*       base=&m_nodes[0];
1791         int i,ni;
1792
1793         for(i=0,ni=m_nodes.size();i<ni;++i)
1794         {
1795                 if(m_nodes[i].m_leaf)
1796                 {
1797                         m_nodes[i].m_leaf->data=*(void**)&i;
1798                 }
1799         }
1800         for(i=0,ni=m_links.size();i<ni;++i)
1801         {
1802                 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
1803                 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
1804         }
1805         for(i=0,ni=m_faces.size();i<ni;++i)
1806         {
1807                 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
1808                 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
1809                 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
1810                 if(m_faces[i].m_leaf)
1811                 {
1812                         m_faces[i].m_leaf->data=*(void**)&i;
1813                 }
1814         }
1815         for(i=0,ni=m_anchors.size();i<ni;++i)
1816         {
1817                 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
1818         }
1819         for(i=0,ni=m_notes.size();i<ni;++i)
1820         {
1821                 for(int j=0;j<m_notes[i].m_rank;++j)
1822                 {
1823                         m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
1824                 }
1825         }
1826 #undef  PTR2IDX
1827 }
1828
1829 //
1830 void                            btSoftBody::indicesToPointers(const int* map)
1831 {
1832 #define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
1833         (&(_b_)[(((char*)_p_)-(char*)0)])
1834         btSoftBody::Node*       base=&m_nodes[0];
1835         int i,ni;
1836
1837         for(i=0,ni=m_nodes.size();i<ni;++i)
1838         {
1839                 if(m_nodes[i].m_leaf)
1840                 {
1841                         m_nodes[i].m_leaf->data=&m_nodes[i];
1842                 }
1843         }
1844         for(i=0,ni=m_links.size();i<ni;++i)
1845         {
1846                 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
1847                 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
1848         }
1849         for(i=0,ni=m_faces.size();i<ni;++i)
1850         {
1851                 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
1852                 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
1853                 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
1854                 if(m_faces[i].m_leaf)
1855                 {
1856                         m_faces[i].m_leaf->data=&m_faces[i];
1857                 }
1858         }
1859         for(i=0,ni=m_anchors.size();i<ni;++i)
1860         {
1861                 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
1862         }
1863         for(i=0,ni=m_notes.size();i<ni;++i)
1864         {
1865                 for(int j=0;j<m_notes[i].m_rank;++j)
1866                 {
1867                         m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
1868                 }
1869         }
1870 #undef  IDX2PTR
1871 }
1872
1873 //
1874 int                                     btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
1875                                                                                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
1876 {
1877         int     cnt=0;
1878         if(bcountonly||m_fdbvt.empty())
1879         {/* Full search */ 
1880                 btVector3 dir = rayTo-rayFrom;
1881                 dir.normalize();
1882
1883                 for(int i=0,ni=m_faces.size();i<ni;++i)
1884                 {
1885                         const btSoftBody::Face& f=m_faces[i];
1886
1887                         const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
1888                                 f.m_n[0]->m_x,
1889                                 f.m_n[1]->m_x,
1890                                 f.m_n[2]->m_x,
1891                                 mint);
1892                         if(t>0)
1893                         {
1894                                 ++cnt;
1895                                 if(!bcountonly)
1896                                 {
1897                                         feature=btSoftBody::eFeature::Face;
1898                                         index=i;
1899                                         mint=t;
1900                                 }
1901                         }
1902                 }
1903         }
1904         else
1905         {/* Use dbvt    */ 
1906                 RayFromToCaster collider(rayFrom,rayTo,mint);
1907
1908                 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
1909                 if(collider.m_face)
1910                 {
1911                         mint=collider.m_mint;
1912                         feature=btSoftBody::eFeature::Face;
1913                         index=(int)(collider.m_face-&m_faces[0]);
1914                         cnt=1;
1915                 }
1916         }
1917         return(cnt);
1918 }
1919
1920 //
1921 void                    btSoftBody::initializeFaceTree()
1922 {
1923         m_fdbvt.clear();
1924         for(int i=0;i<m_faces.size();++i)
1925         {
1926                 Face&   f=m_faces[i];
1927                 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
1928         }
1929 }
1930
1931 //
1932 btVector3               btSoftBody::evaluateCom() const
1933 {
1934         btVector3       com(0,0,0);
1935         if(m_pose.m_bframe)
1936         {
1937                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1938                 {
1939                         com+=m_nodes[i].m_x*m_pose.m_wgh[i];
1940                 }
1941         }
1942         return(com);
1943 }
1944
1945 //
1946 bool                            btSoftBody::checkContact(       btCollisionObject* colObj,
1947                                                                                          const btVector3& x,
1948                                                                                          btScalar margin,
1949                                                                                          btSoftBody::sCti& cti) const
1950 {
1951         btVector3                       nrm;
1952         btCollisionShape*       shp=colObj->getCollisionShape();
1953         btRigidBody* tmpRigid = btRigidBody::upcast(colObj);
1954         const btTransform&      wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform();
1955         btScalar                        dst=m_worldInfo->m_sparsesdf.Evaluate(  wtr.invXform(x),
1956                 shp,
1957                 nrm,
1958                 margin);
1959         if(dst<0)
1960         {
1961                 cti.m_colObj            =       colObj;
1962                 cti.m_normal    =       wtr.getBasis()*nrm;
1963                 cti.m_offset    =       -btDot( cti.m_normal,
1964                         x-cti.m_normal*dst);
1965                 return(true);
1966         }
1967         return(false);
1968 }
1969
1970 //
1971 void                                    btSoftBody::updateNormals()
1972 {
1973         const btVector3 zv(0,0,0);
1974         int i,ni;
1975
1976         for(i=0,ni=m_nodes.size();i<ni;++i)
1977         {
1978                 m_nodes[i].m_n=zv;
1979         }
1980         for(i=0,ni=m_faces.size();i<ni;++i)
1981         {
1982                 btSoftBody::Face&       f=m_faces[i];
1983                 const btVector3         n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
1984                         f.m_n[2]->m_x-f.m_n[0]->m_x);
1985                 f.m_normal=n.normalized();
1986                 f.m_n[0]->m_n+=n;
1987                 f.m_n[1]->m_n+=n;
1988                 f.m_n[2]->m_n+=n;
1989         }
1990         for(i=0,ni=m_nodes.size();i<ni;++i)
1991         {
1992                 btScalar len = m_nodes[i].m_n.length();
1993                 if (len>SIMD_EPSILON)
1994                         m_nodes[i].m_n /= len;
1995         }
1996 }
1997
1998 //
1999 void                                    btSoftBody::updateBounds()
2000 {
2001         if(m_ndbvt.m_root)
2002         {
2003                 const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
2004                 const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
2005                 const btScalar          csm=getCollisionShape()->getMargin();
2006                 const btVector3         mrg=btVector3(  csm,
2007                         csm,
2008                         csm)*1; // ??? to investigate...
2009                 m_bounds[0]=mins-mrg;
2010                 m_bounds[1]=maxs+mrg;
2011                 if(0!=getBroadphaseHandle())
2012                 {                                       
2013                         m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
2014                                 m_bounds[0],
2015                                 m_bounds[1],
2016                                 m_worldInfo->m_dispatcher);
2017                 }
2018         }
2019         else
2020         {
2021                 m_bounds[0]=
2022                         m_bounds[1]=btVector3(0,0,0);
2023         }               
2024 }
2025
2026
2027 //
2028 void                                    btSoftBody::updatePose()
2029 {
2030         if(m_pose.m_bframe)
2031         {
2032                 btSoftBody::Pose&       pose=m_pose;
2033                 const btVector3         com=evaluateCom();
2034                 /* Com                  */ 
2035                 pose.m_com      =       com;
2036                 /* Rotation             */ 
2037                 btMatrix3x3             Apq;
2038                 const btScalar  eps=SIMD_EPSILON;
2039                 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2040                 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2041                 for(int i=0,ni=m_nodes.size();i<ni;++i)
2042                 {
2043                         const btVector3         a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2044                         const btVector3&        b=pose.m_pos[i];
2045                         Apq[0]+=a.x()*b;
2046                         Apq[1]+=a.y()*b;
2047                         Apq[2]+=a.z()*b;
2048                 }
2049                 btMatrix3x3             r,s;
2050                 PolarDecompose(Apq,r,s);
2051                 pose.m_rot=r;
2052                 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2053                 if(m_cfg.maxvolume>1)
2054                 {
2055                         const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
2056                                 1,m_cfg.maxvolume);
2057                         pose.m_scl=Mul(pose.m_scl,idet);
2058                 }
2059
2060         }
2061 }
2062
2063 //
2064 void                            btSoftBody::updateConstants()
2065 {
2066         int i,ni;
2067
2068         /* Links                */ 
2069         for(i=0,ni=m_links.size();i<ni;++i)
2070         {
2071                 Link&           l=m_links[i];
2072                 Material&       m=*l.m_material;
2073                 l.m_rl  =       (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
2074                 l.m_c0  =       (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2075                 l.m_c1  =       l.m_rl*l.m_rl;
2076         }
2077         /* Faces                */ 
2078         for(i=0,ni=m_faces.size();i<ni;++i)
2079         {
2080                 Face&           f=m_faces[i];
2081                 f.m_ra  =       AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2082         }
2083         /* Area's               */ 
2084         btAlignedObjectArray<int>       counts;
2085         counts.resize(m_nodes.size(),0);
2086         for(i=0,ni=m_nodes.size();i<ni;++i)
2087         {
2088                 m_nodes[i].m_area       =       0;
2089         }
2090         for(i=0,ni=m_faces.size();i<ni;++i)
2091         {
2092                 btSoftBody::Face&       f=m_faces[i];
2093                 for(int j=0;j<3;++j)
2094                 {
2095                         const int index=(int)(f.m_n[j]-&m_nodes[0]);
2096                         counts[index]++;
2097                         f.m_n[j]->m_area+=btFabs(f.m_ra);
2098                 }
2099         }
2100         for(i=0,ni=m_nodes.size();i<ni;++i)
2101         {
2102                 if(counts[i]>0)
2103                         m_nodes[i].m_area/=(btScalar)counts[i];
2104                 else
2105                         m_nodes[i].m_area=0;
2106         }
2107 }
2108
2109 //
2110 void                                    btSoftBody::initializeClusters()
2111 {
2112         int i;
2113
2114         for( i=0;i<m_clusters.size();++i)
2115         {
2116                 Cluster&        c=*m_clusters[i];
2117                 c.m_imass=0;
2118                 c.m_masses.resize(c.m_nodes.size());
2119                 for(int j=0;j<c.m_nodes.size();++j)
2120                 {
2121                         if (c.m_nodes[j]->m_im==0)
2122                         {
2123                                 c.m_containsAnchor = true;
2124                                 c.m_masses[j]   =       BT_LARGE_FLOAT;
2125                         } else
2126                         {
2127                                 c.m_masses[j]   =       btScalar(1.)/c.m_nodes[j]->m_im;
2128                         }
2129                         c.m_imass               +=      c.m_masses[j];
2130                 }
2131                 c.m_imass               =       btScalar(1.)/c.m_imass;
2132                 c.m_com                 =       btSoftBody::clusterCom(&c);
2133                 c.m_lv                  =       btVector3(0,0,0);
2134                 c.m_av                  =       btVector3(0,0,0);
2135                 c.m_leaf                =       0;
2136                 /* Inertia      */ 
2137                 btMatrix3x3&    ii=c.m_locii;
2138                 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2139                 {
2140                         int i,ni;
2141
2142                         for(i=0,ni=c.m_nodes.size();i<ni;++i)
2143                         {
2144                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2145                                 const btVector3 q=k*k;
2146                                 const btScalar  m=c.m_masses[i];
2147                                 ii[0][0]        +=      m*(q[1]+q[2]);
2148                                 ii[1][1]        +=      m*(q[0]+q[2]);
2149                                 ii[2][2]        +=      m*(q[0]+q[1]);
2150                                 ii[0][1]        -=      m*k[0]*k[1];
2151                                 ii[0][2]        -=      m*k[0]*k[2];
2152                                 ii[1][2]        -=      m*k[1]*k[2];
2153                         }
2154                 }
2155                 ii[1][0]=ii[0][1];
2156                 ii[2][0]=ii[0][2];
2157                 ii[2][1]=ii[1][2];
2158                 
2159                 ii = ii.inverse();
2160
2161                 /* Frame        */ 
2162                 c.m_framexform.setIdentity();
2163                 c.m_framexform.setOrigin(c.m_com);
2164                 c.m_framerefs.resize(c.m_nodes.size());
2165                 {
2166                         int i;
2167                         for(i=0;i<c.m_framerefs.size();++i)
2168                         {
2169                                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2170                         }
2171                 }
2172         }
2173 }
2174
2175 //
2176 void                                    btSoftBody::updateClusters()
2177 {
2178         BT_PROFILE("UpdateClusters");
2179         int i;
2180
2181         for(i=0;i<m_clusters.size();++i)
2182         {
2183                 btSoftBody::Cluster&    c=*m_clusters[i];
2184                 const int                               n=c.m_nodes.size();
2185                 //const btScalar                        invn=1/(btScalar)n;
2186                 if(n)
2187                 {
2188                         /* Frame                                */ 
2189                         const btScalar  eps=btScalar(0.0001);
2190                         btMatrix3x3             m,r,s;
2191                         m[0]=m[1]=m[2]=btVector3(0,0,0);
2192                         m[0][0]=eps*1;
2193                         m[1][1]=eps*2;
2194                         m[2][2]=eps*3;
2195                         c.m_com=clusterCom(&c);
2196                         for(int i=0;i<c.m_nodes.size();++i)
2197                         {
2198                                 const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
2199                                 const btVector3&        b=c.m_framerefs[i];
2200                                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2201                         }
2202                         PolarDecompose(m,r,s);
2203                         c.m_framexform.setOrigin(c.m_com);
2204                         c.m_framexform.setBasis(r);             
2205                         /* Inertia                      */ 
2206 #if 1/* Constant        */ 
2207                         c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2208 #else
2209 #if 0/* Sphere  */ 
2210                         const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
2211                         const btVector3 inertia(rk,rk,rk);
2212                         const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2213                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2214                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2215
2216                         c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2217 #else/* Actual  */              
2218                         c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2219                         for(int i=0;i<n;++i)
2220                         {
2221                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2222                                 const btVector3         q=k*k;
2223                                 const btScalar          m=1/c.m_nodes[i]->m_im;
2224                                 c.m_invwi[0][0] +=      m*(q[1]+q[2]);
2225                                 c.m_invwi[1][1] +=      m*(q[0]+q[2]);
2226                                 c.m_invwi[2][2] +=      m*(q[0]+q[1]);
2227                                 c.m_invwi[0][1] -=      m*k[0]*k[1];
2228                                 c.m_invwi[0][2] -=      m*k[0]*k[2];
2229                                 c.m_invwi[1][2] -=      m*k[1]*k[2];
2230                         }
2231                         c.m_invwi[1][0]=c.m_invwi[0][1];
2232                         c.m_invwi[2][0]=c.m_invwi[0][2];
2233                         c.m_invwi[2][1]=c.m_invwi[1][2];
2234                         c.m_invwi=c.m_invwi.inverse();
2235 #endif
2236 #endif
2237                         /* Velocities                   */ 
2238                         c.m_lv=btVector3(0,0,0);
2239                         c.m_av=btVector3(0,0,0);
2240                         {
2241                                 int i;
2242
2243                                 for(i=0;i<n;++i)
2244                                 {
2245                                         const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2246                                         c.m_lv  +=      v;
2247                                         c.m_av  +=      btCross(c.m_nodes[i]->m_x-c.m_com,v);
2248                                 }
2249                         }
2250                         c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2251                         c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2252                         c.m_vimpulses[0]        =
2253                                 c.m_vimpulses[1]        = btVector3(0,0,0);
2254                         c.m_dimpulses[0]        =
2255                                 c.m_dimpulses[1]        = btVector3(0,0,0);
2256                         c.m_nvimpulses          = 0;
2257                         c.m_ndimpulses          = 0;
2258                         /* Matching                             */ 
2259                         if(c.m_matching>0)
2260                         {
2261                                 for(int j=0;j<c.m_nodes.size();++j)
2262                                 {
2263                                         Node&                   n=*c.m_nodes[j];
2264                                         const btVector3 x=c.m_framexform*c.m_framerefs[j];
2265                                         n.m_x=Lerp(n.m_x,x,c.m_matching);
2266                                 }
2267                         }                       
2268                         /* Dbvt                                 */ 
2269                         if(c.m_collide)
2270                         {
2271                                 btVector3       mi=c.m_nodes[0]->m_x;
2272                                 btVector3       mx=mi;
2273                                 for(int j=1;j<n;++j)
2274                                 {
2275                                         mi.setMin(c.m_nodes[j]->m_x);
2276                                         mx.setMax(c.m_nodes[j]->m_x);
2277                                 }                       
2278                                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(mi,mx);
2279                                 if(c.m_leaf)
2280                                         m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2281                                 else
2282                                         c.m_leaf=m_cdbvt.insert(bounds,&c);
2283                         }
2284                 }
2285         }
2286
2287
2288 }
2289
2290
2291
2292
2293 //
2294 void                                    btSoftBody::cleanupClusters()
2295 {
2296         for(int i=0;i<m_joints.size();++i)
2297         {
2298                 m_joints[i]->Terminate(m_sst.sdt);
2299                 if(m_joints[i]->m_delete)
2300                 {
2301                         btAlignedFree(m_joints[i]);
2302                         m_joints.remove(m_joints[i--]);
2303                 }       
2304         }
2305 }
2306
2307 //
2308 void                                    btSoftBody::prepareClusters(int iterations)
2309 {
2310         for(int i=0;i<m_joints.size();++i)
2311         {
2312                 m_joints[i]->Prepare(m_sst.sdt,iterations);
2313         }
2314 }
2315
2316
2317 //
2318 void                                    btSoftBody::solveClusters(btScalar sor)
2319 {
2320         for(int i=0,ni=m_joints.size();i<ni;++i)
2321         {
2322                 m_joints[i]->Solve(m_sst.sdt,sor);
2323         }
2324 }
2325
2326 //
2327 void                                    btSoftBody::applyClusters(bool drift)
2328 {
2329         BT_PROFILE("ApplyClusters");
2330 //      const btScalar                                  f0=m_sst.sdt;
2331         //const btScalar                                        f1=f0/2;
2332         btAlignedObjectArray<btVector3> deltas;
2333         btAlignedObjectArray<btScalar> weights;
2334         deltas.resize(m_nodes.size(),btVector3(0,0,0));
2335         weights.resize(m_nodes.size(),0);
2336         int i;
2337
2338         if(drift)
2339         {
2340                 for(i=0;i<m_clusters.size();++i)
2341                 {
2342                         Cluster&        c=*m_clusters[i];
2343                         if(c.m_ndimpulses)
2344                         {
2345                                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2346                                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2347                         }
2348                 }
2349         }
2350         
2351         for(i=0;i<m_clusters.size();++i)
2352         {
2353                 Cluster&        c=*m_clusters[i];       
2354                 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2355                 {
2356                         const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2357                         const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2358                         for(int j=0;j<c.m_nodes.size();++j)
2359                         {
2360                                 const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
2361                                 const btVector3&        x=c.m_nodes[j]->m_x;
2362                                 const btScalar          q=c.m_masses[j];
2363                                 deltas[idx]             +=      (v+btCross(w,x-c.m_com))*q;
2364                                 weights[idx]    +=      q;
2365                         }
2366                 }
2367         }
2368         for(i=0;i<deltas.size();++i)
2369         {
2370                 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
2371         }
2372 }
2373
2374 //
2375 void                                    btSoftBody::dampClusters()
2376 {
2377         int i;
2378
2379         for(i=0;i<m_clusters.size();++i)
2380         {
2381                 Cluster&        c=*m_clusters[i];       
2382                 if(c.m_ndamping>0)
2383                 {
2384                         for(int j=0;j<c.m_nodes.size();++j)
2385                         {
2386                                 Node&                   n=*c.m_nodes[j];
2387                                 if(n.m_im>0)
2388                                 {
2389                                         const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2390                                         if(vx.length2()<=n.m_v.length2())
2391                                                 {
2392                                                 n.m_v   +=      c.m_ndamping*(vx-n.m_v);
2393                                                 }
2394                                 }
2395                         }
2396                 }
2397         }
2398 }
2399
2400 //
2401 void                            btSoftBody::Joint::Prepare(btScalar dt,int)
2402 {
2403         m_bodies[0].activate();
2404         m_bodies[1].activate();
2405 }
2406
2407 //
2408 void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2409 {
2410         static const btScalar   maxdrift=4;
2411         Joint::Prepare(dt,iterations);
2412         m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
2413         m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
2414         m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2415         m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
2416         m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
2417         m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2418                 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2419         if(m_split>0)
2420         {
2421                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2422                 m_drift         *=      1-m_split;
2423         }
2424         m_drift /=(btScalar)iterations;
2425 }
2426
2427 //
2428 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2429 {
2430         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2431         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2432         const btVector3         vr=va-vb;
2433         btSoftBody::Impulse     impulse;
2434         impulse.m_asVelocity    =       1;
2435         impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
2436         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2437         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2438 }
2439
2440 //
2441 void                            btSoftBody::LJoint::Terminate(btScalar dt)
2442 {
2443         if(m_split>0)
2444         {
2445                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2446                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2447         }
2448 }
2449
2450 //
2451 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2452 {
2453         static const btScalar   maxdrift=SIMD_PI/16;
2454         m_icontrol->Prepare(this);
2455         Joint::Prepare(dt,iterations);
2456         m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
2457         m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
2458         m_drift         =       NormalizeAny(btCross(m_axis[1],m_axis[0]));
2459         m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2460         m_drift         *=      m_erp/dt;
2461         m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2462         if(m_split>0)
2463         {
2464                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2465                 m_drift         *=      1-m_split;
2466         }
2467         m_drift /=(btScalar)iterations;
2468 }
2469
2470 //
2471 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2472 {
2473         const btVector3         va=m_bodies[0].angularVelocity();
2474         const btVector3         vb=m_bodies[1].angularVelocity();
2475         const btVector3         vr=va-vb;
2476         const btScalar          sp=btDot(vr,m_axis[0]);
2477         const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2478         btSoftBody::Impulse     impulse;
2479         impulse.m_asVelocity    =       1;
2480         impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
2481         m_bodies[0].applyAImpulse(-impulse);
2482         m_bodies[1].applyAImpulse( impulse);
2483 }
2484
2485 //
2486 void                            btSoftBody::AJoint::Terminate(btScalar dt)
2487 {
2488         if(m_split>0)
2489         {
2490                 m_bodies[0].applyDAImpulse(-m_sdrift);
2491                 m_bodies[1].applyDAImpulse( m_sdrift);
2492         }
2493 }
2494
2495 //
2496 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2497 {
2498         Joint::Prepare(dt,iterations);
2499         const bool      dodrift=(m_life==0);
2500         m_delete=(++m_life)>m_maxlife;
2501         if(dodrift)
2502         {
2503                 m_drift=m_drift*m_erp/dt;
2504                 if(m_split>0)
2505                 {
2506                         m_sdrift        =       m_massmatrix*(m_drift*m_split);
2507                         m_drift         *=      1-m_split;
2508                 }
2509                 m_drift/=(btScalar)iterations;
2510         }
2511         else
2512         {
2513                 m_drift=m_sdrift=btVector3(0,0,0);
2514         }
2515 }
2516
2517 //
2518 void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2519 {
2520         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2521         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2522         const btVector3         vrel=va-vb;
2523         const btScalar          rvac=btDot(vrel,m_normal);
2524         btSoftBody::Impulse     impulse;
2525         impulse.m_asVelocity    =       1;
2526         impulse.m_velocity              =       m_drift;
2527         if(rvac<0)
2528         {
2529                 const btVector3 iv=m_normal*rvac;
2530                 const btVector3 fv=vrel-iv;
2531                 impulse.m_velocity      +=      iv+fv*m_friction;
2532         }
2533         impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2534         
2535         if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2536         {
2537                 if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2538                         (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2539                 {
2540                         if (impulse.m_asVelocity)
2541                         {
2542                                 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2543                                 {
2544                                         
2545                                 } else
2546                                 {
2547                                         m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2548                                         m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2549                                 }
2550                         }
2551                 }
2552         } else
2553         {
2554                 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2555                 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2556         }
2557 }
2558
2559 //
2560 void                            btSoftBody::CJoint::Terminate(btScalar dt)
2561 {
2562         if(m_split>0)
2563         {
2564                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2565                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2566         }
2567 }
2568
2569 //
2570 void                            btSoftBody::applyForces()
2571 {
2572
2573         BT_PROFILE("SoftBody applyForces");
2574         const btScalar                                  dt=m_sst.sdt;
2575         const btScalar                                  kLF=m_cfg.kLF;
2576         const btScalar                                  kDG=m_cfg.kDG;
2577         const btScalar                                  kPR=m_cfg.kPR;
2578         const btScalar                                  kVC=m_cfg.kVC;
2579         const bool                                              as_lift=kLF>0;
2580         const bool                                              as_drag=kDG>0;
2581         const bool                                              as_pressure=kPR!=0;
2582         const bool                                              as_volume=kVC>0;
2583         const bool                                              as_aero=        as_lift         ||
2584                 as_drag         ;
2585         const bool                                              as_vaero=       as_aero         &&
2586                 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
2587         const bool                                              as_faero=       as_aero         &&
2588                 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
2589         const bool                                              use_medium=     as_aero;
2590         const bool                                              use_volume=     as_pressure     ||
2591                 as_volume       ;
2592         btScalar                                                volume=0;
2593         btScalar                                                ivolumetp=0;
2594         btScalar                                                dvolumetv=0;
2595         btSoftBody::sMedium     medium;
2596         if(use_volume)
2597         {
2598                 volume          =       getVolume();
2599                 ivolumetp       =       1/btFabs(volume)*kPR;
2600                 dvolumetv       =       (m_pose.m_volume-volume)*kVC;
2601         }
2602         /* Per vertex forces                    */ 
2603         int i,ni;
2604
2605         for(i=0,ni=m_nodes.size();i<ni;++i)
2606         {
2607                 btSoftBody::Node&       n=m_nodes[i];
2608                 if(n.m_im>0)
2609                 {
2610                         if(use_medium)
2611                         {
2612                                 EvaluateMedium(m_worldInfo,n.m_x,medium);
2613                                 /* Aerodynamics                 */ 
2614                                 if(as_vaero)
2615                                 {                               
2616                                         const btVector3 rel_v=n.m_v-medium.m_velocity;
2617                                         const btScalar  rel_v2=rel_v.length2();
2618                                         if(rel_v2>SIMD_EPSILON)
2619                                         {
2620                                                 btVector3       nrm=n.m_n;
2621                                                 /* Setup normal         */ 
2622                                                 switch(m_cfg.aeromodel)
2623                                                 {
2624                                                 case    btSoftBody::eAeroModel::V_Point:
2625                                                         nrm=NormalizeAny(rel_v);break;
2626                                                 case    btSoftBody::eAeroModel::V_TwoSided:
2627                                                         nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
2628                                                         default:
2629                                                         {
2630                                                         }
2631                                                 }
2632                                                 const btScalar  dvn=btDot(rel_v,nrm);
2633                                                 /* Compute forces       */ 
2634                                                 if(dvn>0)
2635                                                 {
2636                                                         btVector3               force(0,0,0);
2637                                                         const btScalar  c0      =       n.m_area*dvn*rel_v2/2;
2638                                                         const btScalar  c1      =       c0*medium.m_density;
2639                                                         force   +=      nrm*(-c1*kLF);
2640                                                         force   +=      rel_v.normalized()*(-c1*kDG);
2641                                                         ApplyClampedForce(n,force,dt);
2642                                                 }
2643                                         }
2644                                 }
2645                         }
2646                         /* Pressure                             */ 
2647                         if(as_pressure)
2648                         {
2649                                 n.m_f   +=      n.m_n*(n.m_area*ivolumetp);
2650                         }
2651                         /* Volume                               */ 
2652                         if(as_volume)
2653                         {
2654                                 n.m_f   +=      n.m_n*(n.m_area*dvolumetv);
2655                         }
2656                 }
2657         }
2658         /* Per face forces                              */ 
2659         for(i=0,ni=m_faces.size();i<ni;++i)
2660         {
2661                 btSoftBody::Face&       f=m_faces[i];
2662                 if(as_faero)
2663                 {
2664                         const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
2665                         const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
2666                         EvaluateMedium(m_worldInfo,x,medium);
2667                         const btVector3 rel_v=v-medium.m_velocity;
2668                         const btScalar  rel_v2=rel_v.length2();
2669                         if(rel_v2>SIMD_EPSILON)
2670                         {
2671                                 btVector3       nrm=f.m_normal;
2672                                 /* Setup normal         */ 
2673                                 switch(m_cfg.aeromodel)
2674                                 {
2675                                 case    btSoftBody::eAeroModel::F_TwoSided:
2676                                         nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
2677                                         default:
2678                                         {
2679                                         }
2680                                 }
2681                                 const btScalar  dvn=btDot(rel_v,nrm);
2682                                 /* Compute forces       */ 
2683                                 if(dvn>0)
2684                                 {
2685                                         btVector3               force(0,0,0);
2686                                         const btScalar  c0      =       f.m_ra*dvn*rel_v2;
2687                                         const btScalar  c1      =       c0*medium.m_density;
2688                                         force   +=      nrm*(-c1*kLF);
2689                                         force   +=      rel_v.normalized()*(-c1*kDG);
2690                                         force   /=      3;
2691                                         for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
2692                                 }
2693                         }
2694                 }
2695         }
2696 }
2697
2698 //
2699 void                            btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2700 {
2701         const btScalar  kAHR=psb->m_cfg.kAHR*kst;
2702         const btScalar  dt=psb->m_sst.sdt;
2703         for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2704         {
2705                 const Anchor&           a=psb->m_anchors[i];
2706                 const btTransform&      t=a.m_body->getInterpolationWorldTransform();
2707                 Node&                           n=*a.m_node;
2708                 const btVector3         wa=t*a.m_local;
2709                 const btVector3         va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2710                 const btVector3         vb=n.m_x-n.m_q;
2711                 const btVector3         vr=(va-vb)+(wa-n.m_x)*kAHR;
2712                 const btVector3         impulse=a.m_c0*vr;
2713                 n.m_x+=impulse*a.m_c2;
2714                 a.m_body->applyImpulse(-impulse,a.m_c1);
2715         }
2716 }
2717
2718 //
2719 void                            btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
2720 {
2721         const btScalar  dt=psb->m_sst.sdt;
2722         const btScalar  mrg=psb->getCollisionShape()->getMargin();
2723         for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
2724         {
2725                 const RContact&         c=psb->m_rcontacts[i];
2726                 const sCti&                     cti=c.m_cti;    
2727                 btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj);
2728
2729                 const btVector3         va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
2730                 const btVector3         vb=c.m_node->m_x-c.m_node->m_q; 
2731                 const btVector3         vr=vb-va;
2732                 const btScalar          dn=btDot(vr,cti.m_normal);              
2733                 if(dn<=SIMD_EPSILON)
2734                 {
2735                         const btScalar          dp=btMin(btDot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
2736                         const btVector3         fv=vr-cti.m_normal*dn;
2737                         const btVector3         impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
2738                         c.m_node->m_x-=impulse*c.m_c2;
2739                         if (tmpRigid)
2740                                 tmpRigid->applyImpulse(impulse,c.m_c1);
2741                 }
2742         }
2743 }
2744
2745 //
2746 void                            btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
2747 {
2748         for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
2749         {
2750                 const SContact&         c=psb->m_scontacts[i];
2751                 const btVector3&        nr=c.m_normal;
2752                 Node&                           n=*c.m_node;
2753                 Face&                           f=*c.m_face;
2754                 const btVector3         p=BaryEval(     f.m_n[0]->m_x,
2755                         f.m_n[1]->m_x,
2756                         f.m_n[2]->m_x,
2757                         c.m_weights);
2758                 const btVector3         q=BaryEval(     f.m_n[0]->m_q,
2759                         f.m_n[1]->m_q,
2760                         f.m_n[2]->m_q,
2761                         c.m_weights);                                                                                   
2762                 const btVector3         vr=(n.m_x-n.m_q)-(p-q);
2763                 btVector3                       corr(0,0,0);
2764                 btScalar dot = btDot(vr,nr);
2765                 if(dot<0)
2766                 {
2767                         const btScalar  j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
2768                         corr+=c.m_normal*j;
2769                 }
2770                 corr                    -=      ProjectOnPlane(vr,nr)*c.m_friction;
2771                 n.m_x                   +=      corr*c.m_cfm[0];
2772                 f.m_n[0]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.x());
2773                 f.m_n[1]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.y());
2774                 f.m_n[2]->m_x   -=      corr*(c.m_cfm[1]*c.m_weights.z());
2775         }
2776 }
2777
2778 //
2779 void                            btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
2780 {
2781         for(int i=0,ni=psb->m_links.size();i<ni;++i)
2782         {                       
2783                 Link&   l=psb->m_links[i];
2784                 if(l.m_c0>0)
2785                 {
2786                         Node&                   a=*l.m_n[0];
2787                         Node&                   b=*l.m_n[1];
2788                         const btVector3 del=b.m_x-a.m_x;
2789                         const btScalar  len=del.length2();
2790                         const btScalar  k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
2791                         //const btScalar        t=k*a.m_im;
2792                         a.m_x-=del*(k*a.m_im);
2793                         b.m_x+=del*(k*b.m_im);
2794                 }
2795         }
2796 }
2797
2798 //
2799 void                            btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
2800 {
2801         for(int i=0,ni=psb->m_links.size();i<ni;++i)
2802         {                       
2803                 Link&                   l=psb->m_links[i];
2804                 Node**                  n=l.m_n;
2805                 const btScalar  j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
2806                 n[0]->m_v+=     l.m_c3*(j*n[0]->m_im);
2807                 n[1]->m_v-=     l.m_c3*(j*n[1]->m_im);
2808         }
2809 }
2810
2811 //
2812 btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
2813 {
2814         switch(solver)
2815         {
2816         case    ePSolver::Anchors:              
2817                 return(&btSoftBody::PSolve_Anchors);
2818         case    ePSolver::Linear:               
2819                 return(&btSoftBody::PSolve_Links);
2820         case    ePSolver::RContacts:    
2821                 return(&btSoftBody::PSolve_RContacts);
2822         case    ePSolver::SContacts:    
2823                 return(&btSoftBody::PSolve_SContacts);  
2824                 default:
2825                 {
2826                 }
2827         }
2828         return(0);
2829 }
2830
2831 //
2832 btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
2833 {
2834         switch(solver)
2835         {
2836         case    eVSolver::Linear:               return(&btSoftBody::VSolve_Links);
2837                 default:
2838                 {
2839                 }
2840         }
2841         return(0);
2842 }
2843
2844 //
2845 void                    btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
2846 {
2847         switch(m_cfg.collisions&fCollision::RVSmask)
2848         {
2849         case    fCollision::SDF_RS:
2850                 {
2851                         btSoftColliders::CollideSDF_RS  docollide;              
2852                         btRigidBody*            prb1=btRigidBody::upcast(pco);
2853                         btTransform     wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform();
2854
2855                         const btTransform       ctr=pco->getWorldTransform();
2856                         const btScalar          timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
2857                         const btScalar          basemargin=getCollisionShape()->getMargin();
2858                         btVector3                       mins;
2859                         btVector3                       maxs;
2860                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
2861                         pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
2862                                 mins,
2863                                 maxs);
2864                         volume=btDbvtVolume::FromMM(mins,maxs);
2865                         volume.Expand(btVector3(basemargin,basemargin,basemargin));             
2866                         docollide.psb           =       this;
2867                         docollide.m_colObj1 = pco;
2868                         docollide.m_rigidBody = prb1;
2869
2870                         docollide.dynmargin     =       basemargin+timemargin;
2871                         docollide.stamargin     =       basemargin;
2872                         m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
2873                 }
2874                 break;
2875         case    fCollision::CL_RS:
2876                 {
2877                         btSoftColliders::CollideCL_RS   collider;
2878                         collider.Process(this,pco);
2879                 }
2880                 break;
2881         }
2882 }
2883
2884 //
2885 void                    btSoftBody::defaultCollisionHandler(btSoftBody* psb)
2886 {
2887         const int cf=m_cfg.collisions&psb->m_cfg.collisions;
2888         switch(cf&fCollision::SVSmask)
2889         {
2890         case    fCollision::CL_SS:
2891                 {
2892                         
2893                         //support self-collision if CL_SELF flag set
2894                         if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
2895                         {
2896                                 btSoftColliders::CollideCL_SS   docollide;
2897                                 docollide.Process(this,psb);
2898                         }
2899                         
2900                 }
2901                 break;
2902         case    fCollision::VF_SS:
2903                 {
2904                         //only self-collision for Cluster, not Vertex-Face yet
2905                         if (this!=psb)
2906                         {
2907                                 btSoftColliders::CollideVF_SS   docollide;
2908                                 /* common                                       */ 
2909                                 docollide.mrg=  getCollisionShape()->getMargin()+
2910                                         psb->getCollisionShape()->getMargin();
2911                                 /* psb0 nodes vs psb1 faces     */ 
2912                                 docollide.psb[0]=this;
2913                                 docollide.psb[1]=psb;
2914                                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
2915                                         docollide.psb[1]->m_fdbvt.m_root,
2916                                         docollide);
2917                                 /* psb1 nodes vs psb0 faces     */ 
2918                                 docollide.psb[0]=psb;
2919                                 docollide.psb[1]=this;
2920                                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
2921                                         docollide.psb[1]->m_fdbvt.m_root,
2922                                         docollide);
2923                         }
2924                 }
2925                 break;
2926         default:
2927                 {
2928                         
2929               &nb