2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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:
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.
16 ///This file was written by Erwin Coumans
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"
24 #define BTMBSLIDERCONSTRAINT_DIM 5
25 #define EPSILON 0.000001
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, MULTIBODY_CONSTRAINT_SLIDER),
35 m_jointAxis(jointAxis)
37 m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
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),
48 m_jointAxis(jointAxis)
50 m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
53 void btMultiBodySliderConstraint::finalizeMultiDof()
59 btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
63 int btMultiBodySliderConstraint::getIslandIdA() const
66 return m_rigidBodyA->getIslandTag();
72 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
74 return col->getIslandTag();
78 if (m_bodyA->getLink(m_linkA).m_collider)
79 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
85 int btMultiBodySliderConstraint::getIslandIdB() const
88 return m_rigidBodyB->getIslandTag();
93 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
95 return col->getIslandTag();
99 if (m_bodyB->getLink(m_linkB).m_collider)
100 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
105 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
107 // Convert local points back to world
108 btVector3 pivotAworld = m_pivotInA;
109 btMatrix3x3 frameAworld = m_frameInA;
110 btVector3 jointAxis = m_jointAxis;
113 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
114 frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
115 jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
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);
123 btVector3 pivotBworld = m_pivotInB;
124 btMatrix3x3 frameBworld = m_frameInB;
127 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
128 frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
132 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
133 frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
136 btVector3 constraintAxis[2];
137 for (int i = 0; i < 3; ++i)
139 constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
140 if (constraintAxis[0].safeNorm() > EPSILON)
142 constraintAxis[0] = constraintAxis[0].normalized();
143 constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
144 constraintAxis[1] = constraintAxis[1].normalized();
149 btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
151 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
153 int numDim = BTMBSLIDERCONSTRAINT_DIM;
154 for (int i = 0; i < numDim; i++)
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);
166 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
167 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
171 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
175 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
178 btVector3 constraintNormalLin(0, 0, 0);
179 btVector3 constraintNormalAng(0, 0, 0);
180 btScalar posError = 0.0;
183 constraintNormalLin = constraintAxis[i];
184 posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
185 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
186 constraintNormalLin, pivotAworld, pivotBworld,
189 -m_maxAppliedImpulse, m_maxAppliedImpulse);
193 constraintNormalAng = frameAworld.getColumn(i % 3);
194 posError = angleDiff[i % 3];
195 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
196 constraintNormalLin, pivotAworld, pivotBworld,
199 -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
204 void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
211 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
213 drawer->drawTransform(tr, 0.1);
217 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
218 tr.setOrigin(pivotAworld);
219 drawer->drawTransform(tr, 0.1);
223 // that ideally should draw the same frame
224 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
226 drawer->drawTransform(tr, 0.1);
230 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
231 tr.setOrigin(pivotBworld);
232 drawer->drawTransform(tr, 0.1);