Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago
Added Bullet physics engine.
Property svn:eol-style set to `native`
File size: 59.5 KB

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

Note: See TracBrowser for help on using the repository browser.

Download in other formats: