Bullet Collision Detection & Physics Library
btMultiBodyFixedConstraint.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 
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)
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 
51 {
52  //not implemented yet
53  btAssert(0);
54 }
55 
57 {
58 }
59 
61 {
62  if (m_rigidBodyA)
63  return m_rigidBodyA->getIslandTag();
64 
65  if (m_bodyA)
66  {
67  if (m_linkA < 0)
68  {
70  if (col)
71  return col->getIslandTag();
72  }
73  else
74  {
77  }
78  }
79  return -1;
80 }
81 
83 {
84  if (m_rigidBodyB)
85  return m_rigidBodyB->getIslandTag();
86  if (m_bodyB)
87  {
88  if (m_linkB < 0)
89  {
91  if (col)
92  return col->getIslandTag();
93  }
94  else
95  {
98  }
99  }
100  return -1;
101 }
102 
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();
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();
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;
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,
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,
181  }
182  }
183 }
184 
186 {
187  btTransform tr;
188  tr.setIdentity();
189 
190  if (m_rigidBodyA)
191  {
193  tr.setOrigin(pivot);
194  drawer->drawTransform(tr, 0.1);
195  }
196  if (m_bodyA)
197  {
199  tr.setOrigin(pivotAworld);
200  drawer->drawTransform(tr, 0.1);
201  }
202  if (m_rigidBodyB)
203  {
204  // that ideally should draw the same frame
206  tr.setOrigin(pivot);
207  drawer->drawTransform(tr, 0.1);
208  }
209  if (m_bodyB)
210  {
212  tr.setOrigin(pivotBworld);
213  drawer->drawTransform(tr, 0.1);
214  }
215 }
@ MULTIBODY_CONSTRAINT_FIXED
#define BTMBFIXEDCONSTRAINT_DIM
This file was written by Erwin Coumans.
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:153
void resize(int newsize, const T &fillData=T())
int getIslandTag() const
int getCompanionId() const
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1093
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
btAlignedObjectArray< btScalar > m_data
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, btScalar damping=1.0)
virtual void debugDraw(class btIDebugDraw *drawer)
btMultiBodyFixedConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btQuaternion getOrientation() const
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint