cgv
Loading...
Searching...
No Matches
ftransform.h
1#pragma once
2
3#include "fmat.h"
4#include "quaternion.h"
5
6namespace cgv {
7 namespace math {
9 template <typename T> fmat<T, 4, 4>
10 zero4() { fmat<T, 4, 4> M; M.zeros(); return M; }
12 template <typename T> fmat<T, 2, 2>
13 identity2() { fmat<T, 2, 2> M; M.identity(); return M; }
15 template <typename T> fmat<T, 3, 3>
16 identity3() { fmat<T, 3, 3> M; M.identity(); return M; }
18 template <typename T> fmat<T, 4, 4>
19 identity4() { fmat<T, 4, 4> M; M.identity(); return M; }
21 template <typename T> fmat<T, 3, 3>
22 translate2h(const fvec<T, 2>& t) { fmat<T, 3, 3> M; M.identity(); M(0, 2) = t(0); M(1, 2) = t(1); return M; }
24 template <typename T> fmat<T, 3, 3>
25 translate2h(const T& tx, const T& ty) { return translate2h(fvec<T, 2>(tx, ty)); }
27 template <typename T> fmat<T, 4, 4>
28 translate4(const fvec<T, 3>& t) { fmat<T, 4, 4> M; M.identity(); M(0, 3) = t(0); M(1, 3) = t(1); M(2, 3) = t(2); return M; }
30 template <typename T> fmat<T, 4, 4>
31 translate4(const T& tx, const T& ty, const T& tz) { return translate4(fvec<T, 3>(tx, ty, tz)); }
33 template <typename T> fmat<T, 3, 3>
34 scale2h(const fvec<T, 2>& s) { fmat<T, 3, 3> M; M.identity(); M(0, 0) = s(0); M(1, 1) = s(1); return M; }
36 template <typename T> fmat<T, 3, 3>
37 scale2h(const T& sx, const T& sy) { return scale2h(fvec<T, 2>(sx, sy)); }
39 template <typename T> fmat<T, 2, 2>
40 scale2(const fvec<T, 2>& s) { fmat<T, 2, 2> M; M.identity(); M(0, 0) = s(0); M(1, 1) = s(1); return M; }
42 template <typename T> fmat<T, 2, 2>
43 scale2(const T& sx, const T& sy) { return scale2(fvec<T, 2>(sx, sy)); }
45 template <typename T> fmat<T, 3, 3>
46 scale3(const fvec<T, 3>& s) { fmat<T, 3, 3> M; M.identity(); M(0, 0) = s(0); M(1, 1) = s(1); M(2, 2) = s(2); return M; }
48 template <typename T> fmat<T, 3, 3>
49 scale3(const T& sx, const T& sy, const T& sz) { return scale3(fvec<T, 3>(sx, sy, sz)); }
51 template <typename T> fmat<T, 4, 4>
52 scale4(const fvec<T, 3>& s) { fmat<T, 4, 4> M; M.identity(); M(0, 0) = s(0); M(1, 1) = s(1); M(2, 2) = s(2); return M; }
54 template <typename T> fmat<T, 4, 4>
55 scale4(const T& sx, const T& sy, const T& sz) { return scale4(fvec<T, 3>(sx, sy, sz)); }
57 template <typename T> fmat<T, 4, 4>
58 scale4(const T& s) { return scale4(fvec<T, 3>(s, s, s)); }
60 template <typename T> fmat<T, 2, 2>
61 rotate2(const T& A) {
62 fmat<T, 2, 2> M;
63 T angle = T(0.01745329252)*A;
64 T c = cos(angle);
65 T s = sin(angle);
66 M(0, 0) = c;
67 M(0, 1) = -s;
68 M(1, 0) = s;
69 M(1, 1) = c;
70 return M;
71 }
73 template <typename T> fmat<T, 3, 3>
74 rotate2h(const T& A) {
75 fmat<T, 3, 3> M;
76 M.identity();
77 T angle = T(0.01745329252)*A;
78 T c = cos(angle);
79 T s = sin(angle);
80 M(0, 0) = c;
81 M(0, 1) = -s;
82 M(1, 0) = s;
83 M(1, 1) = c;
84 return M;
85 }
87 template <typename T> fmat<T, 3, 3>
88 rotate3(const T& A, const fvec<T, 3>& a) {
89 fmat<T, 3, 3> M;
90 T angle = T(0.01745329252)*A;
91 T c = cos(angle);
92 T s = sin(angle);
93 M(0, 0) = c + a(0) * a(0)*(T(1) - c);
94 M(0, 1) = -a(2) * s + a(1) * a(0)*(T(1) - c);
95 M(0, 2) = a(1) * s + a(2) * a(0)*(T(1) - c);
96 M(1, 0) = a(2) * s + a(0) * a(1)*(T(1) - c);
97 M(1, 1) = c + a(1) * a(1)*(T(1) - c);
98 M(1, 2) = -a(0) * s + a(2) * a(1)*(T(1) - c);
99 M(2, 0) = -a(1) * s + a(0) * a(2)*(T(1) - c);
100 M(2, 1) = a(0) * s + a(1) * a(2)*(T(1) - c);
101 M(2, 2) = c + a(2) * a(2)*(T(1) - c);
102 return M;
103 }
105 template <typename T> fmat<T, 3, 3>
106 rotate3(const fvec<T, 3>& A) {
107 fmat<T, 3, 3> M;
108 fvec<T, 3> angles = cgv::math::fvec<T, 3>(T(0.01745329252))*A;
109 // cos and sin for yaw, pitch and roll in radians
110 T cy = cos(angles[0]);
111 T sy = sin(angles[0]);
112 T cp = cos(angles[1]);
113 T sp = sin(angles[1]);
114 T cr = cos(angles[2]);
115 T sr = sin(angles[2]);
116 // construct matrix of form rotY(yaw) * rotX(pitch) * rotZ(roll)
117 M(0, 0) = sy * sp * sr + cy * cr;
118 M(0, 1) = sy * sp * cr - cy * sr;
119 M(0, 2) = sy * cp;
120 M(1, 0) = cp * sr;
121 M(1, 1) = cp * cr;
122 M(1, 2) = -sp;
123 M(2, 0) = cy * sp * sr - sy * cr;
124 M(2, 1) = cy * sp * cr + sy * sr;
125 M(2, 2) = cy * cp;
126 return M;
127 }
129 template <typename T> fmat<T, 3, 3>
130 rotate3_rpy(const fvec<T, 3>& A)
131 {
132 fmat<T, 3, 3> M;
133 fvec<T, 3> angles = cgv::math::fvec<T, 3>(T(0.01745329252)) * A;
134 T cx = cos(angles[0]);
135 T sx = sin(angles[0]);
136 T cy = cos(angles[1]);
137 T sy = sin(angles[1]);
138 T cz = cos(angles[2]);
139 T sz = sin(angles[2]);
140 M(0, 0) = cy * cz;
141 M(0, 1) = sx * sy * cz - cx * sz;
142 M(0, 2) = cx * sy * cz + sx * sz;
143 M(1, 0) = cy * sz;
144 M(1, 1) = sx * sy * sz + cx * cz;
145 M(1, 2) = cx * sy * sz - sx * cz;
146 M(2, 0) = -sy;
147 M(2, 1) = sx * cy;
148 M(2, 2) = cx * cy;
149 return M;
150 }
152 template <typename T> fmat<T, 3, 3>
153 rotate3s(fvec<T, 3> k) {
154 T o = k.length();
155 k /= std::max(o, T(1e-8f));
156 fmat<T,3,3> K = { 0.0f, k.z(), -k.y(), -k.z(), 0.0f ,k.x(), k.y(),-k.x(), 0.0f };
157 return identity3<float>() + sin(o) * K + (T(1) - cos(o)) * K * K;
158 }
160 template <typename T> fmat<T, 4, 4>
161 rotate4(const T& A, const fvec<T, 3>& a) {
162 fmat<T, 4, 4> M;
163 M.identity();
164 T angle = T(0.01745329252)*A;
165 T c = cos(angle);
166 T s = sin(angle);
167 M(0, 0) = c + a(0) * a(0)*(T(1) - c);
168 M(0, 1) = -a(2) * s + a(1) * a(0)*(T(1) - c);
169 M(0, 2) = a(1) * s + a(2) * a(0)*(T(1) - c);
170 M(1, 0) = a(2) * s + a(0) * a(1)*(T(1) - c);
171 M(1, 1) = c + a(1) * a(1)*(T(1) - c);
172 M(1, 2) = -a(0) * s + a(2) * a(1)*(T(1) - c);
173 M(2, 0) = -a(1) * s + a(0) * a(2)*(T(1) - c);
174 M(2, 1) = a(0) * s + a(1) * a(2)*(T(1) - c);
175 M(2, 2) = c + a(2) * a(2)*(T(1) - c);
176 return M;
177 }
179 template <typename T> fmat<T, 4, 4>
180 rotate4(const T& A, const T& ax, const T& ay, const T& az) { return rotate4(A, fvec<T, 3>(ax, ay, az)); }
181
183 template <typename T> fmat<T, 4, 4>
184 rotate4(const fvec<T, 3>& A) {
185 fmat<T, 4, 4> M;
186 M.identity();
187 fvec<T, 3> angles = cgv::math::fvec<T, 3>(T(0.01745329252))*A;
188 // cos and sin for yaw, pitch and roll in radians
189 T cy = cos(angles[0]);
190 T sy = sin(angles[0]);
191 T cp = cos(angles[1]);
192 T sp = sin(angles[1]);
193 T cr = cos(angles[2]);
194 T sr = sin(angles[2]);
195 // construct matrix of form rotY(yaw) * rotX(pitch) * rotZ(roll)
196 M(0, 0) = sy * sp * sr + cy * cr;
197 M(0, 1) = sy * sp * cr - cy * sr;
198 M(0, 2) = sy * cp;
199 M(1, 0) = cp * sr;
200 M(1, 1) = cp * cr;
201 M(1, 2) = -sp;
202 M(2, 0) = cy * sp * sr - sy * cr;
203 M(2, 1) = cy * sp * cr + sy * sr;
204 M(2, 2) = cy * cp;
205 return M;
206 }
207
209 template <typename T> fmat<T, 4, 4>
210 rotate4_rpy(const fvec<T, 3>& A)
211 {
212 fmat<T, 4, 4> M;
213 M.identity();
214 fvec<T, 3> angles = cgv::math::fvec<T, 3>(T(0.01745329252)) * A;
215 T cx = cos(angles[0]);
216 T sx = sin(angles[0]);
217 T cy = cos(angles[1]);
218 T sy = sin(angles[1]);
219 T cz = cos(angles[2]);
220 T sz = sin(angles[2]);
221 M(0, 0) = cy * cz;
222 M(0, 1) = sx * sy * cz - cx * sz;
223 M(0, 2) = cx * sy * cz + sx * sz;
224 M(1, 0) = cy * sz;
225 M(1, 1) = sx * sy * sz + cx * cz;
226 M(1, 2) = cx * sy * sz - sx * cz;
227 M(2, 0) = -sy;
228 M(2, 1) = sx * cy;
229 M(2, 2) = cx * cy;
230 return M;
231 }
232
234 template <typename T> fmat<T, 4, 4>
235 pose4(const fmat<T,3,4>& M) {
236 fmat<T, 4, 4> M4;
237 M4.set_col(0, fvec<T, 4>(M.col(0), 0));
238 M4.set_col(1, fvec<T, 4>(M.col(1), 0));
239 M4.set_col(2, fvec<T, 4>(M.col(2), 0));
240 M4.set_col(3, fvec<T, 4>(M.col(3), 1));
241 return M4;
242 }
244 template <typename T> fmat<T, 4, 4>
245 pose4(const fmat<T, 3, 3>& R, const fvec<T, 3>& t) {
246 fmat<T, 4, 4> M4;
247 M4.set_col(0, fvec<T, 4>(R.col(0), 0));
248 M4.set_col(1, fvec<T, 4>(R.col(1), 0));
249 M4.set_col(2, fvec<T, 4>(R.col(2), 0));
250 M4.set_col(3, fvec<T, 4>(t, 1));
251 return M4;
252 }
253
255 template <typename T> fmat<T, 4, 4>
256 pose4(const quaternion<T>& q, const fvec<T, 3>& t) {
257 fmat<T, 3, 3> R;
258 q.put_matrix(R);
259 return pose4<T>(R, t);
260 }
261
263 template <typename T> fmat<T, 4, 4>
264 look_at4(const fvec<T, 3>& eye, const fvec<T, 3>& focus, const fvec<T, 3>& view_up_dir) {
265 fvec<T, 3> view_dir = focus-eye;
266 view_dir.normalize();
267 fvec<T, 3> up_dir(view_up_dir);
268 up_dir.normalize();
269 fvec<T, 3> x = cross(view_dir,up_dir);
270 x.normalize();
271 fvec<T, 3> y = cross(x, view_dir);
272 fmat<T, 4, 4> R = identity4<T>();
273 R(0, 0) = x(0); R(0, 1) = x(1); R(0, 2) = x(2);
274 R(1, 0) = y(0); R(1, 1) = y(1); R(1, 2) = y(2);
275 R(2, 0) = -view_dir(0); R(2, 1) = -view_dir(1); R(2, 2) = -view_dir(2);
276 return R*translate4<T>(-eye);
277 }
278
280 template <typename T> fmat<T, 4, 4>
281 ortho4(const T& l, const T& r, const T& b, const T& t, const T& n, const T& f) {
282 fmat<T, 4, 4> M;
283 M.zeros();
284 M(0, 0) = T(2) / (r - l);
285 M(0, 3) = (r + l) / (l - r);
286 M(1, 1) = T(2) / (t - b);
287 M(1, 3) = (t + b) / (b - t);
288 M(2, 2) = T(2) / (n - f);
289 M(2, 3) = (n + f) / (n - f);
290 M(3, 3) = T(1);
291 return M;
292 }
294 template <typename T> fmat<T, 4, 4>
295 frustum4(const T& l, const T& r, const T& b, const T& t, const T& n, const T& f) {
296 fmat<T, 4, 4> M;
297 M.zeros();
298 M(0, 0) = T(2) * n / (r - l);
299 M(0, 2) = (r + l) / (r - l);
300 M(1, 1) = T(2) * n / (t - b);
301 M(1, 2) = (t + b) / (t - b);
302 M(2, 2) = (n + f) / (n - f);
303 M(2, 3) = T(2) * n * f / (n - f);
304 M(3, 2) = -T(1);
305 return M;
306 }
308 template <typename T> fmat<T, 4, 4>
309 perspective4(const T& fovy, const T& aspect, const T& zNear, const T& zFar) {
310 fmat<T, 4, 4> M;
311 M.zeros();
312 T angle = T(0.008726646262)*fovy;
313 T f = cos(angle)/sin(angle);
314 M(0, 0) = f / aspect;
315 M(1, 1) = f;
316 M(2, 2) = (zNear + zFar) / (zNear - zFar);
317 M(2, 3) = T(2) * zNear * zFar / (zNear - zFar);
318 M(3, 2) = -T(1);
319 return M;
320 }
321
323 template <typename T>
324 inline T znear_from_proj4(const fmat<T, 4, 4>& proj) {
325 return (proj[14] + proj[15]) / (proj[10] + proj[11]);
326 }
328 template <typename T>
329 inline T zfar_from_proj4(const fmat<T, 4, 4>& proj) {
330 return (proj[14] - proj[15]) / (proj[10] - proj[11]);
331 }
333 template <typename T>
334 inline T znear_from_invproj4(const fmat<T, 4, 4>& invproj) {
335 return -(invproj[14] - invproj[10]) / (invproj[15] - invproj[11]);
336 }
338 template <typename T>
339 inline T zfar_from_invproj4(const fmat<T, 4, 4>& invproj) {
340 return -(invproj[10] + invproj[14]) / (invproj[11] + invproj[15]);
341 }
342
344 template <typename T> fmat<T, 4, 4>
345 stereo_frustum_screen4(T eye, const T& eyeSeparation, const T& screenWidth, const T& screenHeight,
346 const T& zZeroParallax, const T& zNear, const T& zFar) {
347 T aspect = screenWidth / screenHeight;
348 T top = T(0.5)*screenHeight*zNear / zZeroParallax;
349 T bottom = -top;
350 T delta = T(0.5)*eyeSeparation*eye*screenWidth*zNear / zZeroParallax;
351 T left = bottom * aspect - delta;
352 T right = top * aspect - delta;
353 return frustum4<T>(left, right, bottom, top, zNear, zFar);
354 }
355
357 template <typename T> fmat<T, 4, 4>
358 stereo_translate_screen4(T eye, const T& eyeSeparation, const T& screenWidth) {
359 return translate4<T>(-T(0.5)*eyeSeparation*eye*screenWidth, 0, 0);
360 }
361
363 template <typename T> fmat<T, 4, 4>
364 stereo_translate4(T eye, const T& eyeSeparation, const T& fovy, const T& aspect, const T& zZeroParallax) {
365 return stereo_translate_screen4(eye, eyeSeparation, T(2)*tan(T(.8726646262e-2)*fovy)*zZeroParallax * aspect);
366 }
367
369 template <typename T> fmat<T, 4, 4>
370 stereo_perspective_screen4(T eye, const T& eyeSeparation, const T& screenWidth, const T& screenHeight,
371 const T& zZeroParallax, const T& zNear, const T& zFar) {
372 return stereo_frustum_screen4(eye, eyeSeparation, screenWidth, screenHeight, zZeroParallax, zNear, zFar) *
373 stereo_translate_screen4(eye, eyeSeparation, screenWidth);
374 }
375
377 template <typename T> fmat<T, 4, 4>
378 stereo_perspective4(T eye, const T& eyeSeparation, const T& fovy, const T& aspect,
379 const T& zZeroParallax, const T& zNear, const T& zFar) {
380 T screenHeight = T(2)*tan(T(.8726646262e-2)*fovy)*zZeroParallax;;
381 return stereo_perspective_screen4<double>(eye, eyeSeparation, screenHeight*aspect, screenHeight, zZeroParallax, zNear, zFar);
382 }
383 }
384}
cgv::math::fvec
A vector with zero based index.
Definition fvec.h:26
cgv
the cgv namespace
Definition print.h:11