dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.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 "btMultiBodyJointLimitConstraint.h"
  19 #include "btMultiBody.h"
  20 #include "btMultiBodyLinkCollider.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  22
  23 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
  24         //:btMultiBodyConstraint(body,0,link,-1,2,true),
  25         : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
  26           m_lowerBound(lower),
  27           m_upperBound(upper)
  28 {
  29 }
  30
  31 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
  32 {
  33         // the data.m_jacobians never change, so may as well
  34         // initialize them here
  35
  36         allocateJacobiansMultiDof();
  37
  38         unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
  39
  40         // row 0: the lower bound
  41         jacobianA(0)[offset] = 1;
  42         // row 1: the upper bound
  43         //jacobianA(1)[offset] = -1;
  44         jacobianB(1)[offset] = -1;
  45
  46         m_numDofsFinalized = m_jacSizeBoth;
  47 }
  48
  49 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
  50 {
  51 }
  52
  53 int btMultiBodyJointLimitConstraint::getIslandIdA() const
  54 {
  55         if (m_bodyA)
  56         {
  57                 if (m_linkA < 0)
  58                 {
  59                         btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
  60                         if (col)
  61                                 return col->getIslandTag();
  62                 }
  63                 else
  64                 {
  65                         if (m_bodyA->getLink(m_linkA).m_collider)
  66                                 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
  67                 }
  68         }
  69         return -1;
  70 }
  71
  72 int btMultiBodyJointLimitConstraint::getIslandIdB() const
  73 {
  74         if (m_bodyB)
  75         {
  76                 if (m_linkB < 0)
  77                 {
  78                         btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
  79                         if (col)
  80                                 return col->getIslandTag();
  81                 }
  82                 else
  83                 {
  84                         if (m_bodyB->getLink(m_linkB).m_collider)
  85                                 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
  86                 }
  87         }
  88         return -1;
  89 }
  90
  91 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
  92                                                                                                                    btMultiBodyJacobianData& data,
  93                                                                                                                    const btContactSolverInfo& infoGlobal)
  94 {
  95         // only positions need to be updated -- data.m_jacobians and force
  96         // directions were set in the ctor and never change.
  97
  98         if (m_numDofsFinalized != m_jacSizeBoth)
  99         {
 100                 finalizeMultiDof();
 101         }
 102
 103         // row 0: the lower bound
 104         setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);  //multidof: this is joint-type dependent
 105
 106         // row 1: the upper bound
 107         setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
 108
 109         for (int row = 0; row < getNumRows(); row++)
 110         {
 111                 btScalar penetration = getPosition(row);
 112
 113                 //todo: consider adding some safety threshold here
 114                 if (penetration > 0)
 115                 {
 116                         continue;
 117                 }
 118                 btScalar direction = row ? -1 : 1;
 119
 120                 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 121                 constraintRow.m_orgConstraint = this;
 122                 constraintRow.m_orgDofIndex = row;
 123
 124                 constraintRow.m_multiBodyA = m_bodyA;
 125                 constraintRow.m_multiBodyB = m_bodyB;
 126                 const btScalar posError = 0;  //why assume it's zero?
 127                 const btVector3 dummy(0, 0, 0);
 128
 129                 btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
 130
 131                 {
 132                         //expect either prismatic or revolute joint type for now
 133                         btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 134                         switch (m_bodyA->getLink(m_linkA).m_jointType)
 135                         {
 136                                 case btMultibodyLink::eRevolute:
 137                                 {
 138                                         constraintRow.m_contactNormal1.setZero();
 139                                         constraintRow.m_contactNormal2.setZero();
 140                                         btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
 141                                         constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
 142                                         constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
 143
 144                                         break;
 145                                 }
 146                                 case btMultibodyLink::ePrismatic:
 147                                 {
 148                                         btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
 149                                         constraintRow.m_contactNormal1 = prismaticAxisInWorld;
 150                                         constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 151                                         constraintRow.m_relpos1CrossNormal.setZero();
 152                                         constraintRow.m_relpos2CrossNormal.setZero();
 153
 154                                         break;
 155                                 }
 156                                 default:
 157                                 {
 158                                         btAssert(0);
 159                                 }
 160                         };
 161                 }
 162
 163                 {
 164                         btScalar positionalError = 0.f;
 165                         btScalar velocityError = -rel_vel;  // * damping;
 166                         btScalar erp = infoGlobal.m_erp2;
 167                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 168                         {
 169                                 erp = infoGlobal.m_erp;
 170                         }
 171                         if (penetration > 0)
 172                         {
 173                                 positionalError = 0;
 174                                 velocityError = -penetration / infoGlobal.m_timeStep;
 175                         }
 176                         else
 177                         {
 178                                 positionalError = -penetration * erp / infoGlobal.m_timeStep;
 179                         }
 180
 181                         btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
 182                         btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
 183                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 184                         {
 185                                 //combine position and velocity into rhs
 186                                 constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
 187                                 constraintRow.m_rhsPenetration = 0.f;
 188                         }
 189                         else
 190                         {
 191                                 //split position and velocity into rhs and m_rhsPenetration
 192                                 constraintRow.m_rhs = velocityImpulse;
 193                                 constraintRow.m_rhsPenetration = penetrationImpulse;
 194                         }
 195                 }
 196         }
 197 }