dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

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