Bullet Collision Detection & Physics Library
btMultiBodyJointLimitConstraint.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
23
24
26 //:btMultiBodyConstraint(body,0,link,-1,2,true),
27 :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
28 m_lowerBound(lower),
29 m_upperBound(upper)
30{
31
32}
33
35{
36 // the data.m_jacobians never change, so may as well
37 // initialize them here
38
40
41 unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
42
43 // row 0: the lower bound
44 jacobianA(0)[offset] = 1;
45 // row 1: the upper bound
46 //jacobianA(1)[offset] = -1;
47 jacobianB(1)[offset] = -1;
48
50}
51
53{
54}
55
57{
58 if(m_bodyA)
59 {
61 if (col)
62 return col->getIslandTag();
63 for (int i=0;i<m_bodyA->getNumLinks();i++)
64 {
67 }
68 }
69 return -1;
70}
71
73{
74 if(m_bodyB)
75 {
77 if (col)
78 return col->getIslandTag();
79
80 for (int i=0;i<m_bodyB->getNumLinks();i++)
81 {
82 col = m_bodyB->getLink(i).m_collider;
83 if (col)
84 return col->getIslandTag();
85 }
86 }
87 return -1;
88}
89
90
93 const btContactSolverInfo& infoGlobal)
94{
95
96 // only positions need to be updated -- data.m_jacobians and force
97 // directions were set in the ctor and never change.
98
100 {
102 }
103
104
105 // row 0: the lower bound
106 setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
107
108 // row 1: the upper bound
110
111 for (int row=0;row<getNumRows();row++)
112 {
113 btScalar penetration = getPosition(row);
114
115 //todo: consider adding some safety threshold here
116 if (penetration>0)
117 {
118 continue;
119 }
120 btScalar direction = row? -1 : 1;
121
122 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
123 constraintRow.m_orgConstraint = this;
124 constraintRow.m_orgDofIndex = row;
125
126 constraintRow.m_multiBodyA = m_bodyA;
127 constraintRow.m_multiBodyB = m_bodyB;
128 const btScalar posError = 0; //why assume it's zero?
129 const btVector3 dummy(0, 0, 0);
130
131 btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
132
133 {
134 //expect either prismatic or revolute joint type for now
137 {
139 {
140 constraintRow.m_contactNormal1.setZero();
141 constraintRow.m_contactNormal2.setZero();
143 constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
144 constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
145
146 break;
147 }
149 {
151 constraintRow.m_contactNormal1=prismaticAxisInWorld;
152 constraintRow.m_contactNormal2=-prismaticAxisInWorld;
153 constraintRow.m_relpos1CrossNormal.setZero();
154 constraintRow.m_relpos2CrossNormal.setZero();
155
156 break;
157 }
158 default:
159 {
160 btAssert(0);
161 }
162 };
163
164 }
165
166 {
167
168 btScalar positionalError = 0.f;
169 btScalar velocityError = - rel_vel;// * damping;
170 btScalar erp = infoGlobal.m_erp2;
171 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
172 {
173 erp = infoGlobal.m_erp;
174 }
175 if (penetration>0)
176 {
177 positionalError = 0;
178 velocityError = -penetration / infoGlobal.m_timeStep;
179 } else
180 {
181 positionalError = -penetration * erp/infoGlobal.m_timeStep;
182 }
183
184 btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
185 btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
186 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
187 {
188 //combine position and velocity into rhs
189 constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
190 constraintRow.m_rhsPenetration = 0.f;
191
192 } else
193 {
194 //split position and velocity into rhs and m_rhsPenetration
195 constraintRow.m_rhs = velocityImpulse;
196 constraintRow.m_rhsPenetration = penetrationImpulse;
197 }
198 }
199 }
200
201}
202
203
204
205
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define btAssert(x)
Definition: btScalar.h:131
int getIslandTag() const
btScalar getPosition(int row) const
btScalar * jacobianA(int row)
void setPosition(int row, btScalar pos)
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)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
btScalar getJointPos(int i) const
int getNumLinks() const
Definition: btMultiBody.h:164
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
void setZero()
Definition: btVector3.h:683
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint