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