Bullet Collision Detection & Physics Library
btGeneric6DofSpring2Constraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16/*
172014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
18Pros:
19- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
20- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
21- Servo motor functionality
22- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
23- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
24
25Cons:
26- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
27- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
28*/
29
32
33/*
342007-09-09
35btGeneric6DofConstraint Refactored by Francisco Le?n
36email: projectileman@yahoo.com
37http://gimpact.sf.net
38*/
39
40
41
45#include <new>
46
47
48
51 , m_frameInA(frameInA)
52 , m_frameInB(frameInB)
53 , m_rotateOrder(rotOrder)
54 , m_flags(0)
55{
57}
58
59
62 , m_frameInB(frameInB)
63 , m_rotateOrder(rotOrder)
64 , m_flags(0)
65{
69}
70
71
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
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
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
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
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
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
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
282{
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)
331 m_calculatedAxis[1] = axis2.cross(axis0);
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
344 m_calculatedAxis[2] = axis0.cross(axis1);
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
357 m_calculatedAxis[0] = axis1.cross(axis2);
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
370 m_calculatedAxis[2] = axis0.cross(axis1);
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
383 m_calculatedAxis[0] = axis1.cross(axis2);
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
396 m_calculatedAxis[1] = axis2.cross(axis0);
399 break;
400 }
401 default:
402 btAssert(false);
403 }
404
408
409}
410
412{
414}
415
417{
422
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
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
449{
450 //prepare constraint
452 info->m_numConstraintRows = 0;
453 info->nub = 0;
454 int i;
455 //test linear limits
456 for(i = 0; i < 3; i++)
457 {
459 else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
462 }
463 //test angular limits
464 for (i=0;i<3 ;i++ )
465 {
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
476{
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
490int 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
494 for (int i=0;i<3 ;i++ )
495 {
497 { // re-use rotational motor code
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 ||
532 bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
533 m_angularLimits[indx2].m_currentLimit == 2 ||
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
549int 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
598{
599 m_frameInA = frameA;
600 m_frameInB = frameB;
603}
604
605
607{
610 for(int i = 0; i < 3; i++)
611 {
614 }
615}
616
617void 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
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;
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.
845{
846 if((axis >= 0) && (axis < 3))
847 {
848 switch(num)
849 {
851 m_linearLimits.m_stopERP[axis] = value;
853 break;
855 m_linearLimits.m_stopCFM[axis] = value;
857 break;
858 case BT_CONSTRAINT_ERP :
859 m_linearLimits.m_motorERP[axis] = value;
861 break;
862 case BT_CONSTRAINT_CFM :
863 m_linearLimits.m_motorCFM[axis] = value;
865 break;
866 default :
868 }
869 }
870 else if((axis >=3) && (axis < 6))
871 {
872 switch(num)
873 {
875 m_angularLimits[axis - 3].m_stopERP = value;
877 break;
879 m_angularLimits[axis - 3].m_stopCFM = value;
881 break;
882 case BT_CONSTRAINT_ERP :
883 m_angularLimits[axis - 3].m_motorERP = value;
885 break;
886 case BT_CONSTRAINT_CFM :
887 m_angularLimits[axis - 3].m_motorCFM = value;
889 break;
890 default :
892 }
893 }
894 else
895 {
897 }
898}
899
900//return the local value of parameter
902{
903 btScalar retVal = 0;
904 if((axis >= 0) && (axis < 3))
905 {
906 switch(num)
907 {
910 retVal = m_linearLimits.m_stopERP[axis];
911 break;
914 retVal = m_linearLimits.m_stopCFM[axis];
915 break;
916 case BT_CONSTRAINT_ERP :
918 retVal = m_linearLimits.m_motorERP[axis];
919 break;
920 case BT_CONSTRAINT_CFM :
922 retVal = m_linearLimits.m_motorCFM[axis];
923 break;
924 default :
926 }
927 }
928 else if((axis >=3) && (axis < 6))
929 {
930 switch(num)
931 {
934 retVal = m_angularLimits[axis - 3].m_stopERP;
935 break;
938 retVal = m_angularLimits[axis - 3].m_stopCFM;
939 break;
940 case BT_CONSTRAINT_ERP :
942 retVal = m_angularLimits[axis - 3].m_motorERP;
943 break;
944 case BT_CONSTRAINT_CFM :
946 retVal = m_angularLimits[axis - 3].m_motorCFM;
947 break;
948 default :
950 }
951 }
952 else
953 {
955 }
956 return retVal;
957}
958
959
960
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
976
978}
979
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
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
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
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
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
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
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
1079void 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
1091void 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
1104{
1106 int i;
1107 for( i = 0; i < 3; i++)
1109 for(i = 0; i < 3; i++)
1111}
1112
1114{
1115 btAssert((index >= 0) && (index < 6));
1117 if (index<3)
1119 else
1121}
1122
1124{
1125 btAssert((index >= 0) && (index < 6));
1126 if (index<3)
1128 else
1129 m_angularLimits[index - 3] .m_equilibriumPoint = val;
1130}
1131
1132
1134
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;
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
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
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
#define BT_6DOF_FLAGS_AXIS_SHIFT2
#define SIMD_PI
Definition: btScalar.h:504
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_ZERO
Definition: btScalar.h:524
#define BT_ONE
Definition: btScalar.h:523
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
#define SIMD_INFINITY
Definition: btScalar.h:522
#define SIMD_EPSILON
Definition: btScalar.h:521
btScalar btAsin(btScalar x)
Definition: btScalar.h:487
#define SIMD_HALF_PI
Definition: btScalar.h:506
#define SIMD_2_PI
Definition: btScalar.h:505
#define btAssert(x)
Definition: btScalar.h:131
#define btAssertConstrParams(_par)
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ D6_SPRING_2_CONSTRAINT_TYPE
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
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)
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
void setBounce(int index, btScalar bounce)
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
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...
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void setServoTarget(int index, btScalar target)
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
void setMaxMotorForce(int index, btScalar force)
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
void setFrames(const btTransform &frameA, const btTransform &frameB)
btVector3 getAxis(int axis_index) const
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
void setTargetVelocity(int index, btScalar velocity)
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...
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
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)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
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
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
btScalar getInvMass() const
Definition: btRigidBody.h:273
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:365
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:362
void testLimitValue(btScalar test_value)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
void testLimitValue(int limitIndex, btScalar test_value)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309