Bullet Collision Detection & Physics Library
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
23
24#define BTMBFIXEDCONSTRAINT_DIM 6
25
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
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
51{
52 //not implemented yet
53 btAssert(0);
54}
55
57{
58}
59
61{
62 if (m_rigidBodyA)
63 return m_rigidBodyA->getIslandTag();
64
65 if (m_bodyA)
66 {
67 if (m_linkA < 0)
68 {
70 if (col)
71 return col->getIslandTag();
72 }
73 else
74 {
77 }
78 }
79 return -1;
80}
81
83{
84 if (m_rigidBodyB)
85 return m_rigidBodyB->getIslandTag();
86 if (m_bodyB)
87 {
88 if (m_linkB < 0)
89 {
91 if (col)
92 return col->getIslandTag();
93 }
94 else
95 {
98 }
99 }
100 return -1;
101}
102
104{
106 for (int i = 0; i < numDim; i++)
107 {
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
124 if (m_rigidBodyA)
125 {
126 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
129 }
130 else
131 {
132 if (m_bodyA)
133 {
136 }
137 }
140 if (m_rigidBodyB)
141 {
142 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
145 }
146 else
147 {
148 if (m_bodyB)
149 {
152 }
153 }
154
158
161 btScalar posError = 0.0;
162 if (i < 3)
163 {
164 constraintNormalLin[i] = 1;
168 posError,
171 }
172 else
173 { //i>=3
174 constraintNormalAng = frameAworld.getColumn(i % 3);
175 posError = angleDiff[i % 3];
178 posError,
181 }
182 }
183}
184
186{
188 tr.setIdentity();
189
190 if (m_rigidBodyA)
191 {
193 tr.setOrigin(pivot);
194 drawer->drawTransform(tr, 0.1);
195 }
196 if (m_bodyA)
197 {
199 tr.setOrigin(pivotAworld);
200 drawer->drawTransform(tr, 0.1);
201 }
202 if (m_rigidBodyB)
203 {
204 // that ideally should draw the same frame
206 tr.setOrigin(pivot);
207 drawer->drawTransform(tr, 0.1);
208 }
209 if (m_bodyB)
210 {
212 tr.setOrigin(pivotBworld);
213 drawer->drawTransform(tr, 0.1);
214 }
215}
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
@ MULTIBODY_CONSTRAINT_FIXED
#define BTMBFIXEDCONSTRAINT_DIM
This file was written by Erwin Coumans.
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define btAssert(x)
Definition btScalar.h:153
void resize(int newsize, const T &fillData=T())
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3 inverse() const
Return the inverse of the matrix.
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, btScalar damping=1.0)
virtual void debugDraw(class btIDebugDraw *drawer)
btMultiBodyFixedConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
const btMultibodyLink & getLink(int index) const
const btMultiBodyLinkCollider * getBaseCollider() const
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btQuaternion getOrientation() const
const btTransform & getCenterOfMassTransform() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
void setIdentity()
Set this transformation to the identity.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...