Bullet Collision Detection & Physics Library
btMultiBodySliderConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
27btMultiBodySliderConstraint::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),
33 m_frameInA(frameInA),
34 m_frameInB(frameInB),
35 m_jointAxis(jointAxis)
36{
37 m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
38}
39
40btMultiBodySliderConstraint::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),
42 m_rigidBodyA(0),
43 m_rigidBodyB(0),
44 m_pivotInA(pivotInA),
45 m_pivotInB(pivotInB),
46 m_frameInA(frameInA),
47 m_frameInB(frameInB),
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
63
65{
66 if (m_rigidBodyA)
67 return m_rigidBodyA->getIslandTag();
68
69 if (m_bodyA)
70 {
72 if (col)
73 return col->getIslandTag();
74 for (int i=0;i<m_bodyA->getNumLinks();i++)
75 {
78 }
79 }
80 return -1;
81}
82
84{
85 if (m_rigidBodyB)
86 return m_rigidBodyB->getIslandTag();
87 if (m_bodyB)
88 {
90 if (col)
91 return col->getIslandTag();
92
93 for (int i=0;i<m_bodyB->getNumLinks();i++)
94 {
95 col = m_bodyB->getLink(i).m_collider;
96 if (col)
97 return col->getIslandTag();
98 }
99 }
100 return -1;
101}
102
104{
105 // Convert local points back to world
106 btVector3 pivotAworld = m_pivotInA;
107 btMatrix3x3 frameAworld = m_frameInA;
108 btVector3 jointAxis = m_jointAxis;
109 if (m_rigidBodyA)
110 {
114
115 } else if (m_bodyA) {
116 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
119 }
120 btVector3 pivotBworld = m_pivotInB;
121 btMatrix3x3 frameBworld = m_frameInB;
122 if (m_rigidBodyB)
123 {
126
127 } else if (m_bodyB) {
128 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
130 }
131
132 btVector3 constraintAxis[2];
133 for (int i = 0; i < 3; ++i)
134 {
135 constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
136 if (constraintAxis[0].safeNorm() > EPSILON)
137 {
138 constraintAxis[0] = constraintAxis[0].normalized();
139 constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
140 constraintAxis[1] = constraintAxis[1].normalized();
141 break;
142 }
143 }
144
145 btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
146 btVector3 angleDiff;
148
149 int numDim = BTMBSLIDERCONSTRAINT_DIM;
150 for (int i=0;i<numDim;i++)
151 {
152 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
153 constraintRow.m_orgConstraint = this;
154 constraintRow.m_orgDofIndex = i;
155 constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
156 constraintRow.m_contactNormal1.setValue(0,0,0);
157 constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
158 constraintRow.m_contactNormal2.setValue(0,0,0);
159 constraintRow.m_angularComponentA.setValue(0,0,0);
160 constraintRow.m_angularComponentB.setValue(0,0,0);
161
162 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
163 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
164
165 if (m_rigidBodyA)
166 {
168 }
169 if (m_rigidBodyB)
170 {
172 }
173
174 btVector3 constraintNormalLin(0,0,0);
175 btVector3 constraintNormalAng(0,0,0);
176 btScalar posError = 0.0;
177 if (i < 2) {
178 constraintNormalLin = constraintAxis[i];
179 posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
180 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
181 constraintNormalLin, pivotAworld, pivotBworld,
182 posError,
183 infoGlobal,
185 );
186 }
187 else { //i>=2
188 constraintNormalAng = frameAworld.getColumn(i%3);
189 posError = angleDiff[i%3];
190 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
191 constraintNormalLin, pivotAworld, pivotBworld,
192 posError,
193 infoGlobal,
195 );
196 }
197 }
198}
199
201{
202 btTransform tr;
203 tr.setIdentity();
204
205 if (m_rigidBodyA)
206 {
208 tr.setOrigin(pivot);
209 drawer->drawTransform(tr, 0.1);
210 }
211 if (m_bodyA)
212 {
214 tr.setOrigin(pivotAworld);
215 drawer->drawTransform(tr, 0.1);
216 }
217 if (m_rigidBodyB)
218 {
219 // that ideally should draw the same frame
221 tr.setOrigin(pivot);
222 drawer->drawTransform(tr, 0.1);
223 }
224 if (m_bodyB)
225 {
227 tr.setOrigin(pivotBworld);
228 drawer->drawTransform(tr, 0.1);
229 }
230}
#define BTMBSLIDERCONSTRAINT_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:878
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define btAssert(x)
Definition: btScalar.h:131
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:30
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
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
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)
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)
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:164
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btVector3 localDirToWorld(int i, const btVector3 &vec) const
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
btQuaternion getOrientation() const
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint