Bullet Collision Detection & Physics Library
btMultiBodyPoint2Point.cpp
Go to the documentation of this file.
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 
17 
18 #include "btMultiBodyPoint2Point.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),
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),
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 
50 {
51  //not implemented yet
52  btAssert(0);
53 }
54 
56 {
57 }
58 
60 {
61  if (m_rigidBodyA)
62  return m_rigidBodyA->getIslandTag();
63 
64  if (m_bodyA)
65  {
66  if (m_linkA < 0)
67  {
69  if (col)
70  return col->getIslandTag();
71  }
72  else
73  {
76  }
77  }
78  return -1;
79 }
80 
82 {
83  if (m_rigidBodyB)
84  return m_rigidBodyB->getIslandTag();
85  if (m_bodyB)
86  {
87  if (m_linkB < 0)
88  {
90  if (col)
91  return col->getIslandTag();
92  }
93  else
94  {
97  }
98  }
99  return -1;
100 }
101 
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();
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();
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,
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,
182 #endif
183  }
184 }
185 
187 {
188  btTransform tr;
189  tr.setIdentity();
190 
191  if (m_rigidBodyA)
192  {
194  tr.setOrigin(pivot);
195  drawer->drawTransform(tr, 0.1);
196  }
197  if (m_bodyA)
198  {
200  tr.setOrigin(pivotAworld);
201  drawer->drawTransform(tr, 0.1);
202  }
203  if (m_rigidBodyB)
204  {
205  // that ideally should draw the same frame
207  tr.setOrigin(pivot);
208  drawer->drawTransform(tr, 0.1);
209  }
210  if (m_bodyB)
211  {
213  tr.setOrigin(pivotBworld);
214  drawer->drawTransform(tr, 0.1);
215  }
216 }
btMultiBodyPoint2Point::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyPoint2Point.h:29
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btMultiBodyPoint2Point::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyPoint2Point.cpp:102
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btMultiBodyPoint2Point::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyPoint2Point.cpp:59
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:72
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btMultiBodyPoint2Point::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyPoint2Point.cpp:49
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:47
btMultiBodyPoint2Point.h
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btRigidBody.h
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:394
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodyPoint2Point::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyPoint2Point.h:31
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:26
btMultiBodyPoint2Point::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyPoint2Point.cpp:81
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMultiBodyLinkCollider.h
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:56
btMultiBodyConstraint::fillMultiBodyConstraint
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraint.cpp:52
btMultiBodyPoint2Point::~btMultiBodyPoint2Point
virtual ~btMultiBodyPoint2Point()
Definition: btMultiBodyPoint2Point.cpp:55
btMultiBodyPoint2Point::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyPoint2Point.cpp:186
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btMultiBodyPoint2Point::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodyPoint2Point.h:32
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBody
Definition: btMultiBody.h:49
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:436
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:160
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
BTMBP2PCONSTRAINT_DIM
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodyPoint2Point.cpp:24
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btIDebugDraw.h
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:63
btMultiBodyPoint2Point::btMultiBodyPoint2Point
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
Definition: btMultiBodyPoint2Point.cpp:29
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
Definition: btMultiBody.cpp:464
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btMultiBodyPoint2Point::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodyPoint2Point.h:30
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:446