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