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 
25  //:btMultiBodyConstraint(body,0,link,-1,2,true),
26  :btMultiBodyConstraint(body,body,link,link,2,true),
27  m_lowerBound(lower),
28  m_upperBound(upper)
29 {
30  // the data.m_jacobians never change, so may as well
31  // initialize them here
32 
33  // note: we rely on the fact that data.m_jacobians are
34  // always initialized to zero by the Constraint ctor
35 
36  unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
37 
38  // row 0: the lower bound
39  jacobianA(0)[offset] = 1;
40  // row 1: the upper bound
41  //jacobianA(1)[offset] = -1;
42 
43  jacobianB(1)[offset] = -1;
44 }
46 {
47 }
48 
50 {
51  if(m_bodyA)
52  {
54  if (col)
55  return col->getIslandTag();
56  for (int i=0;i<m_bodyA->getNumLinks();i++)
57  {
58  if (m_bodyA->getLink(i).m_collider)
59  return m_bodyA->getLink(i).m_collider->getIslandTag();
60  }
61  }
62  return -1;
63 }
64 
66 {
67  if(m_bodyB)
68  {
70  if (col)
71  return col->getIslandTag();
72 
73  for (int i=0;i<m_bodyB->getNumLinks();i++)
74  {
75  col = m_bodyB->getLink(i).m_collider;
76  if (col)
77  return col->getIslandTag();
78  }
79  }
80  return -1;
81 }
82 
83 
86  const btContactSolverInfo& infoGlobal)
87 {
88  // only positions need to be updated -- data.m_jacobians and force
89  // directions were set in the ctor and never change.
90 
91  // row 0: the lower bound
92  setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
93 
94  // row 1: the upper bound
96 
97  for (int row=0;row<getNumRows();row++)
98  {
99  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
100  constraintRow.m_multiBodyA = m_bodyA;
101  constraintRow.m_multiBodyB = m_bodyB;
102  const btScalar posError = 0; //why assume it's zero?
103  const btVector3 dummy(0, 0, 0);
104 
105  btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
106  {
107  btScalar penetration = getPosition(row);
108  btScalar positionalError = 0.f;
109  btScalar velocityError = - rel_vel;// * damping;
110  btScalar erp = infoGlobal.m_erp2;
111  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
112  {
113  erp = infoGlobal.m_erp;
114  }
115  if (penetration>0)
116  {
117  positionalError = 0;
118  velocityError = -penetration / infoGlobal.m_timeStep;
119  } else
120  {
121  positionalError = -penetration * erp/infoGlobal.m_timeStep;
122  }
123 
124  btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
125  btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
126  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
127  {
128  //combine position and velocity into rhs
129  constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
130  constraintRow.m_rhsPenetration = 0.f;
131 
132  } else
133  {
134  //split position and velocity into rhs and m_rhsPenetration
135  constraintRow.m_rhs = velocityImpulse;
136  constraintRow.m_rhsPenetration = penetrationImpulse;
137  }
138  }
139  }
140 
141 }
142 
143 
144 
145 
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &contactNormalOnB, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar * jacobianB(int row)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
int getNumLinks() const
Definition: btMultiBody.h:145
btScalar * jacobianA(int row)
btScalar getJointPos(int i) const
btScalar getPosition(int row) const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
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)
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
bool isMultiDof()
Definition: btMultiBody.h:522
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278