dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.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 "btMultiBodySliderConstraint.h"
  19 #include "btMultiBodyLinkCollider.h"
  20 #include "BulletDynamics/Dynamics/btRigidBody.h"
  21 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
  22 #include "LinearMath/btIDebugDraw.h"
  23
  24 #define BTMBSLIDERCONSTRAINT_DIM 5
  25 #define EPSILON 0.000001
  26
  27 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
  28         : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
  29           m_rigidBodyA(0),
  30           m_rigidBodyB(bodyB),
  31           m_pivotInA(pivotInA),
  32           m_pivotInB(pivotInB),
  33           m_frameInA(frameInA),
  34           m_frameInB(frameInB),
  35           m_jointAxis(jointAxis)
  36 {
  37         m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
  38 }
  39
  40 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
  41         : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
  42           m_rigidBodyA(0),
  43           m_rigidBodyB(0),
  44           m_pivotInA(pivotInA),
  45           m_pivotInB(pivotInB),
  46           m_frameInA(frameInA),
  47           m_frameInB(frameInB),
  48           m_jointAxis(jointAxis)
  49 {
  50         m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
  51 }
  52
  53 void btMultiBodySliderConstraint::finalizeMultiDof()
  54 {
  55         //not implemented yet
  56         btAssert(0);
  57 }
  58
  59 btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
  60 {
  61 }
  62
  63 int btMultiBodySliderConstraint::getIslandIdA() const
  64 {
  65         if (m_rigidBodyA)
  66                 return m_rigidBodyA->getIslandTag();
  67
  68         if (m_bodyA)
  69         {
  70                 if (m_linkA < 0)
  71                 {
  72                         btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
  73                         if (col)
  74                                 return col->getIslandTag();
  75                 }
  76                 else
  77                 {
  78                         if (m_bodyA->getLink(m_linkA).m_collider)
  79                                 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
  80                 }
  81         }
  82         return -1;
  83 }
  84
  85 int btMultiBodySliderConstraint::getIslandIdB() const
  86 {
  87         if (m_rigidBodyB)
  88                 return m_rigidBodyB->getIslandTag();
  89         if (m_bodyB)
  90         {
  91                 if (m_linkB < 0)
  92                 {
  93                         btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
  94                         if (col)
  95                                 return col->getIslandTag();
  96                 }
  97                 else
  98                 {
  99                         if (m_bodyB->getLink(m_linkB).m_collider)
 100                                 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
 101                 }
 102         }
 103         return -1;
 104 }
 105 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
 106 {
 107         // Convert local points back to world
 108         btVector3 pivotAworld = m_pivotInA;
 109         btMatrix3x3 frameAworld = m_frameInA;
 110         btVector3 jointAxis = m_jointAxis;
 111         if (m_rigidBodyA)
 112         {
 113                 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
 114                 frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
 115                 jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
 116         }
 117         else if (m_bodyA)
 118         {
 119                 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
 120                 frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
 121                 jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
 122         }
 123         btVector3 pivotBworld = m_pivotInB;
 124         btMatrix3x3 frameBworld = m_frameInB;
 125         if (m_rigidBodyB)
 126         {
 127                 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
 128                 frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
 129         }
 130         else if (m_bodyB)
 131         {
 132                 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
 133                 frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
 134         }
 135
 136         btVector3 constraintAxis[2];
 137         for (int i = 0; i < 3; ++i)
 138         {
 139                 constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
 140                 if (constraintAxis[0].safeNorm() > EPSILON)
 141                 {
 142                         constraintAxis[0] = constraintAxis[0].normalized();
 143                         constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
 144                         constraintAxis[1] = constraintAxis[1].normalized();
 145                         break;
 146                 }
 147         }
 148
 149         btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
 150         btVector3 angleDiff;
 151         btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
 152
 153         int numDim = BTMBSLIDERCONSTRAINT_DIM;
 154         for (int i = 0; i < numDim; i++)
 155         {
 156                 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 157                 constraintRow.m_orgConstraint = this;
 158                 constraintRow.m_orgDofIndex = i;
 159                 constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
 160                 constraintRow.m_contactNormal1.setValue(0, 0, 0);
 161                 constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
 162                 constraintRow.m_contactNormal2.setValue(0, 0, 0);
 163                 constraintRow.m_angularComponentA.setValue(0, 0, 0);
 164                 constraintRow.m_angularComponentB.setValue(0, 0, 0);
 165
 166                 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
 167                 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
 168
 169                 if (m_rigidBodyA)
 170                 {
 171                         constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
 172                 }
 173                 if (m_rigidBodyB)
 174                 {
 175                         constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
 176                 }
 177
 178                 btVector3 constraintNormalLin(0, 0, 0);
 179                 btVector3 constraintNormalAng(0, 0, 0);
 180                 btScalar posError = 0.0;
 181                 if (i < 2)
 182                 {
 183                         constraintNormalLin = constraintAxis[i];
 184                         posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
 185                         fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
 186                                                                         constraintNormalLin, pivotAworld, pivotBworld,
 187                                                                         posError,
 188                                                                         infoGlobal,
 189                                                                         -m_maxAppliedImpulse, m_maxAppliedImpulse);
 190                 }
 191                 else
 192                 {  //i>=2
 193                         constraintNormalAng = frameAworld.getColumn(i % 3);
 194                         posError = angleDiff[i % 3];
 195                         fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
 196                                                                         constraintNormalLin, pivotAworld, pivotBworld,
 197                                                                         posError,
 198                                                                         infoGlobal,
 199                                                                         -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
 200                 }
 201         }
 202 }
 203
 204 void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
 205 {
 206         btTransform tr;
 207         tr.setIdentity();
 208
 209         if (m_rigidBodyA)
 210         {
 211                 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
 212                 tr.setOrigin(pivot);
 213                 drawer->drawTransform(tr, 0.1);
 214         }
 215         if (m_bodyA)
 216         {
 217                 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
 218                 tr.setOrigin(pivotAworld);
 219                 drawer->drawTransform(tr, 0.1);
 220         }
 221         if (m_rigidBodyB)
 222         {
 223                 // that ideally should draw the same frame
 224                 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
 225                 tr.setOrigin(pivot);
 226                 drawer->drawTransform(tr, 0.1);
 227         }
 228         if (m_bodyB)
 229         {
 230                 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
 231                 tr.setOrigin(pivotBworld);
 232                 drawer->drawTransform(tr, 0.1);
 233         }
 234 }