Hello OpenVR : Pt 2 : Basic Rendering + Head Tracking

hellovr.cpp

/****
OpenVR example : OpenGL rendering of Stanford Bunny in stereo, outputs eye textures to the headset, and blits one of the eyes to the screen.
Meant to be a minimal Useful example.
Tested only on DK2, Win7, AMD Rx480.
GLFW supports Vulkan so in theory should be able to update this to use VK rendering
****/

///\todo on dk2 we don't need to do any manual distortion processing for correct rendering but the OpenVR examples have this as a render pass.
/// Do we need to do this ?  Is this because timewarp / distortion are done automatically in Oculus SDK but will break on Vive?

///\todo the app shutdown on escape is also taking a ton of time and not always relinquishing the process without clicking "x" on the console window.

///\todo these aren't defined anywhere - should be in gl-hpp common.h
#define GL_ALT_GL_API 1
#define GL_ALT_GLES_API 2

/// header only extension loader and object oriented bindings -- ref : https://github.com/VirtuosoChris/glhpp
#include <glalt/gl4.5.h>	// opengl api - 4.5 Core
#include <glalt/glext.h>	// opengl extensions - things outside of Core
#include <opengl.hpp>		// object oriented bindings for OpenGL objects

#include <GLFW/glfw3.h>
#include <openvr.h>

#include <iostream>
#include <string>

// https://github.com/VirtuosoChris/Bunny/blob/master/Bunny.h
#define BUNNY_IMPLEMENTATION
#include "Bunny.h"

// file attached in GIST
#define VIRTUOSO_TRANSFORMATIONSLIB_IMPLEMENTATION
#include <Virtuoso/Math/transformationslib.h>

// file attached in GIST
#define VIRTUOSO_SHADERPROGRAMLIB_IMPLEMENTATION
#include <Virtuoso/GL/ShaderProgramLib.h>


static void error_callback(int error, const char* description)
{
	std::cerr<< "Error " << error << " : " << description << std::endl;
}


static void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
	if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
		glfwSetWindowShouldClose(window, GLFW_TRUE);
}


std::ostream& operator<<(std::ostream& str, const GLFWvidmode& vidmode)
{
	str << "\n{ // GLFWvidmode \n";
	str << "\twidth = " << vidmode.width <<'\n';
	str << "\theight = " << vidmode.height <<'\n';
	str << "\tredBits = " << vidmode.redBits<<'\n';
	str << "\tgreenBits = "<< vidmode.greenBits<<'\n';
	str << "\tblueBits = "<< vidmode.blueBits<<'\n';
	str << "\trefreshRate = "<<vidmode.refreshRate<<'\n';
	str << "\n}\n" << std::endl;

	return str;
}


std::string GetTrackedDeviceString( vr::IVRSystem *pHmd, vr::TrackedDeviceIndex_t unDevice, vr::TrackedDeviceProperty prop, vr::TrackedPropertyError *peError = NULL )
{
	uint32_t unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty( unDevice, prop, NULL, 0, peError );
	if( unRequiredBufferLen == 0 )
		return "";

	char *pchBuffer = new char[ unRequiredBufferLen ];
	unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty( unDevice, prop, pchBuffer, unRequiredBufferLen, peError );
	std::string sResult = pchBuffer;
	delete [] pchBuffer;
	return sResult;
}


struct OpenVRApplication
{
	vr::IVRSystem* hmd;
	uint32_t rtWidth;
	uint32_t rtHeight;

	vr::TrackedDevicePose_t trackedDevicePose[vr::k_unMaxTrackedDeviceCount];

	OpenVRApplication() : 
		hmd(NULL),
		rtWidth(0), rtHeight(0)
	{
		if (! vr::VR_IsHmdPresent())
		{
			throw std::runtime_error("Error : HMD not detected on the system");
		}

		if (!vr::VR_IsRuntimeInstalled())
		{
			throw std::runtime_error("Error : OpenVR Runtime not detected on the system");
		}

		initVR();

		if (!vr::VRCompositor())
		{
			throw std::runtime_error("Unable to initialize VR compositor!\n ");
		}

		hmd->GetRecommendedRenderTargetSize(&rtWidth, &rtHeight);

		std::clog<<"Initialized HMD with suggested render target size : " << rtWidth << "x" << rtHeight << std::endl;

		const float freq = hmd->GetFloatTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_DisplayFrequency_Float);

		std::clog << "display frequency : " << freq <<std::endl; 
	}

	void shutdown()
	{
		if (hmd)
		{
			std::clog << "shutting down OpenVR" << std::endl;
			vr::VR_Shutdown();
			hmd = NULL;
		}
	}

	virtual ~OpenVRApplication()
	{
		shutdown();
	}

	void setupFrame()
	{
		vr::VRCompositor()->WaitGetPoses(trackedDevicePose, vr::k_unMaxTrackedDeviceCount, nullptr, 0);
	}

	void submitFramesOpenGL(GLint leftEyeTex, GLint rightEyeTex, bool linear = false)
	{
		if (!hmd)
		{
			throw std::runtime_error("Error : presenting frames when VR system handle is NULL");
		}

		///\todo the documentation on this is unclear.  I am not sure which one is correct for OpenGL srgb.
		vr::EColorSpace colorSpace = linear ? vr::ColorSpace_Linear : vr::ColorSpace_Gamma;

		vr::Texture_t leftEyeTexture = {(void*)leftEyeTex, vr::API_OpenGL, colorSpace};
		vr::Texture_t rightEyeTexture = {(void*)rightEyeTex, vr::API_OpenGL, colorSpace};

		vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture);
		vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture);

		vr::VRCompositor()->PostPresentHandoff();
	}

	void handleVRError(vr::EVRInitError err)
	{
		std::string errStr = vr::VR_GetVRInitErrorAsEnglishDescription(err);
		shutdown();
		throw std::runtime_error(errStr);
	}

	void initVR()
	{
		vr::EVRInitError err = vr::VRInitError_None;

		hmd = vr::VR_Init(&err, vr::VRApplication_Scene);

		if (err != vr::VRInitError_None)
		{
			handleVRError(err);
		}

		std::clog << GetTrackedDeviceString( hmd, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String) << std::endl;
		std::clog << GetTrackedDeviceString( hmd, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_SerialNumber_String) << std::endl;
	}
};


/************ Test application code **************/

struct RenderTarget
{
	gl::Framebuffer fbo;

	unsigned int frameWidth; ///< one half the allocated render target width, since we are using side by side stereo
	unsigned int frameHeight;
	unsigned int multisamples;
	RenderTarget(unsigned int width, unsigned int height, unsigned int samples) :
		frameWidth(width), frameHeight(height), multisamples(samples)
	{
	}
};


struct BasicRenderTarget : public RenderTarget
{
	gl::Renderbuffer depthTex;

	void prime(GLuint tex)
	{
		fbo.Bind(GL_FRAMEBUFFER);

#if GL_ALT_API_NAME == GL_ALT_GLES_API
		if (multisamples > 1)
		{
			glFramebufferTexture2DMultisampleEXT(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D,
				tex, 0, multisamples);
		}
		else
#endif
		{
			fbo.Texture(GL_COLOR_ATTACHMENT0, tex, 0);
		}

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
		{
			throw std::runtime_error("BasicRenderTarget incomplete");
		}
	}


	BasicRenderTarget(int multisamples, unsigned int width, unsigned int height) :
		RenderTarget(width, height, multisamples)
	{

		///\todo because with DSA we don't bind the FB to a target, causing it to be initialized.
		GLint oldFB = gl::Get<GLint>(GL_FRAMEBUFFER_BINDING);
		fbo.Bind(GL_FRAMEBUFFER);
		glBindFramebuffer(GL_FRAMEBUFFER, oldFB);

		const GLenum depthFormat = GL_DEPTH_COMPONENT24;

#if GL_ALT_API_NAME == GL_ALT_GLES_API
		if (multisamples > 1)
		{
			std::clog<<"Side by side with multisamples : "<<multisamples<<std::endl;
			depthTex.Bind();
			glRenderbufferStorageMultisampleEXT(GL_RENDERBUFFER, multisamples, depthFormat, width, height);
		}
		else
#endif
		{
			std::clog<<"Side by side without multisampling"<<std::endl;
			depthTex.Storage(depthFormat, width, height);
		}

		fbo.Renderbuffer(GL_DEPTH_ATTACHMENT, depthTex); ///\todo was depthTex

		static const GLenum draw_buffers[] = {GL_COLOR_ATTACHMENT0};

		glViewport(0, 0, frameWidth, frameHeight);
	}
};


const char* bunnyVert =
	"#version 450\n\n"
	"layout (location=0) in vec3 position;\n"
	"layout (location=1) in vec3 normal;\n\n"
	"out float z;\n"
	"uniform mat4 mvp;\n\n"
	"void main(void)\n"
	"{\n"
	"\tz = normal.z;\n"
	"\tgl_Position = mvp * vec4(position,1.0);\n"
	"}\n";

const char* bunnyFrag = 
	"#version 450\n\n"
	"\nin float z;\n"
	"\nout vec4 col;\n"
	"void main(){\n"
	"\t\n"
	"\tcol = vec4(vec3(z), 1.0);\n"
	"\t\n"
	"}\n";


int main(void)
{
	try
	{
		GLFWwindow* window;

		glfwSetErrorCallback(error_callback);

		if (!glfwInit())
		{
			return 1;
		}

		glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 4);
		glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 5);
		glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
		glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
		glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);

		OpenVRApplication vrApp;

		window = glfwCreateWindow(vrApp.rtWidth, vrApp.rtHeight, "Hello OpenVR", NULL, NULL);

		if (!window)
		{
			glfwTerminate();
			return 1;
		}

		glfwSetKeyCallback(window, key_callback);

		glfwMakeContextCurrent(window);

		// don't wait for vsync - on dk2 should hit 75 fps in FRAPS on front buffer window.  when i use vsync intervals = 2, i get 50 fps on my monitor with refresh set to 100 hz.
		// makes perfect sense.  What if user forces on vsync though?
		glfwSwapInterval(0); 

		glClearColor (0.0f, 0.0f, 1.0f, 1.0f);
		glEnable(GL_DEPTH_TEST);
		glEnable(GL_CULL_FACE);
		
		// scope so GL objects destruct before destroying window + context
		{
			gl::Texture eyeTextures[2];
			
			BasicRenderTarget renderTargets[2] =  {
									BasicRenderTarget(1, vrApp.rtWidth, vrApp.rtHeight),
									BasicRenderTarget(1, vrApp.rtWidth, vrApp.rtHeight)
									};

			for (int i =0; i < 2; i++)
			{
				eyeTextures[i].Storage2D(1, GL_SRGB8_ALPHA8, vrApp.rtWidth, vrApp.rtHeight);
				renderTargets[i].prime(eyeTextures[i].name);
			}

			BunnyVAO bunny = make_bunny_vao();

			gl::Program bunnyProg = Virtuoso::GL::programWithSource(bunnyVert, bunnyFrag);
			bunnyProg.Use();

			Eigen::Matrix4f projectionMatrices[2];

			for (int i =0; i < 2; i++)
			{
				auto rmat = vrApp.hmd->GetProjectionMatrix((vr::EVREye)i, .01, 1000.0f, vr::EGraphicsAPIConvention::API_OpenGL);
				projectionMatrices[i] = Eigen::Map<Eigen::Matrix<float, 4, 4, Eigen::RowMajor> >((float*)rmat.m);
			}

			int winWidth, winHeight;
			glfwGetFramebufferSize(window, &winWidth, &winHeight);

			Eigen::Matrix4f eyeMatrices[2] = {Eigen::Matrix4f::Identity(), Eigen::Matrix4f::Identity()};
			Eigen::Matrix4f bunnyObjectMatrix = translationMatrix(0.0f,0.0f,-1.0f);

			while (!glfwWindowShouldClose(window))
			{
				vrApp.setupFrame();

				vr::TrackedDevicePose_t& hmdPose = vrApp.trackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd];

				Eigen::Matrix4f headPose = Eigen::Matrix4f::Identity();

				if (hmdPose.bDeviceIsConnected && hmdPose.bPoseIsValid)
				{
					auto rot = hmdPose.mDeviceToAbsoluteTracking;

					Eigen::Map< Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > mapp((float*)rot.m);

					Eigen::Matrix<float, 3, 4> myinv = mapp;//.inverse();

					Eigen::Matrix4f finRot = Eigen::Matrix4f::Identity();

					finRot.topLeftCorner<3,4>() = myinv;

					headPose = finRot.inverse();
				}

				// update matrices
				for (int i =0; i < 2; i++)
				{
					auto opvrHeadTransform = vrApp.hmd->GetEyeToHeadTransform((vr::EVREye)i);
					
					Eigen::Matrix4f leyemat = Eigen::Matrix4f::Identity();
						
					Eigen::Map< Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > mapp((float*)opvrHeadTransform.m);

					leyemat.topLeftCorner<3,4>() = mapp;

					eyeMatrices[i] = leyemat.inverse() * headPose;
				}

				/*** draw eye buffers ***/
				
				for (int i =0; i < 2; i++)
				{
					renderTargets[i].fbo.Bind(GL_FRAMEBUFFER);

					glViewport(0, 0, vrApp.rtWidth, vrApp.rtHeight);
					glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

					Eigen::Matrix4f finMat = projectionMatrices[i] * eyeMatrices[i] * bunnyObjectMatrix;

					bunnyProg.UniformMatrix<float, 4, 4>("mvp", finMat.data());

					draw_bunny_in_vao(bunny);
				}

				/*** submit frames ***/
				vrApp.submitFramesOpenGL(eyeTextures[0].name, eyeTextures[1].name);

				/*** blit one eye frame to screen ***/

				glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
				glBindFramebuffer(GL_READ_FRAMEBUFFER, renderTargets[0].fbo.name);

				glBlitFramebuffer(0,0,
					vrApp.rtWidth, vrApp.rtHeight,
					0,0,
					winWidth, winHeight,
					GL_COLOR_BUFFER_BIT,
					GL_LINEAR);

				/*** present to screen ***/
				glfwSwapBuffers(window);
				glfwPollEvents();
			}

			vrApp.shutdown(); ///\todo if I don't include this here, and just let the destructor handle shutting down VR, the process never terminates correctly, and breaks VR until I reboot.
		}

		glfwDestroyWindow(window);
		glfwTerminate();
	}
	catch (const std::runtime_error& err)
	{
		std::cerr<< err.what() <<std::endl;

#ifdef _WIN32
		system("pause");
#endif

		return 1;
	}

	std::clog <<" End" << std::endl;

	return 0;
}

ShaderProgramLib.h

//  Copyright (c) 2013 Virtuoso Engine LLC. All rights reserved.
//
// defines:
// VIRTUOSO_SUPPRESS_OUTPUT to hide compilation logs, etc
// VIRTUOSO_SHADERPROGRAMLIB_IMPLEMENTATION for implementation

#ifndef _GL_SHADER_H_INCLUDED
#define _GL_SHADER_H_INCLUDED

#if !defined(GL_ALT_API_NAME)
#error "Missing dependency : include a glalt header before including ShaderProgramLib.h"
#endif

#if !defined(GL_HPP)
#error "Missing dependency : include opengl.hpp before including ShaderProgramLib.h"
#endif

#include <iostream>
#include <memory>

namespace Virtuoso
{
    namespace GL
    {
        /*class ShaderProgram : public gl::Program
        {
        //inherit from gl alt so we can cache uniform locs, use eigen uniforms easier, etc
        //pretty much that's all a todo though

        //cacheUniformLocations(...)
        };*/

        typedef gl::Program ShaderProgram;
        typedef std::shared_ptr<ShaderProgram> ProgramPtr;

        void initializeProgramFromSource(ShaderProgram& prog, const std::string& vertSrc, const std::string& fragSrc, const std::string& geomSrc = "");
        ShaderProgram programWithSource(const std::string& vertSrc, const std::string& fragSrc, const std::string& geomSrc = "");
        ShaderProgram programWithFiles(const std::string& vertFile, const std::string& fragFile, const std::string& geomFile = "");
        
        gl::Shader Shader(GLenum shaderType, const std::string& src);
        
        gl::Program createProgram(std::initializer_list<gl::Shader> shaders);
    }
}
#endif

#ifdef VIRTUOSO_SHADERPROGRAMLIB_IMPLEMENTATION

#include <fstream>
#include <stdexcept>

namespace Virtuoso
{
    namespace GL
    {
        gl::Shader Shader(GLenum shaderType, const std::string& src)
        {
            gl::Shader rval(shaderType);
            
            rval.Source(src);
            std::string compileLog = rval.Compile();
            
#ifdef VIRTUOSO_LOG_SHADERS
            std::clog<<"Shader SRC : "<< src << std::endl;
#endif
            
            if (compileLog.length())
            {
                std::clog<<"\nShader Compile Log : \nStage : "<< (int)shaderType<<"\n" << compileLog << std::endl;
            }
            
            return rval;
        }
    
        gl::Program Program(std::initializer_list<gl::Shader> shaders)
        {
            gl::Program rval;
            
            for (const gl::Shader& sh: shaders)
            {
                rval.Attach(sh);
            }
  
            std::string linkLog = rval.Link();

            if (linkLog.length())
            {
                std::clog << " Program Link Log: \n" << linkLog << "\n\n";
            }
            
            return rval;
        }
        
        void initializeProgramFromSource(ShaderProgram& prog, const std::string& vertSrc, const std::string& fragSrc, const std::string& geomSrc)
        {
			#ifdef VIRTUOSO_LOG_SHADERS

            std::clog<<"VERT SRC : \n\n"<<vertSrc<<std::endl;
            std::clog<<"FRAG SRC : \n\n"<<fragSrc<<std::endl;
            
			#endif

            gl::Shader vertexShader(GL_VERTEX_SHADER);
            vertexShader.Source(vertSrc);
            std::string vsCompileLog = vertexShader.Compile();

            gl::Shader fragmentShader(GL_FRAGMENT_SHADER);
            fragmentShader.Source(fragSrc);
            std::string fsCompileLog = fragmentShader.Compile();

            std::string gsCompileLog;

            if (geomSrc.length())
            {
#ifdef GL_GEOMETRY_SHADER_EXT
                ///create object, but won't be used unless there's valid source
                gl::Shader geometryShader(GL_GEOMETRY_SHADER_EXT);

                static bool canGeomShader = gl::check_extension("GL_EXT_geometry_shader") || gl::check_extension("ARB_geometry_shader4");

                if (canGeomShader)
                {
                    geometryShader.Source(geomSrc);
                    gsCompileLog = geometryShader.Compile();
                    prog.Attach(geometryShader);
                }
                else
                {
                    throw std::runtime_error("Geometry shader source passed to initializeProgramFromSource, but OpenGL runtime does not support this extension");
                }
#else
                throw std::runtime_error("Geometry shader source passed to initializeProgramFromSource, but OpenGL compile time platform does not support!");
#endif
            }

            prog.Attach(vertexShader);
            prog.Attach(fragmentShader);

            std::string linkLog = prog.Link();

#ifndef VIRTUOSO_SUPPRESS_OUTPUT
            if (vsCompileLog.length())
            {
                std::clog << "Vertex Shader Compile Log: \n" << vsCompileLog << "\n\n";
            }
            if (fsCompileLog.length())
            {
                std::clog << "Fragment Shader Compile Log: \n" << fsCompileLog << "\n\n";
            }
            if (gsCompileLog.length())
            {
                std::clog << "Geometry Shader Compile Log: \n" << gsCompileLog << "\n\n";
            }
            if (linkLog.length())
            {
                std::clog << " Program Link Log: \n" << linkLog << "\n\n";
            }
#endif
        }

        ShaderProgram programWithSource(const std::string& vertSrc, const std::string& fragSrc, const std::string& geomSrc)
        {
            ShaderProgram prog;
            initializeProgramFromSource(prog, vertSrc, fragSrc, geomSrc);
            return prog;
        }

        ShaderProgram programWithFiles(const std::string& vertFile, const std::string& fragFile, const std::string& geomFile)
        {
            ShaderProgram prog;

            std::ifstream vertShaderFile(vertFile.c_str());
            std::ifstream fragShaderFile(fragFile.c_str());

            std::string geomSrc;
            std::string vertSrc;
            std::string fragSrc;

            if (geomFile.length())
            {
                std::ifstream geomShaderFile(geomFile.c_str());
                std::ostringstream s;
                s << geomShaderFile.rdbuf();
                geomSrc = s.str();
                geomShaderFile.close();
            }

            if (vertShaderFile.is_open())
            {
                std::ostringstream s;
                s << vertShaderFile.rdbuf();
                vertSrc = s.str();
                vertShaderFile.close();
            }
            else
            {
                std::string error = "Unable to open vertex shader file : ";
                error += vertFile;
                throw std::runtime_error(error.c_str());
            }

            if (fragShaderFile.is_open())
            {
                std::ostringstream s1;
                s1 << fragShaderFile.rdbuf();
                fragSrc = s1.str();
                fragShaderFile.close();
            }
            else
            {
                std::string error = "Unable to open fragment shader file : ";
                error += fragFile;
                throw std::runtime_error(error.c_str());
            }

            initializeProgramFromSource(prog, vertSrc, fragSrc, geomSrc);

            return prog;
        }
    }
}

#endif

transformationslib.h

#ifndef TRANSFORMATIONS_H_INCLUDED
#define TRANSFORMATIONS_H_INCLUDED

#include <Eigen/Eigen>

///\file These functions create transformation matrices using the Eigen library.  What these particular transforms are and what their
/// arguments do should all hopefully be obvious.  All angles are in radians and sane default arguments are provided where possible

///lookat()
///orthoProjection()

inline Eigen::Vector4f extractCameraPlane(const Eigen::Matrix4f& modelviewProjection)
{
    Eigen::Vector4f cameraplane = modelviewProjection.row(3) + (modelviewProjection.row(4));
    float tmp = cameraplane[3];
    cameraplane[3] = 0.0f;
    cameraplane.normalize();
    cameraplane[3] = tmp;
    return cameraplane;
    
   /* Eigen::Vector4f cameraPlane = -modelview.col(2);
    cameraPlane[3] = -(playerState.position[0] * cameraPlane[0] + playerState.position[1] * cameraPlane[1] + playerState.position[2] * cameraPlane[2]);*/
    ///return -modelview.row(2);
}

Eigen::Matrix4f cameraFrameMatrix(const Eigen::Vector3f& lookVector, const Eigen::Vector3f& upVecIn, const Eigen::Vector3f& position = Eigen::Vector3f(0, 0, 0));

Eigen::Matrix4f perspectiveProjectionInfinite(float fov = 60, float aspect = 1280/720.0, float zNear = .01);
Eigen::Matrix4f perspectiveProjectionInfinite(float left, float right, float bottom, float top, float near);

Eigen::Matrix4f perspectiveProjection(float fov = 60, float aspect = 1280/720.0, float zNear = .01, float zFar = 1000.0 );

Eigen::Matrix4f perspectiveProjectionWithFOVAngles(float fovRadsX, float fovRadsY, const float near, const float far);

Eigen::Matrix4f perspectiveProjectionWithFOVAngles(float fovRadsX, float fovRadsY, const float near);

Eigen::Matrix4f perspectiveProjection(float left, float right, float top, float bottom, float near, float far);

Eigen::Matrix4f rotationMatrixY(float theta);

Eigen::Matrix3f rotationMatrixY_3x3(float theta);

Eigen::Matrix4f rotationMatrixZ(float theta);

Eigen::Matrix4f rotationMatrixX(float theta);

Eigen::Matrix4f rotationMatrixXYZ(float rotX, float rotY, float rotZ);

Eigen::Matrix4f rotateAxisAngle(Eigen::Vector3f axis, float angle);

Eigen::Matrix4f scalingMatrix(float x, float y, float z);
Eigen::Matrix4f scalingMatrix(const Eigen::Vector3f& vec);

Eigen::Matrix4f translationMatrix(float x, float y, float z);

///creates a matrix to transform normals from object space to eye space while maintaining their perpindicularity to the surface
Eigen::Matrix3f normalMatrix(const Eigen::Matrix4f& modelViewMatrix);

#endif


#ifdef VIRTUOSO_TRANSFORMATIONSLIB_IMPLEMENTATION
#include <stdexcept>
#include <cmath>

Eigen::Matrix4f perspectiveProjectionInfinite(float fovDegrees, float aspect, float zNear)
{
    const float pi = (float)M_PI;
    const float fovRads = fovDegrees * pi / 180.0f;

    const float epsilon = 2.4e-7f;

    Eigen::Matrix4f temp;

    float f = 1.0f / tan(fovRads * .5f);

    temp.col(0) = Eigen::Vector4f(f / aspect, 0.0f, 0.0f, 0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f, f, 0.0f, 0.0f);
    temp.col(2) = Eigen::Vector4f(0.0f, 0.0f, epsilon-1.0f, -1.0f);
    temp.col(3) = Eigen::Vector4f(0.0f, 0.0f, (epsilon - 2.0f) *zNear, 0.0f);

    return temp;
}

Eigen::Matrix4f perspectiveProjection(float fovDegrees, float aspect, float zNear, float zFar )
{
    const float pi = (float)M_PI;
    const float fovRads = fovDegrees * pi / 180.0f;

    Eigen::Matrix4f temp;

    float f = 1.0f / tan(  fovRads * .5f );

    temp.col(0) = Eigen::Vector4f(f / aspect, 0.0f, 0.0f , 0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f,  f , 0.0f  ,0.0f);
    temp.col(2) = Eigen::Vector4f(0.0f, 0.0f, (zFar + zNear) / (zNear - zFar) ,-1.0f );
    temp.col(3) = Eigen::Vector4f(0.0f, 0.0f, 2.0f*zFar * zNear / (zNear-zFar) ,0.0f);

    return temp;
}


Eigen::Matrix4f perspectiveProjection(float left, float right, float bottom, float top, float nearPlane, float farPlane)
{
    Eigen::Matrix4f rval;
    
    float n_2 = 2.0f * nearPlane;
    float width = right - left;
    float height = top-bottom;
    
    rval.col(0) = Eigen::Vector4f(n_2 / width, 0.0f, 0.0f, 0.0f);
    rval.col(1) = Eigen::Vector4f(0.0f, n_2 / height, 0.0f, 0.0f);
    rval.col(2) = Eigen::Vector4f((right + left) / width, (top + bottom) / height, -(farPlane + nearPlane) / (farPlane - nearPlane), -1.0f);
    rval.col(3) = Eigen::Vector4f(0.0f, 0.0f, -n_2 * farPlane / (farPlane - nearPlane), 0.0);
    
    return rval;
}


Eigen::Matrix4f perspectiveProjectionInfinite(float left, float right, float bottom, float top, float nearPlane)
{
    Eigen::Matrix4f rval;
    
    float n_2 = 2.0f * nearPlane;
    float width = right - left;
    float height = top-bottom;
    
    rval.col(0) = Eigen::Vector4f(n_2 / width, 0.0f, 0.0f, 0.0f);
    rval.col(1) = Eigen::Vector4f(0.0f, n_2 / height, 0.0f, 0.0f);
    rval.col(2) = Eigen::Vector4f((right + left) / width,
                                  (top + bottom) / height,
                                  -1.0f, //
                                  -1.0f);
    rval.col(3) = Eigen::Vector4f(0.0f,
                                  0.0f,
                                  -n_2,
                                  0.0);
    
    return rval;
}

// Returns a projection matrix based on the given FOV.
Eigen::Matrix4f perspectiveProjectionWithFOVAngles(float fovRadsX, float fovRadsY, const float nearPlane, const float farPlane)
{
    const float halfWidth = nearPlane * tanf(.5f * fovRadsX);
    const float halfHeight = nearPlane * tanf(.5f * fovRadsY);
    
    const float minX = -halfWidth;
    const float maxX = halfWidth;
    
    const float minY = -halfHeight;
    const float maxY = halfHeight;
    
    return perspectiveProjection( minX, maxX, minY, maxY, nearPlane, farPlane);
}

// Returns a projection matrix based on the given FOV.
Eigen::Matrix4f perspectiveProjectionInfiniteWithFOVAngles(float fovRadsX, float fovRadsY, const float nearPlane)
{
    const float halfWidth = nearPlane * tanf(.5f * fovRadsX);
    const float halfHeight = nearPlane * tanf(.5f * fovRadsY);
    
    const float minX = -halfWidth;
    const float maxX = halfWidth;
    
    const float minY = -halfHeight;
    const float maxY = halfHeight;
    
    return perspectiveProjectionInfinite( minX, maxX, minY, maxY, nearPlane);
}




Eigen::Matrix4f rotationMatrixY(float theta)
{
    Eigen::Matrix4f temp;

    temp.col(0) = Eigen::Vector4f(cos(theta), 0.0f, -sin(theta), 0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.0f);
    temp.col(2) = Eigen::Vector4f(sin(theta), 0.0f, cos(theta), 0.0f);
    temp.col(3) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);

    return temp;
}


Eigen::Matrix3f rotationMatrixY_3x3(float theta)
{
    Eigen::Matrix3f temp;

    temp.col(0) = Eigen::Vector3f(cos(theta), 0.0f, -sin(theta));
    temp.col(1) = Eigen::Vector3f(0.0f, 1.0f, 0.0f);
    temp.col(2) = Eigen::Vector3f(sin(theta), 0.0f, cos(theta));

    return temp;
}

Eigen::Matrix4f rotationMatrixZ(float theta)
{
    Eigen::Matrix4f temp;

    temp.col(0) = Eigen::Vector4f(cos(theta), sin(theta), 0.0f, 0.0f);
    temp.col(1) = Eigen::Vector4f(-sin(theta), cos(theta), 0,0);
    temp.col(2) = Eigen::Vector4f(0.0f,0.0f,1.0f,0.0f);
    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0f);

    return temp;
}

Eigen::Matrix4f rotationMatrixX(float theta)
{
    Eigen::Matrix4f temp;

    temp.col(0) = Eigen::Vector4f(1.0f,0.0f,0.0f,0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f,cos(theta), sin(theta), 0.0f);
    temp.col(2) = Eigen::Vector4f(0.0f,-sin(theta), cos(theta), 0.0f);
    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1);

    return temp;
}



Eigen::Matrix4f scalingMatrix(const Eigen::Vector3f& vec)
{
    Eigen::Matrix4f temp;
    
    temp.col(0) = Eigen::Vector4f(vec[0],0.0f,0.0f,0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f,vec[1],0.0f,0.0f);
    temp.col(2) = Eigen::Vector4f(0.0f,0.0f,vec[2],0.0f);
    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0);
    
    return temp;
}


Eigen::Matrix4f scalingMatrix(float x, float y, float z)
{
    Eigen::Matrix4f temp;
    
    temp.col(0) = Eigen::Vector4f(x,0.0f,0.0f,0.0f);
    temp.col(1) = Eigen::Vector4f(0.0f,y,0.0f,0.0f);
    temp.col(2) = Eigen::Vector4f(0.0f,0.0f,z,0.0f);
    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0);

    return temp;
}


Eigen::Matrix4f translationMatrix(const Eigen::Vector3f& trans)
{
    Eigen::Matrix4f temp = Eigen::Matrix4f::Identity();
    
    temp.col(3) = Eigen::Vector4f(trans[0],trans[1],trans[2],1.0f);
    
    return temp;
}

Eigen::Matrix4f translationMatrix(float x, float y, float z)
{
    
    Eigen::Matrix4f temp = Eigen::Matrix4f::Identity();
    
    temp.col(3) = Eigen::Vector4f(x,y,z,1.0f);
    
    return temp;
}

Eigen::Matrix3f normalMatrix( Eigen::Matrix4f& modelViewMatrix)
{
    return modelViewMatrix.topLeftCorner<3,3>().inverse().transpose();
}

Eigen::Matrix4f rotationMatrixXYZ(float rotX, float rotY, float rotZ)
{
    float sinZ = sin(rotZ);
    float sinX = sin(rotX);
    float sinY = sin(rotY);
    
    float cosZ = cos(rotZ);
    float cosX = cos(rotX);
    float cosY = cos(rotY);

    Eigen::Matrix4f temp;

    temp.col(0) = Eigen::Vector4f(cosY * cosZ,
                                  -cosY * sinZ,
                                  sinY,
                                  0.0f);
    
    temp.col(1) = Eigen::Vector4f(cosX*sinZ + sinX * sinY * cosZ,
                                  cosX*cosZ + sinX * sinY * sinZ,
                                  -sinX * cosY,
                                  0.0
                                  );
    
    temp.col(2) = Eigen::Vector4f(sinX*sinZ - cosX * sinY * cosZ,
                                  sinX*cosZ + cosX * sinY * sinZ,
                                  cosX * sinY,
                                  0.0
                                  );

    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0);

    return temp;
}


///\todo http://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
/// Eigen does this for us already, as well as conversions to / from matrices, quats, etc
///  http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html
Eigen::Matrix4f rotateAxisAngle(Eigen::Vector3f axis, float angle)
{
    
    float cosTheta = cos(angle);
    float sinTheta = sin(angle);
    float oneMinusCosTheta = 1.0f - cosTheta;
    
    Eigen::Matrix4f temp;
    
    temp.col(0) = Eigen::Vector4f(axis[0] * axis[0] * oneMinusCosTheta + cosTheta,
                                  axis[1] * axis[0] * oneMinusCosTheta + axis[2] * sinTheta,
                                  axis[2] * axis[0] * oneMinusCosTheta - axis[1] * sinTheta,
                                  0.0);
    
    temp.col(1) = Eigen::Vector4f(axis[1] * axis[0] * oneMinusCosTheta - axis[2] * sinTheta,
                                  axis[1] * axis[1] * oneMinusCosTheta + cosTheta,
                                  axis[2] * axis[1] * oneMinusCosTheta + axis[0] * sinTheta,
                                  0.0
                                );
    
    temp.col(2) = Eigen::Vector4f(axis[2] * axis[0] * oneMinusCosTheta + axis[1] * sinTheta,
                                  axis[1] * axis[2] * oneMinusCosTheta - axis[0] * sinTheta,
                                  axis[2] * axis[2] * oneMinusCosTheta + cosTheta,
                                  0.0
    );
    
    
    temp.col(3) = Eigen::Vector4f(0.0f,0.0f,0.0f,1.0);
    
    return temp;
}


Eigen::Matrix4f cameraFrameMatrix(const Eigen::Vector3f& lookVector, const Eigen::Vector3f& upVecIn, const Eigen::Vector3f& position)
{
    Eigen::Matrix4f temp;
    Eigen::Matrix3f rotPortion;

    //we want the camera to look down the negative z axis (rhs)
    rotPortion.row(2) = -(lookVector);
    rotPortion.row(2).normalize();

    Eigen::Vector3f upVector = upVecIn;
    upVector.normalize();

    rotPortion.row(0) = upVector.cross(rotPortion.row(2));

    // guarantee orthogonality by recalculating the up
    rotPortion.row(1) = rotPortion.row(2).cross(rotPortion.row(0));
    
    
    ///\todo
    rotPortion.row(0).normalize();
    rotPortion.row(1).normalize();
    rotPortion.row(2).normalize();
    
    
    Eigen::Vector3f transPortion = -(rotPortion * position);

    
    temp.topLeftCorner<3, 3>() = rotPortion;
    temp.row(3) = Eigen::Vector4f(0, 0, 0, 1);
    temp.topLeftCorner<3, 4>().col(3) = transPortion;

    return temp;
}
#endif