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