Bullet Collision Detection & Physics Library: btMultiBodySliderConstraint.cpp Source File

Bullet Collision Detection & Physics Library

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
18 #include "btMultiBodySliderConstraint.h"
19 #include "btMultiBodyLinkCollider.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
22 #include "LinearMath/btIDebugDraw.h"
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)
28 :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
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
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),
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
53 void btMultiBodySliderConstraint::finalizeMultiDof()
54 {
55 //not implemented yet
56 btAssert(0);
57 }
58
59 btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
60 {
61 }
62
63
64 int btMultiBodySliderConstraint::getIslandIdA() const
65 {
66 if (m_rigidBodyA)
67 return m_rigidBodyA->getIslandTag();
68
69 if (m_bodyA)
70 {
71 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
72 if (col)
73 return col->getIslandTag();
74 for (int i=0;i<m_bodyA->getNumLinks();i++)
75 {
76 if (m_bodyA->getLink(i).m_collider)
77 return m_bodyA->getLink(i).m_collider->getIslandTag();
78 }
79 }
80 return -1;
81 }
82
83 int btMultiBodySliderConstraint::getIslandIdB() const
84 {
85 if (m_rigidBodyB)
86 return m_rigidBodyB->getIslandTag();
87 if (m_bodyB)
88 {
89 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
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
103 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
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 {
111 pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
112 frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
113 jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis);
114
115 } else if (m_bodyA) {
116 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
117 frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
118 jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
119 }
120 btVector3 pivotBworld = m_pivotInB;
121 btMatrix3x3 frameBworld = m_frameInB;
122 if (m_rigidBodyB)
123 {
124 pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
125 frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
126
127 } else if (m_bodyB) {
128 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
129 frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
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;
147 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,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 {
167 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
168 }
169 if (m_rigidBodyB)
170 {
171 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
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,
184 -m_maxAppliedImpulse, m_maxAppliedImpulse
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,
194 -m_maxAppliedImpulse, m_maxAppliedImpulse, true
195 );
196 }
197 }
198 }
199
200 void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
201 {
202 btTransform tr;
203 tr.setIdentity();
204
205 if (m_rigidBodyA)
206 {
207 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
208 tr.setOrigin(pivot);
209 drawer->drawTransform(tr, 0.1);
210 }
211 if (m_bodyA)
212 {
213 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
214 tr.setOrigin(pivotAworld);
215 drawer->drawTransform(tr, 0.1);
216 }
217 if (m_rigidBodyB)
218 {
219 // that ideally should draw the same frame
220 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
221 tr.setOrigin(pivot);
222 drawer->drawTransform(tr, 0.1);
223 }
224 if (m_bodyB)
225 {
226 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
227 tr.setOrigin(pivotBworld);
228 drawer->drawTransform(tr, 0.1);
229 }
230 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btMultiBodySliderConstraint::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodySliderConstraint.h:30
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:81
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:428
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:155
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btMultiBodySliderConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodySliderConstraint.cpp:83
btMultiBodySliderConstraint::~btMultiBodySliderConstraint
virtual ~btMultiBodySliderConstraint()
Definition: btMultiBodySliderConstraint.cpp:59
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:46
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:47
EPSILON
#define EPSILON
Definition: btMultiBodySliderConstraint.cpp:25
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:121
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btAssert
#define btAssert(x)
Definition: btScalar.h:114
BTMBSLIDERCONSTRAINT_DIM
#define BTMBSLIDERCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodySliderConstraint.cpp:24
btRigidBody::getOrientation
btQuaternion getOrientation() const
Definition: btRigidBody.cpp:405
btMultiBody
Definition: btMultiBody.h:51
btRigidBody.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:887
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:962
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
Definition: btMultiBody.cpp:476
btMultiBodySliderConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodySliderConstraint.cpp:200
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:387
btMultiBodySliderConstraint::m_frameInB
btMatrix3x3 m_frameInB
Definition: btMultiBodySliderConstraint.h:32
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btMultiBodySliderConstraint::btMultiBodySliderConstraint
btMultiBodySliderConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB, const btVector3 &jointAxis)
Definition: btMultiBodySliderConstraint.cpp:27
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:66
btMultiBodyLinkCollider.h
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBodySliderConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodySliderConstraint.cpp:103
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:438
btIDebugDraw.h
btMultiBodySliderConstraint::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodySliderConstraint.h:28
btMultiBody::localDirToWorld
btVector3 localDirToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:456
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btGeneric6DofSpring2Constraint.h
btMultiBodySliderConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodySliderConstraint.cpp:64
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodySliderConstraint::m_frameInA
btMatrix3x3 m_frameInA
Definition: btMultiBodySliderConstraint.h:31
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
btMultiBodySliderConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodySliderConstraint.cpp:53
btMultiBodySliderConstraint::m_jointAxis
btVector3 m_jointAxis
Definition: btMultiBodySliderConstraint.h:33
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:55
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:245
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultiBodySliderConstraint::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodySliderConstraint.h:29
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:448
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btMultiBodySliderConstraint.h
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48
btMultiBodySliderConstraint::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodySliderConstraint.h:27