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

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
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
27btMultiBodySliderConstraint::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, MULTIBODY_CONSTRAINT_SLIDER),
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, MULTIBODY_CONSTRAINT_SLIDER),
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
59btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
60{
61}
62
63 int btMultiBodySliderConstraint::getIslandIdA() const
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
85 int btMultiBodySliderConstraint::getIslandIdB() const
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}
105 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
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 {
113 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
114 frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
115 jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
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 {
127 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
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;
151 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, 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,
189 -m_maxAppliedImpulse, m_maxAppliedImpulse);
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,
199 -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
200 }
201 }
202}
203
204 void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
205{
206 btTransform tr;
207 tr.setIdentity();
208
209 if (m_rigidBodyA)
210 {
211 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
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
224 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
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}
EPSILON
#define EPSILON
btGeneric6DofSpring2Constraint.h
btIDebugDraw.h
MULTIBODY_CONSTRAINT_SLIDER
@ MULTIBODY_CONSTRAINT_SLIDER
Definition btMultiBodyConstraint.h:31
btMultiBodyLinkCollider.h
BTMBSLIDERCONSTRAINT_DIM
#define BTMBSLIDERCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition btMultiBodySliderConstraint.cpp:24
btMultiBodySliderConstraint.h
btMultiBodyConstraintArray
btAlignedObjectArray< btMultiBodySolverConstraint > btMultiBodyConstraintArray
Definition btMultiBodySolverConstraint.h:88
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:888
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition btQuaternion.h:926
btRigidBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btAssert
#define btAssert(x)
Definition btScalar.h:153
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition btCollisionObject.h:448
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:69
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition btIDebugDraw.h:27
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition btIDebugDraw.h:163
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition btMatrix3x3.h:1093
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition btMatrix3x3.h:142
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition btMultiBodyConstraint.h:80
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition btMultiBodyConstraint.h:60
btMultiBodyConstraint::btMultiBodyConstraint
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
Definition btMultiBodyConstraint.cpp:5
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition btMultiBodyConstraint.h:73
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, btScalar damping=1.0)
Definition btMultiBodyConstraint.cpp:53
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition btMultiBodyConstraint.h:59
btMultiBodyConstraint::m_linkB
int m_linkB
Definition btMultiBodyConstraint.h:62
btMultiBodyConstraint::m_linkA
int m_linkA
Definition btMultiBodyConstraint.h:61
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btMultiBodySliderConstraint::m_pivotInB
btVector3 m_pivotInB
Definition btMultiBodySliderConstraint.h:29
btMultiBodySliderConstraint::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition btMultiBodySliderConstraint.h:26
btMultiBodySliderConstraint::m_jointAxis
btVector3 m_jointAxis
Definition btMultiBodySliderConstraint.h:32
btMultiBodySliderConstraint::m_pivotInA
btVector3 m_pivotInA
Definition btMultiBodySliderConstraint.h:28
btMultiBodySliderConstraint::~btMultiBodySliderConstraint
virtual ~btMultiBodySliderConstraint()
Definition btMultiBodySliderConstraint.cpp:59
btMultiBodySliderConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition btMultiBodySliderConstraint.cpp:105
btMultiBodySliderConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition btMultiBodySliderConstraint.cpp:204
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
btMultiBodySliderConstraint::m_frameInB
btMatrix3x3 m_frameInB
Definition btMultiBodySliderConstraint.h:31
btMultiBodySliderConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition btMultiBodySliderConstraint.cpp:85
btMultiBodySliderConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition btMultiBodySliderConstraint.cpp:63
btMultiBodySliderConstraint::m_frameInA
btMatrix3x3 m_frameInA
Definition btMultiBodySliderConstraint.h:30
btMultiBodySliderConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition btMultiBodySliderConstraint.cpp:53
btMultiBodySliderConstraint::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition btMultiBodySliderConstraint.h:27
btMultiBody
Definition btMultiBody.h:51
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition btTransform.h:167
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition btTransform.h:147
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:380
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition btVector3.h:949
btContactSolverInfo
Definition btContactSolverInfo.h:76
btMultiBodyJacobianData
Definition btMultiBodyConstraint.h:44
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition btMultiBodyConstraint.h:52
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition btMultiBodySolverConstraint.h:30
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition btMultiBodySolverConstraint.h:78
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition btMultiBodySolverConstraint.h:42
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition btMultiBodySolverConstraint.h:45
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition btMultiBodySolverConstraint.h:79
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition btMultiBodySolverConstraint.h:43
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition btMultiBodySolverConstraint.h:47
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition btMultiBodySolverConstraint.h:44
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition btMultiBodySolverConstraint.h:73
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition btMultiBodySolverConstraint.h:48