blob: e96f90d3f32d1676e0161d11ee117c48cbb8d1fe (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
#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;
}
}
|