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

Bullet Collision Detection & Physics Library

btMultiBodyPoint2Point.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 "btMultiBodyPoint2Point.h"
19 #include "btMultiBodyLinkCollider.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "LinearMath/btIDebugDraw.h"
22
23 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
24 #define BTMBP2PCONSTRAINT_DIM 3
25 #else
26 #define BTMBP2PCONSTRAINT_DIM 6
27 #endif
28
29 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
30 :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
31 m_rigidBodyA(0),
32 m_rigidBodyB(bodyB),
33 m_pivotInA(pivotInA),
34 m_pivotInB(pivotInB)
35 {
36 m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
37 }
38
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),
41 m_rigidBodyA(0),
42 m_rigidBodyB(0),
43 m_pivotInA(pivotInA),
44 m_pivotInB(pivotInB)
45 {
46 m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
47 }
48
49 void btMultiBodyPoint2Point::finalizeMultiDof()
50 {
51 //not implemented yet
52 btAssert(0);
53 }
54
55 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
56 {
57 }
58
59
60 int btMultiBodyPoint2Point::getIslandIdA() const
61 {
62 if (m_rigidBodyA)
63 return m_rigidBodyA->getIslandTag();
64
65 if (m_bodyA)
66 {
67 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
68 if (col)
69 return col->getIslandTag();
70 for (int i=0;i<m_bodyA->getNumLinks();i++)
71 {
72 if (m_bodyA->getLink(i).m_collider)
73 return m_bodyA->getLink(i).m_collider->getIslandTag();
74 }
75 }
76 return -1;
77 }
78
79 int btMultiBodyPoint2Point::getIslandIdB() const
80 {
81 if (m_rigidBodyB)
82 return m_rigidBodyB->getIslandTag();
83 if (m_bodyB)
84 {
85 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
86 if (col)
87 return col->getIslandTag();
88
89 for (int i=0;i<m_bodyB->getNumLinks();i++)
90 {
91 col = m_bodyB->getLink(i).m_collider;
92 if (col)
93 return col->getIslandTag();
94 }
95 }
96 return -1;
97 }
98
99
100
101 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
102 btMultiBodyJacobianData& data,
103 const btContactSolverInfo& infoGlobal)
104 {
105
106 // int i=1;
107 int numDim = BTMBP2PCONSTRAINT_DIM;
108 for (int i=0;i<numDim;i++)
109 {
110
111 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
112 //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
113 constraintRow.m_orgConstraint = this;
114 constraintRow.m_orgDofIndex = i;
115 constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
116 constraintRow.m_contactNormal1.setValue(0,0,0);
117 constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
118 constraintRow.m_contactNormal2.setValue(0,0,0);
119 constraintRow.m_angularComponentA.setValue(0,0,0);
120 constraintRow.m_angularComponentB.setValue(0,0,0);
121
122 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
123 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
124
125 btVector3 contactNormalOnB(0,0,0);
126 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
127 contactNormalOnB[i] = -1;
128 #else
129 contactNormalOnB[i%3] = -1;
130 #endif
131
132
133 // Convert local points back to world
134 btVector3 pivotAworld = m_pivotInA;
135 if (m_rigidBodyA)
136 {
137
138 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
139 pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
140 } else
141 {
142 if (m_bodyA)
143 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
144 }
145 btVector3 pivotBworld = m_pivotInB;
146 if (m_rigidBodyB)
147 {
148 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
149 pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
150 } else
151 {
152 if (m_bodyB)
153 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
154
155 }
156
157 btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
158
159 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
160
161
162 fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
163 contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
164 posError,
165 infoGlobal,
166 -m_maxAppliedImpulse, m_maxAppliedImpulse
167 );
168 //@todo: support the case of btMultiBody versus btRigidBody,
169 //see btPoint2PointConstraint::getInfo2NonVirtual
170 #else
171 const btVector3 dummy(0, 0, 0);
172
173 btAssert(m_bodyA->isMultiDof());
174
175 btScalar* jac1 = jacobianA(i);
176 const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
177 const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
178
179 m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
180
181 fillMultiBodyConstraint(constraintRow, data, jac1, 0,
182 dummy, dummy, dummy, //sucks but let it be this way "for the time being"
183 posError,
184 infoGlobal,
185 -m_maxAppliedImpulse, m_maxAppliedImpulse
186 );
187 #endif
188 }
189 }
190
191 void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
192 {
193 btTransform tr;
194 tr.setIdentity();
195
196 if (m_rigidBodyA)
197 {
198 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
199 tr.setOrigin(pivot);
200 drawer->drawTransform(tr, 0.1);
201 }
202 if (m_bodyA)
203 {
204 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
205 tr.setOrigin(pivotAworld);
206 drawer->drawTransform(tr, 0.1);
207 }
208 if (m_rigidBodyB)
209 {
210 // that ideally should draw the same frame
211 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
212 tr.setOrigin(pivot);
213 drawer->drawTransform(tr, 0.1);
214 }
215 if (m_bodyB)
216 {
217 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
218 tr.setOrigin(pivotBworld);
219 drawer->drawTransform(tr, 0.1);
220 }
221 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:431
btMultiBodyPoint2Point::~btMultiBodyPoint2Point
virtual ~btMultiBodyPoint2Point()
Definition: btMultiBodyPoint2Point.cpp:55
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:155
btMultiBodyPoint2Point::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyPoint2Point.cpp:191
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
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btMultiBodyPoint2Point::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodyPoint2Point.h:32
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:121
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:114
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:158
btMultiBodyPoint2Point::btMultiBodyPoint2Point
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
Definition: btMultiBodyPoint2Point.cpp:29
btMultiBody
Definition: btMultiBody.h:51
btRigidBody.h
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btMultiBodyPoint2Point::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodyPoint2Point.h:30
btMultiBodyPoint2Point.h
btMultiBodyPoint2Point::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyPoint2Point.h:29
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBodyPoint2Point::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyPoint2Point.cpp:101
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
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
btMultiBodyPoint2Point::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyPoint2Point.cpp:49
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btMultiBodyPoint2Point::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyPoint2Point.cpp:60
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:441
BTMBP2PCONSTRAINT_DIM
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodyPoint2Point.cpp:24
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
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
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
btMultiBodyPoint2Point::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyPoint2Point.h:31
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
btMultiBodyPoint2Point::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyPoint2Point.cpp:79
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:451
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48