Bullet Collision Detection & Physics Library
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
19#include "btMultiBody.h"
22
25 m_gearRatio(1),
26 m_gearAuxLink(-1),
27 m_erp(0),
28 m_relativePositionTarget(0)
29{
30}
31
33{
35
37}
38
40{
41}
42
44{
45 if (m_bodyA)
46 {
47 if (m_linkA < 0)
48 {
50 if (col)
51 return col->getIslandTag();
52 }
53 else
54 {
57 }
58 }
59 return -1;
60}
61
63{
64 if (m_bodyB)
65 {
66 if (m_linkB < 0)
67 {
69 if (col)
70 return col->getIslandTag();
71 }
72 else
73 {
76 }
77 }
78 return -1;
79}
80
84{
85 // only positions need to be updated -- data.m_jacobians and force
86 // directions were set in the ctor and never change.
87
89 {
91 }
92
93 //don't crash
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;
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 {
120
121 int dof = 0;
124 btScalar auxVel = 0;
125
126 if (m_gearAuxLink >= 0)
127 {
129 }
131 if (m_erp != 0)
132 {
134 if (m_gearAuxLink >= 0)
135 {
137 }
141 posError = -m_erp * (desiredPositionDiff - diff);
142 }
143
145
147
148 constraintRow.m_orgConstraint = this;
149 constraintRow.m_orgDofIndex = row;
150 {
151 //expect either prismatic or revolute joint type for now
154 {
156 {
157 constraintRow.m_contactNormal1.setZero();
158 constraintRow.m_contactNormal2.setZero();
160 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
161 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
162
163 break;
164 }
166 {
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}
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
@ MULTIBODY_CONSTRAINT_GEAR
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
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:153
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btScalar * jacobianA(int row)
btScalar * jacobianB(int row)
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)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
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.
btScalar * getJointPosMultiDof(int i)
const btMultibodyLink & getLink(int index) const
const btMultiBodyLinkCollider * getBaseCollider() const
btScalar * getJointVelMultiDof(int i)
btQuaternion getRotation() const
Return a quaternion representing the rotation.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...