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,link,1,true),
26 m_desiredVelocity(desiredVelocity)
27 {
28 int linkDoF = 0;
29
30 m_maxAppliedImpulse = maxMotorImpulse;
31 // the data.m_jacobians never change, so may as well
32 // initialize them here
33
34 // note: we rely on the fact that data.m_jacobians are
35 // always initialized to zero by the Constraint ctor
36
37 unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
38
39 // row 0: the lower bound
40 // row 0: the lower bound
41 jacobianA(0)[offset] = 1;
42 }
43
44
45 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
46 //:btMultiBodyConstraint(body,0,link,-1,1,true),
47 :btMultiBodyConstraint(body,body,link,link,1,true),
48 m_desiredVelocity(desiredVelocity)
49 {
50 btAssert(linkDoF < body->getLink(link).m_dofCount);
51
52 m_maxAppliedImpulse = maxMotorImpulse;
53 // the data.m_jacobians never change, so may as well
54 // initialize them here
55
56 // note: we rely on the fact that data.m_jacobians are
57 // always initialized to zero by the Constraint ctor
58
59 unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
60
61 // row 0: the lower bound
62 // row 0: the lower bound
63 jacobianA(0)[offset] = 1;
64 }
65 btMultiBodyJointMotor::~btMultiBodyJointMotor()
66 {
67 }
68
69 int btMultiBodyJointMotor::getIslandIdA() const
70 {
71 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
72 if (col)
73 return col->getIslandTag();
74 for (int i=0;i<m_bodyA->getNumLinks();i++)
75 {
76 if (m_bodyA->getLink(i).m_collider)
77 return m_bodyA->getLink(i).m_collider->getIslandTag();
78 }
79 return -1;
80 }
81
82 int btMultiBodyJointMotor::getIslandIdB() const
83 {
84 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
85 if (col)
86 return col->getIslandTag();
87
88 for (int i=0;i<m_bodyB->getNumLinks();i++)
89 {
90 col = m_bodyB->getLink(i).m_collider;
91 if (col)
92 return col->getIslandTag();
93 }
94 return -1;
95 }
96
97
98 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
99 btMultiBodyJacobianData& data,
100 const btContactSolverInfo& infoGlobal)
101 {
102 // only positions need to be updated -- data.m_jacobians and force
103 // directions were set in the ctor and never change.
104
105
106 const btScalar posError = 0;
107 const btVector3 dummy(0, 0, 0);
108
109 for (int row=0;row<getNumRows();row++)
110 {
111 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
112
113
114 fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
115 }
116
117 }
118
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:82
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:138
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:27
btMultiBodyJointMotor::~btMultiBodyJointMotor
virtual ~btMultiBodyJointMotor()
Definition: btMultiBodyJointMotor.cpp:65
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:58
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:145
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:128
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:130
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:37
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:94
btMultiBodyJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointMotor.cpp:69
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointMotor.cpp:98
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:117
btMultiBody.h
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:398
btCollisionObject.h
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:124
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:522
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:247
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