Home | History | Annotate | Download | only in BulletSoftBody
      1 /*
      2 Bullet Continuous Collision Detection and Physics Library
      3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
      4 
      5 This software is provided 'as-is', without any express or implied warranty.
      6 In no event will the authors be held liable for any damages arising from the use of this software.
      7 Permission is granted to anyone to use this software for any purpose,
      8 including commercial applications, and to alter it and redistribute it freely,
      9 subject to the following restrictions:
     10 
     11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
     12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
     13 3. This notice may not be removed or altered from any source distribution.
     14 */
     15 ///btSoftBody implementation by Nathanael Presson
     16 
     17 #include "btSoftBodyInternals.h"
     18 #include "BulletSoftBody/btSoftBodySolvers.h"
     19 #include "btSoftBodyData.h"
     20 #include "LinearMath/btSerializer.h"
     21 
     22 
     23 //
     24 btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
     25 :m_softBodySolver(0),m_worldInfo(worldInfo)
     26 {
     27 	/* Init		*/
     28 	initDefaults();
     29 
     30 	/* Default material	*/
     31 	Material*	pm=appendMaterial();
     32 	pm->m_kLST	=	1;
     33 	pm->m_kAST	=	1;
     34 	pm->m_kVST	=	1;
     35 	pm->m_flags	=	fMaterial::Default;
     36 
     37 	/* Nodes			*/
     38 	const btScalar		margin=getCollisionShape()->getMargin();
     39 	m_nodes.resize(node_count);
     40 	for(int i=0,ni=node_count;i<ni;++i)
     41 	{
     42 		Node&	n=m_nodes[i];
     43 		ZeroInitialize(n);
     44 		n.m_x		=	x?*x++:btVector3(0,0,0);
     45 		n.m_q		=	n.m_x;
     46 		n.m_im		=	m?*m++:1;
     47 		n.m_im		=	n.m_im>0?1/n.m_im:0;
     48 		n.m_leaf	=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
     49 		n.m_material=	pm;
     50 	}
     51 	updateBounds();
     52 
     53 }
     54 
     55 btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo)
     56 :m_worldInfo(worldInfo)
     57 {
     58 	initDefaults();
     59 }
     60 
     61 
     62 void	btSoftBody::initDefaults()
     63 {
     64 	m_internalType		=	CO_SOFT_BODY;
     65 	m_cfg.aeromodel		=	eAeroModel::V_Point;
     66 	m_cfg.kVCF			=	1;
     67 	m_cfg.kDG			=	0;
     68 	m_cfg.kLF			=	0;
     69 	m_cfg.kDP			=	0;
     70 	m_cfg.kPR			=	0;
     71 	m_cfg.kVC			=	0;
     72 	m_cfg.kDF			=	(btScalar)0.2;
     73 	m_cfg.kMT			=	0;
     74 	m_cfg.kCHR			=	(btScalar)1.0;
     75 	m_cfg.kKHR			=	(btScalar)0.1;
     76 	m_cfg.kSHR			=	(btScalar)1.0;
     77 	m_cfg.kAHR			=	(btScalar)0.7;
     78 	m_cfg.kSRHR_CL		=	(btScalar)0.1;
     79 	m_cfg.kSKHR_CL		=	(btScalar)1;
     80 	m_cfg.kSSHR_CL		=	(btScalar)0.5;
     81 	m_cfg.kSR_SPLT_CL	=	(btScalar)0.5;
     82 	m_cfg.kSK_SPLT_CL	=	(btScalar)0.5;
     83 	m_cfg.kSS_SPLT_CL	=	(btScalar)0.5;
     84 	m_cfg.maxvolume		=	(btScalar)1;
     85 	m_cfg.timescale		=	1;
     86 	m_cfg.viterations	=	0;
     87 	m_cfg.piterations	=	1;
     88 	m_cfg.diterations	=	0;
     89 	m_cfg.citerations	=	4;
     90 	m_cfg.collisions	=	fCollision::Default;
     91 	m_pose.m_bvolume	=	false;
     92 	m_pose.m_bframe		=	false;
     93 	m_pose.m_volume		=	0;
     94 	m_pose.m_com		=	btVector3(0,0,0);
     95 	m_pose.m_rot.setIdentity();
     96 	m_pose.m_scl.setIdentity();
     97 	m_tag				=	0;
     98 	m_timeacc			=	0;
     99 	m_bUpdateRtCst		=	true;
    100 	m_bounds[0]			=	btVector3(0,0,0);
    101 	m_bounds[1]			=	btVector3(0,0,0);
    102 	m_worldTransform.setIdentity();
    103 	setSolver(eSolverPresets::Positions);
    104 
    105 	/* Collision shape	*/
    106 	///for now, create a collision shape internally
    107 	m_collisionShape = new btSoftBodyCollisionShape(this);
    108 	m_collisionShape->setMargin(0.25f);
    109 
    110 	m_initialWorldTransform.setIdentity();
    111 
    112 	m_windVelocity = btVector3(0,0,0);
    113 	m_restLengthScale = btScalar(1.0);
    114 }
    115 
    116 //
    117 btSoftBody::~btSoftBody()
    118 {
    119 	//for now, delete the internal shape
    120 	delete m_collisionShape;
    121 	int i;
    122 
    123 	releaseClusters();
    124 	for(i=0;i<m_materials.size();++i)
    125 		btAlignedFree(m_materials[i]);
    126 	for(i=0;i<m_joints.size();++i)
    127 		btAlignedFree(m_joints[i]);
    128 }
    129 
    130 //
    131 bool			btSoftBody::checkLink(int node0,int node1) const
    132 {
    133 	return(checkLink(&m_nodes[node0],&m_nodes[node1]));
    134 }
    135 
    136 //
    137 bool			btSoftBody::checkLink(const Node* node0,const Node* node1) const
    138 {
    139 	const Node*	n[]={node0,node1};
    140 	for(int i=0,ni=m_links.size();i<ni;++i)
    141 	{
    142 		const Link&	l=m_links[i];
    143 		if(	(l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
    144 			(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
    145 		{
    146 			return(true);
    147 		}
    148 	}
    149 	return(false);
    150 }
    151 
    152 //
    153 bool			btSoftBody::checkFace(int node0,int node1,int node2) const
    154 {
    155 	const Node*	n[]={	&m_nodes[node0],
    156 		&m_nodes[node1],
    157 		&m_nodes[node2]};
    158 	for(int i=0,ni=m_faces.size();i<ni;++i)
    159 	{
    160 		const Face&	f=m_faces[i];
    161 		int			c=0;
    162 		for(int j=0;j<3;++j)
    163 		{
    164 			if(	(f.m_n[j]==n[0])||
    165 				(f.m_n[j]==n[1])||
    166 				(f.m_n[j]==n[2])) c|=1<<j; else break;
    167 		}
    168 		if(c==7) return(true);
    169 	}
    170 	return(false);
    171 }
    172 
    173 //
    174 btSoftBody::Material*		btSoftBody::appendMaterial()
    175 {
    176 	Material*	pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
    177 	if(m_materials.size()>0)
    178 		*pm=*m_materials[0];
    179 	else
    180 		ZeroInitialize(*pm);
    181 	m_materials.push_back(pm);
    182 	return(pm);
    183 }
    184 
    185 //
    186 void			btSoftBody::appendNote(	const char* text,
    187 									   const btVector3& o,
    188 									   const btVector4& c,
    189 									   Node* n0,
    190 									   Node* n1,
    191 									   Node* n2,
    192 									   Node* n3)
    193 {
    194 	Note	n;
    195 	ZeroInitialize(n);
    196 	n.m_rank		=	0;
    197 	n.m_text		=	text;
    198 	n.m_offset		=	o;
    199 	n.m_coords[0]	=	c.x();
    200 	n.m_coords[1]	=	c.y();
    201 	n.m_coords[2]	=	c.z();
    202 	n.m_coords[3]	=	c.w();
    203 	n.m_nodes[0]	=	n0;n.m_rank+=n0?1:0;
    204 	n.m_nodes[1]	=	n1;n.m_rank+=n1?1:0;
    205 	n.m_nodes[2]	=	n2;n.m_rank+=n2?1:0;
    206 	n.m_nodes[3]	=	n3;n.m_rank+=n3?1:0;
    207 	m_notes.push_back(n);
    208 }
    209 
    210 //
    211 void			btSoftBody::appendNote(	const char* text,
    212 									   const btVector3& o,
    213 									   Node* feature)
    214 {
    215 	appendNote(text,o,btVector4(1,0,0,0),feature);
    216 }
    217 
    218 //
    219 void			btSoftBody::appendNote(	const char* text,
    220 									   const btVector3& o,
    221 									   Link* feature)
    222 {
    223 	static const btScalar	w=1/(btScalar)2;
    224 	appendNote(text,o,btVector4(w,w,0,0),	feature->m_n[0],
    225 		feature->m_n[1]);
    226 }
    227 
    228 //
    229 void			btSoftBody::appendNote(	const char* text,
    230 									   const btVector3& o,
    231 									   Face* feature)
    232 {
    233 	static const btScalar	w=1/(btScalar)3;
    234 	appendNote(text,o,btVector4(w,w,w,0),	feature->m_n[0],
    235 		feature->m_n[1],
    236 		feature->m_n[2]);
    237 }
    238 
    239 //
    240 void			btSoftBody::appendNode(	const btVector3& x,btScalar m)
    241 {
    242 	if(m_nodes.capacity()==m_nodes.size())
    243 	{
    244 		pointersToIndices();
    245 		m_nodes.reserve(m_nodes.size()*2+1);
    246 		indicesToPointers();
    247 	}
    248 	const btScalar	margin=getCollisionShape()->getMargin();
    249 	m_nodes.push_back(Node());
    250 	Node&			n=m_nodes[m_nodes.size()-1];
    251 	ZeroInitialize(n);
    252 	n.m_x			=	x;
    253 	n.m_q			=	n.m_x;
    254 	n.m_im			=	m>0?1/m:0;
    255 	n.m_material	=	m_materials[0];
    256 	n.m_leaf		=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
    257 }
    258 
    259 //
    260 void			btSoftBody::appendLink(int model,Material* mat)
    261 {
    262 	Link	l;
    263 	if(model>=0)
    264 		l=m_links[model];
    265 	else
    266 	{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
    267 	m_links.push_back(l);
    268 }
    269 
    270 //
    271 void			btSoftBody::appendLink(	int node0,
    272 									   int node1,
    273 									   Material* mat,
    274 									   bool bcheckexist)
    275 {
    276 	appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
    277 }
    278 
    279 //
    280 void			btSoftBody::appendLink(	Node* node0,
    281 									   Node* node1,
    282 									   Material* mat,
    283 									   bool bcheckexist)
    284 {
    285 	if((!bcheckexist)||(!checkLink(node0,node1)))
    286 	{
    287 		appendLink(-1,mat);
    288 		Link&	l=m_links[m_links.size()-1];
    289 		l.m_n[0]		=	node0;
    290 		l.m_n[1]		=	node1;
    291 		l.m_rl			=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
    292 		m_bUpdateRtCst=true;
    293 	}
    294 }
    295 
    296 //
    297 void			btSoftBody::appendFace(int model,Material* mat)
    298 {
    299 	Face	f;
    300 	if(model>=0)
    301 	{ f=m_faces[model]; }
    302 	else
    303 	{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
    304 	m_faces.push_back(f);
    305 }
    306 
    307 //
    308 void			btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
    309 {
    310 	if (node0==node1)
    311 		return;
    312 	if (node1==node2)
    313 		return;
    314 	if (node2==node0)
    315 		return;
    316 
    317 	appendFace(-1,mat);
    318 	Face&	f=m_faces[m_faces.size()-1];
    319 	btAssert(node0!=node1);
    320 	btAssert(node1!=node2);
    321 	btAssert(node2!=node0);
    322 	f.m_n[0]	=	&m_nodes[node0];
    323 	f.m_n[1]	=	&m_nodes[node1];
    324 	f.m_n[2]	=	&m_nodes[node2];
    325 	f.m_ra		=	AreaOf(	f.m_n[0]->m_x,
    326 		f.m_n[1]->m_x,
    327 		f.m_n[2]->m_x);
    328 	m_bUpdateRtCst=true;
    329 }
    330 
    331 //
    332 void			btSoftBody::appendTetra(int model,Material* mat)
    333 {
    334 Tetra	t;
    335 if(model>=0)
    336 	t=m_tetras[model];
    337 	else
    338 	{ ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
    339 m_tetras.push_back(t);
    340 }
    341 
    342 //
    343 void			btSoftBody::appendTetra(int node0,
    344 										int node1,
    345 										int node2,
    346 										int node3,
    347 										Material* mat)
    348 {
    349 	appendTetra(-1,mat);
    350 	Tetra&	t=m_tetras[m_tetras.size()-1];
    351 	t.m_n[0]	=	&m_nodes[node0];
    352 	t.m_n[1]	=	&m_nodes[node1];
    353 	t.m_n[2]	=	&m_nodes[node2];
    354 	t.m_n[3]	=	&m_nodes[node3];
    355 	t.m_rv		=	VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
    356 	m_bUpdateRtCst=true;
    357 }
    358 
    359 //
    360 
    361 void			btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence)
    362 {
    363 	btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
    364 	appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence);
    365 }
    366 
    367 //
    368 void			btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
    369 {
    370 	if (disableCollisionBetweenLinkedBodies)
    371 	{
    372 		if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
    373 		{
    374 			m_collisionDisabledObjects.push_back(body);
    375 		}
    376 	}
    377 
    378 	Anchor	a;
    379 	a.m_node			=	&m_nodes[node];
    380 	a.m_body			=	body;
    381 	a.m_local			=	localPivot;
    382 	a.m_node->m_battach	=	1;
    383 	a.m_influence = influence;
    384 	m_anchors.push_back(a);
    385 }
    386 
    387 //
    388 void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
    389 {
    390 	LJoint*		pj	=	new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
    391 	pj->m_bodies[0]	=	body0;
    392 	pj->m_bodies[1]	=	body1;
    393 	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse()*specs.position;
    394 	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse()*specs.position;
    395 	pj->m_cfm		=	specs.cfm;
    396 	pj->m_erp		=	specs.erp;
    397 	pj->m_split		=	specs.split;
    398 	m_joints.push_back(pj);
    399 }
    400 
    401 //
    402 void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
    403 {
    404 	appendLinearJoint(specs,m_clusters[0],body);
    405 }
    406 
    407 //
    408 void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
    409 {
    410 	appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
    411 }
    412 
    413 //
    414 void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
    415 {
    416 	AJoint*		pj	=	new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
    417 	pj->m_bodies[0]	=	body0;
    418 	pj->m_bodies[1]	=	body1;
    419 	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
    420 	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
    421 	pj->m_cfm		=	specs.cfm;
    422 	pj->m_erp		=	specs.erp;
    423 	pj->m_split		=	specs.split;
    424 	pj->m_icontrol	=	specs.icontrol;
    425 	m_joints.push_back(pj);
    426 }
    427 
    428 //
    429 void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
    430 {
    431 	appendAngularJoint(specs,m_clusters[0],body);
    432 }
    433 
    434 //
    435 void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
    436 {
    437 	appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
    438 }
    439 
    440 //
    441 void			btSoftBody::addForce(const btVector3& force)
    442 {
    443 	for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
    444 }
    445 
    446 //
    447 void			btSoftBody::addForce(const btVector3& force,int node)
    448 {
    449 	Node&	n=m_nodes[node];
    450 	if(n.m_im>0)
    451 	{
    452 		n.m_f	+=	force;
    453 	}
    454 }
    455 
    456 void			btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex)
    457 {
    458 	btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
    459 
    460 	const btScalar dt = m_sst.sdt;
    461 	const btScalar kLF = m_cfg.kLF;
    462 	const btScalar kDG = m_cfg.kDG;
    463 	//const btScalar kPR = m_cfg.kPR;
    464 	//const btScalar kVC = m_cfg.kVC;
    465 	const bool as_lift = kLF>0;
    466 	const bool as_drag = kDG>0;
    467 	const bool as_aero = as_lift || as_drag;
    468 	const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
    469 
    470 	Node& n = m_nodes[nodeIndex];
    471 
    472 	if( n.m_im>0 )
    473 	{
    474 		btSoftBody::sMedium	medium;
    475 
    476 		EvaluateMedium(m_worldInfo, n.m_x, medium);
    477 		medium.m_velocity = windVelocity;
    478 		medium.m_density = m_worldInfo->air_density;
    479 
    480 		/* Aerodynamics			*/
    481 		if(as_vaero)
    482 		{
    483 			const btVector3	rel_v = n.m_v - medium.m_velocity;
    484 			const btScalar rel_v_len = rel_v.length();
    485 			const btScalar	rel_v2 = rel_v.length2();
    486 
    487 			if(rel_v2>SIMD_EPSILON)
    488 			{
    489 				const btVector3 rel_v_nrm = rel_v.normalized();
    490 				btVector3	nrm = n.m_n;
    491 
    492 				if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
    493 				{
    494 					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
    495 					btVector3 fDrag(0, 0, 0);
    496 					btVector3 fLift(0, 0, 0);
    497 
    498 					btScalar n_dot_v = nrm.dot(rel_v_nrm);
    499 					btScalar tri_area = 0.5f * n.m_area;
    500 
    501 					fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
    502 
    503 					// Check angle of attack
    504 					// cos(10) = 0.98480
    505 					if ( 0 < n_dot_v && n_dot_v < 0.98480f)
    506 						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
    507 
    508 					// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
    509 					btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt;
    510 					btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
    511 					btScalar v_len2 = n.m_v.length2();
    512 
    513 					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
    514 					{
    515 						btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
    516 						btScalar v_len = n.m_v.length();
    517 						fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
    518 					}
    519 
    520 					n.m_f += fDrag;
    521 					n.m_f += fLift;
    522 				}
    523 				else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
    524 				{
    525 					if (btSoftBody::eAeroModel::V_TwoSided)
    526 						nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
    527 
    528 					const btScalar dvn = btDot(rel_v,nrm);
    529 					/* Compute forces	*/
    530 					if(dvn>0)
    531 					{
    532 						btVector3		force(0,0,0);
    533 						const btScalar	c0	=	n.m_area * dvn * rel_v2/2;
    534 						const btScalar	c1	=	c0 * medium.m_density;
    535 						force	+=	nrm*(-c1*kLF);
    536 						force	+=	rel_v.normalized() * (-c1 * kDG);
    537 						ApplyClampedForce(n, force, dt);
    538 					}
    539 				}
    540 			}
    541 		}
    542 	}
    543 }
    544 
    545 void			btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex)
    546 {
    547 	const btScalar dt = m_sst.sdt;
    548 	const btScalar kLF = m_cfg.kLF;
    549 	const btScalar kDG = m_cfg.kDG;
    550 //	const btScalar kPR = m_cfg.kPR;
    551 //	const btScalar kVC = m_cfg.kVC;
    552 	const bool as_lift = kLF>0;
    553 	const bool as_drag = kDG>0;
    554 	const bool as_aero = as_lift || as_drag;
    555 	const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
    556 
    557 	if(as_faero)
    558 	{
    559 		btSoftBody::Face&	f=m_faces[faceIndex];
    560 
    561 		btSoftBody::sMedium	medium;
    562 
    563 		const btVector3	v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
    564 		const btVector3	x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
    565 		EvaluateMedium(m_worldInfo,x,medium);
    566 		medium.m_velocity = windVelocity;
    567 		medium.m_density = m_worldInfo->air_density;
    568 		const btVector3	rel_v=v-medium.m_velocity;
    569 		const btScalar rel_v_len = rel_v.length();
    570 		const btScalar	rel_v2=rel_v.length2();
    571 
    572 		if(rel_v2>SIMD_EPSILON)
    573 		{
    574 			const btVector3 rel_v_nrm = rel_v.normalized();
    575 			btVector3	nrm = f.m_normal;
    576 
    577 			if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
    578 			{
    579 				nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
    580 
    581 				btVector3 fDrag(0, 0, 0);
    582 				btVector3 fLift(0, 0, 0);
    583 
    584 				btScalar n_dot_v = nrm.dot(rel_v_nrm);
    585 				btScalar tri_area = 0.5f * f.m_ra;
    586 
    587 				fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
    588 
    589 				// Check angle of attack
    590 				// cos(10) = 0.98480
    591 				if ( 0 < n_dot_v && n_dot_v < 0.98480f)
    592 					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
    593 
    594 				fDrag /= 3;
    595 				fLift /= 3;
    596 
    597 				for(int j=0;j<3;++j)
    598 				{
    599 					if (f.m_n[j]->m_im>0)
    600 					{
    601 						// Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
    602 						btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt;
    603 						btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
    604 						btScalar v_len2 = f.m_n[j]->m_v.length2();
    605 
    606 						if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
    607 						{
    608 							btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
    609 							btScalar v_len = f.m_n[j]->m_v.length();
    610 							fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
    611 						}
    612 
    613 						f.m_n[j]->m_f += fDrag;
    614 						f.m_n[j]->m_f += fLift;
    615 					}
    616 				}
    617 			}
    618 			else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
    619 			{
    620 				if (btSoftBody::eAeroModel::F_TwoSided)
    621 					nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
    622 
    623 				const btScalar	dvn=btDot(rel_v,nrm);
    624 				/* Compute forces	*/
    625 				if(dvn>0)
    626 				{
    627 					btVector3		force(0,0,0);
    628 					const btScalar	c0	=	f.m_ra*dvn*rel_v2;
    629 					const btScalar	c1	=	c0*medium.m_density;
    630 					force	+=	nrm*(-c1*kLF);
    631 					force	+=	rel_v.normalized()*(-c1*kDG);
    632 					force	/=	3;
    633 					for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
    634 				}
    635 			}
    636 		}
    637 	}
    638 
    639 }
    640 
    641 //
    642 void			btSoftBody::addVelocity(const btVector3& velocity)
    643 {
    644 	for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
    645 }
    646 
    647 /* Set velocity for the entire body										*/
    648 void				btSoftBody::setVelocity(	const btVector3& velocity)
    649 {
    650 	for(int i=0,ni=m_nodes.size();i<ni;++i)
    651 	{
    652 		Node&	n=m_nodes[i];
    653 		if(n.m_im>0)
    654 		{
    655 			n.m_v	=	velocity;
    656 		}
    657 	}
    658 }
    659 
    660 
    661 //
    662 void			btSoftBody::addVelocity(const btVector3& velocity,int node)
    663 {
    664 	Node&	n=m_nodes[node];
    665 	if(n.m_im>0)
    666 	{
    667 		n.m_v	+=	velocity;
    668 	}
    669 }
    670 
    671 //
    672 void			btSoftBody::setMass(int node,btScalar mass)
    673 {
    674 	m_nodes[node].m_im=mass>0?1/mass:0;
    675 	m_bUpdateRtCst=true;
    676 }
    677 
    678 //
    679 btScalar		btSoftBody::getMass(int node) const
    680 {
    681 	return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
    682 }
    683 
    684 //
    685 btScalar		btSoftBody::getTotalMass() const
    686 {
    687 	btScalar	mass=0;
    688 	for(int i=0;i<m_nodes.size();++i)
    689 	{
    690 		mass+=getMass(i);
    691 	}
    692 	return(mass);
    693 }
    694 
    695 //
    696 void			btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
    697 {
    698 	int i;
    699 
    700 	if(fromfaces)
    701 	{
    702 
    703 		for(i=0;i<m_nodes.size();++i)
    704 		{
    705 			m_nodes[i].m_im=0;
    706 		}
    707 		for(i=0;i<m_faces.size();++i)
    708 		{
    709 			const Face&		f=m_faces[i];
    710 			const btScalar	twicearea=AreaOf(	f.m_n[0]->m_x,
    711 				f.m_n[1]->m_x,
    712 				f.m_n[2]->m_x);
    713 			for(int j=0;j<3;++j)
    714 			{
    715 				f.m_n[j]->m_im+=twicearea;
    716 			}
    717 		}
    718 		for( i=0;i<m_nodes.size();++i)
    719 		{
    720 			m_nodes[i].m_im=1/m_nodes[i].m_im;
    721 		}
    722 	}
    723 	const btScalar	tm=getTotalMass();
    724 	const btScalar	itm=1/tm;
    725 	for( i=0;i<m_nodes.size();++i)
    726 	{
    727 		m_nodes[i].m_im/=itm*mass;
    728 	}
    729 	m_bUpdateRtCst=true;
    730 }
    731 
    732 //
    733 void			btSoftBody::setTotalDensity(btScalar density)
    734 {
    735 	setTotalMass(getVolume()*density,true);
    736 }
    737 
    738 //
    739 void			btSoftBody::setVolumeMass(btScalar mass)
    740 {
    741 btAlignedObjectArray<btScalar>	ranks;
    742 ranks.resize(m_nodes.size(),0);
    743 int i;
    744 
    745 for(i=0;i<m_nodes.size();++i)
    746 	{
    747 	m_nodes[i].m_im=0;
    748 	}
    749 for(i=0;i<m_tetras.size();++i)
    750 	{
    751 	const Tetra& t=m_tetras[i];
    752 	for(int j=0;j<4;++j)
    753 		{
    754 		t.m_n[j]->m_im+=btFabs(t.m_rv);
    755 		ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
    756 		}
    757 	}
    758 for( i=0;i<m_nodes.size();++i)
    759 	{
    760 	if(m_nodes[i].m_im>0)
    761 		{
    762 		m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
    763 		}
    764 	}
    765 setTotalMass(mass,false);
    766 }
    767 
    768 //
    769 void			btSoftBody::setVolumeDensity(btScalar density)
    770 {
    771 btScalar	volume=0;
    772 for(int i=0;i<m_tetras.size();++i)
    773 	{
    774 	const Tetra& t=m_tetras[i];
    775 	for(int j=0;j<4;++j)
    776 		{
    777 		volume+=btFabs(t.m_rv);
    778 		}
    779 	}
    780 setVolumeMass(volume*density/6);
    781 }
    782 
    783 //
    784 void			btSoftBody::transform(const btTransform& trs)
    785 {
    786 	const btScalar	margin=getCollisionShape()->getMargin();
    787 	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
    788 
    789 	for(int i=0,ni=m_nodes.size();i<ni;++i)
    790 	{
    791 		Node&	n=m_nodes[i];
    792 		n.m_x=trs*n.m_x;
    793 		n.m_q=trs*n.m_q;
    794 		n.m_n=trs.getBasis()*n.m_n;
    795 		vol = btDbvtVolume::FromCR(n.m_x,margin);
    796 
    797 		m_ndbvt.update(n.m_leaf,vol);
    798 	}
    799 	updateNormals();
    800 	updateBounds();
    801 	updateConstants();
    802 	m_initialWorldTransform = trs;
    803 }
    804 
    805 //
    806 void			btSoftBody::translate(const btVector3& trs)
    807 {
    808 	btTransform	t;
    809 	t.setIdentity();
    810 	t.setOrigin(trs);
    811 	transform(t);
    812 }
    813 
    814 //
    815 void			btSoftBody::rotate(	const btQuaternion& rot)
    816 {
    817 	btTransform	t;
    818 	t.setIdentity();
    819 	t.setRotation(rot);
    820 	transform(t);
    821 }
    822 
    823 //
    824 void			btSoftBody::scale(const btVector3& scl)
    825 {
    826 
    827 	const btScalar	margin=getCollisionShape()->getMargin();
    828 	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
    829 
    830 	for(int i=0,ni=m_nodes.size();i<ni;++i)
    831 	{
    832 		Node&	n=m_nodes[i];
    833 		n.m_x*=scl;
    834 		n.m_q*=scl;
    835 		vol = btDbvtVolume::FromCR(n.m_x,margin);
    836 		m_ndbvt.update(n.m_leaf,vol);
    837 	}
    838 	updateNormals();
    839 	updateBounds();
    840 	updateConstants();
    841 }
    842 
    843 //
    844 btScalar btSoftBody::getRestLengthScale()
    845 {
    846 	return m_restLengthScale;
    847 }
    848 
    849 //
    850 void btSoftBody::setRestLengthScale(btScalar restLengthScale)
    851 {
    852 	for(int i=0, ni=m_links.size(); i<ni; ++i)
    853 	{
    854 		Link&		l=m_links[i];
    855 		l.m_rl	=	l.m_rl / m_restLengthScale * restLengthScale;
    856 		l.m_c1	=	l.m_rl*l.m_rl;
    857 	}
    858 	m_restLengthScale = restLengthScale;
    859 
    860 	if (getActivationState() == ISLAND_SLEEPING)
    861 		activate();
    862 }
    863 
    864 //
    865 void			btSoftBody::setPose(bool bvolume,bool bframe)
    866 {
    867 	m_pose.m_bvolume	=	bvolume;
    868 	m_pose.m_bframe		=	bframe;
    869 	int i,ni;
    870 
    871 	/* Weights		*/
    872 	const btScalar	omass=getTotalMass();
    873 	const btScalar	kmass=omass*m_nodes.size()*1000;
    874 	btScalar		tmass=omass;
    875 	m_pose.m_wgh.resize(m_nodes.size());
    876 	for(i=0,ni=m_nodes.size();i<ni;++i)
    877 	{
    878 		if(m_nodes[i].m_im<=0) tmass+=kmass;
    879 	}
    880 	for( i=0,ni=m_nodes.size();i<ni;++i)
    881 	{
    882 		Node&	n=m_nodes[i];
    883 		m_pose.m_wgh[i]=	n.m_im>0					?
    884 			1/(m_nodes[i].m_im*tmass)	:
    885 		kmass/tmass;
    886 	}
    887 	/* Pos		*/
    888 	const btVector3	com=evaluateCom();
    889 	m_pose.m_pos.resize(m_nodes.size());
    890 	for( i=0,ni=m_nodes.size();i<ni;++i)
    891 	{
    892 		m_pose.m_pos[i]=m_nodes[i].m_x-com;
    893 	}
    894 	m_pose.m_volume	=	bvolume?getVolume():0;
    895 	m_pose.m_com	=	com;
    896 	m_pose.m_rot.setIdentity();
    897 	m_pose.m_scl.setIdentity();
    898 	/* Aqq		*/
    899 	m_pose.m_aqq[0]	=
    900 		m_pose.m_aqq[1]	=
    901 		m_pose.m_aqq[2]	=	btVector3(0,0,0);
    902 	for( i=0,ni=m_nodes.size();i<ni;++i)
    903 	{
    904 		const btVector3&	q=m_pose.m_pos[i];
    905 		const btVector3		mq=m_pose.m_wgh[i]*q;
    906 		m_pose.m_aqq[0]+=mq.x()*q;
    907 		m_pose.m_aqq[1]+=mq.y()*q;
    908 		m_pose.m_aqq[2]+=mq.z()*q;
    909 	}
    910 	m_pose.m_aqq=m_pose.m_aqq.inverse();
    911 
    912 	updateConstants();
    913 }
    914 
    915 void				btSoftBody::resetLinkRestLengths()
    916 {
    917 	for(int i=0, ni=m_links.size();i<ni;++i)
    918 	{
    919 		Link& l =	m_links[i];
    920 		l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
    921 		l.m_c1	=	l.m_rl*l.m_rl;
    922 	}
    923 }
    924 
    925 //
    926 btScalar		btSoftBody::getVolume() const
    927 {
    928 	btScalar	vol=0;
    929 	if(m_nodes.size()>0)
    930 	{
    931 		int i,ni;
    932 
    933 		const btVector3	org=m_nodes[0].m_x;
    934 		for(i=0,ni=m_faces.size();i<ni;++i)
    935 		{
    936 			const Face&	f=m_faces[i];
    937 			vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
    938 		}
    939 		vol/=(btScalar)6;
    940 	}
    941 	return(vol);
    942 }
    943 
    944 //
    945 int				btSoftBody::clusterCount() const
    946 {
    947 	return(m_clusters.size());
    948 }
    949 
    950 //
    951 btVector3		btSoftBody::clusterCom(const Cluster* cluster)
    952 {
    953 	btVector3		com(0,0,0);
    954 	for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
    955 	{
    956 		com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
    957 	}
    958 	return(com*cluster->m_imass);
    959 }
    960 
    961 //
    962 btVector3		btSoftBody::clusterCom(int cluster) const
    963 {
    964 	return(clusterCom(m_clusters[cluster]));
    965 }
    966 
    967 //
    968 btVector3		btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
    969 {
    970 	return(cluster->m_lv+btCross(cluster->m_av,rpos));
    971 }
    972 
    973 //
    974 void			btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    975 {
    976 	const btVector3	li=cluster->m_imass*impulse;
    977 	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
    978 	cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
    979 	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
    980 	cluster->m_nvimpulses++;
    981 }
    982 
    983 //
    984 void			btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
    985 {
    986 	const btVector3	li=cluster->m_imass*impulse;
    987 	const btVector3	ai=cluster->m_invwi*btCross(rpos,impulse);
    988 	cluster->m_dimpulses[0]+=li;
    989 	cluster->m_dimpulses[1]+=ai;
    990 	cluster->m_ndimpulses++;
    991 }
    992 
    993 //
    994 void			btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
    995 {
    996 	if(impulse.m_asVelocity)	clusterVImpulse(cluster,rpos,impulse.m_velocity);
    997 	if(impulse.m_asDrift)		clusterDImpulse(cluster,rpos,impulse.m_drift);
    998 }
    999 
   1000 //
   1001 void			btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
   1002 {
   1003 	const btVector3	ai=cluster->m_invwi*impulse;
   1004 	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
   1005 	cluster->m_nvimpulses++;
   1006 }
   1007 
   1008 //
   1009 void			btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
   1010 {
   1011 	const btVector3	ai=cluster->m_invwi*impulse;
   1012 	cluster->m_dimpulses[1]+=ai;
   1013 	cluster->m_ndimpulses++;
   1014 }
   1015 
   1016 //
   1017 void			btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
   1018 {
   1019 	if(impulse.m_asVelocity)	clusterVAImpulse(cluster,impulse.m_velocity);
   1020 	if(impulse.m_asDrift)		clusterDAImpulse(cluster,impulse.m_drift);
   1021 }
   1022 
   1023 //
   1024 void			btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
   1025 {
   1026 	cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
   1027 	cluster->m_ndimpulses++;
   1028 }
   1029 
   1030 struct NodeLinks
   1031 {
   1032     btAlignedObjectArray<int> m_links;
   1033 };
   1034 
   1035 
   1036 
   1037 //
   1038 int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
   1039 {
   1040 	int i,j;
   1041 
   1042 	if(distance>1)
   1043 	{
   1044 		/* Build graph	*/
   1045 		const int		n=m_nodes.size();
   1046 		const unsigned	inf=(~(unsigned)0)>>1;
   1047 		unsigned*		adj=new unsigned[n*n];
   1048 
   1049 
   1050 #define IDX(_x_,_y_)	((_y_)*n+(_x_))
   1051 		for(j=0;j<n;++j)
   1052 		{
   1053 			for(i=0;i<n;++i)
   1054 			{
   1055 				if(i!=j)
   1056 				{
   1057 					adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
   1058 				}
   1059 				else
   1060 				{
   1061 					adj[IDX(i,j)]=adj[IDX(j,i)]=0;
   1062 				}
   1063 			}
   1064 		}
   1065 		for( i=0;i<m_links.size();++i)
   1066 		{
   1067 			const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
   1068 			const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
   1069 			adj[IDX(ia,ib)]=1;
   1070 			adj[IDX(ib,ia)]=1;
   1071 		}
   1072 
   1073 
   1074 		//special optimized case for distance == 2
   1075 		if (distance == 2)
   1076 		{
   1077 
   1078 			btAlignedObjectArray<NodeLinks> nodeLinks;
   1079 
   1080 
   1081 			/* Build node links */
   1082 			nodeLinks.resize(m_nodes.size());
   1083 
   1084 			for( i=0;i<m_links.size();++i)
   1085 			{
   1086 				const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
   1087 				const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
   1088 				if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
   1089 					nodeLinks[ia].m_links.push_back(ib);
   1090 
   1091 				if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
   1092 					nodeLinks[ib].m_links.push_back(ia);
   1093 			}
   1094 			for (int ii=0;ii<nodeLinks.size();ii++)
   1095 			{
   1096 				int i=ii;
   1097 
   1098 				for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
   1099 				{
   1100 					int k = nodeLinks[ii].m_links[jj];
   1101 					for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
   1102 					{
   1103 						int j = nodeLinks[k].m_links[kk];
   1104 						if (i!=j)
   1105 						{
   1106 							const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
   1107 							btAssert(sum==2);
   1108 							if(adj[IDX(i,j)]>sum)
   1109 							{
   1110 								adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
   1111 							}
   1112 						}
   1113 
   1114 					}
   1115 				}
   1116 			}
   1117 		} else
   1118 		{
   1119 			///generic Floyd's algorithm
   1120 			for(int k=0;k<n;++k)
   1121 			{
   1122 				for(j=0;j<n;++j)
   1123 				{
   1124 					for(i=j+1;i<n;++i)
   1125 					{
   1126 						const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
   1127 						if(adj[IDX(i,j)]>sum)
   1128 						{
   1129 							adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
   1130 						}
   1131 					}
   1132 				}
   1133 			}
   1134 		}
   1135 
   1136 
   1137 		/* Build links	*/
   1138 		int	nlinks=0;
   1139 		for(j=0;j<n;++j)
   1140 		{
   1141 			for(i=j+1;i<n;++i)
   1142 			{
   1143 				if(adj[IDX(i,j)]==(unsigned)distance)
   1144 				{
   1145 					appendLink(i,j,mat);
   1146 					m_links[m_links.size()-1].m_bbending=1;
   1147 					++nlinks;
   1148 				}
   1149 			}
   1150 		}
   1151 		delete[] adj;
   1152 		return(nlinks);
   1153 	}
   1154 	return(0);
   1155 }
   1156 
   1157 //
   1158 void			btSoftBody::randomizeConstraints()
   1159 {
   1160 	unsigned long	seed=243703;
   1161 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
   1162 	int i,ni;
   1163 
   1164 	for(i=0,ni=m_links.size();i<ni;++i)
   1165 	{
   1166 		btSwap(m_links[i],m_links[NEXTRAND%ni]);
   1167 	}
   1168 	for(i=0,ni=m_faces.size();i<ni;++i)
   1169 	{
   1170 		btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
   1171 	}
   1172 #undef NEXTRAND
   1173 }
   1174 
   1175 //
   1176 void			btSoftBody::releaseCluster(int index)
   1177 {
   1178 	Cluster*	c=m_clusters[index];
   1179 	if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
   1180 	c->~Cluster();
   1181 	btAlignedFree(c);
   1182 	m_clusters.remove(c);
   1183 }
   1184 
   1185 //
   1186 void			btSoftBody::releaseClusters()
   1187 {
   1188 	while(m_clusters.size()>0) releaseCluster(0);
   1189 }
   1190 
   1191 //
   1192 int				btSoftBody::generateClusters(int k,int maxiterations)
   1193 {
   1194 	int i;
   1195 	releaseClusters();
   1196 	m_clusters.resize(btMin(k,m_nodes.size()));
   1197 	for(i=0;i<m_clusters.size();++i)
   1198 	{
   1199 		m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
   1200 		m_clusters[i]->m_collide=	true;
   1201 	}
   1202 	k=m_clusters.size();
   1203 	if(k>0)
   1204 	{
   1205 		/* Initialize		*/
   1206 		btAlignedObjectArray<btVector3>	centers;
   1207 		btVector3						cog(0,0,0);
   1208 		int								i;
   1209 		for(i=0;i<m_nodes.size();++i)
   1210 		{
   1211 			cog+=m_nodes[i].m_x;
   1212 			m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
   1213 		}
   1214 		cog/=(btScalar)m_nodes.size();
   1215 		centers.resize(k,cog);
   1216 		/* Iterate			*/
   1217 		const btScalar	slope=16;
   1218 		bool			changed;
   1219 		int				iterations=0;
   1220 		do	{
   1221 			const btScalar	w=2-btMin<btScalar>(1,iterations/slope);
   1222 			changed=false;
   1223 			iterations++;
   1224 			int i;
   1225 
   1226 			for(i=0;i<k;++i)
   1227 			{
   1228 				btVector3	c(0,0,0);
   1229 				for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
   1230 				{
   1231 					c+=m_clusters[i]->m_nodes[j]->m_x;
   1232 				}
   1233 				if(m_clusters[i]->m_nodes.size())
   1234 				{
   1235 					c			/=	(btScalar)m_clusters[i]->m_nodes.size();
   1236 					c			=	centers[i]+(c-centers[i])*w;
   1237 					changed		|=	((c-centers[i]).length2()>SIMD_EPSILON);
   1238 					centers[i]	=	c;
   1239 					m_clusters[i]->m_nodes.resize(0);
   1240 				}
   1241 			}
   1242 			for(i=0;i<m_nodes.size();++i)
   1243 			{
   1244 				const btVector3	nx=m_nodes[i].m_x;
   1245 				int				kbest=0;
   1246 				btScalar		kdist=ClusterMetric(centers[0],nx);
   1247 				for(int j=1;j<k;++j)
   1248 				{
   1249 					const btScalar	d=ClusterMetric(centers[j],nx);
   1250 					if(d<kdist)
   1251 					{
   1252 						kbest=j;
   1253 						kdist=d;
   1254 					}
   1255 				}
   1256 				m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
   1257 			}
   1258 		} while(changed&&(iterations<maxiterations));
   1259 		/* Merge		*/
   1260 		btAlignedObjectArray<int>	cids;
   1261 		cids.resize(m_nodes.size(),-1);
   1262 		for(i=0;i<m_clusters.size();++i)
   1263 		{
   1264 			for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
   1265 			{
   1266 				cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
   1267 			}
   1268 		}
   1269 		for(i=0;i<m_faces.size();++i)
   1270 		{
   1271 			const int idx[]={	int(m_faces[i].m_n[0]-&m_nodes[0]),
   1272 				int(m_faces[i].m_n[1]-&m_nodes[0]),
   1273 				int(m_faces[i].m_n[2]-&m_nodes[0])};
   1274 			for(int j=0;j<3;++j)
   1275 			{
   1276 				const int cid=cids[idx[j]];
   1277 				for(int q=1;q<3;++q)
   1278 				{
   1279 					const int kid=idx[(j+q)%3];
   1280 					if(cids[kid]!=cid)
   1281 					{
   1282 						if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
   1283 						{
   1284 							m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
   1285 						}
   1286 					}
   1287 				}
   1288 			}
   1289 		}
   1290 		/* Master		*/
   1291 		if(m_clusters.size()>1)
   1292 		{
   1293 			Cluster*	pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
   1294 			pmaster->m_collide	=	false;
   1295 			pmaster->m_nodes.reserve(m_nodes.size());
   1296 			for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
   1297 			m_clusters.push_back(pmaster);
   1298 			btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
   1299 		}
   1300 		/* Terminate	*/
   1301 		for(i=0;i<m_clusters.size();++i)
   1302 		{
   1303 			if(m_clusters[i]->m_nodes.size()==0)
   1304 			{
   1305 				releaseCluster(i--);
   1306 			}
   1307 		}
   1308 	} else
   1309 	{
   1310 		//create a cluster for each tetrahedron (if tetrahedra exist) or each face
   1311 		if (m_tetras.size())
   1312 		{
   1313 			m_clusters.resize(m_tetras.size());
   1314 			for(i=0;i<m_clusters.size();++i)
   1315 			{
   1316 				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
   1317 				m_clusters[i]->m_collide=	true;
   1318 			}
   1319 			for (i=0;i<m_tetras.size();i++)
   1320 			{
   1321 				for (int j=0;j<4;j++)
   1322 				{
   1323 					m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
   1324 				}
   1325 			}
   1326 
   1327 		} else
   1328 		{
   1329 			m_clusters.resize(m_faces.size());
   1330 			for(i=0;i<m_clusters.size();++i)
   1331 			{
   1332 				m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
   1333 				m_clusters[i]->m_collide=	true;
   1334 			}
   1335 
   1336 			for(i=0;i<m_faces.size();++i)
   1337 			{
   1338 				for(int j=0;j<3;++j)
   1339 				{
   1340 					m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
   1341 				}
   1342 			}
   1343 		}
   1344 	}
   1345 
   1346 	if (m_clusters.size())
   1347 	{
   1348 		initializeClusters();
   1349 		updateClusters();
   1350 
   1351 
   1352 		//for self-collision
   1353 		m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
   1354 		{
   1355 			for (int c0=0;c0<m_clusters.size();c0++)
   1356 			{
   1357 				m_clusters[c0]->m_clusterIndex=c0;
   1358 				for (int c1=0;c1<m_clusters.size();c1++)
   1359 				{
   1360 
   1361 					bool connected=false;
   1362 					Cluster* cla = m_clusters[c0];
   1363 					Cluster* clb = m_clusters[c1];
   1364 					for (int i=0;!connected&&i<cla->m_nodes.size();i++)
   1365 					{
   1366 						for (int j=0;j<clb->m_nodes.size();j++)
   1367 						{
   1368 							if (cla->m_nodes[i] == clb->m_nodes[j])
   1369 							{
   1370 								connected=true;
   1371 								break;
   1372 							}
   1373 						}
   1374 					}
   1375 					m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
   1376 				}
   1377 			}
   1378 		}
   1379 	}
   1380 
   1381 	return(m_clusters.size());
   1382 }
   1383 
   1384 //
   1385 void			btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
   1386 {
   1387 	const Node*			nbase = &m_nodes[0];
   1388 	int					ncount = m_nodes.size();
   1389 	btSymMatrix<int>	edges(ncount,-2);
   1390 	int					newnodes=0;
   1391 	int i,j,k,ni;
   1392 
   1393 	/* Filter out		*/
   1394 	for(i=0;i<m_links.size();++i)
   1395 	{
   1396 		Link&	l=m_links[i];
   1397 		if(l.m_bbending)
   1398 		{
   1399 			if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
   1400 			{
   1401 				btSwap(m_links[i],m_links[m_links.size()-1]);
   1402 				m_links.pop_back();--i;
   1403 			}
   1404 		}
   1405 	}
   1406 	/* Fill edges		*/
   1407 	for(i=0;i<m_links.size();++i)
   1408 	{
   1409 		Link&	l=m_links[i];
   1410 		edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
   1411 	}
   1412 	for(i=0;i<m_faces.size();++i)
   1413 	{
   1414 		Face&	f=m_faces[i];
   1415 		edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
   1416 		edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
   1417 		edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
   1418 	}
   1419 	/* Intersect		*/
   1420 	for(i=0;i<ncount;++i)
   1421 	{
   1422 		for(j=i+1;j<ncount;++j)
   1423 		{
   1424 			if(edges(i,j)==-1)
   1425 			{
   1426 				Node&			a=m_nodes[i];
   1427 				Node&			b=m_nodes[j];
   1428 				const btScalar	t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
   1429 				if(t>0)
   1430 				{
   1431 					const btVector3	x=Lerp(a.m_x,b.m_x,t);
   1432 					const btVector3	v=Lerp(a.m_v,b.m_v,t);
   1433 					btScalar		m=0;
   1434 					if(a.m_im>0)
   1435 					{
   1436 						if(b.m_im>0)
   1437 						{
   1438 							const btScalar	ma=1/a.m_im;
   1439 							const btScalar	mb=1/b.m_im;
   1440 							const btScalar	mc=Lerp(ma,mb,t);
   1441 							const btScalar	f=(ma+mb)/(ma+mb+mc);
   1442 							a.m_im=1/(ma*f);
   1443 							b.m_im=1/(mb*f);
   1444 							m=mc*f;
   1445 						}
   1446 						else
   1447 						{ a.m_im/=0.5f;m=1/a.m_im; }
   1448 					}
   1449 					else
   1450 					{
   1451 						if(b.m_im>0)
   1452 						{ b.m_im/=0.5f;m=1/b.m_im; }
   1453 						else
   1454 							m=0;
   1455 					}
   1456 					appendNode(x,m);
   1457 					edges(i,j)=m_nodes.size()-1;
   1458 					m_nodes[edges(i,j)].m_v=v;
   1459 					++newnodes;
   1460 				}
   1461 			}
   1462 		}
   1463 	}
   1464 	nbase=&m_nodes[0];
   1465 	/* Refine links		*/
   1466 	for(i=0,ni=m_links.size();i<ni;++i)
   1467 	{
   1468 		Link&		feat=m_links[i];
   1469 		const int	idx[]={	int(feat.m_n[0]-nbase),
   1470 			int(feat.m_n[1]-nbase)};
   1471 		if((idx[0]<ncount)&&(idx[1]<ncount))
   1472 		{
   1473 			const int ni=edges(idx[0],idx[1]);
   1474 			if(ni>0)
   1475 			{
   1476 				appendLink(i);
   1477 				Link*		pft[]={	&m_links[i],
   1478 					&m_links[m_links.size()-1]};
   1479 				pft[0]->m_n[0]=&m_nodes[idx[0]];
   1480 				pft[0]->m_n[1]=&m_nodes[ni];
   1481 				pft[1]->m_n[0]=&m_nodes[ni];
   1482 				pft[1]->m_n[1]=&m_nodes[idx[1]];
   1483 			}
   1484 		}
   1485 	}
   1486 	/* Refine faces		*/
   1487 	for(i=0;i<m_faces.size();++i)
   1488 	{
   1489 		const Face&	feat=m_faces[i];
   1490 		const int	idx[]={	int(feat.m_n[0]-nbase),
   1491 			int(feat.m_n[1]-nbase),
   1492 			int(feat.m_n[2]-nbase)};
   1493 		for(j=2,k=0;k<3;j=k++)
   1494 		{
   1495 			if((idx[j]<ncount)&&(idx[k]<ncount))
   1496 			{
   1497 				const int ni=edges(idx[j],idx[k]);
   1498 				if(ni>0)
   1499 				{
   1500 					appendFace(i);
   1501 					const int	l=(k+1)%3;
   1502 					Face*		pft[]={	&m_faces[i],
   1503 						&m_faces[m_faces.size()-1]};
   1504 					pft[0]->m_n[0]=&m_nodes[idx[l]];
   1505 					pft[0]->m_n[1]=&m_nodes[idx[j]];
   1506 					pft[0]->m_n[2]=&m_nodes[ni];
   1507 					pft[1]->m_n[0]=&m_nodes[ni];
   1508 					pft[1]->m_n[1]=&m_nodes[idx[k]];
   1509 					pft[1]->m_n[2]=&m_nodes[idx[l]];
   1510 					appendLink(ni,idx[l],pft[0]->m_material);
   1511 					--i;break;
   1512 				}
   1513 			}
   1514 		}
   1515 	}
   1516 	/* Cut				*/
   1517 	if(cut)
   1518 	{
   1519 		btAlignedObjectArray<int>	cnodes;
   1520 		const int					pcount=ncount;
   1521 		int							i;
   1522 		ncount=m_nodes.size();
   1523 		cnodes.resize(ncount,0);
   1524 		/* Nodes		*/
   1525 		for(i=0;i<ncount;++i)
   1526 		{
   1527 			const btVector3	x=m_nodes[i].m_x;
   1528 			if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
   1529 			{
   1530 				const btVector3	v=m_nodes[i].m_v;
   1531 				btScalar		m=getMass(i);
   1532 				if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; }
   1533 				appendNode(x,m);
   1534 				cnodes[i]=m_nodes.size()-1;
   1535 				m_nodes[cnodes[i]].m_v=v;
   1536 			}
   1537 		}
   1538 		nbase=&m_nodes[0];
   1539 		/* Links		*/
   1540 		for(i=0,ni=m_links.size();i<ni;++i)
   1541 		{
   1542 			const int		id[]={	int(m_links[i].m_n[0]-nbase),
   1543 				int(m_links[i].m_n[1]-nbase)};
   1544 			int				todetach=0;
   1545 			if(cnodes[id[0]]&&cnodes[id[1]])
   1546 			{
   1547 				appendLink(i);
   1548 				todetach=m_links.size()-1;
   1549 			}
   1550 			else
   1551 			{
   1552 				if((	(ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
   1553 					(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
   1554 					todetach=i;
   1555 			}
   1556 			if(todetach)
   1557 			{
   1558 				Link&	l=m_links[todetach];
   1559 				for(int j=0;j<2;++j)
   1560 				{
   1561 					int cn=cnodes[int(l.m_n[j]-nbase)];
   1562 					if(cn) l.m_n[j]=&m_nodes[cn];
   1563 				}
   1564 			}
   1565 		}
   1566 		/* Faces		*/
   1567 		for(i=0,ni=m_faces.size();i<ni;++i)
   1568 		{
   1569 			Node**			n=	m_faces[i].m_n;
   1570 			if(	(ifn->Eval(n[0]->m_x)<accurary)&&
   1571 				(ifn->Eval(n[1]->m_x)<accurary)&&
   1572 				(ifn->Eval(n[2]->m_x)<accurary))
   1573 			{
   1574 				for(int j=0;j<3;++j)
   1575 				{
   1576 					int cn=cnodes[int(n[j]-nbase)];
   1577 					if(cn) n[j]=&m_nodes[cn];
   1578 				}
   1579 			}
   1580 		}
   1581 		/* Clean orphans	*/
   1582 		int							nnodes=m_nodes.size();
   1583 		btAlignedObjectArray<int>	ranks;
   1584 		btAlignedObjectArray<int>	todelete;
   1585 		ranks.resize(nnodes,0);
   1586 		for(i=0,ni=m_links.size();i<ni;++i)
   1587 		{
   1588 			for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
   1589 		}
   1590 		for(i=0,ni=m_faces.size();i<ni;++i)
   1591 		{
   1592 			for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
   1593 		}
   1594 		for(i=0;i<m_links.size();++i)
   1595 		{
   1596 			const int	id[]={	int(m_links[i].m_n[0]-nbase),
   1597 				int(m_links[i].m_n[1]-nbase)};
   1598 			const bool	sg[]={	ranks[id[0]]==1,
   1599 				ranks[id[1]]==1};
   1600 			if(sg[0]||sg[1])
   1601 			{
   1602 				--ranks[id[0]];
   1603 				--ranks[id[1]];
   1604 				btSwap(m_links[i],m_links[m_links.size()-1]);
   1605 				m_links.pop_back();--i;
   1606 			}
   1607 		}
   1608 #if 0
   1609 		for(i=nnodes-1;i>=0;--i)
   1610 		{
   1611 			if(!ranks[i]) todelete.push_back(i);
   1612 		}
   1613 		if(todelete.size())
   1614 		{
   1615 			btAlignedObjectArray<int>&	map=ranks;
   1616 			for(int i=0;i<nnodes;++i) map[i]=i;
   1617 			PointersToIndices(this);
   1618 			for(int i=0,ni=todelete.size();i<ni;++i)
   1619 			{
   1620 				int		j=todelete[i];
   1621 				int&	a=map[j];
   1622 				int&	b=map[--nnodes];
   1623 				m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
   1624 				btSwap(m_nodes[a],m_nodes[b]);
   1625 				j=a;a=b;b=j;
   1626 			}
   1627 			IndicesToPointers(this,&map[0]);
   1628 			m_nodes.resize(nnodes);
   1629 		}
   1630 #endif
   1631 	}
   1632 	m_bUpdateRtCst=true;
   1633 }
   1634 
   1635 //
   1636 bool			btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
   1637 {
   1638 	return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
   1639 }
   1640 
   1641 //
   1642 bool			btSoftBody::cutLink(int node0,int node1,btScalar position)
   1643 {
   1644 	bool			done=false;
   1645 	int i,ni;
   1646 //	const btVector3	d=m_nodes[node0].m_x-m_nodes[node1].m_x;
   1647 	const btVector3	x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
   1648 	const btVector3	v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
   1649 	const btScalar	m=1;
   1650 	appendNode(x,m);
   1651 	appendNode(x,m);
   1652 	Node*			pa=&m_nodes[node0];
   1653 	Node*			pb=&m_nodes[node1];
   1654 	Node*			pn[2]={	&m_nodes[m_nodes.size()-2],
   1655 		&m_nodes[m_nodes.size()-1]};
   1656 	pn[0]->m_v=v;
   1657 	pn[1]->m_v=v;
   1658 	for(i=0,ni=m_links.size();i<ni;++i)
   1659 	{
   1660 		const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
   1661 		if(mtch!=-1)
   1662 		{
   1663 			appendLink(i);
   1664 			Link*	pft[]={&m_links[i],&m_links[m_links.size()-1]};
   1665 			pft[0]->m_n[1]=pn[mtch];
   1666 			pft[1]->m_n[0]=pn[1-mtch];
   1667 			done=true;
   1668 		}
   1669 	}
   1670 	for(i=0,ni=m_faces.size();i<ni;++i)
   1671 	{
   1672 		for(int k=2,l=0;l<3;k=l++)
   1673 		{
   1674 			const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
   1675 			if(mtch!=-1)
   1676 			{
   1677 				appendFace(i);
   1678 				Face*	pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
   1679 				pft[0]->m_n[l]=pn[mtch];
   1680 				pft[1]->m_n[k]=pn[1-mtch];
   1681 				appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
   1682 				appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
   1683 			}
   1684 		}
   1685 	}
   1686 	if(!done)
   1687 	{
   1688 		m_ndbvt.remove(pn[0]->m_leaf);
   1689 		m_ndbvt.remove(pn[1]->m_leaf);
   1690 		m_nodes.pop_back();
   1691 		m_nodes.pop_back();
   1692 	}
   1693 	return(done);
   1694 }
   1695 
   1696 //
   1697 bool			btSoftBody::rayTest(const btVector3& rayFrom,
   1698 									const btVector3& rayTo,
   1699 									sRayCast& results)
   1700 {
   1701 	if(m_faces.size()&&m_fdbvt.empty())
   1702 		initializeFaceTree();
   1703 
   1704 	results.body	=	this;
   1705 	results.fraction = 1.f;
   1706 	results.feature	=	eFeature::None;
   1707 	results.index	=	-1;
   1708 
   1709 	return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
   1710 }
   1711 
   1712 //
   1713 void			btSoftBody::setSolver(eSolverPresets::_ preset)
   1714 {
   1715 	m_cfg.m_vsequence.clear();
   1716 	m_cfg.m_psequence.clear();
   1717 	m_cfg.m_dsequence.clear();
   1718 	switch(preset)
   1719 	{
   1720 	case	eSolverPresets::Positions:
   1721 		m_cfg.m_psequence.push_back(ePSolver::Anchors);
   1722 		m_cfg.m_psequence.push_back(ePSolver::RContacts);
   1723 		m_cfg.m_psequence.push_back(ePSolver::SContacts);
   1724 		m_cfg.m_psequence.push_back(ePSolver::Linear);
   1725 		break;
   1726 	case	eSolverPresets::Velocities:
   1727 		m_cfg.m_vsequence.push_back(eVSolver::Linear);
   1728 
   1729 		m_cfg.m_psequence.push_back(ePSolver::Anchors);
   1730 		m_cfg.m_psequence.push_back(ePSolver::RContacts);
   1731 		m_cfg.m_psequence.push_back(ePSolver::SContacts);
   1732 
   1733 		m_cfg.m_dsequence.push_back(ePSolver::Linear);
   1734 		break;
   1735 	}
   1736 }
   1737 
   1738 //
   1739 void			btSoftBody::predictMotion(btScalar dt)
   1740 {
   1741 
   1742 	int i,ni;
   1743 
   1744 	/* Update				*/
   1745 	if(m_bUpdateRtCst)
   1746 	{
   1747 		m_bUpdateRtCst=false;
   1748 		updateConstants();
   1749 		m_fdbvt.clear();
   1750 		if(m_cfg.collisions&fCollision::VF_SS)
   1751 		{
   1752 			initializeFaceTree();
   1753 		}
   1754 	}
   1755 
   1756 	/* Prepare				*/
   1757 	m_sst.sdt		=	dt*m_cfg.timescale;
   1758 	m_sst.isdt		=	1/m_sst.sdt;
   1759 	m_sst.velmrg	=	m_sst.sdt*3;
   1760 	m_sst.radmrg	=	getCollisionShape()->getMargin();
   1761 	m_sst.updmrg	=	m_sst.radmrg*(btScalar)0.25;
   1762 	/* Forces				*/
   1763 	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
   1764 	applyForces();
   1765 	/* Integrate			*/
   1766 	for(i=0,ni=m_nodes.size();i<ni;++i)
   1767 	{
   1768 		Node&	n=m_nodes[i];
   1769 		n.m_q	=	n.m_x;
   1770 		btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt;
   1771 		{
   1772 			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
   1773 			btScalar clampDeltaV = maxDisplacement/m_sst.sdt;
   1774 			for (int c=0;c<3;c++)
   1775 			{
   1776 				if (deltaV[c]>clampDeltaV)
   1777 				{
   1778 					deltaV[c] = clampDeltaV;
   1779 				}
   1780 				if (deltaV[c]<-clampDeltaV)
   1781 				{
   1782 					deltaV[c]=-clampDeltaV;
   1783 				}
   1784 			}
   1785 		}
   1786 		n.m_v	+=	deltaV;
   1787 		n.m_x	+=	n.m_v*m_sst.sdt;
   1788 		n.m_f	=	btVector3(0,0,0);
   1789 	}
   1790 	/* Clusters				*/
   1791 	updateClusters();
   1792 	/* Bounds				*/
   1793 	updateBounds();
   1794 	/* Nodes				*/
   1795 	ATTRIBUTE_ALIGNED16(btDbvtVolume)	vol;
   1796 	for(i=0,ni=m_nodes.size();i<ni;++i)
   1797 	{
   1798 		Node&	n=m_nodes[i];
   1799 		vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
   1800 		m_ndbvt.update(	n.m_leaf,
   1801 			vol,
   1802 			n.m_v*m_sst.velmrg,
   1803 			m_sst.updmrg);
   1804 	}
   1805 	/* Faces				*/
   1806 	if(!m_fdbvt.empty())
   1807 	{
   1808 		for(int i=0;i<m_faces.size();++i)
   1809 		{
   1810 			Face&			f=m_faces[i];
   1811 			const btVector3	v=(	f.m_n[0]->m_v+
   1812 				f.m_n[1]->m_v+
   1813 				f.m_n[2]->m_v)/3;
   1814 			vol = VolumeOf(f,m_sst.radmrg);
   1815 			m_fdbvt.update(	f.m_leaf,
   1816 				vol,
   1817 				v*m_sst.velmrg,
   1818 				m_sst.updmrg);
   1819 		}
   1820 	}
   1821 	/* Pose					*/
   1822 	updatePose();
   1823 	/* Match				*/
   1824 	if(m_pose.m_bframe&&(m_cfg.kMT>0))
   1825 	{
   1826 		const btMatrix3x3	posetrs=m_pose.m_rot;
   1827 		for(int i=0,ni=m_nodes.size();i<ni;++i)
   1828 		{
   1829 			Node&	n=m_nodes[i];
   1830 			if(n.m_im>0)
   1831 			{
   1832 				const btVector3	x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
   1833 				n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
   1834 			}
   1835 		}
   1836 	}
   1837 	/* Clear contacts		*/
   1838 	m_rcontacts.resize(0);
   1839 	m_scontacts.resize(0);
   1840 	/* Optimize dbvt's		*/
   1841 	m_ndbvt.optimizeIncremental(1);
   1842 	m_fdbvt.optimizeIncremental(1);
   1843 	m_cdbvt.optimizeIncremental(1);
   1844 }
   1845 
   1846 //
   1847 void			btSoftBody::solveConstraints()
   1848 {
   1849 
   1850 	/* Apply clusters		*/
   1851 	applyClusters(false);
   1852 	/* Prepare links		*/
   1853 
   1854 	int i,ni;
   1855 
   1856 	for(i=0,ni=m_links.size();i<ni;++i)
   1857 	{
   1858 		Link&	l=m_links[i];
   1859 		l.m_c3		=	l.m_n[1]->m_q-l.m_n[0]->m_q;
   1860 		l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
   1861 	}
   1862 	/* Prepare anchors		*/
   1863 	for(i=0,ni=m_anchors.size();i<ni;++i)
   1864 	{
   1865 		Anchor&			a=m_anchors[i];
   1866 		const btVector3	ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
   1867 		a.m_c0	=	ImpulseMatrix(	m_sst.sdt,
   1868 			a.m_node->m_im,
   1869 			a.m_body->getInvMass(),
   1870 			a.m_body->getInvInertiaTensorWorld(),
   1871 			ra);
   1872 		a.m_c1	=	ra;
   1873 		a.m_c2	=	m_sst.sdt*a.m_node->m_im;
   1874 		a.m_body->activate();
   1875 	}
   1876 	/* Solve velocities		*/
   1877 	if(m_cfg.viterations>0)
   1878 	{
   1879 		/* Solve			*/
   1880 		for(int isolve=0;isolve<m_cfg.viterations;++isolve)
   1881 		{
   1882 			for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
   1883 			{
   1884 				getSolver(m_cfg.m_vsequence[iseq])(this,1);
   1885 			}
   1886 		}
   1887 		/* Update			*/
   1888 		for(i=0,ni=m_nodes.size();i<ni;++i)
   1889 		{
   1890 			Node&	n=m_nodes[i];
   1891 			n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
   1892 		}
   1893 	}
   1894 	/* Solve positions		*/
   1895 	if(m_cfg.piterations>0)
   1896 	{
   1897 		for(int isolve=0;isolve<m_cfg.piterations;++isolve)
   1898 		{
   1899 			const btScalar ti=isolve/(btScalar)m_cfg.piterations;
   1900 			for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
   1901 			{
   1902 				getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
   1903 			}
   1904 		}
   1905 		const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
   1906 		for(i=0,ni=m_nodes.size();i<ni;++i)
   1907 		{
   1908 			Node&	n=m_nodes[i];
   1909 			n.m_v	=	(n.m_x-n.m_q)*vc;
   1910 			n.m_f	=	btVector3(0,0,0);
   1911 		}
   1912 	}
   1913 	/* Solve drift			*/
   1914 	if(m_cfg.diterations>0)
   1915 	{
   1916 		const btScalar	vcf=m_cfg.kVCF*m_sst.isdt;
   1917 		for(i=0,ni=m_nodes.size();i<ni;++i)
   1918 		{
   1919 			Node&	n=m_nodes[i];
   1920 			n.m_q	=	n.m_x;
   1921 		}
   1922 		for(int idrift=0;idrift<m_cfg.diterations;++idrift)
   1923 		{
   1924 			for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
   1925 			{
   1926 				getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
   1927 			}
   1928 		}
   1929 		for(int i=0,ni=m_nodes.size();i<ni;++i)
   1930 		{
   1931 			Node&	n=m_nodes[i];
   1932 			n.m_v	+=	(n.m_x-n.m_q)*vcf;
   1933 		}
   1934 	}
   1935 	/* Apply clusters		*/
   1936 	dampClusters();
   1937 	applyClusters(true);
   1938 }
   1939 
   1940 //
   1941 void			btSoftBody::staticSolve(int iterations)
   1942 {
   1943 	for(int isolve=0;isolve<iterations;++isolve)
   1944 	{
   1945 		for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
   1946 		{
   1947 			getSolver(m_cfg.m_psequence[iseq])(this,1,0);
   1948 		}
   1949 	}
   1950 }
   1951 
   1952 //
   1953 void			btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
   1954 {
   1955 	/// placeholder
   1956 }
   1957 
   1958 //
   1959 void			btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
   1960 {
   1961 	const int	nb=bodies.size();
   1962 	int			iterations=0;
   1963 	int i;
   1964 
   1965 	for(i=0;i<nb;++i)
   1966 	{
   1967 		iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
   1968 	}
   1969 	for(i=0;i<nb;++i)
   1970 	{
   1971 		bodies[i]->prepareClusters(iterations);
   1972 	}
   1973 	for(i=0;i<iterations;++i)
   1974 	{
   1975 		const btScalar sor=1;
   1976 		for(int j=0;j<nb;++j)
   1977 		{
   1978 			bodies[j]->solveClusters(sor);
   1979 		}
   1980 	}
   1981 	for(i=0;i<nb;++i)
   1982 	{
   1983 		bodies[i]->cleanupClusters();
   1984 	}
   1985 }
   1986 
   1987 //
   1988 void			btSoftBody::integrateMotion()
   1989 {
   1990 	/* Update			*/
   1991 	updateNormals();
   1992 }
   1993 
   1994 //
   1995 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
   1996 {
   1997 	m_rayFrom = rayFrom;
   1998 	m_rayNormalizedDirection = (rayTo-rayFrom);
   1999 	m_rayTo = rayTo;
   2000 	m_mint	=	mxt;
   2001 	m_face	=	0;
   2002 	m_tests	=	0;
   2003 }
   2004 
   2005 //
   2006 void				btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
   2007 {
   2008 	btSoftBody::Face&	f=*(btSoftBody::Face*)leaf->data;
   2009 	const btScalar		t=rayFromToTriangle(	m_rayFrom,m_rayTo,m_rayNormalizedDirection,
   2010 		f.m_n[0]->m_x,
   2011 		f.m_n[1]->m_x,
   2012 		f.m_n[2]->m_x,
   2013 		m_mint);
   2014 	if((t>0)&&(t<m_mint))
   2015 	{
   2016 		m_mint=t;m_face=&f;
   2017 	}
   2018 	++m_tests;
   2019 }
   2020 
   2021 //
   2022 btScalar			btSoftBody::RayFromToCaster::rayFromToTriangle(	const btVector3& rayFrom,
   2023 																   const btVector3& rayTo,
   2024 																   const btVector3& rayNormalizedDirection,
   2025 																   const btVector3& a,
   2026 																   const btVector3& b,
   2027 																   const btVector3& c,
   2028 																   btScalar maxt)
   2029 {
   2030 	static const btScalar	ceps=-SIMD_EPSILON*10;
   2031 	static const btScalar	teps=SIMD_EPSILON*10;
   2032 
   2033 	const btVector3			n=btCross(b-a,c-a);
   2034 	const btScalar			d=btDot(a,n);
   2035 	const btScalar			den=btDot(rayNormalizedDirection,n);
   2036 	if(!btFuzzyZero(den))
   2037 	{
   2038 		const btScalar		num=btDot(rayFrom,n)-d;
   2039 		const btScalar		t=-num/den;
   2040 		if((t>teps)&&(t<maxt))
   2041 		{
   2042 			const btVector3	hit=rayFrom+rayNormalizedDirection*t;
   2043 			if(	(btDot(n,btCross(a-hit,b-hit))>ceps)	&&
   2044 				(btDot(n,btCross(b-hit,c-hit))>ceps)	&&
   2045 				(btDot(n,btCross(c-hit,a-hit))>ceps))
   2046 			{
   2047 				return(t);
   2048 			}
   2049 		}
   2050 	}
   2051 	return(-1);
   2052 }
   2053 
   2054 //
   2055 void				btSoftBody::pointersToIndices()
   2056 {
   2057 #define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
   2058 	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0] : 0;
   2059 	int i,ni;
   2060 
   2061 	for(i=0,ni=m_nodes.size();i<ni;++i)
   2062 	{
   2063 		if(m_nodes[i].m_leaf)
   2064 		{
   2065 			m_nodes[i].m_leaf->data=*(void**)&i;
   2066 		}
   2067 	}
   2068 	for(i=0,ni=m_links.size();i<ni;++i)
   2069 	{
   2070 		m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
   2071 		m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
   2072 	}
   2073 	for(i=0,ni=m_faces.size();i<ni;++i)
   2074 	{
   2075 		m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
   2076 		m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
   2077 		m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
   2078 		if(m_faces[i].m_leaf)
   2079 		{
   2080 			m_faces[i].m_leaf->data=*(void**)&i;
   2081 		}
   2082 	}
   2083 	for(i=0,ni=m_anchors.size();i<ni;++i)
   2084 	{
   2085 		m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
   2086 	}
   2087 	for(i=0,ni=m_notes.size();i<ni;++i)
   2088 	{
   2089 		for(int j=0;j<m_notes[i].m_rank;++j)
   2090 		{
   2091 			m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
   2092 		}
   2093 	}
   2094 #undef	PTR2IDX
   2095 }
   2096 
   2097 //
   2098 void				btSoftBody::indicesToPointers(const int* map)
   2099 {
   2100 #define	IDX2PTR(_p_,_b_)	map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):	\
   2101 	(&(_b_)[(((char*)_p_)-(char*)0)])
   2102 	btSoftBody::Node*	base=m_nodes.size() ? &m_nodes[0]:0;
   2103 	int i,ni;
   2104 
   2105 	for(i=0,ni=m_nodes.size();i<ni;++i)
   2106 	{
   2107 		if(m_nodes[i].m_leaf)
   2108 		{
   2109 			m_nodes[i].m_leaf->data=&m_nodes[i];
   2110 		}
   2111 	}
   2112 	for(i=0,ni=m_links.size();i<ni;++i)
   2113 	{
   2114 		m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
   2115 		m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
   2116 	}
   2117 	for(i=0,ni=m_faces.size();i<ni;++i)
   2118 	{
   2119 		m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
   2120 		m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
   2121 		m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
   2122 		if(m_faces[i].m_leaf)
   2123 		{
   2124 			m_faces[i].m_leaf->data=&m_faces[i];
   2125 		}
   2126 	}
   2127 	for(i=0,ni=m_anchors.size();i<ni;++i)
   2128 	{
   2129 		m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
   2130 	}
   2131 	for(i=0,ni=m_notes.size();i<ni;++i)
   2132 	{
   2133 		for(int j=0;j<m_notes[i].m_rank;++j)
   2134 		{
   2135 			m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
   2136 		}
   2137 	}
   2138 #undef	IDX2PTR
   2139 }
   2140 
   2141 //
   2142 int					btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
   2143 										btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
   2144 {
   2145 	int	cnt=0;
   2146 	btVector3 dir = rayTo-rayFrom;
   2147 
   2148 
   2149 	if(bcountonly||m_fdbvt.empty())
   2150 	{/* Full search	*/
   2151 
   2152 		for(int i=0,ni=m_faces.size();i<ni;++i)
   2153 		{
   2154 			const btSoftBody::Face&	f=m_faces[i];
   2155 
   2156 			const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
   2157 				f.m_n[0]->m_x,
   2158 				f.m_n[1]->m_x,
   2159 				f.m_n[2]->m_x,
   2160 				mint);
   2161 			if(t>0)
   2162 			{
   2163 				++cnt;
   2164 				if(!bcountonly)
   2165 				{
   2166 					feature=btSoftBody::eFeature::Face;
   2167 					index=i;
   2168 					mint=t;
   2169 				}
   2170 			}
   2171 		}
   2172 	}
   2173 	else
   2174 	{/* Use dbvt	*/
   2175 		RayFromToCaster	collider(rayFrom,rayTo,mint);
   2176 
   2177 		btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
   2178 		if(collider.m_face)
   2179 		{
   2180 			mint=collider.m_mint;
   2181 			feature=btSoftBody::eFeature::Face;
   2182 			index=(int)(collider.m_face-&m_faces[0]);
   2183 			cnt=1;
   2184 		}
   2185 	}
   2186 
   2187 	for (int i=0;i<m_tetras.size();i++)
   2188 	{
   2189 		const btSoftBody::Tetra& tet = m_tetras[i];
   2190 		int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
   2191 		for (int f=0;f<4;f++)
   2192 		{
   2193 
   2194 			int index0=tetfaces[f][0];
   2195 			int index1=tetfaces[f][1];
   2196 			int index2=tetfaces[f][2];
   2197 			btVector3 v0=tet.m_n[index0]->m_x;
   2198 			btVector3 v1=tet.m_n[index1]->m_x;
   2199 			btVector3 v2=tet.m_n[index2]->m_x;
   2200 
   2201 
   2202 		const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
   2203 			v0,v1,v2,
   2204 				mint);
   2205 		if(t>0)
   2206 			{
   2207 				++cnt;
   2208 				if(!bcountonly)
   2209 				{
   2210 					feature=btSoftBody::eFeature::Tetra;
   2211 					index=i;
   2212 					mint=t;
   2213 				}
   2214 			}
   2215 		}
   2216 	}
   2217 	return(cnt);
   2218 }
   2219 
   2220 //
   2221 void			btSoftBody::initializeFaceTree()
   2222 {
   2223 	m_fdbvt.clear();
   2224 	for(int i=0;i<m_faces.size();++i)
   2225 	{
   2226 		Face&	f=m_faces[i];
   2227 		f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
   2228 	}
   2229 }
   2230 
   2231 //
   2232 btVector3		btSoftBody::evaluateCom() const
   2233 {
   2234 	btVector3	com(0,0,0);
   2235 	if(m_pose.m_bframe)
   2236 	{
   2237 		for(int i=0,ni=m_nodes.size();i<ni;++i)
   2238 		{
   2239 			com+=m_nodes[i].m_x*m_pose.m_wgh[i];
   2240 		}
   2241 	}
   2242 	return(com);
   2243 }
   2244 
   2245 //
   2246 bool				btSoftBody::checkContact(	const btCollisionObjectWrapper* colObjWrap,
   2247 											 const btVector3& x,
   2248 											 btScalar margin,
   2249 											 btSoftBody::sCti& cti) const
   2250 {
   2251 	btVector3 nrm;
   2252 	const btCollisionShape *shp = colObjWrap->getCollisionShape();
   2253 //	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
   2254 	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
   2255 	const btTransform &wtr = colObjWrap->getWorldTransform();
   2256 	//todo: check which transform is needed here
   2257 
   2258 	btScalar dst =
   2259 		m_worldInfo->m_sparsesdf.Evaluate(
   2260 			wtr.invXform(x),
   2261 			shp,
   2262 			nrm,
   2263 			margin);
   2264 	if(dst<0)
   2265 	{
   2266 		cti.m_colObj = colObjWrap->getCollisionObject();
   2267 		cti.m_normal = wtr.getBasis()*nrm;
   2268 		cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
   2269 		return(true);
   2270 	}
   2271 	return(false);
   2272 }
   2273 
   2274 //
   2275 void					btSoftBody::updateNormals()
   2276 {
   2277 
   2278 	const btVector3	zv(0,0,0);
   2279 	int i,ni;
   2280 
   2281 	for(i=0,ni=m_nodes.size();i<ni;++i)
   2282 	{
   2283 		m_nodes[i].m_n=zv;
   2284 	}
   2285 	for(i=0,ni=m_faces.size();i<ni;++i)
   2286 	{
   2287 		btSoftBody::Face&	f=m_faces[i];
   2288 		const btVector3		n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
   2289 			f.m_n[2]->m_x-f.m_n[0]->m_x);
   2290 		f.m_normal=n.normalized();
   2291 		f.m_n[0]->m_n+=n;
   2292 		f.m_n[1]->m_n+=n;
   2293 		f.m_n[2]->m_n+=n;
   2294 	}
   2295 	for(i=0,ni=m_nodes.size();i<ni;++i)
   2296 	{
   2297 		btScalar len = m_nodes[i].m_n.length();
   2298 		if (len>SIMD_EPSILON)
   2299 			m_nodes[i].m_n /= len;
   2300 	}
   2301 }
   2302 
   2303 //
   2304 void					btSoftBody::updateBounds()
   2305 {
   2306 	/*if( m_acceleratedSoftBody )
   2307 	{
   2308 		// If we have an accelerated softbody we need to obtain the bounds correctly
   2309 		// For now (slightly hackily) just have a very large AABB
   2310 		// TODO: Write get bounds kernel
   2311 		// If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
   2312 		// probably do a test and exchange reasonably efficiently.
   2313 
   2314 		m_bounds[0] = btVector3(-1000, -1000, -1000);
   2315 		m_bounds[1] = btVector3(1000, 1000, 1000);
   2316 
   2317 	} else {*/
   2318 		if(m_ndbvt.m_root)
   2319 		{
   2320 			const btVector3&	mins=m_ndbvt.m_root->volume.Mins();
   2321 			const btVector3&	maxs=m_ndbvt.m_root->volume.Maxs();
   2322 			const btScalar		csm=getCollisionShape()->getMargin();
   2323 			const btVector3		mrg=btVector3(	csm,
   2324 				csm,
   2325 				csm)*1; // ??? to investigate...
   2326 			m_bounds[0]=mins-mrg;
   2327 			m_bounds[1]=maxs+mrg;
   2328 			if(0!=getBroadphaseHandle())
   2329 			{
   2330 				m_worldInfo->m_broadphase->setAabb(	getBroadphaseHandle(),
   2331 					m_bounds[0],
   2332 					m_bounds[1],
   2333 					m_worldInfo->m_dispatcher);
   2334 			}
   2335 		}
   2336 		else
   2337 		{
   2338 			m_bounds[0]=
   2339 				m_bounds[1]=btVector3(0,0,0);
   2340 		}
   2341 	//}
   2342 }
   2343 
   2344 
   2345 //
   2346 void					btSoftBody::updatePose()
   2347 {
   2348 	if(m_pose.m_bframe)
   2349 	{
   2350 		btSoftBody::Pose&	pose=m_pose;
   2351 		const btVector3		com=evaluateCom();
   2352 		/* Com			*/
   2353 		pose.m_com	=	com;
   2354 		/* Rotation		*/
   2355 		btMatrix3x3		Apq;
   2356 		const btScalar	eps=SIMD_EPSILON;
   2357 		Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
   2358 		Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
   2359 		for(int i=0,ni=m_nodes.size();i<ni;++i)
   2360 		{
   2361 			const btVector3		a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
   2362 			const btVector3&	b=pose.m_pos[i];
   2363 			Apq[0]+=a.x()*b;
   2364 			Apq[1]+=a.y()*b;
   2365 			Apq[2]+=a.z()*b;
   2366 		}
   2367 		btMatrix3x3		r,s;
   2368 		PolarDecompose(Apq,r,s);
   2369 		pose.m_rot=r;
   2370 		pose.m_scl=pose.m_aqq*r.transpose()*Apq;
   2371 		if(m_cfg.maxvolume>1)
   2372 		{
   2373 			const btScalar	idet=Clamp<btScalar>(	1/pose.m_scl.determinant(),
   2374 				1,m_cfg.maxvolume);
   2375 			pose.m_scl=Mul(pose.m_scl,idet);
   2376 		}
   2377 
   2378 	}
   2379 }
   2380 
   2381 //
   2382 void				btSoftBody::updateArea(bool averageArea)
   2383 {
   2384 	int i,ni;
   2385 
   2386 	/* Face area		*/
   2387 	for(i=0,ni=m_faces.size();i<ni;++i)
   2388 	{
   2389 		Face&		f=m_faces[i];
   2390 		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
   2391 	}
   2392 
   2393 	/* Node area		*/
   2394 
   2395 	if (averageArea)
   2396 	{
   2397 		btAlignedObjectArray<int>	counts;
   2398 		counts.resize(m_nodes.size(),0);
   2399 		for(i=0,ni=m_nodes.size();i<ni;++i)
   2400 		{
   2401 			m_nodes[i].m_area	=	0;
   2402 		}
   2403 		for(i=0,ni=m_faces.size();i<ni;++i)
   2404 		{
   2405 			btSoftBody::Face&	f=m_faces[i];
   2406 			for(int j=0;j<3;++j)
   2407 			{
   2408 				const int index=(int)(f.m_n[j]-&m_nodes[0]);
   2409 				counts[index]++;
   2410 				f.m_n[j]->m_area+=btFabs(f.m_ra);
   2411 			}
   2412 		}
   2413 		for(i=0,ni=m_nodes.size();i<ni;++i)
   2414 		{
   2415 			if(counts[i]>0)
   2416 				m_nodes[i].m_area/=(btScalar)counts[i];
   2417 			else
   2418 				m_nodes[i].m_area=0;
   2419 		}
   2420 	}
   2421 	else
   2422 	{
   2423 		// initialize node area as zero
   2424 		for(i=0,ni=m_nodes.size();i<ni;++i)
   2425 		{
   2426 			m_nodes[i].m_area=0;
   2427 		}
   2428 
   2429 		for(i=0,ni=m_faces.size();i<ni;++i)
   2430 		{
   2431 			btSoftBody::Face&	f=m_faces[i];
   2432 
   2433 			for(int j=0;j<3;++j)
   2434 			{
   2435 				f.m_n[j]->m_area += f.m_ra;
   2436 			}
   2437 		}
   2438 
   2439 		for(i=0,ni=m_nodes.size();i<ni;++i)
   2440 		{
   2441 			m_nodes[i].m_area *= 0.3333333f;
   2442 		}
   2443 	}
   2444 }
   2445 
   2446 
   2447 void				btSoftBody::updateLinkConstants()
   2448 {
   2449 	int i,ni;
   2450 
   2451 	/* Links		*/
   2452 	for(i=0,ni=m_links.size();i<ni;++i)
   2453 	{
   2454 		Link&		l=m_links[i];
   2455 		Material&	m=*l.m_material;
   2456 		l.m_c0	=	(l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
   2457 	}
   2458 }
   2459 
   2460 void				btSoftBody::updateConstants()
   2461 {
   2462 	resetLinkRestLengths();
   2463 	updateLinkConstants();
   2464 	updateArea();
   2465 }
   2466 
   2467 
   2468 
   2469 //
   2470 void					btSoftBody::initializeClusters()
   2471 {
   2472 	int i;
   2473 
   2474 	for( i=0;i<m_clusters.size();++i)
   2475 	{
   2476 		Cluster&	c=*m_clusters[i];
   2477 		c.m_imass=0;
   2478 		c.m_masses.resize(c.m_nodes.size());
   2479 		for(int j=0;j<c.m_nodes.size();++j)
   2480 		{
   2481 			if (c.m_nodes[j]->m_im==0)
   2482 			{
   2483 				c.m_containsAnchor = true;
   2484 				c.m_masses[j]	=	BT_LARGE_FLOAT;
   2485 			} else
   2486 			{
   2487 				c.m_masses[j]	=	btScalar(1.)/c.m_nodes[j]->m_im;
   2488 			}
   2489 			c.m_imass		+=	c.m_masses[j];
   2490 		}
   2491 		c.m_imass		=	btScalar(1.)/c.m_imass;
   2492 		c.m_com			=	btSoftBody::clusterCom(&c);
   2493 		c.m_lv			=	btVector3(0,0,0);
   2494 		c.m_av			=	btVector3(0,0,0);
   2495 		c.m_leaf		=	0;
   2496 		/* Inertia	*/
   2497 		btMatrix3x3&	ii=c.m_locii;
   2498 		ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
   2499 		{
   2500 			int i,ni;
   2501 
   2502 			for(i=0,ni=c.m_nodes.size();i<ni;++i)
   2503 			{
   2504 				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
   2505 				const btVector3	q=k*k;
   2506 				const btScalar	m=c.m_masses[i];
   2507 				ii[0][0]	+=	m*(q[1]+q[2]);
   2508 				ii[1][1]	+=	m*(q[0]+q[2]);
   2509 				ii[2][2]	+=	m*(q[0]+q[1]);
   2510 				ii[0][1]	-=	m*k[0]*k[1];
   2511 				ii[0][2]	-=	m*k[0]*k[2];
   2512 				ii[1][2]	-=	m*k[1]*k[2];
   2513 			}
   2514 		}
   2515 		ii[1][0]=ii[0][1];
   2516 		ii[2][0]=ii[0][2];
   2517 		ii[2][1]=ii[1][2];
   2518 
   2519 		ii = ii.inverse();
   2520 
   2521 		/* Frame	*/
   2522 		c.m_framexform.setIdentity();
   2523 		c.m_framexform.setOrigin(c.m_com);
   2524 		c.m_framerefs.resize(c.m_nodes.size());
   2525 		{
   2526 			int i;
   2527 			for(i=0;i<c.m_framerefs.size();++i)
   2528 			{
   2529 				c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
   2530 			}
   2531 		}
   2532 	}
   2533 }
   2534 
   2535 //
   2536 void					btSoftBody::updateClusters()
   2537 {
   2538 	BT_PROFILE("UpdateClusters");
   2539 	int i;
   2540 
   2541 	for(i=0;i<m_clusters.size();++i)
   2542 	{
   2543 		btSoftBody::Cluster&	c=*m_clusters[i];
   2544 		const int				n=c.m_nodes.size();
   2545 		//const btScalar			invn=1/(btScalar)n;
   2546 		if(n)
   2547 		{
   2548 			/* Frame				*/
   2549 			const btScalar	eps=btScalar(0.0001);
   2550 			btMatrix3x3		m,r,s;
   2551 			m[0]=m[1]=m[2]=btVector3(0,0,0);
   2552 			m[0][0]=eps*1;
   2553 			m[1][1]=eps*2;
   2554 			m[2][2]=eps*3;
   2555 			c.m_com=clusterCom(&c);
   2556 			for(int i=0;i<c.m_nodes.size();++i)
   2557 			{
   2558 				const btVector3		a=c.m_nodes[i]->m_x-c.m_com;
   2559 				const btVector3&	b=c.m_framerefs[i];
   2560 				m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
   2561 			}
   2562 			PolarDecompose(m,r,s);
   2563 			c.m_framexform.setOrigin(c.m_com);
   2564 			c.m_framexform.setBasis(r);
   2565 			/* Inertia			*/
   2566 #if 1/* Constant	*/
   2567 			c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
   2568 #else
   2569 #if 0/* Sphere	*/
   2570 			const btScalar	rk=(2*c.m_extents.length2())/(5*c.m_imass);
   2571 			const btVector3	inertia(rk,rk,rk);
   2572 			const btVector3	iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
   2573 				btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
   2574 				btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
   2575 
   2576 			c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
   2577 #else/* Actual	*/
   2578 			c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
   2579 			for(int i=0;i<n;++i)
   2580 			{
   2581 				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
   2582 				const btVector3		q=k*k;
   2583 				const btScalar		m=1/c.m_nodes[i]->m_im;
   2584 				c.m_invwi[0][0]	+=	m*(q[1]+q[2]);
   2585 				c.m_invwi[1][1]	+=	m*(q[0]+q[2]);
   2586 				c.m_invwi[2][2]	+=	m*(q[0]+q[1]);
   2587 				c.m_invwi[0][1]	-=	m*k[0]*k[1];
   2588 				c.m_invwi[0][2]	-=	m*k[0]*k[2];
   2589 				c.m_invwi[1][2]	-=	m*k[1]*k[2];
   2590 			}
   2591 			c.m_invwi[1][0]=c.m_invwi[0][1];
   2592 			c.m_invwi[2][0]=c.m_invwi[0][2];
   2593 			c.m_invwi[2][1]=c.m_invwi[1][2];
   2594 			c.m_invwi=c.m_invwi.inverse();
   2595 #endif
   2596 #endif
   2597 			/* Velocities			*/
   2598 			c.m_lv=btVector3(0,0,0);
   2599 			c.m_av=btVector3(0,0,0);
   2600 			{
   2601 				int i;
   2602 
   2603 				for(i=0;i<n;++i)
   2604 				{
   2605 					const btVector3	v=c.m_nodes[i]->m_v*c.m_masses[i];
   2606 					c.m_lv	+=	v;
   2607 					c.m_av	+=	btCross(c.m_nodes[i]->m_x-c.m_com,v);
   2608 				}
   2609 			}
   2610 			c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
   2611 			c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
   2612 			c.m_vimpulses[0]	=
   2613 				c.m_vimpulses[1]	= btVector3(0,0,0);
   2614 			c.m_dimpulses[0]	=
   2615 				c.m_dimpulses[1]	= btVector3(0,0,0);
   2616 			c.m_nvimpulses		= 0;
   2617 			c.m_ndimpulses		= 0;
   2618 			/* Matching				*/
   2619 			if(c.m_matching>0)
   2620 			{
   2621 				for(int j=0;j<c.m_nodes.size();++j)
   2622 				{
   2623 					Node&			n=*c.m_nodes[j];
   2624 					const btVector3	x=c.m_framexform*c.m_framerefs[j];
   2625 					n.m_x=Lerp(n.m_x,x,c.m_matching);
   2626 				}
   2627 			}
   2628 			/* Dbvt					*/
   2629 			if(c.m_collide)
   2630 			{
   2631 				btVector3	mi=c.m_nodes[0]->m_x;
   2632 				btVector3	mx=mi;
   2633 				for(int j=1;j<n;++j)
   2634 				{
   2635 					mi.setMin(c.m_nodes[j]->m_x);
   2636 					mx.setMax(c.m_nodes[j]->m_x);
   2637 				}
   2638 				ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(mi,mx);
   2639 				if(c.m_leaf)
   2640 					m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
   2641 				else
   2642 					c.m_leaf=m_cdbvt.insert(bounds,&c);
   2643 			}
   2644 		}
   2645 	}
   2646 
   2647 
   2648 }
   2649 
   2650 
   2651 
   2652 
   2653 //
   2654 void					btSoftBody::cleanupClusters()
   2655 {
   2656 	for(int i=0;i<m_joints.size();++i)
   2657 	{
   2658 		m_joints[i]->Terminate(m_sst.sdt);
   2659 		if(m_joints[i]->m_delete)
   2660 		{
   2661 			btAlignedFree(m_joints[i]);
   2662 			m_joints.remove(m_joints[i--]);
   2663 		}
   2664 	}
   2665 }
   2666 
   2667 //
   2668 void					btSoftBody::prepareClusters(int iterations)
   2669 {
   2670 	for(int i=0;i<m_joints.size();++i)
   2671 	{
   2672 		m_joints[i]->Prepare(m_sst.sdt,iterations);
   2673 	}
   2674 }
   2675 
   2676 
   2677 //
   2678 void					btSoftBody::solveClusters(btScalar sor)
   2679 {
   2680 	for(int i=0,ni=m_joints.size();i<ni;++i)
   2681 	{
   2682 		m_joints[i]->Solve(m_sst.sdt,sor);
   2683 	}
   2684 }
   2685 
   2686 //
   2687 void					btSoftBody::applyClusters(bool drift)
   2688 {
   2689 	BT_PROFILE("ApplyClusters");
   2690 //	const btScalar					f0=m_sst.sdt;
   2691 	//const btScalar					f1=f0/2;
   2692 	btAlignedObjectArray<btVector3> deltas;
   2693 	btAlignedObjectArray<btScalar> weights;
   2694 	deltas.resize(m_nodes.size(),btVector3(0,0,0));
   2695 	weights.resize(m_nodes.size(),0);
   2696 	int i;
   2697 
   2698 	if(drift)
   2699 	{
   2700 		for(i=0;i<m_clusters.size();++i)
   2701 		{
   2702 			Cluster&	c=*m_clusters[i];
   2703 			if(c.m_ndimpulses)
   2704 			{
   2705 				c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
   2706 				c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
   2707 			}
   2708 		}
   2709 	}
   2710 
   2711 	for(i=0;i<m_clusters.size();++i)
   2712 	{
   2713 		Cluster&	c=*m_clusters[i];
   2714 		if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
   2715 		{
   2716 			const btVector3		v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
   2717 			const btVector3		w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
   2718 			for(int j=0;j<c.m_nodes.size();++j)
   2719 			{
   2720 				const int			idx=int(c.m_nodes[j]-&m_nodes[0]);
   2721 				const btVector3&	x=c.m_nodes[j]->m_x;
   2722 				const btScalar		q=c.m_masses[j];
   2723 				deltas[idx]		+=	(v+btCross(w,x-c.m_com))*q;
   2724 				weights[idx]	+=	q;
   2725 			}
   2726 		}
   2727 	}
   2728 	for(i=0;i<deltas.size();++i)
   2729 	{
   2730 		if(weights[i]>0)
   2731 		{
   2732 			m_nodes[i].m_x+=deltas[i]/weights[i];
   2733 		}
   2734 	}
   2735 }
   2736 
   2737 //
   2738 void					btSoftBody::dampClusters()
   2739 {
   2740 	int i;
   2741 
   2742 	for(i=0;i<m_clusters.size();++i)
   2743 	{
   2744 		Cluster&	c=*m_clusters[i];
   2745 		if(c.m_ndamping>0)
   2746 		{
   2747 			for(int j=0;j<c.m_nodes.size();++j)
   2748 			{
   2749 				Node&			n=*c.m_nodes[j];
   2750 				if(n.m_im>0)
   2751 				{
   2752 					const btVector3	vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
   2753 					if(vx.length2()<=n.m_v.length2())
   2754 						{
   2755 						n.m_v	+=	c.m_ndamping*(vx-n.m_v);
   2756 						}
   2757 				}
   2758 			}
   2759 		}
   2760 	}
   2761 }
   2762 
   2763 //
   2764 void				btSoftBody::Joint::Prepare(btScalar dt,int)
   2765 {
   2766 	m_bodies[0].activate();
   2767 	m_bodies[1].activate();
   2768 }
   2769 
   2770 //
   2771 void				btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
   2772 {
   2773 	static const btScalar	maxdrift=4;
   2774 	Joint::Prepare(dt,iterations);
   2775 	m_rpos[0]		=	m_bodies[0].xform()*m_refs[0];
   2776 	m_rpos[1]		=	m_bodies[1].xform()*m_refs[1];
   2777 	m_drift			=	Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
   2778 	m_rpos[0]		-=	m_bodies[0].xform().getOrigin();
   2779 	m_rpos[1]		-=	m_bodies[1].xform().getOrigin();
   2780 	m_massmatrix	=	ImpulseMatrix(	m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
   2781 		m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
   2782 	if(m_split>0)
   2783 	{
   2784 		m_sdrift	=	m_massmatrix*(m_drift*m_split);
   2785 		m_drift		*=	1-m_split;
   2786 	}
   2787 	m_drift	/=(btScalar)iterations;
   2788 }
   2789 
   2790 //
   2791 void				btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
   2792 {
   2793 	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
   2794 	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
   2795 	const btVector3		vr=va-vb;
   2796 	btSoftBody::Impulse	impulse;
   2797 	impulse.m_asVelocity	=	1;
   2798 	impulse.m_velocity		=	m_massmatrix*(m_drift+vr*m_cfm)*sor;
   2799 	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
   2800 	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
   2801 }
   2802 
   2803 //
   2804 void				btSoftBody::LJoint::Terminate(btScalar dt)
   2805 {
   2806 	if(m_split>0)
   2807 	{
   2808 		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
   2809 		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
   2810 	}
   2811 }
   2812 
   2813 //
   2814 void				btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
   2815 {
   2816 	static const btScalar	maxdrift=SIMD_PI/16;
   2817 	m_icontrol->Prepare(this);
   2818 	Joint::Prepare(dt,iterations);
   2819 	m_axis[0]	=	m_bodies[0].xform().getBasis()*m_refs[0];
   2820 	m_axis[1]	=	m_bodies[1].xform().getBasis()*m_refs[1];
   2821 	m_drift		=	NormalizeAny(btCross(m_axis[1],m_axis[0]));
   2822 	m_drift		*=	btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
   2823 	m_drift		*=	m_erp/dt;
   2824 	m_massmatrix=	AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
   2825 	if(m_split>0)
   2826 	{
   2827 		m_sdrift	=	m_massmatrix*(m_drift*m_split);
   2828 		m_drift		*=	1-m_split;
   2829 	}
   2830 	m_drift	/=(btScalar)iterations;
   2831 }
   2832 
   2833 //
   2834 void				btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
   2835 {
   2836 	const btVector3		va=m_bodies[0].angularVelocity();
   2837 	const btVector3		vb=m_bodies[1].angularVelocity();
   2838 	const btVector3		vr=va-vb;
   2839 	const btScalar		sp=btDot(vr,m_axis[0]);
   2840 	const btVector3		vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
   2841 	btSoftBody::Impulse	impulse;
   2842 	impulse.m_asVelocity	=	1;
   2843 	impulse.m_velocity		=	m_massmatrix*(m_drift+vc*m_cfm)*sor;
   2844 	m_bodies[0].applyAImpulse(-impulse);
   2845 	m_bodies[1].applyAImpulse( impulse);
   2846 }
   2847 
   2848 //
   2849 void				btSoftBody::AJoint::Terminate(btScalar dt)
   2850 {
   2851 	if(m_split>0)
   2852 	{
   2853 		m_bodies[0].applyDAImpulse(-m_sdrift);
   2854 		m_bodies[1].applyDAImpulse( m_sdrift);
   2855 	}
   2856 }
   2857 
   2858 //
   2859 void				btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
   2860 {
   2861 	Joint::Prepare(dt,iterations);
   2862 	const bool	dodrift=(m_life==0);
   2863 	m_delete=(++m_life)>m_maxlife;
   2864 	if(dodrift)
   2865 	{
   2866 		m_drift=m_drift*m_erp/dt;
   2867 		if(m_split>0)
   2868 		{
   2869 			m_sdrift	=	m_massmatrix*(m_drift*m_split);
   2870 			m_drift		*=	1-m_split;
   2871 		}
   2872 		m_drift/=(btScalar)iterations;
   2873 	}
   2874 	else
   2875 	{
   2876 		m_drift=m_sdrift=btVector3(0,0,0);
   2877 	}
   2878 }
   2879 
   2880 //
   2881 void				btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
   2882 {
   2883 	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
   2884 	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
   2885 	const btVector3		vrel=va-vb;
   2886 	const btScalar		rvac=btDot(vrel,m_normal);
   2887 	btSoftBody::Impulse	impulse;
   2888 	impulse.m_asVelocity	=	1;
   2889 	impulse.m_velocity		=	m_drift;
   2890 	if(rvac<0)
   2891 	{
   2892 		const btVector3	iv=m_normal*rvac;
   2893 		const btVector3	fv=vrel-iv;
   2894 		impulse.m_velocity	+=	iv+fv*m_friction;
   2895 	}
   2896 	impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
   2897 
   2898 	if (m_bodies[0].m_soft==m_bodies[1].m_soft)
   2899 	{
   2900 		if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
   2901 			(impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
   2902 		{
   2903 			if (impulse.m_asVelocity)
   2904 			{
   2905 				if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
   2906 				{
   2907 
   2908 				} else
   2909 				{
   2910 					m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
   2911 					m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
   2912 				}
   2913 			}
   2914 		}
   2915 	} else
   2916 	{
   2917 		m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
   2918 		m_bodies[1].applyImpulse( impulse,m_rpos[1]);
   2919 	}
   2920 }
   2921 
   2922 //
   2923 void				btSoftBody::CJoint::Terminate(btScalar dt)
   2924 {
   2925 	if(m_split>0)
   2926 	{
   2927 		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
   2928 		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
   2929 	}
   2930 }
   2931 
   2932 //
   2933 void				btSoftBody::applyForces()
   2934 {
   2935 
   2936 	BT_PROFILE("SoftBody applyForces");
   2937 //	const btScalar					dt =			m_sst.sdt;
   2938 	const btScalar					kLF =			m_cfg.kLF;
   2939 	const btScalar					kDG =			m_cfg.kDG;
   2940 	const btScalar					kPR =			m_cfg.kPR;
   2941 	const btScalar					kVC =			m_cfg.kVC;
   2942 	const bool						as_lift =		kLF>0;
   2943 	const bool						as_drag =		kDG>0;
   2944 	const bool						as_pressure =	kPR!=0;
   2945 	const bool						as_volume =		kVC>0;
   2946 	const bool						as_aero =		as_lift	||
   2947 													as_drag		;
   2948 	//const bool						as_vaero =		as_aero	&&
   2949 	//												(m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
   2950 	//const bool						as_faero =		as_aero	&&
   2951 	//												(m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
   2952 	const bool						use_medium =	as_aero;
   2953 	const bool						use_volume =	as_pressure	||
   2954 		as_volume	;
   2955 	btScalar						volume =		0;
   2956 	btScalar						ivolumetp =		0;
   2957 	btScalar						dvolumetv =		0;
   2958 	btSoftBody::sMedium	medium;
   2959 	if(use_volume)
   2960 	{
   2961 		volume		=	getVolume();
   2962 		ivolumetp	=	1/btFabs(volume)*kPR;
   2963 		dvolumetv	=	(m_pose.m_volume-volume)*kVC;
   2964 	}
   2965 	/* Per vertex forces			*/
   2966 	int i,ni;
   2967 
   2968 	for(i=0,ni=m_nodes.size();i<ni;++i)
   2969 	{
   2970 		btSoftBody::Node&	n=m_nodes[i];
   2971 		if(n.m_im>0)
   2972 		{
   2973 			if(use_medium)
   2974 			{
   2975 				/* Aerodynamics			*/
   2976 				addAeroForceToNode(m_windVelocity, i);
   2977 			}
   2978 			/* Pressure				*/
   2979 			if(as_pressure)
   2980 			{
   2981 				n.m_f	+=	n.m_n*(n.m_area*ivolumetp);
   2982 			}
   2983 			/* Volume				*/
   2984 			if(as_volume)
   2985 			{
   2986 				n.m_f	+=	n.m_n*(n.m_area*dvolumetv);
   2987 			}
   2988 		}
   2989 	}
   2990 
   2991 	/* Per face forces				*/
   2992 	for(i=0,ni=m_faces.size();i<ni;++i)
   2993 	{
   2994 	//	btSoftBody::Face&	f=m_faces[i];
   2995 
   2996 		/* Aerodynamics			*/
   2997 		addAeroForceToFace(m_windVelocity, i);
   2998 	}
   2999 }
   3000 
   3001 //
   3002 void				btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
   3003 {
   3004 	const btScalar	kAHR=psb->m_cfg.kAHR*kst;
   3005 	const btScalar	dt=psb->m_sst.sdt;
   3006 	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
   3007 	{
   3008 		const Anchor&		a=psb->m_anchors[i];
   3009 		const btTransform&	t=a.m_body->getWorldTransform();
   3010 		Node&				n=*a.m_node;
   3011 		const btVector3		wa=t*a.m_local;
   3012 		const btVector3		va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
   3013 		const btVector3		vb=n.m_x-n.m_q;
   3014 		const btVector3		vr=(va-vb)+(wa-n.m_x)*kAHR;
   3015 		const btVector3		impulse=a.m_c0*vr*a.m_influence;
   3016 		n.m_x+=impulse*a.m_c2;
   3017 		a.m_body->applyImpulse(-impulse,a.m_c1);
   3018 	}
   3019 }
   3020 
   3021 //
   3022 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
   3023 {
   3024 	const btScalar	dt = psb->m_sst.sdt;
   3025 	const btScalar	mrg = psb->getCollisionShape()->getMargin();
   3026 	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
   3027 	{
   3028 		const RContact&		c = psb->m_rcontacts[i];
   3029 		const sCti&			cti = c.m_cti;
   3030 		if (cti.m_colObj->hasContactResponse())
   3031 		{
   3032 			btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
   3033 			const btVector3		va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
   3034 			const btVector3		vb = c.m_node->m_x-c.m_node->m_q;
   3035 			const btVector3		vr = vb-va;
   3036 			const btScalar		dn = btDot(vr, cti.m_normal);
   3037 			if(dn<=SIMD_EPSILON)
   3038 			{
   3039 				const btScalar		dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
   3040 				const btVector3		fv = vr - (cti.m_normal * dn);
   3041 				// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
   3042 				const btVector3		impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
   3043 				c.m_node->m_x -= impulse * c.m_c2;
   3044 				if (tmpRigid)
   3045 					tmpRigid->applyImpulse(impulse,c.m_c1);
   3046 			}
   3047 		}
   3048 	}
   3049 }
   3050 
   3051 //
   3052 void				btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
   3053 {
   3054 	for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
   3055 	{
   3056 		const SContact&		c=psb->m_scontacts[i];
   3057 		const btVector3&	nr=c.m_normal;
   3058 		Node&				n=*c.m_node;
   3059 		Face&				f=*c.m_face;
   3060 		const btVector3		p=BaryEval(	f.m_n[0]->m_x,
   3061 			f.m_n[1]->m_x,
   3062 			f.m_n[2]->m_x,
   3063 			c.m_weights);
   3064 		const btVector3		q=BaryEval(	f.m_n[0]->m_q,
   3065 			f.m_n[1]->m_q,
   3066 			f.m_n[2]->m_q,
   3067 			c.m_weights);
   3068 		const btVector3		vr=(n.m_x-n.m_q)-(p-q);
   3069 		btVector3			corr(0,0,0);
   3070 		btScalar dot = btDot(vr,nr);
   3071 		if(dot<0)
   3072 		{
   3073 			const btScalar	j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
   3074 			corr+=c.m_normal*j;
   3075 		}
   3076 		corr			-=	ProjectOnPlane(vr,nr)*c.m_friction;
   3077 		n.m_x			+=	corr*c.m_cfm[0];
   3078 		f.m_n[0]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.x());
   3079 		f.m_n[1]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.y());
   3080 		f.m_n[2]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.z());
   3081 	}
   3082 }
   3083 
   3084 //
   3085 void				btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
   3086 {
   3087 	for(int i=0,ni=psb->m_links.size();i<ni;++i)
   3088 	{
   3089 		Link&	l=psb->m_links[i];
   3090 		if(l.m_c0>0)
   3091 		{
   3092 			Node&			a=*l.m_n[0];
   3093 			Node&			b=*l.m_n[1];
   3094 			const btVector3	del=b.m_x-a.m_x;
   3095 			const btScalar	len=del.length2();
   3096 			if (l.m_c1+len > SIMD_EPSILON)
   3097 			{
   3098 				const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
   3099 				a.m_x-=del*(k*a.m_im);
   3100 				b.m_x+=del*(k*b.m_im);
   3101 			}
   3102 		}
   3103 	}
   3104 }
   3105 
   3106 //
   3107 void				btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
   3108 {
   3109 	for(int i=0,ni=psb->m_links.size();i<ni;++i)
   3110 	{
   3111 		Link&			l=psb->m_links[i];
   3112 		Node**			n=l.m_n;
   3113 		const btScalar	j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
   3114 		n[0]->m_v+=	l.m_c3*(j*n[0]->m_im);
   3115 		n[1]->m_v-=	l.m_c3*(j*n[1]->m_im);
   3116 	}
   3117 }
   3118 
   3119 //
   3120 btSoftBody::psolver_t	btSoftBody::getSolver(ePSolver::_ solver)
   3121 {
   3122 	switch(solver)
   3123 	{
   3124 	case	ePSolver::Anchors:
   3125 		return(&btSoftBody::PSolve_Anchors);
   3126 	case	ePSolver::Linear:
   3127 		return(&btSoftBody::PSolve_Links);
   3128 	case	ePSolver::RContacts:
   3129 		return(&btSoftBody::PSolve_RContacts);
   3130 	case	ePSolver::SContacts:
   3131 		return(&btSoftBody::PSolve_SContacts);
   3132 		default:
   3133 		{
   3134 		}
   3135 	}
   3136 	return(0);
   3137 }
   3138 
   3139 //
   3140 btSoftBody::vsolver_t	btSoftBody::getSolver(eVSolver::_ solver)
   3141 {
   3142 	switch(solver)
   3143 	{
   3144 	case	eVSolver::Linear:		return(&btSoftBody::VSolve_Links);
   3145 		default:
   3146 		{
   3147 		}
   3148 	}
   3149 	return(0);
   3150 }
   3151 
   3152 //
   3153 void			btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
   3154 {
   3155 
   3156 	switch(m_cfg.collisions&fCollision::RVSmask)
   3157 	{
   3158 	case	fCollision::SDF_RS:
   3159 		{
   3160 			btSoftColliders::CollideSDF_RS	docollide;
   3161 			btRigidBody*		prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
   3162 			btTransform	wtr=pcoWrap->getWorldTransform();
   3163 
   3164 			const btTransform	ctr=pcoWrap->getWorldTransform();
   3165 			const btScalar		timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
   3166 			const btScalar		basemargin=getCollisionShape()->getMargin();
   3167 			btVector3			mins;
   3168 			btVector3			maxs;
   3169 			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
   3170 			pcoWrap->getCollisionShape()->getAabb(	pcoWrap->getWorldTransform(),
   3171 				mins,
   3172 				maxs);
   3173 			volume=btDbvtVolume::FromMM(mins,maxs);
   3174 			volume.Expand(btVector3(basemargin,basemargin,basemargin));
   3175 			docollide.psb		=	this;
   3176 			docollide.m_colObj1Wrap = pcoWrap;
   3177 			docollide.m_rigidBody = prb1;
   3178 
   3179 			docollide.dynmargin	=	basemargin+timemargin;
   3180 			docollide.stamargin	=	basemargin;
   3181 			m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
   3182 		}
   3183 		break;
   3184 	case	fCollision::CL_RS:
   3185 		{
   3186 			btSoftColliders::CollideCL_RS	collider;
   3187 			collider.ProcessColObj(this,pcoWrap);
   3188 		}
   3189 		break;
   3190 	}
   3191 }
   3192 
   3193 //
   3194 void			btSoftBody::defaultCollisionHandler(btSoftBody* psb)
   3195 {
   3196 	const int cf=m_cfg.collisions&psb->m_cfg.collisions;
   3197 	switch(cf&fCollision::SVSmask)
   3198 	{
   3199 	case	fCollision::CL_SS:
   3200 		{
   3201 
   3202 			//support self-collision if CL_SELF flag set
   3203 			if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
   3204 			{
   3205 				btSoftColliders::CollideCL_SS	docollide;
   3206 				docollide.ProcessSoftSoft(this,psb);
   3207 			}
   3208 
   3209 		}
   3210 		break;
   3211 	case	fCollision::VF_SS:
   3212 		{
   3213 			//only self-collision for Cluster, not Vertex-Face yet
   3214 			if (this!=psb)
   3215 			{
   3216 				btSoftColliders::CollideVF_SS	docollide;
   3217 				/* common					*/
   3218 				docollide.mrg=	getCollisionShape()->getMargin()+
   3219 					psb->getCollisionShape()->getMargin();
   3220 				/* psb0 nodes vs psb1 faces	*/
   3221 				docollide.psb[0]=this;
   3222 				docollide.psb[1]=psb;
   3223 				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
   3224 					docollide.psb[1]->m_fdbvt.m_root,
   3225 					docollide);
   3226 				/* psb1 nodes vs psb0 faces	*/
   3227 				docollide.psb[0]=psb;
   3228 				docollide.psb[1]=this;
   3229 				docollide.psb[0]->m_ndbvt.collideTT(	docollide.psb[0]->m_ndbvt.m_root,
   3230 					docollide.psb[1]->m_fdbvt.m_root,
   3231 					docollide);
   3232 			}
   3233 		}
   3234 		break;
   3235 	default:
   3236 		{
   3237 
   3238 		}
   3239 	}
   3240 }
   3241 
   3242 
   3243 
   3244 void btSoftBody::setWindVelocity( const btVector3 &velocity )
   3245 {
   3246 	m_windVelocity = velocity;
   3247 }
   3248 
   3249 
   3250 const btVector3& btSoftBody::getWindVelocity()
   3251 {
   3252 	return m_windVelocity;
   3253 }
   3254 
   3255 
   3256 
   3257 int	btSoftBody::calculateSerializeBufferSize()	const
   3258 {
   3259 	int sz = sizeof(btSoftBodyData);
   3260 	return sz;
   3261 }
   3262 
   3263 	///fills the dataBuffer and returns the struct name (and 0 on failure)
   3264 const char*	btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
   3265 {
   3266 	btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
   3267 
   3268 	btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
   3269 
   3270 	btHashMap<btHashPtr,int>	m_nodeIndexMap;
   3271 
   3272 	sbd->m_numMaterials = m_materials.size();
   3273 	sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
   3274 
   3275 	if (sbd->m_materials)
   3276 	{
   3277 		int sz = sizeof(SoftBodyMaterialData*);
   3278 		int numElem = sbd->m_numMaterials;
   3279 		btChunk* chunk = serializer->allocate(sz,numElem);
   3280 		//SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
   3281 		SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
   3282 		for (int i=0;i<numElem;i++,memPtr++)
   3283 		{
   3284 			btSoftBody::Material* mat = m_materials[i];
   3285 			*memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
   3286 			if (!serializer->findPointer(mat))
   3287 			{
   3288 				//serialize it here
   3289 				btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
   3290 				SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
   3291 				memPtr->m_flags = mat->m_flags;
   3292 				memPtr->m_angularStiffness = mat->m_kAST;
   3293 				memPtr->m_linearStiffness = mat->m_kLST;
   3294 				memPtr->m_volumeStiffness = mat->m_kVST;
   3295 				serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
   3296 			}
   3297 		}
   3298 		serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
   3299 	}
   3300 
   3301 
   3302 
   3303 
   3304 	sbd->m_numNodes = m_nodes.size();
   3305 	sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
   3306 	if (sbd->m_nodes)
   3307 	{
   3308 		int sz = sizeof(SoftBodyNodeData);
   3309 		int numElem = sbd->m_numNodes;
   3310 		btChunk* chunk = serializer->allocate(sz,numElem);
   3311 		SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
   3312 		for (int i=0;i<numElem;i++,memPtr++)
   3313 		{
   3314 			m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
   3315 			memPtr->m_area = m_nodes[i].m_area;
   3316 			memPtr->m_attach = m_nodes[i].m_battach;
   3317 			memPtr->m_inverseMass = m_nodes[i].m_im;
   3318 			memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
   3319 			m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
   3320 			m_nodes[i].m_x.serializeFloat(memPtr->m_position);
   3321 			m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
   3322 			m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
   3323 			m_nodeIndexMap.insert(&m_nodes[i],i);
   3324 		}
   3325 		serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
   3326 	}
   3327 
   3328 	sbd->m_numLinks = m_links.size();
   3329 	sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
   3330 	if (sbd->m_links)
   3331 	{
   3332 		int sz = sizeof(SoftBodyLinkData);
   3333 		int numElem = sbd->m_numLinks;
   3334 		btChunk* chunk = serializer->allocate(sz,numElem);
   3335 		SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
   3336 		for (int i=0;i<numElem;i++,memPtr++)
   3337 		{
   3338 			memPtr->m_bbending = m_links[i].m_bbending;
   3339 			memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
   3340 			memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
   3341 			memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
   3342 			btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
   3343 			btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
   3344 			memPtr->m_restLength = m_links[i].m_rl;
   3345 		}
   3346 		serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
   3347 
   3348 	}
   3349 
   3350 
   3351 	sbd->m_numFaces = m_faces.size();
   3352 	sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
   3353 	if (sbd->m_faces)
   3354 	{
   3355 		int sz = sizeof(SoftBodyFaceData);
   3356 		int numElem = sbd->m_numFaces;
   3357 		btChunk* chunk = serializer->allocate(sz,numElem);
   3358 		SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
   3359 		for (int i=0;i<numElem;i++,memPtr++)
   3360 		{
   3361 			memPtr->m_material = m_faces[i].m_material ?  (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
   3362 			m_faces[i].m_normal.serializeFloat(	memPtr->m_normal);
   3363 			for (int j=0;j<3;j++)
   3364 			{
   3365 				memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
   3366 			}
   3367 			memPtr->m_restArea = m_faces[i].m_ra;
   3368 		}
   3369 		serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
   3370 	}
   3371 
   3372 
   3373 	sbd->m_numTetrahedra = m_tetras.size();
   3374 	sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
   3375 	if (sbd->m_tetrahedra)
   3376 	{
   3377 		int sz = sizeof(SoftBodyTetraData);
   3378 		int numElem = sbd->m_numTetrahedra;
   3379 		btChunk* chunk = serializer->allocate(sz,numElem);
   3380 		SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
   3381 		for (int i=0;i<numElem;i++,memPtr++)
   3382 		{
   3383 			for (int j=0;j<4;j++)
   3384 			{
   3385 				m_tetras[i].m_c0[j].serializeFloat(	memPtr->m_c0[j] );
   3386 				memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
   3387 			}
   3388 			memPtr->m_c1 = m_tetras[i].m_c1;
   3389 			memPtr->m_c2 = m_tetras[i].m_c2;
   3390 			memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
   3391 			memPtr->m_restVolume = m_tetras[i].m_rv;
   3392 		}
   3393 		serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
   3394 	}
   3395 
   3396 	sbd->m_numAnchors = m_anchors.size();
   3397 	sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
   3398 	if (sbd->m_anchors)
   3399 	{
   3400 		int sz = sizeof(SoftRigidAnchorData);
   3401 		int numElem = sbd->m_numAnchors;
   3402 		btChunk* chunk = serializer->allocate(sz,numElem);
   3403 		SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
   3404 		for (int i=0;i<numElem;i++,memPtr++)
   3405 		{
   3406 			m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
   3407 			m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
   3408 			memPtr->m_c2 = m_anchors[i].m_c2;
   3409 			m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
   3410 			memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
   3411 
   3412 			memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*)  serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
   3413 			btAssert(memPtr->m_nodeIndex < m_nodes.size());
   3414 		}
   3415 		serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
   3416 	}
   3417 
   3418 
   3419 	sbd->m_config.m_dynamicFriction = m_cfg.kDF;
   3420 	sbd->m_config.m_baumgarte = m_cfg.kVCF;
   3421 	sbd->m_config.m_pressure = m_cfg.kPR;
   3422 	sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
   3423 	sbd->m_config.m_lift = m_cfg.kLF;
   3424 	sbd->m_config.m_drag = m_cfg.kDG;
   3425 	sbd->m_config.m_positionIterations = m_cfg.piterations;
   3426 	sbd->m_config.m_driftIterations = m_cfg.diterations;
   3427 	sbd->m_config.m_clusterIterations = m_cfg.citerations;
   3428 	sbd->m_config.m_velocityIterations = m_cfg.viterations;
   3429 	sbd->m_config.m_maxVolume = m_cfg.maxvolume;
   3430 	sbd->m_config.m_damping = m_cfg.kDP;
   3431 	sbd->m_config.m_poseMatch = m_cfg.kMT;
   3432 	sbd->m_config.m_collisionFlags = m_cfg.collisions;
   3433 	sbd->m_config.m_volume = m_cfg.kVC;
   3434 	sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
   3435 	sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
   3436 	sbd->m_config.m_softContactHardness = m_cfg.kSHR;
   3437 	sbd->m_config.m_anchorHardness = m_cfg.kAHR;
   3438 	sbd->m_config.m_timeScale = m_cfg.timescale;
   3439 	sbd->m_config.m_maxVolume = m_cfg.maxvolume;
   3440 	sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
   3441 	sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
   3442 	sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
   3443 	sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
   3444 	sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
   3445 	sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
   3446 
   3447 	//pose for shape matching
   3448 	{
   3449 		sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
   3450 
   3451 		int sz = sizeof(SoftBodyPoseData);
   3452 		btChunk* chunk = serializer->allocate(sz,1);
   3453 		SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
   3454 
   3455 		m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
   3456 		memPtr->m_bframe = m_pose.m_bframe;
   3457 		memPtr->m_bvolume = m_pose.m_bvolume;
   3458 		m_pose.m_com.serializeFloat(memPtr->m_com);
   3459 
   3460 		memPtr->m_numPositions = m_pose.m_pos.size();
   3461 		memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
   3462 		if (memPtr->m_numPositions)
   3463 		{
   3464 			int numElem = memPtr->m_numPositions;
   3465 			int sz = sizeof(btVector3Data);
   3466 			btChunk* chunk = serializer->allocate(sz,numElem);
   3467 			btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
   3468 			for (int i=0;i<numElem;i++,memPtr++)
   3469 			{
   3470 				m_pose.m_pos[i].serializeFloat(*memPtr);
   3471 			}
   3472 			serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
   3473 		}
   3474 		memPtr->m_restVolume = m_pose.m_volume;
   3475 		m_pose.m_rot.serializeFloat(memPtr->m_rot);
   3476 		m_pose.m_scl.serializeFloat(memPtr->m_scale);
   3477 
   3478 		memPtr->m_numWeigts = m_pose.m_wgh.size();
   3479 		memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
   3480 		if (memPtr->m_numWeigts)
   3481 		{
   3482 
   3483 			int numElem = memPtr->m_numWeigts;
   3484 			int sz = sizeof(float);
   3485 			btChunk* chunk = serializer->allocate(sz,numElem);
   3486 			float* memPtr = (float*) chunk->m_oldPtr;
   3487 			for (int i=0;i<numElem;i++,memPtr++)
   3488 			{
   3489 				*memPtr = m_pose.m_wgh[i];
   3490 			}
   3491 			serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
   3492 		}
   3493 
   3494 		serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
   3495 	}
   3496 
   3497 	//clusters for convex-cluster collision detection
   3498 
   3499 	sbd->m_numClusters = m_clusters.size();
   3500 	sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
   3501 	if (sbd->m_numClusters)
   3502 	{
   3503 		int numElem = sbd->m_numClusters;
   3504 		int sz = sizeof(SoftBodyClusterData);
   3505 		btChunk* chunk = serializer->allocate(sz,numElem);
   3506 		SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
   3507 		for (int i=0;i<numElem;i++,memPtr++)
   3508 		{
   3509 			memPtr->m_adamping= m_clusters[i]->m_adamping;
   3510 			m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
   3511 			memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
   3512 			memPtr->m_collide = m_clusters[i]->m_collide;
   3513 			m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
   3514 			memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
   3515 			m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
   3516 			m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
   3517 			m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
   3518 			memPtr->m_idmass = m_clusters[i]->m_idmass;
   3519 			memPtr->m_imass = m_clusters[i]->m_imass;
   3520 			m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
   3521 			memPtr->m_ldamping = m_clusters[i]->m_ldamping;
   3522 			m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
   3523 			m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
   3524 			memPtr->m_matching = m_clusters[i]->m_matching;
   3525 			memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
   3526 			memPtr->m_ndamping = m_clusters[i]->m_ndamping;
   3527 			memPtr->m_ldamping = m_clusters[i]->m_ldamping;
   3528 			memPtr->m_adamping = m_clusters[i]->m_adamping;
   3529 			memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
   3530 
   3531 			memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
   3532 			memPtr->m_numMasses = m_clusters[i]->m_masses.size();
   3533 			memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
   3534 
   3535 			memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
   3536 			m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
   3537 			m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
   3538 			memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
   3539 
   3540 
   3541 
   3542 			memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
   3543 			if (memPtr->m_framerefs)
   3544 			{
   3545 				int numElem = memPtr->m_numFrameRefs;
   3546 				int sz = sizeof(btVector3FloatData);
   3547 				btChunk* chunk = serializer->allocate(sz,numElem);
   3548 				btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
   3549 				for (int j=0;j<numElem;j++,memPtr++)
   3550 				{
   3551 					m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
   3552 				}
   3553 				serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
   3554 			}
   3555 
   3556 			memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
   3557 			if (memPtr->m_masses)
   3558 			{
   3559 				int numElem = memPtr->m_numMasses;
   3560 				int sz = sizeof(float);
   3561 				btChunk* chunk = serializer->allocate(sz,numElem);
   3562 				float* memPtr = (float*) chunk->m_oldPtr;
   3563 				for (int j=0;j<numElem;j++,memPtr++)
   3564 				{
   3565 					*memPtr = m_clusters[i]->m_masses[j];
   3566 				}
   3567 				serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
   3568 			}
   3569 
   3570 			memPtr->m_nodeIndices  = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
   3571 			if (memPtr->m_nodeIndices )
   3572 			{
   3573 				int numElem = memPtr->m_numMasses;
   3574 				int sz = sizeof(int);
   3575 				btChunk* chunk = serializer->allocate(sz,numElem);
   3576 				int* memPtr = (int*) chunk->m_oldPtr;
   3577 				for (int j=0;j<numElem;j++,memPtr++)
   3578 				{
   3579 					int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
   3580 					btAssert(indexPtr);
   3581 					*memPtr = *indexPtr;
   3582 				}
   3583 				serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
   3584 			}
   3585 		}
   3586 		serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
   3587 
   3588 	}
   3589 
   3590 
   3591 
   3592 	sbd->m_numJoints = m_joints.size();
   3593 	sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
   3594 
   3595 	if (sbd->m_joints)
   3596 	{
   3597 		int sz = sizeof(btSoftBodyJointData);
   3598 		int numElem = m_joints.size();
   3599 		btChunk* chunk = serializer->allocate(sz,numElem);
   3600 		btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
   3601 
   3602 		for (int i=0;i<numElem;i++,memPtr++)
   3603 		{
   3604 			memPtr->m_jointType = (int)m_joints[i]->Type();
   3605 			m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
   3606 			m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
   3607 			memPtr->m_cfm = m_joints[i]->m_cfm;
   3608 			memPtr->m_erp = float(m_joints[i]->m_erp);
   3609 			memPtr->m_split = float(m_joints[i]->m_split);
   3610 			memPtr->m_delete = m_joints[i]->m_delete;
   3611 
   3612 			for (int j=0;j<4;j++)
   3613 			{
   3614 				memPtr->m_relPosition[0].m_floats[j] = 0.f;
   3615 				memPtr->m_relPosition[1].m_floats[j] = 0.f;
   3616 			}
   3617 			memPtr->m_bodyA = 0;
   3618 			memPtr->m_bodyB = 0;
   3619 			if (m_joints[i]->m_bodies[0].m_soft)
   3620 			{
   3621 				memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
   3622 				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
   3623 			}
   3624 			if (m_joints[i]->m_bodies[0].m_collisionObject)
   3625 			{
   3626 				memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
   3627 				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
   3628 			}
   3629 			if (m_joints[i]->m_bodies[0].m_rigid)
   3630 			{
   3631 				memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
   3632 				memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
   3633 			}
   3634 
   3635 			if (m_joints[i]->m_bodies[1].m_soft)
   3636 			{
   3637 				memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
   3638 				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
   3639 			}
   3640 			if (m_joints[i]->m_bodies[1].m_collisionObject)
   3641 			{
   3642 				memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
   3643 				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
   3644 			}
   3645 			if (m_joints[i]->m_bodies[1].m_rigid)
   3646 			{
   3647 				memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
   3648 				memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
   3649 			}
   3650 		}
   3651 		serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
   3652 	}
   3653 
   3654 
   3655 	return btSoftBodyDataName;
   3656 }
   3657 
   3658