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)
27  :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
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),
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 
60 
62 {
63  if (m_rigidBodyA)
64  return m_rigidBodyA->getIslandTag();
65 
66  if (m_bodyA)
67  {
69  if (col)
70  return col->getIslandTag();
71  for (int i=0;i<m_bodyA->getNumLinks();i++)
72  {
73  if (m_bodyA->getLink(i).m_collider)
74  return m_bodyA->getLink(i).m_collider->getIslandTag();
75  }
76  }
77  return -1;
78 }
79 
81 {
82  if (m_rigidBodyB)
83  return m_rigidBodyB->getIslandTag();
84  if (m_bodyB)
85  {
87  if (col)
88  return col->getIslandTag();
89 
90  for (int i=0;i<m_bodyB->getNumLinks();i++)
91  {
92  col = m_bodyB->getLink(i).m_collider;
93  if (col)
94  return col->getIslandTag();
95  }
96  }
97  return -1;
98 }
99 
101 {
102  int numDim = BTMBFIXEDCONSTRAINT_DIM;
103  for (int i=0;i<numDim;i++)
104  {
105  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
106  constraintRow.m_orgConstraint = this;
107  constraintRow.m_orgDofIndex = i;
108  constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
109  constraintRow.m_contactNormal1.setValue(0,0,0);
110  constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
111  constraintRow.m_contactNormal2.setValue(0,0,0);
112  constraintRow.m_angularComponentA.setValue(0,0,0);
113  constraintRow.m_angularComponentB.setValue(0,0,0);
114 
115  constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
116  constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
117 
118  // Convert local points back to world
119  btVector3 pivotAworld = m_pivotInA;
120  btMatrix3x3 frameAworld = m_frameInA;
121  if (m_rigidBodyA)
122  {
123 
124  constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
126  frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
127 
128  } else
129  {
130  if (m_bodyA) {
131  pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
132  frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
133  }
134  }
135  btVector3 pivotBworld = m_pivotInB;
136  btMatrix3x3 frameBworld = m_frameInB;
137  if (m_rigidBodyB)
138  {
139  constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
141  frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
142 
143  } else
144  {
145  if (m_bodyB) {
146  pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
147  frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
148  }
149  }
150 
151  btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
152  btVector3 angleDiff;
154 
155  btVector3 constraintNormalLin(0,0,0);
156  btVector3 constraintNormalAng(0,0,0);
157  btScalar posError = 0.0;
158  if (i < 3) {
159  constraintNormalLin[i] = -1;
160  posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
161  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
162  constraintNormalLin, pivotAworld, pivotBworld,
163  posError,
164  infoGlobal,
166  );
167  }
168  else { //i>=3
169  constraintNormalAng = frameAworld.getColumn(i%3);
170  posError = angleDiff[i%3];
171  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
172  constraintNormalLin, pivotAworld, pivotBworld,
173  posError,
174  infoGlobal,
176  );
177  }
178  }
179 }
180 
182 {
183  btTransform tr;
184  tr.setIdentity();
185 
186  if (m_rigidBodyA)
187  {
189  tr.setOrigin(pivot);
190  drawer->drawTransform(tr, 0.1);
191  }
192  if (m_bodyA)
193  {
195  tr.setOrigin(pivotAworld);
196  drawer->drawTransform(tr, 0.1);
197  }
198  if (m_rigidBodyB)
199  {
200  // that ideally should draw the same frame
202  tr.setOrigin(pivot);
203  drawer->drawTransform(tr, 0.1);
204  }
205  if (m_bodyB)
206  {
208  tr.setOrigin(pivotBworld);
209  drawer->drawTransform(tr, 0.1);
210  }
211 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:164
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btMultiBodyConstraint * m_orgConstraint
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:131
btQuaternion getOrientation() const
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
#define BTMBFIXEDCONSTRAINT_DIM
This file was written by Erwin Coumans.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:29
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btAlignedObjectArray< btScalar > m_data
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
int getIslandTag() const
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
void resize(int newsize, const T &fillData=T())
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btMultiBodyFixedConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
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)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
int getCompanionId() const
virtual void debugDraw(class btIDebugDraw *drawer)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292