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