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

Bullet Collision Detection & Physics Library

btMultiBodyGearConstraint.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 "btMultiBodyGearConstraint.h"
19 #include "btMultiBody.h"
20 #include "btMultiBodyLinkCollider.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23 btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
24 :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
25 m_gearRatio(1),
26 m_gearAuxLink(-1),
27 m_erp(0),
28 m_relativePositionTarget(0)
29 {
30
31 }
32
33 void btMultiBodyGearConstraint::finalizeMultiDof()
34 {
35
36 allocateJacobiansMultiDof();
37
38 m_numDofsFinalized = m_jacSizeBoth;
39 }
40
41 btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
42 {
43 }
44
45
46 int btMultiBodyGearConstraint::getIslandIdA() const
47 {
48
49 if (m_bodyA)
50 {
51 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
52 if (col)
53 return col->getIslandTag();
54 for (int i=0;i<m_bodyA->getNumLinks();i++)
55 {
56 if (m_bodyA->getLink(i).m_collider)
57 return m_bodyA->getLink(i).m_collider->getIslandTag();
58 }
59 }
60 return -1;
61 }
62
63 int btMultiBodyGearConstraint::getIslandIdB() const
64 {
65 if (m_bodyB)
66 {
67 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
68 if (col)
69 return col->getIslandTag();
70
71 for (int i=0;i<m_bodyB->getNumLinks();i++)
72 {
73 col = m_bodyB->getLink(i).m_collider;
74 if (col)
75 return col->getIslandTag();
76 }
77 }
78 return -1;
79 }
80
81
82 void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
83 btMultiBodyJacobianData& data,
84 const btContactSolverInfo& infoGlobal)
85 {
86 // only positions need to be updated -- data.m_jacobians and force
87 // directions were set in the ctor and never change.
88
89 if (m_numDofsFinalized != m_jacSizeBoth)
90 {
91 finalizeMultiDof();
92 }
93
94 //don't crash
95 if (m_numDofsFinalized != m_jacSizeBoth)
96 return;
97
98
99 if (m_maxAppliedImpulse==0.f)
100 return;
101
102 // note: we rely on the fact that data.m_jacobians are
103 // always initialized to zero by the Constraint ctor
104 int linkDoF = 0;
105 unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
106 unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
107
108 // row 0: the lower bound
109 jacobianA(0)[offsetA] = 1;
110 jacobianB(0)[offsetB] = m_gearRatio;
111
112 btScalar posError = 0;
113 const btVector3 dummy(0, 0, 0);
114
115 btScalar kp = 1;
116 btScalar kd = 1;
117 int numRows = getNumRows();
118
119 for (int row=0;row<numRows;row++)
120 {
121 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
122
123
124 int dof = 0;
125 btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
126 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
127 btScalar auxVel = 0;
128
129 if (m_gearAuxLink>=0)
130 {
131 auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
132 }
133 currentVelocity += auxVel;
134 if (m_erp!=0)
135 {
136 btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
137 btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
138 btScalar diff = currentPositionB+currentPositionA;
139 btScalar desiredPositionDiff = this->m_relativePositionTarget;
140 posError = -m_erp*(desiredPositionDiff - diff);
141 }
142
143 btScalar desiredRelativeVelocity = auxVel;
144
145 fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
146
147 constraintRow.m_orgConstraint = this;
148 constraintRow.m_orgDofIndex = row;
149 {
150 //expect either prismatic or revolute joint type for now
151 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
152 switch (m_bodyA->getLink(m_linkA).m_jointType)
153 {
154 case btMultibodyLink::eRevolute:
155 {
156 constraintRow.m_contactNormal1.setZero();
157 constraintRow.m_contactNormal2.setZero();
158 btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
159 constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
160 constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
161
162 break;
163 }
164 case btMultibodyLink::ePrismatic:
165 {
166 btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
167 constraintRow.m_contactNormal1=prismaticAxisInWorld;
168 constraintRow.m_contactNormal2=-prismaticAxisInWorld;
169 constraintRow.m_relpos1CrossNormal.setZero();
170 constraintRow.m_relpos2CrossNormal.setZero();
171 break;
172 }
173 default:
174 {
175 btAssert(0);
176 }
177 };
178
179 }
180
181 }
182
183 }
184
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:166
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:164
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyGearConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyGearConstraint.cpp:63
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:74
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:131
btMultiBodyGearConstraint::m_erp
btScalar m_erp
Definition: btMultiBodyGearConstraint.h:35
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:158
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:382
btMultiBodyGearConstraint::m_relativePositionTarget
btScalar m_relativePositionTarget
Definition: btMultiBodyGearConstraint.h:36
btMultiBody
Definition: btMultiBody.h:51
btMultiBodyGearConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyGearConstraint.cpp:46
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btMultiBodyGearConstraint::m_gearRatio
btScalar m_gearRatio
Definition: btMultiBodyGearConstraint.h:33
btMultiBodyGearConstraint::~btMultiBodyGearConstraint
virtual ~btMultiBodyGearConstraint()
Definition: btMultiBodyGearConstraint.cpp:41
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:57
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:97
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:53
btVector3::setZero
void setZero()
Definition: btVector3.h:683
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:108
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:70
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyLinkCollider.h
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:114
btMultiBodyGearConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyGearConstraint.cpp:82
btMultiBody.h
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:443
btMultiBodyGearConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyGearConstraint.cpp:33
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btMultiBodyGearConstraint.h
btCollisionObject.h
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:37
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)
Definition: btMultiBodyConstraint.cpp:55
btMultiBodyGearConstraint::btMultiBodyGearConstraint
btMultiBodyGearConstraint(btMultiBody *bodyA, int linkA, btMultiBody *bodyB, int linkB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
This file was written by Erwin Coumans.
Definition: btMultiBodyGearConstraint.cpp:23
btMultiBodyGearConstraint::m_gearAuxLink
int m_gearAuxLink
Definition: btMultiBodyGearConstraint.h:34
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:245
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:75
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:377
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48