2
Bullet Continuous Collision Detection and Physics Library
3
btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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:
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.
15
Written by: Marcus Hennix
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.)
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.
36
#ifndef BT_CONETWISTCONSTRAINT_H
37
#define BT_CONETWISTCONSTRAINT_H
39
#include "LinearMath/btVector3.h"
40
#include "btJacobianEntry.h"
41
#include "btTypedConstraint.h"
47
BT_CONETWIST_FLAGS_LIN_CFM = 1,
48
BT_CONETWIST_FLAGS_LIN_ERP = 2,
49
BT_CONETWIST_FLAGS_ANG_CFM = 4
52
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
53
class btConeTwistConstraint : public btTypedConstraint
55
#ifdef IN_PARALLELL_SOLVER
58
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
60
btTransform m_rbAFrame;
61
btTransform m_rbBFrame;
63
btScalar m_limitSoftness;
64
btScalar m_biasFactor;
65
btScalar m_relaxationFactor;
69
btScalar m_swingSpan1;
70
btScalar m_swingSpan2;
75
btVector3 m_swingAxis;
76
btVector3 m_twistAxis;
81
btScalar m_twistLimitSign;
82
btScalar m_swingCorrection;
83
btScalar m_twistCorrection;
85
btScalar m_twistAngle;
87
btScalar m_accSwingLimitImpulse;
88
btScalar m_accTwistLimitImpulse;
91
bool m_solveTwistLimit;
92
bool m_solveSwingLimit;
94
bool m_useSolveConstraintObsolete;
97
btScalar m_swingLimitRatio;
98
btScalar m_twistLimitRatio;
99
btVector3 m_twistAxisA;
102
bool m_bMotorEnabled;
103
bool m_bNormalizedMotorStrength;
104
btQuaternion m_qTarget;
105
btScalar m_maxMotorImpulse;
106
btVector3 m_accMotorImpulse;
118
void computeConeLimitInfo(const btQuaternion& qCone, // in
119
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
121
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
122
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
124
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
129
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
131
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
133
virtual void buildJacobian();
135
virtual void getInfo1 (btConstraintInfo1* info);
137
void getInfo1NonVirtual(btConstraintInfo1* info);
139
virtual void getInfo2 (btConstraintInfo2* info);
141
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
143
virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep);
145
void updateRHS(btScalar timeStep);
148
const btRigidBody& getRigidBodyA() const
152
const btRigidBody& getRigidBodyB() const
157
void setAngularOnly(bool angularOnly)
159
m_angularOnly = angularOnly;
162
void setLimit(int limitIndex,btScalar limitValue)
168
m_twistSpan = limitValue;
173
m_swingSpan2 = limitValue;
178
m_swingSpan1 = limitValue;
187
// setLimit(), a few notes:
189
// 0->1, recommend ~0.8->1.
190
// describes % of limits where movement is free.
191
// beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
193
// 0->1?, recommend 0.3 +/-0.3 or so.
194
// strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
195
// __relaxationFactor:
196
// 0->1, recommend to stay near 1.
197
// the lower the value, the less the constraint will fight velocities which violate the angular limits.
198
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
200
m_swingSpan1 = _swingSpan1;
201
m_swingSpan2 = _swingSpan2;
202
m_twistSpan = _twistSpan;
204
m_limitSoftness = _softness;
205
m_biasFactor = _biasFactor;
206
m_relaxationFactor = _relaxationFactor;
209
const btTransform& getAFrame() { return m_rbAFrame; };
210
const btTransform& getBFrame() { return m_rbBFrame; };
212
inline int getSolveTwistLimit()
214
return m_solveTwistLimit;
217
inline int getSolveSwingLimit()
219
return m_solveTwistLimit;
222
inline btScalar getTwistLimitSign()
224
return m_twistLimitSign;
227
void calcAngleInfo();
228
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
230
inline btScalar getSwingSpan1()
234
inline btScalar getSwingSpan2()
238
inline btScalar getTwistSpan()
242
inline btScalar getTwistAngle()
246
bool isPastSwingLimit() { return m_solveSwingLimit; }
248
void setDamping(btScalar damping) { m_damping = damping; }
250
void enableMotor(bool b) { m_bMotorEnabled = b; }
251
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
252
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
254
btScalar getFixThresh() { return m_fixThresh; }
255
void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
258
// q: the desired rotation of bodyA wrt bodyB.
259
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
260
// note: don't forget to enableMotor()
261
void setMotorTarget(const btQuaternion &q);
263
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
264
void setMotorTargetInConstraintSpace(const btQuaternion &q);
266
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
268
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
269
///If no axis is provided, it uses the default axis for this constraint.
270
virtual void setParam(int num, btScalar value, int axis = -1);
272
virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
274
const btTransform& getFrameOffsetA() const
279
const btTransform& getFrameOffsetB() const
285
///return the local value of parameter
286
virtual btScalar getParam(int num, int axis = -1) const;
288
virtual int calculateSerializeBufferSize() const;
290
///fills the dataBuffer and returns the struct name (and 0 on failure)
291
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
295
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
296
struct btConeTwistConstraintData
298
btTypedConstraintData m_typeConstraintData;
299
btTransformFloatData m_rbAFrame;
300
btTransformFloatData m_rbBFrame;
306
float m_limitSoftness;
308
float m_relaxationFactor;
318
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
320
return sizeof(btConeTwistConstraintData);
325
///fills the dataBuffer and returns the struct name (and 0 on failure)
326
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
328
btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
329
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
331
m_rbAFrame.serializeFloat(cone->m_rbAFrame);
332
m_rbBFrame.serializeFloat(cone->m_rbBFrame);
334
cone->m_swingSpan1 = float(m_swingSpan1);
335
cone->m_swingSpan2 = float(m_swingSpan2);
336
cone->m_twistSpan = float(m_twistSpan);
337
cone->m_limitSoftness = float(m_limitSoftness);
338
cone->m_biasFactor = float(m_biasFactor);
339
cone->m_relaxationFactor = float(m_relaxationFactor);
340
cone->m_damping = float(m_damping);
342
return "btConeTwistConstraintData";
346
#endif //BT_CONETWISTCONSTRAINT_H