447 lines
13 KiB
C++
447 lines
13 KiB
C++
/* This file is part of the Pangolin Project.
|
|
* http://github.com/stevenlovegrove/Pangolin
|
|
*
|
|
* Copyright (c) 2011 Steven Lovegrove
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person
|
|
* obtaining a copy of this software and associated documentation
|
|
* files (the "Software"), to deal in the Software without
|
|
* restriction, including without limitation the rights to use,
|
|
* copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the
|
|
* Software is furnished to do so, subject to the following
|
|
* conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be
|
|
* included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
|
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
|
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
|
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
|
* OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pangolin/platform.h>
|
|
#include <pangolin/utils/simple_math.h>
|
|
|
|
#include <vector>
|
|
|
|
#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
|
|
#define USE_EIGEN
|
|
#endif
|
|
|
|
#ifdef USE_EIGEN
|
|
#include <Eigen/Core>
|
|
#include <Eigen/Geometry>
|
|
#endif
|
|
|
|
#ifdef HAVE_TOON
|
|
#include <cstring>
|
|
#include <TooN/TooN.h>
|
|
#include <TooN/se3.h>
|
|
#endif
|
|
|
|
#ifdef HAVE_OCULUS
|
|
#include <pangolin/compat/ovr.h>
|
|
#endif
|
|
|
|
namespace pangolin {
|
|
|
|
#ifdef HAVE_GLES
|
|
typedef float GLprecision;
|
|
#else
|
|
typedef double GLprecision;
|
|
#endif
|
|
|
|
/// Capture OpenGL matrix types in enum to typing.
|
|
enum OpenGlStack {
|
|
GlModelViewStack = 0x1700, // GL_MODELVIEW
|
|
GlProjectionStack = 0x1701, // GL_PROJECTION
|
|
GlTextureStack = 0x1702 // GL_TEXTURE
|
|
};
|
|
|
|
enum AxisDirection
|
|
{
|
|
AxisNone,
|
|
AxisNegX, AxisX,
|
|
AxisNegY, AxisY,
|
|
AxisNegZ, AxisZ
|
|
};
|
|
|
|
struct CameraSpec {
|
|
GLprecision forward[3];
|
|
GLprecision up[3];
|
|
GLprecision right[3];
|
|
GLprecision img_up[2];
|
|
GLprecision img_right[2];
|
|
};
|
|
|
|
const static CameraSpec CameraSpecOpenGl = {{0,0,-1},{0,1,0},{1,0,0},{0,1},{1,0}};
|
|
|
|
const static CameraSpec CameraSpecYDownZForward = {{0,0,1},{0,-1,0},{1,0,0},{0,-1},{1,0}};
|
|
|
|
/// Direction vector for each AxisDirection enum
|
|
const static GLprecision AxisDirectionVector[7][3] = {
|
|
{0,0,0},
|
|
{-1,0,0}, {1,0,0},
|
|
{0,-1,0}, {0,1,0},
|
|
{0,0,-1}, {0,0,1}
|
|
};
|
|
|
|
/// Object representing OpenGl Matrix.
|
|
struct PANGOLIN_EXPORT OpenGlMatrix {
|
|
static OpenGlMatrix Translate(GLprecision x, GLprecision y, GLprecision z);
|
|
static OpenGlMatrix Scale(GLprecision x, GLprecision y, GLprecision z);
|
|
static OpenGlMatrix RotateX(GLprecision theta_rad);
|
|
static OpenGlMatrix RotateY(GLprecision theta_rad);
|
|
static OpenGlMatrix RotateZ(GLprecision theta_rad);
|
|
|
|
|
|
template<typename P>
|
|
static OpenGlMatrix ColMajor4x4(const P* col_major_4x4);
|
|
|
|
OpenGlMatrix();
|
|
|
|
#ifdef USE_EIGEN
|
|
template<typename P>
|
|
OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat);
|
|
|
|
template<typename P>
|
|
OpenGlMatrix(const Eigen::Transform<P,3,Eigen::Affine>& mat) : OpenGlMatrix(mat.matrix()) { }
|
|
|
|
template<typename P>
|
|
operator Eigen::Matrix<P,4,4>() const;
|
|
|
|
template<typename P>
|
|
operator Eigen::Transform<P,3,Eigen::Affine>() const;
|
|
#endif // USE_EIGEN
|
|
|
|
#ifdef HAVE_TOON
|
|
OpenGlMatrix(const TooN::SE3<>& T);
|
|
OpenGlMatrix(const TooN::Matrix<4,4>& M);
|
|
operator const TooN::SE3<>() const;
|
|
operator const TooN::Matrix<4,4>() const;
|
|
#endif // HAVE_TOON
|
|
|
|
#ifdef HAVE_OCULUS
|
|
OpenGlMatrix(const OVR::Matrix4f& M);
|
|
operator const OVR::Matrix4f() const;
|
|
#endif // HAVE_OCULUS
|
|
|
|
// Load matrix on to OpenGl stack
|
|
void Load() const;
|
|
|
|
void Multiply() const;
|
|
|
|
void SetIdentity();
|
|
|
|
OpenGlMatrix Transpose() const;
|
|
|
|
OpenGlMatrix Inverse() const;
|
|
|
|
GLprecision& operator()(int r, int c) {
|
|
return m[4*c +r];
|
|
}
|
|
|
|
GLprecision operator()(int r, int c) const {
|
|
return m[4 * c + r];
|
|
}
|
|
|
|
// Column major Internal buffer
|
|
GLprecision m[16];
|
|
};
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix operator*(const OpenGlMatrix& lhs, const OpenGlMatrix& rhs);
|
|
|
|
PANGOLIN_EXPORT
|
|
std::ostream& operator<<(std::ostream& os, const OpenGlMatrix& mat);
|
|
|
|
/// Deprecated.
|
|
struct PANGOLIN_EXPORT OpenGlMatrixSpec : public OpenGlMatrix {
|
|
// Specify which stack this refers to
|
|
OpenGlStack type;
|
|
};
|
|
|
|
/// Object representing attached OpenGl Matrices / transforms.
|
|
class PANGOLIN_EXPORT OpenGlRenderState
|
|
{
|
|
public:
|
|
OpenGlRenderState();
|
|
OpenGlRenderState(const OpenGlMatrix& projection_matrix);
|
|
OpenGlRenderState(const OpenGlMatrix& projection_matrix, const OpenGlMatrix& modelview_matrix);
|
|
|
|
static void ApplyIdentity();
|
|
|
|
void Apply() const;
|
|
OpenGlRenderState& SetProjectionMatrix(OpenGlMatrix m);
|
|
OpenGlRenderState& SetModelViewMatrix(OpenGlMatrix m);
|
|
|
|
OpenGlMatrix& GetProjectionMatrix();
|
|
OpenGlMatrix GetProjectionMatrix() const;
|
|
|
|
OpenGlMatrix& GetModelViewMatrix();
|
|
OpenGlMatrix GetModelViewMatrix() const;
|
|
|
|
OpenGlMatrix GetProjectionModelViewMatrix() const;
|
|
OpenGlMatrix GetProjectiveTextureMatrix() const;
|
|
|
|
void EnableProjectiveTexturing() const;
|
|
void DisableProjectiveTexturing() const;
|
|
|
|
//! Seemlessly move OpenGl camera relative to changes in T_wc,
|
|
//! whilst still enabling interaction
|
|
void Follow(const OpenGlMatrix& T_wc, bool follow = true);
|
|
void Unfollow();
|
|
|
|
// Experimental - subject to change
|
|
OpenGlMatrix& GetProjectionMatrix(unsigned int view);
|
|
OpenGlMatrix GetProjectionMatrix(unsigned int view) const;
|
|
OpenGlMatrix& GetViewOffset(unsigned int view);
|
|
OpenGlMatrix GetViewOffset(unsigned int view) const;
|
|
OpenGlMatrix GetModelViewMatrix(int i) const;
|
|
void ApplyNView(int view) const;
|
|
|
|
PANGOLIN_DEPRECATED
|
|
OpenGlRenderState& Set(OpenGlMatrixSpec spec);
|
|
|
|
protected:
|
|
OpenGlMatrix modelview;
|
|
std::vector<OpenGlMatrix> projection;
|
|
std::vector<OpenGlMatrix> modelview_premult;
|
|
OpenGlMatrix T_cw;
|
|
bool follow;
|
|
};
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRUB_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRUB_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRDF_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRDF_TopRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRDF_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixRDF_BottomRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
//! Use OpenGl's default frame RUB_BottomLeft
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrix(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec ProjectionMatrixOrthographic(GLprecision l, GLprecision r, GLprecision b, GLprecision t, GLprecision n, GLprecision f );
|
|
|
|
|
|
//! Generate glulookat style model view matrix, looking at (lx,ly,lz)
|
|
//! X-Right, Y-Up, Z-Back
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix ModelViewLookAtRUB(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
|
|
|
|
//! Generate glulookat style model view matrix, looking at (lx,ly,lz)
|
|
//! X-Right, Y-Down, Z-Forward
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix ModelViewLookAtRDF(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
|
|
|
|
//! Generate glulookat style model view matrix, OpenGL Default camera convention (XYZ=RUB), looking at (lx,ly,lz)
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix ModelViewLookAt(GLprecision x, GLprecision y, GLprecision z, GLprecision lx, GLprecision ly, GLprecision lz, AxisDirection up);
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix ModelViewLookAt(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
|
|
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrix IdentityMatrix();
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec IdentityMatrix(OpenGlStack type);
|
|
|
|
PANGOLIN_EXPORT
|
|
OpenGlMatrixSpec negIdentityMatrix(OpenGlStack type);
|
|
|
|
#ifdef HAVE_TOON
|
|
OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw);
|
|
OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M);
|
|
TooN::Matrix<4,4> ToTooN(const OpenGlMatrixSpec& ms);
|
|
TooN::SE3<> ToTooN_SE3(const OpenGlMatrixSpec& ms);
|
|
#endif
|
|
|
|
#ifdef HAVE_EIGEN
|
|
template<typename P>
|
|
Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms);
|
|
#endif
|
|
|
|
}
|
|
|
|
// Inline definitions
|
|
namespace pangolin
|
|
{
|
|
|
|
template<typename P>
|
|
inline OpenGlMatrix OpenGlMatrix::ColMajor4x4(const P* col_major_4x4)
|
|
{
|
|
OpenGlMatrix mat;
|
|
std::copy(col_major_4x4, col_major_4x4 + 16, mat.m);
|
|
return mat;
|
|
}
|
|
|
|
inline OpenGlMatrix::OpenGlMatrix() {
|
|
}
|
|
|
|
#ifdef USE_EIGEN
|
|
template<typename P> inline
|
|
OpenGlMatrix::OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat)
|
|
{
|
|
for(int r=0; r<4; ++r ) {
|
|
for(int c=0; c<4; ++c ) {
|
|
m[c*4+r] = mat(r,c);
|
|
}
|
|
}
|
|
}
|
|
|
|
template<typename P>
|
|
OpenGlMatrix::operator Eigen::Matrix<P,4,4>() const
|
|
{
|
|
return ToEigen<P>(*this);
|
|
}
|
|
|
|
template<typename P>
|
|
OpenGlMatrix::operator Eigen::Transform<P,3,Eigen::Affine>() const
|
|
{
|
|
return Eigen::Transform<P,3,Eigen::Affine>(ToEigen<P>(*this));
|
|
}
|
|
|
|
template<typename P> inline
|
|
Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms)
|
|
{
|
|
Eigen::Matrix<P,4,4> mat;
|
|
for(int r=0; r<4; ++r ) {
|
|
for(int c=0; c<4; ++c ) {
|
|
mat(r,c) = (P)ms.m[c*4+r];
|
|
}
|
|
}
|
|
return mat;
|
|
}
|
|
|
|
#endif // USE_EIGEN
|
|
|
|
#ifdef HAVE_TOON
|
|
inline OpenGlMatrix::OpenGlMatrix(const TooN::SE3<>& T)
|
|
{
|
|
TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
|
|
M.slice<0,0,3,3>() = T.get_rotation().get_matrix();
|
|
M.T()[3].slice<0,3>() = T.get_translation();
|
|
M[3] = TooN::makeVector(0,0,0,1);
|
|
std::memcpy(m, &(M[0][0]),16*sizeof(GLprecision));
|
|
}
|
|
|
|
inline OpenGlMatrix::OpenGlMatrix(const TooN::Matrix<4,4>& M)
|
|
{
|
|
// Read in remembering col-major convension for our matrices
|
|
int el = 0;
|
|
for(int c=0; c<4; ++c)
|
|
for(int r=0; r<4; ++r)
|
|
m[el++] = M[r][c];
|
|
}
|
|
|
|
inline OpenGlMatrix::operator const TooN::SE3<>() const
|
|
{
|
|
const TooN::Matrix<4,4> m = *this;
|
|
const TooN::SO3<> R(m.slice<0,0,3,3>());
|
|
const TooN::Vector<3> t = m.T()[3].slice<0,3>();
|
|
return TooN::SE3<>(R,t);
|
|
}
|
|
|
|
inline OpenGlMatrix::operator const TooN::Matrix<4,4>() const
|
|
{
|
|
TooN::Matrix<4,4> M;
|
|
int el = 0;
|
|
for( int c=0; c<4; ++c )
|
|
for( int r=0; r<4; ++r )
|
|
M(r,c) = m[el++];
|
|
return M;
|
|
}
|
|
|
|
PANGOLIN_DEPRECATED
|
|
inline OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw)
|
|
{
|
|
TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
|
|
M.slice<0,0,3,3>() = T_cw.get_rotation().get_matrix();
|
|
M.T()[3].slice<0,3>() = T_cw.get_translation();
|
|
M[3] = TooN::makeVector(0,0,0,1);
|
|
|
|
OpenGlMatrixSpec P;
|
|
P.type = GlModelViewStack;
|
|
std::memcpy(P.m, &(M[0][0]),16*sizeof(GLprecision));
|
|
return P;
|
|
}
|
|
|
|
PANGOLIN_DEPRECATED
|
|
inline OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M)
|
|
{
|
|
// Read in remembering col-major convension for our matrices
|
|
OpenGlMatrixSpec P;
|
|
P.type = type;
|
|
int el = 0;
|
|
for(int c=0; c<4; ++c)
|
|
for(int r=0; r<4; ++r)
|
|
P.m[el++] = M[r][c];
|
|
return P;
|
|
}
|
|
|
|
PANGOLIN_DEPRECATED
|
|
inline TooN::Matrix<4,4> ToTooN(const OpenGlMatrix& ms)
|
|
{
|
|
TooN::Matrix<4,4> m;
|
|
int el = 0;
|
|
for( int c=0; c<4; ++c )
|
|
for( int r=0; r<4; ++r )
|
|
m(r,c) = ms.m[el++];
|
|
return m;
|
|
}
|
|
|
|
PANGOLIN_DEPRECATED
|
|
inline TooN::SE3<> ToTooN_SE3(const OpenGlMatrix& ms)
|
|
{
|
|
TooN::Matrix<4,4> m = ms;
|
|
const TooN::SO3<> R(m.slice<0,0,3,3>());
|
|
const TooN::Vector<3> t = m.T()[3].slice<0,3>();
|
|
return TooN::SE3<>(R,t);
|
|
}
|
|
|
|
#endif // HAVE_TOON
|
|
|
|
#ifdef HAVE_OCULUS
|
|
inline OpenGlMatrix::OpenGlMatrix(const OVR::Matrix4f& mat)
|
|
{
|
|
for(int r=0; r<4; ++r )
|
|
for(int c=0; c<4; ++c )
|
|
m[c*4+r] = mat.M[r][c];
|
|
}
|
|
|
|
inline OpenGlMatrix::operator const OVR::Matrix4f() const
|
|
{
|
|
OVR::Matrix4f mat;
|
|
for(int r=0; r<4; ++r )
|
|
for(int c=0; c<4; ++c )
|
|
mat.M[r][c] = m[c*4+r];
|
|
return mat;
|
|
}
|
|
#endif // HAVE_OCULUS
|
|
|
|
|
|
}
|