dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

   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
  16 ///This file was written by Erwin Coumans
  17
  18 #include "btMultiBodyGearConstraint.h"
  19 #include "btMultiBody.h"
  20 #include "btMultiBodyLinkCollider.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.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
  32 void btMultiBodyGearConstraint::finalizeMultiDof()
  33 {
  34         allocateJacobiansMultiDof();
  35
  36         m_numDofsFinalized = m_jacSizeBoth;
  37 }
  38
  39 btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
  40 {
  41 }
  42
  43 int btMultiBodyGearConstraint::getIslandIdA() const
  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
  62 int btMultiBodyGearConstraint::getIslandIdB() const
  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
  81 void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
  82                                                                                                          btMultiBodyJacobianData& data,
  83                                                                                                          const btContactSolverInfo& infoGlobal)
  84 {
  85         // only positions need to be updated -- data.m_jacobians and force
  86         // directions were set in the ctor and never change.
  87
  88         if (m_numDofsFinalized != m_jacSizeBoth)
  89         {
  90                 finalizeMultiDof();
  91         }
  92
  93         //don't crash
  94         if (m_numDofsFinalized != m_jacSizeBoth)
  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                         {
 155                                 case btMultibodyLink::eRevolute:
 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                                 }
 165                                 case btMultibodyLink::ePrismatic:
 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 }