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