Blender  V3.3
btMultiBodySliderConstraint.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 BTMBSLIDERCONSTRAINT_DIM 5
25 #define EPSILON 0.000001
26 
27 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
29  m_rigidBodyA(0),
30  m_rigidBodyB(bodyB),
31  m_pivotInA(pivotInA),
32  m_pivotInB(pivotInB),
35  m_jointAxis(jointAxis)
36 {
37  m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
38 }
39 
40 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
41  : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
42  m_rigidBodyA(0),
43  m_rigidBodyB(0),
44  m_pivotInA(pivotInA),
45  m_pivotInB(pivotInB),
48  m_jointAxis(jointAxis)
49 {
50  m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
51 }
52 
54 {
55  //not implemented yet
56  btAssert(0);
57 }
58 
60 {
61 }
62 
64 {
65  if (m_rigidBodyA)
66  return m_rigidBodyA->getIslandTag();
67 
68  if (m_bodyA)
69  {
70  if (m_linkA < 0)
71  {
72  btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
73  if (col)
74  return col->getIslandTag();
75  }
76  else
77  {
78  if (m_bodyA->getLink(m_linkA).m_collider)
79  return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
80  }
81  }
82  return -1;
83 }
84 
86 {
87  if (m_rigidBodyB)
88  return m_rigidBodyB->getIslandTag();
89  if (m_bodyB)
90  {
91  if (m_linkB < 0)
92  {
93  btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
94  if (col)
95  return col->getIslandTag();
96  }
97  else
98  {
99  if (m_bodyB->getLink(m_linkB).m_collider)
100  return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
101  }
102  }
103  return -1;
104 }
106 {
107  // Convert local points back to world
108  btVector3 pivotAworld = m_pivotInA;
109  btMatrix3x3 frameAworld = m_frameInA;
110  btVector3 jointAxis = m_jointAxis;
111  if (m_rigidBodyA)
112  {
114  frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
116  }
117  else if (m_bodyA)
118  {
119  pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
120  frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
121  jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
122  }
123  btVector3 pivotBworld = m_pivotInB;
124  btMatrix3x3 frameBworld = m_frameInB;
125  if (m_rigidBodyB)
126  {
128  frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
129  }
130  else if (m_bodyB)
131  {
132  pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
133  frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
134  }
135 
136  btVector3 constraintAxis[2];
137  for (int i = 0; i < 3; ++i)
138  {
139  constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
140  if (constraintAxis[0].safeNorm() > EPSILON)
141  {
142  constraintAxis[0] = constraintAxis[0].normalized();
143  constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
144  constraintAxis[1] = constraintAxis[1].normalized();
145  break;
146  }
147  }
148 
149  btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
150  btVector3 angleDiff;
152 
153  int numDim = BTMBSLIDERCONSTRAINT_DIM;
154  for (int i = 0; i < numDim; i++)
155  {
156  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
157  constraintRow.m_orgConstraint = this;
158  constraintRow.m_orgDofIndex = i;
159  constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
160  constraintRow.m_contactNormal1.setValue(0, 0, 0);
161  constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
162  constraintRow.m_contactNormal2.setValue(0, 0, 0);
163  constraintRow.m_angularComponentA.setValue(0, 0, 0);
164  constraintRow.m_angularComponentB.setValue(0, 0, 0);
165 
166  constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
167  constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
168 
169  if (m_rigidBodyA)
170  {
171  constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
172  }
173  if (m_rigidBodyB)
174  {
175  constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
176  }
177 
178  btVector3 constraintNormalLin(0, 0, 0);
179  btVector3 constraintNormalAng(0, 0, 0);
180  btScalar posError = 0.0;
181  if (i < 2)
182  {
183  constraintNormalLin = constraintAxis[i];
184  posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
185  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
186  constraintNormalLin, pivotAworld, pivotBworld,
187  posError,
188  infoGlobal,
190  }
191  else
192  { //i>=2
193  constraintNormalAng = frameAworld.getColumn(i % 3);
194  posError = angleDiff[i % 3];
195  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
196  constraintNormalLin, pivotAworld, pivotBworld,
197  posError,
198  infoGlobal,
200  }
201  }
202 }
203 
205 {
206  btTransform tr;
207  tr.setIdentity();
208 
209  if (m_rigidBodyA)
210  {
212  tr.setOrigin(pivot);
213  drawer->drawTransform(tr, 0.1);
214  }
215  if (m_bodyA)
216  {
217  btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
218  tr.setOrigin(pivotAworld);
219  drawer->drawTransform(tr, 0.1);
220  }
221  if (m_rigidBodyB)
222  {
223  // that ideally should draw the same frame
225  tr.setOrigin(pivot);
226  drawer->drawTransform(tr, 0.1);
227  }
228  if (m_bodyB)
229  {
230  btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
231  tr.setOrigin(pivotBworld);
232  drawer->drawTransform(tr, 0.1);
233  }
234 }
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
btTransform m_frameInB
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
@ MULTIBODY_CONSTRAINT_SLIDER
int m_linkB
btAlignedObjectArray< btScalar > m_data
btMultiBody * m_bodyB
int m_linkA
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 m_maxAppliedImpulse
btMultiBodyConstraint
btRigidBody * m_rigidBodyB
#define BTMBSLIDERCONSTRAINT_DIM
This file was written by Erwin Coumans.
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBody
Definition: btMultiBody.h:51
btVector3 m_pivotInA
btVector3 m_pivotInB
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
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:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btTransform m_frameInA
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
SIMD_FORCE_INLINE btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
virtual void debugDraw(class btIDebugDraw *drawer)
btMultiBodySliderConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB, const btVector3 &jointAxis)
btQuaternion getOrientation() const
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
uint col
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)