Blender  V3.3
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, MULTIBODY_CONSTRAINT_GEAR),
25  m_gearRatio(1),
26  m_gearAuxLink(-1),
27  m_erp(0),
28  m_relativePositionTarget(0)
29 {
30 }
31 
33 {
35 
37 }
38 
40 {
41 }
42 
44 {
45  if (m_bodyA)
46  {
47  if (m_linkA < 0)
48  {
49  btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
50  if (col)
51  return col->getIslandTag();
52  }
53  else
54  {
55  if (m_bodyA->getLink(m_linkA).m_collider)
56  return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
57  }
58  }
59  return -1;
60 }
61 
63 {
64  if (m_bodyB)
65  {
66  if (m_linkB < 0)
67  {
68  btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
69  if (col)
70  return col->getIslandTag();
71  }
72  else
73  {
74  if (m_bodyB->getLink(m_linkB).m_collider)
75  return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
76  }
77  }
78  return -1;
79 }
80 
84 {
85  // only positions need to be updated -- data.m_jacobians and force
86  // directions were set in the ctor and never change.
87 
89  {
91  }
92 
93  //don't crash
95  return;
96 
97  if (m_maxAppliedImpulse == 0.f)
98  return;
99 
100  // note: we rely on the fact that data.m_jacobians are
101  // always initialized to zero by the Constraint ctor
102  int linkDoF = 0;
103  unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
104  unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
105 
106  // row 0: the lower bound
107  jacobianA(0)[offsetA] = 1;
108  jacobianB(0)[offsetB] = m_gearRatio;
109 
110  btScalar posError = 0;
111  const btVector3 dummy(0, 0, 0);
112 
113  btScalar kp = 1;
114  btScalar kd = 1;
115  int numRows = getNumRows();
116 
117  for (int row = 0; row < numRows; row++)
118  {
119  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
120 
121  int dof = 0;
122  btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
123  btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
124  btScalar auxVel = 0;
125 
126  if (m_gearAuxLink >= 0)
127  {
128  auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
129  }
130  currentVelocity += auxVel;
131  if (m_erp != 0)
132  {
133  btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
134  if (m_gearAuxLink >= 0)
135  {
136  currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
137  }
138  btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
139  btScalar diff = currentPositionB + currentPositionA;
140  btScalar desiredPositionDiff = this->m_relativePositionTarget;
141  posError = -m_erp * (desiredPositionDiff - diff);
142  }
143 
144  btScalar desiredRelativeVelocity = auxVel;
145 
146  fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
147 
148  constraintRow.m_orgConstraint = this;
149  constraintRow.m_orgDofIndex = row;
150  {
151  //expect either prismatic or revolute joint type for now
152  btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
153  switch (m_bodyA->getLink(m_linkA).m_jointType)
154  {
156  {
157  constraintRow.m_contactNormal1.setZero();
158  constraintRow.m_contactNormal2.setZero();
159  btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
160  constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
161  constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
162 
163  break;
164  }
166  {
167  btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
168  constraintRow.m_contactNormal1 = prismaticAxisInWorld;
169  constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
170  constraintRow.m_relpos1CrossNormal.setZero();
171  constraintRow.m_relpos2CrossNormal.setZero();
172  break;
173  }
174  default:
175  {
176  btAssert(0);
177  }
178  };
179  }
180  }
181 }
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
@ MULTIBODY_CONSTRAINT_GEAR
int m_numDofsFinalized
int m_linkB
btScalar * jacobianB(int row)
btMultiBody * m_bodyB
int m_linkA
void allocateJacobiansMultiDof()
int m_jacSizeBoth
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)
btScalar m_maxAppliedImpulse
btScalar * jacobianA(int row)
btMultiBodyConstraint
int getNumRows() const
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBody
Definition: btMultiBody.h:51
btScalar m_erp
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE T & expandNonInitializing()
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
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.
uint col
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)