Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Featherstone / btMultiBodyJointMotor.cpp
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
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
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->getLink(m_linkA).m_dofOffset + linkDoF);
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
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 }
61 btMultiBodyJointMotor::~btMultiBodyJointMotor()
62 {
63 }
64
65 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
78 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
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                 {
122                         //expect either prismatic or revolute joint type for now
123                         btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
124                         switch (m_bodyA->getLink(m_linkA).m_jointType)
125                         {
126                                 case btMultibodyLink::eRevolute:
127                                 {
128                                         constraintRow.m_contactNormal1.setZero();
129                                         constraintRow.m_contactNormal2.setZero();
130                                         btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
131                                         constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
132                                         constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
133                                         
134                                         break;
135                                 }
136                                 case btMultibodyLink::ePrismatic:
137                                 {
138                                         btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
139                                         constraintRow.m_contactNormal1=prismaticAxisInWorld;
140                                         constraintRow.m_contactNormal2=-prismaticAxisInWorld;
141                                         constraintRow.m_relpos1CrossNormal.setZero();
142                                         constraintRow.m_relpos2CrossNormal.setZero();
143                                         
144                                         break;
145                                 }
146                                 default:
147                                 {
148                                         btAssert(0);
149                                 }
150                         };
151                         
152                 }
153
154         }
155
156 }
157