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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
18#include "btMultiBodyJointLimitConstraint.h"
19#include "btMultiBody.h"
20#include "btMultiBodyLinkCollider.h"
21#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
24 //:btMultiBodyConstraint(body,0,link,-1,2,true),
25 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
26 m_lowerBound(lower),
27 m_upperBound(upper)
28{
29}
30
31 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
32{
33 // the data.m_jacobians never change, so may as well
34 // initialize them here
35
36 allocateJacobiansMultiDof();
37
38 unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
39
40 // row 0: the lower bound
41 jacobianA(0)[offset] = 1;
42 // row 1: the upper bound
43 //jacobianA(1)[offset] = -1;
44 jacobianB(1)[offset] = -1;
45
46 m_numDofsFinalized = m_jacSizeBoth;
47}
48
49btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
50{
51}
52
53 int btMultiBodyJointLimitConstraint::getIslandIdA() const
54{
55 if (m_bodyA)
56 {
57 if (m_linkA < 0)
58 {
59 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
60 if (col)
61 return col->getIslandTag();
62 }
63 else
64 {
65 if (m_bodyA->getLink(m_linkA).m_collider)
66 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
67 }
68 }
69 return -1;
70}
71
72 int btMultiBodyJointLimitConstraint::getIslandIdB() const
73{
74 if (m_bodyB)
75 {
76 if (m_linkB < 0)
77 {
78 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
79 if (col)
80 return col->getIslandTag();
81 }
82 else
83 {
84 if (m_bodyB->getLink(m_linkB).m_collider)
85 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
86 }
87 }
88 return -1;
89}
90
91 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
92 btMultiBodyJacobianData& data,
93 const btContactSolverInfo& infoGlobal)
94{
95 // only positions need to be updated -- data.m_jacobians and force
96 // directions were set in the ctor and never change.
97
98 if (m_numDofsFinalized != m_jacSizeBoth)
99 {
100 finalizeMultiDof();
101 }
102
103 // row 0: the lower bound
104 setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
105
106 // row 1: the upper bound
107 setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
108
109 for (int row = 0; row < getNumRows(); row++)
110 {
111 btScalar penetration = getPosition(row);
112
113 //todo: consider adding some safety threshold here
114 if (penetration > 0)
115 {
116 continue;
117 }
118 btScalar direction = row ? -1 : 1;
119
120 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
121 constraintRow.m_orgConstraint = this;
122 constraintRow.m_orgDofIndex = row;
123
124 constraintRow.m_multiBodyA = m_bodyA;
125 constraintRow.m_multiBodyB = m_bodyB;
126 const btScalar posError = 0; //why assume it's zero?
127 const btVector3 dummy(0, 0, 0);
128
129 btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
130
131 {
132 //expect either prismatic or revolute joint type for now
133 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
134 switch (m_bodyA->getLink(m_linkA).m_jointType)
135 {
136 case btMultibodyLink::eRevolute:
137 {
138 constraintRow.m_contactNormal1.setZero();
139 constraintRow.m_contactNormal2.setZero();
140 btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
141 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
142 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
143
144 break;
145 }
146 case btMultibodyLink::ePrismatic:
147 {
148 btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
149 constraintRow.m_contactNormal1 = prismaticAxisInWorld;
150 constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
151 constraintRow.m_relpos1CrossNormal.setZero();
152 constraintRow.m_relpos2CrossNormal.setZero();
153
154 break;
155 }
156 default:
157 {
158 btAssert(0);
159 }
160 };
161 }
162
163 {
164 btScalar positionalError = 0.f;
165 btScalar velocityError = -rel_vel; // * damping;
166 btScalar erp = infoGlobal.m_erp2;
167 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
168 {
169 erp = infoGlobal.m_erp;
170 }
171 if (penetration > 0)
172 {
173 positionalError = 0;
174 velocityError = -penetration / infoGlobal.m_timeStep;
175 }
176 else
177 {
178 positionalError = -penetration * erp / infoGlobal.m_timeStep;
179 }
180
181 btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
182 btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
183 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
184 {
185 //combine position and velocity into rhs
186 constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
187 constraintRow.m_rhsPenetration = 0.f;
188 }
189 else
190 {
191 //split position and velocity into rhs and m_rhsPenetration
192 constraintRow.m_rhs = velocityImpulse;
193 constraintRow.m_rhsPenetration = penetrationImpulse;
194 }
195 }
196 }
197}
btCollisionObject.h
MULTIBODY_CONSTRAINT_LIMIT
@ MULTIBODY_CONSTRAINT_LIMIT
Definition btMultiBodyConstraint.h:27
btMultiBodyJointLimitConstraint.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
btMultiBodyConstraint
Definition btMultiBodyConstraint.h:57
btMultiBodyConstraint::getPosition
btScalar getPosition(int row) const
Definition btMultiBodyConstraint.h:163
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition btMultiBodyConstraint.h:60
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition btMultiBodyConstraint.h:181
btMultiBodyConstraint::setPosition
void setPosition(int row, btScalar pos)
Definition btMultiBodyConstraint.h:168
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
btMultiBodyJointLimitConstraint::m_lowerBound
btScalar m_lowerBound
Definition btMultiBodyJointLimitConstraint.h:25
btMultiBodyJointLimitConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition btMultiBodyJointLimitConstraint.cpp:72
btMultiBodyJointLimitConstraint::m_upperBound
btScalar m_upperBound
Definition btMultiBodyJointLimitConstraint.h:26
btMultiBodyJointLimitConstraint::finalizeMultiDof
virtual void finalizeMultiDof()
Definition btMultiBodyJointLimitConstraint.cpp:31
btMultiBodyJointLimitConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition btMultiBodyJointLimitConstraint.cpp:53
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint
virtual ~btMultiBodyJointLimitConstraint()
Definition btMultiBodyJointLimitConstraint.cpp:49
btMultiBodyJointLimitConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition btMultiBodyJointLimitConstraint.cpp:91
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
Definition btMultiBodyJointLimitConstraint.cpp:23
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btMultiBody
Definition btMultiBody.h:51
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition btMultiBody.cpp:378
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition btMultiBody.h:114
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition btMultiBody.h:128
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
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition btContactSolverInfo.h:57
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition btContactSolverInfo.h:56
btContactSolverInfoData::m_erp
btScalar m_erp
Definition btContactSolverInfo.h:47
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition btContactSolverInfo.h:42
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition btContactSolverInfo.h:48
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_rhs
btScalar m_rhs
Definition btMultiBodySolverConstraint.h:55
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition btMultiBodySolverConstraint.h:54
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_multiBodyB
btMultiBody * m_multiBodyB
Definition btMultiBodySolverConstraint.h:74
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition btMultiBodySolverConstraint.h:44
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition btMultiBodySolverConstraint.h:60
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition btMultiBodySolverConstraint.h:70
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_dofOffset
int m_dofOffset
Definition btMultiBodyLink.h:110
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