1 /* This file is part of the Pangolin Project.
2 * http://github.com/stevenlovegrove/Pangolin
3 *
4 * Copyright (c) 2011 Steven Lovegrove
5 *
6 * Permission is hereby granted, free of charge, to any person
7 * obtaining a copy of this software and associated documentation
8 * files (the "Software"), to deal in the Software without
9 * restriction, including without limitation the rights to use,
10 * copy, modify, merge, publish, distribute, sublicense, and/or sell
11 * copies of the Software, and to permit persons to whom the
12 * Software is furnished to do so, subject to the following
13 * conditions:
14 *
15 * The above copyright notice and this permission notice shall be
16 * included in all copies or substantial portions of the Software.
17 *
18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
19 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
20 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
21 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
22 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
23 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
24 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
25 * OTHER DEALINGS IN THE SOFTWARE.
26 */
27
28 #pragma once
29
30 #include <pangolin/platform.h>
31 #include <pangolin/utils/simple_math.h>
32
33 #include <vector>
34
35 #if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
36 #define USE_EIGEN
37 #endif
38
39 #ifdef USE_EIGEN
40 #include <Eigen/Core>
41 #include <Eigen/Geometry>
42 #endif
43
44 #ifdef HAVE_TOON
45 #include <cstring>
46 #include <TooN/TooN.h>
47 #include <TooN/se3.h>
48 #endif
49
50 #ifdef HAVE_OCULUS
51 #include <pangolin/compat/ovr.h>
52 #endif
53
54 namespace pangolin {
55
56 #ifdef HAVE_GLES
57 typedef float GLprecision;
58 #else
59 typedef double GLprecision;
60 #endif
61
62 /// Capture OpenGL matrix types in enum to typing.
63 enum OpenGlStack {
64 GlModelViewStack = 0x1700, // GL_MODELVIEW
65 GlProjectionStack = 0x1701, // GL_PROJECTION
66 GlTextureStack = 0x1702 // GL_TEXTURE
67 };
68
69 enum AxisDirection
70 {
71 AxisNone,
72 AxisNegX, AxisX,
73 AxisNegY, AxisY,
74 AxisNegZ, AxisZ
75 };
76
77 struct CameraSpec {
78 GLprecision forward[3];
79 GLprecision up[3];
80 GLprecision right[3];
81 GLprecision img_up[2];
82 GLprecision img_right[2];
83 };
84
85 const static CameraSpec CameraSpecOpenGl = {{0,0,-1},{0,1,0},{1,0,0},{0,1},{1,0}};
86
87 const static CameraSpec CameraSpecYDownZForward = {{0,0,1},{0,-1,0},{1,0,0},{0,-1},{1,0}};
88
89 /// Direction vector for each AxisDirection enum
90 const static GLprecision AxisDirectionVector[7][3] = {
91 {0,0,0},
92 {-1,0,0}, {1,0,0},
93 {0,-1,0}, {0,1,0},
94 {0,0,-1}, {0,0,1}
95 };
96
97 /// Object representing OpenGl Matrix.
98 struct PANGOLIN_EXPORT OpenGlMatrix {
99 static OpenGlMatrix Translate(GLprecision x, GLprecision y, GLprecision z);
100 static OpenGlMatrix Scale(GLprecision x, GLprecision y, GLprecision z);
101 static OpenGlMatrix RotateX(GLprecision theta_rad);
102 static OpenGlMatrix RotateY(GLprecision theta_rad);
103 static OpenGlMatrix RotateZ(GLprecision theta_rad);
104
105
106 template<typename P>
107 static OpenGlMatrix ColMajor4x4(const P* col_major_4x4);
108
109 OpenGlMatrix();
110
111 #ifdef USE_EIGEN
112 template<typename P>
113 OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat);
114
115 template<typename P>
OpenGlMatrixOpenGlMatrix116 OpenGlMatrix(const Eigen::Transform<P,3,Eigen::Affine>& mat) : OpenGlMatrix(mat.matrix()) { }
117
118 template<typename P>
119 operator Eigen::Matrix<P,4,4>() const;
120
121 template<typename P>
122 operator Eigen::Transform<P,3,Eigen::Affine>() const;
123 #endif // USE_EIGEN
124
125 #ifdef HAVE_TOON
126 OpenGlMatrix(const TooN::SE3<>& T);
127 OpenGlMatrix(const TooN::Matrix<4,4>& M);
128 operator const TooN::SE3<>() const;
129 operator const TooN::Matrix<4,4>() const;
130 #endif // HAVE_TOON
131
132 #ifdef HAVE_OCULUS
133 OpenGlMatrix(const OVR::Matrix4f& M);
134 operator const OVR::Matrix4f() const;
135 #endif // HAVE_OCULUS
136
137 // Load matrix on to OpenGl stack
138 void Load() const;
139
140 void Multiply() const;
141
142 void SetIdentity();
143
144 OpenGlMatrix Transpose() const;
145
146 OpenGlMatrix Inverse() const;
147
operatorOpenGlMatrix148 GLprecision& operator()(int r, int c) {
149 return m[4*c +r];
150 }
151
operatorOpenGlMatrix152 GLprecision operator()(int r, int c) const {
153 return m[4 * c + r];
154 }
155
156 // Column major Internal buffer
157 GLprecision m[16];
158 };
159
160 PANGOLIN_EXPORT
161 OpenGlMatrix operator*(const OpenGlMatrix& lhs, const OpenGlMatrix& rhs);
162
163 PANGOLIN_EXPORT
164 std::ostream& operator<<(std::ostream& os, const OpenGlMatrix& mat);
165
166 /// Deprecated.
167 struct PANGOLIN_EXPORT OpenGlMatrixSpec : public OpenGlMatrix {
168 // Specify which stack this refers to
169 OpenGlStack type;
170 };
171
172 /// Object representing attached OpenGl Matrices / transforms.
173 class PANGOLIN_EXPORT OpenGlRenderState
174 {
175 public:
176 OpenGlRenderState();
177 OpenGlRenderState(const OpenGlMatrix& projection_matrix);
178 OpenGlRenderState(const OpenGlMatrix& projection_matrix, const OpenGlMatrix& modelview_matrix);
179
180 static void ApplyIdentity();
181
182 void Apply() const;
183 OpenGlRenderState& SetProjectionMatrix(OpenGlMatrix m);
184 OpenGlRenderState& SetModelViewMatrix(OpenGlMatrix m);
185
186 OpenGlMatrix& GetProjectionMatrix();
187 OpenGlMatrix GetProjectionMatrix() const;
188
189 OpenGlMatrix& GetModelViewMatrix();
190 OpenGlMatrix GetModelViewMatrix() const;
191
192 OpenGlMatrix GetProjectionModelViewMatrix() const;
193 OpenGlMatrix GetProjectiveTextureMatrix() const;
194
195 void EnableProjectiveTexturing() const;
196 void DisableProjectiveTexturing() const;
197
198 //! Seemlessly move OpenGl camera relative to changes in T_wc,
199 //! whilst still enabling interaction
200 void Follow(const OpenGlMatrix& T_wc, bool follow = true);
201 void Unfollow();
202
203 // Experimental - subject to change
204 OpenGlMatrix& GetProjectionMatrix(unsigned int view);
205 OpenGlMatrix GetProjectionMatrix(unsigned int view) const;
206 OpenGlMatrix& GetViewOffset(unsigned int view);
207 OpenGlMatrix GetViewOffset(unsigned int view) const;
208 OpenGlMatrix GetModelViewMatrix(int i) const;
209 void ApplyNView(int view) const;
210
211 PANGOLIN_DEPRECATED
212 OpenGlRenderState& Set(OpenGlMatrixSpec spec);
213
214 protected:
215 OpenGlMatrix modelview;
216 std::vector<OpenGlMatrix> projection;
217 std::vector<OpenGlMatrix> modelview_premult;
218 OpenGlMatrix T_cw;
219 bool follow;
220 };
221
222 PANGOLIN_EXPORT
223 OpenGlMatrixSpec ProjectionMatrixRUB_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
224
225 PANGOLIN_EXPORT
226 OpenGlMatrixSpec ProjectionMatrixRUB_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
227
228 PANGOLIN_EXPORT
229 OpenGlMatrixSpec ProjectionMatrixRDF_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
230
231 PANGOLIN_EXPORT
232 OpenGlMatrixSpec ProjectionMatrixRDF_TopRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
233
234 PANGOLIN_EXPORT
235 OpenGlMatrixSpec ProjectionMatrixRDF_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
236
237 PANGOLIN_EXPORT
238 OpenGlMatrixSpec ProjectionMatrixRDF_BottomRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
239
240 //! Use OpenGl's default frame RUB_BottomLeft
241 PANGOLIN_EXPORT
242 OpenGlMatrixSpec ProjectionMatrix(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
243
244 PANGOLIN_EXPORT
245 OpenGlMatrixSpec ProjectionMatrixOrthographic(GLprecision l, GLprecision r, GLprecision b, GLprecision t, GLprecision n, GLprecision f );
246
247
248 //! Generate glulookat style model view matrix, looking at (lx,ly,lz)
249 //! X-Right, Y-Up, Z-Back
250 PANGOLIN_EXPORT
251 OpenGlMatrix ModelViewLookAtRUB(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
252
253 //! Generate glulookat style model view matrix, looking at (lx,ly,lz)
254 //! X-Right, Y-Down, Z-Forward
255 PANGOLIN_EXPORT
256 OpenGlMatrix ModelViewLookAtRDF(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
257
258 //! Generate glulookat style model view matrix, OpenGL Default camera convention (XYZ=RUB), looking at (lx,ly,lz)
259 PANGOLIN_EXPORT
260 OpenGlMatrix ModelViewLookAt(GLprecision x, GLprecision y, GLprecision z, GLprecision lx, GLprecision ly, GLprecision lz, AxisDirection up);
261
262 PANGOLIN_EXPORT
263 OpenGlMatrix ModelViewLookAt(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
264
265
266 PANGOLIN_EXPORT
267 OpenGlMatrix IdentityMatrix();
268
269 PANGOLIN_EXPORT
270 OpenGlMatrixSpec IdentityMatrix(OpenGlStack type);
271
272 PANGOLIN_EXPORT
273 OpenGlMatrixSpec negIdentityMatrix(OpenGlStack type);
274
275 #ifdef HAVE_TOON
276 OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw);
277 OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M);
278 TooN::Matrix<4,4> ToTooN(const OpenGlMatrixSpec& ms);
279 TooN::SE3<> ToTooN_SE3(const OpenGlMatrixSpec& ms);
280 #endif
281
282 #ifdef HAVE_EIGEN
283 template<typename P>
284 Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms);
285 #endif
286
287 }
288
289 // Inline definitions
290 namespace pangolin
291 {
292
293 template<typename P>
ColMajor4x4(const P * col_major_4x4)294 inline OpenGlMatrix OpenGlMatrix::ColMajor4x4(const P* col_major_4x4)
295 {
296 OpenGlMatrix mat;
297 std::copy(col_major_4x4, col_major_4x4 + 16, mat.m);
298 return mat;
299 }
300
OpenGlMatrix()301 inline OpenGlMatrix::OpenGlMatrix() {
302 }
303
304 #ifdef USE_EIGEN
305 template<typename P> inline
OpenGlMatrix(const Eigen::Matrix<P,4,4> & mat)306 OpenGlMatrix::OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat)
307 {
308 for(int r=0; r<4; ++r ) {
309 for(int c=0; c<4; ++c ) {
310 m[c*4+r] = mat(r,c);
311 }
312 }
313 }
314
315 template<typename P>
316 OpenGlMatrix::operator Eigen::Matrix<P,4,4>() const
317 {
318 return ToEigen<P>(*this);
319 }
320
321 template<typename P>
322 OpenGlMatrix::operator Eigen::Transform<P,3,Eigen::Affine>() const
323 {
324 return Eigen::Transform<P,3,Eigen::Affine>(ToEigen<P>(*this));
325 }
326
327 template<typename P> inline
ToEigen(const OpenGlMatrix & ms)328 Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms)
329 {
330 Eigen::Matrix<P,4,4> mat;
331 for(int r=0; r<4; ++r ) {
332 for(int c=0; c<4; ++c ) {
333 mat(r,c) = (P)ms.m[c*4+r];
334 }
335 }
336 return mat;
337 }
338
339 #endif // USE_EIGEN
340
341 #ifdef HAVE_TOON
OpenGlMatrix(const TooN::SE3<> & T)342 inline OpenGlMatrix::OpenGlMatrix(const TooN::SE3<>& T)
343 {
344 TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
345 M.slice<0,0,3,3>() = T.get_rotation().get_matrix();
346 M.T()[3].slice<0,3>() = T.get_translation();
347 M[3] = TooN::makeVector(0,0,0,1);
348 std::memcpy(m, &(M[0][0]),16*sizeof(GLprecision));
349 }
350
OpenGlMatrix(const TooN::Matrix<4,4> & M)351 inline OpenGlMatrix::OpenGlMatrix(const TooN::Matrix<4,4>& M)
352 {
353 // Read in remembering col-major convension for our matrices
354 int el = 0;
355 for(int c=0; c<4; ++c)
356 for(int r=0; r<4; ++r)
357 m[el++] = M[r][c];
358 }
359
360 inline OpenGlMatrix::operator const TooN::SE3<>() const
361 {
362 const TooN::Matrix<4,4> m = *this;
363 const TooN::SO3<> R(m.slice<0,0,3,3>());
364 const TooN::Vector<3> t = m.T()[3].slice<0,3>();
365 return TooN::SE3<>(R,t);
366 }
367
368 inline OpenGlMatrix::operator const TooN::Matrix<4,4>() const
369 {
370 TooN::Matrix<4,4> M;
371 int el = 0;
372 for( int c=0; c<4; ++c )
373 for( int r=0; r<4; ++r )
374 M(r,c) = m[el++];
375 return M;
376 }
377
378 PANGOLIN_DEPRECATED
FromTooN(const TooN::SE3<> & T_cw)379 inline OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw)
380 {
381 TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
382 M.slice<0,0,3,3>() = T_cw.get_rotation().get_matrix();
383 M.T()[3].slice<0,3>() = T_cw.get_translation();
384 M[3] = TooN::makeVector(0,0,0,1);
385
386 OpenGlMatrixSpec P;
387 P.type = GlModelViewStack;
388 std::memcpy(P.m, &(M[0][0]),16*sizeof(GLprecision));
389 return P;
390 }
391
392 PANGOLIN_DEPRECATED
FromTooN(OpenGlStack type,const TooN::Matrix<4,4> & M)393 inline OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M)
394 {
395 // Read in remembering col-major convension for our matrices
396 OpenGlMatrixSpec P;
397 P.type = type;
398 int el = 0;
399 for(int c=0; c<4; ++c)
400 for(int r=0; r<4; ++r)
401 P.m[el++] = M[r][c];
402 return P;
403 }
404
405 PANGOLIN_DEPRECATED
ToTooN(const OpenGlMatrix & ms)406 inline TooN::Matrix<4,4> ToTooN(const OpenGlMatrix& ms)
407 {
408 TooN::Matrix<4,4> m;
409 int el = 0;
410 for( int c=0; c<4; ++c )
411 for( int r=0; r<4; ++r )
412 m(r,c) = ms.m[el++];
413 return m;
414 }
415
416 PANGOLIN_DEPRECATED
ToTooN_SE3(const OpenGlMatrix & ms)417 inline TooN::SE3<> ToTooN_SE3(const OpenGlMatrix& ms)
418 {
419 TooN::Matrix<4,4> m = ms;
420 const TooN::SO3<> R(m.slice<0,0,3,3>());
421 const TooN::Vector<3> t = m.T()[3].slice<0,3>();
422 return TooN::SE3<>(R,t);
423 }
424
425 #endif // HAVE_TOON
426
427 #ifdef HAVE_OCULUS
OpenGlMatrix(const OVR::Matrix4f & mat)428 inline OpenGlMatrix::OpenGlMatrix(const OVR::Matrix4f& mat)
429 {
430 for(int r=0; r<4; ++r )
431 for(int c=0; c<4; ++c )
432 m[c*4+r] = mat.M[r][c];
433 }
434
Matrix4f()435 inline OpenGlMatrix::operator const OVR::Matrix4f() const
436 {
437 OVR::Matrix4f mat;
438 for(int r=0; r<4; ++r )
439 for(int c=0; c<4; ++c )
440 mat.M[r][c] = m[c*4+r];
441 return mat;
442 }
443 #endif // HAVE_OCULUS
444
445
446 }
447