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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
//
// quaternion.h
// Game
//
// Created by user on 9/8/16.
// Copyright © 2016 user. All rights reserved.
//
#ifndef quaternion_h
#define quaternion_h
typedef struct {
float i,j,k,r;
} Quaternion;
typedef struct {
float x,y,z,theta;
} AxisAngle;
typedef struct {
Quaternion rot; // orientation
Quaternion pos; // positon
} QuaternionBone;
#define printq2(name,quat) printf("%s i %f j %f k %f r %f\n", name, quat.i, quat.j, quat.k, quat.r)
#define QuaternionNormalize(quat) { \
float inv_len= 1.0f / sqrt( quat.i*quat.i + quat.j*quat.j + quat.k*quat.k + quat.r*quat.r ); \
quat.i *= inv_len; \
quat.j *= inv_len; \
quat.k *= inv_len; }
#define QuaternionSet(res,I,J,K,R) { res.i=I; res.j=J; res.k=K; res.r=R; }
#define AxisAngleOfQuaternion(res,q) { \
float len = sqrt(q.i*q.i + q.j*q.j + q.k*q.k); \
if (len > 0.0001) { \
float inv_len = 1.0f / len; \
res.x = q.i * inv_len; \
res.y = q.j * inv_len; \
res.z = q.k * inv_len; \
} else { \
res.x=1.0f; res.y=0.0f; res.z=0.0f; \
} \
res.theta = 2.0 * acos( q.r ); }
#define QuaternionOfAxisAngle(res,aa) QuaternionSetAxisAngle(res, (aa).x, (aa).y, (aa).z, (aa).theta)
#define QuaternionSetAxisAngle(res,x,y,z,theta) { \
float inv_lenXYZ = 1.0f / sqrt( (x)*(x) + (y)*(y) + (z)*(z) ); \
float cos_half_theta = cos( 0.5f*(theta) ); \
float sin_half_theta = sin( 0.5f*(theta) ); \
res.i = (x) * sin_half_theta * inv_lenXYZ; \
res.j = (y) * sin_half_theta * inv_lenXYZ; \
res.k = (z) * sin_half_theta * inv_lenXYZ; \
res.r = cos_half_theta; }
#define QuaternionIdentity(q) { q.i=0.0f; q.j=0.0f; q.k=0.0f; q.r=1.0f; }
#define QuaternionInv(res,a) { \
res.i = -(a).i; \
res.j = -(a).j; \
res.k = -(a).k; \
res.r = (a).r; }
#define QuaternionMult(res,a,b) { \
res.i = (a).i*(b).r + (a).j*(b).k - (a).k*(b).j + (a).r*(b).i; \
res.j = -(a).i*(b).k + (a).j*(b).r + (a).k*(b).i + (a).r*(b).j; \
res.k = (a).i*(b).j - (a).j*(b).i + (a).k*(b).r + (a).r*(b).k; \
res.r = -(a).i*(b).i - (a).j*(b).j - (a).k*(b).k + (a).r*(b).r; }
#define QuaternionSub(res,a,b) { \
res.i = (a).i - (b).i; \
res.j = (a).j - (b).j; \
res.k = (a).k - (b).k; \
res.r = (a).r - (b).r; }
#define QuaternionAdd(res,a,b) { \
res.i = (a).i + (b).i; \
res.j = (a).j + (b).j; \
res.k = (a).k + (b).k; \
res.r = (a).r + (b).r; }
#define QuaternionRot(res,q,p) { \
Quaternion q_inv,left; \
QuaternionInv(q_inv,q); \
QuaternionMult(left,q,p); \
QuaternionMult(res,left,q_inv); }
#define QuaternionSlerp(res,delta,u,v) { \
Quaternion rot,u_inv; \
float theta,inv_s,scale; \
QuaternionInv(u_inv,u); \
QuaternionMult(rot,v,u_inv); \
if (rot.r < 0.998 && rot.r > -0.998) { \
theta = acos(rot.r); \
inv_s = 1.0f / sin(theta); \
theta *= delta; \
scale = inv_s * sin(theta); \
rot.i *= scale; \
rot.j *= scale; \
rot.k *= scale; \
rot.r = cos(theta); \
QuaternionMult(res,rot,u); \
} else { \
res = u; \
} }
/*
#define QuaternionSlerp(res,delta,u,v) { \
Quaternion rot,u_inv; \
float theta,inv_s,scale; \
QuaternionInv(u_inv,u); \
QuaternionMult(rot,v,u_inv); \
if (rot.r < 0.998) { \
printq2("slerp v",v); \
theta = acos(rot.r); \
inv_s = 1.0f / sin(theta); \
printf("slerp theta %f inv_s %f\n", theta, inv_s); \
theta *= delta; \
scale = inv_s * sin(theta); \
rot.i *= scale; \
rot.j *= scale; \
rot.k *= scale; \
rot.r = cos(theta); \
QuaternionMult(res,rot,u); \
} else { \
res = u; \
} \
printq2("slerp res", res); }
*/
#define QuaternionBoneTransform(out,in,bone) { \
Quaternion subtracted,rotated; \
QuaternionSub(subtracted,in,bone.pos); \
QuaternionRot(rotated,bone.rot,subtracted); \
QuaternionAdd(out,rotated,bone.pos); }
#endif /* quaternion_h */
|
