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 "btMultiBodyPoint2Point.h"
19 #include "btMultiBodyLinkCollider.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "LinearMath/btIDebugDraw.h"
23 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
24 #define BTMBP2PCONSTRAINT_DIM 3
26 #define BTMBP2PCONSTRAINT_DIM 6
29 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
30 : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
36 m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
39 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
40 : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
46 m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
49 void btMultiBodyPoint2Point::finalizeMultiDof()
55 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
59 int btMultiBodyPoint2Point::getIslandIdA() const
62 return m_rigidBodyA->getIslandTag();
68 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
70 return col->getIslandTag();
74 if (m_bodyA->getLink(m_linkA).m_collider)
75 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
81 int btMultiBodyPoint2Point::getIslandIdB() const
84 return m_rigidBodyB->getIslandTag();
89 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
91 return col->getIslandTag();
95 if (m_bodyB->getLink(m_linkB).m_collider)
96 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
102 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
103 btMultiBodyJacobianData& data,
104 const btContactSolverInfo& infoGlobal)
107 int numDim = BTMBP2PCONSTRAINT_DIM;
108 for (int i = 0; i < numDim; i++)
110 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
111 //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
112 constraintRow.m_orgConstraint = this;
113 constraintRow.m_orgDofIndex = i;
114 constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
115 constraintRow.m_contactNormal1.setValue(0, 0, 0);
116 constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
117 constraintRow.m_contactNormal2.setValue(0, 0, 0);
118 constraintRow.m_angularComponentA.setValue(0, 0, 0);
119 constraintRow.m_angularComponentB.setValue(0, 0, 0);
121 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
122 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
124 btVector3 contactNormalOnB(0, 0, 0);
125 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
126 contactNormalOnB[i] = -1;
128 contactNormalOnB[i % 3] = -1;
131 // Convert local points back to world
132 btVector3 pivotAworld = m_pivotInA;
135 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
136 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
141 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
143 btVector3 pivotBworld = m_pivotInB;
146 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
147 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
152 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
155 btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
157 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
159 fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
160 contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
163 -m_maxAppliedImpulse, m_maxAppliedImpulse);
164 //@todo: support the case of btMultiBody versus btRigidBody,
165 //see btPoint2PointConstraint::getInfo2NonVirtual
167 const btVector3 dummy(0, 0, 0);
169 btAssert(m_bodyA->isMultiDof());
171 btScalar* jac1 = jacobianA(i);
172 const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
173 const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
175 m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
177 fillMultiBodyConstraint(constraintRow, data, jac1, 0,
178 dummy, dummy, dummy, //sucks but let it be this way "for the time being"
181 -m_maxAppliedImpulse, m_maxAppliedImpulse);
186 void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
193 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
195 drawer->drawTransform(tr, 0.1);
199 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
200 tr.setOrigin(pivotAworld);
201 drawer->drawTransform(tr, 0.1);
205 // that ideally should draw the same frame
206 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
208 drawer->drawTransform(tr, 0.1);
212 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
213 tr.setOrigin(pivotBworld);
214 drawer->drawTransform(tr, 0.1);