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 /*
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 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
25 //:btMultiBodyConstraint(body,0,link,-1,2,true),
26 :btMultiBodyConstraint(body,body,link,link,2,true),
27 m_lowerBound(lower),
28 m_upperBound(upper)
29 {
30 // the data.m_jacobians never change, so may as well
31 // initialize them here
32
33 // note: we rely on the fact that data.m_jacobians are
34 // always initialized to zero by the Constraint ctor
35
36 unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
37
38 // row 0: the lower bound
39 jacobianA(0)[offset] = 1;
40 // row 1: the upper bound
41 //jacobianA(1)[offset] = -1;
42
43 jacobianB(1)[offset] = -1;
44 }
45 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
46 {
47 }
48
49 int btMultiBodyJointLimitConstraint::getIslandIdA() const
50 {
51 if(m_bodyA)
52 {
53 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
54 if (col)
55 return col->getIslandTag();
56 for (int i=0;i<m_bodyA->getNumLinks();i++)
57 {
58 if (m_bodyA->getLink(i).m_collider)
59 return m_bodyA->getLink(i).m_collider->getIslandTag();
60 }
61 }
62 return -1;
63 }
64
65 int btMultiBodyJointLimitConstraint::getIslandIdB() const
66 {
67 if(m_bodyB)
68 {
69 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
70 if (col)
71 return col->getIslandTag();
72
73 for (int i=0;i<m_bodyB->getNumLinks();i++)
74 {
75 col = m_bodyB->getLink(i).m_collider;
76 if (col)
77 return col->getIslandTag();
78 }
79 }
80 return -1;
81 }
82
83
84 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
85 btMultiBodyJacobianData& data,
86 const btContactSolverInfo& infoGlobal)
87 {
88 // only positions need to be updated -- data.m_jacobians and force
89 // directions were set in the ctor and never change.
90
91 // row 0: the lower bound
92 setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
93
94 // row 1: the upper bound
95 setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
96
97 for (int row=0;row<getNumRows();row++)
98 {
99 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
100 constraintRow.m_multiBodyA = m_bodyA;
101 constraintRow.m_multiBodyB = m_bodyB;
102 const btScalar posError = 0; //why assume it's zero?
103 const btVector3 dummy(0, 0, 0);
104
105 btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
106 {
107 btScalar penetration = getPosition(row);
108 btScalar positionalError = 0.f;
109 btScalar velocityError = - rel_vel;// * damping;
110 btScalar erp = infoGlobal.m_erp2;
111 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
112 {
113 erp = infoGlobal.m_erp;
114 }
115 if (penetration>0)
116 {
117 positionalError = 0;
118 velocityError = -penetration / infoGlobal.m_timeStep;
119 } else
120 {
121 positionalError = -penetration * erp/infoGlobal.m_timeStep;
122 }
123
124 btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
125 btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
126 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
127 {
128 //combine position and velocity into rhs
129 constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
130 constraintRow.m_rhsPenetration = 0.f;
131
132 } else
133 {
134 //split position and velocity into rhs and m_rhsPenetration
135 constraintRow.m_rhs = velocityImpulse;
136 constraintRow.m_rhsPenetration = penetrationImpulse;
137 }
138 }
139 }
140
141 }
142
143
144
145
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:58
btMultiBodyConstraint::fillMultiBodyConstraint
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &contactNormalOnB, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraint.cpp:55
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:138
btMultiBodyJointLimitConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointLimitConstraint.cpp:84
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
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:27
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultiBodyJointLimitConstraint.h
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:145
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:128
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btMultiBodyJointLimitConstraint::m_upperBound
btScalar m_upperBound
Definition: btMultiBodyJointLimitConstraint.h:27
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:130
btMultiBodyJointLimitConstraint::m_lowerBound
btScalar m_lowerBound
Definition: btMultiBodyJointLimitConstraint.h:26
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition: btMultiBody.cpp:377
btMultiBody
Definition: btMultiBody.h:37
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:94
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint
virtual ~btMultiBodyJointLimitConstraint()
Definition: btMultiBodyJointLimitConstraint.cpp:45
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodyJointLimitConstraint::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointLimitConstraint.cpp:49
btMultiBodyConstraint::getPosition
btScalar getPosition(int row) const
Definition: btMultiBodyConstraint.h:111
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:73
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyLinkCollider.h
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:53
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:117
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointLimitConstraint.cpp:24
btMultiBody.h
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:398
btMultiBodyConstraint::setPosition
void setPosition(int row, btScalar pos)
Definition: btMultiBodyConstraint.h:116
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:51
btCollisionObject.h
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodyJointLimitConstraint::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointLimitConstraint.cpp:65
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:522
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:52
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:247
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48