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"
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 
50 {
51  //not implemented yet
52  btAssert(0);
53 }
54 
56 {
57 }
58 
59 
61 {
62  if (m_rigidBodyA)
63  return m_rigidBodyA->getIslandTag();
64 
65  if (m_bodyA)
66  {
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 
80 {
81  if (m_rigidBodyB)
82  return m_rigidBodyB->getIslandTag();
83  if (m_bodyB)
84  {
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 
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();
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();
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,
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,
186  );
187 #endif
188  }
189 }
190 
192 {
193  btTransform tr;
194  tr.setIdentity();
195 
196  if (m_rigidBodyA)
197  {
199  tr.setOrigin(pivot);
200  drawer->drawTransform(tr, 0.1);
201  }
202  if (m_bodyA)
203  {
205  tr.setOrigin(pivotAworld);
206  drawer->drawTransform(tr, 0.1);
207  }
208  if (m_rigidBodyB)
209  {
210  // that ideally should draw the same frame
212  tr.setOrigin(pivot);
213  drawer->drawTransform(tr, 0.1);
214  }
215  if (m_bodyB)
216  {
218  tr.setOrigin(pivotBworld);
219  drawer->drawTransform(tr, 0.1);
220  }
221 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:164
virtual void debugDraw(class btIDebugDraw *drawer)
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btAlignedObjectArray< btScalar > scratch_r
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btMultiBodyConstraint * m_orgConstraint
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:131
btScalar * jacobianA(int row)
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
btAlignedObjectArray< btMatrix3x3 > scratch_m
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btAlignedObjectArray< btScalar > m_data
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btAlignedObjectArray< btVector3 > scratch_v
virtual int getIslandIdA() const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
int getIslandTag() const
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
void resize(int newsize, const T &fillData=T())
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
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)
virtual int getIslandIdB() const
int getCompanionId() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292