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

Bullet Collision Detection & Physics Library

btMultiBodyPoint2Point.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
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
29btMultiBodyPoint2Point::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),
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, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
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
55btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
56{
57}
58
59 int btMultiBodyPoint2Point::getIslandIdA() const
60{
61 if (m_rigidBodyA)
62 return m_rigidBodyA->getIslandTag();
63
64 if (m_bodyA)
65 {
66 if (m_linkA < 0)
67 {
68 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
69 if (col)
70 return col->getIslandTag();
71 }
72 else
73 {
74 if (m_bodyA->getLink(m_linkA).m_collider)
75 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
76 }
77 }
78 return -1;
79}
80
81 int btMultiBodyPoint2Point::getIslandIdB() const
82{
83 if (m_rigidBodyB)
84 return m_rigidBodyB->getIslandTag();
85 if (m_bodyB)
86 {
87 if (m_linkB < 0)
88 {
89 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
90 if (col)
91 return col->getIslandTag();
92 }
93 else
94 {
95 if (m_bodyB->getLink(m_linkB).m_collider)
96 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
97 }
98 }
99 return -1;
100}
101
102 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
103 btMultiBodyJacobianData& data,
104 const btContactSolverInfo& infoGlobal)
105{
106 // int i=1;
107 int numDim = BTMBP2PCONSTRAINT_DIM;
108 for (int i = 0; i < numDim; i++)
109 {
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);
120
121 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
122 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
123
124 btVector3 contactNormalOnB(0, 0, 0);
125#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
126 contactNormalOnB[i] = -1;
127#else
128 contactNormalOnB[i % 3] = -1;
129#endif
130
131 // Convert local points back to world
132 btVector3 pivotAworld = m_pivotInA;
133 if (m_rigidBodyA)
134 {
135 constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
136 pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
137 }
138 else
139 {
140 if (m_bodyA)
141 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
142 }
143 btVector3 pivotBworld = m_pivotInB;
144 if (m_rigidBodyB)
145 {
146 constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
147 pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
148 }
149 else
150 {
151 if (m_bodyB)
152 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
153 }
154
155 btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
156
157#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
158
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"
161 posError,
162 infoGlobal,
163 -m_maxAppliedImpulse, m_maxAppliedImpulse);
164 //@todo: support the case of btMultiBody versus btRigidBody,
165 //see btPoint2PointConstraint::getInfo2NonVirtual
166#else
167 const btVector3 dummy(0, 0, 0);
168
169 btAssert(m_bodyA->isMultiDof());
170
171 btScalar* jac1 = jacobianA(i);
172 const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
173 const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
174
175 m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
176
177 fillMultiBodyConstraint(constraintRow, data, jac1, 0,
178 dummy, dummy, dummy, //sucks but let it be this way "for the time being"
179 posError,
180 infoGlobal,
181 -m_maxAppliedImpulse, m_maxAppliedImpulse);
182#endif
183 }
184}
185
186 void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
187{
188 btTransform tr;
189 tr.setIdentity();
190
191 if (m_rigidBodyA)
192 {
193 btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
194 tr.setOrigin(pivot);
195 drawer->drawTransform(tr, 0.1);
196 }
197 if (m_bodyA)
198 {
199 btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
200 tr.setOrigin(pivotAworld);
201 drawer->drawTransform(tr, 0.1);
202 }
203 if (m_rigidBodyB)
204 {
205 // that ideally should draw the same frame
206 btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
207 tr.setOrigin(pivot);
208 drawer->drawTransform(tr, 0.1);
209 }
210 if (m_bodyB)
211 {
212 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
213 tr.setOrigin(pivotBworld);
214 drawer->drawTransform(tr, 0.1);
215 }
216}
btIDebugDraw.h
MULTIBODY_CONSTRAINT_POINT_TO_POINT
@ MULTIBODY_CONSTRAINT_POINT_TO_POINT
Definition: btMultiBodyConstraint.h:30
btMultiBodyLinkCollider.h
BTMBP2PCONSTRAINT_DIM
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
Definition: btMultiBodyPoint2Point.cpp:24
btMultiBodyPoint2Point.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btRigidBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btAlignedObjectArray< btMultiBodySolverConstraint >
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:448
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:458
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:57
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:80
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:60
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:181
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:73
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, btScalar damping=1.0)
Definition: btMultiBodyConstraint.cpp:53
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:59
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:62
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:61
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btMultiBodyPoint2Point::m_rigidBodyA
btRigidBody * m_rigidBodyA
Definition: btMultiBodyPoint2Point.h:29
btMultiBodyPoint2Point::btMultiBodyPoint2Point
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
Definition: btMultiBodyPoint2Point.cpp:29
btMultiBodyPoint2Point::~btMultiBodyPoint2Point
virtual ~btMultiBodyPoint2Point()
Definition: btMultiBodyPoint2Point.cpp:55
btMultiBodyPoint2Point::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyPoint2Point.cpp:49
btMultiBodyPoint2Point::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyPoint2Point.cpp:81
btMultiBodyPoint2Point::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodyPoint2Point.cpp:186
btMultiBodyPoint2Point::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyPoint2Point.cpp:102
btMultiBodyPoint2Point::m_rigidBodyB
btRigidBody * m_rigidBodyB
Definition: btMultiBodyPoint2Point.h:30
btMultiBodyPoint2Point::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyPoint2Point.cpp:59
btMultiBodyPoint2Point::m_pivotInB
btVector3 m_pivotInB
Definition: btMultiBodyPoint2Point.h:32
btMultiBodyPoint2Point::m_pivotInA
btVector3 m_pivotInA
Definition: btMultiBodyPoint2Point.h:31
btMultiBody
Definition: btMultiBody.h:51
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
Definition: btMultiBody.cpp:470
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:167
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:147
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btContactSolverInfo
Definition: btContactSolverInfo.h:76
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:44
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:52
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:48
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:50
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:49
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:30
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:47
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132