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
16 ///This file was written by Erwin Coumans
17
18 #include "btMultiBodyJointLimitConstraint.h"
19 #include "btMultiBody.h"
20 #include "btMultiBodyLinkCollider.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23
24
btMultiBodyJointLimitConstraint(btMultiBody * body,int link,btScalar lower,btScalar upper)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
finalizeMultiDof()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->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
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
~btMultiBodyJointLimitConstraint()52 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
53 {
54 }
55
getIslandIdA() const56 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
getIslandIdB() const72 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
createConstraintRows(btMultiBodyConstraintArray & constraintRows,btMultiBodyJacobianData & data,const btContactSolverInfo & infoGlobal)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,posError,infoGlobal,0,m_maxAppliedImpulse);
126
127 if (m_bodyA->isMultiDof())
128 {
129 //expect either prismatic or revolute joint type for now
130 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
131 switch (m_bodyA->getLink(m_linkA).m_jointType)
132 {
133 case btMultibodyLink::eRevolute:
134 {
135 constraintRow.m_contactNormal1.setZero();
136 constraintRow.m_contactNormal2.setZero();
137 btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
138 constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
139 constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
140
141 break;
142 }
143 case btMultibodyLink::ePrismatic:
144 {
145 btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
146 constraintRow.m_contactNormal1=prismaticAxisInWorld;
147 constraintRow.m_contactNormal2=-prismaticAxisInWorld;
148 constraintRow.m_relpos1CrossNormal.setZero();
149 constraintRow.m_relpos2CrossNormal.setZero();
150
151 break;
152 }
153 default:
154 {
155 btAssert(0);
156 }
157 };
158
159 }
160
161 {
162 btScalar penetration = getPosition(row);
163 btScalar positionalError = 0.f;
164 btScalar velocityError = - rel_vel;// * damping;
165 btScalar erp = infoGlobal.m_erp2;
166 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
167 {
168 erp = infoGlobal.m_erp;
169 }
170 if (penetration>0)
171 {
172 positionalError = 0;
173 velocityError = -penetration / infoGlobal.m_timeStep;
174 } else
175 {
176 positionalError = -penetration * erp/infoGlobal.m_timeStep;
177 }
178
179 btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
180 btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
181 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
182 {
183 //combine position and velocity into rhs
184 constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
185 constraintRow.m_rhsPenetration = 0.f;
186
187 } else
188 {
189 //split position and velocity into rhs and m_rhsPenetration
190 constraintRow.m_rhs = velocityImpulse;
191 constraintRow.m_rhsPenetration = penetrationImpulse;
192 }
193 }
194 }
195
196 }
197
198
199
200
201