Blender  V3.3
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)
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)
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 
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 
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 
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();
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,
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,
182 #endif
183  }
184 }
185 
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
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 }
void debugDraw(btIDebugDraw *debugDrawer)
btActionInterface interface
@ MULTIBODY_CONSTRAINT_POINT_TO_POINT
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
int m_linkB
btAlignedObjectArray< btScalar > m_data
virtual int getIslandIdA() const =0
btMultiBody * m_bodyB
int m_linkA
virtual int getIslandIdB() const =0
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 m_maxAppliedImpulse
btScalar * jacobianA(int row)
btMultiBodyConstraint
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
virtual ~btMultiBodyPoint2Point()
btRigidBody * m_rigidBodyB
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
void finalizeMultiDof()
btMultiBody
Definition: btMultiBody.h:51
btVector3 m_pivotInA
btVector3 m_pivotInB
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
uint col
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)