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
|
#ifndef B3_QUAT_H
#define B3_QUAT_H
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
#include "Bullet3Common/shared/b3Float4.h"
#ifdef __cplusplus
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#define b3Quat b3Quaternion
#define b3QuatConstArg const b3Quaternion&
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
{
return orn.inverse();
}
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
b3Transform tr;
tr.setOrigin(translation);
tr.setRotation(orientation);
return tr(point);
}
#else
typedef float4 b3Quat;
#define b3QuatConstArg const b3Quat
inline float4 b3FastNormalize4(float4 v)
{
v = (float4)(v.xyz,0.f);
return fast_normalize(v);
}
inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
inline b3Quat b3QuatNormalized(b3QuatConstArg in);
inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
inline b3Quat b3QuatInvert(b3QuatConstArg q);
inline b3Quat b3QuatInverse(b3QuatConstArg q);
inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
{
b3Quat ans;
ans = b3Cross3( a, b );
ans += a.w*b+b.w*a;
// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
ans.w = a.w*b.w - b3Dot3F4(a, b);
return ans;
}
inline b3Quat b3QuatNormalized(b3QuatConstArg in)
{
b3Quat q;
q=in;
//return b3FastNormalize4(in);
float len = native_sqrt(dot(q, q));
if(len > 0.f)
{
q *= 1.f / len;
}
else
{
q.x = q.y = q.z = 0.f;
q.w = 1.f;
}
return q;
}
inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
b3Quat qInv = b3QuatInvert( q );
float4 vcpy = vec;
vcpy.w = 0.f;
float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);
return out;
}
inline b3Quat b3QuatInverse(b3QuatConstArg q)
{
return (b3Quat)(-q.xyz, q.w);
}
inline b3Quat b3QuatInvert(b3QuatConstArg q)
{
return (b3Quat)(-q.xyz, q.w);
}
inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
return b3QuatRotate( b3QuatInvert( q ), vec );
}
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
return b3QuatRotate( orientation, point ) + (translation);
}
#endif
#endif //B3_QUAT_H
|