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 "btMultiBodyJointMotor.h"
19 #include "btMultiBody.h"
20 #include "btMultiBodyLinkCollider.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23
btMultiBodyJointMotor(btMultiBody * body,int link,btScalar desiredVelocity,btScalar maxMotorImpulse)24 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
25 :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
26 m_desiredVelocity(desiredVelocity)
27 {
28
29 m_maxAppliedImpulse = maxMotorImpulse;
30 // the data.m_jacobians never change, so may as well
31 // initialize them here
32
33
34 }
35
finalizeMultiDof()36 void btMultiBodyJointMotor::finalizeMultiDof()
37 {
38 allocateJacobiansMultiDof();
39 // note: we rely on the fact that data.m_jacobians are
40 // always initialized to zero by the Constraint ctor
41 int linkDoF = 0;
42 unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
43
44 // row 0: the lower bound
45 // row 0: the lower bound
46 jacobianA(0)[offset] = 1;
47
48 m_numDofsFinalized = m_jacSizeBoth;
49 }
50
btMultiBodyJointMotor(btMultiBody * body,int link,int linkDoF,btScalar desiredVelocity,btScalar maxMotorImpulse)51 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
52 //:btMultiBodyConstraint(body,0,link,-1,1,true),
53 :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
54 m_desiredVelocity(desiredVelocity)
55 {
56 btAssert(linkDoF < body->getLink(link).m_dofCount);
57
58 m_maxAppliedImpulse = maxMotorImpulse;
59
60 }
~btMultiBodyJointMotor()61 btMultiBodyJointMotor::~btMultiBodyJointMotor()
62 {
63 }
64
getIslandIdA() const65 int btMultiBodyJointMotor::getIslandIdA() const
66 {
67 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
68 if (col)
69 return col->getIslandTag();
70 for (int i=0;i<m_bodyA->getNumLinks();i++)
71 {
72 if (m_bodyA->getLink(i).m_collider)
73 return m_bodyA->getLink(i).m_collider->getIslandTag();
74 }
75 return -1;
76 }
77
getIslandIdB() const78 int btMultiBodyJointMotor::getIslandIdB() const
79 {
80 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
81 if (col)
82 return col->getIslandTag();
83
84 for (int i=0;i<m_bodyB->getNumLinks();i++)
85 {
86 col = m_bodyB->getLink(i).m_collider;
87 if (col)
88 return col->getIslandTag();
89 }
90 return -1;
91 }
92
93
createConstraintRows(btMultiBodyConstraintArray & constraintRows,btMultiBodyJacobianData & data,const btContactSolverInfo & infoGlobal)94 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
95 btMultiBodyJacobianData& data,
96 const btContactSolverInfo& infoGlobal)
97 {
98 // only positions need to be updated -- data.m_jacobians and force
99 // directions were set in the ctor and never change.
100
101 if (m_numDofsFinalized != m_jacSizeBoth)
102 {
103 finalizeMultiDof();
104 }
105
106 //don't crash
107 if (m_numDofsFinalized != m_jacSizeBoth)
108 return;
109
110 const btScalar posError = 0;
111 const btVector3 dummy(0, 0, 0);
112
113 for (int row=0;row<getNumRows();row++)
114 {
115 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
116
117
118 fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
119 constraintRow.m_orgConstraint = this;
120 constraintRow.m_orgDofIndex = row;
121 if (m_bodyA->isMultiDof())
122 {
123 //expect either prismatic or revolute joint type for now
124 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
125 switch (m_bodyA->getLink(m_linkA).m_jointType)
126 {
127 case btMultibodyLink::eRevolute:
128 {
129 constraintRow.m_contactNormal1.setZero();
130 constraintRow.m_contactNormal2.setZero();
131 btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
132 constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
133 constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
134
135 break;
136 }
137 case btMultibodyLink::ePrismatic:
138 {
139 btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
140 constraintRow.m_contactNormal1=prismaticAxisInWorld;
141 constraintRow.m_contactNormal2=-prismaticAxisInWorld;
142 constraintRow.m_relpos1CrossNormal.setZero();
143 constraintRow.m_relpos2CrossNormal.setZero();
144
145 break;
146 }
147 default:
148 {
149 btAssert(0);
150 }
151 };
152
153 }
154
155 }
156
157 }
158
159