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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyFixedConstraint.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 "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
26btMultiBodyFixedConstraint::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, MULTIBODY_CONSTRAINT_FIXED),
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, MULTIBODY_CONSTRAINT_FIXED),
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
56btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
57{
58}
59
60 int btMultiBodyFixedConstraint::getIslandIdA() const
61{
62 if (m_rigidBodyA)
63 return m_rigidBodyA->getIslandTag();
64
65 if (m_bodyA)
66 {
67 if (m_linkA < 0)
68 {
69 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
70 if (col)
71 return col->getIslandTag();
72 }
73 else
74 {
75 if (m_bodyA->getLink(m_linkA).m_collider)
76 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
77 }
78 }
79 return -1;
80}
81
82 int btMultiBodyFixedConstraint::getIslandIdB() const
83{
84 if (m_rigidBodyB)
85 return m_rigidBodyB->getIslandTag();
86 if (m_bodyB)
87 {
88 if (m_linkB < 0)
89 {
90 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
91 if (col)
92 return col->getIslandTag();
93 }
94 else
95 {
96 if (m_bodyB->getLink(m_linkB).m_collider)
97 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
98 }
99 }
100 return -1;
101}
102
103 void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
104{
105 int numDim = BTMBFIXEDCONSTRAINT_DIM;
106 for (int i = 0; i < numDim; i++)
107 {
108 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
109 constraintRow.m_orgConstraint = this;
110 constraintRow.m_orgDofIndex = i;
111 constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
112 constraintRow.m_contactNormal1.setValue(0, 0, 0);
113 constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
114 constraintRow.m_contactNormal2.setValue(0, 0, 0);
115 constraintRow.m_angularComponentA.setValue(0, 0, 0);
116 constraintRow.m_angularComponentB.setValue(0, 0, 0);
117
118 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
119 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
120
121 // Convert local points back to world
122 btVector3 pivotAworld = m_pivotInA;
123 btMatrix3x3 frameAworld = m_frameInA;
124 if (m_rigidBodyA)
125 {
126 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
127 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
128 frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
129 }
130 else
131 {
132 if (m_bodyA)
133 {
134 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
135 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
136 }
137 }
138 btVector3 pivotBworld = m_pivotInB;
139 btMatrix3x3 frameBworld = m_frameInB;
140 if (m_rigidBodyB)
141 {
142 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
143 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
144 frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
145 }
146 else
147 {
148 if (m_bodyB)
149 {
150 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
151 frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
152 }
153 }
154
155 btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
156 btVector3 angleDiff;
157 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
158
159 btVector3 constraintNormalLin(0, 0, 0);
160 btVector3 constraintNormalAng(0, 0, 0);
161 btScalar posError = 0.0;
162 if (i < 3)
163 {
164 constraintNormalLin[i] = 1;
165 posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
166 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
167 constraintNormalLin, pivotAworld, pivotBworld,
168 posError,
169 infoGlobal,
170 -m_maxAppliedImpulse, m_maxAppliedImpulse);
171 }
172 else
173 { //i>=3
174 constraintNormalAng = frameAworld.getColumn(i % 3);
175 posError = angleDiff[i % 3];
176 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
177 constraintNormalLin, pivotAworld, pivotBworld,
178 posError,
179 infoGlobal,
180 -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
181 }
182 }
183}
184
185 void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
186{
187 btTransform tr;
188 tr.setIdentity();
189
190 if (m_rigidBodyA)
191 {
192 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
193 tr.setOrigin(pivot);
194 drawer->drawTransform(tr, 0.1);
195 }
196 if (m_bodyA)
197 {
198 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
199 tr.setOrigin(pivotAworld);
200 drawer->drawTransform(tr, 0.1);
201 }
202 if (m_rigidBodyB)
203 {
204 // that ideally should draw the same frame
205 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
206 tr.setOrigin(pivot);
207 drawer->drawTransform(tr, 0.1);
208 }
209 if (m_bodyB)
210 {
211 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
212 tr.setOrigin(pivotBworld);
213 drawer->drawTransform(tr, 0.1);
214 }
215}
btGeneric6DofSpring2Constraint.h
btIDebugDraw.h
MULTIBODY_CONSTRAINT_FIXED
@ MULTIBODY_CONSTRAINT_FIXED
Definition: btMultiBodyConstraint.h:33
BTMBFIXEDCONSTRAINT_DIM
#define BTMBFIXEDCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodyFixedConstraint.cpp:24
btMultiBodyFixedConstraint.h
btMultiBodyLinkCollider.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
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< btMultiBodySolverConstraint >
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:448
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:458
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::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:57
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:80
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:60
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
btMultiBodyFixedConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyFixedConstraint.cpp:82
btMultiBodyFixedConstraint::m_frameInA
btMatrix3x3 m_frameInA
Definition: btMultiBodyFixedConstraint.h:30
btMultiBodyFixedConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyFixedConstraint.cpp:50
btMultiBodyFixedConstraint::m_frameInB
btMatrix3x3 m_frameInB
Definition: btMultiBodyFixedConstraint.h:31
btMultiBodyFixedConstraint::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyFixedConstraint.cpp:185
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
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint
virtual ~btMultiBodyFixedConstraint()
Definition: btMultiBodyFixedConstraint.cpp:56
btMultiBodyFixedConstraint::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyFixedConstraint.h:28
btMultiBodyFixedConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyFixedConstraint.cpp:103
btMultiBodyFixedConstraint::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyFixedConstraint.h:26
btMultiBodyFixedConstraint::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodyFixedConstraint.h:27
btMultiBodyFixedConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyFixedConstraint.cpp:60
btMultiBodyFixedConstraint::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodyFixedConstraint.h:29
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btMultiBody
Definition: btMultiBody.h:51
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
Definition: btMultiBody.cpp:554
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
Definition: btMultiBody.cpp:470
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::getOrientation
btQuaternion getOrientation() const
Definition: btRigidBody.cpp:392
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
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::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
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
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132