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,body,link,link,2,true),
26  m_lowerBound(lower),
27  m_upperBound(upper)
28 {
29  // the data.m_jacobians never change, so may as well
30  // initialize them here
31 
32  // note: we rely on the fact that data.m_jacobians are
33  // always initialized to zero by the Constraint ctor
34 
35  // row 0: the lower bound
36  jacobianA(0)[6 + link] = 1;
37 
38  // row 1: the upper bound
39  jacobianB(1)[6 + link] = -1;
40 }
42 {
43 }
44 
46 {
48  if (col)
49  return col->getIslandTag();
50  for (int i=0;i<m_bodyA->getNumLinks();i++)
51  {
52  if (m_bodyA->getLink(i).m_collider)
53  return m_bodyA->getLink(i).m_collider->getIslandTag();
54  }
55  return -1;
56 }
57 
59 {
61  if (col)
62  return col->getIslandTag();
63 
64  for (int i=0;i<m_bodyB->getNumLinks();i++)
65  {
66  col = m_bodyB->getLink(i).m_collider;
67  if (col)
68  return col->getIslandTag();
69  }
70  return -1;
71 }
72 
73 
76  const btContactSolverInfo& infoGlobal)
77 {
78  // only positions need to be updated -- data.m_jacobians and force
79  // directions were set in the ctor and never change.
80 
81  // row 0: the lower bound
83 
84  // row 1: the upper bound
86 
87  for (int row=0;row<getNumRows();row++)
88  {
89  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
90  constraintRow.m_multiBodyA = m_bodyA;
91  constraintRow.m_multiBodyB = m_bodyB;
92 
93  btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
94  {
95  btScalar penetration = getPosition(row);
96  btScalar positionalError = 0.f;
97  btScalar velocityError = - rel_vel;// * damping;
98  btScalar erp = infoGlobal.m_erp2;
99  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
100  {
101  erp = infoGlobal.m_erp;
102  }
103  if (penetration>0)
104  {
105  positionalError = 0;
106  velocityError = -penetration / infoGlobal.m_timeStep;
107  } else
108  {
109  positionalError = -penetration * erp/infoGlobal.m_timeStep;
110  }
111 
112  btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
113  btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
114  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
115  {
116  //combine position and velocity into rhs
117  constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
118  constraintRow.m_rhsPenetration = 0.f;
119 
120  } else
121  {
122  //split position and velocity into rhs and m_rhsPenetration
123  constraintRow.m_rhs = velocityImpulse;
124  constraintRow.m_rhsPenetration = penetrationImpulse;
125  }
126  }
127  }
128 
129 }
130 
131 
132 
133 
btScalar * jacobianB(int row)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:76
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:112
btScalar * jacobianA(int row)
btScalar getJointPos(int i) const
btScalar getPosition(int row) const
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint &constraintRow, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity, btScalar lowerLimit, btScalar upperLimit)
int getIslandTag() const
void setPosition(int row, btScalar pos)
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:91
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:266