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

Bullet Collision Detection & Physics Library

btMultiBodyJointLimitConstraint.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 "btMultiBodyJointLimitConstraint.h"
19 #include "btMultiBody.h"
20 #include "btMultiBodyLinkCollider.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23
24
25 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
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
34 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
35 {
36 // the data.m_jacobians never change, so may as well
37 // initialize them here
38
39 allocateJacobiansMultiDof();
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
49 m_numDofsFinalized = m_jacSizeBoth;
50 }
51
52 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
53 {
54 }
55
56 int btMultiBodyJointLimitConstraint::getIslandIdA() const
57 {
58 if(m_bodyA)
59 {
60 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
61 if (col)
62 return col->getIslandTag();
63 for (int i=0;i<m_bodyA->getNumLinks();i++)
64 {
65 if (m_bodyA->getLink(i).m_collider)
66 return m_bodyA->getLink(i).m_collider->getIslandTag();
67 }
68 }
69 return -1;
70 }
71
72 int btMultiBodyJointLimitConstraint::getIslandIdB() const
73 {
74 if(m_bodyB)
75 {
76 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
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
91 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
92 btMultiBodyJacobianData& data,
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
99 if (m_numDofsFinalized != m_jacSizeBoth)
100 {
101 finalizeMultiDof();
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
109 setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
110
111 for (int row=0;row<getNumRows();row++)
112 {
113
114 btScalar direction = row? -1 : 1;
115
116 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
117 constraintRow.m_orgConstraint = this;
118 constraintRow.m_orgDofIndex = row;
119
120 constraintRow.m_multiBodyA = m_bodyA;
121 constraintRow.m_multiBodyB = m_bodyB;
122 const btScalar posError = 0; //why assume it's zero?
123 const btVector3 dummy(0, 0, 0);
124
125 btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
126
127 {
128 //expect either prismatic or revolute joint type for now
129 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
130 switch (m_bodyA->getLink(m_linkA).m_jointType)
131 {
132 case btMultibodyLink::eRevolute:
133 {
134 constraintRow.m_contactNormal1.setZero();
135 constraintRow.m_contactNormal2.setZero();
136 btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
137 constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
138 constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
139
140 break;
141 }
142 case btMultibodyLink::ePrismatic:
143 {
144 btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
145 constraintRow.m_contactNormal1=prismaticAxisInWorld;
146 constraintRow.m_contactNormal2=-prismaticAxisInWorld;
147 constraintRow.m_relpos1CrossNormal.setZero();
148 constraintRow.m_relpos2CrossNormal.setZero();
149
150 break;
151 }
152 default:
153 {
154 btAssert(0);
155 }
156 };
157
158 }
159
160 {
161 btScalar penetration = getPosition(row);
162 btScalar positionalError = 0.f;
163 btScalar velocityError = - rel_vel;// * damping;
164 btScalar erp = infoGlobal.m_erp2;
165 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
166 {
167 erp = infoGlobal.m_erp;
168 }
169 if (penetration>0)
170 {
171 positionalError = 0;
172 velocityError = -penetration / infoGlobal.m_timeStep;
173 } else
174 {
175 positionalError = -penetration * erp/infoGlobal.m_timeStep;
176 }
177
178 btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
179 btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
180 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
181 {
182 //combine position and velocity into rhs
183 constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
184 constraintRow.m_rhsPenetration = 0.f;
185
186 } else
187 {
188 //split position and velocity into rhs and m_rhsPenetration
189 constraintRow.m_rhs = velocityImpulse;
190 constraintRow.m_rhsPenetration = penetrationImpulse;
191 }
192 }
193 }
194
195 }
196
197
198
199
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:59
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:166
btMultiBodyJointLimitConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointLimitConstraint.cpp:91
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:155
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
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 >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultiBodyJointLimitConstraint.h
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:73
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:127
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:121
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btMultiBodyJointLimitConstraint::m_upperBound
btScalar m_upperBound
Definition: btMultiBodyJointLimitConstraint.h:27
btAssert
#define btAssert(x)
Definition: btScalar.h:114
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
btMultiBodyConstraint::getPosition
btScalar getPosition(int row) const
Definition: btMultiBodyConstraint.h:139
btMultiBodyJointLimitConstraint::m_lowerBound
btScalar m_lowerBound
Definition: btMultiBodyJointLimitConstraint.h:26
btMultiBody
Definition: btMultiBody.h:51
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:887
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint
virtual ~btMultiBodyJointLimitConstraint()
Definition: btMultiBodyJointLimitConstraint.cpp:52
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:57
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:96
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:53
btVector3::setZero
void setZero()
Definition: btVector3.h:681
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:108
btMultiBodyJointLimitConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyJointLimitConstraint.cpp:34
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:70
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyLinkCollider.h
btMultiBodyJointLimitConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointLimitConstraint.cpp:56
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:104
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointLimitConstraint.cpp:25
btMultiBody.h
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:441
btMultiBodyJointLimitConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointLimitConstraint.cpp:72
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btMultiBodyConstraint::setPosition
void setPosition(int row, btScalar pos)
Definition: btMultiBodyConstraint.h:144
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition: btMultiBody.cpp:365
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:51
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:131
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
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:53
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:245
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:74
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48