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 }
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 
49 {
50 }
51 
52 
54 {
55  if (m_rigidBodyA)
56  return m_rigidBodyA->getIslandTag();
57 
58  if (m_bodyA)
59  {
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 
73 {
74  if (m_rigidBodyB)
75  return m_rigidBodyB->getIslandTag();
76  if (m_bodyB)
77  {
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 
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();
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();
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,
158  );
159  //@todo: support the case of btMultiBody versus btRigidBody,
160  //see btPoint2PointConstraint::getInfo2NonVirtual
161 #else
162  const btVector3 dummy(0, 0, 0);
163 
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,
177  );
178 #endif
179  }
180 }
181 
183 {
184  btTransform tr;
185  tr.setIdentity();
186 
187  if (m_rigidBodyA)
188  {
190  tr.setOrigin(pivot);
191  drawer->drawTransform(tr, 0.1);
192  }
193  if (m_bodyA)
194  {
196  tr.setOrigin(pivotAworld);
197  drawer->drawTransform(tr, 0.1);
198  }
199  if (m_rigidBodyB)
200  {
201  // that ideally should draw the same frame
203  tr.setOrigin(pivot);
204  drawer->drawTransform(tr, 0.1);
205  }
206  if (m_bodyB)
207  {
209  tr.setOrigin(pivotBworld);
210  drawer->drawTransform(tr, 0.1);
211  }
212 }
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
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
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)
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:145
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:124
btAlignedObjectArray< btScalar > scratch_r
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:113
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:28
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
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:137
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
bool isMultiDof()
Definition: btMultiBody.h:522
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:278