Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 2056 was 1972, checked in by rgrieder, 16 years ago
Downgraded Bullet to latest tagged version: 2.72 That should give us more stability.
Property svn:eol-style set to `native`
File size: 58.9 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::rayCast(const btVector3& org,
1226	const btVector3& dir,
1227	sRayCast& results,
1228	btScalar maxtime)
1229	{
1230	if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree();
1231	results.body = this;
1232	results.time = maxtime;
1233	results.feature = eFeature::None;
1234	results.index = -1;
1235	return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0);
1236	}
1237
1238	//
1239	void btSoftBody::setSolver(eSolverPresets::_ preset)
1240	{
1241	m_cfg.m_vsequence.clear();
1242	m_cfg.m_psequence.clear();
1243	m_cfg.m_dsequence.clear();
1244	switch(preset)
1245	{
1246	case eSolverPresets::Positions:
1247	m_cfg.m_psequence.push_back(ePSolver::Anchors);
1248	m_cfg.m_psequence.push_back(ePSolver::RContacts);
1249	m_cfg.m_psequence.push_back(ePSolver::SContacts);
1250	m_cfg.m_psequence.push_back(ePSolver::Linear);
1251	break;
1252	case eSolverPresets::Velocities:
1253	m_cfg.m_vsequence.push_back(eVSolver::Linear);
1254
1255	m_cfg.m_psequence.push_back(ePSolver::Anchors);
1256	m_cfg.m_psequence.push_back(ePSolver::RContacts);
1257	m_cfg.m_psequence.push_back(ePSolver::SContacts);
1258
1259	m_cfg.m_dsequence.push_back(ePSolver::Linear);
1260	break;
1261	}
1262	}
1263
1264	//
1265	void btSoftBody::predictMotion(btScalar dt)
1266	{
1267	int i,ni;
1268
1269	/* Update */
1270	if(m_bUpdateRtCst)
1271	{
1272	m_bUpdateRtCst=false;
1273	updateConstants();
1274	m_fdbvt.clear();
1275	if(m_cfg.collisions&fCollision::VF_SS)
1276	{
1277	initializeFaceTree();
1278	}
1279	}
1280
1281	/* Prepare */
1282	m_sst.sdt = dt*m_cfg.timescale;
1283	m_sst.isdt = 1/m_sst.sdt;
1284	m_sst.velmrg = m_sst.sdt*3;
1285	m_sst.radmrg = getCollisionShape()->getMargin();
1286	m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
1287	/* Forces */
1288	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1289	applyForces();
1290	/* Integrate */
1291	for(i=0,ni=m_nodes.size();i<ni;++i)
1292	{
1293	Node& n=m_nodes[i];
1294	n.m_q = n.m_x;
1295	n.m_v += n.m_fn.m_imm_sst.sdt;
1296	n.m_x += n.m_v*m_sst.sdt;
1297	n.m_f = btVector3(0,0,0);
1298	}
1299	/* Clusters */
1300	updateClusters();
1301	/* Bounds */
1302	updateBounds();
1303	/* Nodes */
1304	for(i=0,ni=m_nodes.size();i<ni;++i)
1305	{
1306	Node& n=m_nodes[i];
1307	m_ndbvt.update( n.m_leaf,
1308	btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
1309	n.m_v*m_sst.velmrg,
1310	m_sst.updmrg);
1311	}
1312	/* Faces */
1313	if(!m_fdbvt.empty())
1314	{
1315	for(int i=0;i<m_faces.size();++i)
1316	{
1317	Face& f=m_faces[i];
1318	const btVector3 v=( f.m_n[0]->m_v+
1319	f.m_n[1]->m_v+
1320	f.m_n[2]->m_v)/3;
1321	m_fdbvt.update( f.m_leaf,
1322	VolumeOf(f,m_sst.radmrg),
1323	v*m_sst.velmrg,
1324	m_sst.updmrg);
1325	}
1326	}
1327	/* Pose */
1328	updatePose();
1329	/* Match */
1330	if(m_pose.m_bframe&&(m_cfg.kMT>0))
1331	{
1332	const btMatrix3x3 posetrs=m_pose.m_rot;
1333	for(int i=0,ni=m_nodes.size();i<ni;++i)
1334	{
1335	Node& n=m_nodes[i];
1336	if(n.m_im>0)
1337	{
1338	const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1339	n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1340	}
1341	}
1342	}
1343	/* Clear contacts */
1344	m_rcontacts.resize(0);
1345	m_scontacts.resize(0);
1346	/* Optimize dbvt's */
1347	m_ndbvt.optimizeIncremental(1);
1348	m_fdbvt.optimizeIncremental(1);
1349	m_cdbvt.optimizeIncremental(1);
1350	}
1351
1352	//
1353	void btSoftBody::solveConstraints()
1354	{
1355	/* Apply clusters */
1356	applyClusters(false);
1357	/* Prepare links */
1358
1359	int i,ni;
1360
1361	for(i=0,ni=m_links.size();i<ni;++i)
1362	{
1363	Link& l=m_links[i];
1364	l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
1365	l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
1366	}
1367	/* Prepare anchors */
1368	for(i=0,ni=m_anchors.size();i<ni;++i)
1369	{
1370	Anchor& a=m_anchors[i];
1371	const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1372	a.m_c0 = ImpulseMatrix( m_sst.sdt,
1373	a.m_node->m_im,
1374	a.m_body->getInvMass(),
1375	a.m_body->getInvInertiaTensorWorld(),
1376	ra);
1377	a.m_c1 = ra;
1378	a.m_c2 = m_sst.sdt*a.m_node->m_im;
1379	a.m_body->activate();
1380	}
1381	/* Solve velocities */
1382	if(m_cfg.viterations>0)
1383	{
1384	/* Solve */
1385	for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1386	{
1387	for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1388	{
1389	getSolver(m_cfg.m_vsequence[iseq])(this,1);
1390	}
1391	}
1392	/* Update */
1393	for(i=0,ni=m_nodes.size();i<ni;++i)
1394	{
1395	Node& n=m_nodes[i];
1396	n.m_x = n.m_q+n.m_v*m_sst.sdt;
1397	}
1398	}
1399	/* Solve positions */
1400	if(m_cfg.piterations>0)
1401	{
1402	for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1403	{
1404	const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1405	for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1406	{
1407	getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1408	}
1409	}
1410	const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
1411	for(i=0,ni=m_nodes.size();i<ni;++i)
1412	{
1413	Node& n=m_nodes[i];
1414	n.m_v = (n.m_x-n.m_q)*vc;
1415	n.m_f = btVector3(0,0,0);
1416	}
1417	}
1418	/* Solve drift */
1419	if(m_cfg.diterations>0)
1420	{
1421	const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
1422	for(i=0,ni=m_nodes.size();i<ni;++i)
1423	{
1424	Node& n=m_nodes[i];
1425	n.m_q = n.m_x;
1426	}
1427	for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1428	{
1429	for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1430	{
1431	getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1432	}
1433	}
1434	for(int i=0,ni=m_nodes.size();i<ni;++i)
1435	{
1436	Node& n=m_nodes[i];
1437	n.m_v += (n.m_x-n.m_q)*vcf;
1438	}
1439	}
1440	/* Apply clusters */
1441	dampClusters();
1442	applyClusters(true);
1443	}
1444
1445	//
1446	void btSoftBody::staticSolve(int iterations)
1447	{
1448	for(int isolve=0;isolve<iterations;++isolve)
1449	{
1450	for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1451	{
1452	getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1453	}
1454	}
1455	}
1456
1457	//
1458	void btSoftBody::solveCommonConstraints(btSoftBody** /bodies/,int /count/,int /iterations/)
1459	{
1460	/// placeholder
1461	}
1462
1463	//
1464	void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1465	{
1466	const int nb=bodies.size();
1467	int iterations=0;
1468	int i;
1469
1470	for(i=0;i<nb;++i)
1471	{
1472	iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1473	}
1474	for(i=0;i<nb;++i)
1475	{
1476	bodies[i]->prepareClusters(iterations);
1477	}
1478	for(i=0;i<iterations;++i)
1479	{
1480	const btScalar sor=1;
1481	for(int j=0;j<nb;++j)
1482	{
1483	bodies[j]->solveClusters(sor);
1484	}
1485	}
1486	for(i=0;i<nb;++i)
1487	{
1488	bodies[i]->cleanupClusters();
1489	}
1490	}
1491
1492	//
1493	void btSoftBody::integrateMotion()
1494	{
1495	/* Update */
1496	updateNormals();
1497	}
1498
1499	//
1500	btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
1501	{
1502	o = org;
1503	d = dir;
1504	mint = mxt;
1505	face = 0;
1506	tests = 0;
1507	}
1508
1509	//
1510	void btSoftBody::RayCaster::Process(const btDbvtNode* leaf)
1511	{
1512	btSoftBody::Face& f=(btSoftBody::Face)leaf->data;
1513	const btScalar t=rayTriangle( o,d,
1514	f.m_n[0]->m_x,
1515	f.m_n[1]->m_x,
1516	f.m_n[2]->m_x,
1517	mint);
1518	if((t>0)&&(t<mint)) { mint=t;face=&f; }
1519	++tests;
1520	}
1521
1522	//
1523	btScalar btSoftBody::RayCaster::rayTriangle( const btVector3& org,
1524	const btVector3& dir,
1525	const btVector3& a,
1526	const btVector3& b,
1527	const btVector3& c,
1528	btScalar maxt)
1529	{
1530	static const btScalar ceps=-SIMD_EPSILON*10;
1531	static const btScalar teps=SIMD_EPSILON*10;
1532	const btVector3 n=cross(b-a,c-a);
1533	const btScalar d=dot(a,n);
1534	const btScalar den=dot(dir,n);
1535	if(!btFuzzyZero(den))
1536	{
1537	const btScalar num=dot(org,n)-d;
1538	const btScalar t=-num/den;
1539	if((t>teps)&&(t<maxt))
1540	{
1541	const btVector3 hit=org+dir*t;
1542	if( (dot(n,cross(a-hit,b-hit))>ceps) &&
1543	(dot(n,cross(b-hit,c-hit))>ceps) &&
1544	(dot(n,cross(c-hit,a-hit))>ceps))
1545	{
1546	return(t);
1547	}
1548	}
1549	}
1550	return(-1);
1551	}
1552
1553	//
1554	void btSoftBody::pointersToIndices()
1555	{
1556	#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
1557	btSoftBody::Node* base=&m_nodes[0];
1558	int i,ni;
1559
1560	for(i=0,ni=m_nodes.size();i<ni;++i)
1561	{
1562	if(m_nodes[i].m_leaf)
1563	{
1564	m_nodes[i].m_leaf->data=(void*)&i;
1565	}
1566	}
1567	for(i=0,ni=m_links.size();i<ni;++i)
1568	{
1569	m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
1570	m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
1571	}
1572	for(i=0,ni=m_faces.size();i<ni;++i)
1573	{
1574	m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
1575	m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
1576	m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
1577	if(m_faces[i].m_leaf)
1578	{
1579	m_faces[i].m_leaf->data=(void*)&i;
1580	}
1581	}
1582	for(i=0,ni=m_anchors.size();i<ni;++i)
1583	{
1584	m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
1585	}
1586	for(i=0,ni=m_notes.size();i<ni;++i)
1587	{
1588	for(int j=0;j<m_notes[i].m_rank;++j)
1589	{
1590	m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
1591	}
1592	}
1593	#undef PTR2IDX
1594	}
1595
1596	//
1597	void btSoftBody::indicesToPointers(const int* map)
1598	{
1599	#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char)_p_)-(char)0)]]): \
1600	(&(_b_)[(((char)_p_)-(char)0)])
1601	btSoftBody::Node* base=&m_nodes[0];
1602	int i,ni;
1603
1604	for(i=0,ni=m_nodes.size();i<ni;++i)
1605	{
1606	if(m_nodes[i].m_leaf)
1607	{
1608	m_nodes[i].m_leaf->data=&m_nodes[i];
1609	}
1610	}
1611	for(i=0,ni=m_links.size();i<ni;++i)
1612	{
1613	m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
1614	m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
1615	}
1616	for(i=0,ni=m_faces.size();i<ni;++i)
1617	{
1618	m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
1619	m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
1620	m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
1621	if(m_faces[i].m_leaf)
1622	{
1623	m_faces[i].m_leaf->data=&m_faces[i];
1624	}
1625	}
1626	for(i=0,ni=m_anchors.size();i<ni;++i)
1627	{
1628	m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
1629	}
1630	for(i=0,ni=m_notes.size();i<ni;++i)
1631	{
1632	for(int j=0;j<m_notes[i].m_rank;++j)
1633	{
1634	m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
1635	}
1636	}
1637	#undef IDX2PTR
1638	}
1639
1640	//
1641	int btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
1642	btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
1643	{
1644	int cnt=0;
1645	if(bcountonly\|\|m_fdbvt.empty())
1646	{/* Full search */
1647	for(int i=0,ni=m_faces.size();i<ni;++i)
1648	{
1649	const btSoftBody::Face& f=m_faces[i];
1650	const btScalar t=RayCaster::rayTriangle( org,dir,
1651	f.m_n[0]->m_x,
1652	f.m_n[1]->m_x,
1653	f.m_n[2]->m_x,
1654	mint);
1655	if(t>0)
1656	{
1657	++cnt;
1658	if(!bcountonly)
1659	{
1660	feature=btSoftBody::eFeature::Face;
1661	index=i;
1662	mint=t;
1663	}
1664	}
1665	}
1666	}
1667	else
1668	{/* Use dbvt */
1669	RayCaster collider(org,dir,mint);
1670	btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider);
1671	if(collider.face)
1672	{
1673	mint=collider.mint;
1674	feature=btSoftBody::eFeature::Face;
1675	index=(int)(collider.face-&m_faces[0]);
1676	cnt=1;
1677	}
1678	}
1679	return(cnt);
1680	}
1681
1682	//
1683	void btSoftBody::initializeFaceTree()
1684	{
1685	m_fdbvt.clear();
1686	for(int i=0;i<m_faces.size();++i)
1687	{
1688	Face& f=m_faces[i];
1689	f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
1690	}
1691	}
1692
1693	//
1694	btVector3 btSoftBody::evaluateCom() const
1695	{
1696	btVector3 com(0,0,0);
1697	if(m_pose.m_bframe)
1698	{
1699	for(int i=0,ni=m_nodes.size();i<ni;++i)
1700	{
1701	com+=m_nodes[i].m_x*m_pose.m_wgh[i];
1702	}
1703	}
1704	return(com);
1705	}
1706
1707	//
1708	bool btSoftBody::checkContact( btRigidBody* prb,
1709	const btVector3& x,
1710	btScalar margin,
1711	btSoftBody::sCti& cti) const
1712	{
1713	btVector3 nrm;
1714	btCollisionShape* shp=prb->getCollisionShape();
1715	const btTransform& wtr=prb->getInterpolationWorldTransform();
1716	btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
1717	shp,
1718	nrm,
1719	margin);
1720	if(dst<0)
1721	{
1722	cti.m_body = prb;
1723	cti.m_normal = wtr.getBasis()*nrm;
1724	cti.m_offset = -dot( cti.m_normal,
1725	x-cti.m_normal*dst);
1726	return(true);
1727	}
1728	return(false);
1729	}
1730
1731	//
1732	void btSoftBody::updateNormals()
1733	{
1734	const btVector3 zv(0,0,0);
1735	int i,ni;
1736
1737	for(i=0,ni=m_nodes.size();i<ni;++i)
1738	{
1739	m_nodes[i].m_n=zv;
1740	}
1741	for(i=0,ni=m_faces.size();i<ni;++i)
1742	{
1743	btSoftBody::Face& f=m_faces[i];
1744	const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
1745	f.m_n[2]->m_x-f.m_n[0]->m_x);
1746	f.m_normal=n.normalized();
1747	f.m_n[0]->m_n+=n;
1748	f.m_n[1]->m_n+=n;
1749	f.m_n[2]->m_n+=n;
1750	}
1751	for(i=0,ni=m_nodes.size();i<ni;++i)
1752	{
1753	btScalar len = m_nodes[i].m_n.length();
1754	if (len>SIMD_EPSILON)
1755	m_nodes[i].m_n /= len;
1756	}
1757	}
1758
1759	//
1760	void btSoftBody::updateBounds()
1761	{
1762	if(m_ndbvt.m_root)
1763	{
1764	const btVector3& mins=m_ndbvt.m_root->volume.Mins();
1765	const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
1766	const btScalar csm=getCollisionShape()->getMargin();
1767	const btVector3 mrg=btVector3( csm,
1768	csm,
1769	csm)*1; // ??? to investigate...
1770	m_bounds[0]=mins-mrg;
1771	m_bounds[1]=maxs+mrg;
1772	if(0!=getBroadphaseHandle())
1773	{
1774	m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
1775	m_bounds[0],
1776	m_bounds[1],
1777	m_worldInfo->m_dispatcher);
1778	}
1779	}
1780	else
1781	{
1782	m_bounds[0]=
1783	m_bounds[1]=btVector3(0,0,0);
1784	}
1785	}
1786
1787
1788	//
1789	void btSoftBody::updatePose()
1790	{
1791	if(m_pose.m_bframe)
1792	{
1793	btSoftBody::Pose& pose=m_pose;
1794	const btVector3 com=evaluateCom();
1795	/* Com */
1796	pose.m_com = com;
1797	/* Rotation */
1798	btMatrix3x3 Apq;
1799	const btScalar eps=SIMD_EPSILON;
1800	Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
1801	Apq[0].setX(eps);Apq[1].setY(eps2);Apq[2].setZ(eps3);
1802	for(int i=0,ni=m_nodes.size();i<ni;++i)
1803	{
1804	const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
1805	const btVector3& b=pose.m_pos[i];
1806	Apq[0]+=a.x()*b;
1807	Apq[1]+=a.y()*b;
1808	Apq[2]+=a.z()*b;
1809	}
1810	btMatrix3x3 r,s;
1811	PolarDecompose(Apq,r,s);
1812	pose.m_rot=r;
1813	pose.m_scl=pose.m_aqqr.transpose()Apq;
1814	if(m_cfg.maxvolume>1)
1815	{
1816	const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
1817	1,m_cfg.maxvolume);
1818	pose.m_scl=Mul(pose.m_scl,idet);
1819	}
1820
1821	}
1822	}
1823
1824	//
1825	void btSoftBody::updateConstants()
1826	{
1827	int i,ni;
1828
1829	/* Links */
1830	for(i=0,ni=m_links.size();i<ni;++i)
1831	{
1832	Link& l=m_links[i];
1833	Material& m=*l.m_material;
1834	l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
1835	l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
1836	l.m_c1 = l.m_rl*l.m_rl;
1837	}
1838	/* Faces */
1839	for(i=0,ni=m_faces.size();i<ni;++i)
1840	{
1841	Face& f=m_faces[i];
1842	f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
1843	}
1844	/* Area's */
1845	btAlignedObjectArray<int> counts;
1846	counts.resize(m_nodes.size(),0);
1847	for(i=0,ni=m_nodes.size();i<ni;++i)
1848	{
1849	m_nodes[i].m_area = 0;
1850	}
1851	for(i=0,ni=m_faces.size();i<ni;++i)
1852	{
1853	btSoftBody::Face& f=m_faces[i];
1854	for(int j=0;j<3;++j)
1855	{
1856	const int index=(int)(f.m_n[j]-&m_nodes[0]);
1857	counts[index]++;
1858	f.m_n[j]->m_area+=btFabs(f.m_ra);
1859	}
1860	}
1861	for(i=0,ni=m_nodes.size();i<ni;++i)
1862	{
1863	if(counts[i]>0)
1864	m_nodes[i].m_area/=(btScalar)counts[i];
1865	else
1866	m_nodes[i].m_area=0;
1867	}
1868	}
1869
1870	//
1871	void btSoftBody::initializeClusters()
1872	{
1873	int i;
1874
1875	for( i=0;i<m_clusters.size();++i)
1876	{
1877	Cluster& c=*m_clusters[i];
1878	c.m_imass=0;
1879	c.m_masses.resize(c.m_nodes.size());
1880	for(int j=0;j<c.m_nodes.size();++j)
1881	{
1882	c.m_masses[j] = c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
1883	c.m_imass += c.m_masses[j];
1884	}
1885	c.m_imass = 1/c.m_imass;
1886	c.m_com = btSoftBody::clusterCom(&c);
1887	c.m_lv = btVector3(0,0,0);
1888	c.m_av = btVector3(0,0,0);
1889	c.m_leaf = 0;
1890	/* Inertia */
1891	btMatrix3x3& ii=c.m_locii;
1892	ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
1893	{
1894	int i,ni;
1895
1896	for(i=0,ni=c.m_nodes.size();i<ni;++i)
1897	{
1898	const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
1899	const btVector3 q=k*k;
1900	const btScalar m=c.m_masses[i];
1901	ii[0][0] += m*(q[1]+q[2]);
1902	ii[1][1] += m*(q[0]+q[2]);
1903	ii[2][2] += m*(q[0]+q[1]);
1904	ii[0][1] -= mk[0]k[1];
1905	ii[0][2] -= mk[0]k[2];
1906	ii[1][2] -= mk[1]k[2];
1907	}
1908	}
1909	ii[1][0]=ii[0][1];
1910	ii[2][0]=ii[0][2];
1911	ii[2][1]=ii[1][2];
1912	ii=ii.inverse();
1913	/* Frame */
1914	c.m_framexform.setIdentity();
1915	c.m_framexform.setOrigin(c.m_com);
1916	c.m_framerefs.resize(c.m_nodes.size());
1917	{
1918	int i;
1919	for(i=0;i<c.m_framerefs.size();++i)
1920	{
1921	c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
1922	}
1923	}
1924	}
1925	}
1926
1927	//
1928	void btSoftBody::updateClusters()
1929	{
1930	BT_PROFILE("UpdateClusters");
1931	int i;
1932
1933	for(i=0;i<m_clusters.size();++i)
1934	{
1935	btSoftBody::Cluster& c=*m_clusters[i];
1936	const int n=c.m_nodes.size();
1937	const btScalar invn=1/(btScalar)n;
1938	if(n)
1939	{
1940	/* Frame */
1941	const btScalar eps=btScalar(0.0001);
1942	btMatrix3x3 m,r,s;
1943	m[0]=m[1]=m[2]=btVector3(0,0,0);
1944	m[0][0]=eps*1;
1945	m[1][1]=eps*2;
1946	m[2][2]=eps*3;
1947	c.m_com=clusterCom(&c);
1948	for(int i=0;i<c.m_nodes.size();++i)
1949	{
1950	const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
1951	const btVector3& b=c.m_framerefs[i];
1952	m[0]+=a[0]b;m[1]+=a[1]b;m[2]+=a[2]*b;
1953	}
1954	PolarDecompose(m,r,s);
1955	c.m_framexform.setOrigin(c.m_com);
1956	c.m_framexform.setBasis(r);
1957	/* Inertia */
1958	#if 1/* Constant */
1959	c.m_invwi=c.m_framexform.getBasis()c.m_lociic.m_framexform.getBasis().transpose();
1960	#else
1961	#if 0/* Sphere */
1962	const btScalar rk=(2c.m_extents.length2())/(5c.m_imass);
1963	const btVector3 inertia(rk,rk,rk);
1964	const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
1965	btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
1966	btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
1967
1968	c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
1969	#else/* Actual */
1970	c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
1971	for(int i=0;i<n;++i)
1972	{
1973	const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
1974	const btVector3 q=k*k;
1975	const btScalar m=1/c.m_nodes[i]->m_im;
1976	c.m_invwi[0][0] += m*(q[1]+q[2]);
1977	c.m_invwi[1][1] += m*(q[0]+q[2]);
1978	c.m_invwi[2][2] += m*(q[0]+q[1]);
1979	c.m_invwi[0][1] -= mk[0]k[1];
1980	c.m_invwi[0][2] -= mk[0]k[2];
1981	c.m_invwi[1][2] -= mk[1]k[2];
1982	}
1983	c.m_invwi[1][0]=c.m_invwi[0][1];
1984	c.m_invwi[2][0]=c.m_invwi[0][2];
1985	c.m_invwi[2][1]=c.m_invwi[1][2];
1986	c.m_invwi=c.m_invwi.inverse();
1987	#endif
1988	#endif
1989	/* Velocities */
1990	c.m_lv=btVector3(0,0,0);
1991	c.m_av=btVector3(0,0,0);
1992	{
1993	int i;
1994
1995	for(i=0;i<n;++i)
1996	{
1997	const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
1998	c.m_lv += v;
1999	c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v);
2000	}
2001	}
2002	c.m_lv=c.m_imassc.m_lv(1-c.m_ldamping);
2003	c.m_av=c.m_invwic.m_av(1-c.m_adamping);
2004	c.m_vimpulses[0] =
2005	c.m_vimpulses[1] = btVector3(0,0,0);
2006	c.m_dimpulses[0] =
2007	c.m_dimpulses[1] = btVector3(0,0,0);
2008	c.m_nvimpulses = 0;
2009	c.m_ndimpulses = 0;
2010	/* Matching */
2011	if(c.m_matching>0)
2012	{
2013	for(int j=0;j<c.m_nodes.size();++j)
2014	{
2015	Node& n=*c.m_nodes[j];
2016	const btVector3 x=c.m_framexform*c.m_framerefs[j];
2017	n.m_x=Lerp(n.m_x,x,c.m_matching);
2018	}
2019	}
2020	/* Dbvt */
2021	if(c.m_collide)
2022	{
2023	btVector3 mi=c.m_nodes[0]->m_x;
2024	btVector3 mx=mi;
2025	for(int j=1;j<n;++j)
2026	{
2027	mi.setMin(c.m_nodes[j]->m_x);
2028	mx.setMax(c.m_nodes[j]->m_x);
2029	}
2030	const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
2031	if(c.m_leaf)
2032	m_cdbvt.update(c.m_leaf,bounds,c.m_lvm_sst.sdt3,m_sst.radmrg);
2033	else
2034	c.m_leaf=m_cdbvt.insert(bounds,&c);
2035	}
2036	}
2037	}
2038	}
2039
2040	//
2041	void btSoftBody::cleanupClusters()
2042	{
2043	for(int i=0;i<m_joints.size();++i)
2044	{
2045	m_joints[i]->Terminate(m_sst.sdt);
2046	if(m_joints[i]->m_delete)
2047	{
2048	btAlignedFree(m_joints[i]);
2049	m_joints.remove(m_joints[i--]);
2050	}
2051	}
2052	}
2053
2054	//
2055	void btSoftBody::prepareClusters(int iterations)
2056	{
2057	for(int i=0;i<m_joints.size();++i)
2058	{
2059	m_joints[i]->Prepare(m_sst.sdt,iterations);
2060	}
2061	}
2062
2063
2064	//
2065	void btSoftBody::solveClusters(btScalar sor)
2066	{
2067	for(int i=0,ni=m_joints.size();i<ni;++i)
2068	{
2069	m_joints[i]->Solve(m_sst.sdt,sor);
2070	}
2071	}
2072
2073	//
2074	void btSoftBody::applyClusters(bool drift)
2075	{
2076	BT_PROFILE("ApplyClusters");
2077	const btScalar f0=m_sst.sdt;
2078	const btScalar f1=f0/2;
2079	btAlignedObjectArray<btVector3> deltas;
2080	btAlignedObjectArray<btScalar> weights;
2081	deltas.resize(m_nodes.size(),btVector3(0,0,0));
2082	weights.resize(m_nodes.size(),0);
2083	int i;
2084
2085	if(drift)
2086	{
2087	for(i=0;i<m_clusters.size();++i)
2088	{
2089	Cluster& c=*m_clusters[i];
2090	if(c.m_ndimpulses)
2091	{
2092	c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2093	c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2094	}
2095	}
2096	}
2097	for(i=0;i<m_clusters.size();++i)
2098	{
2099	Cluster& c=*m_clusters[i];
2100	if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2101	{
2102	const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2103	const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2104	for(int j=0;j<c.m_nodes.size();++j)
2105	{
2106	const int idx=int(c.m_nodes[j]-&m_nodes[0]);
2107	const btVector3& x=c.m_nodes[j]->m_x;
2108	const btScalar q=c.m_masses[j];
2109	deltas[idx] += (v+cross(w,x-c.m_com))*q;
2110	weights[idx] += q;
2111	}
2112	}
2113	}
2114	for(i=0;i<deltas.size();++i)
2115	{
2116	if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
2117	}
2118	}
2119
2120	//
2121	void btSoftBody::dampClusters()
2122	{
2123	int i;
2124
2125	for(i=0;i<m_clusters.size();++i)
2126	{
2127	Cluster& c=*m_clusters[i];
2128	if(c.m_ndamping>0)
2129	{
2130	for(int j=0;j<c.m_nodes.size();++j)
2131	{
2132	Node& n=*c.m_nodes[j];
2133	if(n.m_im>0)
2134	{
2135	const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2136	n.m_v += c.m_ndamping*(vx-n.m_v);
2137	}
2138	}
2139	}
2140	}
2141	}
2142
2143	//
2144	void btSoftBody::Joint::Prepare(btScalar dt,int)
2145	{
2146	m_bodies[0].activate();
2147	m_bodies[1].activate();
2148	}
2149
2150	//
2151	void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2152	{
2153	static const btScalar maxdrift=4;
2154	Joint::Prepare(dt,iterations);
2155	m_rpos[0] = m_bodies[0].xform()*m_refs[0];
2156	m_rpos[1] = m_bodies[1].xform()*m_refs[1];
2157	m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2158	m_rpos[0] -= m_bodies[0].xform().getOrigin();
2159	m_rpos[1] -= m_bodies[1].xform().getOrigin();
2160	m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2161	m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2162	if(m_split>0)
2163	{
2164	m_sdrift = m_massmatrix(m_driftm_split);
2165	m_drift *= 1-m_split;
2166	}
2167	m_drift /=(btScalar)iterations;
2168	}
2169
2170	//
2171	void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2172	{
2173	const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2174	const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2175	const btVector3 vr=va-vb;
2176	btSoftBody::Impulse impulse;
2177	impulse.m_asVelocity = 1;
2178	impulse.m_velocity = m_massmatrix(m_drift+vrm_cfm)*sor;
2179	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2180	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2181	}
2182
2183	//
2184	void btSoftBody::LJoint::Terminate(btScalar dt)
2185	{
2186	if(m_split>0)
2187	{
2188	m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2189	m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2190	}
2191	}
2192
2193	//
2194	void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2195	{
2196	static const btScalar maxdrift=SIMD_PI/16;
2197	m_icontrol->Prepare(this);
2198	Joint::Prepare(dt,iterations);
2199	m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
2200	m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
2201	m_drift = NormalizeAny(cross(m_axis[1],m_axis[0]));
2202	m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
2203	m_drift *= m_erp/dt;
2204	m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2205	if(m_split>0)
2206	{
2207	m_sdrift = m_massmatrix(m_driftm_split);
2208	m_drift *= 1-m_split;
2209	}
2210	m_drift /=(btScalar)iterations;
2211	}
2212
2213	//
2214	void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2215	{
2216	const btVector3 va=m_bodies[0].angularVelocity();
2217	const btVector3 vb=m_bodies[1].angularVelocity();
2218	const btVector3 vr=va-vb;
2219	const btScalar sp=dot(vr,m_axis[0]);
2220	const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2221	btSoftBody::Impulse impulse;
2222	impulse.m_asVelocity = 1;
2223	impulse.m_velocity = m_massmatrix(m_drift+vcm_cfm)*sor;
2224	m_bodies[0].applyAImpulse(-impulse);
2225	m_bodies[1].applyAImpulse( impulse);
2226	}
2227
2228	//
2229	void btSoftBody::AJoint::Terminate(btScalar dt)
2230	{
2231	if(m_split>0)
2232	{
2233	m_bodies[0].applyDAImpulse(-m_sdrift);
2234	m_bodies[1].applyDAImpulse( m_sdrift);
2235	}
2236	}
2237
2238	//
2239	void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2240	{
2241	Joint::Prepare(dt,iterations);
2242	const bool dodrift=(m_life==0);
2243	m_delete=(++m_life)>m_maxlife;
2244	if(dodrift)
2245	{
2246	m_drift=m_drift*m_erp/dt;
2247	if(m_split>0)
2248	{
2249	m_sdrift = m_massmatrix(m_driftm_split);
2250	m_drift *= 1-m_split;
2251	}
2252	m_drift/=(btScalar)iterations;
2253	}
2254	else
2255	{
2256	m_drift=m_sdrift=btVector3(0,0,0);
2257	}
2258	}
2259
2260	//
2261	void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2262	{
2263	const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2264	const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2265	const btVector3 vrel=va-vb;
2266	const btScalar rvac=dot(vrel,m_normal);
2267	btSoftBody::Impulse impulse;
2268	impulse.m_asVelocity = 1;
2269	impulse.m_velocity = m_drift;
2270	if(rvac<0)
2271	{
2272	const btVector3 iv=m_normal*rvac;
2273	const btVector3 fv=vrel-iv;
2274	impulse.m_velocity += iv+fv*m_friction;
2275	}
2276	impulse.m_velocity=m_massmatriximpulse.m_velocitysor;
2277	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2278	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2279	}
2280
2281	//
2282	void btSoftBody::CJoint::Terminate(btScalar dt)
2283	{
2284	if(m_split>0)
2285	{
2286	m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2287	m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2288	}
2289	}
2290
2291	//
2292	void btSoftBody::applyForces()
2293	{
2294	BT_PROFILE("SoftBody applyForces");
2295	const btScalar dt=m_sst.sdt;
2296	const btScalar kLF=m_cfg.kLF;
2297	const btScalar kDG=m_cfg.kDG;
2298	const btScalar kPR=m_cfg.kPR;
2299	const btScalar kVC=m_cfg.kVC;
2300	const bool as_lift=kLF>0;
2301	const bool as_drag=kDG>0;
2302	const bool as_pressure=kPR!=0;
2303	const bool as_volume=kVC>0;
2304	const bool as_aero= as_lift \|\|
2305	as_drag ;
2306	const bool as_vaero= as_aero &&
2307	(m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
2308	const bool as_faero= as_aero &&
2309	(m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
2310	const bool use_medium= as_aero;
2311	const bool use_volume= as_pressure \|\|
2312	as_volume ;
2313	btScalar volume=0;
2314	btScalar ivolumetp=0;
2315	btScalar dvolumetv=0;
2316	btSoftBody::sMedium medium;
2317	if(use_volume)
2318	{
2319	volume = getVolume();
2320	ivolumetp = 1/btFabs(volume)*kPR;
2321	dvolumetv = (m_pose.m_volume-volume)*kVC;
2322	}
2323	/* Per vertex forces */
2324	int i,ni;
2325
2326	for(i=0,ni=m_nodes.size();i<ni;++i)
2327	{
2328	btSoftBody::Node& n=m_nodes[i];
2329	if(n.m_im>0)
2330	{
2331	if(use_medium)
2332	{
2333	EvaluateMedium(m_worldInfo,n.m_x,medium);
2334	/* Aerodynamics */
2335	if(as_vaero)
2336	{
2337	const btVector3 rel_v=n.m_v-medium.m_velocity;
2338	const btScalar rel_v2=rel_v.length2();
2339	if(rel_v2>SIMD_EPSILON)
2340	{
2341	btVector3 nrm=n.m_n;
2342	/* Setup normal */
2343	switch(m_cfg.aeromodel)
2344	{
2345	case btSoftBody::eAeroModel::V_Point:
2346	nrm=NormalizeAny(rel_v);break;
2347	case btSoftBody::eAeroModel::V_TwoSided:
2348	nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
2349	}
2350	const btScalar dvn=dot(rel_v,nrm);
2351	/* Compute forces */
2352	if(dvn>0)
2353	{
2354	btVector3 force(0,0,0);
2355	const btScalar c0 = n.m_areadvnrel_v2/2;
2356	const btScalar c1 = c0*medium.m_density;
2357	force += nrm(-c1kLF);
2358	force += rel_v.normalized()(-c1kDG);
2359	ApplyClampedForce(n,force,dt);
2360	}
2361	}
2362	}
2363	}
2364	/* Pressure */
2365	if(as_pressure)
2366	{
2367	n.m_f += n.m_n(n.m_areaivolumetp);
2368	}
2369	/* Volume */
2370	if(as_volume)
2371	{
2372	n.m_f += n.m_n(n.m_areadvolumetv);
2373	}
2374	}
2375	}
2376	/* Per face forces */
2377	for(i=0,ni=m_faces.size();i<ni;++i)
2378	{
2379	btSoftBody::Face& f=m_faces[i];
2380	if(as_faero)
2381	{
2382	const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
2383	const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
2384	EvaluateMedium(m_worldInfo,x,medium);
2385	const btVector3 rel_v=v-medium.m_velocity;
2386	const btScalar rel_v2=rel_v.length2();
2387	if(rel_v2>SIMD_EPSILON)
2388	{
2389	btVector3 nrm=f.m_normal;
2390	/* Setup normal */
2391	switch(m_cfg.aeromodel)
2392	{
2393	case btSoftBody::eAeroModel::F_TwoSided:
2394	nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
2395	}
2396	const btScalar dvn=dot(rel_v,nrm);
2397	/* Compute forces */
2398	if(dvn>0)
2399	{
2400	btVector3 force(0,0,0);
2401	const btScalar c0 = f.m_radvnrel_v2;
2402	const btScalar c1 = c0*medium.m_density;
2403	force += nrm(-c1kLF);
2404	force += rel_v.normalized()(-c1kDG);
2405	force /= 3;
2406	for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
2407	}
2408	}
2409	}
2410	}
2411	}
2412
2413	//
2414	void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2415	{
2416	const btScalar kAHR=psb->m_cfg.kAHR*kst;
2417	const btScalar dt=psb->m_sst.sdt;
2418	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2419	{
2420	const Anchor& a=psb->m_anchors[i];
2421	const btTransform& t=a.m_body->getInterpolationWorldTransform();
2422	Node& n=*a.m_node;
2423	const btVector3 wa=t*a.m_local;
2424	const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2425	const btVector3 vb=n.m_x-n.m_q;
2426	const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
2427	const btVector3 impulse=a.m_c0*vr;
2428	n.m_x+=impulse*a.m_c2;
2429	a.m_body->applyImpulse(-impulse,a.m_c1);
2430	}
2431	}
2432
2433	//
2434	void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
2435	{
2436	const btScalar dt=psb->m_sst.sdt;
2437	const btScalar mrg=psb->getCollisionShape()->getMargin();
2438	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
2439	{
2440	const RContact& c=psb->m_rcontacts[i];
2441	const sCti& cti=c.m_cti;
2442	const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt;
2443	const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
2444	const btVector3 vr=vb-va;
2445	const btScalar dn=dot(vr,cti.m_normal);
2446	if(dn<=SIMD_EPSILON)
2447	{
2448	const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
2449	const btVector3 fv=vr-cti.m_normal*dn;
2450	const btVector3 impulse=c.m_c0((vr-fvc.m_c3+cti.m_normal(dpc.m_c4))*kst);
2451	c.m_node->m_x-=impulse*c.m_c2;
2452	c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
2453	}
2454	}
2455	}
2456
2457	//
2458	void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
2459	{
2460	for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
2461	{
2462	const SContact& c=psb->m_scontacts[i];
2463	const btVector3& nr=c.m_normal;
2464	Node& n=*c.m_node;
2465	Face& f=*c.m_face;
2466	const btVector3 p=BaryEval( f.m_n[0]->m_x,
2467	f.m_n[1]->m_x,
2468	f.m_n[2]->m_x,
2469	c.m_weights);
2470	const btVector3 q=BaryEval( f.m_n[0]->m_q,
2471	f.m_n[1]->m_q,
2472	f.m_n[2]->m_q,
2473	c.m_weights);
2474	const btVector3 vr=(n.m_x-n.m_q)-(p-q);
2475	btVector3 corr(0,0,0);
2476	if(dot(vr,nr)<0)
2477	{
2478	const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
2479	corr+=c.m_normal*j;
2480	}
2481	corr -= ProjectOnPlane(vr,nr)*c.m_friction;
2482	n.m_x += corr*c.m_cfm[0];
2483	f.m_n[0]->m_x -= corr(c.m_cfm[1]c.m_weights.x());
2484	f.m_n[1]->m_x -= corr(c.m_cfm[1]c.m_weights.y());
2485	f.m_n[2]->m_x -= corr(c.m_cfm[1]c.m_weights.z());
2486	}
2487	}
2488
2489	//
2490	void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
2491	{
2492	for(int i=0,ni=psb->m_links.size();i<ni;++i)
2493	{
2494	Link& l=psb->m_links[i];
2495	if(l.m_c0>0)
2496	{
2497	Node& a=*l.m_n[0];
2498	Node& b=*l.m_n[1];
2499	const btVector3 del=b.m_x-a.m_x;
2500	const btScalar len=del.length2();
2501	const btScalar k=((l.m_c1-len)/(l.m_c0(l.m_c1+len)))kst;
2502	const btScalar t=k*a.m_im;
2503	a.m_x-=del(ka.m_im);
2504	b.m_x+=del(kb.m_im);
2505	}
2506	}
2507	}
2508
2509	//
2510	void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
2511	{
2512	for(int i=0,ni=psb->m_links.size();i<ni;++i)
2513	{
2514	Link& l=psb->m_links[i];
2515	Node** n=l.m_n;
2516	const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)l.m_c2kst;
2517	n[0]->m_v+= l.m_c3(jn[0]->m_im);
2518	n[1]->m_v-= l.m_c3(jn[1]->m_im);
2519	}
2520	}
2521
2522	//
2523	btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
2524	{
2525	switch(solver)
2526	{
2527	case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors);
2528	case ePSolver::Linear: return(&btSoftBody::PSolve_Links);
2529	case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts);
2530	case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts);
2531	}
2532	return(0);
2533	}
2534
2535	//
2536	btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
2537	{
2538	switch(solver)
2539	{
2540	case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
2541	}
2542	return(0);
2543	}
2544
2545	//
2546	void btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
2547	{
2548	switch(m_cfg.collisions&fCollision::RVSmask)
2549	{
2550	case fCollision::SDF_RS:
2551	{
2552	btSoftColliders::CollideSDF_RS docollide;
2553	btRigidBody* prb=btRigidBody::upcast(pco);
2554	const btTransform wtr=prb->getInterpolationWorldTransform();
2555	const btTransform ctr=prb->getWorldTransform();
2556	const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
2557	const btScalar basemargin=getCollisionShape()->getMargin();
2558	btVector3 mins;
2559	btVector3 maxs;
2560	ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
2561	pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(),
2562	mins,
2563	maxs);
2564	volume=btDbvtVolume::FromMM(mins,maxs);
2565	volume.Expand(btVector3(basemargin,basemargin,basemargin));
2566	docollide.psb = this;
2567	docollide.prb = prb;
2568	docollide.dynmargin = basemargin+timemargin;
2569	docollide.stamargin = basemargin;
2570	btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
2571	}
2572	break;
2573	case fCollision::CL_RS:
2574	{
2575	btSoftColliders::CollideCL_RS collider;
2576	collider.Process(this,btRigidBody::upcast(pco));
2577	}
2578	break;
2579	}
2580	}
2581
2582	//
2583	void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
2584	{
2585	const int cf=m_cfg.collisions&psb->m_cfg.collisions;
2586	switch(cf&fCollision::SVSmask)
2587	{
2588	case fCollision::CL_SS:
2589	{
2590	btSoftColliders::CollideCL_SS docollide;
2591	docollide.Process(this,psb);
2592	}
2593	break;
2594	case fCollision::VF_SS:
2595	{
2596	btSoftColliders::CollideVF_SS docollide;
2597	/* common */
2598	docollide.mrg= getCollisionShape()->getMargin()+
2599	psb->getCollisionShape()->getMargin();
2600	/* psb0 nodes vs psb1 faces */
2601	docollide.psb[0]=this;
2602	docollide.psb[1]=psb;
2603	btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
2604	docollide.psb[1]->m_fdbvt.m_root,
2605	docollide);
2606	/* psb1 nodes vs psb0 faces */
2607	docollide.psb[0]=psb;
2608	docollide.psb[1]=this;
2609	btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
2610	docollide.psb[1]->m_fdbvt.m_root,
2611	docollide);
2612	}
2613	break;
2614	}
2615	}

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

Download in other formats: