Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h @ 2258

Last change on this file since 2258 was 2192, checked in by rgrieder, 16 years ago
Reverted all changes of attempt to update physics branch.
Property svn:eol-style set to `native`
File size: 22.6 KB

Rev	Line
[1963]	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	#ifndef _BT_SOFT_BODY_INTERNALS_H
	18	#define _BT_SOFT_BODY_INTERNALS_H
	19
	20	#include "btSoftBody.h"
	21
	22	#include "LinearMath/btQuickprof.h"
	23	#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
	24	#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
	25	#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
	26	#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
	27
	28	//
	29	// btSymMatrix
	30	//
	31	template <typename T>
	32	struct btSymMatrix
	33	{
[1972]	34	btSymMatrix() : dim(0) {}
	35	btSymMatrix(int n,const T& init=T()) { resize(n,init); }
	36	void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); }
	37	int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
	38	T& operator()(int c,int r) { return(store[index(c,r)]); }
	39	const T& operator()(int c,int r) const { return(store[index(c,r)]); }
	40	btAlignedObjectArray<T> store;
	41	int dim;
[1963]	42	};
	43
	44	//
	45	// btSoftBodyCollisionShape
	46	//
	47	class btSoftBodyCollisionShape : public btConcaveShape
	48	{
	49	public:
	50	btSoftBody* m_body;
[1972]	51
[1963]	52	btSoftBodyCollisionShape(btSoftBody* backptr)
	53	{
	54	m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
	55	m_body=backptr;
	56	}
	57
	58	virtual ~btSoftBodyCollisionShape()
	59	{
	60
	61	}
	62
	63	void processAllTriangles(btTriangleCallback* /callback/,const btVector3& /aabbMin/,const btVector3& /aabbMax/) const
	64	{
	65	//not yet
	66	btAssert(0);
	67	}
	68
	69	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
	70	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
	71	{
[1972]	72	/* t should be identity, but better be safe than...fast? */
	73	const btVector3 mins=m_body->m_bounds[0];
	74	const btVector3 maxs=m_body->m_bounds[1];
	75	const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
	76	t*btVector3(maxs.x(),mins.y(),mins.z()),
	77	t*btVector3(maxs.x(),maxs.y(),mins.z()),
	78	t*btVector3(mins.x(),maxs.y(),mins.z()),
	79	t*btVector3(mins.x(),mins.y(),maxs.z()),
	80	t*btVector3(maxs.x(),mins.y(),maxs.z()),
	81	t*btVector3(maxs.x(),maxs.y(),maxs.z()),
	82	t*btVector3(mins.x(),maxs.y(),maxs.z())};
	83	aabbMin=aabbMax=crns[0];
	84	for(int i=1;i<8;++i)
[1963]	85	{
[1972]	86	aabbMin.setMin(crns[i]);
	87	aabbMax.setMax(crns[i]);
[1963]	88	}
	89	}
	90
[1972]	91
[1963]	92	virtual void setLocalScaling(const btVector3& /scaling/)
	93	{
	94	///na
	95	}
	96	virtual const btVector3& getLocalScaling() const
	97	{
[1972]	98	static const btVector3 dummy(1,1,1);
	99	return dummy;
[1963]	100	}
	101	virtual void calculateLocalInertia(btScalar /mass/,btVector3& /inertia/) const
	102	{
	103	///not yet
	104	btAssert(0);
	105	}
	106	virtual const char* getName()const
	107	{
	108	return "SoftBody";
	109	}
	110
	111	};
	112
	113	//
	114	// btSoftClusterCollisionShape
	115	//
	116	class btSoftClusterCollisionShape : public btConvexInternalShape
	117	{
	118	public:
	119	const btSoftBody::Cluster* m_cluster;
	120
	121	btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
[1972]	122
	123
[1963]	124	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
[1972]	125	{
[1963]	126	btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
	127	btScalar d=dot(vec,n[0]->m_x);
	128	int j=0;
	129	for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
[1972]	130	{
[1963]	131	const btScalar k=dot(vec,n[i]->m_x);
	132	if(k>d) { d=k;j=i; }
[1972]	133	}
	134	return(n[j]->m_x);
[1963]	135	}
	136	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
[1972]	137	{
[1963]	138	return(localGetSupportingVertex(vec));
[1972]	139	}
[1963]	140	//notice that the vectors should be unit length
	141	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
	142	{}
	143
	144
	145	virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
	146	{}
	147
	148	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
	149	{}
	150
	151	virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
[1972]	152
[1963]	153	//debugging
	154	virtual const char* getName()const {return "SOFTCLUSTER";}
	155
	156	virtual void setMargin(btScalar margin)
	157	{
	158	btConvexInternalShape::setMargin(margin);
	159	}
	160	virtual btScalar getMargin() const
	161	{
	162	return getMargin();
	163	}
	164	};
	165
	166	//
	167	// Inline's
	168	//
	169
	170	//
	171	template <typename T>
	172	static inline void ZeroInitialize(T& value)
	173	{
[1972]	174	static const T zerodummy;
	175	value=zerodummy;
[1963]	176	}
	177	//
	178	template <typename T>
	179	static inline bool CompLess(const T& a,const T& b)
	180	{ return(a<b); }
	181	//
	182	template <typename T>
	183	static inline bool CompGreater(const T& a,const T& b)
	184	{ return(a>b); }
	185	//
	186	template <typename T>
	187	static inline T Lerp(const T& a,const T& b,btScalar t)
	188	{ return(a+(b-a)*t); }
	189	//
	190	template <typename T>
	191	static inline T InvLerp(const T& a,const T& b,btScalar t)
	192	{ return((b+at-bt)/(a*b)); }
	193	//
	194	static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
[1972]	195	const btMatrix3x3& b,
	196	btScalar t)
[1963]	197	{
[1972]	198	btMatrix3x3 r;
	199	r[0]=Lerp(a[0],b[0],t);
	200	r[1]=Lerp(a[1],b[1],t);
	201	r[2]=Lerp(a[2],b[2],t);
	202	return(r);
[1963]	203	}
	204	//
	205	static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
	206	{
[1972]	207	const btScalar sql=v.length2();
	208	if(sql>(maxlength*maxlength))
	209	return((v*maxlength)/btSqrt(sql));
[1963]	210	else
[1972]	211	return(v);
[1963]	212	}
	213	//
	214	template <typename T>
	215	static inline T Clamp(const T& x,const T& l,const T& h)
	216	{ return(x<l?l:x>h?h:x); }
	217	//
	218	template <typename T>
	219	static inline T Sq(const T& x)
	220	{ return(x*x); }
	221	//
	222	template <typename T>
	223	static inline T Cube(const T& x)
	224	{ return(xxx); }
	225	//
	226	template <typename T>
	227	static inline T Sign(const T& x)
	228	{ return((T)(x<0?-1:+1)); }
	229	//
	230	template <typename T>
	231	static inline bool SameSign(const T& x,const T& y)
	232	{ return((x*y)>0); }
	233	//
	234	static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
	235	{
[1972]	236	const btVector3 d=x-y;
	237	return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
[1963]	238	}
	239	//
	240	static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
	241	{
	242	const btScalar xx=a.x()*a.x();
	243	const btScalar yy=a.y()*a.y();
	244	const btScalar zz=a.z()*a.z();
	245	const btScalar xy=a.x()*a.y();
	246	const btScalar yz=a.y()*a.z();
	247	const btScalar zx=a.z()*a.x();
	248	btMatrix3x3 m;
	249	m[0]=btVector3(1-xx+xxs,xys-xy,zx*s-zx);
	250	m[1]=btVector3(xys-xy,1-yy+yys,yz*s-yz);
	251	m[2]=btVector3(zxs-zx,yzs-yz,1-zz+zz*s);
	252	return(m);
	253	}
	254	//
	255	static inline btMatrix3x3 Cross(const btVector3& v)
	256	{
	257	btMatrix3x3 m;
	258	m[0]=btVector3(0,-v.z(),+v.y());
	259	m[1]=btVector3(+v.z(),0,-v.x());
	260	m[2]=btVector3(-v.y(),+v.x(),0);
	261	return(m);
	262	}
	263	//
	264	static inline btMatrix3x3 Diagonal(btScalar x)
	265	{
	266	btMatrix3x3 m;
	267	m[0]=btVector3(x,0,0);
	268	m[1]=btVector3(0,x,0);
	269	m[2]=btVector3(0,0,x);
	270	return(m);
	271	}
	272	//
	273	static inline btMatrix3x3 Add(const btMatrix3x3& a,
[1972]	274	const btMatrix3x3& b)
[1963]	275	{
	276	btMatrix3x3 r;
	277	for(int i=0;i<3;++i) r[i]=a[i]+b[i];
	278	return(r);
	279	}
	280	//
	281	static inline btMatrix3x3 Sub(const btMatrix3x3& a,
[1972]	282	const btMatrix3x3& b)
[1963]	283	{
	284	btMatrix3x3 r;
	285	for(int i=0;i<3;++i) r[i]=a[i]-b[i];
	286	return(r);
	287	}
	288	//
	289	static inline btMatrix3x3 Mul(const btMatrix3x3& a,
[1972]	290	btScalar b)
[1963]	291	{
	292	btMatrix3x3 r;
	293	for(int i=0;i<3;++i) r[i]=a[i]*b;
	294	return(r);
	295	}
	296	//
	297	static inline void Orthogonalize(btMatrix3x3& m)
	298	{
[1972]	299	m[2]=cross(m[0],m[1]).normalized();
	300	m[1]=cross(m[2],m[0]).normalized();
	301	m[0]=cross(m[1],m[2]).normalized();
[1963]	302	}
	303	//
	304	static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
	305	{
	306	const btMatrix3x3 cr=Cross(r);
	307	return(Sub(Diagonal(im),criwicr));
	308	}
	309
	310	//
	311	static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
[1972]	312	btScalar ima,
	313	btScalar imb,
	314	const btMatrix3x3& iwi,
	315	const btVector3& r)
[1963]	316	{
	317	return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
	318	}
	319
	320	//
	321	static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
[1972]	322	btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
[1963]	323	{
[1972]	324	return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
[1963]	325	}
	326
	327	//
	328	static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
[1972]	329	const btMatrix3x3& iib)
[1963]	330	{
[1972]	331	return(Add(iia,iib).inverse());
[1963]	332	}
	333
	334	//
	335	static inline btVector3 ProjectOnAxis( const btVector3& v,
[1972]	336	const btVector3& a)
[1963]	337	{
	338	return(a*dot(v,a));
	339	}
	340	//
	341	static inline btVector3 ProjectOnPlane( const btVector3& v,
[1972]	342	const btVector3& a)
[1963]	343	{
	344	return(v-ProjectOnAxis(v,a));
	345	}
	346
	347	//
	348	static inline void ProjectOrigin( const btVector3& a,
[1972]	349	const btVector3& b,
	350	btVector3& prj,
	351	btScalar& sqd)
[1963]	352	{
[1972]	353	const btVector3 d=b-a;
	354	const btScalar m2=d.length2();
	355	if(m2>SIMD_EPSILON)
[1963]	356	{
[1972]	357	const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
	358	const btVector3 p=a+d*t;
	359	const btScalar l2=p.length2();
	360	if(l2<sqd)
[1963]	361	{
[1972]	362	prj=p;
	363	sqd=l2;
[1963]	364	}
	365	}
	366	}
	367	//
	368	static inline void ProjectOrigin( const btVector3& a,
[1972]	369	const btVector3& b,
	370	const btVector3& c,
	371	btVector3& prj,
	372	btScalar& sqd)
[1963]	373	{
[1972]	374	const btVector3& q=cross(b-a,c-a);
	375	const btScalar m2=q.length2();
	376	if(m2>SIMD_EPSILON)
[1963]	377	{
[1972]	378	const btVector3 n=q/btSqrt(m2);
	379	const btScalar k=dot(a,n);
	380	const btScalar k2=k*k;
	381	if(k2<sqd)
[1963]	382	{
[1972]	383	const btVector3 p=n*k;
	384	if( (dot(cross(a-p,b-p),q)>0)&&
	385	(dot(cross(b-p,c-p),q)>0)&&
	386	(dot(cross(c-p,a-p),q)>0))
[1963]	387	{
[1972]	388	prj=p;
	389	sqd=k2;
[1963]	390	}
	391	else
	392	{
[1972]	393	ProjectOrigin(a,b,prj,sqd);
	394	ProjectOrigin(b,c,prj,sqd);
	395	ProjectOrigin(c,a,prj,sqd);
[1963]	396	}
	397	}
	398	}
	399	}
	400
	401	//
	402	template <typename T>
	403	static inline T BaryEval( const T& a,
[1972]	404	const T& b,
	405	const T& c,
	406	const btVector3& coord)
[1963]	407	{
	408	return(acoord.x()+bcoord.y()+c*coord.z());
	409	}
	410	//
	411	static inline btVector3 BaryCoord( const btVector3& a,
[1972]	412	const btVector3& b,
	413	const btVector3& c,
	414	const btVector3& p)
[1963]	415	{
[1972]	416	const btScalar w[]={ cross(a-p,b-p).length(),
	417	cross(b-p,c-p).length(),
	418	cross(c-p,a-p).length()};
	419	const btScalar isum=1/(w[0]+w[1]+w[2]);
	420	return(btVector3(w[1]isum,w[2]isum,w[0]*isum));
[1963]	421	}
	422
	423	//
	424	static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
[1972]	425	const btVector3& a,
	426	const btVector3& b,
	427	const btScalar accuracy,
	428	const int maxiterations=256)
[1963]	429	{
[1972]	430	btScalar span[2]={0,1};
	431	btScalar values[2]={fn->Eval(a),fn->Eval(b)};
	432	if(values[0]>values[1])
[1963]	433	{
[1972]	434	btSwap(span[0],span[1]);
	435	btSwap(values[0],values[1]);
[1963]	436	}
[1972]	437	if(values[0]>-accuracy) return(-1);
	438	if(values[1]<+accuracy) return(-1);
	439	for(int i=0;i<maxiterations;++i)
[1963]	440	{
[1972]	441	const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
	442	const btScalar v=fn->Eval(Lerp(a,b,t));
	443	if((t<=0)\|\|(t>=1)) break;
	444	if(btFabs(v)<accuracy) return(t);
	445	if(v<0)
[1963]	446	{ span[0]=t;values[0]=v; }
	447	else
	448	{ span[1]=t;values[1]=v; }
	449	}
[1972]	450	return(-1);
[1963]	451	}
	452
	453	//
	454	static inline btVector3 NormalizeAny(const btVector3& v)
	455	{
	456	const btScalar l=v.length();
	457	if(l>SIMD_EPSILON)
	458	return(v/l);
	459	else
	460	return(btVector3(0,0,0));
	461	}
	462
	463	//
	464	static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
[1972]	465	btScalar margin)
[1963]	466	{
[1972]	467	const btVector3* pts[]={ &f.m_n[0]->m_x,
	468	&f.m_n[1]->m_x,
	469	&f.m_n[2]->m_x};
	470	btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
	471	vol.Expand(btVector3(margin,margin,margin));
	472	return(vol);
[1963]	473	}
	474
	475	//
	476	static inline btVector3 CenterOf( const btSoftBody::Face& f)
	477	{
[1972]	478	return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
[1963]	479	}
	480
	481	//
	482	static inline btScalar AreaOf( const btVector3& x0,
[1972]	483	const btVector3& x1,
	484	const btVector3& x2)
[1963]	485	{
	486	const btVector3 a=x1-x0;
	487	const btVector3 b=x2-x0;
	488	const btVector3 cr=cross(a,b);
	489	const btScalar area=cr.length();
	490	return(area);
	491	}
	492
	493	//
	494	static inline btScalar VolumeOf( const btVector3& x0,
[1972]	495	const btVector3& x1,
	496	const btVector3& x2,
	497	const btVector3& x3)
[1963]	498	{
	499	const btVector3 a=x1-x0;
	500	const btVector3 b=x2-x0;
	501	const btVector3 c=x3-x0;
	502	return(dot(a,cross(b,c)));
	503	}
	504
	505	//
	506	static void EvaluateMedium( const btSoftBodyWorldInfo* wfi,
[1972]	507	const btVector3& x,
	508	btSoftBody::sMedium& medium)
[1963]	509	{
	510	medium.m_velocity = btVector3(0,0,0);
	511	medium.m_pressure = 0;
	512	medium.m_density = wfi->air_density;
	513	if(wfi->water_density>0)
	514	{
	515	const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset);
	516	if(depth>0)
	517	{
	518	medium.m_density = wfi->water_density;
	519	medium.m_pressure = depthwfi->water_densitywfi->m_gravity.length();
	520	}
	521	}
	522	}
	523
	524	//
	525	static inline void ApplyClampedForce( btSoftBody::Node& n,
[1972]	526	const btVector3& f,
	527	btScalar dt)
[1963]	528	{
	529	const btScalar dtim=dt*n.m_im;
	530	if((f*dtim).length2()>n.m_v.length2())
	531	{/* Clamp */
	532	n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
	533	}
	534	else
	535	{/* Apply */
	536	n.m_f+=f;
	537	}
	538	}
	539
	540	//
	541	static inline int MatchEdge( const btSoftBody::Node* a,
[1972]	542	const btSoftBody::Node* b,
	543	const btSoftBody::Node* ma,
	544	const btSoftBody::Node* mb)
[1963]	545	{
[1972]	546	if((a==ma)&&(b==mb)) return(0);
	547	if((a==mb)&&(b==ma)) return(1);
	548	return(-1);
[1963]	549	}
	550
	551	//
	552	// btEigen : Extract eigen system,
	553	// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
	554	// outputs are NOT sorted.
	555	//
	556	struct btEigen
	557	{
[1972]	558	static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
[1963]	559	{
[1972]	560	static const int maxiterations=16;
	561	static const btScalar accuracy=(btScalar)0.0001;
	562	btMatrix3x3& v=*vectors;
	563	int iterations=0;
	564	vectors->setIdentity();
	565	do {
	566	int p=0,q=1;
	567	if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
	568	if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
	569	if(btFabs(a[p][q])>accuracy)
[1963]	570	{
[1972]	571	const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
	572	const btScalar z=btFabs(w);
	573	const btScalar t=w/(z(btSqrt(1+ww)+z));
	574	if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
[1963]	575	{
[1972]	576	const btScalar c=1/btSqrt(t*t+1);
	577	const btScalar s=c*t;
	578	mulPQ(a,c,s,p,q);
	579	mulTPQ(a,c,s,p,q);
	580	mulPQ(v,c,s,p,q);
[1963]	581	} else break;
	582	} else break;
	583	} while((++iterations)<maxiterations);
[1972]	584	if(values)
[1963]	585	{
[1972]	586	*values=btVector3(a[0][0],a[1][1],a[2][2]);
[1963]	587	}
[1972]	588	return(iterations);
[1963]	589	}
	590	private:
[1972]	591	static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
[1963]	592	{
[1972]	593	const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
	594	{a[q][0],a[q][1],a[q][2]}};
	595	int i;
[1963]	596
[1972]	597	for(i=0;i<3;++i) a[p][i]=cm[0][i]-sm[1][i];
	598	for(i=0;i<3;++i) a[q][i]=cm[1][i]+sm[0][i];
[1963]	599	}
[1972]	600	static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
[1963]	601	{
[1972]	602	const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
	603	{a[0][q],a[1][q],a[2][q]}};
	604	int i;
[1963]	605
[1972]	606	for(i=0;i<3;++i) a[i][p]=cm[0][i]-sm[1][i];
	607	for(i=0;i<3;++i) a[i][q]=cm[1][i]+sm[0][i];
[1963]	608	}
	609	};
	610
	611	//
	612	// Polar decomposition,
	613	// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
	614	//
	615	static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
	616	{
	617	static const btScalar half=(btScalar)0.5;
	618	static const btScalar accuracy=(btScalar)0.0001;
	619	static const int maxiterations=16;
	620	int i=0;
	621	btScalar det=0;
	622	q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
	623	det = q.determinant();
	624	if(!btFuzzyZero(det))
	625	{
	626	for(;i<maxiterations;++i)
	627	{
	628	q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
	629	const btScalar ndet=q.determinant();
	630	if(Sq(ndet-det)>accuracy) det=ndet; else break;
	631	}
	632	/* Final orthogonalization */
	633	Orthogonalize(q);
	634	/* Compute 'S' */
	635	s=q.transpose()*m;
	636	}
	637	else
	638	{
	639	q.setIdentity();
	640	s.setIdentity();
	641	}
[1972]	642	return(i);
[1963]	643	}
	644
	645	//
	646	// btSoftColliders
	647	//
	648	struct btSoftColliders
	649	{
	650	//
	651	// ClusterBase
	652	//
	653	struct ClusterBase : btDbvt::ICollide
	654	{
[1972]	655	btScalar erp;
	656	btScalar idt;
	657	btScalar margin;
	658	btScalar friction;
	659	btScalar threshold;
	660	ClusterBase()
[1963]	661	{
[1972]	662	erp =(btScalar)1;
	663	idt =0;
	664	margin =0;
	665	friction =0;
	666	threshold =(btScalar)0;
[1963]	667	}
[1972]	668	bool SolveContact( const btGjkEpaSolver2::sResults& res,
	669	btSoftBody::Body ba,btSoftBody::Body bb,
	670	btSoftBody::CJoint& joint)
[1963]	671	{
[1972]	672	if(res.distance<margin)
[1963]	673	{
[1972]	674	const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
	675	const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
	676	const btVector3 va=ba.velocity(ra);
	677	const btVector3 vb=bb.velocity(rb);
	678	const btVector3 vrel=va-vb;
	679	const btScalar rvac=dot(vrel,res.normal);
	680	const btScalar depth=res.distance-margin;
	681	const btVector3 iv=res.normal*rvac;
	682	const btVector3 fv=vrel-iv;
	683	joint.m_bodies[0] = ba;
	684	joint.m_bodies[1] = bb;
	685	joint.m_refs[0] = ra*ba.xform().getBasis();
	686	joint.m_refs[1] = rb*bb.xform().getBasis();
	687	joint.m_rpos[0] = ra;
	688	joint.m_rpos[1] = rb;
	689	joint.m_cfm = 1;
	690	joint.m_erp = 1;
	691	joint.m_life = 0;
	692	joint.m_maxlife = 0;
	693	joint.m_split = 1;
	694	joint.m_drift = depth*res.normal;
	695	joint.m_normal = res.normal;
	696	joint.m_delete = false;
	697	joint.m_friction = fv.length2()<(-rvac*friction)?1:friction;
	698	joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
	699	bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
	700	return(true);
[1963]	701	}
[1972]	702	return(false);
[1963]	703	}
	704	};
	705	//
	706	// CollideCL_RS
	707	//
	708	struct CollideCL_RS : ClusterBase
	709	{
[1972]	710	btSoftBody* psb;
	711	btRigidBody* prb;
	712	void Process(const btDbvtNode* leaf)
[1963]	713	{
[1972]	714	btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
	715	btSoftClusterCollisionShape cshape(cluster);
	716	const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape();
	717	btGjkEpaSolver2::sResults res;
	718	if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
	719	rshape,prb->getInterpolationWorldTransform(),
	720	btVector3(1,0,0),res))
[1963]	721	{
[1972]	722	btSoftBody::CJoint joint;
	723	if(SolveContact(res,cluster,prb,joint))
[1963]	724	{
[1972]	725	btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
	726	*pj=joint;psb->m_joints.push_back(pj);
	727	if(prb->isStaticOrKinematicObject())
[1963]	728	{
[1972]	729	pj->m_erp *= psb->m_cfg.kSKHR_CL;
	730	pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
[1963]	731	}
	732	else
	733	{
[1972]	734	pj->m_erp *= psb->m_cfg.kSRHR_CL;
	735	pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
[1963]	736	}
	737	}
	738	}
	739	}
[1972]	740	void Process(btSoftBody* ps,btRigidBody* pr)
[1963]	741	{
[1972]	742	psb = ps;
	743	prb = pr;
	744	idt = ps->m_sst.isdt;
	745	margin = ps->getCollisionShape()->getMargin()+
	746	pr->getCollisionShape()->getMargin();
	747	friction = btMin(psb->m_cfg.kDF,prb->getFriction());
	748	btVector3 mins;
	749	btVector3 maxs;
	750
	751	ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
	752	pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
	753	volume=btDbvtVolume::FromMM(mins,maxs);
	754	volume.Expand(btVector3(1,1,1)*margin);
	755	btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
[1963]	756	}
	757	};
	758	//
	759	// CollideCL_SS
	760	//
	761	struct CollideCL_SS : ClusterBase
	762	{
[1972]	763	btSoftBody* bodies[2];
	764	void Process(const btDbvtNode* la,const btDbvtNode* lb)
[1963]	765	{
[1972]	766	btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
	767	btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
	768	btSoftClusterCollisionShape csa(cla);
	769	btSoftClusterCollisionShape csb(clb);
	770	btGjkEpaSolver2::sResults res;
	771	if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(),
	772	&csb,btTransform::getIdentity(),
	773	cla->m_com-clb->m_com,res))
[1963]	774	{
[1972]	775	btSoftBody::CJoint joint;
	776	if(SolveContact(res,cla,clb,joint))
[1963]	777	{
[1972]	778	btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
	779	*pj=joint;bodies[0]->m_joints.push_back(pj);
	780	pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
	781	pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
[1963]	782	}
	783	}
	784	}
[1972]	785	void Process(btSoftBody* psa,btSoftBody* psb)
[1963]	786	{
[1972]	787	idt = psa->m_sst.isdt;
	788	margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
	789	friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
	790	bodies[0] = psa;
	791	bodies[1] = psb;
	792	btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
[1963]	793	}
	794	};
	795	//
	796	// CollideSDF_RS
	797	//
	798	struct CollideSDF_RS : btDbvt::ICollide
	799	{
[1972]	800	void Process(const btDbvtNode* leaf)
[1963]	801	{
[1972]	802	btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
	803	DoNode(*node);
[1963]	804	}
[1972]	805	void DoNode(btSoftBody::Node& n) const
[1963]	806	{
[1972]	807	const btScalar m=n.m_im>0?dynmargin:stamargin;
	808	btSoftBody::RContact c;
	809	if( (!n.m_battach)&&
	810	psb->checkContact(prb,n.m_x,m,c.m_cti))
[1963]	811	{
[1972]	812	const btScalar ima=n.m_im;
	813	const btScalar imb=prb->getInvMass();
	814	const btScalar ms=ima+imb;
	815	if(ms>0)
[1963]	816	{
[1972]	817	const btTransform& wtr=prb->getInterpolationWorldTransform();
	818	const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld();
	819	const btVector3 ra=n.m_x-wtr.getOrigin();
	820	const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
	821	const btVector3 vb=n.m_x-n.m_q;
	822	const btVector3 vr=vb-va;
	823	const btScalar dn=dot(vr,c.m_cti.m_normal);
	824	const btVector3 fv=vr-c.m_cti.m_normal*dn;
	825	const btScalar fc=psb->m_cfg.kDF*prb->getFriction();
	826	c.m_node = &n;
	827	c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
	828	c.m_c1 = ra;
	829	c.m_c2 = ima*psb->m_sst.sdt;
	830	c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc;
	831	c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
	832	psb->m_rcontacts.push_back(c);
	833	prb->activate();
[1963]	834	}
	835	}
	836	}
[1972]	837	btSoftBody* psb;
	838	btRigidBody* prb;
	839	btScalar dynmargin;
	840	btScalar stamargin;
[1963]	841	};
	842	//
	843	// CollideVF_SS
	844	//
	845	struct CollideVF_SS : btDbvt::ICollide
	846	{
[1972]	847	void Process(const btDbvtNode* lnode,
	848	const btDbvtNode* lface)
[1963]	849	{
[1972]	850	btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
	851	btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
	852	btVector3 o=node->m_x;
	853	btVector3 p;
	854	btScalar d=SIMD_INFINITY;
	855	ProjectOrigin( face->m_n[0]->m_x-o,
	856	face->m_n[1]->m_x-o,
	857	face->m_n[2]->m_x-o,
	858	p,d);
	859	const btScalar m=mrg+(o-node->m_q).length()*2;
	860	if(d<(m*m))
[1963]	861	{
[1972]	862	const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
	863	const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
	864	const btScalar ma=node->m_im;
	865	btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
	866	if( (n[0]->m_im<=0)\|\|
	867	(n[1]->m_im<=0)\|\|
	868	(n[2]->m_im<=0))
[1963]	869	{
[1972]	870	mb=0;
[1963]	871	}
[1972]	872	const btScalar ms=ma+mb;
	873	if(ms>0)
[1963]	874	{
[1972]	875	btSoftBody::SContact c;
	876	c.m_normal = p/-btSqrt(d);
	877	c.m_margin = m;
	878	c.m_node = node;
	879	c.m_face = face;
	880	c.m_weights = w;
	881	c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
	882	c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
	883	c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
	884	psb[0]->m_scontacts.push_back(c);
[1963]	885	}
	886	}
	887	}
[1972]	888	btSoftBody* psb[2];
	889	btScalar mrg;
[1963]	890	};
	891	};
	892
	893	#endif //_BT_SOFT_BODY_INTERNALS_H

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

Download in other formats: