cc8715ef
dmayerich
organization
|
1
|
#include "rts/math/matrix.h"
|
a47a23a9
dmayerich
added ENVI functions
|
2
3
4
5
6
7
8
|
#ifndef RTS_QUATERNION_H
#define RTS_QUATERNION_H
namespace rts{
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
9
|
class quaternion
|
a47a23a9
dmayerich
added ENVI functions
|
10
11
12
13
14
15
16
17
18
|
{
public:
T w;
T x;
T y;
T z;
void normalize();
void CreateRotation(T theta, T axis_x, T axis_y, T axis_z);
|
7006df5f
David Mayerich
reformat of direc...
|
19
20
21
22
|
void CreateRotation(T theta, vector<T, 3> axis);
quaternion<T> operator*(quaternion<T> &rhs);
matrix<T, 3> toMatrix3();
matrix<T, 4> toMatrix4();
|
cc8715ef
dmayerich
organization
|
23
|
|
a47a23a9
dmayerich
added ENVI functions
|
24
|
|
7006df5f
David Mayerich
reformat of direc...
|
25
26
|
quaternion();
quaternion(T w, T x, T y, T z);
|
a47a23a9
dmayerich
added ENVI functions
|
27
28
29
30
|
};
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
31
|
void quaternion<T>::normalize()
|
a47a23a9
dmayerich
added ENVI functions
|
32
33
34
35
36
37
38
39
40
|
{
double length=sqrt(w*w + x*x + y*y + z*z);
w=w/length;
x=x/length;
y=y/length;
z=z/length;
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
41
|
void quaternion<T>::CreateRotation(T theta, T axis_x, T axis_y, T axis_z)
|
a47a23a9
dmayerich
added ENVI functions
|
42
43
|
{
//assign the given Euler rotation to this quaternion
|
7006df5f
David Mayerich
reformat of direc...
|
44
45
46
47
|
w = (T)cos(theta/2.0);
x = axis_x*(T)sin(theta/2.0);
y = axis_y*(T)sin(theta/2.0);
z = axis_z*(T)sin(theta/2.0);
|
a47a23a9
dmayerich
added ENVI functions
|
48
49
50
|
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
51
|
void quaternion<T>::CreateRotation(T theta, vector<T, 3> axis)
|
a47a23a9
dmayerich
added ENVI functions
|
52
53
54
55
56
|
{
CreateRotation(theta, axis[0], axis[1], axis[2]);
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
57
|
quaternion<T> quaternion<T>::operator *(quaternion<T> ¶m)
|
a47a23a9
dmayerich
added ENVI functions
|
58
59
60
61
62
63
|
{
float A, B, C, D, E, F, G, H;
A = (w + x)*(param.w + param.x);
B = (z - y)*(param.y - param.z);
|
cc8715ef
dmayerich
organization
|
64
|
C = (w - x)*(param.y + param.z);
|
a47a23a9
dmayerich
added ENVI functions
|
65
66
67
68
69
70
|
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);
|
7006df5f
David Mayerich
reformat of direc...
|
71
|
quaternion<T> result;
|
a47a23a9
dmayerich
added ENVI functions
|
72
|
result.w = B + (-E - F + G + H) /2;
|
cc8715ef
dmayerich
organization
|
73
74
|
result.x = A - (E + F + G + H)/2;
result.y = C + (E - F + G - H)/2;
|
a47a23a9
dmayerich
added ENVI functions
|
75
76
77
78
79
80
|
result.z = D + (E - F - G + H)/2;
return result;
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
81
|
matrix<T, 3> quaternion<T>::toMatrix3()
|
a47a23a9
dmayerich
added ENVI functions
|
82
|
{
|
7006df5f
David Mayerich
reformat of direc...
|
83
|
matrix<T, 3> result;
|
a47a23a9
dmayerich
added ENVI functions
|
84
85
|
|
7006df5f
David Mayerich
reformat of direc...
|
86
|
T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
a47a23a9
dmayerich
added ENVI functions
|
87
88
89
90
91
92
93
94
95
|
// 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;
|
7006df5f
David Mayerich
reformat of direc...
|
96
|
result(0, 0) = (T)1.0 - (yy + zz);
|
a47a23a9
dmayerich
added ENVI functions
|
97
|
result(0, 1) = xy - wz;
|
a47a23a9
dmayerich
added ENVI functions
|
98
99
|
result(0, 2) = xz + wy;
|
a47a23a9
dmayerich
added ENVI functions
|
100
101
|
result(1, 0) = xy + wz;
|
7006df5f
David Mayerich
reformat of direc...
|
102
|
result(1, 1) = (T)1.0 - (xx + zz);
|
a47a23a9
dmayerich
added ENVI functions
|
103
104
|
result(1, 2) = yz - wx;
|
a47a23a9
dmayerich
added ENVI functions
|
105
106
107
|
result(2, 0) = xz - wy;
result(2, 1) = yz + wx;
|
a47a23a9
dmayerich
added ENVI functions
|
108
|
|
7006df5f
David Mayerich
reformat of direc...
|
109
110
111
112
113
114
115
116
117
118
119
120
|
result(2, 2) = (T)1.0 - (xx + yy);
return result;
}
template<typename T>
matrix<T, 4> quaternion<T>::toMatrix4()
{
matrix<T, 4> result;
T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
a47a23a9
dmayerich
added ENVI functions
|
121
|
|
a47a23a9
dmayerich
added ENVI functions
|
122
|
|
7006df5f
David Mayerich
reformat of direc...
|
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
|
// 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;
result(0, 0) = (T)1.0 - (yy + zz);
result(0, 1) = xy - wz;
result(0, 2) = xz + wy;
result(1, 0) = xy + wz;
result(1, 1) = (T)1.0 - (xx + zz);
result(1, 2) = yz - wx;
result(2, 0) = xz - wy;
result(2, 1) = yz + wx;
|
a47a23a9
dmayerich
added ENVI functions
|
142
|
|
7006df5f
David Mayerich
reformat of direc...
|
143
144
145
|
result(2, 2) = (T)1.0 - (xx + yy);
result(3, 3) = (T)1.0;
|
a47a23a9
dmayerich
added ENVI functions
|
146
147
148
149
150
|
return result;
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
151
|
quaternion<T>::quaternion()
|
a47a23a9
dmayerich
added ENVI functions
|
152
153
154
155
156
|
{
w=0.0; x=0.0; y=0.0; z=0.0;
}
template<typename T>
|
7006df5f
David Mayerich
reformat of direc...
|
157
|
quaternion<T>::quaternion(T c, T i, T j, T k)
|
a47a23a9
dmayerich
added ENVI functions
|
158
159
160
161
|
{
w=c; x=i; y=j; z=k;
}
|
7006df5f
David Mayerich
reformat of direc...
|
162
163
164
165
166
|
} //end rts namespace
#if __GNUC__ > 3 && __GNUC_MINOR__ > 7
template<class T> using rtsQuaternion = rts::quaternion<T>;
#endif
|
a47a23a9
dmayerich
added ENVI functions
|
167
168
|
#endif
|