dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btSolverBody.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 #ifndef BT_SOLVER_BODY_H
  17 #define BT_SOLVER_BODY_H
  18
  19 class btRigidBody;
  20 #include "LinearMath/btVector3.h"
  21 #include "LinearMath/btMatrix3x3.h"
  22
  23 #include "LinearMath/btAlignedAllocator.h"
  24 #include "LinearMath/btTransformUtil.h"
  25
  26 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
  27 #ifdef BT_USE_SSE
  28 #define USE_SIMD 1
  29 #endif  //
  30
  31 #ifdef USE_SIMD
  32
  33 struct btSimdScalar
  34 {
  35         SIMD_FORCE_INLINE btSimdScalar()
  36         {
  37         }
  38
  39         SIMD_FORCE_INLINE btSimdScalar(float fl)
  40                 : m_vec128(_mm_set1_ps(fl))
  41         {
  42         }
  43
  44         SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
  45                 : m_vec128(v128)
  46         {
  47         }
  48         union {
  49                 __m128 m_vec128;
  50                 float m_floats[4];
  51                 int m_ints[4];
  52                 btScalar m_unusedPadding;
  53         };
  54         SIMD_FORCE_INLINE __m128 get128()
  55         {
  56                 return m_vec128;
  57         }
  58
  59         SIMD_FORCE_INLINE const __m128 get128() const
  60         {
  61                 return m_vec128;
  62         }
  63
  64         SIMD_FORCE_INLINE void set128(__m128 v128)
  65         {
  66                 m_vec128 = v128;
  67         }
  68
  69         SIMD_FORCE_INLINE operator __m128()
  70         {
  71                 return m_vec128;
  72         }
  73         SIMD_FORCE_INLINE operator const __m128() const
  74         {
  75                 return m_vec128;
  76         }
  77
  78         SIMD_FORCE_INLINE operator float() const
  79         {
  80                 return m_floats[0];
  81         }
  82 };
  83
  84 ///@brief Return the elementwise product of two btSimdScalar
  85 SIMD_FORCE_INLINE btSimdScalar
  86 operator*(const btSimdScalar& v1, const btSimdScalar& v2)
  87 {
  88         return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
  89 }
  90
  91 ///@brief Return the elementwise product of two btSimdScalar
  92 SIMD_FORCE_INLINE btSimdScalar
  93 operator+(const btSimdScalar& v1, const btSimdScalar& v2)
  94 {
  95         return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
  96 }
  97
  98 #else
  99 #define btSimdScalar btScalar
 100 #endif
 101
 102 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 103 ATTRIBUTE_ALIGNED16(struct)
 104 btSolverBody
 105 {
 106         BT_DECLARE_ALIGNED_ALLOCATOR();
 107         btTransform m_worldTransform;
 108         btVector3 m_deltaLinearVelocity;
 109         btVector3 m_deltaAngularVelocity;
 110         btVector3 m_angularFactor;
 111         btVector3 m_linearFactor;
 112         btVector3 m_invMass;
 113         btVector3 m_pushVelocity;
 114         btVector3 m_turnVelocity;
 115         btVector3 m_linearVelocity;
 116         btVector3 m_angularVelocity;
 117         btVector3 m_externalForceImpulse;
 118         btVector3 m_externalTorqueImpulse;
 119
 120         btRigidBody* m_originalBody;
 121         void setWorldTransform(const btTransform& worldTransform)
 122         {
 123                 m_worldTransform = worldTransform;
 124         }
 125
 126         const btTransform& getWorldTransform() const
 127         {
 128                 return m_worldTransform;
 129         }
 130
 131         SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const
 132         {
 133                 if (m_originalBody)
 134                         velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos);
 135                 else
 136                         velocity.setValue(0, 0, 0);
 137         }
 138
 139         SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 140         {
 141                 if (m_originalBody)
 142                         velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 143                 else
 144                         velocity.setValue(0, 0, 0);
 145         }
 146
 147         SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & angVel) const
 148         {
 149                 if (m_originalBody)
 150                         angVel = m_angularVelocity + m_deltaAngularVelocity;
 151                 else
 152                         angVel.setValue(0, 0, 0);
 153         }
 154
 155         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
 156         SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 157         {
 158                 if (m_originalBody)
 159                 {
 160                         m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
 161                         m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 162                 }
 163         }
 164
 165         SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar impulseMagnitude)
 166         {
 167                 if (m_originalBody)
 168                 {
 169                         m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
 170                         m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 171                 }
 172         }
 173
 174         const btVector3& getDeltaLinearVelocity() const
 175         {
 176                 return m_deltaLinearVelocity;
 177         }
 178
 179         const btVector3& getDeltaAngularVelocity() const
 180         {
 181                 return m_deltaAngularVelocity;
 182         }
 183
 184         const btVector3& getPushVelocity() const
 185         {
 186                 return m_pushVelocity;
 187         }
 188
 189         const btVector3& getTurnVelocity() const
 190         {
 191                 return m_turnVelocity;
 192         }
 193
 194         ////////////////////////////////////////////////
 195         ///some internal methods, don't use them
 196
 197         btVector3& internalGetDeltaLinearVelocity()
 198         {
 199                 return m_deltaLinearVelocity;
 200         }
 201
 202         btVector3& internalGetDeltaAngularVelocity()
 203         {
 204                 return m_deltaAngularVelocity;
 205         }
 206
 207         const btVector3& internalGetAngularFactor() const
 208         {
 209                 return m_angularFactor;
 210         }
 211
 212         const btVector3& internalGetInvMass() const
 213         {
 214                 return m_invMass;
 215         }
 216
 217         void internalSetInvMass(const btVector3& invMass)
 218         {
 219                 m_invMass = invMass;
 220         }
 221
 222         btVector3& internalGetPushVelocity()
 223         {
 224                 return m_pushVelocity;
 225         }
 226
 227         btVector3& internalGetTurnVelocity()
 228         {
 229                 return m_turnVelocity;
 230         }
 231
 232         SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 233         {
 234                 velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 235         }
 236
 237         SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & angVel) const
 238         {
 239                 angVel = m_angularVelocity + m_deltaAngularVelocity;
 240         }
 241
 242         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
 243         SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 244         {
 245                 if (m_originalBody)
 246                 {
 247                         m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
 248                         m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 249                 }
 250         }
 251
 252         void writebackVelocity()
 253         {
 254                 if (m_originalBody)
 255                 {
 256                         m_linearVelocity += m_deltaLinearVelocity;
 257                         m_angularVelocity += m_deltaAngularVelocity;
 258
 259                         //m_originalBody->setCompanionId(-1);
 260                 }
 261         }
 262
 263         void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
 264         {
 265                 (void)timeStep;
 266                 if (m_originalBody)
 267                 {
 268                         m_linearVelocity += m_deltaLinearVelocity;
 269                         m_angularVelocity += m_deltaAngularVelocity;
 270
 271                         //correct the position/orientation based on push/turn recovery
 272                         btTransform newTransform;
 273                         if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
 274                         {
 275                                 //      btQuaternion orn = m_worldTransform.getRotation();
 276                                 btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
 277                                 m_worldTransform = newTransform;
 278                         }
 279                         //m_worldTransform.setRotation(orn);
 280                         //m_originalBody->setCompanionId(-1);
 281                 }
 282         }
 283 };
 284
 285 #endif  //BT_SOLVER_BODY_H