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