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