• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2010 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 
16 #ifndef BT_TYPED_CONSTRAINT_H
17 #define BT_TYPED_CONSTRAINT_H
18 
19 
20 #include "LinearMath/btScalar.h"
21 #include "btSolverConstraint.h"
22 #include "BulletDynamics/Dynamics/btRigidBody.h"
23 
24 #ifdef BT_USE_DOUBLE_PRECISION
25 #define btTypedConstraintData2		btTypedConstraintDoubleData
26 #define btTypedConstraintDataName	"btTypedConstraintDoubleData"
27 #else
28 #define btTypedConstraintData2 		btTypedConstraintFloatData
29 #define btTypedConstraintDataName  "btTypedConstraintFloatData"
30 #endif //BT_USE_DOUBLE_PRECISION
31 
32 
33 class btSerializer;
34 
35 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
36 enum btTypedConstraintType
37 {
38 	POINT2POINT_CONSTRAINT_TYPE=3,
39 	HINGE_CONSTRAINT_TYPE,
40 	CONETWIST_CONSTRAINT_TYPE,
41 	D6_CONSTRAINT_TYPE,
42 	SLIDER_CONSTRAINT_TYPE,
43 	CONTACT_CONSTRAINT_TYPE,
44 	D6_SPRING_CONSTRAINT_TYPE,
45 	GEAR_CONSTRAINT_TYPE,
46 	FIXED_CONSTRAINT_TYPE,
47 	D6_SPRING_2_CONSTRAINT_TYPE,
48 	MAX_CONSTRAINT_TYPE
49 };
50 
51 
52 enum btConstraintParams
53 {
54 	BT_CONSTRAINT_ERP=1,
55 	BT_CONSTRAINT_STOP_ERP,
56 	BT_CONSTRAINT_CFM,
57 	BT_CONSTRAINT_STOP_CFM
58 };
59 
60 #if 1
61 	#define btAssertConstrParams(_par) btAssert(_par)
62 #else
63 	#define btAssertConstrParams(_par)
64 #endif
65 
66 
ATTRIBUTE_ALIGNED16(struct)67 ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
68 {
69 	btVector3	m_appliedForceBodyA;
70 	btVector3	m_appliedTorqueBodyA;
71 	btVector3	m_appliedForceBodyB;
72 	btVector3	m_appliedTorqueBodyB;
73 };
74 
75 
76 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
ATTRIBUTE_ALIGNED16(class)77 ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
78 {
79 	int	m_userConstraintType;
80 
81 	union
82 	{
83 		int	m_userConstraintId;
84 		void* m_userConstraintPtr;
85 	};
86 
87 	btScalar	m_breakingImpulseThreshold;
88 	bool		m_isEnabled;
89 	bool		m_needsFeedback;
90 	int			m_overrideNumSolverIterations;
91 
92 
93 	btTypedConstraint&	operator=(btTypedConstraint&	other)
94 	{
95 		btAssert(0);
96 		(void) other;
97 		return *this;
98 	}
99 
100 protected:
101 	btRigidBody&	m_rbA;
102 	btRigidBody&	m_rbB;
103 	btScalar	m_appliedImpulse;
104 	btScalar	m_dbgDrawSize;
105 	btJointFeedback*	m_jointFeedback;
106 
107 	///internal method used by the constraint solver, don't use them directly
108 	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
109 
110 
111 public:
112 
113 	BT_DECLARE_ALIGNED_ALLOCATOR();
114 
115 	virtual ~btTypedConstraint() {};
116 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
117 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
118 
119 	struct btConstraintInfo1 {
120 		int m_numConstraintRows,nub;
121 	};
122 
123 	static btRigidBody& getFixedBody();
124 
125 	struct btConstraintInfo2 {
126 		// integrator parameters: frames per second (1/stepsize), default error
127 		// reduction parameter (0..1).
128 		btScalar fps,erp;
129 
130 		// for the first and second body, pointers to two (linear and angular)
131 		// n*3 jacobian sub matrices, stored by rows. these matrices will have
132 		// been initialized to 0 on entry. if the second body is zero then the
133 		// J2xx pointers may be 0.
134 		btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
135 
136 		// elements to jump from one row to the next in J's
137 		int rowskip;
138 
139 		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
140 		// "constraint force mixing" vector. c is set to zero on entry, cfm is
141 		// set to a constant value (typically very small or zero) value on entry.
142 		btScalar *m_constraintError,*cfm;
143 
144 		// lo and hi limits for variables (set to -/+ infinity on entry).
145 		btScalar *m_lowerLimit,*m_upperLimit;
146 
147 		// findex vector for variables. see the LCP solver interface for a
148 		// description of what this does. this is set to -1 on entry.
149 		// note that the returned indexes are relative to the first index of
150 		// the constraint.
151 		int *findex;
152 		// number of solver iterations
153 		int m_numIterations;
154 
155 		//damping of the velocity
156 		btScalar	m_damping;
157 	};
158 
159 	int	getOverrideNumSolverIterations() const
160 	{
161 		return m_overrideNumSolverIterations;
162 	}
163 
164 	///override the number of constraint solver iterations used to solve this constraint
165 	///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
166 	void setOverrideNumSolverIterations(int overideNumIterations)
167 	{
168 		m_overrideNumSolverIterations = overideNumIterations;
169 	}
170 
171 	///internal method used by the constraint solver, don't use them directly
172 	virtual void	buildJacobian() {};
173 
174 	///internal method used by the constraint solver, don't use them directly
175 	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
176 	{
177         (void)ca;
178         (void)solverBodyA;
179         (void)solverBodyB;
180         (void)timeStep;
181 	}
182 
183 	///internal method used by the constraint solver, don't use them directly
184 	virtual void getInfo1 (btConstraintInfo1* info)=0;
185 
186 	///internal method used by the constraint solver, don't use them directly
187 	virtual void getInfo2 (btConstraintInfo2* info)=0;
188 
189 	///internal method used by the constraint solver, don't use them directly
190 	void	internalSetAppliedImpulse(btScalar appliedImpulse)
191 	{
192 		m_appliedImpulse = appliedImpulse;
193 	}
194 	///internal method used by the constraint solver, don't use them directly
195 	btScalar	internalGetAppliedImpulse()
196 	{
197 		return m_appliedImpulse;
198 	}
199 
200 
201 	btScalar	getBreakingImpulseThreshold() const
202 	{
203 		return 	m_breakingImpulseThreshold;
204 	}
205 
206 	void	setBreakingImpulseThreshold(btScalar threshold)
207 	{
208 		m_breakingImpulseThreshold = threshold;
209 	}
210 
211 	bool	isEnabled() const
212 	{
213 		return m_isEnabled;
214 	}
215 
216 	void	setEnabled(bool enabled)
217 	{
218 		m_isEnabled=enabled;
219 	}
220 
221 
222 	///internal method used by the constraint solver, don't use them directly
223 	virtual	void	solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar	/*timeStep*/) {};
224 
225 
226 	const btRigidBody& getRigidBodyA() const
227 	{
228 		return m_rbA;
229 	}
230 	const btRigidBody& getRigidBodyB() const
231 	{
232 		return m_rbB;
233 	}
234 
235 	btRigidBody& getRigidBodyA()
236 	{
237 		return m_rbA;
238 	}
239 	btRigidBody& getRigidBodyB()
240 	{
241 		return m_rbB;
242 	}
243 
244 	int getUserConstraintType() const
245 	{
246 		return m_userConstraintType ;
247 	}
248 
249 	void	setUserConstraintType(int userConstraintType)
250 	{
251 		m_userConstraintType = userConstraintType;
252 	};
253 
254 	void	setUserConstraintId(int uid)
255 	{
256 		m_userConstraintId = uid;
257 	}
258 
259 	int getUserConstraintId() const
260 	{
261 		return m_userConstraintId;
262 	}
263 
264 	void	setUserConstraintPtr(void* ptr)
265 	{
266 		m_userConstraintPtr = ptr;
267 	}
268 
269 	void*	getUserConstraintPtr()
270 	{
271 		return m_userConstraintPtr;
272 	}
273 
274 	void	setJointFeedback(btJointFeedback* jointFeedback)
275 	{
276 		m_jointFeedback = jointFeedback;
277 	}
278 
279 	const btJointFeedback* getJointFeedback() const
280 	{
281 		return m_jointFeedback;
282 	}
283 
284 	btJointFeedback* getJointFeedback()
285 	{
286 		return m_jointFeedback;
287 	}
288 
289 
290 	int getUid() const
291 	{
292 		return m_userConstraintId;
293 	}
294 
295 	bool	needsFeedback() const
296 	{
297 		return m_needsFeedback;
298 	}
299 
300 	///enableFeedback will allow to read the applied linear and angular impulse
301 	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
302 	void	enableFeedback(bool needsFeedback)
303 	{
304 		m_needsFeedback = needsFeedback;
305 	}
306 
307 	///getAppliedImpulse is an estimated total applied impulse.
308 	///This feedback could be used to determine breaking constraints or playing sounds.
309 	btScalar	getAppliedImpulse() const
310 	{
311 		btAssert(m_needsFeedback);
312 		return m_appliedImpulse;
313 	}
314 
315 	btTypedConstraintType getConstraintType () const
316 	{
317 		return btTypedConstraintType(m_objectType);
318 	}
319 
320 	void setDbgDrawSize(btScalar dbgDrawSize)
321 	{
322 		m_dbgDrawSize = dbgDrawSize;
323 	}
324 	btScalar getDbgDrawSize()
325 	{
326 		return m_dbgDrawSize;
327 	}
328 
329 	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
330 	///If no axis is provided, it uses the default axis for this constraint.
331 	virtual	void	setParam(int num, btScalar value, int axis = -1) = 0;
332 
333 	///return the local value of parameter
334 	virtual	btScalar getParam(int num, int axis = -1) const = 0;
335 
336 	virtual	int	calculateSerializeBufferSize() const;
337 
338 	///fills the dataBuffer and returns the struct name (and 0 on failure)
339 	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
340 
341 };
342 
343 // returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
344 // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
btAdjustAngleToLimits(btScalar angleInRadians,btScalar angleLowerLimitInRadians,btScalar angleUpperLimitInRadians)345 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
346 {
347 	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
348 	{
349 		return angleInRadians;
350 	}
351 	else if(angleInRadians < angleLowerLimitInRadians)
352 	{
353 		btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
354 		btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
355 		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
356 	}
357 	else if(angleInRadians > angleUpperLimitInRadians)
358 	{
359 		btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
360 		btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
361 		return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
362 	}
363 	else
364 	{
365 		return angleInRadians;
366 	}
367 }
368 
369 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
370 struct	btTypedConstraintFloatData
371 {
372 	btRigidBodyFloatData		*m_rbA;
373 	btRigidBodyFloatData		*m_rbB;
374 	char	*m_name;
375 
376 	int	m_objectType;
377 	int	m_userConstraintType;
378 	int	m_userConstraintId;
379 	int	m_needsFeedback;
380 
381 	float	m_appliedImpulse;
382 	float	m_dbgDrawSize;
383 
384 	int	m_disableCollisionsBetweenLinkedBodies;
385 	int	m_overrideNumSolverIterations;
386 
387 	float	m_breakingImpulseThreshold;
388 	int		m_isEnabled;
389 
390 };
391 
392 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
393 
394 #define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
395 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
396 ///this structure is not used, except for loading pre-2.82 .bullet files
397 struct	btTypedConstraintData
398 {
399 	btRigidBodyData		*m_rbA;
400 	btRigidBodyData		*m_rbB;
401 	char	*m_name;
402 
403 	int	m_objectType;
404 	int	m_userConstraintType;
405 	int	m_userConstraintId;
406 	int	m_needsFeedback;
407 
408 	float	m_appliedImpulse;
409 	float	m_dbgDrawSize;
410 
411 	int	m_disableCollisionsBetweenLinkedBodies;
412 	int	m_overrideNumSolverIterations;
413 
414 	float	m_breakingImpulseThreshold;
415 	int		m_isEnabled;
416 
417 };
418 #endif //BACKWARDS_COMPATIBLE
419 
420 struct	btTypedConstraintDoubleData
421 {
422 	btRigidBodyDoubleData		*m_rbA;
423 	btRigidBodyDoubleData		*m_rbB;
424 	char	*m_name;
425 
426 	int	m_objectType;
427 	int	m_userConstraintType;
428 	int	m_userConstraintId;
429 	int	m_needsFeedback;
430 
431 	double	m_appliedImpulse;
432 	double	m_dbgDrawSize;
433 
434 	int	m_disableCollisionsBetweenLinkedBodies;
435 	int	m_overrideNumSolverIterations;
436 
437 	double	m_breakingImpulseThreshold;
438 	int		m_isEnabled;
439 	char	padding[4];
440 
441 };
442 
443 
calculateSerializeBufferSize()444 SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
445 {
446 	return sizeof(btTypedConstraintData2);
447 }
448 
449 
450 
451 class btAngularLimit
452 {
453 private:
454 	btScalar
455 		m_center,
456 		m_halfRange,
457 		m_softness,
458 		m_biasFactor,
459 		m_relaxationFactor,
460 		m_correction,
461 		m_sign;
462 
463 	bool
464 		m_solveLimit;
465 
466 public:
467 	/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()468 	btAngularLimit()
469 		:m_center(0.0f),
470 		m_halfRange(-1.0f),
471 		m_softness(0.9f),
472 		m_biasFactor(0.3f),
473 		m_relaxationFactor(1.0f),
474 		m_correction(0.0f),
475 		m_sign(0.0f),
476 		m_solveLimit(false)
477 	{}
478 
479 	/// Sets all limit's parameters.
480 	/// When low > high limit becomes inactive.
481 	/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
482 	void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
483 
484 	/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
485 	/// correction is calculated.
486 	void test(const btScalar angle);
487 
488 	/// Returns limit's softness
getSoftness()489 	inline btScalar getSoftness() const
490 	{
491 		return m_softness;
492 	}
493 
494 	/// Returns limit's bias factor
getBiasFactor()495 	inline btScalar getBiasFactor() const
496 	{
497 		return m_biasFactor;
498 	}
499 
500 	/// Returns limit's relaxation factor
getRelaxationFactor()501 	inline btScalar getRelaxationFactor() const
502 	{
503 		return m_relaxationFactor;
504 	}
505 
506 	/// Returns correction value evaluated when test() was invoked
getCorrection()507 	inline btScalar getCorrection() const
508 	{
509 		return m_correction;
510 	}
511 
512 	/// Returns sign value evaluated when test() was invoked
getSign()513 	inline btScalar getSign() const
514 	{
515 		return m_sign;
516 	}
517 
518 	/// Gives half of the distance between min and max limit angle
getHalfRange()519 	inline btScalar getHalfRange() const
520 	{
521 		return m_halfRange;
522 	}
523 
524 	/// Returns true when the last test() invocation recognized limit violation
isLimit()525 	inline bool isLimit() const
526 	{
527 		return m_solveLimit;
528 	}
529 
530 	/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
531 	/// returned is modified so it equals to the limit closest to given angle.
532 	void fit(btScalar& angle) const;
533 
534 	/// Returns correction value multiplied by sign value
535 	btScalar getError() const;
536 
537 	btScalar getLow() const;
538 
539 	btScalar getHigh() const;
540 
541 };
542 
543 
544 
545 #endif //BT_TYPED_CONSTRAINT_H
546