Blender  V3.3
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 
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  }
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; }
302  void setMaxMotorImpulse(btScalar maxMotorImpulse)
303  {
304  m_maxMotorImpulse = maxMotorImpulse;
306  }
308  {
309  m_maxMotorImpulse = maxMotorImpulse;
311  }
312 
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
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
bool m_solveTwistLimit
BT_DECLARE_ALIGNED_ALLOCATOR()
btScalar m_twistSpan
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
const btTransform & getFrameOffsetB() const
btTransform m_rbBFrame
const btTransform & getAFrame() const
virtual void setParam(int num, btScalar value, int axis=-1)
void setDamping(btScalar damping)
bool m_bMotorEnabled
virtual int calculateSerializeBufferSize() const
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
btScalar m_twistLimitSign
btScalar m_accSwingLimitImpulse
btScalar m_accTwistLimitImpulse
btScalar m_swingSpan2
void updateRHS(btScalar timeStep)
btScalar m_swingLimitRatio
bool m_bNormalizedMotorStrength
btScalar getBiasFactor() const
btConeTwistFlags
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
bool m_useSolveConstraintObsolete
bool isMaxMotorImpulseNormalized() const
btScalar m_maxMotorImpulse
int getSolveSwingLimit()
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
btScalar getTwistAngle() const
btScalar getTwistLimitSign()
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
int getFlags() const
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
const btRigidBody & getRigidBodyA() const
#define btConeTwistConstraintData2
btScalar m_twistCorrection
btScalar getLimit(int limitIndex) const
int getSolveTwistLimit()
btScalar m_twistAngle
btScalar getSwingSpan2() const
bool m_solveSwingLimit
btScalar m_twistLimitRatio
btScalar m_kSwing
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btScalar getSwingSpan1() const
btVector3 m_accMotorImpulse
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void enableMotor(bool b)
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
btScalar m_swingCorrection
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
btScalar getRelaxationFactor() const
const btQuaternion & getMotorTarget() const
void setAngularOnly(bool angularOnly)
btQuaternion m_qTarget
btScalar m_limitSoftness
btScalar m_biasFactor
btScalar m_angCFM
void calcAngleInfo()
btScalar getMaxMotorImpulse() const
#define btConeTwistConstraintDataName
bool getAngularOnly() const
btScalar getLimitSoftness() const
btTransform m_rbAFrame
const btRigidBody & getRigidBodyB() const
btScalar m_linCFM
void setFixThresh(btScalar fixThresh)
const btTransform & getBFrame() const
btScalar m_linERP
btScalar getFixThresh()
btVector3 m_twistAxis
btScalar m_fixThresh
btScalar m_swingSpan1
bool isPastSwingLimit()
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
btScalar m_kTwist
bool isMotorEnabled() const
bool m_angularOnly
int m_flags
btScalar getTwistSpan() const
void setLimit(int limitIndex, btScalar limitValue)
const btTransform & getFrameOffsetA() const
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
btVector3 m_swingAxis
btVector3 m_twistAxisA
void setMaxMotorImpulse(btScalar maxMotorImpulse)
btScalar m_damping
btScalar m_relaxationFactor
btScalar getDamping() const
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void getInfo1NonVirtual(btConstraintInfo1 *info)
void setMotorTarget(const btQuaternion &q)
btFixedConstraint btRigidBody & rbB
btJacobianEntry
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
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:285
#define SIMD_FORCE_INLINE
Definition: btScalar.h:280
#define btAssert(x)
Definition: btScalar.h:295
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btRigidBody & m_rbA
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btRigidBody & m_rbB
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
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
for serialization
Definition: btTransform.h:245
this structure is not used, except for loading pre-2.82 .bullet files