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.
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
30 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
31 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
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 #ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
42 #define BT_GENERIC_6DOF_CONSTRAINT2_H
43
44 #include "LinearMath/btVector3.h"
45 #include "btJacobianEntry.h"
46 #include "btTypedConstraint.h"
47
48 class btRigidBody;
49
50
51 #ifdef BT_USE_DOUBLE_PRECISION
52 #define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
53 #define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
54 #else
55 #define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
56 #define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
57 #endif //BT_USE_DOUBLE_PRECISION
58
59 enum RotateOrder
60 {
61 RO_XYZ=0,
62 RO_XZY,
63 RO_YXZ,
64 RO_YZX,
65 RO_ZXY,
66 RO_ZYX
67 };
68
69 class btRotationalLimitMotor2
70 {
71 public:
72 // upper < lower means free
73 // upper == lower means locked
74 // upper > lower means limited
75 btScalar m_loLimit;
76 btScalar m_hiLimit;
77 btScalar m_bounce;
78 btScalar m_stopERP;
79 btScalar m_stopCFM;
80 btScalar m_motorERP;
81 btScalar m_motorCFM;
82 bool m_enableMotor;
83 btScalar m_targetVelocity;
84 btScalar m_maxMotorForce;
85 bool m_servoMotor;
86 btScalar m_servoTarget;
87 bool m_enableSpring;
88 btScalar m_springStiffness;
89 bool m_springStiffnessLimited;
90 btScalar m_springDamping;
91 bool m_springDampingLimited;
92 btScalar m_equilibriumPoint;
93
94 btScalar m_currentLimitError;
95 btScalar m_currentLimitErrorHi;
96 btScalar m_currentPosition;
97 int m_currentLimit;
98
btRotationalLimitMotor2()99 btRotationalLimitMotor2()
100 {
101 m_loLimit = 1.0f;
102 m_hiLimit = -1.0f;
103 m_bounce = 0.0f;
104 m_stopERP = 0.2f;
105 m_stopCFM = 0.f;
106 m_motorERP = 0.9f;
107 m_motorCFM = 0.f;
108 m_enableMotor = false;
109 m_targetVelocity = 0;
110 m_maxMotorForce = 0.1f;
111 m_servoMotor = false;
112 m_servoTarget = 0;
113 m_enableSpring = false;
114 m_springStiffness = 0;
115 m_springStiffnessLimited = false;
116 m_springDamping = 0;
117 m_springDampingLimited = false;
118 m_equilibriumPoint = 0;
119
120 m_currentLimitError = 0;
121 m_currentLimitErrorHi = 0;
122 m_currentPosition = 0;
123 m_currentLimit = 0;
124 }
125
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)126 btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
127 {
128 m_loLimit = limot.m_loLimit;
129 m_hiLimit = limot.m_hiLimit;
130 m_bounce = limot.m_bounce;
131 m_stopERP = limot.m_stopERP;
132 m_stopCFM = limot.m_stopCFM;
133 m_motorERP = limot.m_motorERP;
134 m_motorCFM = limot.m_motorCFM;
135 m_enableMotor = limot.m_enableMotor;
136 m_targetVelocity = limot.m_targetVelocity;
137 m_maxMotorForce = limot.m_maxMotorForce;
138 m_servoMotor = limot.m_servoMotor;
139 m_servoTarget = limot.m_servoTarget;
140 m_enableSpring = limot.m_enableSpring;
141 m_springStiffness = limot.m_springStiffness;
142 m_springStiffnessLimited = limot.m_springStiffnessLimited;
143 m_springDamping = limot.m_springDamping;
144 m_springDampingLimited = limot.m_springDampingLimited;
145 m_equilibriumPoint = limot.m_equilibriumPoint;
146
147 m_currentLimitError = limot.m_currentLimitError;
148 m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
149 m_currentPosition = limot.m_currentPosition;
150 m_currentLimit = limot.m_currentLimit;
151 }
152
153
isLimited()154 bool isLimited()
155 {
156 if(m_loLimit > m_hiLimit) return false;
157 return true;
158 }
159
160 void testLimitValue(btScalar test_value);
161 };
162
163
164
165 class btTranslationalLimitMotor2
166 {
167 public:
168 // upper < lower means free
169 // upper == lower means locked
170 // upper > lower means limited
171 btVector3 m_lowerLimit;
172 btVector3 m_upperLimit;
173 btVector3 m_bounce;
174 btVector3 m_stopERP;
175 btVector3 m_stopCFM;
176 btVector3 m_motorERP;
177 btVector3 m_motorCFM;
178 bool m_enableMotor[3];
179 bool m_servoMotor[3];
180 bool m_enableSpring[3];
181 btVector3 m_servoTarget;
182 btVector3 m_springStiffness;
183 bool m_springStiffnessLimited[3];
184 btVector3 m_springDamping;
185 bool m_springDampingLimited[3];
186 btVector3 m_equilibriumPoint;
187 btVector3 m_targetVelocity;
188 btVector3 m_maxMotorForce;
189
190 btVector3 m_currentLimitError;
191 btVector3 m_currentLimitErrorHi;
192 btVector3 m_currentLinearDiff;
193 int m_currentLimit[3];
194
btTranslationalLimitMotor2()195 btTranslationalLimitMotor2()
196 {
197 m_lowerLimit .setValue(0.f , 0.f , 0.f );
198 m_upperLimit .setValue(0.f , 0.f , 0.f );
199 m_bounce .setValue(0.f , 0.f , 0.f );
200 m_stopERP .setValue(0.2f, 0.2f, 0.2f);
201 m_stopCFM .setValue(0.f , 0.f , 0.f );
202 m_motorERP .setValue(0.9f, 0.9f, 0.9f);
203 m_motorCFM .setValue(0.f , 0.f , 0.f );
204
205 m_currentLimitError .setValue(0.f , 0.f , 0.f );
206 m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
207 m_currentLinearDiff .setValue(0.f , 0.f , 0.f );
208
209 for(int i=0; i < 3; i++)
210 {
211 m_enableMotor[i] = false;
212 m_servoMotor[i] = false;
213 m_enableSpring[i] = false;
214 m_servoTarget[i] = btScalar(0.f);
215 m_springStiffness[i] = btScalar(0.f);
216 m_springStiffnessLimited[i] = false;
217 m_springDamping[i] = btScalar(0.f);
218 m_springDampingLimited[i] = false;
219 m_equilibriumPoint[i] = btScalar(0.f);
220 m_targetVelocity[i] = btScalar(0.f);
221 m_maxMotorForce[i] = btScalar(0.f);
222
223 m_currentLimit[i] = 0;
224 }
225 }
226
btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other)227 btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
228 {
229 m_lowerLimit = other.m_lowerLimit;
230 m_upperLimit = other.m_upperLimit;
231 m_bounce = other.m_bounce;
232 m_stopERP = other.m_stopERP;
233 m_stopCFM = other.m_stopCFM;
234 m_motorERP = other.m_motorERP;
235 m_motorCFM = other.m_motorCFM;
236
237 m_currentLimitError = other.m_currentLimitError;
238 m_currentLimitErrorHi = other.m_currentLimitErrorHi;
239 m_currentLinearDiff = other.m_currentLinearDiff;
240
241 for(int i=0; i < 3; i++)
242 {
243 m_enableMotor[i] = other.m_enableMotor[i];
244 m_servoMotor[i] = other.m_servoMotor[i];
245 m_enableSpring[i] = other.m_enableSpring[i];
246 m_servoTarget[i] = other.m_servoTarget[i];
247 m_springStiffness[i] = other.m_springStiffness[i];
248 m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
249 m_springDamping[i] = other.m_springDamping[i];
250 m_springDampingLimited[i] = other.m_springDampingLimited[i];
251 m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
252 m_targetVelocity[i] = other.m_targetVelocity[i];
253 m_maxMotorForce[i] = other.m_maxMotorForce[i];
254
255 m_currentLimit[i] = other.m_currentLimit[i];
256 }
257 }
258
isLimited(int limitIndex)259 inline bool isLimited(int limitIndex)
260 {
261 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
262 }
263
264 void testLimitValue(int limitIndex, btScalar test_value);
265 };
266
267 enum bt6DofFlags2
268 {
269 BT_6DOF_FLAGS_CFM_STOP2 = 1,
270 BT_6DOF_FLAGS_ERP_STOP2 = 2,
271 BT_6DOF_FLAGS_CFM_MOTO2 = 4,
272 BT_6DOF_FLAGS_ERP_MOTO2 = 8
273 };
274 #define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
275
276
ATTRIBUTE_ALIGNED16(class)277 ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
278 {
279 protected:
280
281 btTransform m_frameInA;
282 btTransform m_frameInB;
283
284 btJacobianEntry m_jacLinear[3];
285 btJacobianEntry m_jacAng[3];
286
287 btTranslationalLimitMotor2 m_linearLimits;
288 btRotationalLimitMotor2 m_angularLimits[3];
289
290 RotateOrder m_rotateOrder;
291
292 protected:
293
294 btTransform m_calculatedTransformA;
295 btTransform m_calculatedTransformB;
296 btVector3 m_calculatedAxisAngleDiff;
297 btVector3 m_calculatedAxis[3];
298 btVector3 m_calculatedLinearDiff;
299 btScalar m_factA;
300 btScalar m_factB;
301 bool m_hasStaticBody;
302 int m_flags;
303
304 btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
305 {
306 btAssert(0);
307 return *this;
308 }
309
310 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);
311 int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
312
313 void calculateLinearInfo();
314 void calculateAngleInfo();
315 void testAngularLimitMotor(int axis_index);
316
317 void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
318 int get_limit_motor_info2(btRotationalLimitMotor2* limot,
319 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
320 btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
321
322 static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
323 static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
324 static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
325 static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
326 static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
327 static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
328 static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
329
330 public:
331
332 BT_DECLARE_ALIGNED_ALLOCATOR();
333
334 btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
335 btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
336
337 virtual void buildJacobian() {}
338 virtual void getInfo1 (btConstraintInfo1* info);
339 virtual void getInfo2 (btConstraintInfo2* info);
340 virtual int calculateSerializeBufferSize() const;
341 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
342
343 btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
344 btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
345
346 // Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
347 void calculateTransforms(const btTransform& transA,const btTransform& transB);
348 void calculateTransforms();
349
350 // Gets the global transform of the offset for body A
351 const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
352 // Gets the global transform of the offset for body B
353 const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
354
355 const btTransform & getFrameOffsetA() const { return m_frameInA; }
356 const btTransform & getFrameOffsetB() const { return m_frameInB; }
357
358 btTransform & getFrameOffsetA() { return m_frameInA; }
359 btTransform & getFrameOffsetB() { return m_frameInB; }
360
361 // Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
362 btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
363
364 // Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
365 btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
366
367 // Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
368 btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
369
370 void setFrames(const btTransform & frameA, const btTransform & frameB);
371
372 void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
373 void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
374 void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
375 void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
376
377 void setAngularLowerLimit(const btVector3& angularLower)
378 {
379 for(int i = 0; i < 3; i++)
380 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
381 }
382
383 void setAngularLowerLimitReversed(const btVector3& angularLower)
384 {
385 for(int i = 0; i < 3; i++)
386 m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
387 }
388
389 void getAngularLowerLimit(btVector3& angularLower)
390 {
391 for(int i = 0; i < 3; i++)
392 angularLower[i] = m_angularLimits[i].m_loLimit;
393 }
394
395 void getAngularLowerLimitReversed(btVector3& angularLower)
396 {
397 for(int i = 0; i < 3; i++)
398 angularLower[i] = -m_angularLimits[i].m_hiLimit;
399 }
400
401 void setAngularUpperLimit(const btVector3& angularUpper)
402 {
403 for(int i = 0; i < 3; i++)
404 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
405 }
406
407 void setAngularUpperLimitReversed(const btVector3& angularUpper)
408 {
409 for(int i = 0; i < 3; i++)
410 m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
411 }
412
413 void getAngularUpperLimit(btVector3& angularUpper)
414 {
415 for(int i = 0; i < 3; i++)
416 angularUpper[i] = m_angularLimits[i].m_hiLimit;
417 }
418
419 void getAngularUpperLimitReversed(btVector3& angularUpper)
420 {
421 for(int i = 0; i < 3; i++)
422 angularUpper[i] = -m_angularLimits[i].m_loLimit;
423 }
424
425 //first 3 are linear, next 3 are angular
426
427 void setLimit(int axis, btScalar lo, btScalar hi)
428 {
429 if(axis<3)
430 {
431 m_linearLimits.m_lowerLimit[axis] = lo;
432 m_linearLimits.m_upperLimit[axis] = hi;
433 }
434 else
435 {
436 lo = btNormalizeAngle(lo);
437 hi = btNormalizeAngle(hi);
438 m_angularLimits[axis-3].m_loLimit = lo;
439 m_angularLimits[axis-3].m_hiLimit = hi;
440 }
441 }
442
443 void setLimitReversed(int axis, btScalar lo, btScalar hi)
444 {
445 if(axis<3)
446 {
447 m_linearLimits.m_lowerLimit[axis] = lo;
448 m_linearLimits.m_upperLimit[axis] = hi;
449 }
450 else
451 {
452 lo = btNormalizeAngle(lo);
453 hi = btNormalizeAngle(hi);
454 m_angularLimits[axis-3].m_hiLimit = -lo;
455 m_angularLimits[axis-3].m_loLimit = -hi;
456 }
457 }
458
459 bool isLimited(int limitIndex)
460 {
461 if(limitIndex<3)
462 {
463 return m_linearLimits.isLimited(limitIndex);
464 }
465 return m_angularLimits[limitIndex-3].isLimited();
466 }
467
468 void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
469 RotateOrder getRotationOrder() { return m_rotateOrder; }
470
471 void setAxis( const btVector3& axis1, const btVector3& axis2);
472
473 void setBounce(int index, btScalar bounce);
474
475 void enableMotor(int index, bool onOff);
476 void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
477 void setTargetVelocity(int index, btScalar velocity);
478 void setServoTarget(int index, btScalar target);
479 void setMaxMotorForce(int index, btScalar force);
480
481 void enableSpring(int index, bool onOff);
482 void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
483 void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
484 void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
485 void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
486 void setEquilibriumPoint(int index, btScalar val);
487
488 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
489 //If no axis is provided, it uses the default axis for this constraint.
490 virtual void setParam(int num, btScalar value, int axis = -1);
491 virtual btScalar getParam(int num, int axis = -1) const;
492 };
493
494
495 struct btGeneric6DofSpring2ConstraintData
496 {
497 btTypedConstraintData m_typeConstraintData;
498 btTransformFloatData m_rbAFrame;
499 btTransformFloatData m_rbBFrame;
500
501 btVector3FloatData m_linearUpperLimit;
502 btVector3FloatData m_linearLowerLimit;
503 btVector3FloatData m_linearBounce;
504 btVector3FloatData m_linearStopERP;
505 btVector3FloatData m_linearStopCFM;
506 btVector3FloatData m_linearMotorERP;
507 btVector3FloatData m_linearMotorCFM;
508 btVector3FloatData m_linearTargetVelocity;
509 btVector3FloatData m_linearMaxMotorForce;
510 btVector3FloatData m_linearServoTarget;
511 btVector3FloatData m_linearSpringStiffness;
512 btVector3FloatData m_linearSpringDamping;
513 btVector3FloatData m_linearEquilibriumPoint;
514 char m_linearEnableMotor[4];
515 char m_linearServoMotor[4];
516 char m_linearEnableSpring[4];
517 char m_linearSpringStiffnessLimited[4];
518 char m_linearSpringDampingLimited[4];
519 char m_padding1[4];
520
521 btVector3FloatData m_angularUpperLimit;
522 btVector3FloatData m_angularLowerLimit;
523 btVector3FloatData m_angularBounce;
524 btVector3FloatData m_angularStopERP;
525 btVector3FloatData m_angularStopCFM;
526 btVector3FloatData m_angularMotorERP;
527 btVector3FloatData m_angularMotorCFM;
528 btVector3FloatData m_angularTargetVelocity;
529 btVector3FloatData m_angularMaxMotorForce;
530 btVector3FloatData m_angularServoTarget;
531 btVector3FloatData m_angularSpringStiffness;
532 btVector3FloatData m_angularSpringDamping;
533 btVector3FloatData m_angularEquilibriumPoint;
534 char m_angularEnableMotor[4];
535 char m_angularServoMotor[4];
536 char m_angularEnableSpring[4];
537 char m_angularSpringStiffnessLimited[4];
538 char m_angularSpringDampingLimited[4];
539
540 int m_rotateOrder;
541 };
542
543 struct btGeneric6DofSpring2ConstraintDoubleData2
544 {
545 btTypedConstraintDoubleData m_typeConstraintData;
546 btTransformDoubleData m_rbAFrame;
547 btTransformDoubleData m_rbBFrame;
548
549 btVector3DoubleData m_linearUpperLimit;
550 btVector3DoubleData m_linearLowerLimit;
551 btVector3DoubleData m_linearBounce;
552 btVector3DoubleData m_linearStopERP;
553 btVector3DoubleData m_linearStopCFM;
554 btVector3DoubleData m_linearMotorERP;
555 btVector3DoubleData m_linearMotorCFM;
556 btVector3DoubleData m_linearTargetVelocity;
557 btVector3DoubleData m_linearMaxMotorForce;
558 btVector3DoubleData m_linearServoTarget;
559 btVector3DoubleData m_linearSpringStiffness;
560 btVector3DoubleData m_linearSpringDamping;
561 btVector3DoubleData m_linearEquilibriumPoint;
562 char m_linearEnableMotor[4];
563 char m_linearServoMotor[4];
564 char m_linearEnableSpring[4];
565 char m_linearSpringStiffnessLimited[4];
566 char m_linearSpringDampingLimited[4];
567 char m_padding1[4];
568
569 btVector3DoubleData m_angularUpperLimit;
570 btVector3DoubleData m_angularLowerLimit;
571 btVector3DoubleData m_angularBounce;
572 btVector3DoubleData m_angularStopERP;
573 btVector3DoubleData m_angularStopCFM;
574 btVector3DoubleData m_angularMotorERP;
575 btVector3DoubleData m_angularMotorCFM;
576 btVector3DoubleData m_angularTargetVelocity;
577 btVector3DoubleData m_angularMaxMotorForce;
578 btVector3DoubleData m_angularServoTarget;
579 btVector3DoubleData m_angularSpringStiffness;
580 btVector3DoubleData m_angularSpringDamping;
581 btVector3DoubleData m_angularEquilibriumPoint;
582 char m_angularEnableMotor[4];
583 char m_angularServoMotor[4];
584 char m_angularEnableSpring[4];
585 char m_angularSpringStiffnessLimited[4];
586 char m_angularSpringDampingLimited[4];
587
588 int m_rotateOrder;
589 };
590
calculateSerializeBufferSize()591 SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
592 {
593 return sizeof(btGeneric6DofSpring2ConstraintData2);
594 }
595
serialize(void * dataBuffer,btSerializer * serializer)596 SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
597 {
598 btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
599 btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
600
601 m_frameInA.serialize(dof->m_rbAFrame);
602 m_frameInB.serialize(dof->m_rbBFrame);
603
604 int i;
605 for (i=0;i<3;i++)
606 {
607 dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
608 dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
609 dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
610 dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
611 dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
612 dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
613 dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
614 dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
615 dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
616 dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
617 dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
618 dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
619 dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
620 }
621 dof->m_angularLowerLimit.m_floats[3] = 0;
622 dof->m_angularUpperLimit.m_floats[3] = 0;
623 dof->m_angularBounce.m_floats[3] = 0;
624 dof->m_angularStopERP.m_floats[3] = 0;
625 dof->m_angularStopCFM.m_floats[3] = 0;
626 dof->m_angularMotorERP.m_floats[3] = 0;
627 dof->m_angularMotorCFM.m_floats[3] = 0;
628 dof->m_angularTargetVelocity.m_floats[3] = 0;
629 dof->m_angularMaxMotorForce.m_floats[3] = 0;
630 dof->m_angularServoTarget.m_floats[3] = 0;
631 dof->m_angularSpringStiffness.m_floats[3] = 0;
632 dof->m_angularSpringDamping.m_floats[3] = 0;
633 dof->m_angularEquilibriumPoint.m_floats[3] = 0;
634 for (i=0;i<4;i++)
635 {
636 dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
637 dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
638 dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
639 dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
640 dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
641 }
642
643 m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
644 m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
645 m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
646 m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
647 m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
648 m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
649 m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
650 m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
651 m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
652 m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
653 m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
654 m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
655 m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
656 for (i=0;i<4;i++)
657 {
658 dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
659 dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
660 dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
661 dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
662 dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
663 }
664
665 dof->m_rotateOrder = m_rotateOrder;
666
667 return btGeneric6DofSpring2ConstraintDataName;
668 }
669
670
671
672
673
674 #endif //BT_GENERIC_6DOF_CONSTRAINT_H
675