Upgrade Bullet to version 2.83.
[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                 btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt;
1771                 {
1772                         btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
1773                         btScalar clampDeltaV = maxDisplacement/m_sst.sdt;
1774                         for (int c=0;c<3;c++)
1775                         {
1776                                 if (deltaV[c]>clampDeltaV)
1777                                 {
1778                                         deltaV[c] = clampDeltaV;
1779                                 }
1780                                 if (deltaV[c]<-clampDeltaV)
1781                                 {
1782                                         deltaV[c]=-clampDeltaV;
1783                                 }
1784                         }
1785                 }
1786                 n.m_v   +=      deltaV;
1787                 n.m_x   +=      n.m_v*m_sst.sdt;
1788                 n.m_f   =       btVector3(0,0,0);
1789         }
1790         /* Clusters                             */ 
1791         updateClusters();
1792         /* Bounds                               */ 
1793         updateBounds(); 
1794         /* Nodes                                */ 
1795         ATTRIBUTE_ALIGNED16(btDbvtVolume)       vol;
1796         for(i=0,ni=m_nodes.size();i<ni;++i)
1797         {
1798                 Node&   n=m_nodes[i];
1799                 vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1800                 m_ndbvt.update( n.m_leaf,
1801                         vol,
1802                         n.m_v*m_sst.velmrg,
1803                         m_sst.updmrg);
1804         }
1805         /* Faces                                */ 
1806         if(!m_fdbvt.empty())
1807         {
1808                 for(int i=0;i<m_faces.size();++i)
1809                 {
1810                         Face&                   f=m_faces[i];
1811                         const btVector3 v=(     f.m_n[0]->m_v+
1812                                 f.m_n[1]->m_v+
1813                                 f.m_n[2]->m_v)/3;
1814                         vol = VolumeOf(f,m_sst.radmrg);
1815                         m_fdbvt.update( f.m_leaf,
1816                                 vol,
1817                                 v*m_sst.velmrg,
1818                                 m_sst.updmrg);
1819                 }
1820         }
1821         /* Pose                                 */ 
1822         updatePose();
1823         /* Match                                */ 
1824         if(m_pose.m_bframe&&(m_cfg.kMT>0))
1825         {
1826                 const btMatrix3x3       posetrs=m_pose.m_rot;
1827                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1828                 {
1829                         Node&   n=m_nodes[i];
1830                         if(n.m_im>0)
1831                         {
1832                                 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1833                                 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1834                         }
1835                 }
1836         }
1837         /* Clear contacts               */ 
1838         m_rcontacts.resize(0);
1839         m_scontacts.resize(0);
1840         /* Optimize dbvt's              */ 
1841         m_ndbvt.optimizeIncremental(1);
1842         m_fdbvt.optimizeIncremental(1);
1843         m_cdbvt.optimizeIncremental(1);
1844 }
1845
1846 //
1847 void                    btSoftBody::solveConstraints()
1848 {
1849
1850         /* Apply clusters               */ 
1851         applyClusters(false);
1852         /* Prepare links                */ 
1853
1854         int i,ni;
1855
1856         for(i=0,ni=m_links.size();i<ni;++i)
1857         {
1858                 Link&   l=m_links[i];
1859                 l.m_c3          =       l.m_n[1]->m_q-l.m_n[0]->m_q;
1860                 l.m_c2          =       1/(l.m_c3.length2()*l.m_c0);
1861         }
1862         /* Prepare anchors              */ 
1863         for(i=0,ni=m_anchors.size();i<ni;++i)
1864         {
1865                 Anchor&                 a=m_anchors[i];
1866                 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1867                 a.m_c0  =       ImpulseMatrix(  m_sst.sdt,
1868                         a.m_node->m_im,
1869                         a.m_body->getInvMass(),
1870                         a.m_body->getInvInertiaTensorWorld(),
1871                         ra);
1872                 a.m_c1  =       ra;
1873                 a.m_c2  =       m_sst.sdt*a.m_node->m_im;
1874                 a.m_body->activate();
1875         }
1876         /* Solve velocities             */ 
1877         if(m_cfg.viterations>0)
1878         {
1879                 /* Solve                        */ 
1880                 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1881                 {
1882                         for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1883                         {
1884                                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
1885                         }
1886                 }
1887                 /* Update                       */ 
1888                 for(i=0,ni=m_nodes.size();i<ni;++i)
1889                 {
1890                         Node&   n=m_nodes[i];
1891                         n.m_x   =       n.m_q+n.m_v*m_sst.sdt;
1892                 }
1893         }
1894         /* Solve positions              */ 
1895         if(m_cfg.piterations>0)
1896         {
1897                 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1898                 {
1899                         const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1900                         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1901                         {
1902                                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1903                         }
1904                 }
1905                 const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
1906                 for(i=0,ni=m_nodes.size();i<ni;++i)
1907                 {
1908                         Node&   n=m_nodes[i];
1909                         n.m_v   =       (n.m_x-n.m_q)*vc;
1910                         n.m_f   =       btVector3(0,0,0);               
1911                 }
1912         }
1913         /* Solve drift                  */ 
1914         if(m_cfg.diterations>0)
1915         {
1916                 const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
1917                 for(i=0,ni=m_nodes.size();i<ni;++i)
1918                 {
1919                         Node&   n=m_nodes[i];
1920                         n.m_q   =       n.m_x;
1921                 }
1922                 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1923                 {
1924                         for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1925                         {
1926                                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1927                         }
1928                 }
1929                 for(int i=0,ni=m_nodes.size();i<ni;++i)
1930                 {
1931                         Node&   n=m_nodes[i];
1932                         n.m_v   +=      (n.m_x-n.m_q)*vcf;
1933                 }
1934         }
1935         /* Apply clusters               */ 
1936         dampClusters();
1937         applyClusters(true);
1938 }
1939
1940 //
1941 void                    btSoftBody::staticSolve(int iterations)
1942 {
1943         for(int isolve=0;isolve<iterations;++isolve)
1944         {
1945                 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1946                 {
1947                         getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1948                 }
1949         }
1950 }
1951
1952 //
1953 void                    btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1954 {
1955         /// placeholder
1956 }
1957
1958 //
1959 void                    btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1960 {
1961         const int       nb=bodies.size();
1962         int                     iterations=0;
1963         int i;
1964
1965         for(i=0;i<nb;++i)
1966         {
1967                 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1968         }
1969         for(i=0;i<nb;++i)
1970         {
1971                 bodies[i]->prepareClusters(iterations);
1972         }
1973         for(i=0;i<iterations;++i)
1974         {
1975                 const btScalar sor=1;
1976                 for(int j=0;j<nb;++j)
1977                 {
1978                         bodies[j]->solveClusters(sor);
1979                 }
1980         }
1981         for(i=0;i<nb;++i)
1982         {
1983                 bodies[i]->cleanupClusters();
1984         }
1985 }
1986
1987 //
1988 void                    btSoftBody::integrateMotion()
1989 {
1990         /* Update                       */ 
1991         updateNormals();
1992 }
1993
1994 //
1995 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1996 {
1997         m_rayFrom = rayFrom;
1998         m_rayNormalizedDirection = (rayTo-rayFrom);
1999         m_rayTo = rayTo;
2000         m_mint  =       mxt;
2001         m_face  =       0;
2002         m_tests =       0;
2003 }
2004
2005 //
2006 void                            btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
2007 {
2008         btSoftBody::Face&       f=*(btSoftBody::Face*)leaf->data;
2009         const btScalar          t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
2010                 f.m_n[0]->m_x,
2011                 f.m_n[1]->m_x,
2012                 f.m_n[2]->m_x,
2013                 m_mint);
2014         if((t>0)&&(t<m_mint)) 
2015         { 
2016                 m_mint=t;m_face=&f; 
2017         }
2018         ++m_tests;
2019 }
2020
2021 //
2022 btScalar                        btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
2023                                                                                                                                    const btVector3& rayTo,
2024                                                                                                                                    const btVector3& rayNormalizedDirection,
2025                                                                                                                                    const btVector3& a,
2026                                                                                                                                    const btVector3& b,
2027                                                                                                                                    const btVector3& c,
2028                                                                                                                                    btScalar maxt)
2029 {
2030         static const btScalar   ceps=-SIMD_EPSILON*10;
2031         static const btScalar   teps=SIMD_EPSILON*10;
2032
2033         const btVector3                 n=btCross(b-a,c-a);
2034         const btScalar                  d=btDot(a,n);
2035         const btScalar                  den=btDot(rayNormalizedDirection,n);
2036         if(!btFuzzyZero(den))
2037         {
2038                 const btScalar          num=btDot(rayFrom,n)-d;
2039                 const btScalar          t=-num/den;
2040                 if((t>teps)&&(t<maxt))
2041                 {
2042                         const btVector3 hit=rayFrom+rayNormalizedDirection*t;
2043                         if(     (btDot(n,btCross(a-hit,b-hit))>ceps)    &&                      
2044                                 (btDot(n,btCross(b-hit,c-hit))>ceps)    &&
2045                                 (btDot(n,btCross(c-hit,a-hit))>ceps))
2046                         {
2047                                 return(t);
2048                         }
2049                 }
2050         }
2051         return(-1);
2052 }
2053
2054 //
2055 void                            btSoftBody::pointersToIndices()
2056 {
2057 #define PTR2IDX(_p_,_b_)        reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
2058         btSoftBody::Node*       base=m_nodes.size() ? &m_nodes[0] : 0;
2059         int i,ni;
2060
2061         for(i=0,ni=m_nodes.size();i<ni;++i)
2062         {
2063                 if(m_nodes[i].m_leaf)
2064                 {
2065                         m_nodes[i].m_leaf->data=*(void**)&i;
2066                 }
2067         }
2068         for(i=0,ni=m_links.size();i<ni;++i)
2069         {
2070                 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
2071                 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
2072         }
2073         for(i=0,ni=m_faces.size();i<ni;++i)
2074         {
2075                 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
2076                 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
2077                 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
2078                 if(m_faces[i].m_leaf)
2079                 {
2080                         m_faces[i].m_leaf->data=*(void**)&i;
2081                 }
2082         }
2083         for(i=0,ni=m_anchors.size();i<ni;++i)
2084         {
2085                 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
2086         }
2087         for(i=0,ni=m_notes.size();i<ni;++i)
2088         {
2089                 for(int j=0;j<m_notes[i].m_rank;++j)
2090                 {
2091                         m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
2092                 }
2093         }
2094 #undef  PTR2IDX
2095 }
2096
2097 //
2098 void                            btSoftBody::indicesToPointers(const int* map)
2099 {
2100 #define IDX2PTR(_p_,_b_)        map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
2101         (&(_b_)[(((char*)_p_)-(char*)0)])
2102         btSoftBody::Node*       base=m_nodes.size() ? &m_nodes[0]:0;
2103         int i,ni;
2104
2105         for(i=0,ni=m_nodes.size();i<ni;++i)
2106         {
2107                 if(m_nodes[i].m_leaf)
2108                 {
2109                         m_nodes[i].m_leaf->data=&m_nodes[i];
2110                 }
2111         }
2112         for(i=0,ni=m_links.size();i<ni;++i)
2113         {
2114                 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
2115                 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
2116         }
2117         for(i=0,ni=m_faces.size();i<ni;++i)
2118         {
2119                 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
2120                 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
2121                 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
2122                 if(m_faces[i].m_leaf)
2123                 {
2124                         m_faces[i].m_leaf->data=&m_faces[i];
2125                 }
2126         }
2127         for(i=0,ni=m_anchors.size();i<ni;++i)
2128         {
2129                 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
2130         }
2131         for(i=0,ni=m_notes.size();i<ni;++i)
2132         {
2133                 for(int j=0;j<m_notes[i].m_rank;++j)
2134                 {
2135                         m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
2136                 }
2137         }
2138 #undef  IDX2PTR
2139 }
2140
2141 //
2142 int                                     btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
2143                                                                                 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
2144 {
2145         int     cnt=0;
2146         btVector3 dir = rayTo-rayFrom;
2147         
2148
2149         if(bcountonly||m_fdbvt.empty())
2150         {/* Full search */ 
2151                 
2152                 for(int i=0,ni=m_faces.size();i<ni;++i)
2153                 {
2154                         const btSoftBody::Face& f=m_faces[i];
2155
2156                         const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
2157                                 f.m_n[0]->m_x,
2158                                 f.m_n[1]->m_x,
2159                                 f.m_n[2]->m_x,
2160                                 mint);
2161                         if(t>0)
2162                         {
2163                                 ++cnt;
2164                                 if(!bcountonly)
2165                                 {
2166                                         feature=btSoftBody::eFeature::Face;
2167                                         index=i;
2168                                         mint=t;
2169                                 }
2170                         }
2171                 }
2172         }
2173         else
2174         {/* Use dbvt    */ 
2175                 RayFromToCaster collider(rayFrom,rayTo,mint);
2176
2177                 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
2178                 if(collider.m_face)
2179                 {
2180                         mint=collider.m_mint;
2181                         feature=btSoftBody::eFeature::Face;
2182                         index=(int)(collider.m_face-&m_faces[0]);
2183                         cnt=1;
2184                 }
2185         }
2186
2187         for (int i=0;i<m_tetras.size();i++)
2188         {
2189                 const btSoftBody::Tetra& tet = m_tetras[i];
2190                 int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
2191                 for (int f=0;f<4;f++)
2192                 {
2193
2194                         int index0=tetfaces[f][0];
2195                         int index1=tetfaces[f][1];
2196                         int index2=tetfaces[f][2];
2197                         btVector3 v0=tet.m_n[index0]->m_x;
2198                         btVector3 v1=tet.m_n[index1]->m_x;
2199                         btVector3 v2=tet.m_n[index2]->m_x;
2200
2201
2202                 const btScalar                  t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
2203                         v0,v1,v2,
2204                                 mint);
2205                 if(t>0)
2206                         {
2207                                 ++cnt;
2208                                 if(!bcountonly)
2209                                 {
2210                                         feature=btSoftBody::eFeature::Tetra;
2211                                         index=i;
2212                                         mint=t;
2213                                 }
2214                         }
2215                 }
2216         }
2217         return(cnt);
2218 }
2219
2220 //
2221 void                    btSoftBody::initializeFaceTree()
2222 {
2223         m_fdbvt.clear();
2224         for(int i=0;i<m_faces.size();++i)
2225         {
2226                 Face&   f=m_faces[i];
2227                 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
2228         }
2229 }
2230
2231 //
2232 btVector3               btSoftBody::evaluateCom() const
2233 {
2234         btVector3       com(0,0,0);
2235         if(m_pose.m_bframe)
2236         {
2237                 for(int i=0,ni=m_nodes.size();i<ni;++i)
2238                 {
2239                         com+=m_nodes[i].m_x*m_pose.m_wgh[i];
2240                 }
2241         }
2242         return(com);
2243 }
2244
2245 //
2246 bool                            btSoftBody::checkContact(       const btCollisionObjectWrapper* colObjWrap,
2247                                                                                          const btVector3& x,
2248                                                                                          btScalar margin,
2249                                                                                          btSoftBody::sCti& cti) const
2250 {
2251         btVector3 nrm;
2252         const btCollisionShape *shp = colObjWrap->getCollisionShape();
2253 //      const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2254         //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2255         const btTransform &wtr = colObjWrap->getWorldTransform();
2256         //todo: check which transform is needed here
2257
2258         btScalar dst = 
2259                 m_worldInfo->m_sparsesdf.Evaluate(      
2260                         wtr.invXform(x),
2261                         shp,
2262                         nrm,
2263                         margin);
2264         if(dst<0)
2265         {
2266                 cti.m_colObj = colObjWrap->getCollisionObject();
2267                 cti.m_normal = wtr.getBasis()*nrm;
2268                 cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
2269                 return(true);
2270         }
2271         return(false);
2272 }
2273
2274 //
2275 void                                    btSoftBody::updateNormals()
2276 {
2277
2278         const btVector3 zv(0,0,0);
2279         int i,ni;
2280
2281         for(i=0,ni=m_nodes.size();i<ni;++i)
2282         {
2283                 m_nodes[i].m_n=zv;
2284         }
2285         for(i=0,ni=m_faces.size();i<ni;++i)
2286         {
2287                 btSoftBody::Face&       f=m_faces[i];
2288                 const btVector3         n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
2289                         f.m_n[2]->m_x-f.m_n[0]->m_x);
2290                 f.m_normal=n.normalized();
2291                 f.m_n[0]->m_n+=n;
2292                 f.m_n[1]->m_n+=n;
2293                 f.m_n[2]->m_n+=n;
2294         }
2295         for(i=0,ni=m_nodes.size();i<ni;++i)
2296         {
2297                 btScalar len = m_nodes[i].m_n.length();
2298                 if (len>SIMD_EPSILON)
2299                         m_nodes[i].m_n /= len;
2300         }
2301 }
2302
2303 //
2304 void                                    btSoftBody::updateBounds()
2305 {
2306         /*if( m_acceleratedSoftBody )
2307         {
2308                 // If we have an accelerated softbody we need to obtain the bounds correctly
2309                 // For now (slightly hackily) just have a very large AABB
2310                 // TODO: Write get bounds kernel
2311                 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2312                 // probably do a test and exchange reasonably efficiently.
2313
2314                 m_bounds[0] = btVector3(-1000, -1000, -1000);
2315                 m_bounds[1] = btVector3(1000, 1000, 1000);
2316
2317         } else {*/
2318                 if(m_ndbvt.m_root)
2319                 {
2320                         const btVector3&        mins=m_ndbvt.m_root->volume.Mins();
2321                         const btVector3&        maxs=m_ndbvt.m_root->volume.Maxs();
2322                         const btScalar          csm=getCollisionShape()->getMargin();
2323                         const btVector3         mrg=btVector3(  csm,
2324                                 csm,
2325                                 csm)*1; // ??? to investigate...
2326                         m_bounds[0]=mins-mrg;
2327                         m_bounds[1]=maxs+mrg;
2328                         if(0!=getBroadphaseHandle())
2329                         {                                       
2330                                 m_worldInfo->m_broadphase->setAabb(     getBroadphaseHandle(),
2331                                         m_bounds[0],
2332                                         m_bounds[1],
2333                                         m_worldInfo->m_dispatcher);
2334                         }
2335                 }
2336                 else
2337                 {
2338                         m_bounds[0]=
2339                                 m_bounds[1]=btVector3(0,0,0);
2340                 }               
2341         //}
2342 }
2343
2344
2345 //
2346 void                                    btSoftBody::updatePose()
2347 {
2348         if(m_pose.m_bframe)
2349         {
2350                 btSoftBody::Pose&       pose=m_pose;
2351                 const btVector3         com=evaluateCom();
2352                 /* Com                  */ 
2353                 pose.m_com      =       com;
2354                 /* Rotation             */ 
2355                 btMatrix3x3             Apq;
2356                 const btScalar  eps=SIMD_EPSILON;
2357                 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2358                 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2359                 for(int i=0,ni=m_nodes.size();i<ni;++i)
2360                 {
2361                         const btVector3         a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2362                         const btVector3&        b=pose.m_pos[i];
2363                         Apq[0]+=a.x()*b;
2364                         Apq[1]+=a.y()*b;
2365                         Apq[2]+=a.z()*b;
2366                 }
2367                 btMatrix3x3             r,s;
2368                 PolarDecompose(Apq,r,s);
2369                 pose.m_rot=r;
2370                 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2371                 if(m_cfg.maxvolume>1)
2372                 {
2373                         const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
2374                                 1,m_cfg.maxvolume);
2375                         pose.m_scl=Mul(pose.m_scl,idet);
2376                 }
2377
2378         }
2379 }
2380
2381 //
2382 void                            btSoftBody::updateArea(bool averageArea)
2383 {
2384         int i,ni;
2385
2386         /* Face area            */ 
2387         for(i=0,ni=m_faces.size();i<ni;++i)
2388         {
2389                 Face&           f=m_faces[i];
2390                 f.m_ra  =       AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2391         }
2392         
2393         /* Node area            */ 
2394
2395         if (averageArea)
2396         {
2397                 btAlignedObjectArray<int>       counts;
2398                 counts.resize(m_nodes.size(),0);
2399                 for(i=0,ni=m_nodes.size();i<ni;++i)
2400                 {
2401                         m_nodes[i].m_area       =       0;
2402                 }
2403                 for(i=0,ni=m_faces.size();i<ni;++i)
2404                 {
2405                         btSoftBody::Face&       f=m_faces[i];
2406                         for(int j=0;j<3;++j)
2407                         {
2408                                 const int index=(int)(f.m_n[j]-&m_nodes[0]);
2409                                 counts[index]++;
2410                                 f.m_n[j]->m_area+=btFabs(f.m_ra);
2411                         }
2412                 }
2413                 for(i=0,ni=m_nodes.size();i<ni;++i)
2414                 {
2415                         if(counts[i]>0)
2416                                 m_nodes[i].m_area/=(btScalar)counts[i];
2417                         else
2418                                 m_nodes[i].m_area=0;
2419                 }
2420         }
2421         else
2422         {
2423                 // initialize node area as zero
2424                 for(i=0,ni=m_nodes.size();i<ni;++i)
2425                 {
2426                         m_nodes[i].m_area=0;    
2427                 }
2428
2429                 for(i=0,ni=m_faces.size();i<ni;++i)
2430                 {
2431                         btSoftBody::Face&       f=m_faces[i];
2432
2433                         for(int j=0;j<3;++j)
2434                         {
2435                                 f.m_n[j]->m_area += f.m_ra;
2436                         }
2437                 }
2438
2439                 for(i=0,ni=m_nodes.size();i<ni;++i)
2440                 {
2441                         m_nodes[i].m_area *= 0.3333333f;
2442                 }
2443         }
2444 }
2445
2446
2447 void                            btSoftBody::updateLinkConstants()
2448 {       
2449         int i,ni;
2450
2451         /* Links                */ 
2452         for(i=0,ni=m_links.size();i<ni;++i)
2453         {
2454                 Link&           l=m_links[i];
2455                 Material&       m=*l.m_material;
2456                 l.m_c0  =       (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2457         }
2458 }
2459
2460 void                            btSoftBody::updateConstants()
2461 {
2462         resetLinkRestLengths();
2463         updateLinkConstants();
2464         updateArea();
2465 }
2466
2467
2468
2469 //
2470 void                                    btSoftBody::initializeClusters()
2471 {
2472         int i;
2473
2474         for( i=0;i<m_clusters.size();++i)
2475         {
2476                 Cluster&        c=*m_clusters[i];
2477                 c.m_imass=0;
2478                 c.m_masses.resize(c.m_nodes.size());
2479                 for(int j=0;j<c.m_nodes.size();++j)
2480                 {
2481                         if (c.m_nodes[j]->m_im==0)
2482                         {
2483                                 c.m_containsAnchor = true;
2484                                 c.m_masses[j]   =       BT_LARGE_FLOAT;
2485                         } else
2486                         {
2487                                 c.m_masses[j]   =       btScalar(1.)/c.m_nodes[j]->m_im;
2488                         }
2489                         c.m_imass               +=      c.m_masses[j];
2490                 }
2491                 c.m_imass               =       btScalar(1.)/c.m_imass;
2492                 c.m_com                 =       btSoftBody::clusterCom(&c);
2493                 c.m_lv                  =       btVector3(0,0,0);
2494                 c.m_av                  =       btVector3(0,0,0);
2495                 c.m_leaf                =       0;
2496                 /* Inertia      */ 
2497                 btMatrix3x3&    ii=c.m_locii;
2498                 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2499                 {
2500                         int i,ni;
2501
2502                         for(i=0,ni=c.m_nodes.size();i<ni;++i)
2503                         {
2504                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2505                                 const btVector3 q=k*k;
2506                                 const btScalar  m=c.m_masses[i];
2507                                 ii[0][0]        +=      m*(q[1]+q[2]);
2508                                 ii[1][1]        +=      m*(q[0]+q[2]);
2509                                 ii[2][2]        +=      m*(q[0]+q[1]);
2510                                 ii[0][1]        -=      m*k[0]*k[1];
2511                                 ii[0][2]        -=      m*k[0]*k[2];
2512                                 ii[1][2]        -=      m*k[1]*k[2];
2513                         }
2514                 }
2515                 ii[1][0]=ii[0][1];
2516                 ii[2][0]=ii[0][2];
2517                 ii[2][1]=ii[1][2];
2518                 
2519                 ii = ii.inverse();
2520
2521                 /* Frame        */ 
2522                 c.m_framexform.setIdentity();
2523                 c.m_framexform.setOrigin(c.m_com);
2524                 c.m_framerefs.resize(c.m_nodes.size());
2525                 {
2526                         int i;
2527                         for(i=0;i<c.m_framerefs.size();++i)
2528                         {
2529                                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2530                         }
2531                 }
2532         }
2533 }
2534
2535 //
2536 void                                    btSoftBody::updateClusters()
2537 {
2538         BT_PROFILE("UpdateClusters");
2539         int i;
2540
2541         for(i=0;i<m_clusters.size();++i)
2542         {
2543                 btSoftBody::Cluster&    c=*m_clusters[i];
2544                 const int                               n=c.m_nodes.size();
2545                 //const btScalar                        invn=1/(btScalar)n;
2546                 if(n)
2547                 {
2548                         /* Frame                                */ 
2549                         const btScalar  eps=btScalar(0.0001);
2550                         btMatrix3x3             m,r,s;
2551                         m[0]=m[1]=m[2]=btVector3(0,0,0);
2552                         m[0][0]=eps*1;
2553                         m[1][1]=eps*2;
2554                         m[2][2]=eps*3;
2555                         c.m_com=clusterCom(&c);
2556                         for(int i=0;i<c.m_nodes.size();++i)
2557                         {
2558                                 const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
2559                                 const btVector3&        b=c.m_framerefs[i];
2560                                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2561                         }
2562                         PolarDecompose(m,r,s);
2563                         c.m_framexform.setOrigin(c.m_com);
2564                         c.m_framexform.setBasis(r);             
2565                         /* Inertia                      */ 
2566 #if 1/* Constant        */ 
2567                         c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2568 #else
2569 #if 0/* Sphere  */ 
2570                         const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
2571                         const btVector3 inertia(rk,rk,rk);
2572                         const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2573                                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2574                                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2575
2576                         c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2577 #else/* Actual  */              
2578                         c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2579                         for(int i=0;i<n;++i)
2580                         {
2581                                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2582                                 const btVector3         q=k*k;
2583                                 const btScalar          m=1/c.m_nodes[i]->m_im;
2584                                 c.m_invwi[0][0] +=      m*(q[1]+q[2]);
2585                                 c.m_invwi[1][1] +=      m*(q[0]+q[2]);
2586                                 c.m_invwi[2][2] +=      m*(q[0]+q[1]);
2587                                 c.m_invwi[0][1] -=      m*k[0]*k[1];
2588                                 c.m_invwi[0][2] -=      m*k[0]*k[2];
2589                                 c.m_invwi[1][2] -=      m*k[1]*k[2];
2590                         }
2591                         c.m_invwi[1][0]=c.m_invwi[0][1];
2592                         c.m_invwi[2][0]=c.m_invwi[0][2];
2593                         c.m_invwi[2][1]=c.m_invwi[1][2];
2594                         c.m_invwi=c.m_invwi.inverse();
2595 #endif
2596 #endif
2597                         /* Velocities                   */ 
2598                         c.m_lv=btVector3(0,0,0);
2599                         c.m_av=btVector3(0,0,0);
2600                         {
2601                                 int i;
2602
2603                                 for(i=0;i<n;++i)
2604                                 {
2605                                         const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2606                                         c.m_lv  +=      v;
2607                                         c.m_av  +=      btCross(c.m_nodes[i]->m_x-c.m_com,v);
2608                                 }
2609                         }
2610                         c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2611                         c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2612                         c.m_vimpulses[0]        =
2613                                 c.m_vimpulses[1]        = btVector3(0,0,0);
2614                         c.m_dimpulses[0]        =
2615                                 c.m_dimpulses[1]        = btVector3(0,0,0);
2616                         c.m_nvimpulses          = 0;
2617                         c.m_ndimpulses          = 0;
2618                         /* Matching                             */ 
2619                         if(c.m_matching>0)
2620                         {
2621                                 for(int j=0;j<c.m_nodes.size();++j)
2622                                 {
2623                                         Node&                   n=*c.m_nodes[j];
2624                                         const btVector3 x=c.m_framexform*c.m_framerefs[j];
2625                                         n.m_x=Lerp(n.m_x,x,c.m_matching);
2626                                 }
2627                         }                       
2628                         /* Dbvt                                 */ 
2629                         if(c.m_collide)
2630                         {
2631                                 btVector3       mi=c.m_nodes[0]->m_x;
2632                                 btVector3       mx=mi;
2633                                 for(int j=1;j<n;++j)
2634                                 {
2635                                         mi.setMin(c.m_nodes[j]->m_x);
2636                                         mx.setMax(c.m_nodes[j]->m_x);
2637                                 }                       
2638                                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(mi,mx);
2639                                 if(c.m_leaf)
2640                                         m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2641                                 else
2642                                         c.m_leaf=m_cdbvt.insert(bounds,&c);
2643                         }
2644                 }
2645         }
2646
2647
2648 }
2649
2650
2651
2652
2653 //
2654 void                                    btSoftBody::cleanupClusters()
2655 {
2656         for(int i=0;i<m_joints.size();++i)
2657         {
2658                 m_joints[i]->Terminate(m_sst.sdt);
2659                 if(m_joints[i]->m_delete)
2660                 {
2661                         btAlignedFree(m_joints[i]);
2662                         m_joints.remove(m_joints[i--]);
2663                 }       
2664         }
2665 }
2666
2667 //
2668 void                                    btSoftBody::prepareClusters(int iterations)
2669 {
2670         for(int i=0;i<m_joints.size();++i)
2671         {
2672                 m_joints[i]->Prepare(m_sst.sdt,iterations);
2673         }
2674 }
2675
2676
2677 //
2678 void                                    btSoftBody::solveClusters(btScalar sor)
2679 {
2680         for(int i=0,ni=m_joints.size();i<ni;++i)
2681         {
2682                 m_joints[i]->Solve(m_sst.sdt,sor);
2683         }
2684 }
2685
2686 //
2687 void                                    btSoftBody::applyClusters(bool drift)
2688 {
2689         BT_PROFILE("ApplyClusters");
2690 //      const btScalar                                  f0=m_sst.sdt;
2691         //const btScalar                                        f1=f0/2;
2692         btAlignedObjectArray<btVector3> deltas;
2693         btAlignedObjectArray<btScalar> weights;
2694         deltas.resize(m_nodes.size(),btVector3(0,0,0));
2695         weights.resize(m_nodes.size(),0);
2696         int i;
2697
2698         if(drift)
2699         {
2700                 for(i=0;i<m_clusters.size();++i)
2701                 {
2702                         Cluster&        c=*m_clusters[i];
2703                         if(c.m_ndimpulses)
2704                         {
2705                                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2706                                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2707                         }
2708                 }
2709         }
2710         
2711         for(i=0;i<m_clusters.size();++i)
2712         {
2713                 Cluster&        c=*m_clusters[i];       
2714                 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2715                 {
2716                         const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2717                         const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2718                         for(int j=0;j<c.m_nodes.size();++j)
2719                         {
2720                                 const int                       idx=int(c.m_nodes[j]-&m_nodes[0]);
2721                                 const btVector3&        x=c.m_nodes[j]->m_x;
2722                                 const btScalar          q=c.m_masses[j];
2723                                 deltas[idx]             +=      (v+btCross(w,x-c.m_com))*q;
2724                                 weights[idx]    +=      q;
2725                         }
2726                 }
2727         }
2728         for(i=0;i<deltas.size();++i)
2729         {
2730                 if(weights[i]>0) 
2731                 {
2732                         m_nodes[i].m_x+=deltas[i]/weights[i];
2733                 }
2734         }
2735 }
2736
2737 //
2738 void                                    btSoftBody::dampClusters()
2739 {
2740         int i;
2741
2742         for(i=0;i<m_clusters.size();++i)
2743         {
2744                 Cluster&        c=*m_clusters[i];       
2745                 if(c.m_ndamping>0)
2746                 {
2747                         for(int j=0;j<c.m_nodes.size();++j)
2748                         {
2749                                 Node&                   n=*c.m_nodes[j];
2750                                 if(n.m_im>0)
2751                                 {
2752                                         const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2753                                         if(vx.length2()<=n.m_v.length2())
2754                                                 {
2755                                                 n.m_v   +=      c.m_ndamping*(vx-n.m_v);
2756                                                 }
2757                                 }
2758                         }
2759                 }
2760         }
2761 }
2762
2763 //
2764 void                            btSoftBody::Joint::Prepare(btScalar dt,int)
2765 {
2766         m_bodies[0].activate();
2767         m_bodies[1].activate();
2768 }
2769
2770 //
2771 void                            btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2772 {
2773         static const btScalar   maxdrift=4;
2774         Joint::Prepare(dt,iterations);
2775         m_rpos[0]               =       m_bodies[0].xform()*m_refs[0];
2776         m_rpos[1]               =       m_bodies[1].xform()*m_refs[1];
2777         m_drift                 =       Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2778         m_rpos[0]               -=      m_bodies[0].xform().getOrigin();
2779         m_rpos[1]               -=      m_bodies[1].xform().getOrigin();
2780         m_massmatrix    =       ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2781                 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2782         if(m_split>0)
2783         {
2784                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2785                 m_drift         *=      1-m_split;
2786         }
2787         m_drift /=(btScalar)iterations;
2788 }
2789
2790 //
2791 void                            btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2792 {
2793         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2794         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2795         const btVector3         vr=va-vb;
2796         btSoftBody::Impulse     impulse;
2797         impulse.m_asVelocity    =       1;
2798         impulse.m_velocity              =       m_massmatrix*(m_drift+vr*m_cfm)*sor;
2799         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2800         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2801 }
2802
2803 //
2804 void                            btSoftBody::LJoint::Terminate(btScalar dt)
2805 {
2806         if(m_split>0)
2807         {
2808                 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2809                 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2810         }
2811 }
2812
2813 //
2814 void                            btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2815 {
2816         static const btScalar   maxdrift=SIMD_PI/16;
2817         m_icontrol->Prepare(this);
2818         Joint::Prepare(dt,iterations);
2819         m_axis[0]       =       m_bodies[0].xform().getBasis()*m_refs[0];
2820         m_axis[1]       =       m_bodies[1].xform().getBasis()*m_refs[1];
2821         m_drift         =       NormalizeAny(btCross(m_axis[1],m_axis[0]));
2822         m_drift         *=      btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2823         m_drift         *=      m_erp/dt;
2824         m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2825         if(m_split>0)
2826         {
2827                 m_sdrift        =       m_massmatrix*(m_drift*m_split);
2828                 m_drift         *=      1-m_split;
2829         }
2830         m_drift /=(btScalar)iterations;
2831 }
2832
2833 //
2834 void                            btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2835 {
2836         const btVector3         va=m_bodies[0].angularVelocity();
2837         const btVector3         vb=m_bodies[1].angularVelocity();
2838         const btVector3         vr=va-vb;
2839         const btScalar          sp=btDot(vr,m_axis[0]);
2840         const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2841         btSoftBody::Impulse     impulse;
2842         impulse.m_asVelocity    =       1;
2843         impulse.m_velocity              =       m_massmatrix*(m_drift+vc*m_cfm)*sor;
2844         m_bodies[0].applyAImpulse(-impulse);
2845         m_bodies[1].applyAImpulse( impulse);
2846 }
2847
2848 //
2849 void                            btSoftBody::AJoint::Terminate(btScalar dt)
2850 {
2851         if(m_split>0)
2852         {
2853                 m_bodies[0].applyDAImpulse(-m_sdrift);
2854                 m_bodies[1].applyDAImpulse( m_sdrift);
2855         }
2856 }
2857
2858 //
2859 void                            btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2860 {
2861         Joint::Prepare(dt,iterations);
2862         const bool      dodrift=(m_life==0);
2863         m_delete=(++m_life)>m_maxlife;
2864         if(dodrift)
2865         {
2866                 m_drift=m_drift*m_erp/dt;
2867                 if(m_split>0)
2868                 {
2869                         m_sdrift        =       m_massmatrix*(m_drift*m_split);
2870                         m_drift         *=      1-m_split;
2871                 }
2872                 m_drift/=(btScalar)iterations;
2873         }
2874         else
2875         {
2876                 m_drift=m_sdrift=btVector3(0,0,0);
2877         }
2878 }
2879
2880 //
2881 void                            btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2882 {
2883         const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
2884         const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
2885         const btVector3         vrel=va-vb;
2886         const btScalar          rvac=btDot(vrel,m_normal);
2887         btSoftBody::Impulse     impulse;
2888         impulse.m_asVelocity    =       1;
2889         impulse.m_velocity              =       m_drift;
2890         if(rvac<0)
2891         {
2892                 const btVector3 iv=m_normal*rvac;
2893                 const btVector3 fv=vrel-iv;
2894                 impulse.m_velocity      +=      iv+fv*m_friction;
2895         }
2896         impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2897         
2898         if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2899         {
2900                 if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2901                         (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2902                 {
2903                         if (impulse.m_asVelocity)
2904                         {
2905                                 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2906                                 {
2907                                         
2908                                 } else
2909                                 {
2910                                         m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2911                                         m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2912                                 }
2913                         }
2914                 }
2915         } else