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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyGearConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
23btMultiBodyGearConstraint::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, MULTIBODY_CONSTRAINT_GEAR),
25 m_gearRatio(1),
26 m_gearAuxLink(-1),
27 m_erp(0),
28 m_relativePositionTarget(0)
29{
30}
31
32 void btMultiBodyGearConstraint::finalizeMultiDof()
33{
34 allocateJacobiansMultiDof();
35
36 m_numDofsFinalized = m_jacSizeBoth;
37}
38
39btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
40{
41}
42
43 int btMultiBodyGearConstraint::getIslandIdA() const
44{
45 if (m_bodyA)
46 {
47 if (m_linkA < 0)
48 {
49 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
50 if (col)
51 return col->getIslandTag();
52 }
53 else
54 {
55 if (m_bodyA->getLink(m_linkA).m_collider)
56 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
57 }
58 }
59 return -1;
60}
61
62 int btMultiBodyGearConstraint::getIslandIdB() const
63{
64 if (m_bodyB)
65 {
66 if (m_linkB < 0)
67 {
68 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
69 if (col)
70 return col->getIslandTag();
71 }
72 else
73 {
74 if (m_bodyB->getLink(m_linkB).m_collider)
75 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
76 }
77 }
78 return -1;
79}
80
81 void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
82 btMultiBodyJacobianData& data,
83 const btContactSolverInfo& infoGlobal)
84{
85 // only positions need to be updated -- data.m_jacobians and force
86 // directions were set in the ctor and never change.
87
88 if (m_numDofsFinalized != m_jacSizeBoth)
89 {
90 finalizeMultiDof();
91 }
92
93 //don't crash
94 if (m_numDofsFinalized != m_jacSizeBoth)
95 return;
96
97 if (m_maxAppliedImpulse == 0.f)
98 return;
99
100 // note: we rely on the fact that data.m_jacobians are
101 // always initialized to zero by the Constraint ctor
102 int linkDoF = 0;
103 unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
104 unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
105
106 // row 0: the lower bound
107 jacobianA(0)[offsetA] = 1;
108 jacobianB(0)[offsetB] = m_gearRatio;
109
110 btScalar posError = 0;
111 const btVector3 dummy(0, 0, 0);
112
113 btScalar kp = 1;
114 btScalar kd = 1;
115 int numRows = getNumRows();
116
117 for (int row = 0; row < numRows; row++)
118 {
119 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
120
121 int dof = 0;
122 btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
123 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
124 btScalar auxVel = 0;
125
126 if (m_gearAuxLink >= 0)
127 {
128 auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
129 }
130 currentVelocity += auxVel;
131 if (m_erp != 0)
132 {
133 btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
134 if (m_gearAuxLink >= 0)
135 {
136 currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
137 }
138 btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
139 btScalar diff = currentPositionB + currentPositionA;
140 btScalar desiredPositionDiff = this->m_relativePositionTarget;
141 posError = -m_erp * (desiredPositionDiff - diff);
142 }
143
144 btScalar desiredRelativeVelocity = auxVel;
145
146 fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
147
148 constraintRow.m_orgConstraint = this;
149 constraintRow.m_orgDofIndex = row;
150 {
151 //expect either prismatic or revolute joint type for now
152 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
153 switch (m_bodyA->getLink(m_linkA).m_jointType)
154 {
155 case btMultibodyLink::eRevolute:
156 {
157 constraintRow.m_contactNormal1.setZero();
158 constraintRow.m_contactNormal2.setZero();
159 btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
160 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
161 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
162
163 break;
164 }
165 case btMultibodyLink::ePrismatic:
166 {
167 btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
168 constraintRow.m_contactNormal1 = prismaticAxisInWorld;
169 constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
170 constraintRow.m_relpos1CrossNormal.setZero();
171 constraintRow.m_relpos2CrossNormal.setZero();
172 break;
173 }
174 default:
175 {
176 btAssert(0);
177 }
178 };
179 }
180 }
181}
btCollisionObject.h
MULTIBODY_CONSTRAINT_GEAR
@ MULTIBODY_CONSTRAINT_GEAR
Definition btMultiBodyConstraint.h:29
btMultiBodyGearConstraint.h
btMultiBodyLinkCollider.h
btMultiBody.h
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition btQuaternion.h:926
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btAssert
#define btAssert(x)
Definition btScalar.h:153
btAlignedObjectArray< btMultiBodySolverConstraint >
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition btCollisionObject.h:448
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMultiBodyConstraint
Definition btMultiBodyConstraint.h:57
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition btMultiBodyConstraint.h:60
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition btMultiBodyConstraint.h:181
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition btMultiBodyConstraint.h:73
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition btMultiBodyConstraint.h:189
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition btMultiBodyConstraint.h:72
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition btMultiBodyConstraint.cpp:35
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, btScalar damping=1.0)
Definition btMultiBodyConstraint.cpp:53
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition btMultiBodyConstraint.h:59
btMultiBodyConstraint::m_linkB
int m_linkB
Definition btMultiBodyConstraint.h:62
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition btMultiBodyConstraint.h:68
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition btMultiBodyConstraint.h:125
btMultiBodyConstraint::m_linkA
int m_linkA
Definition btMultiBodyConstraint.h:61
btMultiBodyGearConstraint::~btMultiBodyGearConstraint
virtual ~btMultiBodyGearConstraint()
Definition btMultiBodyGearConstraint.cpp:39
btMultiBodyGearConstraint::m_erp
btScalar m_erp
Definition btMultiBodyGearConstraint.h:34
btMultiBodyGearConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition btMultiBodyGearConstraint.cpp:81
btMultiBodyGearConstraint::m_gearAuxLink
int m_gearAuxLink
Definition btMultiBodyGearConstraint.h:33
btMultiBodyGearConstraint::m_relativePositionTarget
btScalar m_relativePositionTarget
Definition btMultiBodyGearConstraint.h:35
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::finalizeMultiDof
virtual void finalizeMultiDof()
Definition btMultiBodyGearConstraint.cpp:32
btMultiBodyGearConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition btMultiBodyGearConstraint.cpp:43
btMultiBodyGearConstraint::m_gearRatio
btScalar m_gearRatio
Definition btMultiBodyGearConstraint.h:32
btMultiBodyGearConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition btMultiBodyGearConstraint.cpp:62
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btMultiBody
Definition btMultiBody.h:51
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition btMultiBody.cpp:388
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition btMultiBody.h:114
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition btMultiBody.h:128
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition btMultiBody.cpp:393
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition btTransform.h:119
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::setZero
void setZero()
Definition btVector3.h:671
btContactSolverInfo
Definition btContactSolverInfo.h:76
btMultiBodyJacobianData
Definition btMultiBodyConstraint.h:44
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition btMultiBodySolverConstraint.h:30
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition btMultiBodySolverConstraint.h:78
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition btMultiBodySolverConstraint.h:42
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition btMultiBodySolverConstraint.h:45
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition btMultiBodySolverConstraint.h:79
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition btMultiBodySolverConstraint.h:43
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition btMultiBodySolverConstraint.h:44
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition btMultiBodyLink.h:132
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition btMultiBodyLink.h:137
btMultibodyLink::ePrismatic
@ ePrismatic
Definition btMultiBodyLink.h:73
btMultibodyLink::eRevolute
@ eRevolute
Definition btMultiBodyLink.h:72
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition btMultiBodyLink.h:141
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition btMultiBodyLink.h:93
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition btSpatialAlgebra.h:96
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition btSpatialAlgebra.h:96