diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h | 53 |
1 files changed, 23 insertions, 30 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h index e96f90d3f3..56d9118f95 100644 --- a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h +++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h @@ -2,11 +2,8 @@ #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" - - -inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) { - if (bodies[nodeID].m_invMass != 0.f) { float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); @@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod bodies[nodeID].m_angVel.x *= angularDamping; bodies[nodeID].m_angVel.y *= angularDamping; bodies[nodeID].m_angVel.z *= angularDamping; - + b3Float4 angvel = bodies[nodeID].m_angVel; float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); - + //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) { fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; } - if(fAngle < 0.001f) + if (fAngle < 0.001f) { // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle); } - + b3Quat dorn; dorn.x = axis.x; dorn.y = axis.y; @@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod b3Quat orn0 = bodies[nodeID].m_quat; b3Quat predictedOrn = b3QuatMul(dorn, orn0); predictedOrn = b3QuatNormalized(predictedOrn); - bodies[nodeID].m_quat=predictedOrn; + bodies[nodeID].m_quat = predictedOrn; } - //linear velocity - bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; - + //linear velocity + bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; + //apply gravity bodies[nodeID].m_linVel += gravityAcceleration * timeStep; - } - } -inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) { float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); - - if( (body->m_invMass != 0.f)) + + if ((body->m_invMass != 0.f)) { //angular velocity { @@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt body->m_angVel.x *= angularDamping; body->m_angVel.y *= angularDamping; body->m_angVel.z *= angularDamping; - + b3Float4 angvel = body->m_angVel; float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); //limit the angular motion - if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) { fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; } - if(fAngle < 0.001f) + if (fAngle < 0.001f) { // use Taylor's expansions of sync function - axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle); } b3Quat dorn; dorn.x = axis.x; @@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt b3Quat predictedOrn = b3QuatMul(dorn, orn0); predictedOrn = b3QuatNormalized(predictedOrn); - body->m_quat=predictedOrn; + body->m_quat = predictedOrn; } //apply gravity body->m_linVel += gravityAcceleration * timeStep; - //linear velocity - body->m_pos += body->m_linVel * timeStep; - + //linear velocity + body->m_pos += body->m_linVel * timeStep; } - } |