Bullet
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->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
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 
114  btScalar direction = row? -1 : 1;
115 
116  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
117  constraintRow.m_orgConstraint = this;
118  constraintRow.m_orgDofIndex = row;
119 
120  constraintRow.m_multiBodyA = m_bodyA;
121  constraintRow.m_multiBodyB = m_bodyB;
122  const btScalar posError = 0; //why assume it's zero?
123  const btVector3 dummy(0, 0, 0);
124 
125  btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
126 
127  if (m_bodyA->isMultiDof())
128  {
129  //expect either prismatic or revolute joint type for now
132  {
134  {
135  constraintRow.m_contactNormal1.setZero();
136  constraintRow.m_contactNormal2.setZero();
138  constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
139  constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
140 
141  break;
142  }
144  {
146  constraintRow.m_contactNormal1=prismaticAxisInWorld;
147  constraintRow.m_contactNormal2=-prismaticAxisInWorld;
148  constraintRow.m_relpos1CrossNormal.setZero();
149  constraintRow.m_relpos2CrossNormal.setZero();
150 
151  break;
152  }
153  default:
154  {
155  btAssert(0);
156  }
157  };
158 
159  }
160 
161  {
162  btScalar penetration = getPosition(row);
163  btScalar positionalError = 0.f;
164  btScalar velocityError = - rel_vel;// * damping;
165  btScalar erp = infoGlobal.m_erp2;
166  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
167  {
168  erp = infoGlobal.m_erp;
169  }
170  if (penetration>0)
171  {
172  positionalError = 0;
173  velocityError = -penetration / infoGlobal.m_timeStep;
174  } else
175  {
176  positionalError = -penetration * erp/infoGlobal.m_timeStep;
177  }
178 
179  btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
180  btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
181  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
182  {
183  //combine position and velocity into rhs
184  constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
185  constraintRow.m_rhsPenetration = 0.f;
186 
187  } else
188  {
189  //split position and velocity into rhs and m_rhsPenetration
190  constraintRow.m_rhs = velocityImpulse;
191  constraintRow.m_rhsPenetration = penetrationImpulse;
192  }
193  }
194  }
195 
196 }
197 
198 
199 
200 
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:118
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
int getNumLinks() const
Definition: btMultiBody.h:154
btMultiBodyConstraint * m_orgConstraint
#define btAssert(x)
Definition: btScalar.h:113
btScalar * jacobianA(int row)
btScalar getJointPos(int i) const
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
btScalar getPosition(int row) const
void setZero()
Definition: btVector3.h:671
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:133
bool isMultiDof()
Definition: btMultiBody.h:579
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278