57729e5b
David Mayerich
changed directory...
|
1
2
3
|
#ifndef RTS_QUATERNION_H
#define RTS_QUATERNION_H
|
6c4afcac
David Mayerich
introduced a gene...
|
4
|
#include <stim/math/matrix_sq.h>
|
7d1d153a
Pavel Govyadinov
fixed the include...
|
5
|
#include <stim/cuda/cudatools/callable.h>
|
d609550e
David Mayerich
fixed bug in plan...
|
6
|
|
8a86bd56
David Mayerich
changed rts names...
|
7
|
namespace stim{
|
57729e5b
David Mayerich
changed directory...
|
8
9
10
11
12
13
14
15
16
17
|
template<typename T>
class quaternion
{
public:
T w;
T x;
T y;
T z;
|
d609550e
David Mayerich
fixed bug in plan...
|
18
|
CUDA_CALLABLE void normalize(){
|
57729e5b
David Mayerich
changed directory...
|
19
|
|
d609550e
David Mayerich
fixed bug in plan...
|
20
21
22
23
24
25
|
double length=sqrt(w*w + x*x + y*y + z*z);
w=w/length;
x=x/length;
y=y/length;
z=z/length;
}
|
57729e5b
David Mayerich
changed directory...
|
26
|
|
d609550e
David Mayerich
fixed bug in plan...
|
27
|
CUDA_CALLABLE void CreateRotation(T theta, T ux, T uy, T uz){
|
57729e5b
David Mayerich
changed directory...
|
28
|
|
8e4f8364
David Mayerich
started a new opt...
|
29
|
vec3<T> u(ux, uy, uz);
|
d609550e
David Mayerich
fixed bug in plan...
|
30
31
|
CreateRotation(theta, u);
}
|
57729e5b
David Mayerich
changed directory...
|
32
|
|
8e4f8364
David Mayerich
started a new opt...
|
33
|
CUDA_CALLABLE void CreateRotation(T theta, vec3<T> u){
|
57729e5b
David Mayerich
changed directory...
|
34
|
|
8e4f8364
David Mayerich
started a new opt...
|
35
|
vec3<T> u_hat = u.norm();
|
8d4f0940
David Mayerich
ERROR in plane wa...
|
36
|
|
d609550e
David Mayerich
fixed bug in plan...
|
37
38
39
40
41
42
|
//assign the given Euler rotation to this quaternion
w = (T)cos(theta/2);
x = u_hat[0]*(T)sin(theta/2);
y = u_hat[1]*(T)sin(theta/2);
z = u_hat[2]*(T)sin(theta/2);
}
|
57729e5b
David Mayerich
changed directory...
|
43
|
|
8e4f8364
David Mayerich
started a new opt...
|
44
|
CUDA_CALLABLE void CreateRotation(vec3<T> from, vec3<T> to){
|
8d4f0940
David Mayerich
ERROR in plane wa...
|
45
|
|
9339fbad
David Mayerich
implementing mie ...
|
46
47
|
from = from.norm();
to = to.norm();
|
8e4f8364
David Mayerich
started a new opt...
|
48
|
vec3<T> r = from.cross(to); //compute the rotation vector
|
d609550e
David Mayerich
fixed bug in plan...
|
49
50
|
T theta = asin(r.len()); //compute the angle of the rotation about r
//deal with a zero vector (both k and kn point in the same direction)
|
591f04ec
Pavel Govyadinov
fixed a very subt...
|
51
|
if(theta == (T)0){
|
d609550e
David Mayerich
fixed bug in plan...
|
52
|
return;
|
591f04ec
Pavel Govyadinov
fixed a very subt...
|
53
|
}
|
57729e5b
David Mayerich
changed directory...
|
54
|
|
d609550e
David Mayerich
fixed bug in plan...
|
55
56
57
|
//create a quaternion to capture the rotation
CreateRotation(theta, r.norm());
}
|
57729e5b
David Mayerich
changed directory...
|
58
59
|
|
57729e5b
David Mayerich
changed directory...
|
60
|
|
d609550e
David Mayerich
fixed bug in plan...
|
61
|
CUDA_CALLABLE quaternion<T> operator *(quaternion<T> ¶m){
|
57729e5b
David Mayerich
changed directory...
|
62
|
|
d609550e
David Mayerich
fixed bug in plan...
|
63
|
float A, B, C, D, E, F, G, H;
|
57729e5b
David Mayerich
changed directory...
|
64
|
|
57729e5b
David Mayerich
changed directory...
|
65
|
|
d609550e
David Mayerich
fixed bug in plan...
|
66
67
68
69
70
71
72
73
|
A = (w + x)*(param.w + param.x);
B = (z - y)*(param.y - param.z);
C = (w - x)*(param.y + param.z);
D = (y + z)*(param.w - param.x);
E = (x + z)*(param.x + param.y);
F = (x - z)*(param.x - param.y);
G = (w + y)*(param.w - param.z);
H = (w - y)*(param.w + param.z);
|
57729e5b
David Mayerich
changed directory...
|
74
|
|
d609550e
David Mayerich
fixed bug in plan...
|
75
76
77
78
79
|
quaternion<T> result;
result.w = B + (-E - F + G + H) /2;
result.x = A - (E + F + G + H)/2;
result.y = C + (E - F + G - H)/2;
result.z = D + (E - F - G + H)/2;
|
57729e5b
David Mayerich
changed directory...
|
80
|
|
d609550e
David Mayerich
fixed bug in plan...
|
81
82
83
|
return result;
}
|
6c4afcac
David Mayerich
introduced a gene...
|
84
|
CUDA_CALLABLE matrix_sq<T, 3> toMatrix3(){
|
57729e5b
David Mayerich
changed directory...
|
85
|
|
6c4afcac
David Mayerich
introduced a gene...
|
86
|
matrix_sq<T, 3> result;
|
57729e5b
David Mayerich
changed directory...
|
87
|
|
57729e5b
David Mayerich
changed directory...
|
88
|
|
d609550e
David Mayerich
fixed bug in plan...
|
89
|
T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
57729e5b
David Mayerich
changed directory...
|
90
|
|
57729e5b
David Mayerich
changed directory...
|
91
|
|
d609550e
David Mayerich
fixed bug in plan...
|
92
93
94
95
96
97
|
// calculate coefficients
x2 = x + x; y2 = y + y;
z2 = z + z;
xx = x * x2; xy = x * y2; xz = x * z2;
yy = y * y2; yz = y * z2; zz = z * z2;
wx = w * x2; wy = w * y2; wz = w * z2;
|
57729e5b
David Mayerich
changed directory...
|
98
|
|
d609550e
David Mayerich
fixed bug in plan...
|
99
100
|
result(0, 0) = 1 - (yy + zz);
result(0, 1) = xy - wz;
|
57729e5b
David Mayerich
changed directory...
|
101
|
|
d609550e
David Mayerich
fixed bug in plan...
|
102
|
result(0, 2) = xz + wy;
|
57729e5b
David Mayerich
changed directory...
|
103
|
|
d609550e
David Mayerich
fixed bug in plan...
|
104
105
|
result(1, 0) = xy + wz;
result(1, 1) = 1 - (xx + zz);
|
57729e5b
David Mayerich
changed directory...
|
106
|
|
d609550e
David Mayerich
fixed bug in plan...
|
107
|
result(1, 2) = yz - wx;
|
57729e5b
David Mayerich
changed directory...
|
108
|
|
d609550e
David Mayerich
fixed bug in plan...
|
109
110
|
result(2, 0) = xz - wy;
result(2, 1) = yz + wx;
|
57729e5b
David Mayerich
changed directory...
|
111
|
|
d609550e
David Mayerich
fixed bug in plan...
|
112
|
result(2, 2) = 1 - (xx + yy);
|
57729e5b
David Mayerich
changed directory...
|
113
|
|
d609550e
David Mayerich
fixed bug in plan...
|
114
115
|
return result;
}
|
57729e5b
David Mayerich
changed directory...
|
116
|
|
6c4afcac
David Mayerich
introduced a gene...
|
117
|
CUDA_CALLABLE matrix_sq<T, 4> toMatrix4(){
|
57729e5b
David Mayerich
changed directory...
|
118
|
|
6c4afcac
David Mayerich
introduced a gene...
|
119
|
matrix_sq<T, 4> result;
|
d609550e
David Mayerich
fixed bug in plan...
|
120
|
T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
57729e5b
David Mayerich
changed directory...
|
121
|
|
d609550e
David Mayerich
fixed bug in plan...
|
122
123
124
125
126
127
|
// calculate coefficients
x2 = x + x; y2 = y + y;
z2 = z + z;
xx = x * x2; xy = x * y2; xz = x * z2;
yy = y * y2; yz = y * z2; zz = z * z2;
wx = w * x2; wy = w * y2; wz = w * z2;
|
57729e5b
David Mayerich
changed directory...
|
128
|
|
d609550e
David Mayerich
fixed bug in plan...
|
129
130
|
result(0, 0) = 1 - (yy + zz);
result(0, 1) = xy - wz;
|
57729e5b
David Mayerich
changed directory...
|
131
|
|
d609550e
David Mayerich
fixed bug in plan...
|
132
|
result(0, 2) = xz + wy;
|
57729e5b
David Mayerich
changed directory...
|
133
|
|
d609550e
David Mayerich
fixed bug in plan...
|
134
135
|
result(1, 0) = xy + wz;
result(1, 1) = 1 - (xx + zz);
|
57729e5b
David Mayerich
changed directory...
|
136
|
|
d609550e
David Mayerich
fixed bug in plan...
|
137
|
result(1, 2) = yz - wx;
|
57729e5b
David Mayerich
changed directory...
|
138
|
|
d609550e
David Mayerich
fixed bug in plan...
|
139
140
|
result(2, 0) = xz - wy;
result(2, 1) = yz + wx;
|
57729e5b
David Mayerich
changed directory...
|
141
|
|
d609550e
David Mayerich
fixed bug in plan...
|
142
|
result(2, 2) = 1 - (xx + yy);
|
57729e5b
David Mayerich
changed directory...
|
143
|
|
d609550e
David Mayerich
fixed bug in plan...
|
144
|
result(3, 3) = 1;
|
57729e5b
David Mayerich
changed directory...
|
145
|
|
d609550e
David Mayerich
fixed bug in plan...
|
146
147
148
149
150
151
152
153
154
155
156
157
158
|
return result;
}
CUDA_CALLABLE quaternion(){
w=0; x=0; y=0; z=0;
}
CUDA_CALLABLE quaternion(T c, T i, T j, T k){
w=c; x=i; y=j; z=k;
}
};
|
57729e5b
David Mayerich
changed directory...
|
159
160
161
|
} //end rts namespace
|
57729e5b
David Mayerich
changed directory...
|
162
163
|
#endif
|