dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.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 "btMultiBodySphericalJointLimit.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 #include "LinearMath/btIDebugDraw.h"
  25
  26 btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link,
  27         btScalar swingxRange,
  28         btScalar swingyRange,
  29         btScalar twistRange,
  30         btScalar maxAppliedImpulse)
  31         : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
  32         m_desiredVelocity(0, 0, 0),
  33         m_desiredPosition(0,0,0,1),
  34         m_use_multi_dof_params(false),
  35         m_kd(1., 1., 1.),
  36         m_kp(0.2, 0.2, 0.2),
  37         m_erp(1),
  38         m_rhsClamp(SIMD_INFINITY),
  39         m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
  40         m_pivotA(m_bodyA->getLink(link).m_eVector),
  41         m_pivotB(m_bodyB->getLink(link).m_eVector),
  42         m_swingxRange(swingxRange),
  43         m_swingyRange(swingyRange),
  44         m_twistRange(twistRange)
  45
  46 {
  47
  48         m_maxAppliedImpulse = maxAppliedImpulse;
  49 }
  50
  51
  52 void btMultiBodySphericalJointLimit::finalizeMultiDof()
  53 {
  54         allocateJacobiansMultiDof();
  55         // note: we rely on the fact that data.m_jacobians are
  56         // always initialized to zero by the Constraint ctor
  57         int linkDoF = 0;
  58         unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
  59
  60         // row 0: the lower bound
  61         // row 0: the lower bound
  62         jacobianA(0)[offset] = 1;
  63
  64         jacobianB(1)[offset] = -1;
  65
  66         m_numDofsFinalized = m_jacSizeBoth;
  67 }
  68
  69
  70 btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit()
  71 {
  72 }
  73
  74 int btMultiBodySphericalJointLimit::getIslandIdA() const
  75 {
  76         if (this->m_linkA < 0)
  77         {
  78                 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
  79                 if (col)
  80                         return col->getIslandTag();
  81         }
  82         else
  83         {
  84                 if (m_bodyA->getLink(m_linkA).m_collider)
  85                 {
  86                         return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
  87                 }
  88         }
  89         return -1;
  90 }
  91
  92 int btMultiBodySphericalJointLimit::getIslandIdB() const
  93 {
  94         if (m_linkB < 0)
  95         {
  96                 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
  97                 if (col)
  98                         return col->getIslandTag();
  99         }
 100         else
 101         {
 102                 if (m_bodyB->getLink(m_linkB).m_collider)
 103                 {
 104                         return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
 105                 }
 106         }
 107         return -1;
 108 }
 109
 110 void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 111                                                                                                  btMultiBodyJacobianData& data,
 112                                                                                                  const btContactSolverInfo& infoGlobal)
 113 {
 114         // only positions need to be updated -- data.m_jacobians and force
 115         // directions were set in the ctor and never change.
 116
 117         if (m_numDofsFinalized != m_jacSizeBoth)
 118         {
 119                 finalizeMultiDof();
 120         }
 121
 122         //don't crash
 123         if (m_numDofsFinalized != m_jacSizeBoth)
 124                 return;
 125
 126
 127         if (m_maxAppliedImpulse == 0.f)
 128                 return;
 129
 130         const btScalar posError = 0;
 131         const btVector3 zero(0, 0, 0);
 132
 133
 134         btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
 135
 136         btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
 137                 m_bodyA->getJointPosMultiDof(m_linkA)[1],
 138                 m_bodyA->getJointPosMultiDof(m_linkA)[2],
 139                 m_bodyA->getJointPosMultiDof(m_linkA)[3]);
 140
 141         btQuaternion refQuat = m_desiredPosition;
 142         btVector3 vTwist(0,0,1);
 143
 144         btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist);
 145         vConeNoTwist.normalize();
 146         btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
 147         qABCone.normalize();
 148         btQuaternion qABTwist = qABCone.inverse() * currentQuat;
 149         qABTwist.normalize();
 150         btQuaternion desiredQuat = qABTwist;
 151
 152
 153         btQuaternion relRot = currentQuat.inverse() * desiredQuat;
 154         btVector3 angleDiff;
 155         btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
 156
 157         btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange};
 158
 159         /// twist axis/angle
 160         btQuaternion qMinTwist = qABTwist;
 161         btScalar twistAngle = qABTwist.getAngle();
 162
 163         if (twistAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 164         {
 165                 qMinTwist = -(qABTwist);
 166                 twistAngle = qMinTwist.getAngle();
 167         }
 168         btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
 169         if (twistAngle > SIMD_EPSILON)
 170                 vTwistAxis.normalize();
 171
 172         if (vTwistAxis.dot(vTwist)<0)
 173                 twistAngle*=-1.;
 174
 175         angleDiff[2] = twistAngle;
 176
 177
 178         for (int row = 0; row < getNumRows(); row++)
 179         {
 180                 btScalar allowed = limitRanges[row];
 181                 btScalar damp = 1;
 182                 if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
 183                 {
 184                         angleDiff[row]=0;
 185                         damp=0;
 186
 187                 } else
 188                 {
 189                         if (angleDiff[row]>allowed)
 190                         {
 191                                 angleDiff[row]-=allowed;
 192
 193                         }
 194                         if (angleDiff[row]<-allowed)
 195                         {
 196                                 angleDiff[row]+=allowed;
 197
 198                         }
 199                 }
 200
 201
 202                 int dof = row;
 203
 204                 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
 205                 btScalar desiredVelocity = this->m_desiredVelocity[row];
 206
 207                 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
 208                 btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
 209
 210                 btMatrix3x3 frameAworld;
 211                 frameAworld.setIdentity();
 212                 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
 213                 btScalar posError = 0;
 214                 {
 215                         btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
 216                         switch (m_bodyA->getLink(m_linkA).m_jointType)
 217                         {
 218                                 case btMultibodyLink::eSpherical:
 219                                 {
 220                                         btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
 221                                         double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
 222                                         posError = kp*angleDiff[row % 3];
 223                                         double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
 224                                         //should multiply by time step
 225                                         //max_applied_impulse *= infoGlobal.m_timeStep
 226
 227                                         double min_applied_impulse = -max_applied_impulse;
 228
 229
 230                                         if (posError>0)
 231                                                 max_applied_impulse=0;
 232                                         else
 233                                                 min_applied_impulse=0;
 234
 235                                         if (btFabs(posError)>SIMD_EPSILON)
 236                                         {
 237                                                 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 238                                                 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
 239                                                         zero, zero, zero,//pure angular, so zero out linear parts
 240                                                         posError,
 241                                                         infoGlobal,
 242                                                         min_applied_impulse, max_applied_impulse, true,
 243                                                         1.0, false, 0, 0,
 244                                                         damp);
 245                                                 constraintRow.m_orgConstraint = this;
 246                                                 constraintRow.m_orgDofIndex = row;
 247                                         }
 248                                         break;
 249                                 }
 250                                 default:
 251                                 {
 252                                         btAssert(0);
 253                                 }
 254                         };
 255                 }
 256         }
 257 }
 258
 259
 260 void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer)
 261 {
 262         btTransform tr;
 263         tr.setIdentity();
 264         if (m_bodyB)
 265         {
 266                 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB);
 267                 tr.setOrigin(pivotBworld);
 268                 drawer->drawTransform(tr, 0.1);
 269         }
 270 }