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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2018 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 "btMultiBodySphericalJointMotor.h"
  19 #include "btMultiBody.h"
  20 #include "btMultiBodyLinkCollider.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  22 #include "LinearMath/btTransformUtil.h"
  23 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
  24
  25 btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
  26         : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
  27         m_desiredVelocity(0, 0, 0),
  28         m_desiredPosition(0,0,0,1),
  29         m_use_multi_dof_params(false),
  30         m_kd(1., 1., 1.),
  31         m_kp(0.2, 0.2, 0.2),
  32         m_erp(1),
  33         m_rhsClamp(SIMD_INFINITY),
  34         m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
  35         m_damping(1.0, 1.0, 1.0)
  36 {
  37
  38         m_maxAppliedImpulse = maxMotorImpulse;
  39 }
  40
  41
  42 void btMultiBodySphericalJointMotor::finalizeMultiDof()
  43 {
  44         allocateJacobiansMultiDof();
  45         // note: we rely on the fact that data.m_jacobians are
  46         // always initialized to zero by the Constraint ctor
  47         int linkDoF = 0;
  48         unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
  49
  50         // row 0: the lower bound
  51         // row 0: the lower bound
  52         jacobianA(0)[offset] = 1;
  53
  54         m_numDofsFinalized = m_jacSizeBoth;
  55 }
  56
  57
  58 btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
  59 {
  60 }
  61
  62 int btMultiBodySphericalJointMotor::getIslandIdA() const
  63 {
  64         if (this->m_linkA < 0)
  65         {
  66                 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
  67                 if (col)
  68                         return col->getIslandTag();
  69         }
  70         else
  71         {
  72                 if (m_bodyA->getLink(m_linkA).m_collider)
  73                 {
  74                         return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
  75                 }
  76         }
  77         return -1;
  78 }
  79
  80 int btMultiBodySphericalJointMotor::getIslandIdB() const
  81 {
  82         if (m_linkB < 0)
  83         {
  84                 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
  85                 if (col)
  86                         return col->getIslandTag();
  87         }
  88         else
  89         {
  90                 if (m_bodyB->getLink(m_linkB).m_collider)
  91                 {
  92                         return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
  93                 }
  94         }
  95         return -1;
  96 }
  97
  98 void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
  99                                                                                                  btMultiBodyJacobianData& data,
 100                                                                                                  const btContactSolverInfo& infoGlobal)
 101 {
 102         // only positions need to be updated -- data.m_jacobians and force
 103         // directions were set in the ctor and never change.
 104
 105         if (m_numDofsFinalized != m_jacSizeBoth)
 106         {
 107                 finalizeMultiDof();
 108         }
 109
 110         //don't crash
 111         if (m_numDofsFinalized != m_jacSizeBoth)
 112                 return;
 113
 114
 115         if (m_maxAppliedImpulse == 0.f)
 116                 return;
 117
 118         const btScalar posError = 0;
 119         const btVector3 dummy(0, 0, 0);
 120
 121
 122         btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
 123
 124         btQuaternion desiredQuat = m_desiredPosition;
 125         btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
 126                 m_bodyA->getJointPosMultiDof(m_linkA)[1],
 127                 m_bodyA->getJointPosMultiDof(m_linkA)[2],
 128                 m_bodyA->getJointPosMultiDof(m_linkA)[3]);
 129
 130 btQuaternion relRot = currentQuat.inverse() * desiredQuat;
 131         btVector3 angleDiff;
 132         btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
 133
 134
 135
 136         for (int row = 0; row < getNumRows(); row++)
 137         {
 138                 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 139
 140                 int dof = row;
 141
 142                 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
 143                 btScalar desiredVelocity = this->m_desiredVelocity[row];
 144
 145                 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
 146                 btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
 147
 148                 btMatrix3x3 frameAworld;
 149                 frameAworld.setIdentity();
 150                 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
 151                 btScalar posError = 0;
 152                 {
 153                         btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
 154                         switch (m_bodyA->getLink(m_linkA).m_jointType)
 155                         {
 156                                 case btMultibodyLink::eSpherical:
 157                                 {
 158                                         btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
 159                                         double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
 160                                         posError = kp*angleDiff[row % 3];
 161                                         double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
 162                                         fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
 163                                                 btVector3(0,0,0), dummy, dummy,
 164                                                 posError,
 165                                                 infoGlobal,
 166                                                 -max_applied_impulse, max_applied_impulse, true,
 167                                                 1.0, false, 0, 0,
 168                                                 m_damping[row % 3]);
 169                                         constraintRow.m_orgConstraint = this;
 170                                         constraintRow.m_orgDofIndex = row;
 171                                         break;
 172                                 }
 173                                 default:
 174                                 {
 175                                         btAssert(0);
 176                                 }
 177                         };
 178                 }
 179         }
 180 }