dali-physics/third-party/bullet3/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h

   1 /*
   2 Copyright (c) 2013 Advanced Micro Devices, Inc.
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 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.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14 //Originally written by Erwin Coumans
  15
  16 #ifndef B3_GPU_SOLVER_BODY_H
  17 #define B3_GPU_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 ///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
  31 B3_ATTRIBUTE_ALIGNED16(struct)
  32 b3GpuSolverBody
  33 {
  34         B3_DECLARE_ALIGNED_ALLOCATOR();
  35         //      b3Transform             m_worldTransformUnused;
  36         b3Vector3 m_deltaLinearVelocity;
  37         b3Vector3 m_deltaAngularVelocity;
  38         b3Vector3 m_angularFactor;
  39         b3Vector3 m_linearFactor;
  40         b3Vector3 m_invMass;
  41         b3Vector3 m_pushVelocity;
  42         b3Vector3 m_turnVelocity;
  43         b3Vector3 m_linearVelocity;
  44         b3Vector3 m_angularVelocity;
  45
  46         union {
  47                 void* m_originalBody;
  48                 int m_originalBodyIndex;
  49         };
  50
  51         int padding[3];
  52
  53         /*
  54         void    setWorldTransform(const b3Transform& worldTransform)
  55         {
  56                 m_worldTransform = worldTransform;
  57         }
  58
  59         const b3Transform& getWorldTransform() const
  60         {
  61                 return m_worldTransform;
  62         }
  63         */
  64         B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
  65         {
  66                 if (m_originalBody)
  67                         velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
  68                 else
  69                         velocity.setValue(0, 0, 0);
  70         }
  71
  72         B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
  73         {
  74                 if (m_originalBody)
  75                         angVel = m_angularVelocity + m_deltaAngularVelocity;
  76                 else
  77                         angVel.setValue(0, 0, 0);
  78         }
  79
  80         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
  81         B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
  82         {
  83                 if (m_originalBody)
  84                 {
  85                         m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
  86                         m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
  87                 }
  88         }
  89
  90         B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
  91         {
  92                 if (m_originalBody)
  93                 {
  94                         m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
  95                         m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
  96                 }
  97         }
  98
  99         const b3Vector3& getDeltaLinearVelocity() const
 100         {
 101                 return m_deltaLinearVelocity;
 102         }
 103
 104         const b3Vector3& getDeltaAngularVelocity() const
 105         {
 106                 return m_deltaAngularVelocity;
 107         }
 108
 109         const b3Vector3& getPushVelocity() const
 110         {
 111                 return m_pushVelocity;
 112         }
 113
 114         const b3Vector3& getTurnVelocity() const
 115         {
 116                 return m_turnVelocity;
 117         }
 118
 119         ////////////////////////////////////////////////
 120         ///some internal methods, don't use them
 121
 122         b3Vector3& internalGetDeltaLinearVelocity()
 123         {
 124                 return m_deltaLinearVelocity;
 125         }
 126
 127         b3Vector3& internalGetDeltaAngularVelocity()
 128         {
 129                 return m_deltaAngularVelocity;
 130         }
 131
 132         const b3Vector3& internalGetAngularFactor() const
 133         {
 134                 return m_angularFactor;
 135         }
 136
 137         const b3Vector3& internalGetInvMass() const
 138         {
 139                 return m_invMass;
 140         }
 141
 142         void internalSetInvMass(const b3Vector3& invMass)
 143         {
 144                 m_invMass = invMass;
 145         }
 146
 147         b3Vector3& internalGetPushVelocity()
 148         {
 149                 return m_pushVelocity;
 150         }
 151
 152         b3Vector3& internalGetTurnVelocity()
 153         {
 154                 return m_turnVelocity;
 155         }
 156
 157         B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
 158         {
 159                 velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 160         }
 161
 162         B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
 163         {
 164                 angVel = m_angularVelocity + m_deltaAngularVelocity;
 165         }
 166
 167         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
 168         B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
 169         {
 170                 //if (m_originalBody)
 171                 {
 172                         m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
 173                         m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 174                 }
 175         }
 176
 177         void writebackVelocity()
 178         {
 179                 //if (m_originalBody>=0)
 180                 {
 181                         m_linearVelocity += m_deltaLinearVelocity;
 182                         m_angularVelocity += m_deltaAngularVelocity;
 183
 184                         //m_originalBody->setCompanionId(-1);
 185                 }
 186         }
 187
 188         void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
 189         {
 190                 (void)timeStep;
 191                 if (m_originalBody)
 192                 {
 193                         m_linearVelocity += m_deltaLinearVelocity;
 194                         m_angularVelocity += m_deltaAngularVelocity;
 195
 196                         //correct the position/orientation based on push/turn recovery
 197                         b3Transform newTransform;
 198                         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)
 199                         {
 200                                 //      b3Quaternion orn = m_worldTransform.getRotation();
 201                                 //                              b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
 202                                 //                              m_worldTransform = newTransform;
 203                         }
 204                         //m_worldTransform.setRotation(orn);
 205                         //m_originalBody->setCompanionId(-1);
 206                 }
 207         }
 208 };
 209
 210 #endif  //B3_SOLVER_BODY_H