Bullet Collision Detection & Physics Library
btMultiBodyGearConstraint.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 btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
24  :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
25  m_gearRatio(1),
26  m_gearAuxLink(-1),
27  m_erp(0),
28  m_relativePositionTarget(0)
29 {
30 
31 }
32 
34 {
35 
37 
39 }
40 
42 {
43 }
44 
45 
47 {
48 
49  if (m_bodyA)
50  {
52  if (col)
53  return col->getIslandTag();
54  for (int i=0;i<m_bodyA->getNumLinks();i++)
55  {
56  if (m_bodyA->getLink(i).m_collider)
57  return m_bodyA->getLink(i).m_collider->getIslandTag();
58  }
59  }
60  return -1;
61 }
62 
64 {
65  if (m_bodyB)
66  {
68  if (col)
69  return col->getIslandTag();
70 
71  for (int i=0;i<m_bodyB->getNumLinks();i++)
72  {
73  col = m_bodyB->getLink(i).m_collider;
74  if (col)
75  return col->getIslandTag();
76  }
77  }
78  return -1;
79 }
80 
81 
84  const btContactSolverInfo& infoGlobal)
85 {
86  // only positions need to be updated -- data.m_jacobians and force
87  // directions were set in the ctor and never change.
88 
90  {
92  }
93 
94  //don't crash
96  return;
97 
98 
99  if (m_maxAppliedImpulse==0.f)
100  return;
101 
102  // note: we rely on the fact that data.m_jacobians are
103  // always initialized to zero by the Constraint ctor
104  int linkDoF = 0;
105  unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
106  unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
107 
108  // row 0: the lower bound
109  jacobianA(0)[offsetA] = 1;
110  jacobianB(0)[offsetB] = m_gearRatio;
111 
112  btScalar posError = 0;
113  const btVector3 dummy(0, 0, 0);
114 
115  btScalar kp = 1;
116  btScalar kd = 1;
117  int numRows = getNumRows();
118 
119  for (int row=0;row<numRows;row++)
120  {
121  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
122 
123 
124  int dof = 0;
125  btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
126  btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
127  btScalar auxVel = 0;
128 
129  if (m_gearAuxLink>=0)
130  {
131  auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
132  }
133  currentVelocity += auxVel;
134  if (m_erp!=0)
135  {
136  btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
137  btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
138  btScalar diff = currentPositionB+currentPositionA;
139  btScalar desiredPositionDiff = this->m_relativePositionTarget;
140  posError = -m_erp*(desiredPositionDiff - diff);
141  }
142 
143  btScalar desiredRelativeVelocity = auxVel;
144 
145  fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
146 
147  constraintRow.m_orgConstraint = this;
148  constraintRow.m_orgDofIndex = row;
149  {
150  //expect either prismatic or revolute joint type for now
153  {
155  {
156  constraintRow.m_contactNormal1.setZero();
157  constraintRow.m_contactNormal2.setZero();
159  constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
160  constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
161 
162  break;
163  }
165  {
167  constraintRow.m_contactNormal1=prismaticAxisInWorld;
168  constraintRow.m_contactNormal2=-prismaticAxisInWorld;
169  constraintRow.m_relpos1CrossNormal.setZero();
170  constraintRow.m_relpos2CrossNormal.setZero();
171  break;
172  }
173  default:
174  {
175  btAssert(0);
176  }
177  };
178 
179  }
180 
181  }
182 
183 }
184 
btScalar * jacobianB(int row)
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 * getJointVelMultiDof(int i)
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
void setZero()
Definition: btVector3.h:683
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
int getIslandTag() const
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
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)
btMultiBodyGearConstraint(btMultiBody *bodyA, int linkA, btMultiBody *bodyB, int linkB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
This file was written by Erwin Coumans.
btScalar * getJointPosMultiDof(int i)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292