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_H
18 #define _BT_SOFT_BODY_H
19 
20 #include "LinearMath/btAlignedObjectArray.h"
21 #include "LinearMath/btTransform.h"
22 #include "LinearMath/btIDebugDraw.h"
23 #include "BulletDynamics/Dynamics/btRigidBody.h"
24 
25 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
26 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
27 #include "btSparseSDF.h"
28 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
29 
30 //#ifdef BT_USE_DOUBLE_PRECISION
31 //#define btRigidBodyData	btRigidBodyDoubleData
32 //#define btRigidBodyDataName	"btRigidBodyDoubleData"
33 //#else
34 #define btSoftBodyData	btSoftBodyFloatData
35 #define btSoftBodyDataName	"btSoftBodyFloatData"
36 //#endif //BT_USE_DOUBLE_PRECISION
37 
38 class btBroadphaseInterface;
39 class btDispatcher;
40 class btSoftBodySolver;
41 
42 /* btSoftBodyWorldInfo	*/
43 struct	btSoftBodyWorldInfo
44 {
45 	btScalar				air_density;
46 	btScalar				water_density;
47 	btScalar				water_offset;
48 	btScalar				m_maxDisplacement;
49 	btVector3				water_normal;
50 	btBroadphaseInterface*	m_broadphase;
51 	btDispatcher*	m_dispatcher;
52 	btVector3				m_gravity;
53 	btSparseSdf<3>			m_sparsesdf;
54 
btSoftBodyWorldInfobtSoftBodyWorldInfo55 	btSoftBodyWorldInfo()
56 		:air_density((btScalar)1.2),
57 		water_density(0),
58 		water_offset(0),
59 		m_maxDisplacement(1000.f),//avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame
60 		water_normal(0,0,0),
61 		m_broadphase(0),
62 		m_dispatcher(0),
63 		m_gravity(0,-10,0)
64 	{
65 	}
66 };
67 
68 
69 ///The btSoftBody is an class to simulate cloth and volumetric soft bodies.
70 ///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject.
71 class	btSoftBody : public btCollisionObject
72 {
73 public:
74 	btAlignedObjectArray<const class btCollisionObject*> m_collisionDisabledObjects;
75 
76 	// The solver object that handles this soft body
77 	btSoftBodySolver *m_softBodySolver;
78 
79 	//
80 	// Enumerations
81 	//
82 
83 	///eAeroModel
84 	struct eAeroModel { enum _ {
85 		V_Point,			///Vertex normals are oriented toward velocity
86 		V_TwoSided,			///Vertex normals are flipped to match velocity
87 		V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied
88 		V_OneSided,			///Vertex normals are taken as it is
89 		F_TwoSided,			///Face normals are flipped to match velocity
90 		F_TwoSidedLiftDrag,	///Face normals are flipped to match velocity and lift and drag forces are applied
91 		F_OneSided,			///Face normals are taken as it is
92 		END
93 	};};
94 
95 	///eVSolver : velocities solvers
96 	struct	eVSolver { enum _ {
97 		Linear,		///Linear solver
98 		END
99 	};};
100 
101 	///ePSolver : positions solvers
102 	struct	ePSolver { enum _ {
103 		Linear,		///Linear solver
104 		Anchors,	///Anchor solver
105 		RContacts,	///Rigid contacts solver
106 		SContacts,	///Soft contacts solver
107 		END
108 	};};
109 
110 	///eSolverPresets
111 	struct	eSolverPresets { enum _ {
112 		Positions,
113 		Velocities,
114 		Default	=	Positions,
115 		END
116 	};};
117 
118 	///eFeature
119 	struct	eFeature { enum _ {
120 		None,
121 		Node,
122 		Link,
123 		Face,
124 		Tetra,
125 		END
126 	};};
127 
128 	typedef btAlignedObjectArray<eVSolver::_>	tVSolverArray;
129 	typedef btAlignedObjectArray<ePSolver::_>	tPSolverArray;
130 
131 	//
132 	// Flags
133 	//
134 
135 	///fCollision
136 	struct fCollision { enum _ {
137 		RVSmask	=	0x000f,	///Rigid versus soft mask
138 		SDF_RS	=	0x0001,	///SDF based rigid vs soft
139 		CL_RS	=	0x0002, ///Cluster vs convex rigid vs soft
140 
141 		SVSmask	=	0x0030,	///Rigid versus soft mask
142 		VF_SS	=	0x0010,	///Vertex vs face soft vs soft handling
143 		CL_SS	=	0x0020, ///Cluster vs cluster soft vs soft handling
144 		CL_SELF =	0x0040, ///Cluster soft body self collision
145 		/* presets	*/
146 		Default	=	SDF_RS,
147 		END
148 	};};
149 
150 	///fMaterial
151 	struct fMaterial { enum _ {
152 		DebugDraw	=	0x0001,	/// Enable debug draw
153 		/* presets	*/
154 		Default		=	DebugDraw,
155 		END
156 	};};
157 
158 	//
159 	// API Types
160 	//
161 
162 	/* sRayCast		*/
163 	struct sRayCast
164 	{
165 		btSoftBody*	body;		/// soft body
166 		eFeature::_	feature;	/// feature type
167 		int			index;		/// feature index
168 		btScalar	fraction;		/// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
169 	};
170 
171 	/* ImplicitFn	*/
172 	struct	ImplicitFn
173 	{
~ImplicitFnImplicitFn174 		virtual ~ImplicitFn() {}
175 		virtual btScalar	Eval(const btVector3& x)=0;
176 	};
177 
178 	//
179 	// Internal types
180 	//
181 
182 	typedef btAlignedObjectArray<btScalar>	tScalarArray;
183 	typedef btAlignedObjectArray<btVector3>	tVector3Array;
184 
185 	/* sCti is Softbody contact info	*/
186 	struct	sCti
187 	{
188 		const btCollisionObject*	m_colObj;		/* Rigid body			*/
189 		btVector3		m_normal;	/* Outward normal		*/
190 		btScalar		m_offset;	/* Offset from origin	*/
191 	};
192 
193 	/* sMedium		*/
194 	struct	sMedium
195 	{
196 		btVector3		m_velocity;	/* Velocity				*/
197 		btScalar		m_pressure;	/* Pressure				*/
198 		btScalar		m_density;	/* Density				*/
199 	};
200 
201 	/* Base type	*/
202 	struct	Element
203 	{
204 		void*			m_tag;			// User data
ElementElement205 		Element() : m_tag(0) {}
206 	};
207 	/* Material		*/
208 	struct	Material : Element
209 	{
210 		btScalar				m_kLST;			// Linear stiffness coefficient [0,1]
211 		btScalar				m_kAST;			// Area/Angular stiffness coefficient [0,1]
212 		btScalar				m_kVST;			// Volume stiffness coefficient [0,1]
213 		int						m_flags;		// Flags
214 	};
215 
216 	/* Feature		*/
217 	struct	Feature : Element
218 	{
219 		Material*				m_material;		// Material
220 	};
221 	/* Node			*/
222 	struct	Node : Feature
223 	{
224 		btVector3				m_x;			// Position
225 		btVector3				m_q;			// Previous step position
226 		btVector3				m_v;			// Velocity
227 		btVector3				m_f;			// Force accumulator
228 		btVector3				m_n;			// Normal
229 		btScalar				m_im;			// 1/mass
230 		btScalar				m_area;			// Area
231 		btDbvtNode*				m_leaf;			// Leaf data
232 		int						m_battach:1;	// Attached
233 	};
234 	/* Link			*/
ATTRIBUTE_ALIGNED16(struct)235 	ATTRIBUTE_ALIGNED16(struct)	Link : Feature
236 	{
237 		btVector3				m_c3;			// gradient
238 		Node*					m_n[2];			// Node pointers
239 		btScalar				m_rl;			// Rest length
240 		int						m_bbending:1;	// Bending link
241 		btScalar				m_c0;			// (ima+imb)*kLST
242 		btScalar				m_c1;			// rl^2
243 		btScalar				m_c2;			// |gradient|^2/c0
244 
245 		BT_DECLARE_ALIGNED_ALLOCATOR();
246 
247 	};
248 	/* Face			*/
249 	struct	Face : Feature
250 	{
251 		Node*					m_n[3];			// Node pointers
252 		btVector3				m_normal;		// Normal
253 		btScalar				m_ra;			// Rest area
254 		btDbvtNode*				m_leaf;			// Leaf data
255 	};
256 	/* Tetra		*/
257 	struct	Tetra : Feature
258 	{
259 		Node*					m_n[4];			// Node pointers
260 		btScalar				m_rv;			// Rest volume
261 		btDbvtNode*				m_leaf;			// Leaf data
262 		btVector3				m_c0[4];		// gradients
263 		btScalar				m_c1;			// (4*kVST)/(im0+im1+im2+im3)
264 		btScalar				m_c2;			// m_c1/sum(|g0..3|^2)
265 	};
266 	/* RContact		*/
267 	struct	RContact
268 	{
269 		sCti		m_cti;			// Contact infos
270 		Node*					m_node;			// Owner node
271 		btMatrix3x3				m_c0;			// Impulse matrix
272 		btVector3				m_c1;			// Relative anchor
273 		btScalar				m_c2;			// ima*dt
274 		btScalar				m_c3;			// Friction
275 		btScalar				m_c4;			// Hardness
276 	};
277 	/* SContact		*/
278 	struct	SContact
279 	{
280 		Node*					m_node;			// Node
281 		Face*					m_face;			// Face
282 		btVector3				m_weights;		// Weigths
283 		btVector3				m_normal;		// Normal
284 		btScalar				m_margin;		// Margin
285 		btScalar				m_friction;		// Friction
286 		btScalar				m_cfm[2];		// Constraint force mixing
287 	};
288 	/* Anchor		*/
289 	struct	Anchor
290 	{
291 		Node*					m_node;			// Node pointer
292 		btVector3				m_local;		// Anchor position in body space
293 		btRigidBody*			m_body;			// Body
294 		btScalar				m_influence;
295 		btMatrix3x3				m_c0;			// Impulse matrix
296 		btVector3				m_c1;			// Relative anchor
297 		btScalar				m_c2;			// ima*dt
298 	};
299 	/* Note			*/
300 	struct	Note : Element
301 	{
302 		const char*				m_text;			// Text
303 		btVector3				m_offset;		// Offset
304 		int						m_rank;			// Rank
305 		Node*					m_nodes[4];		// Nodes
306 		btScalar				m_coords[4];	// Coordinates
307 	};
308 	/* Pose			*/
309 	struct	Pose
310 	{
311 		bool					m_bvolume;		// Is valid
312 		bool					m_bframe;		// Is frame
313 		btScalar				m_volume;		// Rest volume
314 		tVector3Array			m_pos;			// Reference positions
315 		tScalarArray			m_wgh;			// Weights
316 		btVector3				m_com;			// COM
317 		btMatrix3x3				m_rot;			// Rotation
318 		btMatrix3x3				m_scl;			// Scale
319 		btMatrix3x3				m_aqq;			// Base scaling
320 	};
321 	/* Cluster		*/
322 	struct	Cluster
323 	{
324 		tScalarArray				m_masses;
325 		btAlignedObjectArray<Node*>	m_nodes;
326 		tVector3Array				m_framerefs;
327 		btTransform					m_framexform;
328 		btScalar					m_idmass;
329 		btScalar					m_imass;
330 		btMatrix3x3					m_locii;
331 		btMatrix3x3					m_invwi;
332 		btVector3					m_com;
333 		btVector3					m_vimpulses[2];
334 		btVector3					m_dimpulses[2];
335 		int							m_nvimpulses;
336 		int							m_ndimpulses;
337 		btVector3					m_lv;
338 		btVector3					m_av;
339 		btDbvtNode*					m_leaf;
340 		btScalar					m_ndamping;	/* Node damping		*/
341 		btScalar					m_ldamping;	/* Linear damping	*/
342 		btScalar					m_adamping;	/* Angular damping	*/
343 		btScalar					m_matching;
344 		btScalar					m_maxSelfCollisionImpulse;
345 		btScalar					m_selfCollisionImpulseFactor;
346 		bool						m_containsAnchor;
347 		bool						m_collide;
348 		int							m_clusterIndex;
ClusterCluster349 		Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0)
350 		,m_maxSelfCollisionImpulse(100.f),
351 		m_selfCollisionImpulseFactor(0.01f),
352 		m_containsAnchor(false)
353 		{}
354 	};
355 	/* Impulse		*/
356 	struct	Impulse
357 	{
358 		btVector3					m_velocity;
359 		btVector3					m_drift;
360 		int							m_asVelocity:1;
361 		int							m_asDrift:1;
ImpulseImpulse362 		Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0)	{}
363 		Impulse						operator -() const
364 		{
365 			Impulse i=*this;
366 			i.m_velocity=-i.m_velocity;
367 			i.m_drift=-i.m_drift;
368 			return(i);
369 		}
370 		Impulse						operator*(btScalar x) const
371 		{
372 			Impulse i=*this;
373 			i.m_velocity*=x;
374 			i.m_drift*=x;
375 			return(i);
376 		}
377 	};
378 	/* Body			*/
379 	struct	Body
380 	{
381 		Cluster*			m_soft;
382 		btRigidBody*		m_rigid;
383 		const btCollisionObject*	m_collisionObject;
384 
BodyBody385 		Body() : m_soft(0),m_rigid(0),m_collisionObject(0)				{}
BodyBody386 		Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0)	{}
BodyBody387 		Body(const btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj)
388 		{
389 			m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject);
390 		}
391 
activateBody392 		void						activate() const
393 		{
394 			if(m_rigid)
395 				m_rigid->activate();
396 			if (m_collisionObject)
397 				m_collisionObject->activate();
398 
399 		}
invWorldInertiaBody400 		const btMatrix3x3&			invWorldInertia() const
401 		{
402 			static const btMatrix3x3	iwi(0,0,0,0,0,0,0,0,0);
403 			if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
404 			if(m_soft)	return(m_soft->m_invwi);
405 			return(iwi);
406 		}
invMassBody407 		btScalar					invMass() const
408 		{
409 			if(m_rigid) return(m_rigid->getInvMass());
410 			if(m_soft)	return(m_soft->m_imass);
411 			return(0);
412 		}
xformBody413 		const btTransform&			xform() const
414 		{
415 			static const btTransform	identity=btTransform::getIdentity();
416 			if(m_collisionObject) return(m_collisionObject->getWorldTransform());
417 			if(m_soft)	return(m_soft->m_framexform);
418 			return(identity);
419 		}
linearVelocityBody420 		btVector3					linearVelocity() const
421 		{
422 			if(m_rigid) return(m_rigid->getLinearVelocity());
423 			if(m_soft)	return(m_soft->m_lv);
424 			return(btVector3(0,0,0));
425 		}
angularVelocityBody426 		btVector3					angularVelocity(const btVector3& rpos) const
427 		{
428 			if(m_rigid) return(btCross(m_rigid->getAngularVelocity(),rpos));
429 			if(m_soft)	return(btCross(m_soft->m_av,rpos));
430 			return(btVector3(0,0,0));
431 		}
angularVelocityBody432 		btVector3					angularVelocity() const
433 		{
434 			if(m_rigid) return(m_rigid->getAngularVelocity());
435 			if(m_soft)	return(m_soft->m_av);
436 			return(btVector3(0,0,0));
437 		}
velocityBody438 		btVector3					velocity(const btVector3& rpos) const
439 		{
440 			return(linearVelocity()+angularVelocity(rpos));
441 		}
applyVImpulseBody442 		void						applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
443 		{
444 			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
445 			if(m_soft)	btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
446 		}
applyDImpulseBody447 		void						applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
448 		{
449 			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
450 			if(m_soft)	btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
451 		}
applyImpulseBody452 		void						applyImpulse(const Impulse& impulse,const btVector3& rpos) const
453 		{
454 			if(impulse.m_asVelocity)
455 			{
456 //				printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ());
457 				applyVImpulse(impulse.m_velocity,rpos);
458 			}
459 			if(impulse.m_asDrift)
460 			{
461 //				printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ());
462 				applyDImpulse(impulse.m_drift,rpos);
463 			}
464 		}
applyVAImpulseBody465 		void						applyVAImpulse(const btVector3& impulse) const
466 		{
467 			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
468 			if(m_soft)	btSoftBody::clusterVAImpulse(m_soft,impulse);
469 		}
applyDAImpulseBody470 		void						applyDAImpulse(const btVector3& impulse) const
471 		{
472 			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
473 			if(m_soft)	btSoftBody::clusterDAImpulse(m_soft,impulse);
474 		}
applyAImpulseBody475 		void						applyAImpulse(const Impulse& impulse) const
476 		{
477 			if(impulse.m_asVelocity)	applyVAImpulse(impulse.m_velocity);
478 			if(impulse.m_asDrift)		applyDAImpulse(impulse.m_drift);
479 		}
applyDCImpulseBody480 		void						applyDCImpulse(const btVector3& impulse) const
481 		{
482 			if(m_rigid)	m_rigid->applyCentralImpulse(impulse);
483 			if(m_soft)	btSoftBody::clusterDCImpulse(m_soft,impulse);
484 		}
485 	};
486 	/* Joint		*/
487 	struct	Joint
488 	{
489 		struct eType { enum _ {
490 			Linear=0,
491 			Angular,
492 			Contact
493 		};};
494 		struct Specs
495 		{
SpecsJoint::Specs496 			Specs() : erp(1),cfm(1),split(1) {}
497 			btScalar	erp;
498 			btScalar	cfm;
499 			btScalar	split;
500 		};
501 		Body						m_bodies[2];
502 		btVector3					m_refs[2];
503 		btScalar					m_cfm;
504 		btScalar					m_erp;
505 		btScalar					m_split;
506 		btVector3					m_drift;
507 		btVector3					m_sdrift;
508 		btMatrix3x3					m_massmatrix;
509 		bool						m_delete;
~JointJoint510 		virtual						~Joint() {}
JointJoint511 		Joint() : m_delete(false) {}
512 		virtual void				Prepare(btScalar dt,int iterations);
513 		virtual void				Solve(btScalar dt,btScalar sor)=0;
514 		virtual void				Terminate(btScalar dt)=0;
515 		virtual eType::_			Type() const=0;
516 	};
517 	/* LJoint		*/
518 	struct	LJoint : Joint
519 	{
520 		struct Specs : Joint::Specs
521 		{
522 			btVector3	position;
523 		};
524 		btVector3					m_rpos[2];
525 		void						Prepare(btScalar dt,int iterations);
526 		void						Solve(btScalar dt,btScalar sor);
527 		void						Terminate(btScalar dt);
TypeLJoint528 		eType::_					Type() const { return(eType::Linear); }
529 	};
530 	/* AJoint		*/
531 	struct	AJoint : Joint
532 	{
533 		struct IControl
534 		{
~IControlAJoint::IControl535 			virtual ~IControl() {}
PrepareAJoint::IControl536 			virtual void			Prepare(AJoint*)				{}
SpeedAJoint::IControl537 			virtual btScalar		Speed(AJoint*,btScalar current) { return(current); }
DefaultAJoint::IControl538 			static IControl*		Default()						{ static IControl def;return(&def); }
539 		};
540 		struct Specs : Joint::Specs
541 		{
SpecsAJoint::Specs542 			Specs() : icontrol(IControl::Default()) {}
543 			btVector3	axis;
544 			IControl*	icontrol;
545 		};
546 		btVector3					m_axis[2];
547 		IControl*					m_icontrol;
548 		void						Prepare(btScalar dt,int iterations);
549 		void						Solve(btScalar dt,btScalar sor);
550 		void						Terminate(btScalar dt);
TypeAJoint551 		eType::_					Type() const { return(eType::Angular); }
552 	};
553 	/* CJoint		*/
554 	struct	CJoint : Joint
555 	{
556 		int							m_life;
557 		int							m_maxlife;
558 		btVector3					m_rpos[2];
559 		btVector3					m_normal;
560 		btScalar					m_friction;
561 		void						Prepare(btScalar dt,int iterations);
562 		void						Solve(btScalar dt,btScalar sor);
563 		void						Terminate(btScalar dt);
TypeCJoint564 		eType::_					Type() const { return(eType::Contact); }
565 	};
566 	/* Config		*/
567 	struct	Config
568 	{
569 		eAeroModel::_			aeromodel;		// Aerodynamic model (default: V_Point)
570 		btScalar				kVCF;			// Velocities correction factor (Baumgarte)
571 		btScalar				kDP;			// Damping coefficient [0,1]
572 		btScalar				kDG;			// Drag coefficient [0,+inf]
573 		btScalar				kLF;			// Lift coefficient [0,+inf]
574 		btScalar				kPR;			// Pressure coefficient [-inf,+inf]
575 		btScalar				kVC;			// Volume conversation coefficient [0,+inf]
576 		btScalar				kDF;			// Dynamic friction coefficient [0,1]
577 		btScalar				kMT;			// Pose matching coefficient [0,1]
578 		btScalar				kCHR;			// Rigid contacts hardness [0,1]
579 		btScalar				kKHR;			// Kinetic contacts hardness [0,1]
580 		btScalar				kSHR;			// Soft contacts hardness [0,1]
581 		btScalar				kAHR;			// Anchors hardness [0,1]
582 		btScalar				kSRHR_CL;		// Soft vs rigid hardness [0,1] (cluster only)
583 		btScalar				kSKHR_CL;		// Soft vs kinetic hardness [0,1] (cluster only)
584 		btScalar				kSSHR_CL;		// Soft vs soft hardness [0,1] (cluster only)
585 		btScalar				kSR_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
586 		btScalar				kSK_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
587 		btScalar				kSS_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
588 		btScalar				maxvolume;		// Maximum volume ratio for pose
589 		btScalar				timescale;		// Time scale
590 		int						viterations;	// Velocities solver iterations
591 		int						piterations;	// Positions solver iterations
592 		int						diterations;	// Drift solver iterations
593 		int						citerations;	// Cluster solver iterations
594 		int						collisions;		// Collisions flags
595 		tVSolverArray			m_vsequence;	// Velocity solvers sequence
596 		tPSolverArray			m_psequence;	// Position solvers sequence
597 		tPSolverArray			m_dsequence;	// Drift solvers sequence
598 	};
599 	/* SolverState	*/
600 	struct	SolverState
601 	{
602 		btScalar				sdt;			// dt*timescale
603 		btScalar				isdt;			// 1/sdt
604 		btScalar				velmrg;			// velocity margin
605 		btScalar				radmrg;			// radial margin
606 		btScalar				updmrg;			// Update margin
607 	};
608 	/// RayFromToCaster takes a ray from, ray to (instead of direction!)
609 	struct	RayFromToCaster : btDbvt::ICollide
610 	{
611 		btVector3			m_rayFrom;
612 		btVector3			m_rayTo;
613 		btVector3			m_rayNormalizedDirection;
614 		btScalar			m_mint;
615 		Face*				m_face;
616 		int					m_tests;
617 		RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
618 		void					Process(const btDbvtNode* leaf);
619 
620 		static inline btScalar	rayFromToTriangle(const btVector3& rayFrom,
621 			const btVector3& rayTo,
622 			const btVector3& rayNormalizedDirection,
623 			const btVector3& a,
624 			const btVector3& b,
625 			const btVector3& c,
626 			btScalar maxt=SIMD_INFINITY);
627 	};
628 
629 	//
630 	// Typedefs
631 	//
632 
633 	typedef void								(*psolver_t)(btSoftBody*,btScalar,btScalar);
634 	typedef void								(*vsolver_t)(btSoftBody*,btScalar);
635 	typedef btAlignedObjectArray<Cluster*>		tClusterArray;
636 	typedef btAlignedObjectArray<Note>			tNoteArray;
637 	typedef btAlignedObjectArray<Node>			tNodeArray;
638 	typedef btAlignedObjectArray<btDbvtNode*>	tLeafArray;
639 	typedef btAlignedObjectArray<Link>			tLinkArray;
640 	typedef btAlignedObjectArray<Face>			tFaceArray;
641 	typedef btAlignedObjectArray<Tetra>			tTetraArray;
642 	typedef btAlignedObjectArray<Anchor>		tAnchorArray;
643 	typedef btAlignedObjectArray<RContact>		tRContactArray;
644 	typedef btAlignedObjectArray<SContact>		tSContactArray;
645 	typedef btAlignedObjectArray<Material*>		tMaterialArray;
646 	typedef btAlignedObjectArray<Joint*>		tJointArray;
647 	typedef btAlignedObjectArray<btSoftBody*>	tSoftBodyArray;
648 
649 	//
650 	// Fields
651 	//
652 
653 	Config					m_cfg;			// Configuration
654 	SolverState				m_sst;			// Solver state
655 	Pose					m_pose;			// Pose
656 	void*					m_tag;			// User data
657 	btSoftBodyWorldInfo*	m_worldInfo;	// World info
658 	tNoteArray				m_notes;		// Notes
659 	tNodeArray				m_nodes;		// Nodes
660 	tLinkArray				m_links;		// Links
661 	tFaceArray				m_faces;		// Faces
662 	tTetraArray				m_tetras;		// Tetras
663 	tAnchorArray			m_anchors;		// Anchors
664 	tRContactArray			m_rcontacts;	// Rigid contacts
665 	tSContactArray			m_scontacts;	// Soft contacts
666 	tJointArray				m_joints;		// Joints
667 	tMaterialArray			m_materials;	// Materials
668 	btScalar				m_timeacc;		// Time accumulator
669 	btVector3				m_bounds[2];	// Spatial bounds
670 	bool					m_bUpdateRtCst;	// Update runtime constants
671 	btDbvt					m_ndbvt;		// Nodes tree
672 	btDbvt					m_fdbvt;		// Faces tree
673 	btDbvt					m_cdbvt;		// Clusters tree
674 	tClusterArray			m_clusters;		// Clusters
675 
676 	btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision
677 
678 	btTransform			m_initialWorldTransform;
679 
680 	btVector3			m_windVelocity;
681 
682 	btScalar        m_restLengthScale;
683 
684 	//
685 	// Api
686 	//
687 
688 	/* ctor																	*/
689 	btSoftBody(	btSoftBodyWorldInfo* worldInfo,int node_count,		const btVector3* x,		const btScalar* m);
690 
691 	/* ctor																	*/
692 	btSoftBody(	btSoftBodyWorldInfo* worldInfo);
693 
694 	void	initDefaults();
695 
696 	/* dtor																	*/
697 	virtual ~btSoftBody();
698 	/* Check for existing link												*/
699 
700 	btAlignedObjectArray<int>	m_userIndexMapping;
701 
getWorldInfo()702 	btSoftBodyWorldInfo*	getWorldInfo()
703 	{
704 		return m_worldInfo;
705 	}
706 
707 	///@todo: avoid internal softbody shape hack and move collision code to collision library
setCollisionShape(btCollisionShape * collisionShape)708 	virtual void	setCollisionShape(btCollisionShape* collisionShape)
709 	{
710 
711 	}
712 
713 	bool				checkLink(	int node0,
714 		int node1) const;
715 	bool				checkLink(	const Node* node0,
716 		const Node* node1) const;
717 	/* Check for existring face												*/
718 	bool				checkFace(	int node0,
719 		int node1,
720 		int node2) const;
721 	/* Append material														*/
722 	Material*			appendMaterial();
723 	/* Append note															*/
724 	void				appendNote(	const char* text,
725 		const btVector3& o,
726 		const btVector4& c=btVector4(1,0,0,0),
727 		Node* n0=0,
728 		Node* n1=0,
729 		Node* n2=0,
730 		Node* n3=0);
731 	void				appendNote(	const char* text,
732 		const btVector3& o,
733 		Node* feature);
734 	void				appendNote(	const char* text,
735 		const btVector3& o,
736 		Link* feature);
737 	void				appendNote(	const char* text,
738 		const btVector3& o,
739 		Face* feature);
740 	/* Append node															*/
741 	void				appendNode(	const btVector3& x,btScalar m);
742 	/* Append link															*/
743 	void				appendLink(int model=-1,Material* mat=0);
744 	void				appendLink(	int node0,
745 		int node1,
746 		Material* mat=0,
747 		bool bcheckexist=false);
748 	void				appendLink(	Node* node0,
749 		Node* node1,
750 		Material* mat=0,
751 		bool bcheckexist=false);
752 	/* Append face															*/
753 	void				appendFace(int model=-1,Material* mat=0);
754 	void				appendFace(	int node0,
755 		int node1,
756 		int node2,
757 		Material* mat=0);
758 	void			appendTetra(int model,Material* mat);
759 	//
760 	void			appendTetra(int node0,
761 										int node1,
762 										int node2,
763 										int node3,
764 										Material* mat=0);
765 
766 
767 	/* Append anchor														*/
768 	void				appendAnchor(	int node,
769 		btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1);
770 	void			appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1);
771 	/* Append linear joint													*/
772 	void				appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
773 	void				appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
774 	void				appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body);
775 	/* Append linear joint													*/
776 	void				appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1);
777 	void				appendAngularJoint(const AJoint::Specs& specs,Body body=Body());
778 	void				appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body);
779 	/* Add force (or gravity) to the entire body							*/
780 	void				addForce(		const btVector3& force);
781 	/* Add force (or gravity) to a node of the body							*/
782 	void				addForce(		const btVector3& force,
783 		int node);
784 	/* Add aero force to a node of the body */
785 	void			    addAeroForceToNode(const btVector3& windVelocity,int nodeIndex);
786 
787 	/* Add aero force to a face of the body */
788 	void			    addAeroForceToFace(const btVector3& windVelocity,int faceIndex);
789 
790 	/* Add velocity to the entire body										*/
791 	void				addVelocity(	const btVector3& velocity);
792 
793 	/* Set velocity for the entire body										*/
794 	void				setVelocity(	const btVector3& velocity);
795 
796 	/* Add velocity to a node of the body									*/
797 	void				addVelocity(	const btVector3& velocity,
798 		int node);
799 	/* Set mass																*/
800 	void				setMass(		int node,
801 		btScalar mass);
802 	/* Get mass																*/
803 	btScalar			getMass(		int node) const;
804 	/* Get total mass														*/
805 	btScalar			getTotalMass() const;
806 	/* Set total mass (weighted by previous masses)							*/
807 	void				setTotalMass(	btScalar mass,
808 		bool fromfaces=false);
809 	/* Set total density													*/
810 	void				setTotalDensity(btScalar density);
811 	/* Set volume mass (using tetrahedrons)									*/
812 	void				setVolumeMass(		btScalar mass);
813 	/* Set volume density (using tetrahedrons)								*/
814 	void				setVolumeDensity(	btScalar density);
815 	/* Transform															*/
816 	void				transform(		const btTransform& trs);
817 	/* Translate															*/
818 	void				translate(		const btVector3& trs);
819 	/* Rotate															*/
820 	void				rotate(	const btQuaternion& rot);
821 	/* Scale																*/
822 	void				scale(	const btVector3& scl);
823 	/* Get link resting lengths scale										*/
824 	btScalar			getRestLengthScale();
825 	/* Scale resting length of all springs									*/
826 	void				setRestLengthScale(btScalar restLength);
827 	/* Set current state as pose											*/
828 	void				setPose(		bool bvolume,
829 		bool bframe);
830 	/* Set current link lengths as resting lengths							*/
831 	void				resetLinkRestLengths();
832 	/* Return the volume													*/
833 	btScalar			getVolume() const;
834 	/* Cluster count														*/
835 	int					clusterCount() const;
836 	/* Cluster center of mass												*/
837 	static btVector3	clusterCom(const Cluster* cluster);
838 	btVector3			clusterCom(int cluster) const;
839 	/* Cluster velocity at rpos												*/
840 	static btVector3	clusterVelocity(const Cluster* cluster,const btVector3& rpos);
841 	/* Cluster impulse														*/
842 	static void			clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
843 	static void			clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
844 	static void			clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse);
845 	static void			clusterVAImpulse(Cluster* cluster,const btVector3& impulse);
846 	static void			clusterDAImpulse(Cluster* cluster,const btVector3& impulse);
847 	static void			clusterAImpulse(Cluster* cluster,const Impulse& impulse);
848 	static void			clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
849 	/* Generate bending constraints based on distance in the adjency graph	*/
850 	int					generateBendingConstraints(	int distance,
851 		Material* mat=0);
852 	/* Randomize constraints to reduce solver bias							*/
853 	void				randomizeConstraints();
854 	/* Release clusters														*/
855 	void				releaseCluster(int index);
856 	void				releaseClusters();
857 	/* Generate clusters (K-mean)											*/
858 	///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle
859 	///otherwise an approximation will be used (better performance)
860 	int					generateClusters(int k,int maxiterations=8192);
861 	/* Refine																*/
862 	void				refine(ImplicitFn* ifn,btScalar accurary,bool cut);
863 	/* CutLink																*/
864 	bool				cutLink(int node0,int node1,btScalar position);
865 	bool				cutLink(const Node* node0,const Node* node1,btScalar position);
866 
867 	///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
868 	bool				rayTest(const btVector3& rayFrom,
869 		const btVector3& rayTo,
870 		sRayCast& results);
871 	/* Solver presets														*/
872 	void				setSolver(eSolverPresets::_ preset);
873 	/* predictMotion														*/
874 	void				predictMotion(btScalar dt);
875 	/* solveConstraints														*/
876 	void				solveConstraints();
877 	/* staticSolve															*/
878 	void				staticSolve(int iterations);
879 	/* solveCommonConstraints												*/
880 	static void			solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
881 	/* solveClusters														*/
882 	static void			solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
883 	/* integrateMotion														*/
884 	void				integrateMotion();
885 	/* defaultCollisionHandlers												*/
886 	void				defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
887 	void				defaultCollisionHandler(btSoftBody* psb);
888 
889 
890 
891 	//
892 	// Functionality to deal with new accelerated solvers.
893 	//
894 
895 	/**
896 	 * Set a wind velocity for interaction with the air.
897 	 */
898 	void setWindVelocity( const btVector3 &velocity );
899 
900 
901 	/**
902 	 * Return the wind velocity for interaction with the air.
903 	 */
904 	const btVector3& getWindVelocity();
905 
906 	//
907 	// Set the solver that handles this soft body
908 	// Should not be allowed to get out of sync with reality
909 	// Currently called internally on addition to the world
setSoftBodySolver(btSoftBodySolver * softBodySolver)910 	void setSoftBodySolver( btSoftBodySolver *softBodySolver )
911 	{
912 		m_softBodySolver = softBodySolver;
913 	}
914 
915 	//
916 	// Return the solver that handles this soft body
917 	//
getSoftBodySolver()918 	btSoftBodySolver *getSoftBodySolver()
919 	{
920 		return m_softBodySolver;
921 	}
922 
923 	//
924 	// Return the solver that handles this soft body
925 	//
getSoftBodySolver()926 	btSoftBodySolver *getSoftBodySolver() const
927 	{
928 		return m_softBodySolver;
929 	}
930 
931 
932 	//
933 	// Cast
934 	//
935 
upcast(const btCollisionObject * colObj)936 	static const btSoftBody*	upcast(const btCollisionObject* colObj)
937 	{
938 		if (colObj->getInternalType()==CO_SOFT_BODY)
939 			return (const btSoftBody*)colObj;
940 		return 0;
941 	}
upcast(btCollisionObject * colObj)942 	static btSoftBody*			upcast(btCollisionObject* colObj)
943 	{
944 		if (colObj->getInternalType()==CO_SOFT_BODY)
945 			return (btSoftBody*)colObj;
946 		return 0;
947 	}
948 
949 	//
950 	// ::btCollisionObject
951 	//
952 
getAabb(btVector3 & aabbMin,btVector3 & aabbMax)953 	virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const
954 	{
955 		aabbMin = m_bounds[0];
956 		aabbMax = m_bounds[1];
957 	}
958 	//
959 	// Private
960 	//
961 	void				pointersToIndices();
962 	void				indicesToPointers(const int* map=0);
963 
964 	int					rayTest(const btVector3& rayFrom,const btVector3& rayTo,
965 		btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
966 	void				initializeFaceTree();
967 	btVector3			evaluateCom() const;
968 	bool				checkContact(const btCollisionObjectWrapper* colObjWrap,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;
969 	void				updateNormals();
970 	void				updateBounds();
971 	void				updatePose();
972 	void				updateConstants();
973 	void				updateLinkConstants();
974 	void				updateArea(bool averageArea = true);
975 	void				initializeClusters();
976 	void				updateClusters();
977 	void				cleanupClusters();
978 	void				prepareClusters(int iterations);
979 	void				solveClusters(btScalar sor);
980 	void				applyClusters(bool drift);
981 	void				dampClusters();
982 	void				applyForces();
983 	static void			PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti);
984 	static void			PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti);
985 	static void			PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti);
986 	static void			PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti);
987 	static void			VSolve_Links(btSoftBody* psb,btScalar kst);
988 	static psolver_t	getSolver(ePSolver::_ solver);
989 	static vsolver_t	getSolver(eVSolver::_ solver);
990 
991 
992 	virtual	int	calculateSerializeBufferSize()	const;
993 
994 	///fills the dataBuffer and returns the struct name (and 0 on failure)
995 	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
996 
997 	//virtual void serializeSingleObject(class btSerializer* serializer) const;
998 
999 
1000 };
1001 
1002 
1003 
1004 
1005 #endif //_BT_SOFT_BODY_H
1006