Bullet Collision Detection & Physics Library
btConeTwistConstraint.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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 Written by: Marcus Hennix
16 */
17 
18 /*
19 Overview:
20 
21 btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
22 It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
23 It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
24 Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
25 (Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
26 
27 In the contraint's frame of reference:
28 twist is along the x-axis,
29 and swing 1 and 2 are along the z and y axes respectively.
30 */
31 
32 #ifndef BT_CONETWISTCONSTRAINT_H
33 #define BT_CONETWISTCONSTRAINT_H
34 
35 #include "LinearMath/btVector3.h"
36 #include "btJacobianEntry.h"
37 #include "btTypedConstraint.h"
38 
39 #ifdef BT_USE_DOUBLE_PRECISION
40 #define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
41 #define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
42 #else
43 #define btConeTwistConstraintData2 btConeTwistConstraintData
44 #define btConeTwistConstraintDataName "btConeTwistConstraintData"
45 #endif //BT_USE_DOUBLE_PRECISION
46 
47 class btRigidBody;
48 
50 {
54 };
55 
59 {
60 #ifdef IN_PARALLELL_SOLVER
61 public:
62 #endif
63  btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
64 
67 
71 
73 
77 
79 
82 
85 
89 
91 
94 
98 
100 
101  // not yet used...
105 
106  // motor
112 
113  // parameters
114  int m_flags;
118 
119 protected:
120  void init();
121 
122  void computeConeLimitInfo(const btQuaternion& qCone, // in
123  btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
124 
125  void computeTwistLimitInfo(const btQuaternion& qTwist, // in
126  btScalar& twistAngle, btVector3& vTwistAxis); // all outs
127 
128  void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
129 
130 public:
132 
133  btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
134 
135  btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
136 
137  virtual void buildJacobian();
138 
139  virtual void getInfo1(btConstraintInfo1 * info);
140 
141  void getInfo1NonVirtual(btConstraintInfo1 * info);
142 
143  virtual void getInfo2(btConstraintInfo2 * info);
144 
145  void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
146 
147  virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
148 
149  void updateRHS(btScalar timeStep);
150 
151  const btRigidBody& getRigidBodyA() const
152  {
153  return m_rbA;
154  }
155  const btRigidBody& getRigidBodyB() const
156  {
157  return m_rbB;
158  }
159 
160  void setAngularOnly(bool angularOnly)
161  {
162  m_angularOnly = angularOnly;
163  }
164 
165  bool getAngularOnly() const
166  {
167  return m_angularOnly;
168  }
169 
170  void setLimit(int limitIndex, btScalar limitValue)
171  {
172  switch (limitIndex)
173  {
174  case 3:
175  {
176  m_twistSpan = limitValue;
177  break;
178  }
179  case 4:
180  {
181  m_swingSpan2 = limitValue;
182  break;
183  }
184  case 5:
185  {
186  m_swingSpan1 = limitValue;
187  break;
188  }
189  default:
190  {
191  }
192  };
193  }
194 
195  btScalar getLimit(int limitIndex) const
196  {
197  switch (limitIndex)
198  {
199  case 3:
200  {
201  return m_twistSpan;
202  break;
203  }
204  case 4:
205  {
206  return m_swingSpan2;
207  break;
208  }
209  case 5:
210  {
211  return m_swingSpan1;
212  break;
213  }
214  default:
215  {
216  btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
217  return 0.0;
218  }
219  };
220  }
221 
222  // setLimit(), a few notes:
223  // _softness:
224  // 0->1, recommend ~0.8->1.
225  // describes % of limits where movement is free.
226  // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
227  // _biasFactor:
228  // 0->1?, recommend 0.3 +/-0.3 or so.
229  // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
230  // __relaxationFactor:
231  // 0->1, recommend to stay near 1.
232  // the lower the value, the less the constraint will fight velocities which violate the angular limits.
233  void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
234  {
235  m_swingSpan1 = _swingSpan1;
236  m_swingSpan2 = _swingSpan2;
237  m_twistSpan = _twistSpan;
238 
239  m_limitSoftness = _softness;
240  m_biasFactor = _biasFactor;
241  m_relaxationFactor = _relaxationFactor;
242  }
243 
244  const btTransform& getAFrame() const { return m_rbAFrame; };
245  const btTransform& getBFrame() const { return m_rbBFrame; };
246 
247  inline int getSolveTwistLimit()
248  {
249  return m_solveTwistLimit;
250  }
251 
252  inline int getSolveSwingLimit()
253  {
254  return m_solveSwingLimit;
255  }
256 
258  {
259  return m_twistLimitSign;
260  }
261 
262  void calcAngleInfo();
263  void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
264 
265  inline btScalar getSwingSpan1() const
266  {
267  return m_swingSpan1;
268  }
269  inline btScalar getSwingSpan2() const
270  {
271  return m_swingSpan2;
272  }
273  inline btScalar getTwistSpan() const
274  {
275  return m_twistSpan;
276  }
277  inline btScalar getLimitSoftness() const
278  {
279  return m_limitSoftness;
280  }
281  inline btScalar getBiasFactor() const
282  {
283  return m_biasFactor;
284  }
286  {
287  return m_relaxationFactor;
288  }
289  inline btScalar getTwistAngle() const
290  {
291  return m_twistAngle;
292  }
293  bool isPastSwingLimit() { return m_solveSwingLimit; }
294 
295  btScalar getDamping() const { return m_damping; }
296  void setDamping(btScalar damping) { m_damping = damping; }
297 
298  void enableMotor(bool b) { m_bMotorEnabled = b; }
299  bool isMotorEnabled() const { return m_bMotorEnabled; }
300  btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
301  bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
302  void setMaxMotorImpulse(btScalar maxMotorImpulse)
303  {
304  m_maxMotorImpulse = maxMotorImpulse;
305  m_bNormalizedMotorStrength = false;
306  }
308  {
309  m_maxMotorImpulse = maxMotorImpulse;
310  m_bNormalizedMotorStrength = true;
311  }
312 
313  btScalar getFixThresh() { return m_fixThresh; }
314  void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
315 
316  // setMotorTarget:
317  // q: the desired rotation of bodyA wrt bodyB.
318  // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
319  // note: don't forget to enableMotor()
320  void setMotorTarget(const btQuaternion& q);
321  const btQuaternion& getMotorTarget() const { return m_qTarget; }
322 
323  // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
324  void setMotorTargetInConstraintSpace(const btQuaternion& q);
325 
326  btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
327 
330  virtual void setParam(int num, btScalar value, int axis = -1);
331 
332  virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
333 
335  {
336  return m_rbAFrame;
337  }
338 
340  {
341  return m_rbBFrame;
342  }
343 
345  virtual btScalar getParam(int num, int axis = -1) const;
346 
347  int getFlags() const
348  {
349  return m_flags;
350  }
351 
352  virtual int calculateSerializeBufferSize() const;
353 
355  virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
356 };
357 
359 {
363 
364  //limits
365  double m_swingSpan1;
366  double m_swingSpan2;
367  double m_twistSpan;
369  double m_biasFactor;
371 
372  double m_damping;
373 };
374 
375 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
378 {
382 
383  //limits
386  float m_twistSpan;
390 
391  float m_damping;
392 
393  char m_pad[4];
394 };
395 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
396 //
397 
399 {
400  return sizeof(btConeTwistConstraintData2);
401 }
402 
404 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
405 {
407  btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
408 
409  m_rbAFrame.serialize(cone->m_rbAFrame);
410  m_rbBFrame.serialize(cone->m_rbBFrame);
411 
412  cone->m_swingSpan1 = m_swingSpan1;
413  cone->m_swingSpan2 = m_swingSpan2;
414  cone->m_twistSpan = m_twistSpan;
415  cone->m_limitSoftness = m_limitSoftness;
416  cone->m_biasFactor = m_biasFactor;
417  cone->m_relaxationFactor = m_relaxationFactor;
418  cone->m_damping = m_damping;
419 
421 }
422 
423 #endif //BT_CONETWISTCONSTRAINT_H
btConeTwistFlags
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
#define btConeTwistConstraintData2
#define btConeTwistConstraintDataName
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
#define btAssert(x)
Definition: btScalar.h:153
btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
bool isMaxMotorImpulseNormalized() const
btScalar getLimit(int limitIndex) const
btScalar getSwingSpan1() const
const btTransform & getFrameOffsetA() const
const btTransform & getFrameOffsetB() const
btScalar getBiasFactor() const
void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness=1.f, btScalar _biasFactor=0.3f, btScalar _relaxationFactor=1.0f)
const btQuaternion & getMotorTarget() const
void setAngularOnly(bool angularOnly)
virtual int calculateSerializeBufferSize() const
btScalar getTwistAngle() const
void setLimit(int limitIndex, btScalar limitValue)
void setMaxMotorImpulse(btScalar maxMotorImpulse)
const btTransform & getAFrame() const
const btRigidBody & getRigidBodyB() const
void setFixThresh(btScalar fixThresh)
btScalar getLimitSoftness() const
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
btScalar getSwingSpan2() const
const btRigidBody & getRigidBodyA() const
void setDamping(btScalar damping)
btScalar getRelaxationFactor() const
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btScalar getMaxMotorImpulse() const
const btTransform & getBFrame() const
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
virtual int calculateSerializeBufferSize() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
void serialize(struct btTransformData &dataOut) const
Definition: btTransform.h:256
TypedConstraint is the baseclass for Bullet constraints and vehicles.
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
this structure is not used, except for loading pre-2.82 .bullet files
btTypedConstraintData m_typeConstraintData
btTransformFloatData m_rbBFrame
btTransformFloatData m_rbAFrame
btTypedConstraintDoubleData m_typeConstraintData
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
for serialization
Definition: btTransform.h:245
this structure is not used, except for loading pre-2.82 .bullet files