Bullet Collision Detection & Physics Library
btMultiBodySphericalJointLimit.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
17
19#include "btMultiBody.h"
25
31 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
32 m_desiredVelocity(0, 0, 0),
33 m_desiredPosition(0,0,0,1),
34 m_use_multi_dof_params(false),
35 m_kd(1., 1., 1.),
36 m_kp(0.2, 0.2, 0.2),
37 m_erp(1),
38 m_rhsClamp(SIMD_INFINITY),
39 m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
40 m_pivotA(m_bodyA->getLink(link).m_eVector),
41 m_pivotB(m_bodyB->getLink(link).m_eVector),
42 m_swingxRange(swingxRange),
43 m_swingyRange(swingyRange),
44 m_twistRange(twistRange)
45
46{
47
49}
50
51
53{
55 // note: we rely on the fact that data.m_jacobians are
56 // always initialized to zero by the Constraint ctor
57 int linkDoF = 0;
58 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
59
60 // row 0: the lower bound
61 // row 0: the lower bound
62 jacobianA(0)[offset] = 1;
63
64 jacobianB(1)[offset] = -1;
65
67}
68
69
71{
72}
73
75{
76 if (this->m_linkA < 0)
77 {
79 if (col)
80 return col->getIslandTag();
81 }
82 else
83 {
85 {
87 }
88 }
89 return -1;
90}
91
93{
94 if (m_linkB < 0)
95 {
97 if (col)
98 return col->getIslandTag();
99 }
100 else
101 {
103 {
105 }
106 }
107 return -1;
108}
109
113{
114 // only positions need to be updated -- data.m_jacobians and force
115 // directions were set in the ctor and never change.
116
118 {
120 }
121
122 //don't crash
124 return;
125
126
127 if (m_maxAppliedImpulse == 0.f)
128 return;
129
130 const btScalar posError = 0;
131 const btVector3 zero(0, 0, 0);
132
133
134 btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
135
140
142 btVector3 vTwist(0,0,1);
143
145 vConeNoTwist.normalize();
147 qABCone.normalize();
151
152
156
158
162
163 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
164 {
165 qMinTwist = -(qABTwist);
166 twistAngle = qMinTwist.getAngle();
167 }
170 vTwistAxis.normalize();
171
172 if (vTwistAxis.dot(vTwist)<0)
173 twistAngle*=-1.;
174
176
177
178 for (int row = 0; row < getNumRows(); row++)
179 {
181 btScalar damp = 1;
183 {
184 angleDiff[row]=0;
185 damp=0;
186
187 } else
188 {
189 if (angleDiff[row]>allowed)
190 {
192
193 }
194 if (angleDiff[row]<-allowed)
195 {
197
198 }
199 }
200
201
202 int dof = row;
203
206
207 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
209
213 btScalar posError = 0;
214 {
217 {
219 {
221 double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
222 posError = kp*angleDiff[row % 3];
224 //should multiply by time step
225 //max_applied_impulse *= infoGlobal.m_timeStep
226
228
229
230 if (posError>0)
232 else
234
236 {
239 zero, zero, zero,//pure angular, so zero out linear parts
240 posError,
243 1.0, false, 0, 0,
244 damp);
245 constraintRow.m_orgConstraint = this;
246 constraintRow.m_orgDofIndex = row;
247 }
248 break;
249 }
250 default:
251 {
252 btAssert(0);
253 }
254 };
255 }
256 }
257}
258
259
261{
263 tr.setIdentity();
264 if (m_bodyB)
265 {
267 tr.setOrigin(pivotBworld);
268 drawer->drawTransform(tr, 0.1);
269 }
270}
static btVector3 vTwist(1, 0, 0)
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
@ MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
#define SIMD_PI
Definition btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btScalar btFabs(btScalar x)
Definition btScalar.h:497
#define SIMD_INFINITY
Definition btScalar.h:544
#define SIMD_EPSILON
Definition btScalar.h:543
#define btAssert(x)
Definition btScalar.h:153
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
void setIdentity()
Set the matrix to the identity.
btScalar * jacobianA(int row)
btScalar * jacobianB(int row)
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0, btScalar damping=1.0)
virtual void debugDraw(class btIDebugDraw *drawer)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMultiBodySphericalJointLimit(btMultiBody *body, int link, btScalar swingxRange, btScalar swingyRange, btScalar twistRange, btScalar maxAppliedImpulse)
This file was written by Erwin Coumans.
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
btScalar * getJointPosMultiDof(int i)
const btMultibodyLink & getLink(int index) const
const btMultiBodyLinkCollider * getBaseCollider() const
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
btScalar * getJointVelMultiDof(int i)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
btQuaternion inverse() const
Return the inverse of this quaternion.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
void setIdentity()
Set this transformation to the identity.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...