Bullet Collision Detection & Physics Library
btMultiBodyJointLimitConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 
17 
19 #include "btMultiBody.h"
22 
24  //:btMultiBodyConstraint(body,0,link,-1,2,true),
25  : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true),
26  m_lowerBound(lower),
27  m_upperBound(upper)
28 {
29 }
30 
32 {
33  // the data.m_jacobians never change, so may as well
34  // initialize them here
35 
37 
38  unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
39 
40  // row 0: the lower bound
41  jacobianA(0)[offset] = 1;
42  // row 1: the upper bound
43  //jacobianA(1)[offset] = -1;
44  jacobianB(1)[offset] = -1;
45 
47 }
48 
50 {
51 }
52 
54 {
55  if (m_bodyA)
56  {
57  if (m_linkA < 0)
58  {
60  if (col)
61  return col->getIslandTag();
62  }
63  else
64  {
67  }
68  }
69  return -1;
70 }
71 
73 {
74  if (m_bodyB)
75  {
76  if (m_linkB < 0)
77  {
79  if (col)
80  return col->getIslandTag();
81  }
82  else
83  {
86  }
87  }
88  return -1;
89 }
90 
93  const btContactSolverInfo& infoGlobal)
94 {
95  // only positions need to be updated -- data.m_jacobians and force
96  // directions were set in the ctor and never change.
97 
99  {
101  }
102 
103  // row 0: the lower bound
104  setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
105 
106  // row 1: the upper bound
108 
109  for (int row = 0; row < getNumRows(); row++)
110  {
111  btScalar penetration = getPosition(row);
112 
113  //todo: consider adding some safety threshold here
114  if (penetration > 0)
115  {
116  continue;
117  }
118  btScalar direction = row ? -1 : 1;
119 
120  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
121  constraintRow.m_orgConstraint = this;
122  constraintRow.m_orgDofIndex = row;
123 
124  constraintRow.m_multiBodyA = m_bodyA;
125  constraintRow.m_multiBodyB = m_bodyB;
126  const btScalar posError = 0; //why assume it's zero?
127  const btVector3 dummy(0, 0, 0);
128 
129  btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
130 
131  {
132  //expect either prismatic or revolute joint type for now
135  {
137  {
138  constraintRow.m_contactNormal1.setZero();
139  constraintRow.m_contactNormal2.setZero();
141  constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
142  constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
143 
144  break;
145  }
147  {
149  constraintRow.m_contactNormal1 = prismaticAxisInWorld;
150  constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
151  constraintRow.m_relpos1CrossNormal.setZero();
152  constraintRow.m_relpos2CrossNormal.setZero();
153 
154  break;
155  }
156  default:
157  {
158  btAssert(0);
159  }
160  };
161  }
162 
163  {
164  btScalar positionalError = 0.f;
165  btScalar velocityError = -rel_vel; // * damping;
166  btScalar erp = infoGlobal.m_erp2;
167  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
168  {
169  erp = infoGlobal.m_erp;
170  }
171  if (penetration > 0)
172  {
173  positionalError = 0;
174  velocityError = -penetration / infoGlobal.m_timeStep;
175  }
176  else
177  {
178  positionalError = -penetration * erp / infoGlobal.m_timeStep;
179  }
180 
181  btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
182  btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
183  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
184  {
185  //combine position and velocity into rhs
186  constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
187  constraintRow.m_rhsPenetration = 0.f;
188  }
189  else
190  {
191  //split position and velocity into rhs and m_rhsPenetration
192  constraintRow.m_rhs = velocityImpulse;
193  constraintRow.m_rhsPenetration = penetrationImpulse;
194  }
195  }
196  }
197 }
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyJointLimitConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointLimitConstraint.cpp:72
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:55
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:72
btMultiBodyJointLimitConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointLimitConstraint.cpp:53
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:60
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:104
btMultiBodyJointLimitConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyJointLimitConstraint.cpp:31
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btMultiBodyConstraint::setPosition
void setPosition(int row, btScalar pos)
Definition: btMultiBodyConstraint.h:147
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:56
btMultiBodyConstraint::fillMultiBodyConstraint
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)
Definition: btMultiBodyConstraint.cpp:52
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btMultiBodyConstraint::getPosition
btScalar getPosition(int row) const
Definition: btMultiBodyConstraint.h:142
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:54
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBodyJointLimitConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointLimitConstraint.cpp:91
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btMultiBody
Definition: btMultiBody.h:49
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:436
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:55
btMultiBodyJointLimitConstraint.h
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:34
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:160
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition: btMultiBody.cpp:372
btMultiBody.h
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:54
btMultiBodyJointLimitConstraint::m_upperBound
btScalar m_upperBound
Definition: btMultiBodyJointLimitConstraint.h:26
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btMultiBodyJointLimitConstraint::m_lowerBound
btScalar m_lowerBound
Definition: btMultiBodyJointLimitConstraint.h:25
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:55
btCollisionObject.h
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:51
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:168
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint
virtual ~btMultiBodyJointLimitConstraint()
Definition: btMultiBodyJointLimitConstraint.cpp:49
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointLimitConstraint.cpp:23
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926