Bullet Collision Detection & Physics Library
btMultiBodySphericalJointMotor.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
17
19#include "btMultiBody.h"
24
26 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
27 m_desiredVelocity(0, 0, 0),
28 m_desiredPosition(0,0,0,1),
29 m_use_multi_dof_params(false),
30 m_kd(1., 1., 1.),
31 m_kp(0.2, 0.2, 0.2),
32 m_erp(1),
33 m_rhsClamp(SIMD_INFINITY),
34 m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
35 m_damping(1.0, 1.0, 1.0)
36{
37
39}
40
41
43{
45 // note: we rely on the fact that data.m_jacobians are
46 // always initialized to zero by the Constraint ctor
47 int linkDoF = 0;
48 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
49
50 // row 0: the lower bound
51 // row 0: the lower bound
52 jacobianA(0)[offset] = 1;
53
55}
56
57
59{
60}
61
63{
64 if (this->m_linkA < 0)
65 {
67 if (col)
68 return col->getIslandTag();
69 }
70 else
71 {
73 {
75 }
76 }
77 return -1;
78}
79
81{
82 if (m_linkB < 0)
83 {
85 if (col)
86 return col->getIslandTag();
87 }
88 else
89 {
91 {
93 }
94 }
95 return -1;
96}
97
101{
102 // only positions need to be updated -- data.m_jacobians and force
103 // directions were set in the ctor and never change.
104
106 {
108 }
109
110 //don't crash
112 return;
113
114
115 if (m_maxAppliedImpulse == 0.f)
116 return;
117
118 const btScalar posError = 0;
119 const btVector3 dummy(0, 0, 0);
120
121
122 btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
123
129
133
134
135
136 for (int row = 0; row < getNumRows(); row++)
137 {
139
140 int dof = row;
141
144
145 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
147
151 btScalar posError = 0;
152 {
155 {
157 {
159 double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
160 posError = kp*angleDiff[row % 3];
163 btVector3(0,0,0), dummy, dummy,
164 posError,
167 1.0, false, 0, 0,
168 m_damping[row % 3]);
169 constraintRow.m_orgConstraint = this;
170 constraintRow.m_orgDofIndex = row;
171 break;
172 }
173 default:
174 {
175 btAssert(0);
176 }
177 };
178 }
179 }
180}
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
@ MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define SIMD_INFINITY
Definition btScalar.h:544
#define btAssert(x)
Definition btScalar.h:153
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
void setIdentity()
Set the matrix to the identity.
btScalar * jacobianA(int row)
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0, btScalar damping=1.0)
btMultiBodySphericalJointMotor(btMultiBody *body, int link, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
btScalar * getJointPosMultiDof(int i)
const btMultibodyLink & getLink(int index) const
const btMultiBodyLinkCollider * getBaseCollider() const
btScalar * getJointVelMultiDof(int i)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btQuaternion inverse() const
Return the inverse of this quaternion.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...