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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
41
42 #include "btGeneric6DofSpring2Constraint.h"
43 #include "BulletDynamics/Dynamics/btRigidBody.h"
44 #include "LinearMath/btTransformUtil.h"
45 #include <new>
46
47
48
49 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
50 : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
51 , m_frameInA(frameInA)
52 , m_frameInB(frameInB)
53 , m_rotateOrder(rotOrder)
54 , m_flags(0)
55 {
56 calculateTransforms();
57 }
58
59
60 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
61 : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
62 , m_frameInB(frameInB)
63 , m_rotateOrder(rotOrder)
64 , m_flags(0)
65 {
67 m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
68 calculateTransforms();
69 }
70
71
72 btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
73 {
74 int i = index%3;
75 int j = index/3;
76 return mat[i][j];
77 }
78
79 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
80
81 bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
82 {
83 // rot = cy*cz -cy*sz sy
84 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
85 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
86
87 btScalar fi = btGetMatrixElem(mat,2);
88 if (fi < btScalar(1.0f))
89 {
90 if (fi > btScalar(-1.0f))
91 {
92 xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
93 xyz[1] = btAsin(btGetMatrixElem(mat,2));
94 xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
95 return true;
96 }
97 else
98 {
99 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
100 xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
101 xyz[1] = -SIMD_HALF_PI;
102 xyz[2] = btScalar(0.0);
103 return false;
104 }
105 }
106 else
107 {
108 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
109 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
110 xyz[1] = SIMD_HALF_PI;
111 xyz[2] = 0.0;
112 }
113 return false;
114 }
115
116 bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
117 {
118 // rot = cy*cz -sz sy*cz
119 // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
120 // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
121
122 btScalar fi = btGetMatrixElem(mat,1);
123 if (fi < btScalar(1.0f))
124 {
125 if (fi > btScalar(-1.0f))
126 {
127 xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
128 xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
129 xyz[2] = btAsin(-btGetMatrixElem(mat,1));
130 return true;
131 }
132 else
133 {
134 xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
135 xyz[1] = btScalar(0.0);
136 xyz[2] = SIMD_HALF_PI;
137 return false;
138 }
139 }
140 else
141 {
142 xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
143 xyz[1] = 0.0;
144 xyz[2] = -SIMD_HALF_PI;
145 }
146 return false;
147 }
148
149 bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
150 {
151 // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
152 // cx*sz cx*cz -sx
153 // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
154
155 btScalar fi = btGetMatrixElem(mat,5);
156 if (fi < btScalar(1.0f))
157 {
158 if (fi > btScalar(-1.0f))
159 {
160 xyz[0] = btAsin(-btGetMatrixElem(mat,5));
161 xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
162 xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
163 return true;
164 }
165 else
166 {
167 xyz[0] = SIMD_HALF_PI;
168 xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
169 xyz[2] = btScalar(0.0);
170 return false;
171 }
172 }
173 else
174 {
175 xyz[0] = -SIMD_HALF_PI;
176 xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
177 xyz[2] = 0.0;
178 }
179 return false;
180 }
181
182 bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
183 {
184 // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
185 // sz cz*cx -cz*sx
186 // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
187
188 btScalar fi = btGetMatrixElem(mat,3);
189 if (fi < btScalar(1.0f))
190 {
191 if (fi > btScalar(-1.0f))
192 {
193 xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
194 xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
195 xyz[2] = btAsin(btGetMatrixElem(mat,3));
196 return true;
197 }
198 else
199 {
200 xyz[0] = btScalar(0.0);
201 xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
202 xyz[2] = -SIMD_HALF_PI;
203 return false;
204 }
205 }
206 else
207 {
208 xyz[0] = btScalar(0.0);
209 xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
210 xyz[2] = SIMD_HALF_PI;
211 }
212 return false;
213 }
214
215 bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
216 {
217 // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
218 // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
219 // -cx*sy sx cx*cy
220
221 btScalar fi = btGetMatrixElem(mat,7);
222 if (fi < btScalar(1.0f))
223 {
224 if (fi > btScalar(-1.0f))
225 {
226 xyz[0] = btAsin(btGetMatrixElem(mat,7));
227 xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
228 xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
229 return true;
230 }
231 else
232 {
233 xyz[0] = -SIMD_HALF_PI;
234 xyz[1] = btScalar(0.0);
235 xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
236 return false;
237 }
238 }
239 else
240 {
241 xyz[0] = SIMD_HALF_PI;
242 xyz[1] = btScalar(0.0);
243 xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
244 }
245 return false;
246 }
247
248 bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
249 {
250 // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
251 // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
252 // -sy cy*sx cy*cx
253
254 btScalar fi = btGetMatrixElem(mat,6);
255 if (fi < btScalar(1.0f))
256 {
257 if (fi > btScalar(-1.0f))
258 {
259 xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
260 xyz[1] = btAsin(-btGetMatrixElem(mat,6));
261 xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
262 return true;
263 }
264 else
265 {
266 xyz[0] = btScalar(0.0);
267 xyz[1] = SIMD_HALF_PI;
268 xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
269 return false;
270 }
271 }
272 else
273 {
274 xyz[0] = btScalar(0.0);
275 xyz[1] = -SIMD_HALF_PI;
276 xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
277 }
278 return false;
279 }
280
281 void btGeneric6DofSpring2Constraint::calculateAngleInfo()
282 {
283 btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
284 switch (m_rotateOrder)
285 {
286 case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
287 case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
288 case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
289 case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
290 case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
291 case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
292 default : 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
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
439 void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
440 {
441 btScalar angle = m_calculatedAxisAngleDiff[axis_index];
442 angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
443 m_angularLimits[axis_index].m_currentPosition = angle;
444 m_angularLimits[axis_index].testLimitValue(angle);
445 }
446
447
448 void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
449 {
450 //prepare constraint
451 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
452 info->m_numConstraintRows = 0;
453 info->nub = 0;
454 int i;
455 //test linear limits
456 for(i = 0; i < 3; i++)
457 {
458 if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
459 else if (m_linearLimits.m_currentLimit[i]!=0) 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) info->m_numConstraintRows += 2;
468 else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
469 if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
470 if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
471 }
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
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 }
544 return row;
545 }
546
547
548
549 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)
550 {
551 int row = row_offset;
552
553 //order of rotational constraint rows
554 int cIdx[] = {0, 1, 2};
555 switch(m_rotateOrder)
556 {
557 case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
558 case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
559 case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
560 case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
561 case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
562 case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
563 default : btAssert(false);
564 }
565
566 for (int ii = 0; ii < 3 ; ii++ )
567 {
568 int i = cIdx[ii];
569 if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
570 {
571 btVector3 axis = getAxis(i);
572 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
573 if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
574 {
575 m_angularLimits[i].m_stopCFM = info->cfm[0];
576 }
577 if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
578 {
579 m_angularLimits[i].m_stopERP = info->erp;
580 }
581 if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
582 {
583 m_angularLimits[i].m_motorCFM = info->cfm[0];
584 }
585 if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
586 {
587 m_angularLimits[i].m_motorERP = info->erp;
588 }
589 row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
590 }
591 }
592
593 return row;
594 }
595
596
597 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
598 {
599 m_frameInA = frameA;
600 m_frameInB = frameB;
601 buildJacobian();
602 calculateTransforms();
603 }
604
605
606 void btGeneric6DofSpring2Constraint::calculateLinearInfo()
607 {
608 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
609 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
610 for(int i = 0; i < 3; i++)
611 {
612 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
613 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
614 }
615 }
616
617 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
618 {
619 btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
620 btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
621
622 J1[srow+0] = ax1[0];
623 J1[srow+1] = ax1[1];
624 J1[srow+2] = ax1[2];
625
626 J2[srow+0] = -ax1[0];
627 J2[srow+1] = -ax1[1];
628 J2[srow+2] = -ax1[2];
629
630 if(!rotational)
631 {
632 btVector3 tmpA, tmpB, relA, relB;
633 // get vector from bodyB to frameB in WCS
634 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
635 // same for bodyA
636 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
637 tmpA = relA.cross(ax1);
638 tmpB = relB.cross(ax1);
639 if(m_hasStaticBody && (!rotAllowed))
640 {
641 tmpA *= m_factA;
642 tmpB *= m_factB;
643 }
644 int i;
645 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
646 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
647 }
648 }
649
650
651 int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
652 btRotationalLimitMotor2 * limot,
653 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
654 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
655 {
656 int count = 0;
657 int srow = row * info->rowskip;
658
659 if (limot->m_currentLimit==4)
660 {
661 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
662
663 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
664 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
665 if (rotational) {
666 if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
667 btScalar bounceerror = -limot->m_bounce* vel;
668 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
669 }
670 } else {
671 if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
672 btScalar bounceerror = -limot->m_bounce* vel;
673 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
674 }
675 }
676 info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
677 info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
678 info->cfm[srow] = limot->m_stopCFM;
679 srow += info->rowskip;
680 ++count;
681
682 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
683 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
684 if (rotational) {
685 if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
686 btScalar bounceerror = -limot->m_bounce* vel;
687 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
688 }
689 } else {
690 if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
691 btScalar bounceerror = -limot->m_bounce* vel;
692 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
693 }
694 }
695 info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
696 info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
697 info->cfm[srow] = limot->m_stopCFM;
698 srow += info->rowskip;
699 ++count;
700 } else
701 if (limot->m_currentLimit==3)
702 {
703 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
704 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
705 info->m_lowerLimit[srow] = -SIMD_INFINITY;
706 info->m_upperLimit[srow] = SIMD_INFINITY;
707 info->cfm[srow] = limot->m_stopCFM;
708 srow += info->rowskip;
709 ++count;
710 }
711
712 if (limot->m_enableMotor && !limot->m_servoMotor)
713 {
714 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
715 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
716 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
717 limot->m_loLimit,
718 limot->m_hiLimit,
719 tag_vel,
720 info->fps * limot->m_motorERP);
721 info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
722 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
723 info->m_upperLimit[srow] = limot->m_maxMotorForce;
724 info->cfm[srow] = limot->m_motorCFM;
725 srow += info->rowskip;
726 ++count;
727 }
728
729 if (limot->m_enableMotor && limot->m_servoMotor)
730 {
731 btScalar error = limot->m_currentPosition - limot->m_servoTarget;
732 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
733 btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
734 btScalar tag_vel = -targetvelocity;
735 btScalar mot_fact;
736 if(error != 0)
737 {
738 btScalar lowLimit;
739 btScalar hiLimit;
740 if(limot->m_loLimit > limot->m_hiLimit)
741 {
742 lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
743 hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
744 }
745 else
746 {
747 lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
748 hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
749 }
750 mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
751 }
752 else
753 {
754 mot_fact = 0;
755 }
756 info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
757 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
758 info->m_upperLimit[srow] = limot->m_maxMotorForce;
759 info->cfm[srow] = limot->m_motorCFM;
760 srow += info->rowskip;
761 ++count;
762 }
763
764 if (limot->m_enableSpring)
765 {
766 btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
767 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
768
769 //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
770 //if(cfm > 0.99999)
771 // cfm = 0.99999;
772 //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
773 //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
774 //info->m_lowerLimit[srow] = -SIMD_INFINITY;
775 //info->m_upperLimit[srow] = SIMD_INFINITY;
776
777 btScalar dt = BT_ONE / info->fps;
778 btScalar kd = limot->m_springDamping;
779 btScalar ks = limot->m_springStiffness;
780 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
781 // btScalar erp = 0.1;
782 btScalar cfm = BT_ZERO;
783 btScalar mA = BT_ONE / m_rbA.getInvMass();
784 btScalar mB = BT_ONE / m_rbB.getInvMass();
785 btScalar m = mA > mB ? mB : mA;
786 btScalar angularfreq = sqrt(ks / m);
787
788
789 //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
790 if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
791 {
792 ks = BT_ONE / dt / dt / btScalar(16.0) * m;
793 }
794 //avoid damping that would blow up the spring
795 if(limot->m_springDampingLimited && kd * dt > m)
796 {
797 kd = m / dt;
798 }
799 btScalar fs = ks * error * dt;
800 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
801 btScalar f = (fs+fd);
802
803 info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
804
805 btScalar minf = f < fd ? f : fd;
806 btScalar maxf = f < fd ? fd : f;
807 if(!rotational)
808 {
809 info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
810 info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
811 }
812 else
813 {
814 info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
815 info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
816 }
817
818 info->cfm[srow] = cfm;
819 srow += info->rowskip;
820 ++count;
821 }
822
823 return count;
824 }
825
826
827 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
828 //If no axis is provided, it uses the default axis for this constraint.
829 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
830 {
831 if((axis >= 0) && (axis < 3))
832 {
833 switch(num)
834 {
835 case BT_CONSTRAINT_STOP_ERP :
836 m_linearLimits.m_stopERP[axis] = value;
837 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
838 break;
839 case BT_CONSTRAINT_STOP_CFM :
840 m_linearLimits.m_stopCFM[axis] = value;
841 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
842 break;
843 case BT_CONSTRAINT_ERP :
844 m_linearLimits.m_motorERP[axis] = value;
845 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
846 break;
847 case BT_CONSTRAINT_CFM :
848 m_linearLimits.m_motorCFM[axis] = value;
849 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
850 break;
851 default :
852 btAssertConstrParams(0);
853 }
854 }
855 else if((axis >=3) && (axis < 6))
856 {
857 switch(num)
858 {
859 case BT_CONSTRAINT_STOP_ERP :
860 m_angularLimits[axis - 3].m_stopERP = value;
861 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
862 break;
863 case BT_CONSTRAINT_STOP_CFM :
864 m_angularLimits[axis - 3].m_stopCFM = value;
865 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
866 break;
867 case BT_CONSTRAINT_ERP :
868 m_angularLimits[axis - 3].m_motorERP = value;
869 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
870 break;
871 case BT_CONSTRAINT_CFM :
872 m_angularLimits[axis - 3].m_motorCFM = value;
873 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
874 break;
875 default :
876 btAssertConstrParams(0);
877 }
878 }
879 else
880 {
881 btAssertConstrParams(0);
882 }
883 }
884
885 //return the local value of parameter
886 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
887 {
888 btScalar retVal = 0;
889 if((axis >= 0) && (axis < 3))
890 {
891 switch(num)
892 {
893 case BT_CONSTRAINT_STOP_ERP :
894 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
895 retVal = m_linearLimits.m_stopERP[axis];
896 break;
897 case BT_CONSTRAINT_STOP_CFM :
898 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
899 retVal = m_linearLimits.m_stopCFM[axis];
900 break;
901 case BT_CONSTRAINT_ERP :
902 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
903 retVal = m_linearLimits.m_motorERP[axis];
904 break;
905 case BT_CONSTRAINT_CFM :
906 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
907 retVal = m_linearLimits.m_motorCFM[axis];
908 break;
909 default :
910 btAssertConstrParams(0);
911 }
912 }
913 else if((axis >=3) && (axis < 6))
914 {
915 switch(num)
916 {
917 case BT_CONSTRAINT_STOP_ERP :
918 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
919 retVal = m_angularLimits[axis - 3].m_stopERP;
920 break;
921 case BT_CONSTRAINT_STOP_CFM :
922 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
923 retVal = m_angularLimits[axis - 3].m_stopCFM;
924 break;
925 case BT_CONSTRAINT_ERP :
926 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
927 retVal = m_angularLimits[axis - 3].m_motorERP;
928 break;
929 case BT_CONSTRAINT_CFM :
930 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
931 retVal = m_angularLimits[axis - 3].m_motorCFM;
932 break;
933 default :
934 btAssertConstrParams(0);
935 }
936 }
937 else
938 {
939 btAssertConstrParams(0);
940 }
941 return retVal;
942 }
943
944
945
946 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
947 {
948 btVector3 zAxis = axis1.normalized();
949 btVector3 yAxis = axis2.normalized();
950 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
951
952 btTransform frameInW;
953 frameInW.setIdentity();
954 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
955 xAxis[1], yAxis[1], zAxis[1],
956 xAxis[2], yAxis[2], zAxis[2]);
957
958 // now get constraint frame in local coordinate systems
959 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
960 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
961
962 calculateTransforms();
963 }
964
965 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
966 {
967 btAssert((index >= 0) && (index < 6));
968 if (index<3)
969 m_linearLimits.m_bounce[index] = bounce;
970 else
971 m_angularLimits[index - 3].m_bounce = bounce;
972 }
973
974 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
975 {
976 btAssert((index >= 0) && (index < 6));
977 if (index<3)
978 m_linearLimits.m_enableMotor[index] = onOff;
979 else
980 m_angularLimits[index - 3].m_enableMotor = onOff;
981 }
982
983 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
984 {
985 btAssert((index >= 0) && (index < 6));
986 if (index<3)
987 m_linearLimits.m_servoMotor[index] = onOff;
988 else
989 m_angularLimits[index - 3].m_servoMotor = onOff;
990 }
991
992 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
993 {
994 btAssert((index >= 0) && (index < 6));
995 if (index<3)
996 m_linearLimits.m_targetVelocity[index] = velocity;
997 else
998 m_angularLimits[index - 3].m_targetVelocity = velocity;
999 }
1000
1001 void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
1002 {
1003 btAssert((index >= 0) && (index < 6));
1004 if (index<3)
1005 m_linearLimits.m_servoTarget[index] = target;
1006 else
1007 m_angularLimits[index - 3].m_servoTarget = target;
1008 }
1009
1010 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
1011 {
1012 btAssert((index >= 0) && (index < 6));
1013 if (index<3)
1014 m_linearLimits.m_maxMotorForce[index] = force;
1015 else
1016 m_angularLimits[index - 3].m_maxMotorForce = force;
1017 }
1018
1019 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
1020 {
1021 btAssert((index >= 0) && (index < 6));
1022 if (index<3)
1023 m_linearLimits.m_enableSpring[index] = onOff;
1024 else
1025 m_angularLimits[index - 3] .m_enableSpring = onOff;
1026 }
1027
1028 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1029 {
1030 btAssert((index >= 0) && (index < 6));
1031 if (index<3) {
1032 m_linearLimits.m_springStiffness[index] = stiffness;
1033 m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1034 } else {
1035 m_angularLimits[index - 3].m_springStiffness = stiffness;
1036 m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1037 }
1038 }
1039
1040 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1041 {
1042 btAssert((index >= 0) && (index < 6));
1043 if (index<3) {
1044 m_linearLimits.m_springDamping[index] = damping;
1045 m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1046 } else {
1047 m_angularLimits[index - 3].m_springDamping = damping;
1048 m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1049 }
1050 }
1051
1052 void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
1053 {
1054 calculateTransforms();
1055 int i;
1056 for( i = 0; i < 3; i++)
1057 m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1058 for(i = 0; i < 3; i++)
1059 m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1060 }
1061
1062 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
1063 {
1064 btAssert((index >= 0) && (index < 6));
1065 calculateTransforms();
1066 if (index<3)
1067 m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1068 else
1069 m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1070 }
1071
1072 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
1073 {
1074 btAssert((index >= 0) && (index < 6));
1075 if (index<3)
1076 m_linearLimits.m_equilibriumPoint[index] = val;
1077 else
1078 m_angularLimits[index - 3] .m_equilibriumPoint = val;
1079 }
1080
1081
1083
1084 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
1085 {
1086 //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
1087 if(m_loLimit > m_hiLimit) {
1088 m_currentLimit = 0;
1089 m_currentLimitError = btScalar(0.f);
1090 }
1091 else if(m_loLimit == m_hiLimit) {
1092 m_currentLimitError = test_value - m_loLimit;
1093 m_currentLimit = 3;
1094 } else {
1095 m_currentLimitError = test_value - m_loLimit;
1096 m_currentLimitErrorHi = test_value - m_hiLimit;
1097 m_currentLimit = 4;
1098 }
1099 }
1100
1102
1103 void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
1104 {
1105 btScalar loLimit = m_lowerLimit[limitIndex];
1106 btScalar hiLimit = m_upperLimit[limitIndex];
1107 if(loLimit > hiLimit) {
1108 m_currentLimitError[limitIndex] = 0;
1109 m_currentLimit[limitIndex] = 0;
1110 }
1111 else if(loLimit == hiLimit) {
1112 m_currentLimitError[limitIndex] = test_value - loLimit;
1113 m_currentLimit[limitIndex] = 3;
1114 } else {
1115 m_currentLimitError[limitIndex] = test_value - loLimit;
1116 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1117 m_currentLimit[limitIndex] = 4;
1118 }
1119 }
1120
1121
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:143
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
RO_YXZ
Definition: btGeneric6DofSpring2Constraint.h:63
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btTransformUtil.h
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:81
btGeneric6DofSpring2Constraint::setTargetVelocity
void setTargetVelocity(int index, btScalar velocity)
Definition: btGeneric6DofSpring2Constraint.cpp:992
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:135
btGeneric6DofSpring2Constraint::matrixToEulerZXY
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:215
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:146
btTranslationalLimitMotor2::m_maxMotorForce
btVector3 m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:188
D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
btRotationalLimitMotor2::m_stopERP
btScalar m_stopERP
Definition: btGeneric6DofSpring2Constraint.h:78
btTranslationalLimitMotor2::m_springStiffness
btVector3 m_springStiffness
Definition: btGeneric6DofSpring2Constraint.h:182
btGeneric6DofSpring2Constraint::setMaxMotorForce
void setMaxMotorForce(int index, btScalar force)
Definition: btGeneric6DofSpring2Constraint.cpp:1010
btRotationalLimitMotor2::m_enableMotor
bool m_enableMotor
Definition: btGeneric6DofSpring2Constraint.h:82
btRotationalLimitMotor2::m_servoMotor
bool m_servoMotor
Definition: btGeneric6DofSpring2Constraint.h:85
BT_6DOF_FLAGS_AXIS_SHIFT2
#define BT_6DOF_FLAGS_AXIS_SHIFT2
Definition: btGeneric6DofSpring2Constraint.h:274
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:102
btTranslationalLimitMotor2::testLimitValue
void testLimitValue(int limitIndex, btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1103
btRotationalLimitMotor2::m_loLimit
btScalar m_loLimit
Definition: btGeneric6DofSpring2Constraint.h:75
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:126
btGeneric6DofSpring2Constraint::m_hasStaticBody
bool m_hasStaticBody
Definition: btGeneric6DofSpring2Constraint.h:301
btGeneric6DofSpring2Constraint::matrixToEulerYXZ
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:149
btTranslationalLimitMotor2::m_stopCFM
btVector3 m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:175
btRotationalLimitMotor2::m_motorERP
btScalar m_motorERP
Definition: btGeneric6DofSpring2Constraint.h:80
btTranslationalLimitMotor2::m_enableMotor
bool m_enableMotor[3]
Definition: btGeneric6DofSpring2Constraint.h:178
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:651
btGeneric6DofSpring2Constraint::setServo
void setServo(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:983
BT_ONE
#define BT_ONE
Definition: btScalar.h:496
btGeneric6DofSpring2Constraint::m_flags
int m_flags
Definition: btGeneric6DofSpring2Constraint.h:302
btGeneric6DofSpring2Constraint::m_factB
btScalar m_factB
Definition: btGeneric6DofSpring2Constraint.h:300
btTranslationalLimitMotor2::m_springStiffnessLimited
bool m_springStiffnessLimited[3]
Definition: btGeneric6DofSpring2Constraint.h:183
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btGeneric6DofSpring2Constraint::m_factA
btScalar m_factA
Definition: btGeneric6DofSpring2Constraint.h:299
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:121
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btRotationalLimitMotor2::m_springStiffnessLimited
bool m_springStiffnessLimited
Definition: btGeneric6DofSpring2Constraint.h:89
btRotationalLimitMotor2::m_maxMotorForce
btScalar m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:84
btTranslationalLimitMotor2::m_springDamping
btVector3 m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:184
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:143
btGeneric6DofSpring2Constraint::btGetMatrixElem
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
Definition: btGeneric6DofSpring2Constraint.cpp:72
btGeneric6DofSpring2Constraint::setAxis
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
Definition: btGeneric6DofSpring2Constraint.cpp:946
BT_6DOF_FLAGS_CFM_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:271
btGeneric6DofSpring2Constraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btGeneric6DofSpring2Constraint.cpp:886
btGeneric6DofSpring2Constraint::calculateLinearInfo
void calculateLinearInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:606
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::getAxis
btVector3 getAxis(int axis_index) const
Definition: btGeneric6DofSpring2Constraint.h:362
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:135
btTranslationalLimitMotor2::m_springDampingLimited
bool m_springDampingLimited[3]
Definition: btGeneric6DofSpring2Constraint.h:185
BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:56
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btRigidBody.h
btGeneric6DofSpring2Constraint::matrixToEulerZYX
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:248
btGeneric6DofSpring2Constraint::m_linearLimits
btTranslationalLimitMotor2 m_linearLimits
Definition: btGeneric6DofSpring2Constraint.h:287
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:478
btRotationalLimitMotor2
Definition: btGeneric6DofSpring2Constraint.h:69
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
BT_ZERO
#define BT_ZERO
Definition: btScalar.h:497
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btGeneric6DofSpring2Constraint::calculateTransforms
void calculateTransforms()
Definition: btGeneric6DofSpring2Constraint.cpp:411
RO_YZX
Definition: btGeneric6DofSpring2Constraint.h:64
btGeneric6DofSpring2Constraint::m_frameInA
btTransform m_frameInA
Definition: btGeneric6DofSpring2Constraint.h:281
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:55
btTranslationalLimitMotor2::m_currentLinearDiff
btVector3 m_currentLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:192
btGeneric6DofSpring2Constraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:298
D6_SPRING_2_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:47
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:495
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:57
btRotationalLimitMotor2::m_currentLimitError
btScalar m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:94
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:135
btRotationalLimitMotor2::m_springDampingLimited
bool m_springDampingLimited
Definition: btGeneric6DofSpring2Constraint.h:91
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btTranslationalLimitMotor2::m_bounce
btVector3 m_bounce
Definition: btGeneric6DofSpring2Constraint.h:173
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:129
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btTranslationalLimitMotor2::m_lowerLimit
btVector3 m_lowerLimit
Definition: btGeneric6DofSpring2Constraint.h:171
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:362
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:146
btRotationalLimitMotor2::m_servoTarget
btScalar m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:86
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btRotationalLimitMotor2::m_hiLimit
btScalar m_hiLimit
Definition: btGeneric6DofSpring2Constraint.h:76
btTranslationalLimitMotor2::m_currentLimitErrorHi
btVector3 m_currentLimitErrorHi
Definition: btGeneric6DofSpring2Constraint.h:191
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:198
btGeneric6DofSpring2Constraint::m_calculatedTransformB
btTransform m_calculatedTransformB
Definition: btGeneric6DofSpring2Constraint.h:295
btRotationalLimitMotor2::m_currentLimitErrorHi
btScalar m_currentLimitErrorHi
Definition: btGeneric6DofSpring2Constraint.h:95
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btGeneric6DofSpring2Constraint::m_calculatedTransformA
btTransform m_calculatedTransformA
Definition: btGeneric6DofSpring2Constraint.h:294
btGeneric6DofSpring2Constraint::setEquilibriumPoint
void setEquilibriumPoint()
Definition: btGeneric6DofSpring2Constraint.cpp:1052
BT_6DOF_FLAGS_ERP_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:272
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btTranslationalLimitMotor2::m_servoMotor
bool m_servoMotor[3]
Definition: btGeneric6DofSpring2Constraint.h:179
btTranslationalLimitMotor2::m_equilibriumPoint
btVector3 m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:186
btRotationalLimitMotor2::m_targetVelocity
btScalar m_targetVelocity
Definition: btGeneric6DofSpring2Constraint.h:83
RO_XZY
Definition: btGeneric6DofSpring2Constraint.h:62
btRotationalLimitMotor2::m_bounce
btScalar m_bounce
Definition: btGeneric6DofSpring2Constraint.h:77
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:49
btRotationalLimitMotor2::m_motorCFM
btScalar m_motorCFM
Definition: btGeneric6DofSpring2Constraint.h:81
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:829
btRotationalLimitMotor2::m_currentLimit
int m_currentLimit
Definition: btGeneric6DofSpring2Constraint.h:97
btGeneric6DofSpring2Constraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don&#39;t use them directly
Definition: btGeneric6DofSpring2Constraint.h:337
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:346
btTranslationalLimitMotor2::m_currentLimitError
btVector3 m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:190
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btGeneric6DofSpring2Constraint::matrixToEulerXZY
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:116
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:138
btTypedConstraint::getFixedBody
static btRigidBody & getFixedBody()
Definition: btTypedConstraint.cpp:148
btGeneric6DofSpring2Constraint::m_rotateOrder
RotateOrder m_rotateOrder
Definition: btGeneric6DofSpring2Constraint.h:290
RO_ZYX
Definition: btGeneric6DofSpring2Constraint.h:66
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
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:617
btTranslationalLimitMotor2::m_servoTarget
btVector3 m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:181
btGeneric6DofSpring2Constraint::m_calculatedAxis
btVector3 m_calculatedAxis[3]
Definition: btGeneric6DofSpring2Constraint.h:297
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:103
btTranslationalLimitMotor2::m_stopERP
btVector3 m_stopERP
Definition: btGeneric6DofSpring2Constraint.h:174
btTranslationalLimitMotor2::m_motorCFM
btVector3 m_motorCFM
Definition: btGeneric6DofSpring2Constraint.h:177
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:135
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:78
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:60
btGeneric6DofSpring2Constraint.h
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:227
btGeneric6DofSpring2Constraint::setStiffness
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
Definition: btGeneric6DofSpring2Constraint.cpp:1028
btTranslationalLimitMotor2::m_targetVelocity
btVector3 m_targetVelocity
Definition: btGeneric6DofSpring2Constraint.h:187
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:121
btTranslationalLimitMotor2::m_enableSpring
bool m_enableSpring[3]
Definition: btGeneric6DofSpring2Constraint.h:180
btTranslationalLimitMotor2::m_currentLimit
int m_currentLimit[3]
Definition: btGeneric6DofSpring2Constraint.h:193
btRotationalLimitMotor2::m_equilibriumPoint
btScalar m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:92
btRotationalLimitMotor2::m_stopCFM
btScalar m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:79
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:129
btGeneric6DofSpring2Constraint::m_angularLimits
btRotationalLimitMotor2 m_angularLimits[3]
Definition: btGeneric6DofSpring2Constraint.h:288
RotateOrder
RotateOrder
Definition: btGeneric6DofSpring2Constraint.h:59
BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:54
btGeneric6DofSpring2Constraint::matrixToEulerYZX
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:182
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
BT_6DOF_FLAGS_ERP_STOP2
Definition: btGeneric6DofSpring2Constraint.h:270
RO_ZXY
Definition: btGeneric6DofSpring2Constraint.h:65
btRotationalLimitMotor2::testLimitValue
void testLimitValue(btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1084
btRotationalLimitMotor2::m_currentPosition
btScalar m_currentPosition
Definition: btGeneric6DofSpring2Constraint.h:96
btGeneric6DofSpring2Constraint::enableSpring
void enableSpring(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1019
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:359
btAsin
btScalar btAsin(btScalar x)
Definition: btScalar.h:460
btTranslationalLimitMotor2::m_motorERP
btVector3 m_motorERP
Definition: btGeneric6DofSpring2Constraint.h:176
btGeneric6DofSpring2Constraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btGeneric6DofSpring2Constraint.cpp:597
btRotationalLimitMotor2::m_enableSpring
bool m_enableSpring
Definition: btGeneric6DofSpring2Constraint.h:87
btGeneric6DofSpring2Constraint::m_frameInB
btTransform m_frameInB
Definition: btGeneric6DofSpring2Constraint.h:282
btGeneric6DofSpring2Constraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btGeneric6DofSpring2Constraint.cpp:448
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:61
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:120
btRotationalLimitMotor2::m_springStiffness
btScalar m_springStiffness
Definition: btGeneric6DofSpring2Constraint.h:88
btGeneric6DofSpring2Constraint::setDamping
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
Definition: btGeneric6DofSpring2Constraint.cpp:1040
btGeneric6DofSpring2Constraint::calculateAngleInfo
void calculateAngleInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:281
btGeneric6DofSpring2Constraint::m_calculatedAxisAngleDiff
btVector3 m_calculatedAxisAngleDiff
Definition: btGeneric6DofSpring2Constraint.h:296
btGeneric6DofSpring2Constraint::testAngularLimitMotor
void testAngularLimitMotor(int axis_index)
Definition: btGeneric6DofSpring2Constraint.cpp:439
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:231
RO_XYZ
Definition: btGeneric6DofSpring2Constraint.h:61
btGeneric6DofSpring2Constraint::setServoTarget
void setServoTarget(int index, btScalar target)
Definition: btGeneric6DofSpring2Constraint.cpp:1001
btRotationalLimitMotor2::m_springDamping
btScalar m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:90
btTranslationalLimitMotor2::m_upperLimit
btVector3 m_upperLimit
Definition: btGeneric6DofSpring2Constraint.h:172
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:278
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:549
btGeneric6DofSpring2Constraint::enableMotor
void enableMotor(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:974
BT_6DOF_FLAGS_CFM_STOP2
Definition: btGeneric6DofSpring2Constraint.h:269
btGeneric6DofSpring2Constraint::setBounce
void setBounce(int index, btScalar bounce)
Definition: btGeneric6DofSpring2Constraint.cpp:965