Bullet Collision Detection & Physics Library: btMultiBodyJointMotor.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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"
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
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
btMultiBodyJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointMotor.cpp:78
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:155
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btMultiBodyJointMotor::~btMultiBodyJointMotor
virtual ~btMultiBodyJointMotor()
Definition: btMultiBodyJointMotor.cpp:61
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:155
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:73
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:127
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:121
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btMultiBodyJointMotor.h
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:147
btMultiBodyJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyJointMotor.cpp:36
btMultiBodyJointMotor::btMultiBodyJointMotor
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointMotor.cpp:24
btMultiBody
Definition: btMultiBody.h:51
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:98
btMultiBodyJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointMotor.cpp:65
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:57
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:96
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:53
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:70
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointMotor.cpp:94
btMultiBodyLinkCollider.h
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:104
btMultiBody.h
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:398
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btCollisionObject.h
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:131
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:37
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:245
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:74
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodyJointMotor::m_desiredVelocity
btScalar m_desiredVelocity
Definition: btMultiBodyJointMotor.h:29
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48