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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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 }
37
38 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
39 :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
40 m_rigidBodyA(0),
41 m_rigidBodyB(0),
42 m_pivotInA(pivotInA),
43 m_pivotInB(pivotInB)
44 {
45 }
46
47
48 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
49 {
50 }
51
52
53 int btMultiBodyPoint2Point::getIslandIdA() const
54 {
55 if (m_rigidBodyA)
56 return m_rigidBodyA->getIslandTag();
57
58 if (m_bodyA)
59 {
60 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
61 if (col)
62 return col->getIslandTag();
63 for (int i=0;i<m_bodyA->getNumLinks();i++)
64 {
65 if (m_bodyA->getLink(i).m_collider)
66 return m_bodyA->getLink(i).m_collider->getIslandTag();
67 }
68 }
69 return -1;
70 }
71
72 int btMultiBodyPoint2Point::getIslandIdB() const
73 {
74 if (m_rigidBodyB)
75 return m_rigidBodyB->getIslandTag();
76 if (m_bodyB)
77 {
78 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
79 if (col)
80 return col->getIslandTag();
81
82 for (int i=0;i<m_bodyB->getNumLinks();i++)
83 {
84 col = m_bodyB->getLink(i).m_collider;
85 if (col)
86 return col->getIslandTag();
87 }
88 }
89 return -1;
90 }
91
92
93
94 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
95 btMultiBodyJacobianData& data,
96 const btContactSolverInfo& infoGlobal)
97 {
98
99 // int i=1;
100 int numDim = BTMBP2PCONSTRAINT_DIM;
101 for (int i=0;i<numDim;i++)
102 {
103
104 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
105 //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
106 constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
107 constraintRow.m_contactNormal1.setValue(0,0,0);
108 constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
109 constraintRow.m_contactNormal2.setValue(0,0,0);
110 constraintRow.m_angularComponentA.setValue(0,0,0);
111 constraintRow.m_angularComponentB.setValue(0,0,0);
112
113 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
114 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
115
116 btVector3 contactNormalOnB(0,0,0);
117 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
118 contactNormalOnB[i] = -1;
119 #else
120 contactNormalOnB[i%3] = -1;
121 #endif
122
123
124 // Convert local points back to world
125 btVector3 pivotAworld = m_pivotInA;
126 if (m_rigidBodyA)
127 {
128
129 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
130 pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
131 } else
132 {
133 if (m_bodyA)
134 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
135 }
136 btVector3 pivotBworld = m_pivotInB;
137 if (m_rigidBodyB)
138 {
139 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
140 pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
141 } else
142 {
143 if (m_bodyB)
144 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
145
146 }
147
148 btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
149
150 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
151
152
153 fillMultiBodyConstraint(constraintRow, data, 0, 0,
154 contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
155 posError,
156 infoGlobal,
157 -m_maxAppliedImpulse, m_maxAppliedImpulse
158 );
159 //@todo: support the case of btMultiBody versus btRigidBody,
160 //see btPoint2PointConstraint::getInfo2NonVirtual
161 #else
162 const btVector3 dummy(0, 0, 0);
163
164 btAssert(m_bodyA->isMultiDof());
165
166 btScalar* jac1 = jacobianA(i);
167 const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
168 const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
169
170 m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
171
172 fillMultiBodyConstraint(constraintRow, data, jac1, 0,
173 dummy, dummy, dummy, //sucks but let it be this way "for the time being"
174 posError,
175 infoGlobal,
176 -m_maxAppliedImpulse, m_maxAppliedImpulse
177 );
178 #endif
179 }
180 }
181
182 void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
183 {
184 btTransform tr;
185 tr.setIdentity();
186
187 if (m_rigidBodyA)
188 {
189 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
190 tr.setOrigin(pivot);
191 drawer->drawTransform(tr, 0.1);
192 }
193 if (m_bodyA)
194 {
195 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
196 tr.setOrigin(pivotAworld);
197 drawer->drawTransform(tr, 0.1);
198 }
199 if (m_rigidBodyB)
200 {
201 // that ideally should draw the same frame
202 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
203 tr.setOrigin(pivot);
204 drawer->drawTransform(tr, 0.1);
205 }
206 if (m_bodyB)
207 {
208 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
209 tr.setOrigin(pivotBworld);
210 drawer->drawTransform(tr, 0.1);
211 }
212 }
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btMultiBodyConstraint::fillMultiBodyConstraint
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &contactNormalOnB, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraint.cpp:55
btMultiBodyPoint2Point::~btMultiBodyPoint2Point
virtual ~btMultiBodyPoint2Point()
Definition: btMultiBodyPoint2Point.cpp:48
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
btMultiBodyPoint2Point::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyPoint2Point.cpp:182
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:39
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:27
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:42
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:640
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:45
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:145
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:46
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:128
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btMultiBodyPoint2Point::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyPoint2Point.cpp:53
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:130
btMultiBodyPoint2Point::btMultiBodyPoint2Point
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
Definition: btMultiBodyPoint2Point.cpp:29
btMultiBody
Definition: btMultiBody.h:37
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
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btMultiBodyPoint2Point.h
btMultiBodyPoint2Point::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyPoint2Point.h:29
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBody::filConstraintJacobianMultiDof
void filConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.cpp:2626
btMultiBodyPoint2Point::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyPoint2Point.cpp:94
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:28
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyLinkCollider.h
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:408
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
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:72
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:398
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:137
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:41
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
btMultiBodyPoint2Point::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyPoint2Point.cpp:72
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:522
btMultiBodyPoint2Point::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyPoint2Point.h:31
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &vec) const
Definition: btMultiBody.cpp:432
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:68
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:247
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:40
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48