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_springDamping = m_linearLimits.m_springDamping[i];
509 limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
510 limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
511 limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
512 limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
513 limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
514 btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
515 int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
516 limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
517 limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
518 limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
519 limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
520
521 //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.
522 int indx1 = (i + 1) % 3;
523 int indx2 = (i + 2) % 3;
524 int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
525 #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
526 bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
527 m_angularLimits[indx1].m_currentLimit == 2 ||
528 ( 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 ) ) ||
529 ( 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 ) );
530 bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
531 m_angularLimits[indx2].m_currentLimit == 2 ||
532 ( 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 ) ) ||
533 ( 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 ) );
534 if( indx1Violated && indx2Violated )
535 {
536 rotAllowed = 0;
537 }
538 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
539
540 }
541 }
542 return row;
543 }
544
545
546
547 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)
548 {
549 int row = row_offset;
550
551 //order of rotational constraint rows
552 int cIdx[] = {0, 1, 2};
553 switch(m_rotateOrder)
554 {
555 case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
556 case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
557 case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
558 case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
559 case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
560 case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
561 default : btAssert(false);
562 }
563
564 for (int ii = 0; ii < 3 ; ii++ )
565 {
566 int i = cIdx[ii];
567 if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
568 {
569 btVector3 axis = getAxis(i);
570 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
571 if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
572 {
573 m_angularLimits[i].m_stopCFM = info->cfm[0];
574 }
575 if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
576 {
577 m_angularLimits[i].m_stopERP = info->erp;
578 }
579 if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
580 {
581 m_angularLimits[i].m_motorCFM = info->cfm[0];
582 }
583 if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
584 {
585 m_angularLimits[i].m_motorERP = info->erp;
586 }
587 row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
588 }
589 }
590
591 return row;
592 }
593
594
595 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
596 {
597 m_frameInA = frameA;
598 m_frameInB = frameB;
599 buildJacobian();
600 calculateTransforms();
601 }
602
603
604 void btGeneric6DofSpring2Constraint::calculateLinearInfo()
605 {
606 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
607 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
608 for(int i = 0; i < 3; i++)
609 {
610 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
611 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
612 }
613 }
614
615 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
616 {
617 btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
618 btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
619
620 J1[srow+0] = ax1[0];
621 J1[srow+1] = ax1[1];
622 J1[srow+2] = ax1[2];
623
624 J2[srow+0] = -ax1[0];
625 J2[srow+1] = -ax1[1];
626 J2[srow+2] = -ax1[2];
627
628 if(!rotational)
629 {
630 btVector3 tmpA, tmpB, relA, relB;
631 // get vector from bodyB to frameB in WCS
632 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
633 // same for bodyA
634 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
635 tmpA = relA.cross(ax1);
636 tmpB = relB.cross(ax1);
637 if(m_hasStaticBody && (!rotAllowed))
638 {
639 tmpA *= m_factA;
640 tmpB *= m_factB;
641 }
642 int i;
643 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
644 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
645 }
646 }
647
648
649 int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
650 btRotationalLimitMotor2 * limot,
651 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
652 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
653 {
654 int count = 0;
655 int srow = row * info->rowskip;
656
657 if (limot->m_currentLimit==4)
658 {
659 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
660
661 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
662 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
663 if (rotational) {
664 if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
665 btScalar bounceerror = -limot->m_bounce* vel;
666 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
667 }
668 } else {
669 if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
670 btScalar bounceerror = -limot->m_bounce* vel;
671 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
672 }
673 }
674 info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
675 info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
676 info->cfm[srow] = limot->m_stopCFM;
677 srow += info->rowskip;
678 ++count;
679
680 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
681 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
682 if (rotational) {
683 if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
684 btScalar bounceerror = -limot->m_bounce* vel;
685 if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
686 }
687 } else {
688 if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
689 btScalar bounceerror = -limot->m_bounce* vel;
690 if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
691 }
692 }
693 info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
694 info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
695 info->cfm[srow] = limot->m_stopCFM;
696 srow += info->rowskip;
697 ++count;
698 } else
699 if (limot->m_currentLimit==3)
700 {
701 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
702 info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
703 info->m_lowerLimit[srow] = -SIMD_INFINITY;
704 info->m_upperLimit[srow] = SIMD_INFINITY;
705 info->cfm[srow] = limot->m_stopCFM;
706 srow += info->rowskip;
707 ++count;
708 }
709
710 if (limot->m_enableMotor && !limot->m_servoMotor)
711 {
712 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
713 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
714 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
715 limot->m_loLimit,
716 limot->m_hiLimit,
717 tag_vel,
718 info->fps * limot->m_motorERP);
719 info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
720 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
721 info->m_upperLimit[srow] = limot->m_maxMotorForce;
722 info->cfm[srow] = limot->m_motorCFM;
723 srow += info->rowskip;
724 ++count;
725 }
726
727 if (limot->m_enableMotor && limot->m_servoMotor)
728 {
729 btScalar error = limot->m_currentPosition - limot->m_servoTarget;
730 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
731 btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
732 btScalar tag_vel = -targetvelocity;
733 btScalar mot_fact;
734 if(error != 0)
735 {
736 btScalar lowLimit;
737 btScalar hiLimit;
738 if(limot->m_loLimit > limot->m_hiLimit)
739 {
740 lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
741 hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
742 }
743 else
744 {
745 lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
746 hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
747 }
748 mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
749 }
750 else
751 {
752 mot_fact = 0;
753 }
754 info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
755 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
756 info->m_upperLimit[srow] = limot->m_maxMotorForce;
757 info->cfm[srow] = limot->m_motorCFM;
758 srow += info->rowskip;
759 ++count;
760 }
761
762 if (limot->m_enableSpring)
763 {
764 btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
765 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
766
767 //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
768 //if(cfm > 0.99999)
769 // cfm = 0.99999;
770 //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
771 //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
772 //info->m_lowerLimit[srow] = -SIMD_INFINITY;
773 //info->m_upperLimit[srow] = SIMD_INFINITY;
774
775 btScalar dt = BT_ONE / info->fps;
776 btScalar kd = limot->m_springDamping;
777 btScalar ks = limot->m_springStiffness;
778 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
779 // btScalar erp = 0.1;
780 btScalar cfm = BT_ZERO;
781 btScalar mA = BT_ONE / m_rbA.getInvMass();
782 btScalar mB = BT_ONE / m_rbB.getInvMass();
783 btScalar m = mA > mB ? mB : mA;
784 btScalar angularfreq = sqrt(ks / m);
785
786
787 //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
788 if( 0.25 < angularfreq * dt)
789 {
790 ks = BT_ONE / dt / dt / btScalar(16.0) / m;
791 }
792 //avoid overdamping
793 if(kd * dt > m)
794 {
795 kd = m / dt;
796 }
797 btScalar fs = ks * error * dt;
798 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
799 btScalar f = (fs+fd);
800
801 info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
802
803 btScalar minf = f < fd ? f : fd;
804 btScalar maxf = f < fd ? fd : f;
805 if(!rotational)
806 {
807 info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
808 info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
809 }
810 else
811 {
812 info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
813 info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
814 }
815
816 info->cfm[srow] = cfm;
817 srow += info->rowskip;
818 ++count;
819 }
820
821 return count;
822 }
823
824
825 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
826 //If no axis is provided, it uses the default axis for this constraint.
827 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
828 {
829 if((axis >= 0) && (axis < 3))
830 {
831 switch(num)
832 {
833 case BT_CONSTRAINT_STOP_ERP :
834 m_linearLimits.m_stopERP[axis] = value;
835 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
836 break;
837 case BT_CONSTRAINT_STOP_CFM :
838 m_linearLimits.m_stopCFM[axis] = value;
839 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
840 break;
841 case BT_CONSTRAINT_ERP :
842 m_linearLimits.m_motorERP[axis] = value;
843 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
844 break;
845 case BT_CONSTRAINT_CFM :
846 m_linearLimits.m_motorCFM[axis] = value;
847 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
848 break;
849 default :
850 btAssertConstrParams(0);
851 }
852 }
853 else if((axis >=3) && (axis < 6))
854 {
855 switch(num)
856 {
857 case BT_CONSTRAINT_STOP_ERP :
858 m_angularLimits[axis - 3].m_stopERP = value;
859 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
860 break;
861 case BT_CONSTRAINT_STOP_CFM :
862 m_angularLimits[axis - 3].m_stopCFM = value;
863 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
864 break;
865 case BT_CONSTRAINT_ERP :
866 m_angularLimits[axis - 3].m_motorERP = value;
867 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
868 break;
869 case BT_CONSTRAINT_CFM :
870 m_angularLimits[axis - 3].m_motorCFM = value;
871 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
872 break;
873 default :
874 btAssertConstrParams(0);
875 }
876 }
877 else
878 {
879 btAssertConstrParams(0);
880 }
881 }
882
883 //return the local value of parameter
884 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
885 {
886 btScalar retVal = 0;
887 if((axis >= 0) && (axis < 3))
888 {
889 switch(num)
890 {
891 case BT_CONSTRAINT_STOP_ERP :
892 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
893 retVal = m_linearLimits.m_stopERP[axis];
894 break;
895 case BT_CONSTRAINT_STOP_CFM :
896 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
897 retVal = m_linearLimits.m_stopCFM[axis];
898 break;
899 case BT_CONSTRAINT_ERP :
900 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
901 retVal = m_linearLimits.m_motorERP[axis];
902 break;
903 case BT_CONSTRAINT_CFM :
904 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
905 retVal = m_linearLimits.m_motorCFM[axis];
906 break;
907 default :
908 btAssertConstrParams(0);
909 }
910 }
911 else if((axis >=3) && (axis < 6))
912 {
913 switch(num)
914 {
915 case BT_CONSTRAINT_STOP_ERP :
916 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
917 retVal = m_angularLimits[axis - 3].m_stopERP;
918 break;
919 case BT_CONSTRAINT_STOP_CFM :
920 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
921 retVal = m_angularLimits[axis - 3].m_stopCFM;
922 break;
923 case BT_CONSTRAINT_ERP :
924 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
925 retVal = m_angularLimits[axis - 3].m_motorERP;
926 break;
927 case BT_CONSTRAINT_CFM :
928 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
929 retVal = m_angularLimits[axis - 3].m_motorCFM;
930 break;
931 default :
932 btAssertConstrParams(0);
933 }
934 }
935 else
936 {
937 btAssertConstrParams(0);
938 }
939 return retVal;
940 }
941
942
943
944 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
945 {
946 btVector3 zAxis = axis1.normalized();
947 btVector3 yAxis = axis2.normalized();
948 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
949
950 btTransform frameInW;
951 frameInW.setIdentity();
952 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
953 xAxis[1], yAxis[1], zAxis[1],
954 xAxis[2], yAxis[2], zAxis[2]);
955
956 // now get constraint frame in local coordinate systems
957 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
958 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
959
960 calculateTransforms();
961 }
962
963 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
964 {
965 btAssert((index >= 0) && (index < 6));
966 if (index<3)
967 m_linearLimits.m_bounce[index] = bounce;
968 else
969 m_angularLimits[index - 3].m_bounce = bounce;
970 }
971
972 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
973 {
974 btAssert((index >= 0) && (index < 6));
975 if (index<3)
976 m_linearLimits.m_enableMotor[index] = onOff;
977 else
978 m_angularLimits[index - 3].m_enableMotor = onOff;
979 }
980
981 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
982 {
983 btAssert((index >= 0) && (index < 6));
984 if (index<3)
985 m_linearLimits.m_servoMotor[index] = onOff;
986 else
987 m_angularLimits[index - 3].m_servoMotor = onOff;
988 }
989
990 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
991 {
992 btAssert((index >= 0) && (index < 6));
993 if (index<3)
994 m_linearLimits.m_targetVelocity[index] = velocity;
995 else
996 m_angularLimits[index - 3].m_targetVelocity = velocity;
997 }
998
999 void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
1000 {
1001 btAssert((index >= 0) && (index < 6));
1002 if (index<3)
1003 m_linearLimits.m_servoTarget[index] = target;
1004 else
1005 m_angularLimits[index - 3].m_servoTarget = target;
1006 }
1007
1008 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
1009 {
1010 btAssert((index >= 0) && (index < 6));
1011 if (index<3)
1012 m_linearLimits.m_maxMotorForce[index] = force;
1013 else
1014 m_angularLimits[index - 3].m_maxMotorForce = force;
1015 }
1016
1017 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
1018 {
1019 btAssert((index >= 0) && (index < 6));
1020 if (index<3)
1021 m_linearLimits.m_enableSpring[index] = onOff;
1022 else
1023 m_angularLimits[index - 3] .m_enableSpring = onOff;
1024 }
1025
1026 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness)
1027 {
1028 btAssert((index >= 0) && (index < 6));
1029 if (index<3)
1030 m_linearLimits.m_springStiffness[index] = stiffness;
1031 else
1032 m_angularLimits[index - 3] .m_springStiffness = stiffness;
1033 }
1034
1035 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping)
1036 {
1037 btAssert((index >= 0) && (index < 6));
1038 if (index<3)
1039 m_linearLimits.m_springDamping[index] = damping;
1040 else
1041 m_angularLimits[index - 3] .m_springDamping = damping;
1042 }
1043
1044 void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
1045 {
1046 calculateTransforms();
1047 int i;
1048 for( i = 0; i < 3; i++)
1049 m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1050 for(i = 0; i < 3; i++)
1051 m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1052 }
1053
1054 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
1055 {
1056 btAssert((index >= 0) && (index < 6));
1057 calculateTransforms();
1058 if (index<3)
1059 m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1060 else
1061 m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1062 }
1063
1064 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
1065 {
1066 btAssert((index >= 0) && (index < 6));
1067 if (index<3)
1068 m_linearLimits.m_equilibriumPoint[index] = val;
1069 else
1070 m_angularLimits[index - 3] .m_equilibriumPoint = val;
1071 }
1072
1073
1075
1076 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
1077 {
1078 //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
1079 if(m_loLimit > m_hiLimit) {
1080 m_currentLimit = 0;
1081 m_currentLimitError = btScalar(0.f);
1082 }
1083 else if(m_loLimit == m_hiLimit) {
1084 m_currentLimitError = test_value - m_loLimit;
1085 m_currentLimit = 3;
1086 } else {
1087 m_currentLimitError = test_value - m_loLimit;
1088 m_currentLimitErrorHi = test_value - m_hiLimit;
1089 m_currentLimit = 4;
1090 }
1091 }
1092
1094
1095 void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
1096 {
1097 btScalar loLimit = m_lowerLimit[limitIndex];
1098 btScalar hiLimit = m_upperLimit[limitIndex];
1099 if(loLimit > hiLimit) {
1100 m_currentLimitError[limitIndex] = 0;
1101 m_currentLimit[limitIndex] = 0;
1102 }
1103 else if(loLimit == hiLimit) {
1104 m_currentLimitError[limitIndex] = test_value - loLimit;
1105 m_currentLimit[limitIndex] = 3;
1106 } else {
1107 m_currentLimitError[limitIndex] = test_value - loLimit;
1108 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1109 m_currentLimit[limitIndex] = 4;
1110 }
1111 }
1112
1113
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:142
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:990
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:134
btGeneric6DofSpring2Constraint::matrixToEulerZXY
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:215
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:145
btTranslationalLimitMotor2::m_maxMotorForce
btVector3 m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:180
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:176
btGeneric6DofSpring2Constraint::setMaxMotorForce
void setMaxMotorForce(int index, btScalar force)
Definition: btGeneric6DofSpring2Constraint.cpp:1008
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:262
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:101
btTranslationalLimitMotor2::testLimitValue
void testLimitValue(int limitIndex, btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1095
btRotationalLimitMotor2::m_loLimit
btScalar m_loLimit
Definition: btGeneric6DofSpring2Constraint.h:75
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:125
btGeneric6DofSpring2Constraint::m_hasStaticBody
bool m_hasStaticBody
Definition: btGeneric6DofSpring2Constraint.h:289
btGeneric6DofSpring2Constraint::matrixToEulerYXZ
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:149
btTranslationalLimitMotor2::m_stopCFM
btVector3 m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:169
btRotationalLimitMotor2::m_motorERP
btScalar m_motorERP
Definition: btGeneric6DofSpring2Constraint.h:80
btTranslationalLimitMotor2::m_enableMotor
bool m_enableMotor[3]
Definition: btGeneric6DofSpring2Constraint.h:172
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:649
btGeneric6DofSpring2Constraint::setDamping
void setDamping(int index, btScalar damping)
Definition: btGeneric6DofSpring2Constraint.cpp:1035
btGeneric6DofSpring2Constraint::setServo
void setServo(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:981
BT_ONE
#define BT_ONE
Definition: btScalar.h:496
btGeneric6DofSpring2Constraint::m_flags
int m_flags
Definition: btGeneric6DofSpring2Constraint.h:290
btGeneric6DofSpring2Constraint::m_factB
btScalar m_factB
Definition: btGeneric6DofSpring2Constraint.h:288
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btGeneric6DofSpring2Constraint::m_factA
btScalar m_factA
Definition: btGeneric6DofSpring2Constraint.h:287
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:120
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btRotationalLimitMotor2::m_maxMotorForce
btScalar m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:84
btTranslationalLimitMotor2::m_springDamping
btVector3 m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:177
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:142
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:944
BT_6DOF_FLAGS_CFM_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:259
btGeneric6DofSpring2Constraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btGeneric6DofSpring2Constraint.cpp:884
btGeneric6DofSpring2Constraint::calculateLinearInfo
void calculateLinearInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:604
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:350
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:134
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:275
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:269
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:55
btTranslationalLimitMotor2::m_currentLinearDiff
btVector3 m_currentLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:184
btGeneric6DofSpring2Constraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:286
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:92
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:134
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btTranslationalLimitMotor2::m_bounce
btVector3 m_bounce
Definition: btGeneric6DofSpring2Constraint.h:167
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:128
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btTranslationalLimitMotor2::m_lowerLimit
btVector3 m_lowerLimit
Definition: btGeneric6DofSpring2Constraint.h:165
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:145
btRotationalLimitMotor2::m_servoTarget
btScalar m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:86
btGeneric6DofSpring2Constraint::setStiffness
void setStiffness(int index, btScalar stiffness)
Definition: btGeneric6DofSpring2Constraint.cpp:1026
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:183
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:283
btRotationalLimitMotor2::m_currentLimitErrorHi
btScalar m_currentLimitErrorHi
Definition: btGeneric6DofSpring2Constraint.h:93
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btGeneric6DofSpring2Constraint::m_calculatedTransformA
btTransform m_calculatedTransformA
Definition: btGeneric6DofSpring2Constraint.h:282
btGeneric6DofSpring2Constraint::setEquilibriumPoint
void setEquilibriumPoint()
Definition: btGeneric6DofSpring2Constraint.cpp:1044
BT_6DOF_FLAGS_ERP_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:260
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:173
btTranslationalLimitMotor2::m_equilibriumPoint
btVector3 m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:178
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:827
btRotationalLimitMotor2::m_currentLimit
int m_currentLimit
Definition: btGeneric6DofSpring2Constraint.h:95
btGeneric6DofSpring2Constraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofSpring2Constraint.h:325
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:345
btTranslationalLimitMotor2::m_currentLimitError
btVector3 m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:182
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:137
btGeneric6DofSpring2Constraint::m_rotateOrder
RotateOrder m_rotateOrder
Definition: btGeneric6DofSpring2Constraint.h:278
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:615
btTranslationalLimitMotor2::m_servoTarget
btVector3 m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:175
btGeneric6DofSpring2Constraint::m_calculatedAxis
btVector3 m_calculatedAxis[3]
Definition: btGeneric6DofSpring2Constraint.h:285
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:102
btTranslationalLimitMotor2::m_stopERP
btVector3 m_stopERP
Definition: btGeneric6DofSpring2Constraint.h:168
btTranslationalLimitMotor2::m_motorCFM
btVector3 m_motorCFM
Definition: btGeneric6DofSpring2Constraint.h:171
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:134
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:77
btTypedConstraint::getMotorFactor
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.cpp:60
btGeneric6DofSpring2Constraint.h
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:226
btTranslationalLimitMotor2::m_targetVelocity
btVector3 m_targetVelocity
Definition: btGeneric6DofSpring2Constraint.h:179
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:120
btTranslationalLimitMotor2::m_enableSpring
bool m_enableSpring[3]
Definition: btGeneric6DofSpring2Constraint.h:174
btTranslationalLimitMotor2::m_currentLimit
int m_currentLimit[3]
Definition: btGeneric6DofSpring2Constraint.h:185
btRotationalLimitMotor2::m_equilibriumPoint
btScalar m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:90
btRotationalLimitMotor2::m_stopCFM
btScalar m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:79
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:128
btGeneric6DofSpring2Constraint::m_angularLimits
btRotationalLimitMotor2 m_angularLimits[3]
Definition: btGeneric6DofSpring2Constraint.h:276
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:258
RO_ZXY
Definition: btGeneric6DofSpring2Constraint.h:65
btRotationalLimitMotor2::testLimitValue
void testLimitValue(btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1076
btRotationalLimitMotor2::m_currentPosition
btScalar m_currentPosition
Definition: btGeneric6DofSpring2Constraint.h:94
btGeneric6DofSpring2Constraint::enableSpring
void enableSpring(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1017
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:170
btGeneric6DofSpring2Constraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btGeneric6DofSpring2Constraint.cpp:595
btRotationalLimitMotor2::m_enableSpring
bool m_enableSpring
Definition: btGeneric6DofSpring2Constraint.h:87
btGeneric6DofSpring2Constraint::m_frameInB
btTransform m_frameInB
Definition: btGeneric6DofSpring2Constraint.h:270
btGeneric6DofSpring2Constraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofSpring2Constraint.cpp:448
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:61
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:119
btRotationalLimitMotor2::m_springStiffness
btScalar m_springStiffness
Definition: btGeneric6DofSpring2Constraint.h:88
btGeneric6DofSpring2Constraint::calculateAngleInfo
void calculateAngleInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:281
btGeneric6DofSpring2Constraint::m_calculatedAxisAngleDiff
btVector3 m_calculatedAxisAngleDiff
Definition: btGeneric6DofSpring2Constraint.h:284
btGeneric6DofSpring2Constraint::testAngularLimitMotor
void testAngularLimitMotor(int axis_index)
Definition: btGeneric6DofSpring2Constraint.cpp:439
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:230
RO_XYZ
Definition: btGeneric6DofSpring2Constraint.h:61
btGeneric6DofSpring2Constraint::setServoTarget
void setServoTarget(int index, btScalar target)
Definition: btGeneric6DofSpring2Constraint.cpp:999
btRotationalLimitMotor2::m_springDamping
btScalar m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:89
btTranslationalLimitMotor2::m_upperLimit
btVector3 m_upperLimit
Definition: btGeneric6DofSpring2Constraint.h:166
btGeneric6DofSpring2Constraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don'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:547
btGeneric6DofSpring2Constraint::enableMotor
void enableMotor(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:972
BT_6DOF_FLAGS_CFM_STOP2
Definition: btGeneric6DofSpring2Constraint.h:257
btGeneric6DofSpring2Constraint::setBounce
void setBounce(int index, btScalar bounce)
Definition: btGeneric6DofSpring2Constraint.cpp:963