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

Bullet Collision Detection & Physics Library

btGeneric6DofSpring2Constraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://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
16/*
172014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
18Pros:
19- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
20- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
21- Servo motor functionality
22- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
23- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
24
25Cons:
26- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
27- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
28*/
29
32
33/*
342007-09-09
35btGeneric6DofConstraint Refactored by Francisco Le?n
36email: projectileman@yahoo.com
37http://gimpact.sf.net
38*/
39
40#include "btGeneric6DofSpring2Constraint.h"
41#include "BulletDynamics/Dynamics/btRigidBody.h"
42#include "LinearMath/btTransformUtil.h"
43#include <cmath>
44#include <new>
45
46btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
47 : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
48{
49 calculateTransforms();
50}
51
52 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
53 : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
54{
56 m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
57 calculateTransforms();
58}
59
60 btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
61{
62 int i = index % 3;
63 int j = index / 3;
64 return mat[i][j];
65}
66
67// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
68
69 bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
70{
71 // rot = cy*cz -cy*sz sy
72 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
73 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
74
75 btScalar fi = btGetMatrixElem(mat, 2);
76 if (fi < btScalar(1.0f))
77 {
78 if (fi > btScalar(-1.0f))
79 {
80 xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
81 xyz[1] = btAsin(btGetMatrixElem(mat, 2));
82 xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
83 return true;
84 }
85 else
86 {
87 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
88 xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
89 xyz[1] = -SIMD_HALF_PI;
90 xyz[2] = btScalar(0.0);
91 return false;
92 }
93 }
94 else
95 {
96 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
97 xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
98 xyz[1] = SIMD_HALF_PI;
99 xyz[2] = 0.0;
100 }
101 return false;
102}
103
104 bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz)
105{
106 // rot = cy*cz -sz sy*cz
107 // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
108 // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
109
110 btScalar fi = btGetMatrixElem(mat, 1);
111 if (fi < btScalar(1.0f))
112 {
113 if (fi > btScalar(-1.0f))
114 {
115 xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
116 xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
117 xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
118 return true;
119 }
120 else
121 {
122 xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
123 xyz[1] = btScalar(0.0);
124 xyz[2] = SIMD_HALF_PI;
125 return false;
126 }
127 }
128 else
129 {
130 xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
131 xyz[1] = 0.0;
132 xyz[2] = -SIMD_HALF_PI;
133 }
134 return false;
135}
136
137 bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz)
138{
139 // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
140 // cx*sz cx*cz -sx
141 // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
142
143 btScalar fi = btGetMatrixElem(mat, 5);
144 if (fi < btScalar(1.0f))
145 {
146 if (fi > btScalar(-1.0f))
147 {
148 xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
149 xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
150 xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
151 return true;
152 }
153 else
154 {
155 xyz[0] = SIMD_HALF_PI;
156 xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
157 xyz[2] = btScalar(0.0);
158 return false;
159 }
160 }
161 else
162 {
163 xyz[0] = -SIMD_HALF_PI;
164 xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
165 xyz[2] = 0.0;
166 }
167 return false;
168}
169
170 bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz)
171{
172 // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
173 // sz cz*cx -cz*sx
174 // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
175
176 btScalar fi = btGetMatrixElem(mat, 3);
177 if (fi < btScalar(1.0f))
178 {
179 if (fi > btScalar(-1.0f))
180 {
181 xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
182 xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
183 xyz[2] = btAsin(btGetMatrixElem(mat, 3));
184 return true;
185 }
186 else
187 {
188 xyz[0] = btScalar(0.0);
189 xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
190 xyz[2] = -SIMD_HALF_PI;
191 return false;
192 }
193 }
194 else
195 {
196 xyz[0] = btScalar(0.0);
197 xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
198 xyz[2] = SIMD_HALF_PI;
199 }
200 return false;
201}
202
203 bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz)
204{
205 // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
206 // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
207 // -cx*sy sx cx*cy
208
209 btScalar fi = btGetMatrixElem(mat, 7);
210 if (fi < btScalar(1.0f))
211 {
212 if (fi > btScalar(-1.0f))
213 {
214 xyz[0] = btAsin(btGetMatrixElem(mat, 7));
215 xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
216 xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
217 return true;
218 }
219 else
220 {
221 xyz[0] = -SIMD_HALF_PI;
222 xyz[1] = btScalar(0.0);
223 xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
224 return false;
225 }
226 }
227 else
228 {
229 xyz[0] = SIMD_HALF_PI;
230 xyz[1] = btScalar(0.0);
231 xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
232 }
233 return false;
234}
235
236 bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz)
237{
238 // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
239 // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
240 // -sy cy*sx cy*cx
241
242 btScalar fi = btGetMatrixElem(mat, 6);
243 if (fi < btScalar(1.0f))
244 {
245 if (fi > btScalar(-1.0f))
246 {
247 xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
248 xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
249 xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
250 return true;
251 }
252 else
253 {
254 xyz[0] = btScalar(0.0);
255 xyz[1] = SIMD_HALF_PI;
256 xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
257 return false;
258 }
259 }
260 else
261 {
262 xyz[0] = btScalar(0.0);
263 xyz[1] = -SIMD_HALF_PI;
264 xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
265 }
266 return false;
267}
268
269 void btGeneric6DofSpring2Constraint::calculateAngleInfo()
270{
271 btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
272 switch (m_rotateOrder)
273 {
274 case RO_XYZ:
275 matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
276 break;
277 case RO_XZY:
278 matrixToEulerXZY(relative_frame, m_calculatedAxisAngleDiff);
279 break;
280 case RO_YXZ:
281 matrixToEulerYXZ(relative_frame, m_calculatedAxisAngleDiff);
282 break;
283 case RO_YZX:
284 matrixToEulerYZX(relative_frame, m_calculatedAxisAngleDiff);
285 break;
286 case RO_ZXY:
287 matrixToEulerZXY(relative_frame, m_calculatedAxisAngleDiff);
288 break;
289 case RO_ZYX:
290 matrixToEulerZYX(relative_frame, m_calculatedAxisAngleDiff);
291 break;
292 default:
293 btAssert(false);
294 }
295 // in euler angle mode we do not actually constrain the angular velocity
296 // along the axes axis[0] and axis[2] (although we do use axis[1]) :
297 //
298 // to get constrain w2-w1 along ...not
299 // ------ --------------------- ------
300 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
301 // d(angle[1])/dt = 0 ax[1]
302 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
303 //
304 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
305 // to prove the result for angle[0], write the expression for angle[0] from
306 // GetInfo1 then take the derivative. to prove this for angle[2] it is
307 // easier to take the euler rate expression for d(angle[2])/dt with respect
308 // to the components of w and set that to 0.
309 switch (m_rotateOrder)
310 {
311 case RO_XYZ:
312 {
313 //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
314 //The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
315 //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
316 //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
317 // x' = Nperp = N.cross(axis2)
318 // y' = N = axis2.cross(axis0)
319 // z' = z
320 //
321 // x" = X
322 // y" = y'
323 // z" = ??
324 //in other words:
325 //first rotate around z
326 //second rotate around y'= z.cross(X)
327 //third rotate around x" = X
328 //Original XYZ extrinsic rotation order.
329 //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X)
330 btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
331 btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
332 m_calculatedAxis[1] = axis2.cross(axis0);
333 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
334 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
335 break;
336 }
337 case RO_XZY:
338 {
339 //planes: xz,ZY normals: y, X
340 //first rotate around y
341 //second rotate around z'= y.cross(X)
342 //third rotate around x" = X
343 btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
344 btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
345 m_calculatedAxis[2] = axis0.cross(axis1);
346 m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
347 m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
348 break;
349 }
350 case RO_YXZ:
351 {
352 //planes: yx,XZ normals: z, Y
353 //first rotate around z
354 //second rotate around x'= z.cross(Y)
355 //third rotate around y" = Y
356 btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
357 btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
358 m_calculatedAxis[0] = axis1.cross(axis2);
359 m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
360 m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
361 break;
362 }
363 case RO_YZX:
364 {
365 //planes: yz,ZX normals: x, Y
366 //first rotate around x
367 //second rotate around z'= x.cross(Y)
368 //third rotate around y" = Y
369 btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
370 btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
371 m_calculatedAxis[2] = axis0.cross(axis1);
372 m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
373 m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
374 break;
375 }
376 case RO_ZXY:
377 {
378 //planes: zx,XY normals: y, Z
379 //first rotate around y
380 //second rotate around x'= y.cross(Z)
381 //third rotate around z" = Z
382 btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
383 btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
384 m_calculatedAxis[0] = axis1.cross(axis2);
385 m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
386 m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
387 break;
388 }
389 case RO_ZYX:
390 {
391 //planes: zy,YX normals: x, Z
392 //first rotate around x
393 //second rotate around y' = x.cross(Z)
394 //third rotate around z" = Z
395 btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
396 btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
397 m_calculatedAxis[1] = axis2.cross(axis0);
398 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
399 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
400 break;
401 }
402 default:
403 btAssert(false);
404 }
405
406 m_calculatedAxis[0].normalize();
407 m_calculatedAxis[1].normalize();
408 m_calculatedAxis[2].normalize();
409}
410
411 void btGeneric6DofSpring2Constraint::calculateTransforms()
412{
413 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
414}
415
416 void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
417{
418 m_calculatedTransformA = transA * m_frameInA;
419 m_calculatedTransformB = transB * m_frameInB;
420 calculateLinearInfo();
421 calculateAngleInfo();
422
423 btScalar miA = getRigidBodyA().getInvMass();
424 btScalar miB = getRigidBodyB().getInvMass();
425 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
426 btScalar miS = miA + miB;
427 if (miS > btScalar(0.f))
428 {
429 m_factA = miB / miS;
430 }
431 else
432 {
433 m_factA = btScalar(0.5f);
434 }
435 m_factB = btScalar(1.0f) - m_factA;
436}
437
438 void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
439{
440 btScalar angle = m_calculatedAxisAngleDiff[axis_index];
441 angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
442 m_angularLimits[axis_index].m_currentPosition = angle;
443 m_angularLimits[axis_index].testLimitValue(angle);
444}
445
446 void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info)
447{
448 //prepare constraint
449 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
450 info->m_numConstraintRows = 0;
451 info->nub = 0;
452 int i;
453 //test linear limits
454 for (i = 0; i < 3; i++)
455 {
456 if (m_linearLimits.m_currentLimit[i] == 4)
457 info->m_numConstraintRows += 2;
458 else if (m_linearLimits.m_currentLimit[i] != 0)
459 info->m_numConstraintRows += 1;
460 if (m_linearLimits.m_enableMotor[i]) info->m_numConstraintRows += 1;
461 if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
462 }
463 //test angular limits
464 for (i = 0; i < 3; i++)
465 {
466 testAngularLimitMotor(i);
467 if (m_angularLimits[i].m_currentLimit == 4)
468 info->m_numConstraintRows += 2;
469 else if (m_angularLimits[i].m_currentLimit != 0)
470 info->m_numConstraintRows += 1;
471 if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
472 if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
473 }
474}
475
476 void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info)
477{
478 const btTransform& transA = m_rbA.getCenterOfMassTransform();
479 const btTransform& transB = m_rbB.getCenterOfMassTransform();
480 const btVector3& linVelA = m_rbA.getLinearVelocity();
481 const btVector3& linVelB = m_rbB.getLinearVelocity();
482 const btVector3& angVelA = m_rbA.getAngularVelocity();
483 const btVector3& angVelB = m_rbB.getAngularVelocity();
484
485 // for stability better to solve angular limits first
486 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
487 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
488}
489
490 int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
491{
492 //solve linear limits
493 btRotationalLimitMotor2 limot;
494 for (int i = 0; i < 3; i++)
495 {
496 if (m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
497 { // re-use rotational motor code
498 limot.m_bounce = m_linearLimits.m_bounce[i];
499 limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
500 limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
501 limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
502 limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
503 limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
504 limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
505 limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
506 limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
507 limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
508 limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
509 limot.m_springDamping = m_linearLimits.m_springDamping[i];
510 limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
511 limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
512 limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
513 limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
514 limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
515 limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
516 btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
517 int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
518 limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
519 limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
520 limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
521 limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
522
523 //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
524 int indx1 = (i + 1) % 3;
525 int indx2 = (i + 2) % 3;
526 int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
527#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
528 bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
529 m_angularLimits[indx1].m_currentLimit == 2 ||
530 (m_angularLimits[indx1].m_currentLimit == 3 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
531 (m_angularLimits[indx1].m_currentLimit == 4 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
532 bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
533 m_angularLimits[indx2].m_currentLimit == 2 ||
534 (m_angularLimits[indx2].m_currentLimit == 3 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
535 (m_angularLimits[indx2].m_currentLimit == 4 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
536 if (indx1Violated && indx2Violated)
537 {
538 rotAllowed = 0;
539 }
540 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
541 }
542 }
543 return row;
544}
545
546 int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
547{
548 int row = row_offset;
549
550 //order of rotational constraint rows
551 int cIdx[] = {0, 1, 2};
552 switch (m_rotateOrder)
553 {
554 case RO_XYZ:
555 cIdx[0] = 0;
556 cIdx[1] = 1;
557 cIdx[2] = 2;
558 break;
559 case RO_XZY:
560 cIdx[0] = 0;
561 cIdx[1] = 2;
562 cIdx[2] = 1;
563 break;
564 case RO_YXZ:
565 cIdx[0] = 1;
566 cIdx[1] = 0;
567 cIdx[2] = 2;
568 break;
569 case RO_YZX:
570 cIdx[0] = 1;
571 cIdx[1] = 2;
572 cIdx[2] = 0;
573 break;
574 case RO_ZXY:
575 cIdx[0] = 2;
576 cIdx[1] = 0;
577 cIdx[2] = 1;
578 break;
579 case RO_ZYX:
580 cIdx[0] = 2;
581 cIdx[1] = 1;
582 cIdx[2] = 0;
583 break;
584 default:
585 btAssert(false);
586 }
587
588 for (int ii = 0; ii < 3; ii++)
589 {
590 int i = cIdx[ii];
591 if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
592 {
593 btVector3 axis = getAxis(i);
594 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
595 if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
596 {
597 m_angularLimits[i].m_stopCFM = info->cfm[0];
598 }
599 if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
600 {
601 m_angularLimits[i].m_stopERP = info->erp;
602 }
603 if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
604 {
605 m_angularLimits[i].m_motorCFM = info->cfm[0];
606 }
607 if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
608 {
609 m_angularLimits[i].m_motorERP = info->erp;
610 }
611 row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
612 }
613 }
614
615 return row;
616}
617
618 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
619{
620 m_frameInA = frameA;
621 m_frameInB = frameB;
622 buildJacobian();
623 calculateTransforms();
624}
625
626 void btGeneric6DofSpring2Constraint::calculateLinearInfo()
627{
628 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
629 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
630 for (int i = 0; i < 3; i++)
631 {
632 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
633 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
634 }
635}
636
637 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
638{
639 btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
640 btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
641
642 J1[srow + 0] = ax1[0];
643 J1[srow + 1] = ax1[1];
644 J1[srow + 2] = ax1[2];
645
646 J2[srow + 0] = -ax1[0];
647 J2[srow + 1] = -ax1[1];
648 J2[srow + 2] = -ax1[2];
649
650 if (!rotational)
651 {
652 btVector3 tmpA, tmpB, relA, relB;
653 // get vector from bodyB to frameB in WCS
654 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
655 // same for bodyA
656 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
657 tmpA = relA.cross(ax1);
658 tmpB = relB.cross(ax1);
659 if (m_hasStaticBody && (!rotAllowed))
660 {
661 tmpA *= m_factA;
662 tmpB *= m_factB;
663 }
664 int i;
665 for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
666 for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
667 }
668}
669
670 int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
671 btRotationalLimitMotor2* limot,
672 const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
673 btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
674{
675 int count = 0;
676 int srow = row * info->rowskip;
677
678 if (limot->m_currentLimit == 4)
679 {
680 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
681
682 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
683 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
684 if (rotational)
685 {
686 if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
687 {
688 btScalar bounceerror = -limot->m_bounce * vel;
689 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
690 }
691 }
692 else
693 {
694 if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
695 {
696 btScalar bounceerror = -limot->m_bounce * vel;
697 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
698 }
699 }
700 info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
701 info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
702 info->cfm[srow] = limot->m_stopCFM;
703 srow += info->rowskip;
704 ++count;
705
706 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
707 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
708 if (rotational)
709 {
710 if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
711 {
712 btScalar bounceerror = -limot->m_bounce * vel;
713 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
714 }
715 }
716 else
717 {
718 if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
719 {
720 btScalar bounceerror = -limot->m_bounce * vel;
721 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
722 }
723 }
724 info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
725 info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
726 info->cfm[srow] = limot->m_stopCFM;
727 srow += info->rowskip;
728 ++count;
729 }
730 else if (limot->m_currentLimit == 3)
731 {
732 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
733 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
734 info->m_lowerLimit[srow] = -SIMD_INFINITY;
735 info->m_upperLimit[srow] = SIMD_INFINITY;
736 info->cfm[srow] = limot->m_stopCFM;
737 srow += info->rowskip;
738 ++count;
739 }
740
741 if (limot->m_enableMotor && !limot->m_servoMotor)
742 {
743 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
744 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
745 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
746 limot->m_loLimit,
747 limot->m_hiLimit,
748 tag_vel,
749 info->fps * limot->m_motorERP);
750 info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
751 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
752 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
753 info->cfm[srow] = limot->m_motorCFM;
754 srow += info->rowskip;
755 ++count;
756 }
757
758 if (limot->m_enableMotor && limot->m_servoMotor)
759 {
760 btScalar error = limot->m_currentPosition - limot->m_servoTarget;
761 btScalar curServoTarget = limot->m_servoTarget;
762 if (rotational)
763 {
764 if (error > SIMD_PI)
765 {
766 error -= SIMD_2_PI;
767 curServoTarget += SIMD_2_PI;
768 }
769 if (error < -SIMD_PI)
770 {
771 error += SIMD_2_PI;
772 curServoTarget -= SIMD_2_PI;
773 }
774 }
775
776 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
777 btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
778 btScalar tag_vel = -targetvelocity;
779 btScalar mot_fact;
780 if (error != 0)
781 {
782 btScalar lowLimit;
783 btScalar hiLimit;
784 if (limot->m_loLimit > limot->m_hiLimit)
785 {
786 lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
787 hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
788 }
789 else
790 {
791 lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
792 hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
793 }
794 mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
795 }
796 else
797 {
798 mot_fact = 0;
799 }
800 info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
801 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
802 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
803 info->cfm[srow] = limot->m_motorCFM;
804 srow += info->rowskip;
805 ++count;
806 }
807
808 if (limot->m_enableSpring)
809 {
810 btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
811 calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
812
813 //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
814 //if(cfm > 0.99999)
815 // cfm = 0.99999;
816 //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
817 //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
818 //info->m_lowerLimit[srow] = -SIMD_INFINITY;
819 //info->m_upperLimit[srow] = SIMD_INFINITY;
820
821 btScalar dt = BT_ONE / info->fps;
822 btScalar kd = limot->m_springDamping;
823 btScalar ks = limot->m_springStiffness;
824 btScalar vel;
825 if (rotational)
826 {
827 vel = angVelA.dot(ax1) - angVelB.dot(ax1);
828 }
829 else
830 {
831 btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
832 btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
833 vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
834 }
835 btScalar cfm = BT_ZERO;
836 btScalar mA = BT_ONE / m_rbA.getInvMass();
837 btScalar mB = BT_ONE / m_rbB.getInvMass();
838 if (rotational)
839 {
840 btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
841 btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
842 if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
843 if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
844 }
845 btScalar m;
846 if (m_rbA.getInvMass() == 0) m = mB; else
847 if (m_rbB.getInvMass() == 0) m = mA; else
848 m = mA*mB / (mA + mB);
849 btScalar angularfreq = btSqrt(ks / m);
850
851 //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
852 if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
853 {
854 ks = BT_ONE / dt / dt / btScalar(16.0) * m;
855 }
856 //avoid damping that would blow up the spring
857 if (limot->m_springDampingLimited && kd * dt > m)
858 {
859 kd = m / dt;
860 }
861 btScalar fs = ks * error * dt;
862 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
863 btScalar f = (fs + fd);
864
865 // after the spring force affecting the body(es) the new velocity will be
866 // vel + f / m * (rotational ? -1 : 1)
867 // so in theory this should be set here for m_constraintError
868 // (with m_constraintError we set a desired velocity for the affected body(es))
869 // however in practice any value is fine as long as it is greater than the "proper" velocity,
870 // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
871 // so it is much simpler (and more robust) just to simply use inf (with the proper sign)
872 // (Even with our best intent the "new" velocity is only an estimation. If we underestimate
873 // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
874 // matter, because the solver will limit it according the force limit)
875 // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
876 // will we not request a velocity with the wrong direction ?
877 // and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
878 // so the sign of the force that is really matters
879 if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
880 info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
881 else
882 info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
883
884 btScalar minf = f < fd ? f : fd;
885 btScalar maxf = f < fd ? fd : f;
886 if (!rotational)
887 {
888 info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
889 info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
890 }
891 else
892 {
893 info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
894 info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
895 }
896
897 info->cfm[srow] = cfm;
898 srow += info->rowskip;
899 ++count;
900 }
901
902 return count;
903}
904
905//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
906//If no axis is provided, it uses the default axis for this constraint.
907 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
908{
909 if ((axis >= 0) && (axis < 3))
910 {
911 switch (num)
912 {
913 case BT_CONSTRAINT_STOP_ERP:
914 m_linearLimits.m_stopERP[axis] = value;
915 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
916 break;
917 case BT_CONSTRAINT_STOP_CFM:
918 m_linearLimits.m_stopCFM[axis] = value;
919 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
920 break;
921 case BT_CONSTRAINT_ERP:
922 m_linearLimits.m_motorERP[axis] = value;
923 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
924 break;
925 case BT_CONSTRAINT_CFM:
926 m_linearLimits.m_motorCFM[axis] = value;
927 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
928 break;
929 default:
930 btAssertConstrParams(0);
931 }
932 }
933 else if ((axis >= 3) && (axis < 6))
934 {
935 switch (num)
936 {
937 case BT_CONSTRAINT_STOP_ERP:
938 m_angularLimits[axis - 3].m_stopERP = value;
939 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
940 break;
941 case BT_CONSTRAINT_STOP_CFM:
942 m_angularLimits[axis - 3].m_stopCFM = value;
943 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
944 break;
945 case BT_CONSTRAINT_ERP:
946 m_angularLimits[axis - 3].m_motorERP = value;
947 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
948 break;
949 case BT_CONSTRAINT_CFM:
950 m_angularLimits[axis - 3].m_motorCFM = value;
951 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
952 break;
953 default:
954 btAssertConstrParams(0);
955 }
956 }
957 else
958 {
959 btAssertConstrParams(0);
960 }
961}
962
963//return the local value of parameter
964 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
965{
966 btScalar retVal = 0;
967 if ((axis >= 0) && (axis < 3))
968 {
969 switch (num)
970 {
971 case BT_CONSTRAINT_STOP_ERP:
972 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
973 retVal = m_linearLimits.m_stopERP[axis];
974 break;
975 case BT_CONSTRAINT_STOP_CFM:
976 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
977 retVal = m_linearLimits.m_stopCFM[axis];
978 break;
979 case BT_CONSTRAINT_ERP:
980 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
981 retVal = m_linearLimits.m_motorERP[axis];
982 break;
983 case BT_CONSTRAINT_CFM:
984 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
985 retVal = m_linearLimits.m_motorCFM[axis];
986 break;
987 default:
988 btAssertConstrParams(0);
989 }
990 }
991 else if ((axis >= 3) && (axis < 6))
992 {
993 switch (num)
994 {
995 case BT_CONSTRAINT_STOP_ERP:
996 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
997 retVal = m_angularLimits[axis - 3].m_stopERP;
998 break;
999 case BT_CONSTRAINT_STOP_CFM:
1000 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1001 retVal = m_angularLimits[axis - 3].m_stopCFM;
1002 break;
1003 case BT_CONSTRAINT_ERP:
1004 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1005 retVal = m_angularLimits[axis - 3].m_motorERP;
1006 break;
1007 case BT_CONSTRAINT_CFM:
1008 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
1009 retVal = m_angularLimits[axis - 3].m_motorCFM;
1010 break;
1011 default:
1012 btAssertConstrParams(0);
1013 }
1014 }
1015 else
1016 {
1017 btAssertConstrParams(0);
1018 }
1019 return retVal;
1020}
1021
1022 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1, const btVector3& axis2)
1023{
1024 btVector3 zAxis = axis1.normalized();
1025 btVector3 yAxis = axis2.normalized();
1026 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1027
1028 btTransform frameInW;
1029 frameInW.setIdentity();
1030 frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1031 xAxis[1], yAxis[1], zAxis[1],
1032 xAxis[2], yAxis[2], zAxis[2]);
1033
1034 // now get constraint frame in local coordinate systems
1035 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1036 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1037
1038 calculateTransforms();
1039}
1040
1041 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
1042{
1043 btAssert((index >= 0) && (index < 6));
1044 if (index < 3)
1045 m_linearLimits.m_bounce[index] = bounce;
1046 else
1047 m_angularLimits[index - 3].m_bounce = bounce;
1048}
1049
1050 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
1051{
1052 btAssert((index >= 0) && (index < 6));
1053 if (index < 3)
1054 m_linearLimits.m_enableMotor[index] = onOff;
1055 else
1056 m_angularLimits[index - 3].m_enableMotor = onOff;
1057}
1058
1059 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
1060{
1061 btAssert((index >= 0) && (index < 6));
1062 if (index < 3)
1063 m_linearLimits.m_servoMotor[index] = onOff;
1064 else
1065 m_angularLimits[index - 3].m_servoMotor = onOff;
1066}
1067
1068 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
1069{
1070 btAssert((index >= 0) && (index < 6));
1071 if (index < 3)
1072 m_linearLimits.m_targetVelocity[index] = velocity;
1073 else
1074 m_angularLimits[index - 3].m_targetVelocity = velocity;
1075}
1076
1077 void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
1078{
1079 btAssert((index >= 0) && (index < 6));
1080 if (index < 3)
1081 {
1082 m_linearLimits.m_servoTarget[index] = targetOrg;
1083 }
1084 else
1085 {
1086 //wrap between -PI and PI, see also
1087 //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1088
1089 btScalar target = targetOrg + SIMD_PI;
1090 if (1)
1091 {
1092 btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
1093 // handle boundary cases resulted from floating-point cut off:
1094 {
1095 if (m >= SIMD_2_PI)
1096 {
1097 target = 0;
1098 }
1099 else
1100 {
1101 if (m < 0)
1102 {
1103 if (SIMD_2_PI + m == SIMD_2_PI)
1104 target = 0;
1105 else
1106 target = SIMD_2_PI + m;
1107 }
1108 else
1109 {
1110 target = m;
1111 }
1112 }
1113 }
1114 target -= SIMD_PI;
1115 }
1116
1117 m_angularLimits[index - 3].m_servoTarget = target;
1118 }
1119}
1120
1121 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
1122{
1123 btAssert((index >= 0) && (index < 6));
1124 if (index < 3)
1125 m_linearLimits.m_maxMotorForce[index] = force;
1126 else
1127 m_angularLimits[index - 3].m_maxMotorForce = force;
1128}
1129
1130 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
1131{
1132 btAssert((index >= 0) && (index < 6));
1133 if (index < 3)
1134 m_linearLimits.m_enableSpring[index] = onOff;
1135 else
1136 m_angularLimits[index - 3].m_enableSpring = onOff;
1137}
1138
1139 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1140{
1141 btAssert((index >= 0) && (index < 6));
1142 if (index < 3)
1143 {
1144 m_linearLimits.m_springStiffness[index] = stiffness;
1145 m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1146 }
1147 else
1148 {
1149 m_angularLimits[index - 3].m_springStiffness = stiffness;
1150 m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1151 }
1152}
1153
1154 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1155{
1156 btAssert((index >= 0) && (index < 6));
1157 if (index < 3)
1158 {
1159 m_linearLimits.m_springDamping[index] = damping;
1160 m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1161 }
1162 else
1163 {
1164 m_angularLimits[index - 3].m_springDamping = damping;
1165 m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1166 }
1167}
1168
1169 void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
1170{
1171 calculateTransforms();
1172 int i;
1173 for (i = 0; i < 3; i++)
1174 m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1175 for (i = 0; i < 3; i++)
1176 m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1177}
1178
1179 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
1180{
1181 btAssert((index >= 0) && (index < 6));
1182 calculateTransforms();
1183 if (index < 3)
1184 m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1185 else
1186 m_angularLimits[index - 3].m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1187}
1188
1189 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
1190{
1191 btAssert((index >= 0) && (index < 6));
1192 if (index < 3)
1193 m_linearLimits.m_equilibriumPoint[index] = val;
1194 else
1195 m_angularLimits[index - 3].m_equilibriumPoint = val;
1196}
1197
1199
1200 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
1201{
1202 //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
1203 if (m_loLimit > m_hiLimit)
1204 {
1205 m_currentLimit = 0;
1206 m_currentLimitError = btScalar(0.f);
1207 }
1208 else if (m_loLimit == m_hiLimit)
1209 {
1210 m_currentLimitError = test_value - m_loLimit;
1211 m_currentLimit = 3;
1212 }
1213 else
1214 {
1215 m_currentLimitError = test_value - m_loLimit;
1216 m_currentLimitErrorHi = test_value - m_hiLimit;
1217 m_currentLimit = 4;
1218 }
1219}
1220
1222
1223 void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
1224{
1225 btScalar loLimit = m_lowerLimit[limitIndex];
1226 btScalar hiLimit = m_upperLimit[limitIndex];
1227 if (loLimit > hiLimit)
1228 {
1229 m_currentLimitError[limitIndex] = 0;
1230 m_currentLimit[limitIndex] = 0;
1231 }
1232 else if (loLimit == hiLimit)
1233 {
1234 m_currentLimitError[limitIndex] = test_value - loLimit;
1235 m_currentLimit[limitIndex] = 3;
1236 }
1237 else
1238 {
1239 m_currentLimitError[limitIndex] = test_value - loLimit;
1240 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1241 m_currentLimit[limitIndex] = 4;
1242 }
1243}
D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
btGeneric6DofSpring2Constraint.h
BT_6DOF_FLAGS_AXIS_SHIFT2
#define BT_6DOF_FLAGS_AXIS_SHIFT2
Definition btGeneric6DofSpring2Constraint.h:270
RotateOrder
RotateOrder
Definition btGeneric6DofSpring2Constraint.h:58
RO_ZYX
@ RO_ZYX
Definition btGeneric6DofSpring2Constraint.h:64
RO_XZY
@ RO_XZY
Definition btGeneric6DofSpring2Constraint.h:60
RO_XYZ
@ RO_XYZ
Definition btGeneric6DofSpring2Constraint.h:59
RO_YZX
@ RO_YZX
Definition btGeneric6DofSpring2Constraint.h:62
RO_YXZ
@ RO_YXZ
Definition btGeneric6DofSpring2Constraint.h:61
RO_ZXY
@ RO_ZXY
Definition btGeneric6DofSpring2Constraint.h:63
BT_6DOF_FLAGS_ERP_MOTO2
@ BT_6DOF_FLAGS_ERP_MOTO2
Definition btGeneric6DofSpring2Constraint.h:267
BT_6DOF_FLAGS_CFM_MOTO2
@ BT_6DOF_FLAGS_CFM_MOTO2
Definition btGeneric6DofSpring2Constraint.h:266
BT_6DOF_FLAGS_ERP_STOP2
@ BT_6DOF_FLAGS_ERP_STOP2
Definition btGeneric6DofSpring2Constraint.h:265
BT_6DOF_FLAGS_CFM_STOP2
@ BT_6DOF_FLAGS_CFM_STOP2
Definition btGeneric6DofSpring2Constraint.h:264
BT_6DOF_FLAGS_USE_INFINITE_ERROR
@ BT_6DOF_FLAGS_USE_INFINITE_ERROR
Definition btGeneric6DofSpring2Constraint.h:268
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:888
btRigidBody.h
SIMD_PI
#define SIMD_PI
Definition btScalar.h:526
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
BT_ZERO
#define BT_ZERO
Definition btScalar.h:546
btSqrt
btScalar btSqrt(btScalar y)
Definition btScalar.h:466
BT_ONE
#define BT_ONE
Definition btScalar.h:545
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition btScalar.h:518
SIMD_INFINITY
#define SIMD_INFINITY
Definition btScalar.h:544
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
btAsin
btScalar btAsin(btScalar x)
Definition btScalar.h:509
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition btScalar.h:528
SIMD_2_PI
#define SIMD_2_PI
Definition btScalar.h:527
btAssert
#define btAssert(x)
Definition btScalar.h:153
btTransformUtil.h
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition btTypedConstraint.h:58
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition btTypedConstraint.h:331
BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_CFM
Definition btTypedConstraint.h:53
BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_ERP
Definition btTypedConstraint.h:51
BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_CFM
Definition btTypedConstraint.h:54
BT_CONSTRAINT_STOP_ERP
@ BT_CONSTRAINT_STOP_ERP
Definition btTypedConstraint.h:52
D6_SPRING_2_CONSTRAINT_TYPE
@ D6_SPRING_2_CONSTRAINT_TYPE
Definition btTypedConstraint.h:45
btGeneric6DofSpring2Constraint::m_calculatedAxis
btVector3 m_calculatedAxis[3]
Definition btGeneric6DofSpring2Constraint.h:291
btGeneric6DofSpring2Constraint::matrixToEulerYZX
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:170
btGeneric6DofSpring2Constraint::m_angularLimits
btRotationalLimitMotor2 m_angularLimits[3]
Definition btGeneric6DofSpring2Constraint.h:283
btGeneric6DofSpring2Constraint::m_factA
btScalar m_factA
Definition btGeneric6DofSpring2Constraint.h:293
btGeneric6DofSpring2Constraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition btGeneric6DofSpring2Constraint.cpp:446
btGeneric6DofSpring2Constraint::setStiffness
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
Definition btGeneric6DofSpring2Constraint.cpp:1139
btGeneric6DofSpring2Constraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition btGeneric6DofSpring2Constraint.cpp:964
btGeneric6DofSpring2Constraint::calculateLinearInfo
void calculateLinearInfo()
Definition btGeneric6DofSpring2Constraint.cpp:626
btGeneric6DofSpring2Constraint::matrixToEulerYXZ
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:137
btGeneric6DofSpring2Constraint::setAxis
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
Definition btGeneric6DofSpring2Constraint.cpp:1022
btGeneric6DofSpring2Constraint::m_frameInB
btTransform m_frameInB
Definition btGeneric6DofSpring2Constraint.h:277
btGeneric6DofSpring2Constraint::get_limit_motor_info2
int get_limit_motor_info2(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB, btConstraintInfo2 *info, int row, btVector3 &ax1, int rotational, int rotAllowed=false)
Definition btGeneric6DofSpring2Constraint.cpp:670
btGeneric6DofSpring2Constraint::setLinearLimits
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition btGeneric6DofSpring2Constraint.cpp:490
btGeneric6DofSpring2Constraint::m_calculatedAxisAngleDiff
btVector3 m_calculatedAxisAngleDiff
Definition btGeneric6DofSpring2Constraint.h:290
btGeneric6DofSpring2Constraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition btGeneric6DofSpring2Constraint.h:292
btGeneric6DofSpring2Constraint::setDamping
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
Definition btGeneric6DofSpring2Constraint.cpp:1154
btGeneric6DofSpring2Constraint::setBounce
void setBounce(int index, btScalar bounce)
Definition btGeneric6DofSpring2Constraint.cpp:1041
btGeneric6DofSpring2Constraint::matrixToEulerZXY
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:203
btGeneric6DofSpring2Constraint::setParam
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
Definition btGeneric6DofSpring2Constraint.cpp:907
btGeneric6DofSpring2Constraint::m_frameInA
btTransform m_frameInA
Definition btGeneric6DofSpring2Constraint.h:276
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:69
btGeneric6DofSpring2Constraint::m_calculatedTransformA
btTransform m_calculatedTransformA
Definition btGeneric6DofSpring2Constraint.h:288
btGeneric6DofSpring2Constraint::enableMotor
void enableMotor(int index, bool onOff)
Definition btGeneric6DofSpring2Constraint.cpp:1050
btGeneric6DofSpring2Constraint::m_linearLimits
btTranslationalLimitMotor2 m_linearLimits
Definition btGeneric6DofSpring2Constraint.h:282
btGeneric6DofSpring2Constraint::m_hasStaticBody
bool m_hasStaticBody
Definition btGeneric6DofSpring2Constraint.h:295
btGeneric6DofSpring2Constraint::m_calculatedTransformB
btTransform m_calculatedTransformB
Definition btGeneric6DofSpring2Constraint.h:289
btGeneric6DofSpring2Constraint::calculateJacobi
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
Definition btGeneric6DofSpring2Constraint.cpp:637
btGeneric6DofSpring2Constraint::calculateAngleInfo
void calculateAngleInfo()
Definition btGeneric6DofSpring2Constraint.cpp:269
btGeneric6DofSpring2Constraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
Definition btGeneric6DofSpring2Constraint.cpp:476
btGeneric6DofSpring2Constraint::enableSpring
void enableSpring(int index, bool onOff)
Definition btGeneric6DofSpring2Constraint.cpp:1130
btGeneric6DofSpring2Constraint::m_rotateOrder
RotateOrder m_rotateOrder
Definition btGeneric6DofSpring2Constraint.h:285
btGeneric6DofSpring2Constraint::setServoTarget
void setServoTarget(int index, btScalar target)
Definition btGeneric6DofSpring2Constraint.cpp:1077
btGeneric6DofSpring2Constraint::testAngularLimitMotor
void testAngularLimitMotor(int axis_index)
Definition btGeneric6DofSpring2Constraint.cpp:438
btGeneric6DofSpring2Constraint::btGetMatrixElem
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
Definition btGeneric6DofSpring2Constraint.cpp:60
btGeneric6DofSpring2Constraint::setMaxMotorForce
void setMaxMotorForce(int index, btScalar force)
Definition btGeneric6DofSpring2Constraint.cpp:1121
btGeneric6DofSpring2Constraint::m_flags
int m_flags
Definition btGeneric6DofSpring2Constraint.h:296
btGeneric6DofSpring2Constraint::matrixToEulerZYX
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:236
btGeneric6DofSpring2Constraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition btGeneric6DofSpring2Constraint.cpp:618
btGeneric6DofSpring2Constraint::getAxis
btVector3 getAxis(int axis_index) const
Definition btGeneric6DofSpring2Constraint.h:347
btGeneric6DofSpring2Constraint::matrixToEulerXZY
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition btGeneric6DofSpring2Constraint.cpp:104
btGeneric6DofSpring2Constraint::setTargetVelocity
void setTargetVelocity(int index, btScalar velocity)
Definition btGeneric6DofSpring2Constraint.cpp:1068
btGeneric6DofSpring2Constraint::setEquilibriumPoint
void setEquilibriumPoint()
Definition btGeneric6DofSpring2Constraint.cpp:1169
btGeneric6DofSpring2Constraint::setServo
void setServo(int index, bool onOff)
Definition btGeneric6DofSpring2Constraint.cpp:1059
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint
btGeneric6DofSpring2Constraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, RotateOrder rotOrder=RO_XYZ)
2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev Added support for generic constrain...
Definition btGeneric6DofSpring2Constraint.cpp:46
btGeneric6DofSpring2Constraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition btGeneric6DofSpring2Constraint.h:322
btGeneric6DofSpring2Constraint::setAngularLimits
int setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition btGeneric6DofSpring2Constraint.cpp:546
btGeneric6DofSpring2Constraint::m_factB
btScalar m_factB
Definition btGeneric6DofSpring2Constraint.h:294
btGeneric6DofSpring2Constraint::calculateTransforms
void calculateTransforms()
Definition btGeneric6DofSpring2Constraint.cpp:411
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3::setValue
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major).
Definition btMatrix3x3.h:204
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btRigidBody::getInvMass
btScalar getInvMass() const
Definition btRigidBody.h:263
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition btRigidBody.h:429
btRotationalLimitMotor2
Definition btGeneric6DofSpring2Constraint.h:68
btRotationalLimitMotor2::m_springDampingLimited
bool m_springDampingLimited
Definition btGeneric6DofSpring2Constraint.h:89
btRotationalLimitMotor2::m_targetVelocity
btScalar m_targetVelocity
Definition btGeneric6DofSpring2Constraint.h:81
btRotationalLimitMotor2::m_enableMotor
bool m_enableMotor
Definition btGeneric6DofSpring2Constraint.h:80
btRotationalLimitMotor2::testLimitValue
void testLimitValue(btScalar test_value)
Definition btGeneric6DofSpring2Constraint.cpp:1200
btRotationalLimitMotor2::m_currentLimitErrorHi
btScalar m_currentLimitErrorHi
Definition btGeneric6DofSpring2Constraint.h:93
btRotationalLimitMotor2::m_servoMotor
bool m_servoMotor
Definition btGeneric6DofSpring2Constraint.h:83
btRotationalLimitMotor2::m_currentLimit
int m_currentLimit
Definition btGeneric6DofSpring2Constraint.h:95
btRotationalLimitMotor2::m_currentPosition
btScalar m_currentPosition
Definition btGeneric6DofSpring2Constraint.h:94
btRotationalLimitMotor2::m_stopCFM
btScalar m_stopCFM
Definition btGeneric6DofSpring2Constraint.h:77
btRotationalLimitMotor2::m_hiLimit
btScalar m_hiLimit
Definition btGeneric6DofSpring2Constraint.h:74
btRotationalLimitMotor2::m_motorCFM
btScalar m_motorCFM
Definition btGeneric6DofSpring2Constraint.h:79
btRotationalLimitMotor2::m_maxMotorForce
btScalar m_maxMotorForce
Definition btGeneric6DofSpring2Constraint.h:82
btRotationalLimitMotor2::m_springStiffnessLimited
bool m_springStiffnessLimited
Definition btGeneric6DofSpring2Constraint.h:87
btRotationalLimitMotor2::m_servoTarget
btScalar m_servoTarget
Definition btGeneric6DofSpring2Constraint.h:84
btRotationalLimitMotor2::m_currentLimitError
btScalar m_currentLimitError
Definition btGeneric6DofSpring2Constraint.h:92
btRotationalLimitMotor2::m_springDamping
btScalar m_springDamping
Definition btGeneric6DofSpring2Constraint.h:88
btRotationalLimitMotor2::m_enableSpring
bool m_enableSpring
Definition btGeneric6DofSpring2Constraint.h:85
btRotationalLimitMotor2::m_bounce
btScalar m_bounce
Definition btGeneric6DofSpring2Constraint.h:75
btRotationalLimitMotor2::m_motorERP
btScalar m_motorERP
Definition btGeneric6DofSpring2Constraint.h:78
btRotationalLimitMotor2::m_springStiffness
btScalar m_springStiffness
Definition btGeneric6DofSpring2Constraint.h:86
btRotationalLimitMotor2::m_equilibriumPoint
btScalar m_equilibriumPoint
Definition btGeneric6DofSpring2Constraint.h:90
btRotationalLimitMotor2::m_loLimit
btScalar m_loLimit
Definition btGeneric6DofSpring2Constraint.h:73
btRotationalLimitMotor2::m_stopERP
btScalar m_stopERP
Definition btGeneric6DofSpring2Constraint.h:76
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition btTransform.h:109
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition btTransform.h:167
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition btTransform.h:114
btTranslationalLimitMotor2::m_lowerLimit
btVector3 m_lowerLimit
Definition btGeneric6DofSpring2Constraint.h:166
btTranslationalLimitMotor2::m_currentLimitError
btVector3 m_currentLimitError
Definition btGeneric6DofSpring2Constraint.h:185
btTranslationalLimitMotor2::testLimitValue
void testLimitValue(int limitIndex, btScalar test_value)
Definition btGeneric6DofSpring2Constraint.cpp:1223
btTranslationalLimitMotor2::m_upperLimit
btVector3 m_upperLimit
Definition btGeneric6DofSpring2Constraint.h:167
btTranslationalLimitMotor2::m_currentLimitErrorHi
btVector3 m_currentLimitErrorHi
Definition btGeneric6DofSpring2Constraint.h:186
btTranslationalLimitMotor2::m_currentLimit
int m_currentLimit[3]
Definition btGeneric6DofSpring2Constraint.h:188
btTypedConstraint::getMotorFactor
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
Definition btTypedConstraint.cpp:54
btTypedConstraint::getFixedBody
static btRigidBody & getFixedBody()
Definition btTypedConstraint.cpp:142
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition btTypedConstraint.h:214
btTypedConstraint::btTypedConstraint
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
Definition btTypedConstraint.cpp:22
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition btTypedConstraint.h:97
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition btTypedConstraint.h:218
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition btTypedConstraint.h:98
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition btVector3.h:949
btTypedConstraint::btConstraintInfo1
Definition btTypedConstraint.h:114
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition btTypedConstraint.h:115
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition btTypedConstraint.h:115
btTypedConstraint::btConstraintInfo2
Definition btTypedConstraint.h:121
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition btTypedConstraint.h:124
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition btTypedConstraint.h:138
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition btTypedConstraint.h:124
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition btTypedConstraint.h:138
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition btTypedConstraint.h:133