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"
21 
22 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
23  :btMultiBodyConstraint(body,0,link,-1,3,false),
24  m_rigidBodyA(0),
25  m_rigidBodyB(bodyB),
26  m_pivotInA(pivotInA),
27  m_pivotInB(pivotInB)
28 {
29 }
30 
31 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
32  :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
33  m_rigidBodyA(0),
34  m_rigidBodyB(0),
35  m_pivotInA(pivotInA),
36  m_pivotInB(pivotInB)
37 {
38 }
39 
40 
42 {
43 }
44 
45 
47 {
48  if (m_rigidBodyA)
49  return m_rigidBodyA->getIslandTag();
50 
51  if (m_bodyA)
52  {
54  if (col)
55  return col->getIslandTag();
56  for (int i=0;i<m_bodyA->getNumLinks();i++)
57  {
58  if (m_bodyA->getLink(i).m_collider)
59  return m_bodyA->getLink(i).m_collider->getIslandTag();
60  }
61  }
62  return -1;
63 }
64 
66 {
67  if (m_rigidBodyB)
68  return m_rigidBodyB->getIslandTag();
69  if (m_bodyB)
70  {
72  if (col)
73  return col->getIslandTag();
74 
75  for (int i=0;i<m_bodyB->getNumLinks();i++)
76  {
77  col = m_bodyB->getLink(i).m_collider;
78  if (col)
79  return col->getIslandTag();
80  }
81  }
82  return -1;
83 }
84 
85 
86 
89  const btContactSolverInfo& infoGlobal)
90 {
91 
92 // int i=1;
93  for (int i=0;i<3;i++)
94  {
95 
96  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
97 
98  constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
99  constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
100 
101 
102  btVector3 contactNormalOnB(0,0,0);
103  contactNormalOnB[i] = -1;
104 
105  btScalar penetration = 0;
106 
107  // Convert local points back to world
108  btVector3 pivotAworld = m_pivotInA;
109  if (m_rigidBodyA)
110  {
111 
112  constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
114  } else
115  {
116  if (m_bodyA)
117  pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
118  }
119  btVector3 pivotBworld = m_pivotInB;
120  if (m_rigidBodyB)
121  {
122  constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
124  } else
125  {
126  if (m_bodyB)
127  pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
128 
129  }
130  btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
131  btScalar relaxation = 1.f;
132  fillMultiBodyConstraintMixed(constraintRow, data,
133  contactNormalOnB,
134  pivotAworld, pivotBworld,
135  position,
136  infoGlobal,
137  relaxation,
138  false);
139  constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
140  constraintRow.m_upperLimit = m_maxAppliedImpulse;
141 
142  }
143 }
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, const btVector3 &contactNormalOnB, const btVector3 &posAworld, const btVector3 &posBworld, btScalar position, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:76
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
int getNumLinks() const
Definition: btMultiBody.h:112
virtual int getIslandIdA() const
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
This file was written by Erwin Coumans.
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:353
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
int getCompanionId() const
int getIslandTag() const
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:91
virtual int getIslandIdB() const
btVector3 localPosToWorld(int i, const btVector3 &vec) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:266