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

Bullet Collision Detection & Physics Library

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