dali-physics/third-party/bullet3/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h

   1
   2
   3 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
   4
   5 inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
   6 {
   7         if (bodies[nodeID].m_invMass != 0.f)
   8         {
   9                 float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
  10
  11                 //angular velocity
  12                 {
  13                         b3Float4 axis;
  14                         //add some hardcoded angular damping
  15                         bodies[nodeID].m_angVel.x *= angularDamping;
  16                         bodies[nodeID].m_angVel.y *= angularDamping;
  17                         bodies[nodeID].m_angVel.z *= angularDamping;
  18
  19                         b3Float4 angvel = bodies[nodeID].m_angVel;
  20
  21                         float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
  22
  23                         //limit the angular motion
  24                         if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
  25                         {
  26                                 fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
  27                         }
  28                         if (fAngle < 0.001f)
  29                         {
  30                                 // use Taylor's expansions of sync function
  31                                 axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
  32                         }
  33                         else
  34                         {
  35                                 // sync(fAngle) = sin(c*fAngle)/t
  36                                 axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
  37                         }
  38
  39                         b3Quat dorn;
  40                         dorn.x = axis.x;
  41                         dorn.y = axis.y;
  42                         dorn.z = axis.z;
  43                         dorn.w = b3Cos(fAngle * timeStep * 0.5f);
  44                         b3Quat orn0 = bodies[nodeID].m_quat;
  45                         b3Quat predictedOrn = b3QuatMul(dorn, orn0);
  46                         predictedOrn = b3QuatNormalized(predictedOrn);
  47                         bodies[nodeID].m_quat = predictedOrn;
  48                 }
  49                 //linear velocity
  50                 bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
  51
  52                 //apply gravity
  53                 bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
  54         }
  55 }
  56
  57 inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
  58 {
  59         float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
  60
  61         if ((body->m_invMass != 0.f))
  62         {
  63                 //angular velocity
  64                 {
  65                         b3Float4 axis;
  66                         //add some hardcoded angular damping
  67                         body->m_angVel.x *= angularDamping;
  68                         body->m_angVel.y *= angularDamping;
  69                         body->m_angVel.z *= angularDamping;
  70
  71                         b3Float4 angvel = body->m_angVel;
  72                         float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
  73                         //limit the angular motion
  74                         if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
  75                         {
  76                                 fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
  77                         }
  78                         if (fAngle < 0.001f)
  79                         {
  80                                 // use Taylor's expansions of sync function
  81                                 axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
  82                         }
  83                         else
  84                         {
  85                                 // sync(fAngle) = sin(c*fAngle)/t
  86                                 axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
  87                         }
  88                         b3Quat dorn;
  89                         dorn.x = axis.x;
  90                         dorn.y = axis.y;
  91                         dorn.z = axis.z;
  92                         dorn.w = b3Cos(fAngle * timeStep * 0.5f);
  93                         b3Quat orn0 = body->m_quat;
  94
  95                         b3Quat predictedOrn = b3QuatMul(dorn, orn0);
  96                         predictedOrn = b3QuatNormalized(predictedOrn);
  97                         body->m_quat = predictedOrn;
  98                 }
  99
 100                 //apply gravity
 101                 body->m_linVel += gravityAcceleration * timeStep;
 102
 103                 //linear velocity
 104                 body->m_pos += body->m_linVel * timeStep;
 105         }
 106 }