Bullet Collision Detection & Physics Library
btMultiBodyPoint2Point.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 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
22
23#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
24 #define BTMBP2PCONSTRAINT_DIM 3
25#else
26 #define BTMBP2PCONSTRAINT_DIM 6
27#endif
28
29btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
30 :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
31 m_rigidBodyA(0),
32 m_rigidBodyB(bodyB),
33 m_pivotInA(pivotInA),
34 m_pivotInB(pivotInB)
35{
36 m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
37}
38
39btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
40 :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
41 m_rigidBodyA(0),
42 m_rigidBodyB(0),
43 m_pivotInA(pivotInA),
44 m_pivotInB(pivotInB)
45{
46 m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
47}
48
50{
51 //not implemented yet
52 btAssert(0);
53}
54
56{
57}
58
59
61{
62 if (m_rigidBodyA)
63 return m_rigidBodyA->getIslandTag();
64
65 if (m_bodyA)
66 {
68 if (col)
69 return col->getIslandTag();
70 for (int i=0;i<m_bodyA->getNumLinks();i++)
71 {
74 }
75 }
76 return -1;
77}
78
80{
81 if (m_rigidBodyB)
82 return m_rigidBodyB->getIslandTag();
83 if (m_bodyB)
84 {
86 if (col)
87 return col->getIslandTag();
88
89 for (int i=0;i<m_bodyB->getNumLinks();i++)
90 {
91 col = m_bodyB->getLink(i).m_collider;
92 if (col)
93 return col->getIslandTag();
94 }
95 }
96 return -1;
97}
98
99
100
103 const btContactSolverInfo& infoGlobal)
104{
105
106// int i=1;
107int numDim = BTMBP2PCONSTRAINT_DIM;
108 for (int i=0;i<numDim;i++)
109 {
110
111 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
112 //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
113 constraintRow.m_orgConstraint = this;
114 constraintRow.m_orgDofIndex = i;
115 constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
116 constraintRow.m_contactNormal1.setValue(0,0,0);
117 constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
118 constraintRow.m_contactNormal2.setValue(0,0,0);
119 constraintRow.m_angularComponentA.setValue(0,0,0);
120 constraintRow.m_angularComponentB.setValue(0,0,0);
121
122 constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
123 constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
124
125 btVector3 contactNormalOnB(0,0,0);
126#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
127 contactNormalOnB[i] = -1;
128#else
129 contactNormalOnB[i%3] = -1;
130#endif
131
132
133 // Convert local points back to world
134 btVector3 pivotAworld = m_pivotInA;
135 if (m_rigidBodyA)
136 {
137
140 } else
141 {
142 if (m_bodyA)
143 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
144 }
145 btVector3 pivotBworld = m_pivotInB;
146 if (m_rigidBodyB)
147 {
150 } else
151 {
152 if (m_bodyB)
153 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
154
155 }
156
157 btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
158
159#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
160
161
162 fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
163 contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
164 posError,
165 infoGlobal,
167 );
168 //@todo: support the case of btMultiBody versus btRigidBody,
169 //see btPoint2PointConstraint::getInfo2NonVirtual
170#else
171 const btVector3 dummy(0, 0, 0);
172
173 btAssert(m_bodyA->isMultiDof());
174
175 btScalar* jac1 = jacobianA(i);
176 const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
177 const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
178
179 m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
180
181 fillMultiBodyConstraint(constraintRow, data, jac1, 0,
182 dummy, dummy, dummy, //sucks but let it be this way "for the time being"
183 posError,
184 infoGlobal,
186 );
187#endif
188 }
189}
190
192{
193 btTransform tr;
194 tr.setIdentity();
195
196 if (m_rigidBodyA)
197 {
199 tr.setOrigin(pivot);
200 drawer->drawTransform(tr, 0.1);
201 }
202 if (m_bodyA)
203 {
205 tr.setOrigin(pivotAworld);
206 drawer->drawTransform(tr, 0.1);
207 }
208 if (m_rigidBodyB)
209 {
210 // that ideally should draw the same frame
212 tr.setOrigin(pivot);
213 drawer->drawTransform(tr, 0.1);
214 }
215 if (m_bodyB)
216 {
218 tr.setOrigin(pivotBworld);
219 drawer->drawTransform(tr, 0.1);
220 }
221}
#define BTMBP2PCONSTRAINT_DIM
This file was written by Erwin Coumans.
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define btAssert(x)
Definition: btScalar.h:131
void resize(int newsize, const T &fillData=T())
int getIslandTag() const
int getCompanionId() const
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:30
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
btAlignedObjectArray< btScalar > m_data
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)
btMultiBodyPoint2Point(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB)
virtual int getIslandIdB() const
virtual void debugDraw(class btIDebugDraw *drawer)
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
virtual int getIslandIdA() const
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:164
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint