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