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

Bullet Collision Detection & Physics Library

btGeneric6DofSpring2Constraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 /*
17 2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
18 Pros:
19 - Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
20 - Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
21 - Servo motor functionality
22 - Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
23 - Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
24
25 Cons:
26 - It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
27 - At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
28 */
29
32
33 /*
34 2007-09-09
35 btGeneric6DofConstraint Refactored by Francisco Le?n
36 email: projectileman@yahoo.com
37 http://gimpact.sf.net
38 */
39
40
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 btScalar curServoTarget = limot->m_servoTarget;
733 if (rotational)
734 {
735 if (error > SIMD_PI)
736 {
737 error -= SIMD_2_PI;
738 curServoTarget +=SIMD_2_PI;
739 }
740 if (error < -SIMD_PI)
741 {
742 error += SIMD_2_PI;
743 curServoTarget -=SIMD_2_PI;
744 }
745 }
746
747 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
748 btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
749 btScalar tag_vel = -targetvelocity;
750 btScalar mot_fact;
751 if(error != 0)
752 {
753 btScalar lowLimit;
754 btScalar hiLimit;
755 if(limot->m_loLimit > limot->m_hiLimit)
756 {
757 lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
758 hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
759 }
760 else
761 {
762 lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
763 hiLimit = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
764 }
765 mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
766 }
767 else
768 {
769 mot_fact = 0;
770 }
771 info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
772 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
773 info->m_upperLimit[srow] = limot->m_maxMotorForce;
774 info->cfm[srow] = limot->m_motorCFM;
775 srow += info->rowskip;
776 ++count;
777 }
778
779 if (limot->m_enableSpring)
780 {
781 btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
782 calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
783
784 //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
785 //if(cfm > 0.99999)
786 // cfm = 0.99999;
787 //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
788 //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
789 //info->m_lowerLimit[srow] = -SIMD_INFINITY;
790 //info->m_upperLimit[srow] = SIMD_INFINITY;
791
792 btScalar dt = BT_ONE / info->fps;
793 btScalar kd = limot->m_springDamping;
794 btScalar ks = limot->m_springStiffness;
795 btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
796 // btScalar erp = 0.1;
797 btScalar cfm = BT_ZERO;
798 btScalar mA = BT_ONE / m_rbA.getInvMass();
799 btScalar mB = BT_ONE / m_rbB.getInvMass();
800 btScalar m = mA > mB ? mB : mA;
801 btScalar angularfreq = sqrt(ks / m);
802
803
804 //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
805 if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
806 {
807 ks = BT_ONE / dt / dt / btScalar(16.0) * m;
808 }
809 //avoid damping that would blow up the spring
810 if(limot->m_springDampingLimited && kd * dt > m)
811 {
812 kd = m / dt;
813 }
814 btScalar fs = ks * error * dt;
815 btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
816 btScalar f = (fs+fd);
817
818 info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
819
820 btScalar minf = f < fd ? f : fd;
821 btScalar maxf = f < fd ? fd : f;
822 if(!rotational)
823 {
824 info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
825 info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
826 }
827 else
828 {
829 info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
830 info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
831 }
832
833 info->cfm[srow] = cfm;
834 srow += info->rowskip;
835 ++count;
836 }
837
838 return count;
839 }
840
841
842 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
843 //If no axis is provided, it uses the default axis for this constraint.
844 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
845 {
846 if((axis >= 0) && (axis < 3))
847 {
848 switch(num)
849 {
850 case BT_CONSTRAINT_STOP_ERP :
851 m_linearLimits.m_stopERP[axis] = value;
852 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
853 break;
854 case BT_CONSTRAINT_STOP_CFM :
855 m_linearLimits.m_stopCFM[axis] = value;
856 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
857 break;
858 case BT_CONSTRAINT_ERP :
859 m_linearLimits.m_motorERP[axis] = value;
860 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
861 break;
862 case BT_CONSTRAINT_CFM :
863 m_linearLimits.m_motorCFM[axis] = value;
864 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
865 break;
866 default :
867 btAssertConstrParams(0);
868 }
869 }
870 else if((axis >=3) && (axis < 6))
871 {
872 switch(num)
873 {
874 case BT_CONSTRAINT_STOP_ERP :
875 m_angularLimits[axis - 3].m_stopERP = value;
876 m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
877 break;
878 case BT_CONSTRAINT_STOP_CFM :
879 m_angularLimits[axis - 3].m_stopCFM = value;
880 m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
881 break;
882 case BT_CONSTRAINT_ERP :
883 m_angularLimits[axis - 3].m_motorERP = value;
884 m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
885 break;
886 case BT_CONSTRAINT_CFM :
887 m_angularLimits[axis - 3].m_motorCFM = value;
888 m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
889 break;
890 default :
891 btAssertConstrParams(0);
892 }
893 }
894 else
895 {
896 btAssertConstrParams(0);
897 }
898 }
899
900 //return the local value of parameter
901 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
902 {
903 btScalar retVal = 0;
904 if((axis >= 0) && (axis < 3))
905 {
906 switch(num)
907 {
908 case BT_CONSTRAINT_STOP_ERP :
909 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
910 retVal = m_linearLimits.m_stopERP[axis];
911 break;
912 case BT_CONSTRAINT_STOP_CFM :
913 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
914 retVal = m_linearLimits.m_stopCFM[axis];
915 break;
916 case BT_CONSTRAINT_ERP :
917 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
918 retVal = m_linearLimits.m_motorERP[axis];
919 break;
920 case BT_CONSTRAINT_CFM :
921 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
922 retVal = m_linearLimits.m_motorCFM[axis];
923 break;
924 default :
925 btAssertConstrParams(0);
926 }
927 }
928 else if((axis >=3) && (axis < 6))
929 {
930 switch(num)
931 {
932 case BT_CONSTRAINT_STOP_ERP :
933 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
934 retVal = m_angularLimits[axis - 3].m_stopERP;
935 break;
936 case BT_CONSTRAINT_STOP_CFM :
937 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
938 retVal = m_angularLimits[axis - 3].m_stopCFM;
939 break;
940 case BT_CONSTRAINT_ERP :
941 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
942 retVal = m_angularLimits[axis - 3].m_motorERP;
943 break;
944 case BT_CONSTRAINT_CFM :
945 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
946 retVal = m_angularLimits[axis - 3].m_motorCFM;
947 break;
948 default :
949 btAssertConstrParams(0);
950 }
951 }
952 else
953 {
954 btAssertConstrParams(0);
955 }
956 return retVal;
957 }
958
959
960
961 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
962 {
963 btVector3 zAxis = axis1.normalized();
964 btVector3 yAxis = axis2.normalized();
965 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
966
967 btTransform frameInW;
968 frameInW.setIdentity();
969 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
970 xAxis[1], yAxis[1], zAxis[1],
971 xAxis[2], yAxis[2], zAxis[2]);
972
973 // now get constraint frame in local coordinate systems
974 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
975 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
976
977 calculateTransforms();
978 }
979
980 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
981 {
982 btAssert((index >= 0) && (index < 6));
983 if (index<3)
984 m_linearLimits.m_bounce[index] = bounce;
985 else
986 m_angularLimits[index - 3].m_bounce = bounce;
987 }
988
989 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
990 {
991 btAssert((index >= 0) && (index < 6));
992 if (index<3)
993 m_linearLimits.m_enableMotor[index] = onOff;
994 else
995 m_angularLimits[index - 3].m_enableMotor = onOff;
996 }
997
998 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
999 {
1000 btAssert((index >= 0) && (index < 6));
1001 if (index<3)
1002 m_linearLimits.m_servoMotor[index] = onOff;
1003 else
1004 m_angularLimits[index - 3].m_servoMotor = onOff;
1005 }
1006
1007 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
1008 {
1009 btAssert((index >= 0) && (index < 6));
1010 if (index<3)
1011 m_linearLimits.m_targetVelocity[index] = velocity;
1012 else
1013 m_angularLimits[index - 3].m_targetVelocity = velocity;
1014 }
1015
1016
1017
1018 void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
1019 {
1020 btAssert((index >= 0) && (index < 6));
1021 if (index<3)
1022 {
1023 m_linearLimits.m_servoTarget[index] = targetOrg;
1024 }
1025 else
1026 {
1027 //wrap between -PI and PI, see also
1028 //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1029
1030 btScalar target = targetOrg+SIMD_PI;
1031 if (1)
1032 {
1033 btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
1034 // handle boundary cases resulted from floating-point cut off:
1035 {
1036 if (m>=SIMD_2_PI)
1037 {
1038 target = 0;
1039 } else
1040 {
1041 if (m<0 )
1042 {
1043 if (SIMD_2_PI+m == SIMD_2_PI)
1044 target = 0;
1045 else
1046 target = SIMD_2_PI+m;
1047 }
1048 else
1049 {
1050 target = m;
1051 }
1052 }
1053 }
1054 target -= SIMD_PI;
1055 }
1056
1057 m_angularLimits[index - 3].m_servoTarget = target;
1058 }
1059 }
1060
1061 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
1062 {
1063 btAssert((index >= 0) && (index < 6));
1064 if (index<3)
1065 m_linearLimits.m_maxMotorForce[index] = force;
1066 else
1067 m_angularLimits[index - 3].m_maxMotorForce = force;
1068 }
1069
1070 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
1071 {
1072 btAssert((index >= 0) && (index < 6));
1073 if (index<3)
1074 m_linearLimits.m_enableSpring[index] = onOff;
1075 else
1076 m_angularLimits[index - 3] .m_enableSpring = onOff;
1077 }
1078
1079 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1080 {
1081 btAssert((index >= 0) && (index < 6));
1082 if (index<3) {
1083 m_linearLimits.m_springStiffness[index] = stiffness;
1084 m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1085 } else {
1086 m_angularLimits[index - 3].m_springStiffness = stiffness;
1087 m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1088 }
1089 }
1090
1091 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1092 {
1093 btAssert((index >= 0) && (index < 6));
1094 if (index<3) {
1095 m_linearLimits.m_springDamping[index] = damping;
1096 m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1097 } else {
1098 m_angularLimits[index - 3].m_springDamping = damping;
1099 m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1100 }
1101 }
1102
1103 void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
1104 {
1105 calculateTransforms();
1106 int i;
1107 for( i = 0; i < 3; i++)
1108 m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1109 for(i = 0; i < 3; i++)
1110 m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1111 }
1112
1113 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
1114 {
1115 btAssert((index >= 0) && (index < 6));
1116 calculateTransforms();
1117 if (index<3)
1118 m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1119 else
1120 m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1121 }
1122
1123 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
1124 {
1125 btAssert((index >= 0) && (index < 6));
1126 if (index<3)
1127 m_linearLimits.m_equilibriumPoint[index] = val;
1128 else
1129 m_angularLimits[index - 3] .m_equilibriumPoint = val;
1130 }
1131
1132
1134
1135 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
1136 {
1137 //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
1138 if(m_loLimit > m_hiLimit) {
1139 m_currentLimit = 0;
1140 m_currentLimitError = btScalar(0.f);
1141 }
1142 else if(m_loLimit == m_hiLimit) {
1143 m_currentLimitError = test_value - m_loLimit;
1144 m_currentLimit = 3;
1145 } else {
1146 m_currentLimitError = test_value - m_loLimit;
1147 m_currentLimitErrorHi = test_value - m_hiLimit;
1148 m_currentLimit = 4;
1149 }
1150 }
1151
1153
1154 void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
1155 {
1156 btScalar loLimit = m_lowerLimit[limitIndex];
1157 btScalar hiLimit = m_upperLimit[limitIndex];
1158 if(loLimit > hiLimit) {
1159 m_currentLimitError[limitIndex] = 0;
1160 m_currentLimit[limitIndex] = 0;
1161 }
1162 else if(loLimit == hiLimit) {
1163 m_currentLimitError[limitIndex] = test_value - loLimit;
1164 m_currentLimit[limitIndex] = 3;
1165 } else {
1166 m_currentLimitError[limitIndex] = test_value - loLimit;
1167 m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1168 m_currentLimit[limitIndex] = 4;
1169 }
1170 }
1171
1172
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:143
RO_YXZ
Definition: btGeneric6DofSpring2Constraint.h:63
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
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:1007
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
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:365
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:1061
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:1154
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::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btGeneric6DofSpring2Constraint.cpp:901
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btGeneric6DofSpring2Constraint::setServo
void setServo(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:998
BT_ONE
#define BT_ONE
Definition: btScalar.h:523
btGeneric6DofSpring2Constraint::m_flags
int m_flags
Definition: btGeneric6DofSpring2Constraint.h:302
btGeneric6DofSpring2Constraint::m_factB
btScalar m_factB
Definition: btGeneric6DofSpring2Constraint.h:300
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:222
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:131
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:961
BT_6DOF_FLAGS_CFM_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:271
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
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
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:226
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:506
btRotationalLimitMotor2
Definition: btGeneric6DofSpring2Constraint.h:69
BT_ZERO
#define BT_ZERO
Definition: btScalar.h:524
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btGeneric6DofSpring2Constraint::calculateTransforms
void calculateTransforms()
Definition: btGeneric6DofSpring2Constraint.cpp:411
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
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::getAxis
btVector3 getAxis(int axis_index) const
Definition: btGeneric6DofSpring2Constraint.h:354
btGeneric6DofSpring2Constraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:298
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
D6_SPRING_2_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:47
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:504
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:522
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:57
btRotationalLimitMotor2::m_currentLimitError
btScalar m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:94
SIMD_2_PI
#define SIMD_2_PI
Definition: btScalar.h:505
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
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:362
btTranslationalLimitMotor2::m_bounce
btVector3 m_bounce
Definition: btGeneric6DofSpring2Constraint.h:173
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:273
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:129
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
btTranslationalLimitMotor2::m_lowerLimit
btVector3 m_lowerLimit
Definition: btGeneric6DofSpring2Constraint.h:171
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:1103
BT_6DOF_FLAGS_ERP_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:272
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:844
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:329
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:341
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
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
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
btGeneric6DofSpring2Constraint::setStiffness
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
Definition: btGeneric6DofSpring2Constraint.cpp:1079
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:1135
btRotationalLimitMotor2::m_currentPosition
btScalar m_currentPosition
Definition: btGeneric6DofSpring2Constraint.h:96
btGeneric6DofSpring2Constraint::enableSpring
void enableSpring(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1070
btAsin
btScalar btAsin(btScalar x)
Definition: btScalar.h:487
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
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
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:1091
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
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
RO_XYZ
Definition: btGeneric6DofSpring2Constraint.h:61
btGeneric6DofSpring2Constraint::setServoTarget
void setServoTarget(int index, btScalar target)
Definition: btGeneric6DofSpring2Constraint.cpp:1018
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:292
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:989
BT_6DOF_FLAGS_CFM_STOP2
Definition: btGeneric6DofSpring2Constraint.h:269
btGeneric6DofSpring2Constraint::setBounce
void setBounce(int index, btScalar bounce)
Definition: btGeneric6DofSpring2Constraint.cpp:980