dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.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 "btMultiBodyJointMotor.h"
  19 #include "btMultiBody.h"
  20 #include "btMultiBodyLinkCollider.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  22
  23 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
  24         : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
  25           m_desiredVelocity(desiredVelocity),
  26           m_desiredPosition(0),
  27           m_kd(1.),
  28           m_kp(0),
  29           m_erp(1),
  30           m_rhsClamp(SIMD_INFINITY)
  31 {
  32         m_maxAppliedImpulse = maxMotorImpulse;
  33         // the data.m_jacobians never change, so may as well
  34         // initialize them here
  35 }
  36
  37 void btMultiBodyJointMotor::finalizeMultiDof()
  38 {
  39         allocateJacobiansMultiDof();
  40         // note: we rely on the fact that data.m_jacobians are
  41         // always initialized to zero by the Constraint ctor
  42         int linkDoF = 0;
  43         unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
  44
  45         // row 0: the lower bound
  46         // row 0: the lower bound
  47         jacobianA(0)[offset] = 1;
  48
  49         m_numDofsFinalized = m_jacSizeBoth;
  50 }
  51
  52 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
  53         //:btMultiBodyConstraint(body,0,link,-1,1,true),
  54         : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
  55           m_desiredVelocity(desiredVelocity),
  56           m_desiredPosition(0),
  57           m_kd(1.),
  58           m_kp(0),
  59           m_erp(1),
  60           m_rhsClamp(SIMD_INFINITY)
  61 {
  62         btAssert(linkDoF < body->getLink(link).m_dofCount);
  63
  64         m_maxAppliedImpulse = maxMotorImpulse;
  65 }
  66 btMultiBodyJointMotor::~btMultiBodyJointMotor()
  67 {
  68 }
  69
  70 int btMultiBodyJointMotor::getIslandIdA() const
  71 {
  72         if (this->m_linkA < 0)
  73         {
  74                 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
  75                 if (col)
  76                         return col->getIslandTag();
  77         }
  78         else
  79         {
  80                 if (m_bodyA->getLink(m_linkA).m_collider)
  81                 {
  82                         return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
  83                 }
  84         }
  85         return -1;
  86 }
  87
  88 int btMultiBodyJointMotor::getIslandIdB() const
  89 {
  90         if (m_linkB < 0)
  91         {
  92                 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
  93                 if (col)
  94                         return col->getIslandTag();
  95         }
  96         else
  97         {
  98                 if (m_bodyB->getLink(m_linkB).m_collider)
  99                 {
 100                         return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
 101                 }
 102         }
 103         return -1;
 104 }
 105
 106 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 107                                                                                                  btMultiBodyJacobianData& data,
 108                                                                                                  const btContactSolverInfo& infoGlobal)
 109 {
 110         // only positions need to be updated -- data.m_jacobians and force
 111         // directions were set in the ctor and never change.
 112
 113         if (m_numDofsFinalized != m_jacSizeBoth)
 114         {
 115                 finalizeMultiDof();
 116         }
 117
 118         //don't crash
 119         if (m_numDofsFinalized != m_jacSizeBoth)
 120                 return;
 121
 122         if (m_maxAppliedImpulse == 0.f)
 123                 return;
 124
 125         const btScalar posError = 0;
 126         const btVector3 dummy(0, 0, 0);
 127
 128         for (int row = 0; row < getNumRows(); row++)
 129         {
 130                 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 131
 132                 int dof = 0;
 133                 btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
 134                 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
 135                 btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
 136
 137                 btScalar velocityError = (m_desiredVelocity - currentVelocity);
 138                 btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
 139                 if (rhs > m_rhsClamp)
 140                 {
 141                         rhs = m_rhsClamp;
 142                 }
 143                 if (rhs < -m_rhsClamp)
 144                 {
 145                         rhs = -m_rhsClamp;
 146                 }
 147
 148                 fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
 149                 constraintRow.m_orgConstraint = this;
 150                 constraintRow.m_orgDofIndex = row;
 151                 {
 152                         //expect either prismatic or revolute joint type for now
 153                         btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 154                         switch (m_bodyA->getLink(m_linkA).m_jointType)
 155                         {
 156                                 case btMultibodyLink::eRevolute:
 157                                 {
 158                                         constraintRow.m_contactNormal1.setZero();
 159                                         constraintRow.m_contactNormal2.setZero();
 160                                         btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
 161                                         constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
 162                                         constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
 163
 164                                         break;
 165                                 }
 166                                 case btMultibodyLink::ePrismatic:
 167                                 {
 168                                         btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
 169                                         constraintRow.m_contactNormal1 = prismaticAxisInWorld;
 170                                         constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 171                                         constraintRow.m_relpos1CrossNormal.setZero();
 172                                         constraintRow.m_relpos2CrossNormal.setZero();
 173
 174                                         break;
 175                                 }
 176                                 default:
 177                                 {
 178                                         btAssert(0);
 179                                 }
 180                         };
 181                 }
 182         }
 183 }