Bullet Collision Detection & Physics Library
btMultiBodyJointMotor.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 "btMultiBodyJointMotor.h"
19 #include "btMultiBody.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 
37 {
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 
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 }
62 {
63 }
64 
66 {
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 
79 {
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 
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 
102  {
104  }
105 
106  //don't crash
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
126  {
128  {
129  constraintRow.m_contactNormal1.setZero();
130  constraintRow.m_contactNormal2.setZero();
132  constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
133  constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
134 
135  break;
136  }
138  {
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 
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)
virtual int getIslandIdB() const
btScalar * jacobianB(int row)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
int getNumLinks() const
Definition: btMultiBody.h:154
btMultiBodyConstraint * m_orgConstraint
#define btAssert(x)
Definition: btScalar.h:113
btScalar * jacobianA(int row)
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
virtual int getIslandIdA() const
void setZero()
Definition: btVector3.h:671
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
int getIslandTag() const
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:133
bool isMultiDof()
Definition: btMultiBody.h:579
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278