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

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
18 #include "btMultiBodyFixedConstraint.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 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
50 void btMultiBodyFixedConstraint::finalizeMultiDof()
51 {
52 //not implemented yet
53 btAssert(0);
54 }
55
56 btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
57 {
58 }
59
60
61 int btMultiBodyFixedConstraint::getIslandIdA() const
62 {
63 if (m_rigidBodyA)
64 return m_rigidBodyA->getIslandTag();
65
66 if (m_bodyA)
67 {
68 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
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
80 int btMultiBodyFixedConstraint::getIslandIdB() const
81 {
82 if (m_rigidBodyB)
83 return m_rigidBodyB->getIslandTag();
84 if (m_bodyB)
85 {
86 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
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
100 void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
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();
125 pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
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();
140 pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
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;
153 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,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,
165 -m_maxAppliedImpulse, m_maxAppliedImpulse
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,
175 -m_maxAppliedImpulse, m_maxAppliedImpulse, true
176 );
177 }
178 }
179 }
180
181 void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
182 {
183 btTransform tr;
184 tr.setIdentity();
185
186 if (m_rigidBodyA)
187 {
188 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
189 tr.setOrigin(pivot);
190 drawer->drawTransform(tr, 0.1);
191 }
192 if (m_bodyA)
193 {
194 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
195 tr.setOrigin(pivotAworld);
196 drawer->drawTransform(tr, 0.1);
197 }
198 if (m_rigidBodyB)
199 {
200 // that ideally should draw the same frame
201 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
202 tr.setOrigin(pivot);
203 drawer->drawTransform(tr, 0.1);
204 }
205 if (m_bodyB)
206 {
207 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
208 tr.setOrigin(pivotBworld);
209 drawer->drawTransform(tr, 0.1);
210 }
211 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
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:433
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:164
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:652
btMultiBodyFixedConstraint::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyFixedConstraint.h:29
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
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:131
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:131
btMultiBodyFixedConstraint::m_frameInA
btMatrix3x3 m_frameInA
Definition: btMultiBodyFixedConstraint.h:31
btMultiBodyFixedConstraint::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodyFixedConstraint.h:28
btRigidBody::getOrientation
btQuaternion getOrientation() const
Definition: btRigidBody.cpp:405
btMultiBody
Definition: btMultiBody.h:51
btRigidBody.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btMultiBodyFixedConstraint.h
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btMultiBodyFixedConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyFixedConstraint.cpp:80
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
Definition: btMultiBody.cpp:510
btMultiBodyFixedConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyFixedConstraint.cpp:100
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint
virtual ~btMultiBodyFixedConstraint()
Definition: btMultiBodyFixedConstraint.cpp:56
BTMBFIXEDCONSTRAINT_DIM
#define BTMBFIXEDCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodyFixedConstraint.cpp:24
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
btMultiBodyFixedConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyFixedConstraint.cpp:61
btMultiBodyFixedConstraint::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyFixedConstraint.h: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:69
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:443
btIDebugDraw.h
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
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
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:878
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint
btMultiBodyFixedConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
Definition: btMultiBodyFixedConstraint.cpp:26
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:1003
btMultiBodyFixedConstraint::m_frameInB
btMatrix3x3 m_frameInB
Definition: btMultiBodyFixedConstraint.h:32
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
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:453
btMultiBodyFixedConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyFixedConstraint.cpp:181
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btMultiBodyFixedConstraint::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodyFixedConstraint.h:30
btMultiBodyFixedConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyFixedConstraint.cpp:50
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48