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