Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btGeneric6DofSpring2Constraint.h
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
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
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
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
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
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
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
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
591 SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
592 {
593         return sizeof(btGeneric6DofSpring2ConstraintData2);
594 }
595
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