114 lines
3.0 KiB
C++
114 lines
3.0 KiB
C++
|
|
||
|
|
|
||
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
//angular velocity
|
||
|
|
{
|
||
|
|
b3Float4 axis;
|
||
|
|
//add some hardcoded angular damping
|
||
|
|
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)
|
||
|
|
{
|
||
|
|
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
|
||
|
|
}
|
||
|
|
if(fAngle < 0.001f)
|
||
|
|
{
|
||
|
|
// use Taylor's expansions of sync function
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
|
||
|
|
b3Quat dorn;
|
||
|
|
dorn.x = axis.x;
|
||
|
|
dorn.y = axis.y;
|
||
|
|
dorn.z = axis.z;
|
||
|
|
dorn.w = b3Cos(fAngle * timeStep * 0.5f);
|
||
|
|
b3Quat orn0 = bodies[nodeID].m_quat;
|
||
|
|
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
|
||
|
|
predictedOrn = b3QuatNormalized(predictedOrn);
|
||
|
|
bodies[nodeID].m_quat=predictedOrn;
|
||
|
|
}
|
||
|
|
//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)
|
||
|
|
{
|
||
|
|
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
|
||
|
|
|
||
|
|
if( (body->m_invMass != 0.f))
|
||
|
|
{
|
||
|
|
//angular velocity
|
||
|
|
{
|
||
|
|
b3Float4 axis;
|
||
|
|
//add some hardcoded angular damping
|
||
|
|
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)
|
||
|
|
{
|
||
|
|
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
|
||
|
|
}
|
||
|
|
if(fAngle < 0.001f)
|
||
|
|
{
|
||
|
|
// use Taylor's expansions of sync function
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
b3Quat dorn;
|
||
|
|
dorn.x = axis.x;
|
||
|
|
dorn.y = axis.y;
|
||
|
|
dorn.z = axis.z;
|
||
|
|
dorn.w = b3Cos(fAngle * timeStep * 0.5f);
|
||
|
|
b3Quat orn0 = body->m_quat;
|
||
|
|
|
||
|
|
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
|
||
|
|
predictedOrn = b3QuatNormalized(predictedOrn);
|
||
|
|
body->m_quat=predictedOrn;
|
||
|
|
}
|
||
|
|
|
||
|
|
//apply gravity
|
||
|
|
body->m_linVel += gravityAcceleration * timeStep;
|
||
|
|
|
||
|
|
//linear velocity
|
||
|
|
body->m_pos += body->m_linVel * timeStep;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|