From 5f7f3858a8e3586b6db735aece9c078ede6de62d Mon Sep 17 00:00:00 2001
From: scalinon <sylvain.calinon@idiap.ch>
Date: Mon, 30 Dec 2019 09:55:19 +0100
Subject: [PATCH] Ergodic control examples added

---
 CMakeLists.txt                             |  266 +--
 README.md                                  |    2 +-
 include/ImGuizmo.h                         |  173 ++
 include/gfx.h                              |  187 ++
 include/gfx2.h                             |  292 +--
 include/gfx3.h                             |  675 +++++++
 include/gfx_common.h                       |  360 ++++
 include/gl2ps.h                            |   54 +-
 include/imgui_impl_glfw_gl3.h              |   31 +
 include/tensor.h                           |  149 +-
 include/window_utils.h                     |  106 -
 src/demo_GMR01.cpp                         |   25 +-
 src/demo_GPR01.cpp                         |   25 +-
 src/demo_HSMM_batchLQR01.cpp               |   40 +-
 src/demo_LWR_batch01.cpp                   |   49 +-
 src/demo_LWR_iterative01.cpp               |   49 +-
 src/demo_MPC_batch01.cpp                   |   65 +-
 src/demo_MPC_iterative01.cpp               |   66 +-
 src/demo_MPC_semitied01.cpp                |   50 +-
 src/demo_MPC_velocity01.cpp                |  103 +-
 src/demo_Riemannian_cov_GMR01.cpp          |   21 +-
 src/demo_Riemannian_cov_GMR_Mandel01.cpp   |   19 +-
 src/demo_Riemannian_cov_interp02.cpp       |   56 +-
 src/demo_Riemannian_pose_GMM01.cpp         |    6 +-
 src/demo_Riemannian_pose_TPGMM01.cpp       |    6 +-
 src/demo_Riemannian_pose_batchLQR01.cpp    |   42 +-
 src/demo_Riemannian_pose_infHorLQR01.cpp   |   40 +-
 src/demo_Riemannian_quat_TPGMM01.cpp       |   53 +-
 src/demo_Riemannian_quat_infHorLQR01.cpp   |   49 +-
 src/demo_Riemannian_sphere_GMM01.cpp       |   25 +-
 src/demo_Riemannian_sphere_TPGMM01.cpp     |   24 +-
 src/demo_Riemannian_sphere_infHorLQR01.cpp |   20 +-
 src/demo_Riemannian_sphere_product01.cpp   |   22 +-
 src/demo_TPGMMProduct01.cpp                |   42 +-
 src/demo_TPGMR01.cpp                       |   41 +-
 src/demo_TPbatchLQR01.cpp                  |   37 +-
 src/demo_online_GMM01.cpp                  |   53 +-
 src/demo_online_HSMM01.cpp                 |   65 +-
 src/demo_proMP01.cpp                       |   41 +-
 src/utils/ImGuizmo.cpp                     | 1985 +++++++++++++++++++
 src/utils/gfx.cpp                          |  909 +++++++++
 src/utils/gfx2.cpp                         |  490 +----
 src/utils/gfx3.cpp                         | 2036 ++++++++++++++++++++
 src/utils/gfx_common.cpp                   |  652 +++++++
 src/utils/gl2ps.c                          |  506 ++++-
 src/utils/imgui_impl_glfw_gl3.cpp          |  530 +++++
 46 files changed, 8782 insertions(+), 1755 deletions(-)
 create mode 100644 include/ImGuizmo.h
 create mode 100644 include/gfx.h
 create mode 100644 include/gfx3.h
 create mode 100644 include/gfx_common.h
 create mode 100644 include/imgui_impl_glfw_gl3.h
 delete mode 100644 include/window_utils.h
 create mode 100644 src/utils/ImGuizmo.cpp
 create mode 100644 src/utils/gfx.cpp
 create mode 100644 src/utils/gfx3.cpp
 create mode 100644 src/utils/gfx_common.cpp
 create mode 100644 src/utils/imgui_impl_glfw_gl3.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9cb27be..56a0295 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-project(pbdlib_gui)
+project(pbdlib_cpp)
 
 set(CMAKE_MAJOR_VERSION 1)
 set(CMAKE_MINOR_VERSION 0)
@@ -9,22 +9,60 @@ set(CMAKE_PATCH_VERSION 0)
 
 # Dependencies
 find_package(OpenGL REQUIRED)
-find_package(Armadillo 5.4 REQUIRED)
-find_package(GLEW REQUIRED)
+
+if(NOT WIN32)
+	find_package(GLEW REQUIRED)
+	find_package(Armadillo 5.4 REQUIRED)
+else()
+	set(ARMADILLO_DIR "ARMADILLO_DIR-NOTFOUND" CACHE PATH "Path to Armadillo")
+	set(GLEW_DIR "GLEW_DIR-NOTFOUND" CACHE PATH "Path to GLEW")
+	set(GLFW_DIR "GLFW_DIR-NOTFOUND" CACHE PATH "Path to GLFW")
+
+	if (NOT ${ARMADILLO_DIR} STREQUAL "ARMADILLO_DIR-NOTFOUND")
+		set(ARMADILLO_INCLUDE_DIR "${ARMADILLO_DIR}/include")
+		set(BLAS_LIB "${ARMADILLO_DIR}/examples/lib_win64/blas_win64_MT.lib")
+		set(LAPACK_LIB "${ARMADILLO_DIR}/examples/lib_win64/lapack_win64_MT.lib")
+
+		FILE(COPY "${ARMADILLO_DIR}/examples/lib_win64/blas_win64_MT.dll" DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/)
+		FILE(COPY "${ARMADILLO_DIR}/examples/lib_win64/lapack_win64_MT.dll" DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/)
+	else()
+		message(FATAL_ERROR "Please indicate the path to the following dependencies: Armadillo, GLEW and GLFW")
+	endif()
+
+	if (NOT ${GLEW_DIR} STREQUAL "GLEW_DIR-NOTFOUND")
+		set(GLEW_INCLUDE_DIR "${GLEW_DIR}/include")
+		set(GLEW_LIBRARIES "${GLEW_DIR}/lib/Release/x64/glew32s.lib")
+	else()
+		message(FATAL_ERROR "Please indicate the path to the following dependencies: Armadillo, GLEW and GLFW")
+	endif()
+
+	if (NOT ${GLFW_DIR} STREQUAL "GLFW_DIR-NOTFOUND")
+		set(GLFW_INCLUDE_DIR "${GLFW_DIR}/include")
+		set(GLFW_LIB "${GLFW_DIR}/lib-vc2015/glfw3.lib")
+	else()
+		message(FATAL_ERROR "Please indicate the path to the following dependencies: Armadillo, GLEW and GLFW")
+	endif()
+endif()
 
 
 if(APPLE)
 	set(ARMADILLO_LIBRARIES
 		${ARMADILLO_LIBRARIES}
 		lapack
-		blas)
+		blas
+	)
 
 	include_directories(
 		/usr/local/include
 	)
 
+	if(EXISTS /usr/local/Frameworks)
+		link_directories(
+			/usr/local/Frameworks
+		)
+	endif()
+
 	link_directories(
-		/usr/local/Frameworks
 		/usr/local/lib
 	)
 
@@ -35,7 +73,7 @@ if(APPLE)
 	set(GLFW_LIB
 		${GLFW_LIB}
 		"-framework CoreFoundation"
-		"-framework OpenGL"	
+		"-framework OpenGL" 
 		"-framework Cocoa"
 		"-framework AppKit"
 		"-framework CoreVideo"
@@ -45,6 +83,9 @@ if(APPLE)
 		"-framework GLUT"
 	)
 
+	# OpenGL is deprecated since macOS 10.14, we need this to silence the warnings
+	add_definitions(-DGL_SILENCE_DEPRECATION)
+
 elseif(UNIX)
 
 	set(GLFWLIB_SEARCH_PATH /usr/lib/x86_64-linux-gnu/ /usr/local/lib/)
@@ -58,11 +99,11 @@ include(CheckCXXCompilerFlag)
 check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
 check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 if(COMPILER_SUPPORTS_CXX11)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 elseif(COMPILER_SUPPORTS_CXX0X)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
 else()
-    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+	message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
 
@@ -74,10 +115,42 @@ FILE(COPY ${PROJECT_SOURCE_DIR}/data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/)
 include_directories(
 	${PROJECT_SOURCE_DIR}/include/
 	${PROJECT_SOURCE_DIR}/include/arpack-arma/include
-	${ARMADILLO_INCLUDE_DIRS}
+	${ARMADILLO_INCLUDE_DIR}
+	${GLEW_INCLUDE_DIR}
+)
+
+
+if(WIN32)
+	include_directories(
+		${GLFW_INCLUDE_DIR}
+	)
+endif()
+
+
+# Setup the libraries to link
+if(WIN32)
+	set(ARMADILLO_LIBRARIES
+		${ARMADILLO_LIBRARIES}
+		${BLAS_LIB}
+		${LAPACK_LIB}
+	)
+endif()
+
+set(LIBRARIES
+	${OPENGL_LIBRARIES}
+	${GLEW_LIBRARIES}
+	${GLFW_LIB}
+	${ARMADILLO_LIBRARIES}
 )
 
 
+# Additional flags needed for Visual Studio
+if(MSVC)
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4244 /wd4267")
+	set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /NODEFAULTLIB:LIBCMT /NODEFAULTLIB:MSVCRT")
+endif()
+
+
 # Declare the support files needed for OpenGL2 demos
 set(GL2_SUPPORT_SOURCES
 	${PROJECT_SOURCE_DIR}/src/utils/imgui_impl_glfw_gl2.cpp
@@ -87,6 +160,13 @@ set(GL2_SUPPORT_SOURCES
 )
 
 
+# Declare the support files needed for OpenGL2 demos (using gfx.h)
+set(GFX_SUPPORT_SOURCES
+	${GL2_SUPPORT_SOURCES}
+	${PROJECT_SOURCE_DIR}/src/utils/gfx.cpp
+)
+
+
 # Declare the support files needed for OpenGL2 (using gfx2.h) demos
 set(GFX2_SUPPORT_SOURCES
 	${GL2_SUPPORT_SOURCES}
@@ -94,6 +174,17 @@ set(GFX2_SUPPORT_SOURCES
 )
 
 
+# Declare the support files needed for OpenGL3 (using GLSL & gfx3.h) demos
+set(GFX3_SUPPORT_SOURCES
+	${PROJECT_SOURCE_DIR}/src/utils/imgui_impl_glfw_gl3.cpp
+	${PROJECT_SOURCE_DIR}/src/utils/imgui.cpp
+	${PROJECT_SOURCE_DIR}/src/utils/imgui_draw.cpp
+	${PROJECT_SOURCE_DIR}/src/utils/gfx_ui.cpp
+	${PROJECT_SOURCE_DIR}/src/utils/gfx3.cpp
+)
+
+
+
 # Executables
 add_executable(demo_ergodicControl_2D01
 	${GFX2_SUPPORT_SOURCES}
@@ -119,10 +210,7 @@ add_executable(demo_GMR01
 	${PROJECT_SOURCE_DIR}/src/demo_GMR01.cpp
 )
 target_link_libraries(demo_GMR01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_GPR01
@@ -130,22 +218,15 @@ add_executable(demo_GPR01
 	${PROJECT_SOURCE_DIR}/src/demo_GPR01.cpp
 )
 target_link_libraries(demo_GPR01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_HSMM_batchLQR01
 	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_HSMM_batchLQR01.cpp
 )
-
 target_link_libraries(demo_HSMM_batchLQR01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
-	${GLEW_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_LWR_batch01
@@ -153,10 +234,7 @@ add_executable(demo_LWR_batch01
 	${PROJECT_SOURCE_DIR}/src/demo_LWR_batch01.cpp
 )
 target_link_libraries(demo_LWR_batch01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_LWR_iterative01
@@ -164,10 +242,7 @@ add_executable(demo_LWR_iterative01
 	${PROJECT_SOURCE_DIR}/src/demo_LWR_iterative01.cpp
 )
 target_link_libraries(demo_LWR_iterative01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_MPC_batch01
@@ -176,12 +251,8 @@ add_executable(demo_MPC_batch01
 	${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
 	${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
 )
-
 target_link_libraries(demo_MPC_batch01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
-	${GLEW_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_MPC_iterative01
@@ -189,12 +260,8 @@ add_executable(demo_MPC_iterative01
 	${PROJECT_SOURCE_DIR}/src/demo_MPC_iterative01.cpp
 	${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
 )
-
 target_link_libraries(demo_MPC_iterative01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
-	${GLEW_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_MPC_semitied01
@@ -204,10 +271,7 @@ add_executable(demo_MPC_semitied01
 	${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
 )
 target_link_libraries(demo_MPC_semitied01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
-	${GLEW_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_MPC_velocity01
@@ -217,30 +281,23 @@ add_executable(demo_MPC_velocity01
 	${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
 )
 target_link_libraries(demo_MPC_velocity01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
-	${GLEW_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_online_GMM01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_online_GMM01.cpp
 )
 target_link_libraries(demo_online_GMM01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_online_HSMM01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_online_HSMM01.cpp
 )
 target_link_libraries(demo_online_HSMM01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_proMP01
@@ -248,10 +305,7 @@ add_executable(demo_proMP01
 	${PROJECT_SOURCE_DIR}/src/demo_proMP01.cpp
 )
 target_link_libraries(demo_proMP01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_cov_GMR_Mandel01
@@ -259,10 +313,7 @@ add_executable(demo_Riemannian_cov_GMR_Mandel01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_cov_GMR_Mandel01.cpp
 )
 target_link_libraries(demo_Riemannian_cov_GMR_Mandel01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_cov_GMR01
@@ -270,10 +321,7 @@ add_executable(demo_Riemannian_cov_GMR01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_cov_GMR01.cpp
 )
 target_link_libraries(demo_Riemannian_cov_GMR01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_cov_interp02
@@ -281,20 +329,15 @@ add_executable(demo_Riemannian_cov_interp02
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_cov_interp02.cpp
 )
 target_link_libraries(demo_Riemannian_cov_interp02
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_pose_batchLQR01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_batchLQR01.cpp
 )
 target_link_libraries(demo_Riemannian_pose_batchLQR01
-	${ARMADILLO_LIBRARIES}
-	${OPENGL_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_pose_GMM01
@@ -305,13 +348,11 @@ target_link_libraries(demo_Riemannian_pose_GMM01
 )
 
 add_executable(demo_Riemannian_pose_infHorLQR01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_infHorLQR01.cpp
 )
 target_link_libraries(demo_Riemannian_pose_infHorLQR01
-	${ARMADILLO_LIBRARIES}
-	${OPENGL_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_pose_TPGMM01
@@ -322,23 +363,19 @@ target_link_libraries(demo_Riemannian_pose_TPGMM01
 )
 
 add_executable(demo_Riemannian_quat_infHorLQR01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_quat_infHorLQR01.cpp
 )
 target_link_libraries(demo_Riemannian_quat_infHorLQR01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_quat_TPGMM01
-	${GL2_SUPPORT_SOURCES}
+	${GFX2_SUPPORT_SOURCES}
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_quat_TPGMM01.cpp
 )
 target_link_libraries(demo_Riemannian_quat_TPGMM01
-	${ARMADILLO_LIBRARIES}
-	${OPENGL_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_sphere_GMM01
@@ -346,10 +383,7 @@ add_executable(demo_Riemannian_sphere_GMM01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_GMM01.cpp
 )
 target_link_libraries(demo_Riemannian_sphere_GMM01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_sphere_infHorLQR01
@@ -357,10 +391,7 @@ add_executable(demo_Riemannian_sphere_infHorLQR01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_infHorLQR01.cpp
 )
 target_link_libraries(demo_Riemannian_sphere_infHorLQR01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_sphere_product01
@@ -368,10 +399,7 @@ add_executable(demo_Riemannian_sphere_product01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_product01.cpp
 )
 target_link_libraries(demo_Riemannian_sphere_product01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_Riemannian_sphere_TPGMM01
@@ -379,10 +407,7 @@ add_executable(demo_Riemannian_sphere_TPGMM01
 	${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_TPGMM01.cpp
 )
 target_link_libraries(demo_Riemannian_sphere_TPGMM01
-	${OPENGL_LIBRARIES}
-	${ARMADILLO_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
+	${LIBRARIES}
 )
 
 add_executable(demo_TPbatchLQR01
@@ -390,30 +415,21 @@ add_executable(demo_TPbatchLQR01
 	${PROJECT_SOURCE_DIR}/src/demo_TPbatchLQR01.cpp
 )
 target_link_libraries(demo_TPbatchLQR01
-	${OPENGL_LIBRARIES}
-	${GLEW_LIBRARIES}
-	${GLFW_LIB}
-	${ARMADILLO_LIBRARIES}
+	${LIBRARIES}
 )
 
 add_executable(demo_TPGMMProduct01
-		${GFX2_SUPPORT_SOURCES}
-		${PROJECT_SOURCE_DIR}/src/demo_TPGMMProduct01.cpp
-		)
+	${GFX2_SUPPORT_SOURCES}
+	${PROJECT_SOURCE_DIR}/src/demo_TPGMMProduct01.cpp
+)
 target_link_libraries(demo_TPGMMProduct01
-		${OPENGL_LIBRARIES}
-		${GLEW_LIBRARIES}
-		${GLFW_LIB}
-		${ARMADILLO_LIBRARIES}
-		)
+	${LIBRARIES}
+)
 
 add_executable(demo_TPGMR01
-		${GFX2_SUPPORT_SOURCES}
-		${PROJECT_SOURCE_DIR}/src/demo_TPGMR01.cpp
-		)
+	${GFX2_SUPPORT_SOURCES}
+	${PROJECT_SOURCE_DIR}/src/demo_TPGMR01.cpp
+)
 target_link_libraries(demo_TPGMR01
-		${OPENGL_LIBRARIES}
-		${GLEW_LIBRARIES}
-		${GLFW_LIB}
-		${ARMADILLO_LIBRARIES}
-		)
+	${LIBRARIES}
+)
diff --git a/README.md b/README.md
index 3d4924f..ab46135 100644
--- a/README.md
+++ b/README.md
@@ -7,7 +7,7 @@
 
 ### Usage
 
-This project builds a number of executables in the `bin` folder, with the corresponding C++ source codes in the `src` folder listed in the table below. The corresponding publications are also listed below. 
+This project builds a number of executables in the `build` folder, with the corresponding C++ source codes in the `src` folder listed in the table below. The corresponding publications are also listed below. 
 
 
 ### List of examples
diff --git a/include/ImGuizmo.h b/include/ImGuizmo.h
new file mode 100644
index 0000000..08da1c5
--- /dev/null
+++ b/include/ImGuizmo.h
@@ -0,0 +1,173 @@
+// https://github.com/CedricGuillemet/ImGuizmo
+// v 1.61 WIP
+//
+// The MIT License(MIT)
+// 
+// Copyright(c) 2016 Cedric Guillemet
+// 
+// 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.
+//
+// -------------------------------------------------------------------------------------------
+// History : 
+// 2016/09/11 Behind camera culling. Scaling Delta matrix not multiplied by source matrix scales. local/world rotation and translation fixed. Display message is incorrect (X: ... Y:...) in local mode.
+// 2016/09/09 Hatched negative axis. Snapping. Documentation update.
+// 2016/09/04 Axis switch and translation plan autohiding. Scale transform stability improved
+// 2016/09/01 Mogwai changed to Manipulate. Draw debug cube. Fixed inverted scale. Mixing scale and translation/rotation gives bad results.
+// 2016/08/31 First version
+//
+// -------------------------------------------------------------------------------------------
+// Future (no order):
+//
+// - Multi view
+// - display rotation/translation/scale infos in local/world space and not only local
+// - finish local/world matrix application
+// - OPERATION as bitmask
+// 
+// -------------------------------------------------------------------------------------------
+// Example 
+#if 0
+void EditTransform(const Camera& camera, matrix_t& matrix)
+{
+	static ImGuizmo::OPERATION mCurrentGizmoOperation(ImGuizmo::ROTATE);
+	static ImGuizmo::MODE mCurrentGizmoMode(ImGuizmo::WORLD);
+	if (ImGui::IsKeyPressed(90))
+		mCurrentGizmoOperation = ImGuizmo::TRANSLATE;
+	if (ImGui::IsKeyPressed(69))
+		mCurrentGizmoOperation = ImGuizmo::ROTATE;
+	if (ImGui::IsKeyPressed(82)) // r Key
+		mCurrentGizmoOperation = ImGuizmo::SCALE;
+	if (ImGui::RadioButton("Translate", mCurrentGizmoOperation == ImGuizmo::TRANSLATE))
+		mCurrentGizmoOperation = ImGuizmo::TRANSLATE;
+	ImGui::SameLine();
+	if (ImGui::RadioButton("Rotate", mCurrentGizmoOperation == ImGuizmo::ROTATE))
+		mCurrentGizmoOperation = ImGuizmo::ROTATE;
+	ImGui::SameLine();
+	if (ImGui::RadioButton("Scale", mCurrentGizmoOperation == ImGuizmo::SCALE))
+		mCurrentGizmoOperation = ImGuizmo::SCALE;
+	float matrixTranslation[3], matrixRotation[3], matrixScale[3];
+	ImGuizmo::DecomposeMatrixToComponents(matrix.m16, matrixTranslation, matrixRotation, matrixScale);
+	ImGui::InputFloat3("Tr", matrixTranslation, 3);
+	ImGui::InputFloat3("Rt", matrixRotation, 3);
+	ImGui::InputFloat3("Sc", matrixScale, 3);
+	ImGuizmo::RecomposeMatrixFromComponents(matrixTranslation, matrixRotation, matrixScale, matrix.m16);
+
+	if (mCurrentGizmoOperation != ImGuizmo::SCALE)
+	{
+		if (ImGui::RadioButton("Local", mCurrentGizmoMode == ImGuizmo::LOCAL))
+			mCurrentGizmoMode = ImGuizmo::LOCAL;
+		ImGui::SameLine();
+		if (ImGui::RadioButton("World", mCurrentGizmoMode == ImGuizmo::WORLD))
+			mCurrentGizmoMode = ImGuizmo::WORLD;
+	}
+	static bool useSnap(false);
+	if (ImGui::IsKeyPressed(83))
+		useSnap = !useSnap;
+	ImGui::Checkbox("", &useSnap);
+	ImGui::SameLine();
+	vec_t snap;
+	switch (mCurrentGizmoOperation)
+	{
+	case ImGuizmo::TRANSLATE:
+		snap = config.mSnapTranslation;
+		ImGui::InputFloat3("Snap", &snap.x);
+		break;
+	case ImGuizmo::ROTATE:
+		snap = config.mSnapRotation;
+		ImGui::InputFloat("Angle Snap", &snap.x);
+		break;
+	case ImGuizmo::SCALE:
+		snap = config.mSnapScale;
+		ImGui::InputFloat("Scale Snap", &snap.x);
+		break;
+	}
+	ImGuiIO& io = ImGui::GetIO();
+	ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
+	ImGuizmo::Manipulate(camera.mView.m16, camera.mProjection.m16, mCurrentGizmoOperation, mCurrentGizmoMode, matrix.m16, NULL, useSnap ? &snap.x : NULL);
+}
+#endif
+#pragma once
+
+#ifdef USE_IMGUI_API
+#include "imconfig.h"
+#endif
+#ifndef IMGUI_API
+#define IMGUI_API
+#endif
+
+namespace ImGuizmo
+{
+	// call inside your own window and before Manipulate() in order to draw gizmo to that window.
+	IMGUI_API void SetDrawlist();
+
+	// call BeginFrame right after ImGui_XXXX_NewFrame();
+	IMGUI_API void BeginFrame();
+
+	// return true if mouse cursor is over any gizmo control (axis, plan or screen component)
+	IMGUI_API bool IsOver();
+
+	// return true if mouse IsOver or if the gizmo is in moving state
+	IMGUI_API bool IsUsing();
+
+	// enable/disable the gizmo. Stay in the state until next call to Enable.
+	// gizmo is rendered with gray half transparent color when disabled
+	IMGUI_API void Enable(bool enable);
+
+	// helper functions for manualy editing translation/rotation/scale with an input float
+	// translation, rotation and scale float points to 3 floats each
+	// Angles are in degrees (more suitable for human editing)
+	// example:
+	// float matrixTranslation[3], matrixRotation[3], matrixScale[3];
+	// ImGuizmo::DecomposeMatrixToComponents(gizmoMatrix.m16, matrixTranslation, matrixRotation, matrixScale);
+	// ImGui::InputFloat3("Tr", matrixTranslation, 3);
+	// ImGui::InputFloat3("Rt", matrixRotation, 3);
+	// ImGui::InputFloat3("Sc", matrixScale, 3);
+	// ImGuizmo::RecomposeMatrixFromComponents(matrixTranslation, matrixRotation, matrixScale, gizmoMatrix.m16);
+	//
+	// These functions have some numerical stability issues for now. Use with caution.
+	IMGUI_API void DecomposeMatrixToComponents(const float *matrix, float *translation, float *rotation, float *scale);
+	IMGUI_API void RecomposeMatrixFromComponents(const float *translation, const float *rotation, const float *scale, float *matrix);
+
+	IMGUI_API void SetRect(float x, float y, float width, float height);
+	// default is false
+	IMGUI_API void SetOrthographic(bool isOrthographic);
+
+	// Render a cube with face color corresponding to face normal. Usefull for debug/tests
+	IMGUI_API void DrawCube(const float *view, const float *projection, const float *matrix);
+	IMGUI_API void DrawGrid(const float *view, const float *projection, const float *matrix, const float gridSize);
+
+	// call it when you want a gizmo
+	// Needs view and projection matrices. 
+	// matrix parameter is the source matrix (where will be gizmo be drawn) and might be transformed by the function. Return deltaMatrix is optional
+	// translation is applied in world space
+	enum OPERATION
+	{
+		TRANSLATE,
+		ROTATE,
+		SCALE,
+		BOUNDS,
+	};
+
+	enum MODE
+	{
+		LOCAL,
+		WORLD
+	};
+
+	IMGUI_API void Manipulate(const float *view, const float *projection, OPERATION operation, MODE mode, float *matrix, float *deltaMatrix = 0, float *snap = 0, float *localBounds = NULL, float *boundsSnap = NULL);
+};
diff --git a/include/gfx.h b/include/gfx.h
new file mode 100644
index 0000000..918dbf6
--- /dev/null
+++ b/include/gfx.h
@@ -0,0 +1,187 @@
+// Minimalist OpenGL utilities
+// adapted from https://github.com/colormotor/colormotor
+
+#pragma once
+
+#ifdef _WIN32
+	#define _USE_MATH_DEFINES
+#endif
+
+#include <stdio.h>
+#include <stdarg.h>
+#include <math.h>
+#include <time.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include <iostream>
+#include <vector>
+#include <map>
+#include <string>
+#include <sstream>	//for ostringsream
+#include <iomanip>	//for setprecision
+#include <errno.h>
+#include <functional>
+#include <fstream>
+#include <thread>
+#include <complex>
+
+#include "armadillo"
+
+// Detect platform
+#ifdef _WIN32
+	#define GFX_WINDOWS
+	#define NOMINMAX
+	#include <windows.h>
+#elif __APPLE__
+	#define GFX_OSX
+#elif __linux__
+	#define GFX_LINUX
+#elif __unix__
+	#define GFX_LINUX
+#elif defined(_POSIX_VERSION)
+	#define GFX_LINUX
+#else
+	#error "Unknown platform"
+#endif
+
+// GL includes
+#ifdef GFX_WINDOWS
+	#include <windows.h>
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#define GLEW_STATIC
+		#include <GL/glew.h>
+	#endif
+	#include <GL/glu.h>
+	#ifndef GL_CLAMP_TO_EDGE
+		#define GL_CLAMP_TO_EDGE 0x812F
+	#endif
+#elif defined GFX_LINUX
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#include <GL/glew.h>
+		#include <GL/glu.h>
+		#include <GL/gl.h>
+		#include <GL/glx.h>
+	#endif
+#elif defined GFX_OSX
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#include <GL/glew.h>
+	#endif
+	#include <OpenGL/glu.h>
+	#include <OpenGL/OpenGL.h>
+	#include <OpenGL/gl.h>
+	#include <OpenGL/glext.h>
+#endif
+
+namespace gfx
+{
+
+void init();
+
+int checkGlExtension(char const *name);
+
+//////////////////////////////////////////////////
+/// Projections/view/transformations
+//@{
+/// Returns a perspective projection matrix
+arma::fmat perspective(float fovy, float aspect, float zNear, float zFar);
+/// Sets perspective projection (as in MESA standard)
+void setPerspectiveProjection(float fovy, float aspect, float zNear, float zFar);
+/// Set orthographic projection with origin at (0,0) top-left
+void setOrtho( float w, float h );
+/// Wrapper around glMultMatrix with armadillo matrices
+/// handles 2d and 3d affine matrices (3x3 and 4x4)
+void multMatrix( const arma::mat& mat );
+/// Returns a view (or camera) matrix
+arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target, const arma::fvec& up);
+/// Returns a rotation matrix for the provided axis and angle
+arma::fmat rotate(const arma::fvec& axis, float angle);
+//@}
+
+//////////////////////////////////////////////////
+/// Drawing
+//@{
+/// Draw a 2d quad providing texture coordinates (u,v)
+void drawUVQuad( float x  , float y	 , float w , float h, float maxU=0.0f, float maxV=1.0f, bool flip=true );
+/// Draw a 2d quad
+void drawQuad( float x, float y, float w, float h );
+/// Draw a line between a and b
+void drawLine( const arma::vec& a, const arma::vec& b );
+/// draw a set of points (columns of the matrix) with a given GL primitive defined with @prim,
+/// e.g. GL_LINE_STRIP, GL_LINE_LOOP, GL_POINTS
+void draw( const arma::mat & P, int prim=GL_LINE_STRIP );
+/// Draw 3d axes defined by a 4x4 matrix (have to test here)
+void drawAxis( const arma::mat& mat, float scale );
+//@}
+
+//////////////////////////////////////
+/// Shader interface (needs further testing)
+//@{
+/// Convert to string (handy to create a shader, but messes up line numbers, which makes it tricky with errors)
+#define STRINGIFY( expr ) #expr
+
+void removeShader( int id );
+void deleteShaderProgram( int id );
+void deleteAllShaders();
+
+void setShaderVersion( const std::string& version );
+
+int loadShader( const std::string& vs, const std::string& ps );
+int reloadShader( int id, const std::string& vs, const std::string& ps );
+std::string shaderString( const std::string& path );
+bool setTexture( const std::string& handle, int sampler );
+void bindShader( int id );
+void unbindShader();
+
+bool setInt( const std::string& handle, int v );
+bool setBool( const std::string& handle, bool v );
+bool setVector( const std::string& handle, const arma::vec& v );
+bool setMatrix( const std::string& handle, const arma::mat& v );
+//@}
+
+//////////////////////////////////////
+/// Texture interface
+/// Example combinations of parameters:
+///	   { GL_RGB8, GL_BGR, GL_UNSIGNED_BYTE,3 }, //R8G8B8,
+///	   { GL_RGBA8, GL_BGRA, GL_UNSIGNED_BYTE,4 }, //A8R8G8B8
+///	   { GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,1 }, //L8,
+//@{
+int createTexture(int w, int h, int glFormat=GL_RGB8, int dataFormat=GL_BGRA, int dataType=GL_UNSIGNED_BYTE );
+int createTexture(void* data, int w, int h, int glFormat=GL_RGB8, int dataFormat=GL_BGRA, int dataType=GL_UNSIGNED_BYTE );
+void bindTexture( int id, int sampler=0 );
+void unbindTexture( int sampler=0 );
+void grabFrameBuffer( int texId, int w, int h );
+//@}
+
+} // end gfx
+
+
+//////////////////////////////////////
+// OpenGL transformation matrix utils
+
+/// 3d transformations, when @affine is set to true a 4x4 matrix is created to handle homogeneous coords
+//@{
+arma::mat rotX3d( double theta, bool affine=true );
+arma::mat rotY3d( double theta, bool affine=true );
+arma::mat rotZ3d( double theta, bool affine=true );
+arma::mat trans3d( const arma::vec& xyz );
+arma::mat trans3d( double x, double y, double z );
+arma::mat scaling3d( const arma::vec& xyz, bool affine=true );
+arma::mat scaling3d( double s, bool affine=true );
+arma::mat scaling3d( double x, double y, double z, bool affine=true );
+//@}
+
+//////////////////////////////////////
+/// 2d transformations, when @affine is set to true a 3x3 matrix is created to handle homogeneous coords
+//@{
+arma::mat rot2d( double theta, bool affine=true );
+arma::mat trans2d( const arma::vec& xy, bool affine=true );
+arma::mat trans2d( double x, double y, bool affine=true );
+arma::mat scaling2d( const arma::vec& xy, bool affine=true );
+arma::mat scaling2d( double s, bool affine=true );
+arma::mat scaling2d( double x, double y, bool affine=true );
+//@}
diff --git a/include/gfx2.h b/include/gfx2.h
index a16db3b..63a2df4 100644
--- a/include/gfx2.h
+++ b/include/gfx2.h
@@ -8,6 +8,10 @@
 
 #pragma once
 
+#ifdef _WIN32
+	#define _USE_MATH_DEFINES
+#endif
+
 #define ARMA_DONT_PRINT_ERRORS
 #include <armadillo>
 #include <map>
@@ -15,6 +19,7 @@
 // Detect the platform
 #ifdef _WIN32
 	#define GFX_WINDOWS
+	#define NOMINMAX
 	#include <windows.h>
 #elif __APPLE__
 	#define GFX_OSX
@@ -26,8 +31,12 @@
 
 // OpenGL includes
 #ifdef GFX_WINDOWS
-	// NOT TESTED HERE
 	#include <windows.h>
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#define GLEW_STATIC
+		#include <GL/glew.h>
+	#endif
 	#include <GL/glu.h>
 	#ifndef GL_CLAMP_TO_EDGE
 		#define GL_CLAMP_TO_EDGE 0x812F
@@ -52,220 +61,32 @@
 #endif
 
 
+#define GFX_NAMESPACE gfx2
+#include "gfx_common.h"
+#undef GFX_NAMESPACE
+
 
 namespace gfx2
 {
-	/**************************** UTILITY FUNCTIONS **************************/
-
-	//-------------------------------------------------------------------------
-	// Holds the sizes of the window and of the OpenGL front-buffer (they might
-	// be different, for instance on a 4K screen)
-	//-------------------------------------------------------------------------
-	struct window_size_t {
-		int win_width;
-		int win_height;
-		int fb_width;
-		int fb_height;
-
-		inline float scale_x() const {
-			return (float) fb_width / (float) win_width;
-		}
-
-		inline float scale_y() const {
-			return (float) fb_height / (float) win_height;
-		}
-	};
-
-	//-------------------------------------------------------------------------
-	// Initialisations
-	//-------------------------------------------------------------------------
-	void init();
-
-	//-------------------------------------------------------------------------
-	// Convert radian to degrees
-	//-------------------------------------------------------------------------
-	double deg2rad(double deg);
-
-	//-------------------------------------------------------------------------
-	// Returns the sinus of an angle in degrees
-	//-------------------------------------------------------------------------
-	double sin_deg(double deg);
-
-	//-------------------------------------------------------------------------
-	// Returns the cosinus of an angle in degrees
-	//-------------------------------------------------------------------------
-	double cos_deg(double deg);
-
-	//-------------------------------------------------------------------------
-	// Indicates if two values are close enough
-	//-------------------------------------------------------------------------
-	bool is_close(float a, float b, float epsilon = 1e-6f);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from UI-space to OpenGL-space
-	//
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
-	//-------------------------------------------------------------------------
-	arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from UI-space to OpenGL-space
-	//
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
-	//-------------------------------------------------------------------------
-	arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
-					int fb_width, int fb_height);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from UI-space to OpenGL-space
-	//
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
-	// (fb_width / 2, -fb_height / 2)
-	//-------------------------------------------------------------------------
-	arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from UI-space to OpenGL-space
-	//
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
-	// (fb_width / 2, -fb_height / 2)
-	//-------------------------------------------------------------------------
-	arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
-							 int fb_width, int fb_height);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from OpenGL-space to UI-space
-	//
-	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
-	// (fb_width / 2, -fb_height / 2)
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	//-------------------------------------------------------------------------
-	arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
-					int fb_width, int fb_height);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from OpenGL-space to UI-space
-	//
-	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	//-------------------------------------------------------------------------
-	arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from OpenGL-space to UI-space
-	//
-	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	//-------------------------------------------------------------------------
-	arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
-							 int fb_width, int fb_height);
-
-	//-------------------------------------------------------------------------
-	// Converts some coordinates from OpenGL-space to UI-space
-	//
-	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
-	// (fb_width / 2, -fb_height / 2)
-	// UI coordinates range from (0, 0) to (win_width, win_height)
-	//-------------------------------------------------------------------------
-	arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size);
-
-
-	/*********************** PROJECTION & VIEW MATRICES **********************/
-
-	//-------------------------------------------------------------------------
-	// Returns a perspective projection matrix
-	//-------------------------------------------------------------------------
-	arma::fmat perspective(float fovy, float aspect, float zNear, float zFar);
-
-	// //-------------------------------------------------------------------------
-	// // Returns a orthographic projection matrix
-	// //-------------------------------------------------------------------------
-	// arma::fmat orthographic(float width, float height, float zNear, float zFar);
-
-	//-------------------------------------------------------------------------
-	// Returns a view matrix
-	//-------------------------------------------------------------------------
-	arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
-					  const arma::fvec& up);
-
-
-	/**************************** TRANSFORMATIONS ****************************/
+	/******************************* LIGHTNING *******************************/
 
 	//-------------------------------------------------------------------------
-	// Holds all the transformations needed for a 3D entity
-	//
-	// Can be organised in a hierarchy, where the parent transforms affect the
-	// children ones
+	// Holds all the needed informations about a point light
 	//-------------------------------------------------------------------------
-	struct transforms_t {
+	struct point_light_t {
 
 		// Constructor
-		transforms_t()
-		: parent(0)
+		point_light_t()
+		: diffuse_color({1.0f, 1.0f, 1.0f, 1.0f}),
+		  ambient_color({0.0f, 0.0f, 0.0f, 1.0f}),
+		  specular_color({1.0f, 1.0f, 1.0f, 1.0f})
 		{
-			position.zeros(3);
-			rotation.eye(4, 4);
 		}
 
-		transforms_t* parent;
-
-		arma::fvec position;
-		arma::fmat rotation;
-	};
-
-	//-------------------------------------------------------------------------
-	// Returns a rotation matrix given an axis and an angle (in radian)
-	//-------------------------------------------------------------------------
-	arma::fmat rotate(const arma::fvec& axis, float angle);
-
-	//-------------------------------------------------------------------------
-	// Returns the rotation matrix to go from one direction to another one
-	//-------------------------------------------------------------------------
-	arma::fmat rotation(const arma::fvec& from, const arma::fvec& to);
-
-	//-------------------------------------------------------------------------
-	// Compute the translation and rotation to apply to a list of 3D points A to
-	// obtain the list of 3D points B.
-	//
-	// Points are organised in columns:
-	//	[ x0 x1 x2 ...
-	//	  y0 y1 y2
-	//	  z0 z1 z2 ]
-	//-------------------------------------------------------------------------
-	void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
-							arma::fmat &rotation, arma::fvec &translation);
-
-	//-------------------------------------------------------------------------
-	// Returns the full world transformation matrix corresponding to the given
-	// transforms structure, taking all its parent hierarchy into account
-	//-------------------------------------------------------------------------
-	arma::fmat worldTransforms(const transforms_t* transforms);
-
-	//-------------------------------------------------------------------------
-	// Returns the full world position corresponding to the given transforms
-	// structure, taking all its parent hierarchy into account
-	//-------------------------------------------------------------------------
-	arma::fvec worldPosition(const transforms_t* transforms);
-
-	//-------------------------------------------------------------------------
-	// Returns the full world rotation matrix corresponding to the given
-	// transforms structure, taking all its parent hierarchy into account
-	//-------------------------------------------------------------------------
-	arma::fmat worldRotation(const transforms_t* transforms);
-
-
-	/******************************* LIGHTNING *******************************/
-
-	//-------------------------------------------------------------------------
-	// Holds all the needed informations about a point light
-	//-------------------------------------------------------------------------
-	struct point_light_t {
-		transforms_t	transforms;
-		arma::fvec		color;
+		transforms_t transforms;
+		arma::fvec   diffuse_color;
+		arma::fvec   ambient_color;
+		arma::fvec   specular_color;
 	};
 
 	//-------------------------------------------------------------------------
@@ -343,7 +164,7 @@ namespace gfx2
 	model_t create_rectangle(const arma::fvec& color, float width, float height,
 							 const arma::fvec& position = arma::zeros<arma::fvec>(3),
 							 const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-							 transforms_t* parent_transforms = 0);
+							 const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a rectangular mesh, textured (no lightning)
@@ -351,7 +172,7 @@ namespace gfx2
 	model_t create_rectangle(const texture_t& texture, float width, float height,
 							 const arma::fvec& position = arma::zeros<arma::fvec>(3),
 							 const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-							 transforms_t* parent_transforms = 0);
+							 const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a square mesh, colored (no lightning)
@@ -359,7 +180,7 @@ namespace gfx2
 	model_t create_square(const arma::fvec& color, float size,
 						  const arma::fvec& position = arma::zeros<arma::fvec>(3),
 						  const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-						  transforms_t* parent_transforms = 0);
+						  const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a sphere mesh, lighted
@@ -367,7 +188,7 @@ namespace gfx2
 	model_t create_sphere(float radius = 1.0f,
 						  const arma::fvec& position = arma::zeros<arma::fvec>(3),
 						  const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-						  transforms_t* parent_transforms = 0);
+						  const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a line mesh, colored (no lightning), from a matrix containing the
@@ -376,7 +197,7 @@ namespace gfx2
 	model_t create_line(const arma::fvec& color, const arma::mat& points,
 						const arma::fvec& position = arma::zeros<arma::fvec>(3),
 						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-						transforms_t* parent_transforms = 0,
+						const transforms_t* parent_transforms = 0,
 						bool line_strip = true);
 
 	//-------------------------------------------------------------------------
@@ -386,7 +207,7 @@ namespace gfx2
 	model_t create_line(const arma::fvec& color, const std::vector<arma::vec>& points,
 						const arma::fvec& position = arma::zeros<arma::fvec>(3),
 						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-						transforms_t* parent_transforms = 0,
+						const transforms_t* parent_transforms = 0,
 						bool line_strip = true);
 
 	//-------------------------------------------------------------------------
@@ -396,7 +217,7 @@ namespace gfx2
 	model_t create_mesh(const arma::fvec& color, const arma::mat& vertices,
 						const arma::fvec& position = arma::zeros<arma::fvec>(3),
 						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-						transforms_t* parent_transforms = 0);
+						const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a mesh representing a gaussian, colored (no lightning)
@@ -405,7 +226,7 @@ namespace gfx2
 									   const arma::mat& sigma,
 									   const arma::fvec& position = arma::zeros<arma::fvec>(3),
 									   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-									   transforms_t* parent_transforms = 0);
+									   const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Create a line mesh representing a gaussian border, colored (no lightning)
@@ -414,7 +235,7 @@ namespace gfx2
 								   const arma::mat& sigma,
 								   const arma::fvec& position = arma::zeros<arma::fvec>(3),
 								   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
-								   transforms_t* parent_transforms = 0);
+								   const transforms_t* parent_transforms = 0);
 
 	//-------------------------------------------------------------------------
 	// Release the OpenGL resources used by the model
@@ -501,49 +322,10 @@ namespace gfx2
 		return draw_gaussian(color, mu, sigma, true, false);
 	}
 
-
-	/****************************** RAY CASTING ******************************/
-
-	//-------------------------------------------------------------------------
-	// Represents a 3D ray (in world coordinates)
-	//-------------------------------------------------------------------------
-	struct ray_t {
-		arma::fvec origin;
-		arma::fvec direction;
-	};
-
-	ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
-					 const arma::fmat& view, const arma::fmat& projection,
-					 int window_width, int window_height);
-
-	bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
-					arma::fvec &result);
-
-
-	/******************************** OTHERS *********************************/
-
-	//-------------------------------------------------------------------------
-	// Returns the vertices needed to create a mesh representing the background
-	// a gaussian
-	//
-	// The result is a matrix of shape (2, nb_points * 3)
-	//-------------------------------------------------------------------------
-	arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
-											   int nb_points = 60);
-
 	//-------------------------------------------------------------------------
-	// Returns the vertices needed to create a line representing the border of
-	// a gaussian
-	//
-	// If line_strip is true:
-	//  - The result is a matrix of shape (2, nb_points)
-	//  - The rendering mode must be GL_LINE_STRIP
-	//
-	// If line_strip is false:
-	//  - The result is a matrix of shape (2, nb_points * 2)
-	//  - The rendering mode must be GL_LINES
+	// Render a 3D gaussian, colored (no lightning)
 	//-------------------------------------------------------------------------
-	arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
-										   int nb_points = 60, bool line_strip = true);
+	bool draw_gaussian_3D(const arma::fvec& color, const arma::vec& mu,
+						  const arma::mat& sigma);
 
 };
diff --git a/include/gfx3.h b/include/gfx3.h
new file mode 100644
index 0000000..e21633b
--- /dev/null
+++ b/include/gfx3.h
@@ -0,0 +1,675 @@
+/*
+ * gfx3.h
+ *
+ * Rendering utility structures and functions based on OpenGL 3.3+
+ *
+ * Authors: Philip Abbet
+ */
+
+#pragma once
+
+#ifdef _WIN32
+	#define _USE_MATH_DEFINES
+#endif
+
+#define ARMA_DONT_PRINT_ERRORS
+#include <armadillo>
+#include <map>
+
+// Detect the platform
+#ifdef _WIN32
+	#define GFX_WINDOWS
+	#define NOMINMAX
+	#include <windows.h>
+#elif __APPLE__
+	#define GFX_OSX
+#elif __linux__ || __unix__ || defined(_POSIX_VERSION)
+	#define GFX_LINUX
+#else
+	#error "Unknown platform"
+#endif
+
+// OpenGL includes
+#ifdef GFX_WINDOWS
+	#include <windows.h>
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#define GLEW_STATIC
+		#include <GL/glew.h>
+	#endif
+	#include <GL/glu.h>
+	#ifndef GL_CLAMP_TO_EDGE
+		#define GL_CLAMP_TO_EDGE 0x812F
+	#endif
+#elif defined GFX_LINUX
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#include <GL/glew.h>
+		#include <GL/glu.h>
+		#include <GL/gl.h>
+		#include <GL/glx.h>
+	#endif
+#elif defined GFX_OSX
+	#ifndef __glew_h__
+		#define GL_GLEXT_PROTOTYPES
+		#include <GL/glew.h>
+	#endif
+	#include <OpenGL/glu.h>
+	#include <OpenGL/OpenGL.h>
+	#include <OpenGL/gl.h>
+	#include <OpenGL/glext.h>
+#endif
+
+
+#define GFX_NAMESPACE gfx3
+#include "gfx_common.h"
+#undef GFX_NAMESPACE
+
+
+namespace gfx3
+{
+	/**************************** UTILITY FUNCTIONS **************************/
+
+	//-------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to shader-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// Shader coordinates range from (sh_left, sh_top) to (sh_right, sh_bottom)
+	// by default: (-1, 1) to (1, -1)
+	//-------------------------------------------------------------------------
+	arma::vec ui2shader(const arma::vec& coords, int win_width, int win_height,
+						int fb_width, int fb_height, float sh_left = -1.0f,
+						float sh_top = 1.0f, float sh_right = 1.0f,
+						float sh_bottom = -1.0f);
+
+	//-------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to shader-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// Shader coordinates range from (sh_left, sh_top) to (sh_right, sh_bottom)
+	// by default: (-1, 1) to (1, -1)
+	//-------------------------------------------------------------------------
+	arma::vec ui2shader(const arma::vec& coords, const window_size_t& window_size,
+						float sh_left = -1.0f, float sh_top = 1.0f,
+						float sh_right = 1.0f, float sh_bottom = -1.0f);
+
+	//-------------------------------------------------------------------------
+	// Converts some coordinates from OpenGL-space to shader-space
+	//
+	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+	// (fb_width / 2, -fb_height / 2)
+	// Shader coordinates range from (sh_left, sh_top) to (sh_right, sh_bottom)
+	// by default: (-1, 1) to (1, -1)
+	//-------------------------------------------------------------------------
+	arma::vec fb2shader_centered(const arma::vec& coords, const window_size_t& window_size,
+								 float sh_left = -1.0f, float sh_top = 1.0f,
+								 float sh_right = 1.0f, float sh_bottom = -1.0f);
+
+
+	/******************************** SHADERS ********************************/
+
+	struct shader_fmat_uniform_t {
+		GLint	   handle;
+		arma::fmat value;
+	};
+
+
+	struct shader_fvec_uniform_t {
+		GLint	   handle;
+		arma::fvec value;
+	};
+
+
+	struct shader_float_uniform_t {
+		GLint handle;
+		float value;
+	};
+
+
+	struct shader_bool_uniform_t {
+		GLint handle;
+		bool value;
+	};
+
+
+	struct shader_int_uniform_t {
+		GLint handle;
+		int value;
+	};
+
+
+	struct shader_texture_uniform_t {
+		GLint handle;
+		GLuint texture;
+		int nb_dimensions;
+	};
+
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a GLSL program
+	//-------------------------------------------------------------------------
+	struct shader_t {
+		GLuint		id;
+
+		bool		use_lightning;
+
+		// Matrices
+		GLint		model_matrix_handle;
+		GLint		view_matrix_handle;
+		GLint		projection_matrix_handle;
+
+		// Material
+		GLint		ambiant_color_handle;	// valid if lightning is used
+		GLint		diffuse_color_handle;
+		GLint		specular_color_handle;	// valid if lightning is used
+		GLint		specular_power_handle;	// valid if lightning is used
+
+		// Textures
+		GLint		diffuse_texture_handle; // valid if textures are used
+
+		// Light
+		GLint		light_position_handle;	// valid if lightning is used
+		GLint		light_color_handle;		// valid if lightning is used
+		GLint		light_power_handle;		// valid if lightning is used
+
+		// RTT-specific
+		GLint		backbuffer_handle;
+
+		// Application-dependant uniforms
+		std::map<std::string, shader_fmat_uniform_t>    fmat_uniforms;
+		std::map<std::string, shader_fvec_uniform_t>    fvec_uniforms;
+		std::map<std::string, shader_float_uniform_t>   float_uniforms;
+		std::map<std::string, shader_bool_uniform_t>    bool_uniforms;
+		std::map<std::string, shader_int_uniform_t>     int_uniforms;
+		std::map<std::string, shader_texture_uniform_t> texture_uniforms;
+
+		void setUniform(const std::string& name, const arma::fmat& value);
+		void setUniform(const std::string& name, const arma::mat& value);
+		void setUniform(const std::string& name, const arma::fvec& value);
+		void setUniform(const std::string& name, const arma::vec& value);
+		void setUniform(const std::string& name, float value);
+		void setUniform(const std::string& name, bool value);
+		void setUniform(const std::string& name, int value);
+		void setTexture(const std::string& name, GLuint texture, int nb_dimensions = 2);
+	};
+
+
+	//-------------------------------------------------------------------------
+	// Convert to string (handy to create a shader, but messes up line numbers,
+	// which makes it tricky with errors)
+	//-------------------------------------------------------------------------
+	#define STRINGIFY( expr ) #expr
+
+	//-------------------------------------------------------------------------
+	// Load and compile a vertex and a fragment shaders into a GLSL program
+	//-------------------------------------------------------------------------
+	shader_t loadShader(const std::string& vertex_shader,
+						const std::string& fragment_shader,
+						const std::string& version = "330 core");
+
+	//-------------------------------------------------------------------------
+	// Vertex and fragment shaders for a mesh with normals and one point light
+	// source
+	//-------------------------------------------------------------------------
+	extern const char* VERTEX_SHADER_ONE_LIGHT;
+	extern const char* FRAGMENT_SHADER_ONE_LIGHT;
+
+	//-------------------------------------------------------------------------
+	// Vertex and fragment shaders without any lightning
+	//-------------------------------------------------------------------------
+	extern const char* VERTEX_SHADER_COLORED;
+	extern const char* FRAGMENT_SHADER_COLORED;
+
+	//-------------------------------------------------------------------------
+	// Vertex and fragment shaders with a texture, without any lightning
+	//-------------------------------------------------------------------------
+	extern const char* VERTEX_SHADER_TEXTURED;
+	extern const char* FRAGMENT_SHADER_ONE_TEXTURE;
+	extern const char* FRAGMENT_SHADER_GAUSSIAN;
+
+	//-------------------------------------------------------------------------
+	// Vertex and fragment shaders for 3D gaussians
+	//-------------------------------------------------------------------------
+	extern const char* VERTEX_SHADER_3D_GAUSSIAN;
+	extern const char* FRAGMENT_SHADER_3D_GAUSSIAN;
+
+	//-------------------------------------------------------------------------
+	// Vertex and fragment shaders to do render-to-texture
+	//-------------------------------------------------------------------------
+	extern const char* RTT_VERTEX_SHADER;
+	extern const char* RTT_FRAGMENT_SHADER_GAUSSIAN;
+	extern const char* RTT_FRAGMENT_SHADER_TRANSPARENT_GAUSSIAN;
+	extern const char* RTT_FRAGMENT_SHADER_FLOW_FIELD;
+	extern const char* RTT_FRAGMENT_SHADER_LIC;
+	extern const char* RTT_FRAGMENT_SHADER_LIC_COLORED_MEAN;
+
+
+	/******************************* LIGHTNING *******************************/
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a point light
+	//-------------------------------------------------------------------------
+	struct point_light_t {
+		transforms_t	transforms;
+		arma::fvec		color;
+		float			power;
+	};
+
+	//-------------------------------------------------------------------------
+	// A list of lights
+	//-------------------------------------------------------------------------
+	typedef std::vector<point_light_t> light_list_t;
+
+
+	/******************************** TEXTURE ********************************/
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a texture
+	//-------------------------------------------------------------------------
+	struct texture_t {
+		GLuint 			id;
+		unsigned int	width;
+		unsigned int	height;
+		unsigned int	depth;
+	};
+
+	//-------------------------------------------------------------------------
+	// Represent a list of textures
+	//-------------------------------------------------------------------------
+	typedef std::vector<gfx3::texture_t> texture_list_t;
+
+	//-------------------------------------------------------------------------
+	// Create a 2D texture object initialised with an array of pixels
+	//
+	// The texture will be stored on the GPU as unsigned bytes
+	//-------------------------------------------------------------------------
+	texture_t create_texture(unsigned int width, unsigned int height,
+							 unsigned int nb_channels, const unsigned char* pixels);
+
+	//-------------------------------------------------------------------------
+	// Create a 2D texture object initialised with an array of pixels
+	//
+	// The texture will be stored on the GPU as floats
+	//-------------------------------------------------------------------------
+	texture_t create_texture(unsigned int width, unsigned int height,
+							 unsigned int nb_channels, const float* pixels);
+
+	//-------------------------------------------------------------------------
+	// Create a 3D texture object initialised with an array of pixels
+	//
+	// The texture will be stored on the GPU as floats
+	//-------------------------------------------------------------------------
+	texture_t create_texture(unsigned int width, unsigned int height,
+							 unsigned int depth, unsigned int nb_channels,
+							 const float* pixels);
+
+	//-------------------------------------------------------------------------
+	// Release the OpenGL resources used by the texture
+	//-------------------------------------------------------------------------
+	void destroy(const texture_t& texture);
+
+
+	/*************************** RENDER-TO-TEXTURE ***************************/
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a texture and its associated
+	// framebuffer (to be used by a render-to-texture object)
+	//-------------------------------------------------------------------------
+	struct render_to_texture_buffer_t {
+		GLuint    framebuffer;
+		texture_t texture;
+	};
+
+	//-------------------------------------------------------------------------
+	// Represent a list of render-to-texture buffers
+	//-------------------------------------------------------------------------
+	typedef std::vector<gfx3::render_to_texture_buffer_t> render_to_texture_buffer_list_t;
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a render-to-texture object
+	//-------------------------------------------------------------------------
+	struct render_to_texture_t {
+
+		// Textures
+		render_to_texture_buffer_list_t	buffers;
+		unsigned int					nb_buffers;
+		unsigned int					current_buffer;
+		unsigned int					width;
+		unsigned int					height;
+
+		// Rectangular mesh
+		GLuint nb_vertices;
+		GLuint vertex_buffer;
+
+		// Shader
+		shader_t const* shader;
+
+		render_to_texture_t()
+		: nb_buffers(0), current_buffer(0), width(0), height(0), nb_vertices(0),
+		  vertex_buffer(-1), shader(0)
+		{
+		}
+
+		inline GLuint texture() const {
+			return buffers[current_buffer].texture.id;
+		};
+
+		inline GLuint previous_texture() const {
+			int previous_buffer = current_buffer - 1;
+			if (previous_buffer < 0)
+				previous_buffer = nb_buffers - 1;
+
+			return buffers[previous_buffer].texture.id;
+		};
+	};
+
+	//-------------------------------------------------------------------------
+	// Represent a list of render-to-texture objects
+	//-------------------------------------------------------------------------
+	typedef std::vector<gfx3::render_to_texture_t> render_to_texture_list_t;
+
+	//-------------------------------------------------------------------------
+	// Create a render-to-texture object
+	//
+	// The number of channels of the texture (RGB or RGBA) depends on the
+	// number of dimensions of the provided initial color
+	//
+	// channel_size can be 8 (unsigned byte) or 32 (float)
+	//-------------------------------------------------------------------------
+	render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+								  unsigned int height, const arma::fvec& color,
+								  unsigned int nb_buffers = 1,
+								  unsigned int channel_size = 8);
+
+	//-------------------------------------------------------------------------
+	// Create a render-to-texture object initialised with an array of pixels
+	//
+	// The texture will be stored on the GPU as unsigned bytes
+	//-------------------------------------------------------------------------
+	render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+								  unsigned int height, unsigned int nb_channels,
+								  const unsigned char* pixels,
+								  unsigned int nb_buffers = 1);
+
+	//-------------------------------------------------------------------------
+	// Create a render-to-texture object initialised with an array of pixels
+	//
+	// The texture will be stored on the GPU as floats
+	//-------------------------------------------------------------------------
+	render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+								  unsigned int height, unsigned int nb_channels,
+								  const float* pixels,
+								  unsigned int nb_buffers = 1);
+
+	//-------------------------------------------------------------------------
+	// Render a render-to-texture object
+	//-------------------------------------------------------------------------
+	bool draw(render_to_texture_t &rtt);
+
+	//-------------------------------------------------------------------------
+	// Release the OpenGL resources used by the render-to-texture object
+	//-------------------------------------------------------------------------
+	void destroy(const render_to_texture_t& rtt);
+
+
+	/********************************* MESHES ********************************/
+
+	//-------------------------------------------------------------------------
+	// Holds all the needed informations about a mesh
+	//-------------------------------------------------------------------------
+	struct model_t {
+
+		model_t()
+		: mode(0), nb_vertices(0), vertex_buffer(0), normal_buffer(0),
+		  uv_buffer(0), uvw_buffer(0), shader(0), diffuse_texture(0),
+		  use_transparency(false)
+		{}
+
+
+		GLenum			mode;
+
+		// Vertex data
+		GLuint			nb_vertices;
+		GLuint			vertex_buffer;
+		GLuint			normal_buffer;
+		GLuint			uv_buffer;
+		GLuint			uvw_buffer;
+
+		// Transforms
+		transforms_t	transforms;
+
+		// Shader
+		shader_t const* shader;
+
+		// Material
+		arma::fvec		ambiant_color;
+		arma::fvec		diffuse_color;
+		arma::fvec		specular_color;
+		float			specular_power;
+
+		// Textures
+		GLuint			diffuse_texture;
+
+		// Other
+		bool			use_transparency;
+	};
+
+	//-------------------------------------------------------------------------
+	// Represent a list of models
+	//-------------------------------------------------------------------------
+	typedef std::vector<gfx3::model_t> model_list_t;
+
+	//-------------------------------------------------------------------------
+	// Create a rectangular mesh, colored (no lightning)
+	//-------------------------------------------------------------------------
+	model_t create_rectangle(const shader_t& shader, const arma::fvec& color,
+							 float width, float height,
+							 const arma::fvec& position = arma::zeros<arma::fvec>(3),
+							 const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+							 const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a square mesh, colored (no lightning)
+	//-------------------------------------------------------------------------
+	model_t create_square(const shader_t& shader, const arma::fvec& color, float size,
+						  const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						  const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						  const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a sphere mesh, either colored or that can be lighted (it depends
+	// on the shader)
+	//-------------------------------------------------------------------------
+	model_t create_sphere(const shader_t& shader, float radius = 1.0f,
+						  const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						  const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						  const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a line mesh, colored (no lightning), from a matrix containing the
+	// point coordinates
+	//-------------------------------------------------------------------------
+	model_t create_line(const shader_t& shader, const arma::fvec& color,
+						const arma::mat& points,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						const transforms_t* parent_transforms = 0,
+						bool line_strip = true);
+
+	//-------------------------------------------------------------------------
+	// Create a line mesh, colored (no lightning), from a matrix containing the
+	// point coordinates, with the specified width
+	//-------------------------------------------------------------------------
+	model_t create_line(const shader_t& shader, const arma::fvec& color,
+						const arma::mat& points, float width,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a line mesh, colored (no lightning), from an array containing the
+	// point coordinates
+	//-------------------------------------------------------------------------
+	model_t create_line(const shader_t& shader, const arma::fvec& color,
+						const std::vector<arma::vec>& points,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						const transforms_t* parent_transforms = 0,
+						bool line_strip = true);
+
+	//-------------------------------------------------------------------------
+	// Create a line mesh, colored (no lightning), from an array containing the
+	// point coordinates, with the specified width
+	//-------------------------------------------------------------------------
+	model_t create_line(const shader_t& shader, const arma::fvec& color,
+						const std::vector<arma::vec>& points, float width,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a mesh, colored (no lightning), from a matrix containing the
+	// vertex coordinates
+	//-------------------------------------------------------------------------
+	model_t create_mesh(const shader_t& shader, const arma::fvec& color,
+						const arma::mat& vertices,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+						const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a line mesh representing a gaussian border, colored (no lightning)
+	//-------------------------------------------------------------------------
+	model_t create_gaussian_border(const shader_t& shader, const arma::fvec& color,
+								   const arma::vec& mu, const arma::mat& sigma,
+								   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+								   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+								   const transforms_t* parent_transforms = 0);
+
+	//-------------------------------------------------------------------------
+	// Create a plane mesh, attached to the camera, used to display 3D
+	// gaussians, using draw_gaussian_3D()
+	//-------------------------------------------------------------------------
+	model_t create_gaussian_plane(const shader_t& shader, const camera_t& camera);
+
+	//-------------------------------------------------------------------------
+	// Release the OpenGL resources used by the model
+	//-------------------------------------------------------------------------
+	void destroy(const model_t& model);
+
+
+	/******************************* RENDERING *******************************/
+
+	//-------------------------------------------------------------------------
+	// Render a mesh
+	//-------------------------------------------------------------------------
+	bool draw(const model_t& model, const arma::fmat& view,
+			  const arma::fmat& projection,
+			  const light_list_t& lights);
+
+	//-------------------------------------------------------------------------
+	// Render a mesh (shortcut when lights aren't used by the shaders)
+	//-------------------------------------------------------------------------
+	inline bool draw(const model_t& model, const arma::fmat& view,
+					 const arma::fmat& projection)
+	{
+		light_list_t lights;
+		return draw(model, view, projection, lights);
+	}
+
+	//-------------------------------------------------------------------------
+	// Render a rectangular mesh, colored (no lightning)
+	//-------------------------------------------------------------------------
+	bool draw_rectangle(const shader_t& shader, const arma::fvec& color,
+						float width, float height, const arma::fmat& view,
+						const arma::fmat& projection,
+						const arma::fvec& position = arma::zeros<arma::fvec>(3),
+						const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+
+	//-------------------------------------------------------------------------
+	// Render a line, colored (no lightning), from a matrix containing the
+	// point coordinates
+	//-------------------------------------------------------------------------
+	bool draw_line(const shader_t& shader, const arma::fvec& color,
+				   const arma::mat& points, const arma::fmat& view,
+				   const arma::fmat& projection,
+				   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+				   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+				   bool line_strip = true);
+
+	//-------------------------------------------------------------------------
+	// Render a line, colored (no lightning), from a matrix containing the
+	// point coordinates, with the specified width
+	//-------------------------------------------------------------------------
+	bool draw_line(const shader_t& shader, const arma::fvec& color,
+				   const arma::mat& points, float width,
+				   const arma::fmat& view, const arma::fmat& projection,
+				   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+				   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+
+	//-------------------------------------------------------------------------
+	// Render a line, colored (no lightning), from an array containing the
+	// point coordinates
+	//-------------------------------------------------------------------------
+	bool draw_line(const shader_t& shader, const arma::fvec& color,
+				   const std::vector<arma::vec>& points, const arma::fmat& view,
+				   const arma::fmat& projection,
+				   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+				   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+				   bool line_strip = true);
+
+	//-------------------------------------------------------------------------
+	// Render a line, colored (no lightning), from an array containing the
+	// point coordinates, with the specified width
+	//-------------------------------------------------------------------------
+	bool draw_line(const shader_t& shader, const arma::fvec& color,
+				   const std::vector<arma::vec>& points, float width,
+				   const arma::fmat& view, const arma::fmat& projection,
+				   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+				   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+
+	//-------------------------------------------------------------------------
+	// Render a mesh, colored (no lightning), from a matrix containing the
+	// vertex coordinates
+	//-------------------------------------------------------------------------
+	bool draw_mesh(const shader_t& shader, const arma::fvec& color,
+				   const arma::mat& vertices,const arma::fmat& view,
+				   const arma::fmat& projection,
+				   const arma::fvec& position = arma::zeros<arma::fvec>(3),
+				   const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+
+	//-------------------------------------------------------------------------
+	// Render a gaussian, colored (no lightning)
+	//-------------------------------------------------------------------------
+	bool draw_gaussian(shader_t* shader, const arma::fvec& color,
+					   const arma::vec& mu, const arma::mat& sigma,
+					   const arma::fmat& view, const arma::fmat& projection,
+					   float viewport_width, float viewport_height);
+
+	//-------------------------------------------------------------------------
+	// Render the border of a gaussian, colored (no lightning)
+	//-------------------------------------------------------------------------
+	bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
+							  const arma::vec& mu, const arma::mat& sigma,
+							  const arma::fmat& view,
+							  const arma::fmat& projection,
+							  float viewport_width, float viewport_height);
+
+	//-------------------------------------------------------------------------
+	// Render the border of a gaussian, colored (no lightning)
+	//-------------------------------------------------------------------------
+	bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
+							  const arma::vec& mu, const arma::mat& sigma,
+							  float width, const arma::fmat& view,
+							  const arma::fmat& projection,
+							  float viewport_width, float viewport_height);
+
+	//-------------------------------------------------------------------------
+	// Render a gaussian, colored (no lightning)
+	//-------------------------------------------------------------------------
+	bool draw_gaussian_3D(model_t* plane, shader_t* shader,
+						  const arma::fvec& color,
+						  const arma::vec& mu, const arma::mat& sigma,
+						  const arma::fmat& view, const arma::fmat& projection);
+
+};
diff --git a/include/gfx_common.h b/include/gfx_common.h
new file mode 100644
index 0000000..827b271
--- /dev/null
+++ b/include/gfx_common.h
@@ -0,0 +1,360 @@
+/*
+ * window_utils.h
+ *
+ * Helper functions to manage windows taking multiple monitors
+ * into account.
+ *
+ * Authors: Philip Abbet
+ */
+
+#pragma once
+
+#ifndef GFX_NAMESPACE
+#error "Don't include 'gfx_common.h' directly, it must be done by 'gfx2.h' or 'gfx3.h'"
+#endif
+
+#include <GLFW/glfw3.h>
+#include <algorithm>
+
+
+namespace GFX_NAMESPACE
+{
+
+	/************************** WINDOW-RELATED FUNCTIONS ************************/
+
+	//----------------------------------------------------------------------------
+	// Holds the sizes of the window and of the OpenGL front-buffer (they might
+	// be different, for instance on a 4K screen)
+	//----------------------------------------------------------------------------
+	struct window_size_t {
+		int win_width;
+		int win_height;
+		int fb_width;
+		int fb_height;
+
+		inline float scale_x() const {
+			return (float) fb_width / (float) win_width;
+		}
+
+		inline float scale_y() const {
+			return (float) fb_height / (float) win_height;
+		}
+	};
+
+	//----------------------------------------------------------------------------
+	// Returns the monitor on which the provided window is located.
+	//
+	// This functionality doesn't exists in GLFW.
+	//----------------------------------------------------------------------------
+	GLFWmonitor* get_current_monitor(GLFWwindow* window);
+
+	//----------------------------------------------------------------------------
+	// Create a window as large as possiblem but adapted to the current monitor.
+	// On input, the provided dimensions are used as an hint for the desired aspect
+	// ratio. On output, the real dimensions of the window is returned.
+	//----------------------------------------------------------------------------
+	GLFWwindow* create_window_at_optimal_size(const char* title, int &width, int &height);
+
+	//----------------------------------------------------------------------------
+	// Result codes for the function 'handle_window_resizing()'
+	//----------------------------------------------------------------------------
+	enum window_result_t {
+		INVALID_SIZE,   // The window size is invalid (happens at the beginning of the
+		                // program)
+		NO_CHANGE,      // Nothing has changed
+		WINDOW_READY,   // The window is ready (only returned once)
+		WINDOW_RESIZED, // The window was resized
+	};
+
+	//----------------------------------------------------------------------------
+	// Handle the resizing of the window
+	//
+	// Call it periodically (each frame)
+	//----------------------------------------------------------------------------
+	window_result_t handle_window_resizing(GLFWwindow* window,
+										   window_size_t* window_size,
+										   window_size_t* previous_size = 0
+										  );
+
+
+	/***************************** UTILITY FUNCTIONS ****************************/
+
+	//----------------------------------------------------------------------------
+	// Initialisations
+	//----------------------------------------------------------------------------
+	void init();
+
+	//----------------------------------------------------------------------------
+	// Convert radian to degrees
+	//----------------------------------------------------------------------------
+	double deg2rad(double deg);
+
+	//----------------------------------------------------------------------------
+	// Returns the sinus of an angle in degrees
+	//----------------------------------------------------------------------------
+	double sin_deg(double deg);
+
+	//----------------------------------------------------------------------------
+	// Returns the cosinus of an angle in degrees
+	//----------------------------------------------------------------------------
+	double cos_deg(double deg);
+
+	//----------------------------------------------------------------------------
+	// Indicates if two values are close enough
+	//----------------------------------------------------------------------------
+	bool is_close(float a, float b, float epsilon = 1e-6f);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to OpenGL-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+	//----------------------------------------------------------------------------
+	arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to OpenGL-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+	//----------------------------------------------------------------------------
+	arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
+					int fb_width, int fb_height);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to OpenGL-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+	// (fb_width / 2, -fb_height / 2)
+	//----------------------------------------------------------------------------
+	arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from UI-space to OpenGL-space
+	//
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+	// (fb_width / 2, -fb_height / 2)
+	//----------------------------------------------------------------------------
+	arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
+							 int fb_width, int fb_height);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from OpenGL-space to UI-space
+	//
+	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	//----------------------------------------------------------------------------
+	arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
+					int fb_width, int fb_height);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from OpenGL-space to UI-space
+	//
+	// OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	//----------------------------------------------------------------------------
+	arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from OpenGL-space to UI-space
+	//
+	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+	// (fb_width / 2, -fb_height / 2)
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	//----------------------------------------------------------------------------
+	arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
+							 int fb_width, int fb_height);
+
+	//----------------------------------------------------------------------------
+	// Converts some coordinates from OpenGL-space to UI-space
+	//
+	// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+	// (fb_width / 2, -fb_height / 2)
+	// UI coordinates range from (0, 0) to (win_width, win_height)
+	//----------------------------------------------------------------------------
+	arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size);
+
+
+	/************************ PROJECTION & VIEW MATRICES ************************/
+
+	//----------------------------------------------------------------------------
+	// Returns a perspective projection matrix
+	//----------------------------------------------------------------------------
+	arma::fmat perspective(float fovy, float aspect, float zNear, float zFar);
+
+	//----------------------------------------------------------------------------
+	// Returns a orthographic projection matrix
+	//----------------------------------------------------------------------------
+	arma::fmat orthographic(float width, float height, float zNear, float zFar);
+
+	//----------------------------------------------------------------------------
+	// Returns a view matrix
+	//----------------------------------------------------------------------------
+	arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
+					  const arma::fvec& up);
+
+
+	/***************************** TRANSFORMATIONS ******************************/
+
+	//----------------------------------------------------------------------------
+	// Holds all the transformations needed for a 3D entity
+	//
+	// Can be organised in a hierarchy, where the parent transforms affect the
+	// children ones
+	//----------------------------------------------------------------------------
+	struct transforms_t {
+
+		// Constructor
+		transforms_t()
+		: parent(0)
+		{
+			position.zeros(3);
+			rotation.eye(4, 4);
+		}
+
+		const transforms_t* parent;
+
+		arma::fvec position;
+		arma::fmat rotation;
+	};
+
+	//----------------------------------------------------------------------------
+	// Returns a rotation matrix given an axis and an angle (in radian)
+	//----------------------------------------------------------------------------
+	arma::fmat rotate(const arma::fvec& axis, float angle);
+
+	//----------------------------------------------------------------------------
+	// Returns the rotation matrix to go from one direction to another one
+	//----------------------------------------------------------------------------
+	arma::fmat rotation(const arma::fvec& from, const arma::fvec& to);
+
+	//----------------------------------------------------------------------------
+	// Compute the translation and rotation to apply to a list of 3D points A to
+	// obtain the list of 3D points B.
+	//
+	// Points are organised in columns:
+	//	[ x0 x1 x2 ...
+	//	  y0 y1 y2
+	//	  z0 z1 z2 ]
+	//----------------------------------------------------------------------------
+	void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
+							arma::fmat &rotation, arma::fvec &translation);
+
+	//----------------------------------------------------------------------------
+	// Returns the full world transformation matrix corresponding to the given
+	// transforms structure, taking all its parent hierarchy into account
+	//----------------------------------------------------------------------------
+	arma::fmat worldTransforms(const transforms_t* transforms);
+
+	//----------------------------------------------------------------------------
+	// Returns the full world position corresponding to the given transforms
+	// structure, taking all its parent hierarchy into account
+	//----------------------------------------------------------------------------
+	arma::fvec worldPosition(const transforms_t* transforms);
+
+	//----------------------------------------------------------------------------
+	// Returns the full world rotation matrix corresponding to the given
+	// transforms structure, taking all its parent hierarchy into account
+	//----------------------------------------------------------------------------
+	arma::fmat worldRotation(const transforms_t* transforms);
+
+
+	/******************************** CAMERAS ********************************/
+
+	//----------------------------------------------------------------------------
+	// Represents a camera in the 3D scene
+	//
+	// A camera points to a target point, looking at it from a distance
+	//----------------------------------------------------------------------------
+	struct camera_t {
+
+		// Constructor
+		camera_t()
+		{
+			transforms.position(2) = 5.0f;
+			rotator.parent = &this->target;
+			transforms.parent = &this->rotator;
+		}
+
+		// Constructor
+		camera_t(const arma::fvec& target, float distance)
+		{
+			transforms.position(2) = distance;
+
+			this->target.position = target;
+			rotator.parent = &this->target;
+			transforms.parent = &this->rotator;
+		}
+
+		transforms_t target;
+		transforms_t rotator;
+		transforms_t transforms;
+	};
+
+	//----------------------------------------------------------------------------
+	// Rotate the camera around the Y-axis of its target (left or right)
+	//
+	// The 'delta' is an angle in radian
+	//----------------------------------------------------------------------------
+	void yaw(camera_t &camera, float delta);
+
+	//----------------------------------------------------------------------------
+	// Rotate the camera around the X-axis of its target (up or down)
+	//
+	// The 'delta' is an angle in radian
+	//----------------------------------------------------------------------------
+	void pitch(camera_t &camera, float delta);
+
+	//----------------------------------------------------------------------------
+	// Returns the view matrix corresponding to the camera
+	//----------------------------------------------------------------------------
+	arma::fmat view_matrix(const camera_t& camera);
+
+
+	/******************************* RAY CASTING ********************************/
+
+	//----------------------------------------------------------------------------
+	// Represents a 3D ray (in world coordinates)
+	//----------------------------------------------------------------------------
+	struct ray_t {
+		arma::fvec origin;
+		arma::fvec direction;
+	};
+
+	ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
+					 const arma::fmat& view, const arma::fmat& projection,
+					 int window_width, int window_height);
+
+	bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
+					arma::fvec &result);
+
+
+	/********************************* OTHERS ***********************************/
+
+	//----------------------------------------------------------------------------
+	// Returns the vertices needed to create a mesh representing the background
+	// a gaussian
+	//
+	// The result is a matrix of shape (2, nb_points * 3)
+	//----------------------------------------------------------------------------
+	arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
+											   int nb_points = 60);
+
+	//----------------------------------------------------------------------------
+	// Returns the vertices needed to create a line representing the border of
+	// a gaussian
+	//
+	// If line_strip is true:
+	//  - The result is a matrix of shape (2, nb_points)
+	//  - The rendering mode must be GL_LINE_STRIP
+	//
+	// If line_strip is false:
+	//  - The result is a matrix of shape (2, nb_points * 2)
+	//  - The rendering mode must be GL_LINES
+	//----------------------------------------------------------------------------
+	arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
+										   int nb_points = 60, bool line_strip = true);
+
+}
diff --git a/include/gl2ps.h b/include/gl2ps.h
index d3e7c0c..8cc4c29 100644
--- a/include/gl2ps.h
+++ b/include/gl2ps.h
@@ -1,6 +1,6 @@
 /*
  * GL2PS, an OpenGL to PostScript Printing Library
- * Copyright (C) 1999-2015 C. Geuzaine
+ * Copyright (C) 1999-2017 C. Geuzaine
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of either:
@@ -44,9 +44,12 @@
 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
 #  if defined(_MSC_VER)
 #    pragma warning(disable:4115)
+#    pragma warning(disable:4127)
 #    pragma warning(disable:4996)
 #  endif
+#  define NOMINMAX
 #  include <windows.h>
+#  undef NOMINMAX
 #  if defined(GL2PSDLL)
 #    if defined(GL2PSDLL_EXPORTS)
 #      define GL2PSDLL_API __declspec(dllexport)
@@ -83,15 +86,15 @@
 /* Version number */
 
 #define GL2PS_MAJOR_VERSION 1
-#define GL2PS_MINOR_VERSION 3
-#define GL2PS_PATCH_VERSION 9
+#define GL2PS_MINOR_VERSION 4
+#define GL2PS_PATCH_VERSION 0
 #define GL2PS_EXTRA_VERSION ""
 
 #define GL2PS_VERSION (GL2PS_MAJOR_VERSION + \
                        0.01 * GL2PS_MINOR_VERSION + \
                        0.0001 * GL2PS_PATCH_VERSION)
 
-#define GL2PS_COPYRIGHT "(C) 1999-2015 C. Geuzaine"
+#define GL2PS_COPYRIGHT "(C) 1999-2017 C. Geuzaine"
 
 /* Output file formats (the values and the ordering are important!) */
 
@@ -134,6 +137,7 @@
 #define GL2PS_COMPRESS             (1<<10)
 #define GL2PS_NO_BLENDING          (1<<11)
 #define GL2PS_TIGHT_BOUNDING_BOX   (1<<12)
+#define GL2PS_NO_OPENGL_CONTEXT    (1<<13)
 
 /* Arguments for gl2psEnable/gl2psDisable */
 
@@ -142,6 +146,17 @@
 #define GL2PS_LINE_STIPPLE        3
 #define GL2PS_BLEND               4
 
+
+/* Arguments for gl2psLineCap/Join */
+
+#define GL2PS_LINE_CAP_BUTT       0
+#define GL2PS_LINE_CAP_ROUND      1
+#define GL2PS_LINE_CAP_SQUARE     2
+
+#define GL2PS_LINE_JOIN_MITER     0
+#define GL2PS_LINE_JOIN_ROUND     1
+#define GL2PS_LINE_JOIN_BEVEL     2
+
 /* Text alignment (o=raster position; default mode is BL):
    +---+ +---+ +---+ +---+ +---+ +---+ +-o-+ o---+ +---o
    | o | o   | |   o |   | |   | |   | |   | |   | |   |
@@ -159,6 +174,25 @@
 #define GL2PS_TEXT_TR 9
 
 typedef GLfloat GL2PSrgba[4];
+typedef GLfloat GL2PSxyz[3];
+
+typedef struct {
+  GL2PSxyz xyz;
+  GL2PSrgba rgba;
+} GL2PSvertex;
+
+/* Primitive types */
+#define GL2PS_NO_TYPE          -1
+#define GL2PS_TEXT             1
+#define GL2PS_POINT            2
+#define GL2PS_LINE             3
+#define GL2PS_QUADRANGLE       4
+#define GL2PS_TRIANGLE         5
+#define GL2PS_PIXMAP           6
+#define GL2PS_IMAGEMAP         7
+#define GL2PS_IMAGEMAP_WRITTEN 8
+#define GL2PS_IMAGEMAP_VISIBLE 9
+#define GL2PS_SPECIAL          10
 
 #if defined(__cplusplus)
 extern "C" {
@@ -183,15 +217,27 @@ GL2PSDLL_API GLint gl2psTextOptColor(const char *str, const char *fontname,
                                      GLshort fontsize, GLint align, GLfloat angle,
                                      GL2PSrgba color);
 GL2PSDLL_API GLint gl2psSpecial(GLint format, const char *str);
+GL2PSDLL_API GLint gl2psSpecialColor(GLint format, const char *str, GL2PSrgba rgba);
 GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
                                    GLint xorig, GLint yorig,
                                    GLenum format, GLenum type, const void *pixels);
 GL2PSDLL_API GLint gl2psEnable(GLint mode);
 GL2PSDLL_API GLint gl2psDisable(GLint mode);
 GL2PSDLL_API GLint gl2psPointSize(GLfloat value);
+GL2PSDLL_API GLint gl2psLineCap(GLint value);
+GL2PSDLL_API GLint gl2psLineJoin(GLint value);
 GL2PSDLL_API GLint gl2psLineWidth(GLfloat value);
 GL2PSDLL_API GLint gl2psBlendFunc(GLenum sfactor, GLenum dfactor);
 
+/* referenced in the documentation, but not fully documented */
+GL2PSDLL_API GLint gl2psForceRasterPos(GL2PSvertex *vert);
+GL2PSDLL_API void gl2psAddPolyPrimitive(GLshort type, GLshort numverts,
+                                        GL2PSvertex *verts, GLint offset,
+                                        GLfloat ofactor, GLfloat ounits,
+                                        GLushort pattern, GLint factor,
+                                        GLfloat width, GLint linecap,
+                                        GLint linejoin, char boundary);
+
 /* undocumented */
 GL2PSDLL_API GLint gl2psDrawImageMap(GLsizei width, GLsizei height,
                                      const GLfloat position[3],
diff --git a/include/imgui_impl_glfw_gl3.h b/include/imgui_impl_glfw_gl3.h
new file mode 100644
index 0000000..33b5329
--- /dev/null
+++ b/include/imgui_impl_glfw_gl3.h
@@ -0,0 +1,31 @@
+// ImGui GLFW binding with OpenGL3 + shaders
+// (GLFW is a cross-platform general purpose library for handling windows, inputs, OpenGL/Vulkan graphics context creation, etc.)
+// (GL3W is a helper library to access OpenGL functions since there is no standard header to access modern OpenGL functions easily. Alternatives are GLEW, Glad, etc.)
+
+// Implemented features:
+//  [X] User texture binding. Cast 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID in imgui.cpp.
+//  [X] Gamepad navigation mapping. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'.
+
+// You can copy and use unmodified imgui_impl_* files in your project. See main.cpp for an example of using this.
+// If you use this binding you'll need to call 4 functions: ImGui_ImplXXXX_Init(), ImGui_ImplXXXX_NewFrame(), ImGui::Render() and ImGui_ImplXXXX_Shutdown().
+// If you are new to ImGui, see examples/README.txt and documentation at the top of imgui.cpp.
+// https://github.com/ocornut/imgui
+
+struct GLFWwindow;
+
+IMGUI_API bool        ImGui_ImplGlfwGL3_Init(GLFWwindow* window, bool install_callbacks, const char* glsl_version = NULL);
+IMGUI_API void        ImGui_ImplGlfwGL3_Shutdown();
+IMGUI_API void        ImGui_ImplGlfwGL3_NewFrame();
+IMGUI_API void        ImGui_ImplGlfwGL3_RenderDrawData(ImDrawData* draw_data);
+
+// Use if you want to reset your rendering device without losing ImGui state.
+IMGUI_API void        ImGui_ImplGlfwGL3_InvalidateDeviceObjects();
+IMGUI_API bool        ImGui_ImplGlfwGL3_CreateDeviceObjects();
+
+// GLFW callbacks (installed by default if you enable 'install_callbacks' during initialization)
+// Provided here if you want to chain callbacks.
+// You can also handle inputs yourself and use those as a reference.
+IMGUI_API void        ImGui_ImplGlfw_MouseButtonCallback(GLFWwindow* window, int button, int action, int mods);
+IMGUI_API void        ImGui_ImplGlfw_ScrollCallback(GLFWwindow* window, double xoffset, double yoffset);
+IMGUI_API void        ImGui_ImplGlfw_KeyCallback(GLFWwindow* window, int key, int scancode, int action, int mods);
+IMGUI_API void        ImGui_ImplGlfw_CharCallback(GLFWwindow* window, unsigned int c);
diff --git a/include/tensor.h b/include/tensor.h
index 53364aa..ac4bbd5 100644
--- a/include/tensor.h
+++ b/include/tensor.h
@@ -17,7 +17,7 @@
 #include <armadillo>
 
 
-template <typename T, size_t N>
+template <typename T>
 class Tensor {
 
 public:
@@ -47,8 +47,6 @@ public:
 	Tensor(const arma::ivec& dims)
 	: size(1)
 	{
-		assert(dims.n_elem == N);
-
 		this->dims = dims;
 
 		for (int i = 0; i < dims.n_elem; ++i)
@@ -66,10 +64,10 @@ public:
 
 	//-------------------------------------------------------------------------
 	// Returns the element index (in the flattened vector) corresponding to the
-	// provided N indices
+	// provided subscript indices
 	//-------------------------------------------------------------------------
 	int indice(const arma::ivec& subscripts) const {
-		assert(subscripts.n_elem == N);
+		assert(subscripts.n_elem == dims.n_elem);
 
 		int indice = 0;
 
@@ -82,11 +80,11 @@ public:
 
 	//-------------------------------------------------------------------------
 	// Returns the element index (in the flattened vector) corresponding to the
-	// provided N indices
+	// provided subscript indices
 	//-------------------------------------------------------------------------
 	template<typename... Ints>
 	int indice(Ints... subscripts) const {
-		static_assert(sizeof...(Ints) == N, "Wrong number of arguments");
+		assert(sizeof...(Ints) == dims.n_elem);
 
 		return indice(arma::ivec({subscripts...}));
 	}
@@ -94,18 +92,41 @@ public:
 
 	//-------------------------------------------------------------------------
 	// Returns the list of element indices (in the flattened vector)
-	// corresponding to the provided N indices
+	// corresponding to the provided subscript indices
 	//-------------------------------------------------------------------------
 	template<typename... Spans>
 	arma::uvec indices(Spans... spans) const {
-		static_assert(sizeof...(Spans) == N, "Wrong number of arguments");
+		assert(sizeof...(Spans) == dims.n_elem);
+
+		std::vector<arma::span> all_spans(dims.n_elem);
+		_process_spans(all_spans, 0, spans...);
 
-		arma::uvec result(size);
-		size_t nb_indices = 0;
+		arma::ivec subscripts(dims.n_elem);
+		int nb_indices = 1;
 
-		_compute_indices(result, nb_indices, 0, 0, spans...);
+		for (int i = 0; i < dims.n_elem; ++i) {
+			subscripts(i) = all_spans[i].a;
+			nb_indices *= all_spans[i].b - all_spans[i].a + 1;
+		}
+
+		arma::uvec result(nb_indices);
+		for (int i = 0; i < nb_indices; ++i) {
+			result(i) = indice(subscripts);
+
+			int j = 0;
+
+			while (j < dims.n_elem) {
+				subscripts(j) += 1;
+				if (subscripts(j) > all_spans[j].b) {
+					subscripts(j) = all_spans[j].a;
+					++j;
+				} else {
+					break;
+				}
+			}
+		}
 
-		return result.subvec(0, nb_indices - 1);
+		return result;
 	}
 
 
@@ -118,17 +139,17 @@ public:
 
 
 	//-------------------------------------------------------------------------
-	// Returns A COPY of the elements located at the provided N indices/spans
+	// Returns A COPY of the elements located at the provided indices/spans
 	//-------------------------------------------------------------------------
 	template<typename... Spans>
-	inline Tensor<T, N> operator()(Spans... spans) const {
-		static_assert(sizeof...(Spans) == N, "Wrong number of arguments");
+	inline Tensor<T> operator()(Spans... spans) const {
+		assert(sizeof...(Spans) == dims.n_elem);
 
 		arma::ivec dims(this->dims.n_elem);
 
 		_compute_dims(dims, 0, spans...);
 
-		Tensor<T, N> result(dims);
+		Tensor<T> result(dims);
 		result.data = data(indices(spans...));
 		return result;
 	}
@@ -155,7 +176,7 @@ public:
 	//-------------------------------------------------------------------------
 	// Assignement operator
 	//-------------------------------------------------------------------------
-	inline void operator=(const Tensor<T, N>& t) {
+	inline void operator=(const Tensor<T>& t) {
 		assert(size == t.size);
 		assert(arma::all(dims == t.dims));
 		data = t.data;
@@ -163,15 +184,24 @@ public:
 
 
 	//-------------------------------------------------------------------------
-	// Set the value at the locations given by provided N indices
+	// Set the value at the location given by provided index
+	//-------------------------------------------------------------------------
+	inline void set(int index, T value) {
+		data(index) = value;
+	}
+
+
+	//-------------------------------------------------------------------------
+	// Set the value at the locations given by provided indices
 	//-------------------------------------------------------------------------
 	inline void set(const arma::uvec& indices, T value) {
-		data(indices) = arma::Col<T>({ value });
+		for (int i = 0; i < indices.n_elem; ++i)
+			data(indices(i)) = value;
 	}
 
 
 	//-------------------------------------------------------------------------
-	// Set the values at the locations given by provided N indices
+	// Set the values at the locations given by provided indices
 	//-------------------------------------------------------------------------
 	inline void set(const arma::uvec& indices, const arma::Col<T>& values) {
 		data(indices) = values;
@@ -179,22 +209,22 @@ public:
 
 
 	//-------------------------------------------------------------------------
-	// Set the value at the locations given by provided N indices
+	// Set the value at the locations given by provided indices
 	//-------------------------------------------------------------------------
 	template<typename... Spans>
 	inline void set(Spans... spans, T value) {
-		static_assert(sizeof...(Spans) == N, "Wrong number of arguments");
+		static_assert(sizeof...(Spans) == dims.n_elem, "Wrong number of arguments");
 
 		data(indices(spans...)) = arma::Col<T>({ value });
 	}
 
 
 	//-------------------------------------------------------------------------
-	// Set the values at the locations given by provided N indices
+	// Set the values at the locations given by provided indices
 	//-------------------------------------------------------------------------
 	template<typename... Spans>
 	inline void set(Spans... spans, const arma::Col<T>& values) {
-		static_assert(sizeof...(Spans) == N, "Wrong number of arguments");
+		static_assert(sizeof...(Spans) == dims.n_elem, "Wrong number of arguments");
 
 		data(indices(spans...)) = values;
 	}
@@ -203,8 +233,8 @@ public:
 	//-------------------------------------------------------------------------
 	// Multiplication by a scalar
 	//-------------------------------------------------------------------------
-	Tensor<T, N> operator*(T scalar) const {
-		Tensor<T, N> result(dims);
+	Tensor<T> operator*(T scalar) const {
+		Tensor<T> result(dims);
 		result.data = data * scalar;
 		return result;
 	}
@@ -213,8 +243,8 @@ public:
 	//-------------------------------------------------------------------------
 	// Division by a scalar
 	//-------------------------------------------------------------------------
-	Tensor<T, N> operator/(T scalar) const {
-		Tensor<T, N> result(dims);
+	Tensor<T> operator/(T scalar) const {
+		Tensor<T> result(dims);
 		result.data = data / scalar;
 		return result;
 	}
@@ -223,8 +253,8 @@ public:
 	//-------------------------------------------------------------------------
 	// Addition of a scalar
 	//-------------------------------------------------------------------------
-	Tensor<T, N> operator+(T scalar) const {
-		Tensor<T, N> result(dims);
+	Tensor<T> operator+(T scalar) const {
+		Tensor<T> result(dims);
 		result.data = data + scalar;
 		return result;
 	}
@@ -233,8 +263,8 @@ public:
 	//-------------------------------------------------------------------------
 	// Addition of a tensor of same dimensions
 	//-------------------------------------------------------------------------
-	Tensor<T, N> operator+(const Tensor<T, N>& m) const {
-		Tensor<T, N> result(dims);
+	Tensor<T> operator+(const Tensor<T>& m) const {
+		Tensor<T> result(dims);
 		result.data = data + m.data;
 		return result;
 	}
@@ -243,8 +273,8 @@ public:
 	//-------------------------------------------------------------------------
 	// Substraction of a tensor of same dimensions
 	//-------------------------------------------------------------------------
-	Tensor<T, N> operator-(const Tensor<T, N>& m) const {
-		Tensor<T, N> result(dims);
+	Tensor<T> operator-(const Tensor<T>& m) const {
+		Tensor<T> result(dims);
 		result.data = data - m.data;
 		return result;
 	}
@@ -261,42 +291,36 @@ protected:
 
 
 	template<typename... Spans>
-	void _compute_indices(arma::uvec &result, size_t &nb_indices, unsigned int dim,
-						  unsigned int base_index, int index, Spans... spans) const {
-		unsigned int next_index = base_index + index * _indices(dim);
-		_compute_indices(result, nb_indices, dim + 1, next_index, spans...);
+	void _process_spans(std::vector<arma::span> &result, unsigned int dim,
+						int index, Spans... spans) const {
+
+		result[dim] = arma::span(index);
+		_process_spans(result, dim + 1, spans...);
 	}
 
 
 	template<typename... Spans>
-	void _compute_indices(arma::uvec &result, size_t &nb_indices, unsigned int dim,
-						  unsigned int base_index, arma::span indices, Spans... spans) const {
-		_process_span(indices, dims(dim));
+	void _process_spans(std::vector<arma::span> &result, unsigned int dim,
+						arma::span indices, Spans... spans) const {
 
-		for (int i = indices.a; i <= indices.b; ++i) {
-			unsigned int next_index = base_index + i * _indices(dim);
-			_compute_indices(result, nb_indices, dim + 1, next_index, spans...);
-		}
+		_process_span(indices, dims[dim]);
+		result[dim] = indices;
+		_process_spans(result, dim + 1, spans...);
 	}
 
 
-	void _compute_indices(arma::uvec &result, size_t &nb_indices, unsigned int dim,
-						  unsigned int base_index, int index) const {
-		unsigned int next_index = base_index + index * _indices(dim);
-		result(nb_indices) = next_index;
-		++nb_indices;
+	void _process_spans(std::vector<arma::span> &result, unsigned int dim,
+						int index) const {
+
+		result[dim] = arma::span(index);
 	}
 
 
-	void _compute_indices(arma::uvec &result, size_t &nb_indices, unsigned int dim,
-						  unsigned int base_index, arma::span indices) const {
-		_process_span(indices, dims(dim));
+	void _process_spans(std::vector<arma::span> &result, unsigned int dim,
+						arma::span indices) const {
 
-		for (int i = indices.a; i <= indices.b; ++i) {
-			unsigned int next_index = base_index + i * _indices(dim);
-			result(nb_indices) = next_index;
-			++nb_indices;
-		}
+		_process_span(indices, dims[dim]);
+		result[dim] = indices;
 	}
 
 
@@ -326,3 +350,10 @@ public:
 protected:
 	arma::ivec		_indices;	// Used to compute 1-D indices from N-D ones
 };
+
+
+typedef Tensor<double>      TensorDouble;
+typedef Tensor<float>       TensorFloat;
+typedef Tensor<arma::sword> TensorInt;
+typedef Tensor<arma::uword> TensorUInt;
+
diff --git a/include/window_utils.h b/include/window_utils.h
deleted file mode 100644
index af47019..0000000
--- a/include/window_utils.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * window_utils.h
- *
- * Helper functions to manage windows taking multiple monitors
- * into account.
- *
- * Authors: Philip Abbet
- */
-
-#pragma once
-
-#include <GLFW/glfw3.h>
-#include <algorithm>
-
-
-//-----------------------------------------------------------------------------
-// Returns the monitor on which the provided window is located.
-//
-// This functionality doesn't exists in GLFW.
-//-----------------------------------------------------------------------------
-GLFWmonitor* get_current_monitor(GLFWwindow* window)
-{
-	int win_x, win_y, win_width, win_height;
-	glfwGetWindowPos(window, &win_x, &win_y);
-	glfwGetWindowSize(window, &win_width, &win_height);
-
-	int nb_monitors;
-	GLFWmonitor** monitors;
-	monitors = glfwGetMonitors(&nb_monitors);
-
-	int best_overlap = 0;
-	GLFWmonitor* best_monitor = 0;
-
-	for (int i = 0; i < nb_monitors; ++i) {
-		const GLFWvidmode* mode = glfwGetVideoMode(monitors[i]);
-
-		int monitor_x, monitor_y;
-		glfwGetMonitorPos(monitors[i], &monitor_x, &monitor_y);
-
-		int monitor_width = mode->width;
-		int monitor_height = mode->height;
-
-		int overlap =
-			std::max(0, std::min(win_x + win_width, monitor_x + monitor_width) - std::max(win_x, monitor_x)) *
-			std::max(0, std::min(win_y + win_height, monitor_y + monitor_height) - std::max(win_y, monitor_y)
-		);
-
-		if (best_overlap < overlap) {
-			best_overlap = overlap;
-			best_monitor = monitors[i];
-		}
-	}
-
-	return best_monitor;
-}
-
-
-//-----------------------------------------------------------------------------
-// Create a window as large as possiblem but adapted to the current monitor.
-// On input, the provided dimensions are used as an hint for the desired aspect
-// ratio. On output, the real dimensions of the window is returned.
-//-----------------------------------------------------------------------------
-GLFWwindow* create_window_at_optimal_size(const char* title,
-										  int &width, int &height) {
-
-	// First create a window of the desired size
-	GLFWwindow* window = glfwCreateWindow(
-		width, height, title, NULL, NULL
-	);
-
-	// Next determine on which monitor the window is located
-	GLFWmonitor* monitor = get_current_monitor(window);
-
-	// Then compute the "optimal" size of the window on that monitor
-	const GLFWvidmode* mode = glfwGetVideoMode(monitor);
-
-	int original_width = width;
-	int original_height = height;
-
-	if (mode->height >= 2000)
-		height = 1800;
-	else if (mode->height >= 1440)
-		height = 1200;
-	else if (mode->height >= 1200)
-		height = 1000;
-	else if (mode->height >= 1000)
-		height = 900;
-	else if (mode->height >= 900)
-		height = 800;
-	else
-		height = 600;
-
-	width = original_width * height / original_height;
-
-	if (width >= mode->width)
-	{
-		width = mode->width - 100;
-		height = original_height * width / original_width;
-	}
-
-	// Finally, resize the window and center it
-	glfwSetWindowSize(window, width, height);
-	glfwSetWindowPos(window, (mode->width - width) / 2, (mode->height - height) / 2);
-
-	return window;
-}
diff --git a/src/demo_GMR01.cpp b/src/demo_GMR01.cpp
index da4e073..4bd52fe 100644
--- a/src/demo_GMR01.cpp
+++ b/src/demo_GMR01.cpp
@@ -22,15 +22,12 @@
 
 #include <stdio.h>
 #include <armadillo>
-#include <mvn.h>
 
 #include <gfx2.h>
 #include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <imgui_internal.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -279,7 +276,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0) {
+					double near_distance = -1.0, double far_distance = 1.0) {
 
 	viewport->x = x;
 	viewport->y = y;
@@ -289,8 +286,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 }
 
 
@@ -697,7 +694,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Conditioning on trajectory distributions",
 		window_size.win_width, window_size.win_height
 	);
@@ -705,7 +702,7 @@ int main(int argc, char **argv) {
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -750,14 +747,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 2 - 1;
 			viewport_height = window_size.fb_height / 2 - 1;
diff --git a/src/demo_GPR01.cpp b/src/demo_GPR01.cpp
index ee63177..c7857ab 100644
--- a/src/demo_GPR01.cpp
+++ b/src/demo_GPR01.cpp
@@ -21,15 +21,12 @@
 
 #include <stdio.h>
 #include <armadillo>
-#include <mvn.h>
 
 #include <gfx2.h>
 #include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <imgui_internal.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -166,7 +163,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0) {
+					double near_distance = -1.0, double far_distance = 1.0) {
 
 	viewport->x = x;
 	viewport->y = y;
@@ -176,8 +173,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 }
 
 
@@ -585,7 +582,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Gaussian process regression (GPR) with RBF kernel",
 		window_size.win_width, window_size.win_height
 	);
@@ -593,7 +590,7 @@ int main(int argc, char **argv) {
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -641,14 +638,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 2 - 1;
 			viewport_height = window_size.fb_height / 2 - 1;
diff --git a/src/demo_HSMM_batchLQR01.cpp b/src/demo_HSMM_batchLQR01.cpp
index d6c93bf..a1f453a 100644
--- a/src/demo_HSMM_batchLQR01.cpp
+++ b/src/demo_HSMM_batchLQR01.cpp
@@ -14,8 +14,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
-
 
 using namespace arma;
 
@@ -518,7 +516,7 @@ int main(int argc, char **argv){
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo HSMM batch", window_size.win_width, window_size.win_height
 	);
 
@@ -557,35 +555,29 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)){
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
-
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		// Handling of the resizing of the window
+		gfx2::window_size_t previous_size;
 
-			int previous_fb_width = window_size.fb_width;
-			int previous_fb_height = window_size.fb_height;
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size, &previous_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if (window_result == gfx2::WINDOW_RESIZED) {
 
 			// Rescale the demonstrations so they stay in the window
-			if (!first) {
-				float scale_x = (float) window_size.fb_width / previous_fb_width;
-				float scale_y = (float) window_size.fb_height / previous_fb_height;
+			float scale_x = (float) window_size.fb_width / previous_size.fb_width;
+			float scale_y = (float) window_size.fb_height / previous_size.fb_height;
 
-				for (size_t i = 0; i < original_trajectories.size(); ++i) {
-					for (size_t j = 0; j < original_trajectories[i].size(); ++j) {
-						original_trajectories[i][j](0) *= scale_x;
-						original_trajectories[i][j](1) *= scale_y;
-					}
+			for (size_t i = 0; i < original_trajectories.size(); ++i) {
+				for (size_t j = 0; j < original_trajectories[i].size(); ++j) {
+					original_trajectories[i][j](0) *= scale_x;
+					original_trajectories[i][j](1) *= scale_y;
 				}
-
-				gui_state.are_parameters_modified = true;
 			}
+
+			gui_state.are_parameters_modified = true;
 		}
 
 
diff --git a/src/demo_LWR_batch01.cpp b/src/demo_LWR_batch01.cpp
index 731aab2..747abe3 100644
--- a/src/demo_LWR_batch01.cpp
+++ b/src/demo_LWR_batch01.cpp
@@ -16,8 +16,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <imgui_internal.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -33,8 +31,9 @@ typedef std::vector<mat> matrix_list_t;
 // modifiable through the UI, others are hard-coded.
 //-----------------------------------------------------------------------------
 struct parameters_t {
-	int nb_RBF;   // Number of radial basis functions
-	int nb_data;  // Number of datapoints in a trajectory
+	int   nb_RBF;     // Number of radial basis functions
+	int   nb_data;    // Number of datapoints in a trajectory
+	float sigma_RBF;  // Homogeneous covariance
 };
 
 
@@ -63,15 +62,14 @@ std::tuple<mat, mat> compute_LWR(const parameters_t& parameters, const mat& demo
 	// Set centroids equally spread in time
 	vec mu_RBF = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_RBF);
 
-	// Set constant homogeneous covariance
-	double sigma_RBF = 100.0;
-
 	mat H = zeros(parameters.nb_RBF, parameters.nb_data);
 	for (int i = 0; i < parameters.nb_RBF; ++i) {
 		H(i, span::all) = gaussPDF(linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data),
-								   mu_RBF(i), sigma_RBF).t();
+								   mu_RBF(i), parameters.sigma_RBF).t();
 	}
 
+	H = H / repmat(sum(H, 0), parameters.nb_RBF, 1);  // Rescaling
+
 	// Batch estimate (Least squares estimate of weights)
 	mat w = solve(H.t(), demonstration.t());
 
@@ -110,7 +108,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0) {
+					double near_distance = -1.0, double far_distance = 1.0) {
 
 	viewport->x = x;
 	viewport->y = y;
@@ -120,8 +118,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 }
 
 
@@ -238,6 +236,7 @@ struct gui_state_t {
 	// in parameters_t)
 	int parameter_nb_RBF;
 	int parameter_nb_data;
+	float parameter_sigma_RBF;
 };
 
 
@@ -461,8 +460,9 @@ int main(int argc, char **argv) {
 
 	// Parameters
 	parameters_t parameters;
-	parameters.nb_RBF  = 8;
-	parameters.nb_data = 100;
+	parameters.nb_RBF    = 8;
+	parameters.nb_data   = 100;
+	parameters.sigma_RBF = 1e2f;
 
 
 	// Take 4k screens into account (framebuffer size != window size)
@@ -486,7 +486,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Locally weighted regression (LWR)",
 		window_size.win_width, window_size.win_height
 	);
@@ -494,7 +494,7 @@ int main(int argc, char **argv) {
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -520,6 +520,7 @@ int main(int argc, char **argv) {
 	gui_state.must_recompute_LWR = false;
 	gui_state.parameter_nb_RBF = parameters.nb_RBF;
 	gui_state.parameter_nb_data = parameters.nb_data;
+	gui_state.parameter_sigma_RBF = parameters.sigma_RBF;
 
 
 	// Main loop
@@ -532,14 +533,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 2 - 1;
 			viewport_height = window_size.fb_height / 2 - 1;
@@ -561,6 +562,7 @@ int main(int argc, char **argv) {
 			}
 
 			parameters.nb_RBF = gui_state.parameter_nb_RBF;
+			parameters.sigma_RBF = gui_state.parameter_sigma_RBF;
 
 			gui_state.must_recompute_LWR = true;
 			gui_state.are_parameters_modified = false;
@@ -627,9 +629,9 @@ int main(int argc, char **argv) {
 
 
 		// Window: Parameters
-		ImGui::SetNextWindowSize(ImVec2(440, 106));
+		ImGui::SetNextWindowSize(ImVec2(440, 126));
 		ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2,
-									   (window_size.win_height - 106) / 2));
+									   (window_size.win_height - 126) / 2));
 		ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
 
 		if (gui_state.is_parameters_dialog_displayed)
@@ -641,6 +643,7 @@ int main(int argc, char **argv) {
 
 			ImGui::SliderInt("Nb RBF", &gui_state.parameter_nb_RBF, 2, 20);
 			ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+			ImGui::SliderFloat("Sigma RBF", &gui_state.parameter_sigma_RBF, 1e0, 1e3);
 
 			if (ImGui::Button("Close")) {
 				ImGui::CloseCurrentPopup();
diff --git a/src/demo_LWR_iterative01.cpp b/src/demo_LWR_iterative01.cpp
index df3ec34..22ad3c2 100644
--- a/src/demo_LWR_iterative01.cpp
+++ b/src/demo_LWR_iterative01.cpp
@@ -16,8 +16,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <imgui_internal.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -33,8 +31,9 @@ typedef std::vector<mat> matrix_list_t;
 // modifiable through the UI, others are hard-coded.
 //-----------------------------------------------------------------------------
 struct parameters_t {
-	int nb_RBF;   // Number of radial basis functions
-	int nb_data;  // Number of datapoints in a trajectory
+	int   nb_RBF;     // Number of radial basis functions
+	int   nb_data;    // Number of datapoints in a trajectory
+	float sigma_RBF;  // Homogeneous covariance
 };
 
 
@@ -64,15 +63,14 @@ std::tuple<matrix_list_t, mat> compute_LWR(const parameters_t& parameters,
 	// Set centroids equally spread in time
 	vec mu_RBF = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_RBF);
 
-	// Set constant homogeneous covariance
-	double sigma_RBF = 100.0;
-
 	mat H = zeros(parameters.nb_RBF, parameters.nb_data);
 	for (int i = 0; i < parameters.nb_RBF; ++i) {
 		H(i, span::all) = gaussPDF(linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data),
-								   mu_RBF(i), sigma_RBF).t();
+								   mu_RBF(i), parameters.sigma_RBF).t();
 	}
 
+	H = H / repmat(sum(H, 0), parameters.nb_RBF, 1);  // Rescaling
+
 	// Incremental estimate (does not require matrix inversion)
 	mat w = zeros(parameters.nb_RBF, 2); // Initial estimate of w
 	mat iB = eye(parameters.nb_RBF, parameters.nb_RBF) * 1e3; // Initial estimate of iB (set to big value)
@@ -132,7 +130,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0) {
+					double near_distance = -1.0, double far_distance = 1.0) {
 
 	viewport->x = x;
 	viewport->y = y;
@@ -142,8 +140,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 }
 
 
@@ -225,6 +223,7 @@ struct gui_state_t {
 	// in parameters_t)
 	int parameter_nb_RBF;
 	int parameter_nb_data;
+	float parameter_sigma_RBF;
 };
 
 
@@ -484,8 +483,9 @@ int main(int argc, char **argv) {
 
 	// Parameters
 	parameters_t parameters;
-	parameters.nb_RBF  = 16;
-	parameters.nb_data = 100;
+	parameters.nb_RBF    = 16;
+	parameters.nb_data   = 100;
+	parameters.sigma_RBF = 1e2f;
 
 
 	// Take 4k screens into account (framebuffer size != window size)
@@ -509,7 +509,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Locally weighted regression (LWR)",
 		window_size.win_width, window_size.win_height
 	);
@@ -517,7 +517,7 @@ int main(int argc, char **argv) {
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -543,6 +543,7 @@ int main(int argc, char **argv) {
 	gui_state.must_recompute_LWR = false;
 	gui_state.parameter_nb_RBF = parameters.nb_RBF;
 	gui_state.parameter_nb_data = parameters.nb_data;
+	gui_state.parameter_sigma_RBF = parameters.sigma_RBF;
 
 
 	// Main loop
@@ -556,14 +557,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 2 - 1;
 			viewport_height = window_size.fb_height / 2 - 1;
@@ -584,6 +585,7 @@ int main(int argc, char **argv) {
 
 			parameters.nb_RBF = gui_state.parameter_nb_RBF;
 			parameters.nb_data = gui_state.parameter_nb_data;
+			parameters.sigma_RBF = gui_state.parameter_sigma_RBF;
 
 			gui_state.must_recompute_LWR = true;
 			gui_state.are_parameters_modified = false;
@@ -665,9 +667,9 @@ int main(int argc, char **argv) {
 
 
 		// Window: Parameters
-		ImGui::SetNextWindowSize(ImVec2(440, 106));
+		ImGui::SetNextWindowSize(ImVec2(440, 126));
 		ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2,
-									   (window_size.win_height - 106) / 2));
+									   (window_size.win_height - 126) / 2));
 		ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
 
 		if (gui_state.is_parameters_dialog_displayed)
@@ -679,6 +681,7 @@ int main(int argc, char **argv) {
 
 			ImGui::SliderInt("Nb RBF", &gui_state.parameter_nb_RBF, 2, 20);
 			ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+			ImGui::SliderFloat("Sigma RBF", &gui_state.parameter_sigma_RBF, 1e0, 1e3);
 
 			if (ImGui::Button("Close")) {
 				ImGui::CloseCurrentPopup();
diff --git a/src/demo_MPC_batch01.cpp b/src/demo_MPC_batch01.cpp
index cf7876a..008ad82 100644
--- a/src/demo_MPC_batch01.cpp
+++ b/src/demo_MPC_batch01.cpp
@@ -15,7 +15,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 #include <gl2ps.h>
 
 using namespace arma;
@@ -225,14 +224,14 @@ int main(int argc, char **argv){
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Batch MPC", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -260,47 +259,41 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		// At the very first frame: random initialisation of the gaussians (taking 4K
+		// screens into account)
+		if (window_result == gfx2::WINDOW_READY) {
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+			Mu = randu(2, parameters.nb_targets);
+			Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+			Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
 
-			// At the very first frame: random initialisation of the gaussians (taking 4K
-			// screens into account)
-			if (first && (window_size.fb_width > 0)) {
+			randomCovariances(
+				&Sigma, Mu,
+				vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+					  100 * (float) window_size.fb_height / window_size.win_height
+				}),
+				true, 0.0, 0.2
+			);
 
-				Mu = randu(2, parameters.nb_targets);
-				Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
-				Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+			for (int i = 0; i < parameters.nb_targets; ++i) {
+				gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
 
-				randomCovariances(
-					&Sigma, Mu,
-					vec({ 100 * (float) window_size.fb_width / window_size.win_width,
-						  100 * (float) window_size.fb_height / window_size.win_height
-					}),
-					true, 0.0, 0.2
-				);
+				fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
 
-				for (int i = 0; i < parameters.nb_targets; ++i) {
-					gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
+				fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+				t2ds[i].x.x = x(0);
+				t2ds[i].x.y = x(1);
 
-					fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
-
-					fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
-					t2ds[i].x.x = x(0);
-					t2ds[i].x.y = x(1);
-
-					fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
-					t2ds[i].y.x = y(0);
-					t2ds[i].y.y = y(1);
-				}
+				fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+				t2ds[i].y.x = y(0);
+				t2ds[i].y.y = y(1);
 			}
 		}
 
diff --git a/src/demo_MPC_iterative01.cpp b/src/demo_MPC_iterative01.cpp
index 2f8abb6..fe9ee4b 100644
--- a/src/demo_MPC_iterative01.cpp
+++ b/src/demo_MPC_iterative01.cpp
@@ -16,7 +16,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -234,14 +233,14 @@ int main(int argc, char **argv){
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Iterative MPC", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -267,47 +266,40 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		// At the very first frame: random initialisation of the gaussians (taking 4K
+		// screens into account)
+		if (window_result == gfx2::WINDOW_READY) {
+			Mu = randu(2, parameters.nb_targets);
+			Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+			Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+			randomCovariances(
+				&Sigma, Mu,
+				vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+					  100 * (float) window_size.fb_height / window_size.win_height
+				}),
+				true, 0.0, 0.2
+			);
 
-			// At the very first frame: random initialisation of the gaussians (taking 4K
-			// screens into account)
-			if (first && (window_size.fb_width > 0)) {
+			for (int i = 0; i < parameters.nb_targets; ++i) {
+				gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
 
-				Mu = randu(2, parameters.nb_targets);
-				Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
-				Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+				fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
 
-				randomCovariances(
-					&Sigma, Mu,
-					vec({ 100 * (float) window_size.fb_width / window_size.win_width,
-						  100 * (float) window_size.fb_height / window_size.win_height
-					}),
-					true, 0.0, 0.2
-				);
+				fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+				t2ds[i].x.x = x(0);
+				t2ds[i].x.y = x(1);
 
-				for (int i = 0; i < parameters.nb_targets; ++i) {
-					gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
-
-					fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
-
-					fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
-					t2ds[i].x.x = x(0);
-					t2ds[i].x.y = x(1);
-
-					fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
-					t2ds[i].y.x = y(0);
-					t2ds[i].y.y = y(1);
-				}
+				fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+				t2ds[i].y.x = y(0);
+				t2ds[i].y.y = y(1);
 			}
 		}
 
diff --git a/src/demo_MPC_semitied01.cpp b/src/demo_MPC_semitied01.cpp
index d390059..b58fa56 100644
--- a/src/demo_MPC_semitied01.cpp
+++ b/src/demo_MPC_semitied01.cpp
@@ -15,7 +15,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 #include <gl2ps.h>
 
 using namespace arma;
@@ -259,14 +258,14 @@ int main(int argc, char **argv){
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Semitied MPC", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -294,32 +293,25 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
-
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
-
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-
-			// At the very first frame: random initialisation of the gaussians (taking 4K
-			// screens into account)
-			if (first && (window_size.fb_width > 0)) {
-
-				Mu = randu(2, parameters.nb_targets);
-				Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
-				Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
-
-				semitied_params = {
-					0.0,
-					datum::pi / 2.0,
-					100.0 * (float) window_size.fb_height / window_size.win_height
-				};
-			}
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
+
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
+
+		// At the very first frame: random initialisation of the gaussians (taking 4K
+		// screens into account)
+		if (window_result == gfx2::WINDOW_READY) {
+			Mu = randu(2, parameters.nb_targets);
+			Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+			Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+
+			semitied_params = {
+				0.0,
+				datum::pi / 2.0,
+				100.0 * (float) window_size.fb_height / window_size.win_height
+			};
 		}
 
 
diff --git a/src/demo_MPC_velocity01.cpp b/src/demo_MPC_velocity01.cpp
index 69677ca..dd9de26 100644
--- a/src/demo_MPC_velocity01.cpp
+++ b/src/demo_MPC_velocity01.cpp
@@ -15,7 +15,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -262,14 +261,14 @@ int main(int argc, char **argv){
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Velocity MPC", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -299,72 +298,66 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		// At the very first frame: random initialisation of the gaussians
+		if (window_result == gfx2::WINDOW_READY) {
+			Mu = randu(2, parameters.nb_targets);
+			Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+			Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
 
-			// At the very first frame: initialise the UI widgets of the gaussians
-			if (first && (window_size.fb_width > 0)) {
+			randomCovariances(
+				&Sigma, Mu,
+				vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+					  100 * (float) window_size.fb_height / window_size.win_height
+				}),
+				true, 0.0, 0.2
+			);
 
-				Mu = randu(2, parameters.nb_targets);
-				Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
-				Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+			Mu0 = randu(2, parameters.nb_repulsive_gaussians);
+			Mu0.row(0) = Mu0.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+			Mu0.row(1) = Mu0.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
 
-				randomCovariances(
-					&Sigma, Mu,
-					vec({ 100 * (float) window_size.fb_width / window_size.win_width,
-						  100 * (float) window_size.fb_height / window_size.win_height
-					}),
-					true, 0.0, 0.2
-				);
+			randomCovariances(
+				&Sigma0, Mu0,
+				vec({ 150 * (float) window_size.fb_width / window_size.win_width,
+					  150 * (float) window_size.fb_height / window_size.win_height
+				}),
+				true, 0.0, 0.2
+			);
 
-				Mu0 = randu(2, parameters.nb_repulsive_gaussians);
-				Mu0.row(0) = Mu0.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
-				Mu0.row(1) = Mu0.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+			for (int i = 0; i < parameters.nb_targets; ++i) {
+				gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
 
-				randomCovariances(
-					&Sigma0, Mu0,
-					vec({ 150 * (float) window_size.fb_width / window_size.win_width,
-						  150 * (float) window_size.fb_height / window_size.win_height
-					}),
-					true, 0.0, 0.2
-				);
+				fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
 
-				for (int i = 0; i < parameters.nb_targets; ++i) {
-					gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
+				fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+				t2ds[i].x.x = x(0);
+				t2ds[i].x.y = x(1);
 
-					fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
-
-					fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
-					t2ds[i].x.x = x(0);
-					t2ds[i].x.y = x(1);
-
-					fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
-					t2ds[i].y.x = y(0);
-					t2ds[i].y.y = y(1);
-				}
+				fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+				t2ds[i].y.x = y(0);
+				t2ds[i].y.y = y(1);
+			}
 
-				for (int i = 0; i < parameters.nb_repulsive_gaussians; ++i) {
-					gauss_to_trans2d(Mu0.col(i), Sigma0.slice(i), window_size, t2ds0[i]);
+			for (int i = 0; i < parameters.nb_repulsive_gaussians; ++i) {
+				gauss_to_trans2d(Mu0.col(i), Sigma0.slice(i), window_size, t2ds0[i]);
 
-					fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
+				fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
 
-					fvec x = rotation * fvec({ t2ds0[i].x.x, t2ds0[i].x.y, 0.0f, 0.0f });
-					t2ds0[i].x.x = x(0);
-					t2ds0[i].x.y = x(1);
+				fvec x = rotation * fvec({ t2ds0[i].x.x, t2ds0[i].x.y, 0.0f, 0.0f });
+				t2ds0[i].x.x = x(0);
+				t2ds0[i].x.y = x(1);
 
-					fvec y = rotation * fvec({ t2ds0[i].y.x, t2ds0[i].y.y, 0.0f, 0.0f });
-					t2ds0[i].y.x = y(0);
-					t2ds0[i].y.y = y(1);
-				}
+				fvec y = rotation * fvec({ t2ds0[i].y.x, t2ds0[i].y.y, 0.0f, 0.0f });
+				t2ds0[i].y.x = y(0);
+				t2ds0[i].y.y = y(1);
 			}
 		}
 
diff --git a/src/demo_Riemannian_cov_GMR01.cpp b/src/demo_Riemannian_cov_GMR01.cpp
index c7644eb..addd798 100644
--- a/src/demo_Riemannian_cov_GMR01.cpp
+++ b/src/demo_Riemannian_cov_GMR01.cpp
@@ -26,7 +26,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -60,7 +59,7 @@ const arma::fmat COLORS({
 /*********************************** TYPES ***********************************/
 
 // 4-dimensional tensor of doubles
-typedef Tensor<double, 4> Tensor4D;
+typedef TensorDouble Tensor4D;
 
 //-----------------------------------------------
 
@@ -762,7 +761,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Gaussian Mixture Regression",
 		window_size.win_width, window_size.win_height
 	);
@@ -771,7 +770,7 @@ int main(int argc, char **argv) {
 
 	float workspace_size = (parameters.nb_data + 1) * parameters.dt;
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_CULL_FACE);
 	glEnable(GL_LINE_SMOOTH);
@@ -787,16 +786,12 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
 
 		// Detect if the parameters have changed
diff --git a/src/demo_Riemannian_cov_GMR_Mandel01.cpp b/src/demo_Riemannian_cov_GMR_Mandel01.cpp
index 3aa49a4..9ea3bf7 100644
--- a/src/demo_Riemannian_cov_GMR_Mandel01.cpp
+++ b/src/demo_Riemannian_cov_GMR_Mandel01.cpp
@@ -27,7 +27,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -556,7 +555,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Gaussian Mixture Regression",
 		window_size.win_width, window_size.win_height
 	);
@@ -565,7 +564,7 @@ int main(int argc, char **argv) {
 
 	float workspace_size = (parameters.nb_data + 1) * parameters.dt;
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_CULL_FACE);
 	glEnable(GL_LINE_SMOOTH);
@@ -581,16 +580,12 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
 
 		// Detect if the parameters have changed
diff --git a/src/demo_Riemannian_cov_interp02.cpp b/src/demo_Riemannian_cov_interp02.cpp
index 40e145c..877e9ed 100644
--- a/src/demo_Riemannian_cov_interp02.cpp
+++ b/src/demo_Riemannian_cov_interp02.cpp
@@ -17,7 +17,6 @@
 #include <gfx2.h>
 #include <gfx_ui.h>
 #include <GLFW/glfw3.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -169,14 +168,14 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Covariance interpolation",
 		window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -195,16 +194,12 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
 		// Start of rendering
 		ImGui_ImplGlfwGL2_NewFrame();
@@ -223,28 +218,25 @@ int main(int argc, char **argv) {
 
 		glPushMatrix();
 
-		// Ensure that the number of desired states hasn't changed (only once everything
-		// is initialised)
-		if (window_size.fb_width > 0) {
-			if (nb_states > gaussian_transforms.size()) {
-				for (int i = gaussian_transforms.size(); i < nb_states; ++i) {
-					arma::vec mu = arma::randu(2);
-					mu(0) = mu(0) * (window_size.win_width - 200) + 100;
-					mu(1) = mu(1) * (window_size.win_height - 200) + 100;
-
-					arma::vec xy = arma::randu(2);
-					xy(0) = (xy(0) * window_size.win_width / 6 + 20);
-					xy(1) = (xy(1) * window_size.win_height / 6 + 20);
-
-					gaussian_transforms.push_back(ui::Trans2d(ImVec2((int) xy(0), 0),
-															  ImVec2(0, (int) xy(1)),
-															  ImVec2((int) mu(0), (int) mu(1))));
-				}
-			}
-			else if (nb_states < gaussian_transforms.size()) {
-				gaussian_transforms.resize(nb_states);
+		// Ensure that the number of desired states hasn't changed
+		if (nb_states > gaussian_transforms.size()) {
+			for (int i = gaussian_transforms.size(); i < nb_states; ++i) {
+				arma::vec mu = arma::randu(2);
+				mu(0) = mu(0) * (window_size.win_width - 200) + 100;
+				mu(1) = mu(1) * (window_size.win_height - 200) + 100;
+
+				arma::vec xy = arma::randu(2);
+				xy(0) = (xy(0) * window_size.win_width / 6 + 20);
+				xy(1) = (xy(1) * window_size.win_height / 6 + 20);
+
+				gaussian_transforms.push_back(ui::Trans2d(ImVec2((int) xy(0), 0),
+														  ImVec2(0, (int) xy(1)),
+														  ImVec2((int) mu(0), (int) mu(1))));
 			}
 		}
+		else if (nb_states < gaussian_transforms.size()) {
+			gaussian_transforms.resize(nb_states);
+		}
 
 		// Interpolation between the gaussians
 		std::vector<arma::vec> interpolated_mu;
diff --git a/src/demo_Riemannian_pose_GMM01.cpp b/src/demo_Riemannian_pose_GMM01.cpp
index 6898975..5e9b3a4 100644
--- a/src/demo_Riemannian_pose_GMM01.cpp
+++ b/src/demo_Riemannian_pose_GMM01.cpp
@@ -7,6 +7,10 @@
  *      Author: Andras Kupcsik
  */
 
+#ifdef _WIN32
+#define _USE_MATH_DEFINES
+#endif
+
 #include <stdio.h>
 #include <armadillo>
 #include <cfloat>
@@ -279,7 +283,7 @@ int main(int argc, char **argv) {
 			mat demoUColID = zeros(nbVar, id.n_cols);
 
 			for (int j = 0; j < id.n_elem; j++) {
-				demoUColID.col(j) = demoU.col((uint) id(j));
+				demoUColID.col(j) = demoU.col((unsigned int) id(j));
 			}
 
 			Mu.col(i) = mean(demoUColID, 1);
diff --git a/src/demo_Riemannian_pose_TPGMM01.cpp b/src/demo_Riemannian_pose_TPGMM01.cpp
index 9c9b2ee..45f47c5 100644
--- a/src/demo_Riemannian_pose_TPGMM01.cpp
+++ b/src/demo_Riemannian_pose_TPGMM01.cpp
@@ -7,6 +7,10 @@
  *      Author: Andras Kupcsik
  */
 
+#ifdef _WIN32
+#define _USE_MATH_DEFINES
+#endif
+
 #include <stdio.h>
 #include <armadillo>
 
@@ -292,7 +296,7 @@ int main(int argc, char **argv) {
 			for (int j = 0; j < nbStates; j++) {
 				vec idLoc = nbSamplesCumSum(i) + round(linspace(idloc(j), idloc(j+1)-1, idloc(j+1)-1-idloc(j)));
 				for (int k = 0; k < idLoc.n_elem; k++) {
-					tmpField(j) = join_horiz(tmpField(j), demoU.col((uint) idLoc(k)));
+					tmpField(j) = join_horiz(tmpField(j), demoU.col((unsigned int) idLoc(k)));
 				}
 			}
 		}
diff --git a/src/demo_Riemannian_pose_batchLQR01.cpp b/src/demo_Riemannian_pose_batchLQR01.cpp
index eb0205e..27eda59 100644
--- a/src/demo_Riemannian_pose_batchLQR01.cpp
+++ b/src/demo_Riemannian_pose_batchLQR01.cpp
@@ -12,37 +12,16 @@
 #include <vector>
 #include <ctime>
 
+#include <gfx2.h>
+#include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <gfx_ui.h>
-#include <window_utils.h>
 
 using namespace arma;
 
 
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-
-	inline float scale_x() const {
-		return (float) fb_width / (float) win_width;
-	}
-
-	inline float scale_y() const {
-		return (float) fb_height / (float) win_height;
-	}
-};
-
-//-----------------------------------------------
-
 static void error_callback(int error, const char* description) {
 	fprintf(stderr, "Error %d: %s\n", error, description);
 }
@@ -247,7 +226,7 @@ const arma::fmat COLORS({
 int main(int argc, char **argv) {
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 780;
 	window_size.win_height = 540;
 	window_size.fb_width = -1;	// Will be known later
@@ -258,7 +237,7 @@ int main(int argc, char **argv) {
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Riemannian Pose batchLQR", window_size.win_width, window_size.win_height
 	);
 
@@ -342,16 +321,13 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
 
 		ImGui_ImplGlfwGL2_NewFrame();
 
diff --git a/src/demo_Riemannian_pose_infHorLQR01.cpp b/src/demo_Riemannian_pose_infHorLQR01.cpp
index 3513882..3e01b73 100644
--- a/src/demo_Riemannian_pose_infHorLQR01.cpp
+++ b/src/demo_Riemannian_pose_infHorLQR01.cpp
@@ -12,35 +12,16 @@
 
 #include <armadillo>
 
+#include <gfx2.h>
+#include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <gfx_ui.h>
-#include <window_utils.h>
 
 using namespace arma;
 
 
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-
-	inline float scale_x() const {
-		return (float) fb_width / (float) win_width;
-	}
-
-	inline float scale_y() const {
-		return (float) fb_height / (float) win_height;
-	}
-};
-
 //-----------------------------------------------
 
 static void error_callback(int error, const char* description) {
@@ -268,7 +249,7 @@ const arma::fmat COLORS({
 int main(int argc, char **argv) {
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 780;
 	window_size.win_height = 540;
 	window_size.fb_width = -1;	// Will be known later
@@ -279,7 +260,7 @@ int main(int argc, char **argv) {
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Riemannian Pose InfHorLQR", window_size.win_width, window_size.win_height
 	);
 
@@ -347,16 +328,13 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
 
 		ImGui_ImplGlfwGL2_NewFrame();
 
diff --git a/src/demo_Riemannian_quat_TPGMM01.cpp b/src/demo_Riemannian_quat_TPGMM01.cpp
index db8d3d9..56ef0f5 100644
--- a/src/demo_Riemannian_quat_TPGMM01.cpp
+++ b/src/demo_Riemannian_quat_TPGMM01.cpp
@@ -8,41 +8,21 @@
  */
 
 #include <stdio.h>
+#include <vector>
+#include <armadillo>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <GLFW/glfw3.h>
-#include <gfx_ui.h>
-#include <window_utils.h>
 
-#include <vector>
-#include <armadillo>
 #include <lqr.h>
 
 using namespace arma;
 
 
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-
-	inline float scale_x() const {
-		return (float) fb_width / (float) win_width;
-	}
-
-	inline float scale_y() const {
-		return (float) fb_height / (float) win_height;
-	}
-};
-
-//-----------------------------------------------
-
 static void error_callback(int error, const char* description) {
 	fprintf(stderr, "Error %d: %s\n", error, description);
 }
@@ -225,7 +205,7 @@ void DrawFrame(ImGuiWindow& win, float lineWidth, mat A, vec center) {
 int main(int argc, char **argv) {
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 780;
 	window_size.win_height = 540;
 	window_size.fb_width = -1;	// Will be known later
@@ -236,7 +216,7 @@ int main(int argc, char **argv) {
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Riemannian quaternion TP-GMM", window_size.win_width, window_size.win_height
 	);
 
@@ -355,7 +335,7 @@ int main(int argc, char **argv) {
 
 				for (int j = 0; j < id.n_elem; j++) {
 
-					demoUColID.col(j) = demoU.col((uint) id(j));
+					demoUColID.col(j) = demoU.col((unsigned int) id(j));
 				}
 
 				Mu.col(i) = mean(demoUColID, 1);
@@ -537,16 +517,9 @@ int main(int argc, char **argv) {
 
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
-
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
 		ImGui_ImplGlfwGL2_NewFrame();
 
@@ -566,7 +539,7 @@ int main(int argc, char **argv) {
 		ImGui::SetNextWindowSize(ImVec2(400, 54));
 		ImGui::Begin("Parameters", NULL, ImGuiWindowFlags_NoResize |
 					 ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoMove);
-		ImGui::SliderInt("Nb states", &nbStates, 2, 7);
+		ImGui::SliderInt("Nb states", &nbStates, 2, 6);
 
 		ImGui::End();
 		ImVec4 clip_rect2(0, 0, window_size.win_width, window_size.win_height);
diff --git a/src/demo_Riemannian_quat_infHorLQR01.cpp b/src/demo_Riemannian_quat_infHorLQR01.cpp
index 9923e97..b22c066 100644
--- a/src/demo_Riemannian_quat_infHorLQR01.cpp
+++ b/src/demo_Riemannian_quat_infHorLQR01.cpp
@@ -13,12 +13,12 @@
 #include <stdlib.h>
 #include <math.h>
 
+#include <gfx2.h>
+#include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <gfx_ui.h>
-#include <window_utils.h>
 
 namespace linmath{
 #include <linmath.h>
@@ -50,26 +50,6 @@ typedef struct {float x; float y; float z;} vertex_t;
 void DrawSphereBand( GLfloat long_lo, GLfloat long_hi );
 void DrawSphere( void );
 
-
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-
-	inline float scale_x() const {
-		return (float) fb_width / (float) win_width;
-	}
-
-	inline float scale_y() const {
-		return (float) fb_height / (float) win_height;
-	}
-};
-
 //-----------------------------------------------
 
 static void error_callback(int error, const char* description)
@@ -227,7 +207,7 @@ arma::mat logmap(arma::mat x, arma::vec mu)
 //-----------------------------------------------
 
 void DrawResults(int nbData, int nbRepros, const arma::mat& x_y,
-				 const window_size_t& window_size)
+				 const gfx2::window_size_t& window_size)
 {
 	const int plot_width = (window_size.win_width - 3 * MARGIN) / 2;
 	const int plot_height = (window_size.win_height - 3 * MARGIN) / 2;
@@ -551,7 +531,7 @@ int main(int argc, char **argv){
 	//------------------------ Plots ------------------------
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 500;
 	window_size.win_height = 500;
 	window_size.fb_width = -1;	// Will be known later
@@ -562,7 +542,7 @@ int main(int argc, char **argv){
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Quaternions", window_size.win_width, window_size.win_height
 	);
 
@@ -581,22 +561,13 @@ int main(int argc, char **argv){
 
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		int current_win_width;
-		int current_win_height;
-		glfwGetWindowSize(window, &current_win_width, &current_win_height);
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-		if ((current_win_width != window_size.win_width) ||
-			(current_win_height != window_size.win_height) ||
-			(window_size.fb_width == -1)) {
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			// Retrieve the new window size
-			window_size.win_width = current_win_width;
-			window_size.win_height = current_win_height;
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
 
 		if (!REFERENTIALS)
 		{
diff --git a/src/demo_Riemannian_sphere_GMM01.cpp b/src/demo_Riemannian_sphere_GMM01.cpp
index f63f869..a8372d1 100644
--- a/src/demo_Riemannian_sphere_GMM01.cpp
+++ b/src/demo_Riemannian_sphere_GMM01.cpp
@@ -9,15 +9,13 @@
 
 #include <stdio.h>
 #include <armadillo>
+#include <sstream>
 
 #include <gfx2.h>
 #include <gfx_ui.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
-
-#include <sstream>
 
 using namespace arma;
 
@@ -218,13 +216,13 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Riemannian sphere gmm", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -268,8 +266,9 @@ int main(int argc, char **argv) {
 
 	// Creation of the light
 	gfx2::point_light_t light;
-	light.transforms.position = arma::fvec( { 0.0f, 0.0f, 5.0f, 1.0f });
-	light.color = arma::fvec( { 1.0f, 1.0f, 1.0f });
+	light.transforms.position = arma::fvec({ 0.0f, 0.0f, 5.0f, 1.0f });
+	light.diffuse_color = arma::fvec({ 1.0f, 1.0f, 1.0f });
+	light.ambient_color = arma::fvec({ 0.1f, 0.1f, 0.1f });
 
 	gfx2::light_list_t lights;
 	lights.push_back(light);
@@ -342,15 +341,13 @@ int main(int argc, char **argv) {
 		glfwPollEvents();
 		previous_nbStates = nbStates;
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
 
 		if (must_recompute) {
 			for (int i = 0; i < gaussian_models.size(); i++) {
diff --git a/src/demo_Riemannian_sphere_TPGMM01.cpp b/src/demo_Riemannian_sphere_TPGMM01.cpp
index 3c91a78..aff34dd 100644
--- a/src/demo_Riemannian_sphere_TPGMM01.cpp
+++ b/src/demo_Riemannian_sphere_TPGMM01.cpp
@@ -15,7 +15,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -242,13 +241,13 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Riemannian sphere tpgmm", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -304,8 +303,9 @@ int main(int argc, char **argv) {
 
 	// Creation of the light
 	gfx2::point_light_t light;
-	light.transforms.position = arma::fvec( { -3.0f, 3.0f, 5.0f, 1.0f });
-	light.color = arma::fvec( { 1.0f, 1.0f, 1.0f });
+	light.transforms.position = arma::fvec({ 0.0f, 0.0f, 5.0f, 1.0f });
+	light.diffuse_color = arma::fvec({ 1.0f, 1.0f, 1.0f });
+	light.ambient_color = arma::fvec({ 0.1f, 0.1f, 0.1f });
 
 	gfx2::light_list_t lights;
 	lights.push_back(light);
@@ -412,14 +412,14 @@ int main(int argc, char **argv) {
 		glfwPollEvents();
 		previous_nbStates = nbStates;
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			// Update the projection matrix
 			projection = gfx2::perspective(
@@ -489,7 +489,7 @@ int main(int argc, char **argv) {
 
 				for (int j = 0; j < id.n_elem; j++) {
 
-					demoUColID.col(j) = demoU.col((uint) id(j));
+					demoUColID.col(j) = demoU.col((unsigned int) id(j));
 				}
 
 				Mu.col(i) = mean(demoUColID, 1);
diff --git a/src/demo_Riemannian_sphere_infHorLQR01.cpp b/src/demo_Riemannian_sphere_infHorLQR01.cpp
index cceeed2..efaa5cc 100644
--- a/src/demo_Riemannian_sphere_infHorLQR01.cpp
+++ b/src/demo_Riemannian_sphere_infHorLQR01.cpp
@@ -16,7 +16,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -302,13 +301,13 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Riemannian sphere", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -408,7 +407,8 @@ int main(int argc, char **argv) {
 	// Creation of the light
 	gfx2::point_light_t light;
 	light.transforms.position = arma::fvec({0.0f, 0.0f, 5.0f, 1.0f});
-	light.color = arma::fvec({1.0f, 1.0f, 1.0f});
+	light.diffuse_color = arma::fvec({1.0f, 1.0f, 1.0f});
+	light.ambient_color = arma::fvec({0.1f, 0.1f, 0.1f});
 
 	gfx2::light_list_t lights;
 	lights.push_back(light);
@@ -430,14 +430,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			// Update the projection matrix
 			projection = gfx2::perspective(
diff --git a/src/demo_Riemannian_sphere_product01.cpp b/src/demo_Riemannian_sphere_product01.cpp
index cb932bc..09e1f97 100644
--- a/src/demo_Riemannian_sphere_product01.cpp
+++ b/src/demo_Riemannian_sphere_product01.cpp
@@ -20,7 +20,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -237,13 +236,13 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Riemannian sphere product", window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_DEPTH_TEST);
 	glEnable(GL_CULL_FACE);
@@ -341,8 +340,9 @@ int main(int argc, char **argv) {
 
 	// Creation of the light
 	gfx2::point_light_t light;
-	light.transforms.position = arma::fvec( { 0.0f, 0.0f, 5.0f, 1.0f });
-	light.color = arma::fvec( { 1.0f, 1.0f, 1.0f });
+	light.transforms.position = arma::fvec({ 0.0f, 0.0f, 5.0f, 1.0f });
+	light.diffuse_color = arma::fvec({ 1.0f, 1.0f, 1.0f });
+	light.ambient_color = arma::fvec({ 0.1f, 0.1f, 0.1f });
 
 	gfx2::light_list_t lights;
 	lights.push_back(light);
@@ -359,14 +359,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			// Update the projection matrix
 			projection = gfx2::perspective(
diff --git a/src/demo_TPGMMProduct01.cpp b/src/demo_TPGMMProduct01.cpp
index 5fb0870..8aca48f 100644
--- a/src/demo_TPGMMProduct01.cpp
+++ b/src/demo_TPGMMProduct01.cpp
@@ -28,7 +28,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui_internal.h>
@@ -429,7 +428,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0,
+					double near_distance = -1.0, double far_distance = 1.0,
 					const fvec& view_transforms = zeros<fvec>(3)) {
 
 	viewport->x = x;
@@ -440,8 +439,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 	viewport->view = view_transforms;
 }
 
@@ -537,7 +536,7 @@ class Handler
 {
 public:
 	Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
-			int index, bool small)
+			int index, bool is_small)
 	: viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
 	{
 		ui_position = position;
@@ -545,7 +544,7 @@ public:
 
 		fvec color = HANDLER_COLORS.row(index).t();
 
-		if (small) {
+		if (is_small) {
 			models[0] = gfx2::create_rectangle(color, 12.0f, 2.0f);
 
 			models[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
@@ -724,13 +723,13 @@ struct gui_state_t {
 // Create a handler at a random position (within the given boundaries)
 //-----------------------------------------------------------------------------
 Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
-							   int min_y, int max_x, int max_y, bool small) {
+							   int min_y, int max_x, int max_y, bool is_small) {
 	return new Handler(viewport,
 					   ImVec2((randu() * 0.8f + 0.1f) * (max_x - min_x) + min_x,
 							  (randu() * 0.5f + 0.1f) * (max_y - min_y) + min_y),
 					   ImVec2((randu() - 0.5) * 10, randu() * -10 - 10),
 					   index,
-					   small
+					   is_small
 	);
 }
 
@@ -1106,14 +1105,14 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
-			"Demo - Gaussian Product for TPGMM",
-			window_size.win_width, window_size.win_height
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
+		"Demo - Gaussian Product for TPGMM",
+		window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -1156,16 +1155,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
-
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 2 - 1;
 			viewport_height = window_size.fb_height;
@@ -1178,9 +1175,8 @@ int main(int argc, char **argv) {
 						   window_size.fb_height - viewport_height,
 						   viewport_width, viewport_height);
 
-
 			// Update all the handlers
-			if (!first) {
+			if (window_result == gfx2::WINDOW_RESIZED) {
 				for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
 					current_demonstration_handlers[i]->viewport_resized(window_size);
 
@@ -1194,7 +1190,7 @@ int main(int argc, char **argv) {
 			//
 			// Note: The loaded data was recorded in another demo with 3x2 viewports,
 			// so appropriate scaling must be applied
-			else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
+			else if (window_result == gfx2::WINDOW_READY) {
 				cube loaded_trajectories;
 				mat loaded_frames;
 				loaded_trajectories.load("data/data_4_trajectories.txt");
diff --git a/src/demo_TPGMR01.cpp b/src/demo_TPGMR01.cpp
index 987d0c2..4dac2a4 100644
--- a/src/demo_TPGMR01.cpp
+++ b/src/demo_TPGMR01.cpp
@@ -29,7 +29,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui_internal.h>
@@ -436,7 +435,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0,
+					double near_distance = -1.0, double far_distance = 1.0,
 					const fvec& view_transforms = zeros<fvec>(3)) {
 
 	viewport->x = x;
@@ -447,8 +446,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 	viewport->view = view_transforms;
 }
 
@@ -544,7 +543,7 @@ class Handler
 {
 public:
 	Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
-			int index, bool small)
+			int index, bool is_small)
 	: viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
 	{
 		ui_position = position;
@@ -552,7 +551,7 @@ public:
 
 		fvec color = HANDLER_COLORS.row(index).t();
 
-		if (small) {
+		if (is_small) {
 			models[0] = gfx2::create_rectangle(color, 12.0f, 2.0f);
 
 			models[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
@@ -733,13 +732,13 @@ struct gui_state_t {
 // Create a handler at a random position (within the given boundaries)
 //-----------------------------------------------------------------------------
 Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
-							   int min_y, int max_x, int max_y, bool small) {
+							   int min_y, int max_x, int max_y, bool is_small) {
 	return new Handler(viewport,
 					   ImVec2((randu() * 0.8f + 0.1f) * (max_x - min_x) + min_x,
 							  (randu() * 0.5f + 0.1f) * (max_y - min_y) + min_y),
 					   ImVec2((randu() - 0.5) * 10, randu() * -10 - 10),
 					   index,
-					   small
+					   is_small
 	);
 }
 
@@ -1328,14 +1327,14 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
-			"Demo - TPGMR",
-			window_size.win_width, window_size.win_height
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
+		"Demo - TPGMR",
+		window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -1386,16 +1385,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
-
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 3 - 1;
 			viewport_height = window_size.fb_height;
@@ -1414,7 +1411,7 @@ int main(int argc, char **argv) {
 
 
 			// Update all the handlers
-			if (!first) {
+			if (window_result == gfx2::WINDOW_RESIZED) {
 				for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
 					current_demonstration_handlers[i]->viewport_resized(window_size);
 
@@ -1431,7 +1428,7 @@ int main(int argc, char **argv) {
 			//
 			// Note: The loaded data was recorded in another demo with 3x2 viewports,
 			// so appropriate scaling must be applied
-			else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
+			else if (window_result == gfx2::WINDOW_READY) {
 
 				cube loaded_trajectories;
 				mat loaded_frames;
diff --git a/src/demo_TPbatchLQR01.cpp b/src/demo_TPbatchLQR01.cpp
index 7e1832f..6ca1547 100644
--- a/src/demo_TPbatchLQR01.cpp
+++ b/src/demo_TPbatchLQR01.cpp
@@ -30,7 +30,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui_internal.h>
@@ -717,7 +716,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0,
+					double near_distance = -1.0, double far_distance = 1.0,
 					const fvec& view_transforms = zeros<fvec>(3)) {
 
 	viewport->x = x;
@@ -728,8 +727,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 	viewport->view = view_transforms;
 }
 
@@ -825,7 +824,7 @@ class Handler
 {
 public:
 	Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
-			int index, bool small)
+			int index, bool is_small)
 	: viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
 	{
 		ui_position = position;
@@ -833,7 +832,7 @@ public:
 
 		fvec color = HANDLER_COLORS.row(index).t();
 
-		if (small) {
+		if (is_small) {
 			models[0] = gfx2::create_rectangle(color, 12.0f, 2.0f);
 
 			models[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
@@ -1019,13 +1018,13 @@ struct gui_state_t {
 // Create a handler at a random position (within the given boundaries)
 //-----------------------------------------------------------------------------
 Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
-							   int min_y, int max_x, int max_y, bool small) {
+							   int min_y, int max_x, int max_y, bool is_small) {
 	return new Handler(viewport,
 					   ImVec2((randu() * 0.8f + 0.1f) * (max_x - min_x) + min_x,
 							  (randu() * 0.5f + 0.1f) * (max_y - min_y) + min_y),
 					   ImVec2((randu() - 0.5) * 10, randu() * -10 - 10),
 					   index,
-					   small
+					   is_small
 	);
 }
 
@@ -1390,14 +1389,14 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo - Linear quadratic optimal control",
 		window_size.win_width, window_size.win_height
 	);
 
 	glfwMakeContextCurrent(window);
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -1452,16 +1451,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
-
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 3 - 1;
 			viewport_height = window_size.fb_height / 2 - 1;
@@ -1490,7 +1487,7 @@ int main(int argc, char **argv) {
 						   viewport_width, viewport_height);
 
 			// Update all the handlers
-			if (!first) {
+			if (window_result == gfx2::WINDOW_RESIZED) {
 				for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
 					current_demonstration_handlers[i]->viewport_resized(window_size);
 
@@ -1501,7 +1498,7 @@ int main(int argc, char **argv) {
 			// At the very first frame: load initial data from files (can't be done
 			// outside the loop because we need to know the size of the OpenGL front
 			// buffer)
-			else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
+			else if (window_result == gfx2::WINDOW_READY) {
 				cube loaded_trajectories;
 				mat loaded_frames;
 				loaded_trajectories.load("data/data_4_trajectories.txt");
diff --git a/src/demo_online_GMM01.cpp b/src/demo_online_GMM01.cpp
index 10e7c81..233ebf5 100644
--- a/src/demo_online_GMM01.cpp
+++ b/src/demo_online_GMM01.cpp
@@ -9,11 +9,10 @@
 #include <stdio.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
+#include <gfx2.h>
 #include <GLFW/glfw3.h>
-#include <window_utils.h>
 
 #include <armadillo>
-// #include <lqr.h>
 #include <mvn.h>
 
 using namespace arma;
@@ -48,7 +47,7 @@ class GMM_Model
 {
 public:
 	// Constructor
-	GMM_Model(uint _nSTATES, uint _nVARS) {
+	GMM_Model(unsigned int _nSTATES, unsigned int _nVARS) {
 		nVARS = _nVARS;
 		nSTATES = _nSTATES;
 		PRIORS = rowvec(_nSTATES);
@@ -58,7 +57,7 @@ public:
 	}
 
 
-	inline uint getNumSTATES() const {
+	inline unsigned int getNumSTATES() const {
 		return nSTATES;
 	}
 
@@ -156,25 +155,13 @@ public:
 
 
 private:
-	uint			nVARS;
-	uint			nSTATES;
+	unsigned int	nVARS;
+	unsigned int	nSTATES;
 	gaussian_list_t COMPONENTS;
 	rowvec			PRIORS;
 };
 
 
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-};
-
-
 //---------------------------------------------------------
 
 
@@ -212,7 +199,7 @@ int main(int argc, char **argv){
 	//--------------- Setup of the rendering ---------------
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 1200;
 	window_size.win_height = 600;
 	window_size.fb_width = -1;	// Will be known later
@@ -223,7 +210,7 @@ int main(int argc, char **argv){
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Online GMM", window_size.win_width, window_size.win_height
 	);
 
@@ -245,16 +232,12 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
-
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
 		// Start of rendering
 		ImGui_ImplGlfwGL2_NewFrame();
@@ -331,12 +314,12 @@ int main(int argc, char **argv){
 					for (int i = 0; i < (int) gmm.getNumSTATES(); i++) {
 						h.row(i) = trans(mvn::getPDFValue(gmm.getCOMPONENTS(i).mu, gmm.getCOMPONENTS(i).sigma, demo));
 					}
-					uword imax[points.size()];
-					for (int t=0; t<points.size(); t++){
-						vec vTmp = h.col(t);
-						vTmp.max(imax[t]);
-					}
-
+					//uword imax[points.size()];
+					//for (int t=0; t<points.size(); t++){
+					//	vec vTmp = h.col(t);
+					//	vTmp.max(imax[t]);
+					//}
+					//
 					// //LQR
 					// vec vTmp(4,fill::zeros);
 					// mat QTmp(4,4,fill::zeros);
diff --git a/src/demo_online_HSMM01.cpp b/src/demo_online_HSMM01.cpp
index 298d0ec..99f16f7 100644
--- a/src/demo_online_HSMM01.cpp
+++ b/src/demo_online_HSMM01.cpp
@@ -7,13 +7,12 @@
  */
 
 #include <stdio.h>
+#include <gfx2.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
 #include <GLFW/glfw3.h>
-#include <window_utils.h>
 
 #include <armadillo>
-// #include <lqr.h>
 #include <mvn.h>
 
 using namespace arma;
@@ -59,7 +58,7 @@ class HSMM
 {
 public:
 	// Constructor
-	HSMM(uint _nSTATES, uint _nVARS) {
+	HSMM(unsigned int _nSTATES, unsigned int _nVARS) {
 		nVARS = _nVARS;
 		nSTATES = _nSTATES;
 		PRIORS = rowvec(_nSTATES);
@@ -69,7 +68,7 @@ public:
 	}
 
 
-	inline uint getNumSTATES() const {
+	inline unsigned int getNumSTATES() const {
 		return nSTATES;
 	}
 
@@ -167,7 +166,7 @@ public:
 
 		vec S = zeros<vec>(nSTATES);
 
-		for (uint i = 0; i < _AlphaPred.n_cols; i++)
+		for (unsigned int i = 0; i < _AlphaPred.n_cols; i++)
 		{
 			// Alpha variable
 			if (i > 0)
@@ -238,7 +237,7 @@ public:
 				h1 = join_rows( join_rows(
 						zeros<rowvec>(currTime), cumsum(this->Pd.row(iList(iList.n_elem-2))) ),
 						ones<rowvec>( std::max( (nbData-currTime-nbD),0) ));
-				currTime = currTime + round( DurationCOMPONENTS[(uint)iList(iList.n_elem-2)].mu(0,0) );
+				currTime = currTime + round( DurationCOMPONENTS[(unsigned int)iList(iList.n_elem-2)].mu(0,0) );
 			}
 			h2 = join_rows( join_rows(
 					ones<rowvec>(currTime),
@@ -268,7 +267,7 @@ public:
 		for (int i=0; i<(int)nSTATES; i++){
 			h.row(i) = trans(mvn::getPDFValue(getCOMPONENTS(i).mu, getCOMPONENTS(i).sigma, demo));
 		}
-		uword imax[nPoints];
+		uword* imax = new uword[nPoints];
 		get_state_seq(imax, h);
 
 		hsmm_transition.resize(nSTATES, nSTATES);
@@ -283,7 +282,7 @@ public:
 		//update duration statistics
 		hsmm_duration_stats.resize(nSTATES);
 
-		uint s0 = imax[0], currStateDuration = 1;
+		unsigned int s0 = imax[0], currStateDuration = 1;
 		for (int t = 0; t < nPoints; t++) {
 			if ( (imax[t] != s0) || (t+1 == nPoints) ) {
 				if (t + 1 == nPoints)
@@ -298,6 +297,8 @@ public:
 			currStateDuration += 1;
 		}
 
+		delete[] imax;
+
 		// normalize the transition matrix
 		for (int i = 0; i < hsmm_transition.n_rows ; i++){
 			double row_sum = accu(hsmm_transition.row(i));
@@ -314,7 +315,7 @@ public:
 		colvec _MU = zeros(1,1);
 		gaussian_list_t hsmm_components;
 
-		for (uint i=0; i<nSTATES; i++) {
+		for (unsigned int i=0; i<nSTATES; i++) {
 			_MU(0,0) = hsmm_duration_stats[i].mean();//state_duration_means(i);
 			_SIGMA(0,0) = std::max( hsmm_duration_stats[i].var(), 1.0);
 			hsmm_components.push_back(gaussian_t(_MU, _SIGMA));
@@ -328,7 +329,7 @@ private:
 	void initialize_forward_calculation() {
 		// Calculate PD size:
 		PdSize = 0;
-		for (uint i = 0;i<nSTATES;i++)
+		for (unsigned int i = 0;i<nSTATES;i++)
 		{
 			if (PdSize < accu(DurationCOMPONENTS[i].mu))
 				PdSize = accu(DurationCOMPONENTS[i].mu);
@@ -342,7 +343,7 @@ private:
 		// Duration input vector
 		mat dur = linspace(1,PdSize, PdSize);
 		// Pre-Calculate Pd
-		for (uint i = 0;i<nSTATES;i++)
+		for (unsigned int i = 0;i<nSTATES;i++)
 		{
 			// Note that we need to transpose twice....
 			// getPDFValue accepts a rowvec of values, but returns a colvec....
@@ -373,8 +374,8 @@ private:
 
 
 private:
-	uint nVARS;
-	uint nSTATES;
+	unsigned int nVARS;
+	unsigned int nSTATES;
 
 	gaussian_list_t COMPONENTS;
 	gaussian_list_t DurationCOMPONENTS;
@@ -384,7 +385,7 @@ private:
 
 	// Variables for state duration
 	mat  Pd;     // Matrix with precomputed probability values for the duration;
-	uint PdSize; // Maximum maximum duration step
+	unsigned int PdSize; // Maximum maximum duration step
 
 	// For demonstration integration
 	mat    hsmm_transition;
@@ -438,18 +439,6 @@ static void error_callback(int error, const char* description){
 // }
 
 
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
-	int win_width;
-	int win_height;
-	int fb_width;
-	int fb_height;
-};
-
-
 //---------------------------------------------------------
 
 
@@ -485,7 +474,7 @@ int main(int argc, char **argv){
 	//--------------- Setup of the rendering ---------------
 
 	// Take 4k screens into account (framebuffer size != window size)
-	window_size_t window_size;
+	gfx2::window_size_t window_size;
 	window_size.win_width = 1200;
 	window_size.win_height = 600;
 	window_size.fb_width = -1;	// Will be known later
@@ -496,7 +485,7 @@ int main(int argc, char **argv){
 	if (!glfwInit())
 		exit(1);
 
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Online HSMM", window_size.win_width, window_size.win_height
 	);
 
@@ -523,16 +512,12 @@ int main(int argc, char **argv){
 	while (!glfwWindowShouldClose(window)){
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			// Retrieve the new window size
-			glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
-
-			// Retrieve the new framebuffer size
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-		}
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
 		// Start of rendering
 		ImGui_ImplGlfwGL2_NewFrame();
@@ -654,9 +639,9 @@ int main(int argc, char **argv){
 					        				 demo)
 						);
 					}
-					uword imax[points.size()];
-					get_state_seq(imax, h);
-
+					//uword imax[points.size()];
+					//get_state_seq(imax, h);
+					//
 					// //LQR
 					// mat rData = run_lqr(A, B, dt,
 					// 		R, points.size(), demo.col(0), imax,
diff --git a/src/demo_proMP01.cpp b/src/demo_proMP01.cpp
index 0b0e94c..82a5b45 100644
--- a/src/demo_proMP01.cpp
+++ b/src/demo_proMP01.cpp
@@ -33,7 +33,6 @@
 #include <GLFW/glfw3.h>
 #include <imgui.h>
 #include <imgui_impl_glfw_gl2.h>
-#include <window_utils.h>
 
 using namespace arma;
 
@@ -224,7 +223,7 @@ struct viewport_t {
 // Helper function to setup a viewport
 //-----------------------------------------------------------------------------
 void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
-					double near = -1.0, double far = 1.0) {
+					double near_distance = -1.0, double far_distance = 1.0) {
 
 	viewport->x = x;
 	viewport->y = y;
@@ -234,8 +233,8 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
 										  (double) height / 2 });
 	viewport->projection_bottom_right = vec({ (double) width / 2,
 											  (double) -height / 2 });
-	viewport->projection_near = near;
-	viewport->projection_far = far;
+	viewport->projection_near = near_distance;
+	viewport->projection_far = far_distance;
 }
 
 
@@ -409,7 +408,7 @@ void draw_model_viewport(const viewport_t& viewport,
 
 			span id = span(i * model.nb_var, i * model.nb_var + 1);
 
-			gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % 10).t()),
+			gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % COLORS.n_rows).t()),
 								model.mu_w(id), model.sigma_w(id, id));
 		}
 
@@ -448,13 +447,13 @@ void draw_reproductions_viewport(const viewport_t& viewport,
 		// Create a color map
 		mat colors(model.parameters.nb_states, 3);
 		for (int i = 0; i < model.parameters.nb_states; ++i)
-			colors(i, span::all) = COLORS(i % 10, span::all);
+			colors(i, span::all) = COLORS(i % COLORS.n_rows, span::all);
 
 		// Draw the reproductions as points colored by the gmm states (if any)
 		for (auto iter = reproductions.begin(); iter != reproductions.end(); ++iter) {
 			const mat& datapoints = (*iter);
 
-			for (uint i = 0; i < datapoints.n_cols; ++i) {
+			for (unsigned int i = 0; i < datapoints.n_cols; ++i) {
 				fvec color = conv_to<fvec>::from(model.H.col(i).t() * colors);
 
 				fvec position = zeros<fvec>(3);
@@ -484,13 +483,13 @@ int main(int argc, char **argv) {
 	model_t model;
 
 	// Parameters
-	model.parameters.nb_states							 = 10;
-	model.parameters.nb_data							 = 200;
-	model.parameters.nb_reproductions					 = 10;
-	model.parameters.dt									 = 0.01f;
-	model.parameters.rbf_width							 = 1e-2f;
+	model.parameters.nb_states                           = 10;
+	model.parameters.nb_data                             = 200;
+	model.parameters.nb_reproductions                    = 10;
+	model.parameters.dt                                  = 0.01f;
+	model.parameters.rbf_width                           = 1e-2f;
 	model.parameters.regularized_pseudoinverse_parameter = 3e-2f;
-	model.parameters.minimum_variance_parameter			 = 1.0f;
+	model.parameters.minimum_variance_parameter          = 1.0f;
 
 
 	// Take 4k screens into account (framebuffer size != window size)
@@ -514,7 +513,7 @@ int main(int argc, char **argv) {
 	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
 
 	// Open a window and create its OpenGL context
-	GLFWwindow* window = create_window_at_optimal_size(
+	GLFWwindow* window = gfx2::create_window_at_optimal_size(
 		"Demo Conditioning on trajectory distributions",
 		window_size.win_width, window_size.win_height
 	);
@@ -522,7 +521,7 @@ int main(int argc, char **argv) {
 	glfwMakeContextCurrent(window);
 
 
-	// Setup GLSL
+	// Setup OpenGL
 	gfx2::init();
 	glEnable(GL_SCISSOR_TEST);
 	glEnable(GL_DEPTH_TEST);
@@ -569,14 +568,14 @@ int main(int argc, char **argv) {
 	while (!glfwWindowShouldClose(window)) {
 		glfwPollEvents();
 
-		// Detect when the window was resized
-		if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
-			(ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+		// Handling of the resizing of the window
+		gfx2::window_result_t window_result =
+			gfx2::handle_window_resizing(window, &window_size);
 
-			window_size.win_width = ImGui::GetIO().DisplaySize.x;
-			window_size.win_height = ImGui::GetIO().DisplaySize.y;
+		if (window_result == gfx2::INVALID_SIZE)
+			continue;
 
-			glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+		if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
 
 			viewport_width = window_size.fb_width / 3 - 1;
 			viewport_height = window_size.fb_height;
diff --git a/src/utils/ImGuizmo.cpp b/src/utils/ImGuizmo.cpp
new file mode 100644
index 0000000..c28058b
--- /dev/null
+++ b/src/utils/ImGuizmo.cpp
@@ -0,0 +1,1985 @@
+// The MIT License(MIT)
+//
+// Copyright(c) 2016 Cedric Guillemet
+//
+// 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.
+
+#include "imgui.h"
+#ifndef IMGUI_DEFINE_MATH_OPERATORS
+#define IMGUI_DEFINE_MATH_OPERATORS
+#endif
+#include "imgui_internal.h"
+#include "ImGuizmo.h"
+
+// includes patches for multiview from
+// https://github.com/CedricGuillemet/ImGuizmo/issues/15
+
+namespace ImGuizmo
+{
+   static const float ZPI = 3.14159265358979323846f;
+   static const float RAD2DEG = (180.f / ZPI);
+   static const float DEG2RAD = (ZPI / 180.f);
+   static const float gGizmoSizeClipSpace = 0.1f;
+   const float screenRotateSize = 0.06f;
+
+   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+   // utility and math
+
+   void FPU_MatrixF_x_MatrixF(const float *a, const float *b, float *r)
+   {
+      r[0] = a[0] * b[0] + a[1] * b[4] + a[2] * b[8] + a[3] * b[12];
+      r[1] = a[0] * b[1] + a[1] * b[5] + a[2] * b[9] + a[3] * b[13];
+      r[2] = a[0] * b[2] + a[1] * b[6] + a[2] * b[10] + a[3] * b[14];
+      r[3] = a[0] * b[3] + a[1] * b[7] + a[2] * b[11] + a[3] * b[15];
+
+      r[4] = a[4] * b[0] + a[5] * b[4] + a[6] * b[8] + a[7] * b[12];
+      r[5] = a[4] * b[1] + a[5] * b[5] + a[6] * b[9] + a[7] * b[13];
+      r[6] = a[4] * b[2] + a[5] * b[6] + a[6] * b[10] + a[7] * b[14];
+      r[7] = a[4] * b[3] + a[5] * b[7] + a[6] * b[11] + a[7] * b[15];
+
+      r[8] = a[8] * b[0] + a[9] * b[4] + a[10] * b[8] + a[11] * b[12];
+      r[9] = a[8] * b[1] + a[9] * b[5] + a[10] * b[9] + a[11] * b[13];
+      r[10] = a[8] * b[2] + a[9] * b[6] + a[10] * b[10] + a[11] * b[14];
+      r[11] = a[8] * b[3] + a[9] * b[7] + a[10] * b[11] + a[11] * b[15];
+
+      r[12] = a[12] * b[0] + a[13] * b[4] + a[14] * b[8] + a[15] * b[12];
+      r[13] = a[12] * b[1] + a[13] * b[5] + a[14] * b[9] + a[15] * b[13];
+      r[14] = a[12] * b[2] + a[13] * b[6] + a[14] * b[10] + a[15] * b[14];
+      r[15] = a[12] * b[3] + a[13] * b[7] + a[14] * b[11] + a[15] * b[15];
+   }
+
+   //template <typename T> T LERP(T x, T y, float z) { return (x + (y - x)*z); }
+   template <typename T> T Clamp(T x, T y, T z) { return ((x<y) ? y : ((x>z) ? z : x)); }
+   template <typename T> T max(T x, T y) { return (x > y) ? x : y; }
+   template <typename T> T min(T x, T y) { return (x < y) ? x : y; }
+   template <typename T> bool IsWithin(T x, T y, T z) { return (x>=y) && (x<=z); }
+
+   struct matrix_t;
+   struct vec_t
+   {
+   public:
+      float x, y, z, w;
+
+      void Lerp(const vec_t& v, float t)
+      {
+         x += (v.x - x) * t;
+         y += (v.y - y) * t;
+         z += (v.z - z) * t;
+         w += (v.w - w) * t;
+      }
+
+      void Set(float v) { x = y = z = w = v; }
+      void Set(float _x, float _y, float _z = 0.f, float _w = 0.f) { x = _x; y = _y; z = _z; w = _w; }
+
+      vec_t& operator -= (const vec_t& v) { x -= v.x; y -= v.y; z -= v.z; w -= v.w; return *this; }
+      vec_t& operator += (const vec_t& v) { x += v.x; y += v.y; z += v.z; w += v.w; return *this; }
+      vec_t& operator *= (const vec_t& v) { x *= v.x; y *= v.y; z *= v.z; w *= v.w; return *this; }
+      vec_t& operator *= (float v) { x *= v;    y *= v;    z *= v;    w *= v;    return *this; }
+
+      vec_t operator * (float f) const;
+      vec_t operator - () const;
+      vec_t operator - (const vec_t& v) const;
+      vec_t operator + (const vec_t& v) const;
+      vec_t operator * (const vec_t& v) const;
+
+      const vec_t& operator + () const { return (*this); }
+      float Length() const { return sqrtf(x*x + y*y + z*z); };
+      float LengthSq() const { return (x*x + y*y + z*z); };
+      vec_t Normalize() { (*this) *= (1.f / Length()); return (*this); }
+      vec_t Normalize(const vec_t& v) { this->Set(v.x, v.y, v.z, v.w); this->Normalize(); return (*this); }
+      vec_t Abs() const;
+      void Cross(const vec_t& v)
+      {
+         vec_t res;
+         res.x = y * v.z - z * v.y;
+         res.y = z * v.x - x * v.z;
+         res.z = x * v.y - y * v.x;
+
+         x = res.x;
+         y = res.y;
+         z = res.z;
+         w = 0.f;
+      }
+      void Cross(const vec_t& v1, const vec_t& v2)
+      {
+         x = v1.y * v2.z - v1.z * v2.y;
+         y = v1.z * v2.x - v1.x * v2.z;
+         z = v1.x * v2.y - v1.y * v2.x;
+         w = 0.f;
+      }
+      float Dot(const vec_t &v) const
+      {
+         return (x * v.x) + (y * v.y) + (z * v.z) + (w * v.w);
+      }
+      float Dot3(const vec_t &v) const
+      {
+         return (x * v.x) + (y * v.y) + (z * v.z);
+      }
+
+      void Transform(const matrix_t& matrix);
+      void Transform(const vec_t & s, const matrix_t& matrix);
+
+      void TransformVector(const matrix_t& matrix);
+      void TransformPoint(const matrix_t& matrix);
+      void TransformVector(const vec_t& v, const matrix_t& matrix) { (*this) = v; this->TransformVector(matrix); }
+      void TransformPoint(const vec_t& v, const matrix_t& matrix) { (*this) = v; this->TransformPoint(matrix); }
+
+      float& operator [] (size_t index) { return ((float*)&x)[index]; }
+      const float& operator [] (size_t index) const { return ((float*)&x)[index]; }
+   };
+
+   vec_t makeVect(float _x, float _y, float _z = 0.f, float _w = 0.f) { vec_t res; res.x = _x; res.y = _y; res.z = _z; res.w = _w; return res; }
+   vec_t makeVect(ImVec2 v) { vec_t res; res.x = v.x; res.y = v.y; res.z = 0.f; res.w = 0.f; return res; }
+   vec_t vec_t::operator * (float f) const { return makeVect(x * f, y * f, z * f, w *f); }
+   vec_t vec_t::operator - () const { return makeVect(-x, -y, -z, -w); }
+   vec_t vec_t::operator - (const vec_t& v) const { return makeVect(x - v.x, y - v.y, z - v.z, w - v.w); }
+   vec_t vec_t::operator + (const vec_t& v) const { return makeVect(x + v.x, y + v.y, z + v.z, w + v.w); }
+   vec_t vec_t::operator * (const vec_t& v) const { return makeVect(x * v.x, y * v.y, z * v.z, w * v.w); }
+   vec_t vec_t::Abs() const { return makeVect(fabsf(x), fabsf(y), fabsf(z)); }
+
+   vec_t Normalized(const vec_t& v) { vec_t res; res = v; res.Normalize(); return res; }
+   vec_t Cross(const vec_t& v1, const vec_t& v2)
+   {
+      vec_t res;
+      res.x = v1.y * v2.z - v1.z * v2.y;
+      res.y = v1.z * v2.x - v1.x * v2.z;
+      res.z = v1.x * v2.y - v1.y * v2.x;
+      res.w = 0.f;
+      return res;
+   }
+
+   float Dot(const vec_t &v1, const vec_t &v2)
+   {
+      return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
+   }
+
+   vec_t BuildPlan(const vec_t & p_point1, const vec_t & p_normal)
+   {
+      vec_t normal, res;
+      normal.Normalize(p_normal);
+      res.w = normal.Dot(p_point1);
+      res.x = normal.x;
+      res.y = normal.y;
+      res.z = normal.z;
+      return res;
+   }
+
+   struct matrix_t
+   {
+   public:
+
+      union
+      {
+         float m[4][4];
+         float m16[16];
+         struct
+         {
+            vec_t right, up, dir, position;
+         } v;
+         vec_t component[4];
+      };
+
+      matrix_t(const matrix_t& other) { memcpy(&m16[0], &other.m16[0], sizeof(float) * 16); }
+      matrix_t() {}
+
+      operator float * () { return m16; }
+      operator const float* () const { return m16; }
+      void Translation(float _x, float _y, float _z) { this->Translation(makeVect(_x, _y, _z)); }
+
+      void Translation(const vec_t& vt)
+      {
+         v.right.Set(1.f, 0.f, 0.f, 0.f);
+         v.up.Set(0.f, 1.f, 0.f, 0.f);
+         v.dir.Set(0.f, 0.f, 1.f, 0.f);
+         v.position.Set(vt.x, vt.y, vt.z, 1.f);
+      }
+
+      void Scale(float _x, float _y, float _z)
+      {
+         v.right.Set(_x, 0.f, 0.f, 0.f);
+         v.up.Set(0.f, _y, 0.f, 0.f);
+         v.dir.Set(0.f, 0.f, _z, 0.f);
+         v.position.Set(0.f, 0.f, 0.f, 1.f);
+      }
+      void Scale(const vec_t& s) { Scale(s.x, s.y, s.z); }
+
+      matrix_t& operator *= (const matrix_t& mat)
+      {
+         matrix_t tmpMat;
+         tmpMat = *this;
+         tmpMat.Multiply(mat);
+         *this = tmpMat;
+         return *this;
+      }
+      matrix_t operator * (const matrix_t& mat) const
+      {
+         matrix_t matT;
+         matT.Multiply(*this, mat);
+         return matT;
+      }
+
+      void Multiply(const matrix_t &matrix)
+      {
+         matrix_t tmp;
+         tmp = *this;
+
+         FPU_MatrixF_x_MatrixF((float*)&tmp, (float*)&matrix, (float*)this);
+      }
+
+      void Multiply(const matrix_t &m1, const matrix_t &m2)
+      {
+         FPU_MatrixF_x_MatrixF((float*)&m1, (float*)&m2, (float*)this);
+      }
+
+      float GetDeterminant() const
+      {
+         return m[0][0] * m[1][1] * m[2][2] + m[0][1] * m[1][2] * m[2][0] + m[0][2] * m[1][0] * m[2][1] -
+            m[0][2] * m[1][1] * m[2][0] - m[0][1] * m[1][0] * m[2][2] - m[0][0] * m[1][2] * m[2][1];
+      }
+
+      float Inverse(const matrix_t &srcMatrix, bool affine = false);
+      void SetToIdentity()
+      {
+         v.right.Set(1.f, 0.f, 0.f, 0.f);
+         v.up.Set(0.f, 1.f, 0.f, 0.f);
+         v.dir.Set(0.f, 0.f, 1.f, 0.f);
+         v.position.Set(0.f, 0.f, 0.f, 1.f);
+      }
+      void Transpose()
+      {
+         matrix_t tmpm;
+         for (int l = 0; l < 4; l++)
+         {
+            for (int c = 0; c < 4; c++)
+            {
+               tmpm.m[l][c] = m[c][l];
+            }
+         }
+         (*this) = tmpm;
+      }
+
+      void RotationAxis(const vec_t & axis, float angle);
+
+      void OrthoNormalize()
+      {
+         v.right.Normalize();
+         v.up.Normalize();
+         v.dir.Normalize();
+      }
+   };
+
+   void vec_t::Transform(const matrix_t& matrix)
+   {
+      vec_t out;
+
+      out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0] + w * matrix.m[3][0];
+      out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1] + w * matrix.m[3][1];
+      out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2] + w * matrix.m[3][2];
+      out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3] + w * matrix.m[3][3];
+
+      x = out.x;
+      y = out.y;
+      z = out.z;
+      w = out.w;
+   }
+
+   void vec_t::Transform(const vec_t & s, const matrix_t& matrix)
+   {
+      *this = s;
+      Transform(matrix);
+   }
+
+   void vec_t::TransformPoint(const matrix_t& matrix)
+   {
+      vec_t out;
+
+      out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0] + matrix.m[3][0];
+      out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1] + matrix.m[3][1];
+      out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2] + matrix.m[3][2];
+      out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3] + matrix.m[3][3];
+
+      x = out.x;
+      y = out.y;
+      z = out.z;
+      w = out.w;
+   }
+
+
+   void vec_t::TransformVector(const matrix_t& matrix)
+   {
+      vec_t out;
+
+      out.x = x * matrix.m[0][0] + y * matrix.m[1][0] + z * matrix.m[2][0];
+      out.y = x * matrix.m[0][1] + y * matrix.m[1][1] + z * matrix.m[2][1];
+      out.z = x * matrix.m[0][2] + y * matrix.m[1][2] + z * matrix.m[2][2];
+      out.w = x * matrix.m[0][3] + y * matrix.m[1][3] + z * matrix.m[2][3];
+
+      x = out.x;
+      y = out.y;
+      z = out.z;
+      w = out.w;
+   }
+
+   float matrix_t::Inverse(const matrix_t &srcMatrix, bool affine)
+   {
+      float det = 0;
+
+      if (affine)
+      {
+         det = GetDeterminant();
+         float s = 1 / det;
+         m[0][0] = (srcMatrix.m[1][1] * srcMatrix.m[2][2] - srcMatrix.m[1][2] * srcMatrix.m[2][1]) * s;
+         m[0][1] = (srcMatrix.m[2][1] * srcMatrix.m[0][2] - srcMatrix.m[2][2] * srcMatrix.m[0][1]) * s;
+         m[0][2] = (srcMatrix.m[0][1] * srcMatrix.m[1][2] - srcMatrix.m[0][2] * srcMatrix.m[1][1]) * s;
+         m[1][0] = (srcMatrix.m[1][2] * srcMatrix.m[2][0] - srcMatrix.m[1][0] * srcMatrix.m[2][2]) * s;
+         m[1][1] = (srcMatrix.m[2][2] * srcMatrix.m[0][0] - srcMatrix.m[2][0] * srcMatrix.m[0][2]) * s;
+         m[1][2] = (srcMatrix.m[0][2] * srcMatrix.m[1][0] - srcMatrix.m[0][0] * srcMatrix.m[1][2]) * s;
+         m[2][0] = (srcMatrix.m[1][0] * srcMatrix.m[2][1] - srcMatrix.m[1][1] * srcMatrix.m[2][0]) * s;
+         m[2][1] = (srcMatrix.m[2][0] * srcMatrix.m[0][1] - srcMatrix.m[2][1] * srcMatrix.m[0][0]) * s;
+         m[2][2] = (srcMatrix.m[0][0] * srcMatrix.m[1][1] - srcMatrix.m[0][1] * srcMatrix.m[1][0]) * s;
+         m[3][0] = -(m[0][0] * srcMatrix.m[3][0] + m[1][0] * srcMatrix.m[3][1] + m[2][0] * srcMatrix.m[3][2]);
+         m[3][1] = -(m[0][1] * srcMatrix.m[3][0] + m[1][1] * srcMatrix.m[3][1] + m[2][1] * srcMatrix.m[3][2]);
+         m[3][2] = -(m[0][2] * srcMatrix.m[3][0] + m[1][2] * srcMatrix.m[3][1] + m[2][2] * srcMatrix.m[3][2]);
+      }
+      else
+      {
+         // transpose matrix
+         float src[16];
+         for (int i = 0; i < 4; ++i)
+         {
+            src[i] = srcMatrix.m16[i * 4];
+            src[i + 4] = srcMatrix.m16[i * 4 + 1];
+            src[i + 8] = srcMatrix.m16[i * 4 + 2];
+            src[i + 12] = srcMatrix.m16[i * 4 + 3];
+         }
+
+         // calculate pairs for first 8 elements (cofactors)
+         float tmp[12]; // temp array for pairs
+         tmp[0] = src[10] * src[15];
+         tmp[1] = src[11] * src[14];
+         tmp[2] = src[9] * src[15];
+         tmp[3] = src[11] * src[13];
+         tmp[4] = src[9] * src[14];
+         tmp[5] = src[10] * src[13];
+         tmp[6] = src[8] * src[15];
+         tmp[7] = src[11] * src[12];
+         tmp[8] = src[8] * src[14];
+         tmp[9] = src[10] * src[12];
+         tmp[10] = src[8] * src[13];
+         tmp[11] = src[9] * src[12];
+
+         // calculate first 8 elements (cofactors)
+         m16[0] = (tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7]) - (tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7]);
+         m16[1] = (tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7]) - (tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7]);
+         m16[2] = (tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7]) - (tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7]);
+         m16[3] = (tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6]) - (tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6]);
+         m16[4] = (tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3]) - (tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3]);
+         m16[5] = (tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3]) - (tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3]);
+         m16[6] = (tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3]) - (tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3]);
+         m16[7] = (tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2]) - (tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2]);
+
+         // calculate pairs for second 8 elements (cofactors)
+         tmp[0] = src[2] * src[7];
+         tmp[1] = src[3] * src[6];
+         tmp[2] = src[1] * src[7];
+         tmp[3] = src[3] * src[5];
+         tmp[4] = src[1] * src[6];
+         tmp[5] = src[2] * src[5];
+         tmp[6] = src[0] * src[7];
+         tmp[7] = src[3] * src[4];
+         tmp[8] = src[0] * src[6];
+         tmp[9] = src[2] * src[4];
+         tmp[10] = src[0] * src[5];
+         tmp[11] = src[1] * src[4];
+
+         // calculate second 8 elements (cofactors)
+         m16[8] = (tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15]) - (tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15]);
+         m16[9] = (tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15]) - (tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15]);
+         m16[10] = (tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15]) - (tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15]);
+         m16[11] = (tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14]) - (tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14]);
+         m16[12] = (tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9]) - (tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10]);
+         m16[13] = (tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10]) - (tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8]);
+         m16[14] = (tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8]) - (tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9]);
+         m16[15] = (tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9]) - (tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8]);
+
+         // calculate determinant
+         det = src[0] * m16[0] + src[1] * m16[1] + src[2] * m16[2] + src[3] * m16[3];
+
+         // calculate matrix inverse
+         float invdet = 1 / det;
+         for (int j = 0; j < 16; ++j)
+         {
+            m16[j] *= invdet;
+         }
+      }
+
+      return det;
+   }
+
+   void matrix_t::RotationAxis(const vec_t & axis, float angle)
+   {
+      float length2 = axis.LengthSq();
+      if (length2 < FLT_EPSILON)
+      {
+         SetToIdentity();
+         return;
+      }
+
+      vec_t n = axis * (1.f / sqrtf(length2));
+      float s = sinf(angle);
+      float c = cosf(angle);
+      float k = 1.f - c;
+
+      float xx = n.x * n.x * k + c;
+      float yy = n.y * n.y * k + c;
+      float zz = n.z * n.z * k + c;
+      float xy = n.x * n.y * k;
+      float yz = n.y * n.z * k;
+      float zx = n.z * n.x * k;
+      float xs = n.x * s;
+      float ys = n.y * s;
+      float zs = n.z * s;
+
+      m[0][0] = xx;
+      m[0][1] = xy + zs;
+      m[0][2] = zx - ys;
+      m[0][3] = 0.f;
+      m[1][0] = xy - zs;
+      m[1][1] = yy;
+      m[1][2] = yz + xs;
+      m[1][3] = 0.f;
+      m[2][0] = zx + ys;
+      m[2][1] = yz - xs;
+      m[2][2] = zz;
+      m[2][3] = 0.f;
+      m[3][0] = 0.f;
+      m[3][1] = 0.f;
+      m[3][2] = 0.f;
+      m[3][3] = 1.f;
+   }
+
+   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+   //
+
+   enum MOVETYPE
+   {
+      NONE,
+      MOVE_X,
+      MOVE_Y,
+      MOVE_Z,
+      MOVE_YZ,
+      MOVE_ZX,
+      MOVE_XY,
+      MOVE_SCREEN,
+      ROTATE_X,
+      ROTATE_Y,
+      ROTATE_Z,
+      ROTATE_SCREEN,
+      SCALE_X,
+      SCALE_Y,
+      SCALE_Z,
+      SCALE_XYZ
+   };
+
+   struct Context
+   {
+      Context() : mbUsing(false), mbEnable(true), mbUsingBounds(false)
+      {
+      }
+
+      ImDrawList* mDrawList;
+
+      MODE mMode;
+      matrix_t mViewMat;
+      matrix_t mProjectionMat;
+      matrix_t mModel;
+      matrix_t mModelInverse;
+      matrix_t mModelSource;
+      matrix_t mModelSourceInverse;
+      matrix_t mMVP;
+      matrix_t mViewProjection;
+
+      vec_t mModelScaleOrigin;
+      vec_t mCameraEye;
+      vec_t mCameraRight;
+      vec_t mCameraDir;
+      vec_t mCameraUp;
+      vec_t mRayOrigin;
+      vec_t mRayVector;
+
+      float  mRadiusSquareCenter;
+      ImVec2 mScreenSquareCenter;
+      ImVec2 mScreenSquareMin;
+      ImVec2 mScreenSquareMax;
+
+      float mScreenFactor;
+      vec_t mRelativeOrigin;
+
+      bool mbUsing;
+      bool mbEnable;
+
+      // translation
+      vec_t mTranslationPlan;
+      vec_t mTranslationPlanOrigin;
+      vec_t mMatrixOrigin;
+
+      // rotation
+      vec_t mRotationVectorSource;
+      float mRotationAngle;
+      float mRotationAngleOrigin;
+      //vec_t mWorldToLocalAxis;
+
+      // scale
+      vec_t mScale;
+      vec_t mScaleValueOrigin;
+      float mSaveMousePosx;
+
+      // save axis factor when using gizmo
+      bool mBelowAxisLimit[3];
+      bool mBelowPlaneLimit[3];
+      float mAxisFactor[3];
+
+      // bounds stretching
+      vec_t mBoundsPivot;
+      vec_t mBoundsAnchor;
+      vec_t mBoundsPlan;
+      vec_t mBoundsLocalPivot;
+      int mBoundsBestAxis;
+      int mBoundsAxis[2];
+      bool mbUsingBounds;
+      matrix_t mBoundsMatrix;
+
+      //
+      int mCurrentOperation;
+
+      float mX = 0.f;
+      float mY = 0.f;
+      float mWidth = 0.f;
+      float mHeight = 0.f;
+      float mXMax = 0.f;
+      float mYMax = 0.f;
+     float mDisplayRatio = 1.f;
+
+     bool mIsOrthographic = false;
+   };
+
+   static Context gContext;
+
+   static const float angleLimit = 0.96f;
+   static const float planeLimit = 0.2f;
+
+   static const vec_t directionUnary[3] = { makeVect(1.f, 0.f, 0.f), makeVect(0.f, 1.f, 0.f), makeVect(0.f, 0.f, 1.f) };
+   static const ImU32 directionColor[3] = { 0xFF0000AA, 0xFF00AA00, 0xFFAA0000 };
+
+   // Alpha: 100%: FF, 87%: DE, 70%: B3, 54%: 8A, 50%: 80, 38%: 61, 12%: 1F
+   static const ImU32 planeColor[3] = { 0x610000AA, 0x6100AA00, 0x61AA0000 };
+   static const ImU32 selectionColor = 0x8A1080FF;
+   static const ImU32 inactiveColor = 0x99999999;
+   static const ImU32 translationLineColor = 0xAAAAAAAA;
+   static const char *translationInfoMask[] = { "X : %5.3f", "Y : %5.3f", "Z : %5.3f",
+      "Y : %5.3f Z : %5.3f", "X : %5.3f Z : %5.3f", "X : %5.3f Y : %5.3f",
+      "X : %5.3f Y : %5.3f Z : %5.3f" };
+   static const char *scaleInfoMask[] = { "X : %5.2f", "Y : %5.2f", "Z : %5.2f", "XYZ : %5.2f" };
+   static const char *rotationInfoMask[] = { "X : %5.2f deg %5.2f rad", "Y : %5.2f deg %5.2f rad", "Z : %5.2f deg %5.2f rad", "Screen : %5.2f deg %5.2f rad" };
+   static const int translationInfoIndex[] = { 0,0,0, 1,0,0, 2,0,0, 1,2,0, 0,2,0, 0,1,0, 0,1,2 };
+   static const float quadMin = 0.5f;
+   static const float quadMax = 0.8f;
+   static const float quadUV[8] = { quadMin, quadMin, quadMin, quadMax, quadMax, quadMax, quadMax, quadMin };
+   static const int halfCircleSegmentCount = 64;
+   static const float snapTension = 0.5f;
+
+   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+   //
+   static int GetMoveType(vec_t *gizmoHitProportion);
+   static int GetRotateType();
+   static int GetScaleType();
+
+   static ImVec2 worldToPos(const vec_t& worldPos, const matrix_t& mat)
+   {
+      vec_t trans;
+      trans.TransformPoint(worldPos, mat);
+      trans *= 0.5f / trans.w;
+      trans += makeVect(0.5f, 0.5f);
+      trans.y = 1.f - trans.y;
+      trans.x *= gContext.mWidth;
+      trans.y *= gContext.mHeight;
+      trans.x += gContext.mX;
+      trans.y += gContext.mY;
+      return ImVec2(trans.x, trans.y);
+   }
+
+   static void ComputeCameraRay(vec_t &rayOrigin, vec_t &rayDir)
+   {
+      ImGuiIO& io = ImGui::GetIO();
+
+      matrix_t mViewProjInverse;
+      mViewProjInverse.Inverse(gContext.mViewMat * gContext.mProjectionMat);
+
+      float mox = ((io.MousePos.x - gContext.mX) / gContext.mWidth) * 2.f - 1.f;
+      float moy = (1.f - ((io.MousePos.y - gContext.mY) / gContext.mHeight)) * 2.f - 1.f;
+
+      rayOrigin.Transform(makeVect(mox, moy, 0.f, 1.f), mViewProjInverse);
+      rayOrigin *= 1.f / rayOrigin.w;
+      vec_t rayEnd;
+      rayEnd.Transform(makeVect(mox, moy, 1.f, 1.f), mViewProjInverse);
+      rayEnd *= 1.f / rayEnd.w;
+      rayDir = Normalized(rayEnd - rayOrigin);
+   }
+
+   static float GetSegmentLengthClipSpace(const vec_t& start, const vec_t& end)
+   {
+      vec_t startOfSegment = start;
+      startOfSegment.TransformPoint(gContext.mMVP);
+      if (fabsf(startOfSegment.w)> FLT_EPSILON) // check for axis aligned with camera direction
+         startOfSegment *= 1.f / startOfSegment.w;
+
+      vec_t endOfSegment = end;
+      endOfSegment.TransformPoint(gContext.mMVP);
+      if (fabsf(endOfSegment.w)> FLT_EPSILON) // check for axis aligned with camera direction
+         endOfSegment *= 1.f / endOfSegment.w;
+
+      vec_t clipSpaceAxis = endOfSegment - startOfSegment;
+      clipSpaceAxis.y /= gContext.mDisplayRatio;
+      float segmentLengthInClipSpace = sqrtf(clipSpaceAxis.x*clipSpaceAxis.x + clipSpaceAxis.y*clipSpaceAxis.y);
+      return segmentLengthInClipSpace;
+   }
+
+   static float GetParallelogram(const vec_t& ptO, const vec_t& ptA, const vec_t& ptB)
+   {
+      vec_t pts[] = { ptO, ptA, ptB };
+      for (unsigned int i = 0; i < 3; i++)
+      {
+         pts[i].TransformPoint(gContext.mMVP);
+         if (fabsf(pts[i].w)> FLT_EPSILON) // check for axis aligned with camera direction
+            pts[i] *= 1.f / pts[i].w;
+      }
+      vec_t segA = pts[1] - pts[0];
+      vec_t segB = pts[2] - pts[0];
+      segA.y /= gContext.mDisplayRatio;
+      segB.y /= gContext.mDisplayRatio;
+      vec_t segAOrtho = makeVect(-segA.y, segA.x);
+      segAOrtho.Normalize();
+      float dt = segAOrtho.Dot3(segB);
+      float surface = sqrtf(segA.x*segA.x + segA.y*segA.y) * fabsf(dt);
+      return surface;
+   }
+
+   inline vec_t PointOnSegment(const vec_t & point, const vec_t & vertPos1, const vec_t & vertPos2)
+   {
+      vec_t c = point - vertPos1;
+      vec_t V;
+
+      V.Normalize(vertPos2 - vertPos1);
+      float d = (vertPos2 - vertPos1).Length();
+      float t = V.Dot3(c);
+
+      if (t < 0.f)
+         return vertPos1;
+
+      if (t > d)
+         return vertPos2;
+
+      return vertPos1 + V * t;
+   }
+
+   static float IntersectRayPlane(const vec_t & rOrigin, const vec_t& rVector, const vec_t& plan)
+   {
+      float numer = plan.Dot3(rOrigin) - plan.w;
+      float denom = plan.Dot3(rVector);
+
+      if (fabsf(denom) < FLT_EPSILON)  // normal is orthogonal to vector, cant intersect
+         return -1.0f;
+
+      return -(numer / denom);
+   }
+
+   static bool IsInContextRect( ImVec2 p )
+   {
+       return IsWithin( p.x, gContext.mX, gContext.mXMax ) && IsWithin(p.y, gContext.mY, gContext.mYMax );
+   }
+
+   void SetRect(float x, float y, float width, float height)
+   {
+       gContext.mX = x;
+       gContext.mY = y;
+       gContext.mWidth = width;
+       gContext.mHeight = height;
+       gContext.mXMax = gContext.mX + gContext.mWidth;
+       gContext.mYMax = gContext.mY + gContext.mXMax;
+      gContext.mDisplayRatio = width / height;
+   }
+
+   IMGUI_API void SetOrthographic(bool isOrthographic)
+   {
+      gContext.mIsOrthographic = isOrthographic;
+   }
+
+   void SetDrawlist()
+   {
+      gContext.mDrawList = ImGui::GetWindowDrawList();
+   }
+
+   void BeginFrame()
+   {
+      ImGuiIO& io = ImGui::GetIO();
+
+      const ImU32 flags = ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoFocusOnAppearing | ImGuiWindowFlags_NoBringToFrontOnFocus;
+      ImGui::SetNextWindowSize(io.DisplaySize);
+      ImGui::SetNextWindowPos(ImVec2(0, 0));
+      
+      ImGui::PushStyleColor(ImGuiCol_WindowBg, 0);
+      ImGui::PushStyleColor(ImGuiCol_Border, 0);
+      ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
+     
+      ImGui::Begin("gizmo", NULL, flags);
+      gContext.mDrawList = ImGui::GetWindowDrawList();
+      ImGui::End();
+      ImGui::PopStyleVar();
+      ImGui::PopStyleColor(2);
+   }
+
+   bool IsUsing()
+   {
+      return gContext.mbUsing||gContext.mbUsingBounds;
+   }
+
+   bool IsOver()
+   {
+      return (GetMoveType(NULL) != NONE) || GetRotateType() != NONE || GetScaleType() != NONE || IsUsing();
+   }
+
+   void Enable(bool enable)
+   {
+      gContext.mbEnable = enable;
+      if (!enable)
+      {
+          gContext.mbUsing = false;
+          gContext.mbUsingBounds = false;
+      }
+   }
+
+   static float GetUniform(const vec_t& position, const matrix_t& mat)
+   {
+      vec_t trf = makeVect(position.x, position.y, position.z, 1.f);
+      trf.Transform(mat);
+      return trf.w;
+   }
+
+   static void ComputeContext(const float *view, const float *projection, float *matrix, MODE mode)
+   {
+      gContext.mMode = mode;
+      gContext.mViewMat = *(matrix_t*)view;
+      gContext.mProjectionMat = *(matrix_t*)projection;
+
+      if (mode == LOCAL)
+      {
+         gContext.mModel = *(matrix_t*)matrix;
+         gContext.mModel.OrthoNormalize();
+      }
+      else
+      {
+         gContext.mModel.Translation(((matrix_t*)matrix)->v.position);
+      }
+      gContext.mModelSource = *(matrix_t*)matrix;
+      gContext.mModelScaleOrigin.Set(gContext.mModelSource.v.right.Length(), gContext.mModelSource.v.up.Length(), gContext.mModelSource.v.dir.Length());
+
+      gContext.mModelInverse.Inverse(gContext.mModel);
+      gContext.mModelSourceInverse.Inverse(gContext.mModelSource);
+      gContext.mViewProjection = gContext.mViewMat * gContext.mProjectionMat;
+      gContext.mMVP = gContext.mModel * gContext.mViewProjection;
+
+      matrix_t viewInverse;
+      viewInverse.Inverse(gContext.mViewMat);
+      gContext.mCameraDir = viewInverse.v.dir;
+      gContext.mCameraEye = viewInverse.v.position;
+      gContext.mCameraRight = viewInverse.v.right;
+      gContext.mCameraUp = viewInverse.v.up;
+
+     // compute scale from the size of camera right vector projected on screen at the matrix position
+     vec_t pointRight = viewInverse.v.right;
+     pointRight.TransformPoint(gContext.mViewProjection);
+     gContext.mScreenFactor = gGizmoSizeClipSpace / (pointRight.x / pointRight.w - gContext.mMVP.v.position.x / gContext.mMVP.v.position.w);
+
+     vec_t rightViewInverse = viewInverse.v.right;
+     rightViewInverse.TransformVector(gContext.mModelInverse);
+     float rightLength = GetSegmentLengthClipSpace(makeVect(0.f, 0.f), rightViewInverse);
+     gContext.mScreenFactor = gGizmoSizeClipSpace / rightLength;
+
+      ImVec2 centerSSpace = worldToPos(makeVect(0.f, 0.f), gContext.mMVP);
+      gContext.mScreenSquareCenter = centerSSpace;
+      gContext.mScreenSquareMin = ImVec2(centerSSpace.x - 10.f, centerSSpace.y - 10.f);
+      gContext.mScreenSquareMax = ImVec2(centerSSpace.x + 10.f, centerSSpace.y + 10.f);
+
+      ComputeCameraRay(gContext.mRayOrigin, gContext.mRayVector);
+   }
+
+   static void ComputeColors(ImU32 *colors, int type, OPERATION operation)
+   {
+      if (gContext.mbEnable)
+      {
+         switch (operation)
+         {
+         case TRANSLATE:
+            colors[0] = (type == MOVE_SCREEN) ? selectionColor : 0xFFFFFFFF;
+            for (int i = 0; i < 3; i++)
+            {
+               colors[i + 1] = (type == (int)(MOVE_X + i)) ? selectionColor : directionColor[i];
+               colors[i + 4] = (type == (int)(MOVE_YZ + i)) ? selectionColor : planeColor[i];
+               colors[i + 4] = (type == MOVE_SCREEN) ? selectionColor : colors[i + 4];
+            }
+            break;
+         case ROTATE:
+            colors[0] = (type == ROTATE_SCREEN) ? selectionColor : 0xFFFFFFFF;
+            for (int i = 0; i < 3; i++)
+               colors[i + 1] = (type == (int)(ROTATE_X + i)) ? selectionColor : directionColor[i];
+            break;
+         case SCALE:
+            colors[0] = (type == SCALE_XYZ) ? selectionColor : 0xFFFFFFFF;
+            for (int i = 0; i < 3; i++)
+               colors[i + 1] = (type == (int)(SCALE_X + i)) ? selectionColor : directionColor[i];
+            break;
+         case BOUNDS:
+            break;
+         }
+      }
+      else
+      {
+         for (int i = 0; i < 7; i++)
+            colors[i] = inactiveColor;
+      }
+   }
+
+   static void ComputeTripodAxisAndVisibility(int axisIndex, vec_t& dirAxis, vec_t& dirPlaneX, vec_t& dirPlaneY, bool& belowAxisLimit, bool& belowPlaneLimit)
+   {
+      dirAxis = directionUnary[axisIndex];
+      dirPlaneX = directionUnary[(axisIndex + 1) % 3];
+      dirPlaneY = directionUnary[(axisIndex + 2) % 3];
+
+      if (gContext.mbUsing)
+      {
+         // when using, use stored factors so the gizmo doesn't flip when we translate
+         belowAxisLimit = gContext.mBelowAxisLimit[axisIndex];
+         belowPlaneLimit = gContext.mBelowPlaneLimit[axisIndex];
+
+         dirAxis *= gContext.mAxisFactor[axisIndex];
+         dirPlaneX *= gContext.mAxisFactor[(axisIndex + 1) % 3];
+         dirPlaneY *= gContext.mAxisFactor[(axisIndex + 2) % 3];
+      }
+      else
+      {
+         // new method
+         float lenDir = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), dirAxis);
+         float lenDirMinus = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), -dirAxis);
+
+         float lenDirPlaneX = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), dirPlaneX);
+         float lenDirMinusPlaneX = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), -dirPlaneX);
+
+         float lenDirPlaneY = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), dirPlaneY);
+         float lenDirMinusPlaneY = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), -dirPlaneY);
+
+         float mulAxis = (lenDir < lenDirMinus && fabsf(lenDir - lenDirMinus) > FLT_EPSILON) ? -1.f : 1.f;
+         float mulAxisX = (lenDirPlaneX < lenDirMinusPlaneX && fabsf(lenDirPlaneX - lenDirMinusPlaneX) > FLT_EPSILON) ? -1.f : 1.f;
+         float mulAxisY = (lenDirPlaneY < lenDirMinusPlaneY && fabsf(lenDirPlaneY - lenDirMinusPlaneY) > FLT_EPSILON) ? -1.f : 1.f;
+         dirAxis *= mulAxis;
+         dirPlaneX *= mulAxisX;
+         dirPlaneY *= mulAxisY;
+
+         // for axis
+         float axisLengthInClipSpace = GetSegmentLengthClipSpace(makeVect(0.f, 0.f, 0.f), dirAxis * gContext.mScreenFactor);
+
+         float paraSurf = GetParallelogram(makeVect(0.f, 0.f, 0.f), dirPlaneX * gContext.mScreenFactor, dirPlaneY * gContext.mScreenFactor);
+         belowPlaneLimit = (paraSurf > 0.0025f);
+         belowAxisLimit = (axisLengthInClipSpace > 0.02f);
+
+         // and store values
+         gContext.mAxisFactor[axisIndex] = mulAxis;
+         gContext.mAxisFactor[(axisIndex + 1) % 3] = mulAxisX;
+         gContext.mAxisFactor[(axisIndex + 2) % 3] = mulAxisY;
+         gContext.mBelowAxisLimit[axisIndex] = belowAxisLimit;
+         gContext.mBelowPlaneLimit[axisIndex] = belowPlaneLimit;
+      }
+   }
+
+   static void ComputeSnap(float*value, float snap)
+   {
+      if (snap <= FLT_EPSILON)
+         return;
+      float modulo = fmodf(*value, snap);
+      float moduloRatio = fabsf(modulo) / snap;
+      if (moduloRatio < snapTension)
+         *value -= modulo;
+      else if (moduloRatio >(1.f - snapTension))
+         *value = *value - modulo + snap * ((*value<0.f) ? -1.f : 1.f);
+   }
+   static void ComputeSnap(vec_t& value, float *snap)
+   {
+      for (int i = 0; i < 3; i++)
+      {
+         ComputeSnap(&value[i], snap[i]);
+      }
+   }
+
+   static float ComputeAngleOnPlan()
+   {
+      const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan);
+      vec_t localPos = Normalized(gContext.mRayOrigin + gContext.mRayVector * len - gContext.mModel.v.position);
+
+      vec_t perpendicularVector;
+      perpendicularVector.Cross(gContext.mRotationVectorSource, gContext.mTranslationPlan);
+      perpendicularVector.Normalize();
+      float acosAngle = Clamp(Dot(localPos, gContext.mRotationVectorSource), -0.9999f, 0.9999f);
+      float angle = acosf(acosAngle);
+      angle *= (Dot(localPos, perpendicularVector) < 0.f) ? 1.f : -1.f;
+      return angle;
+   }
+
+   static void DrawRotationGizmo(int type)
+   {
+      ImDrawList* drawList = gContext.mDrawList;
+
+      // colors
+      ImU32 colors[7];
+      ComputeColors(colors, type, ROTATE);
+
+     vec_t cameraToModelNormalized;
+     if (gContext.mIsOrthographic)
+     {
+        matrix_t viewInverse;
+        viewInverse.Inverse(*(matrix_t*)&gContext.mViewMat);
+        cameraToModelNormalized = viewInverse.v.dir;
+     }
+     else
+     {
+        cameraToModelNormalized = Normalized(gContext.mModel.v.position - gContext.mCameraEye);
+     }
+
+      cameraToModelNormalized.TransformVector(gContext.mModelInverse);
+
+      gContext.mRadiusSquareCenter = screenRotateSize * gContext.mHeight;
+
+     for (int axis = 0; axis < 3; axis++)
+      {
+         ImVec2 circlePos[halfCircleSegmentCount];
+
+         float angleStart = atan2f(cameraToModelNormalized[(4-axis)%3], cameraToModelNormalized[(3 - axis) % 3]) + ZPI * 0.5f;
+
+         for (unsigned int i = 0; i < halfCircleSegmentCount; i++)
+         {
+            float ng = angleStart + ZPI * ((float)i / (float)halfCircleSegmentCount);
+            vec_t axisPos = makeVect(cosf(ng), sinf(ng), 0.f);
+            vec_t pos = makeVect(axisPos[axis], axisPos[(axis+1)%3], axisPos[(axis+2)%3]) * gContext.mScreenFactor;
+            circlePos[i] = worldToPos(pos, gContext.mMVP);
+         }
+
+         float radiusAxis = sqrtf( (ImLengthSqr(worldToPos(gContext.mModel.v.position, gContext.mViewProjection) - circlePos[0]) ));
+         if(radiusAxis > gContext.mRadiusSquareCenter)
+           gContext.mRadiusSquareCenter = radiusAxis;
+
+         drawList->AddPolyline(circlePos, halfCircleSegmentCount, colors[3 - axis], false, 2);
+      }
+      drawList->AddCircle(worldToPos(gContext.mModel.v.position, gContext.mViewProjection), gContext.mRadiusSquareCenter, colors[0], 64, 3.f);
+
+      if (gContext.mbUsing)
+      {
+         ImVec2 circlePos[halfCircleSegmentCount +1];
+
+         circlePos[0] = worldToPos(gContext.mModel.v.position, gContext.mViewProjection);
+         for (unsigned int i = 1; i < halfCircleSegmentCount; i++)
+         {
+            float ng = gContext.mRotationAngle * ((float)(i-1) / (float)(halfCircleSegmentCount -1));
+            matrix_t rotateVectorMatrix;
+            rotateVectorMatrix.RotationAxis(gContext.mTranslationPlan, ng);
+            vec_t pos;
+            pos.TransformPoint(gContext.mRotationVectorSource, rotateVectorMatrix);
+            pos *= gContext.mScreenFactor;
+            circlePos[i] = worldToPos(pos + gContext.mModel.v.position, gContext.mViewProjection);
+         }
+         drawList->AddConvexPolyFilled(circlePos, halfCircleSegmentCount, 0x801080FF);
+         drawList->AddPolyline(circlePos, halfCircleSegmentCount, 0xFF1080FF, true, 2);
+
+         ImVec2 destinationPosOnScreen = circlePos[1];
+         char tmps[512];
+         ImFormatString(tmps, sizeof(tmps), rotationInfoMask[type - ROTATE_X], (gContext.mRotationAngle/ZPI)*180.f, gContext.mRotationAngle);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), 0xFF000000, tmps);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), 0xFFFFFFFF, tmps);
+      }
+   }
+
+   static void DrawHatchedAxis(const vec_t& axis)
+   {
+      for (int j = 1; j < 10; j++)
+      {
+         ImVec2 baseSSpace2 = worldToPos(axis * 0.05f * (float)(j * 2) * gContext.mScreenFactor, gContext.mMVP);
+         ImVec2 worldDirSSpace2 = worldToPos(axis * 0.05f * (float)(j * 2 + 1) * gContext.mScreenFactor, gContext.mMVP);
+         gContext.mDrawList->AddLine(baseSSpace2, worldDirSSpace2, 0x80000000, 6.f);
+      }
+   }
+
+   static void DrawScaleGizmo(int type)
+   {
+      ImDrawList* drawList = gContext.mDrawList;
+
+      // colors
+      ImU32 colors[7];
+      ComputeColors(colors, type, SCALE);
+
+      // draw
+      vec_t scaleDisplay = { 1.f, 1.f, 1.f, 1.f };
+
+      if (gContext.mbUsing)
+         scaleDisplay = gContext.mScale;
+
+      for (unsigned int i = 0; i < 3; i++)
+      {
+        vec_t dirPlaneX, dirPlaneY, dirAxis;
+         bool belowAxisLimit, belowPlaneLimit;
+         ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit);
+
+         // draw axis
+         if (belowAxisLimit)
+         {
+            ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVP);
+            ImVec2 worldDirSSpaceNoScale = worldToPos(dirAxis * gContext.mScreenFactor, gContext.mMVP);
+            ImVec2 worldDirSSpace = worldToPos((dirAxis * scaleDisplay[i]) * gContext.mScreenFactor, gContext.mMVP);
+
+            if (gContext.mbUsing)
+            {
+               drawList->AddLine(baseSSpace, worldDirSSpaceNoScale, 0xFF404040, 3.f);
+               drawList->AddCircleFilled(worldDirSSpaceNoScale, 6.f, 0xFF404040);
+            }
+
+            drawList->AddLine(baseSSpace, worldDirSSpace, colors[i + 1], 3.f);
+            drawList->AddCircleFilled(worldDirSSpace, 6.f, colors[i + 1]);
+
+            if (gContext.mAxisFactor[i] < 0.f)
+               DrawHatchedAxis(dirAxis * scaleDisplay[i]);
+         }
+      }
+
+      // draw screen cirle
+      drawList->AddCircleFilled(gContext.mScreenSquareCenter, 6.f, colors[0], 32);
+
+      if (gContext.mbUsing)
+      {
+         //ImVec2 sourcePosOnScreen = worldToPos(gContext.mMatrixOrigin, gContext.mViewProjection);
+         ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection);
+         /*vec_t dif(destinationPosOnScreen.x - sourcePosOnScreen.x, destinationPosOnScreen.y - sourcePosOnScreen.y);
+         dif.Normalize();
+         dif *= 5.f;
+         drawList->AddCircle(sourcePosOnScreen, 6.f, translationLineColor);
+         drawList->AddCircle(destinationPosOnScreen, 6.f, translationLineColor);
+         drawList->AddLine(ImVec2(sourcePosOnScreen.x + dif.x, sourcePosOnScreen.y + dif.y), ImVec2(destinationPosOnScreen.x - dif.x, destinationPosOnScreen.y - dif.y), translationLineColor, 2.f);
+         */
+         char tmps[512];
+         //vec_t deltaInfo = gContext.mModel.v.position - gContext.mMatrixOrigin;
+         int componentInfoIndex = (type - SCALE_X) * 3;
+         ImFormatString(tmps, sizeof(tmps), scaleInfoMask[type - SCALE_X], scaleDisplay[translationInfoIndex[componentInfoIndex]]);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), 0xFF000000, tmps);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), 0xFFFFFFFF, tmps);
+      }
+   }
+
+
+   static void DrawTranslationGizmo(int type)
+   {
+      ImDrawList* drawList = gContext.mDrawList;
+      if (!drawList)
+          return;
+
+      // colors
+      ImU32 colors[7];
+      ComputeColors(colors, type, TRANSLATE);
+
+      const ImVec2 origin = worldToPos(gContext.mModel.v.position, gContext.mViewProjection);
+
+      // draw
+      bool belowAxisLimit = false;
+      bool belowPlaneLimit = false;
+      for (unsigned int i = 0; i < 3; ++i)
+      {
+         vec_t dirPlaneX, dirPlaneY, dirAxis;
+         ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit);
+
+         // draw axis
+         if (belowAxisLimit)
+         {
+            ImVec2 baseSSpace = worldToPos(dirAxis * 0.1f * gContext.mScreenFactor, gContext.mMVP);
+            ImVec2 worldDirSSpace = worldToPos(dirAxis * gContext.mScreenFactor, gContext.mMVP);
+
+            drawList->AddLine(baseSSpace, worldDirSSpace, colors[i + 1], 3.f);
+
+            // Arrow head begin
+            ImVec2 dir(origin - worldDirSSpace);
+
+            float d = sqrtf(ImLengthSqr(dir));
+            dir /= d; // Normalize
+            dir *= 6.0f;
+
+            ImVec2 ortogonalDir(dir.y, -dir.x); // Perpendicular vector
+            ImVec2 a(worldDirSSpace + dir);
+            drawList->AddTriangleFilled(worldDirSSpace - dir, a + ortogonalDir, a - ortogonalDir, colors[i + 1]);
+            // Arrow head end
+
+            if (gContext.mAxisFactor[i] < 0.f)
+               DrawHatchedAxis(dirAxis);
+         }
+
+         // draw plane
+         if (belowPlaneLimit)
+         {
+            ImVec2 screenQuadPts[4];
+            for (int j = 0; j < 4; ++j)
+            {
+               vec_t cornerWorldPos = (dirPlaneX * quadUV[j * 2] + dirPlaneY  * quadUV[j * 2 + 1]) * gContext.mScreenFactor;
+               screenQuadPts[j] = worldToPos(cornerWorldPos, gContext.mMVP);
+            }
+            drawList->AddPolyline(screenQuadPts, 4, directionColor[i], true, 1.0f);
+            drawList->AddConvexPolyFilled(screenQuadPts, 4, colors[i + 4]);
+         }
+      }
+
+      drawList->AddCircleFilled(gContext.mScreenSquareCenter, 6.f, colors[0], 32);
+
+      if (gContext.mbUsing)
+      {
+         ImVec2 sourcePosOnScreen = worldToPos(gContext.mMatrixOrigin, gContext.mViewProjection);
+         ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection);
+         vec_t dif = { destinationPosOnScreen.x - sourcePosOnScreen.x, destinationPosOnScreen.y - sourcePosOnScreen.y, 0.f, 0.f };
+         dif.Normalize();
+         dif *= 5.f;
+         drawList->AddCircle(sourcePosOnScreen, 6.f, translationLineColor);
+         drawList->AddCircle(destinationPosOnScreen, 6.f, translationLineColor);
+         drawList->AddLine(ImVec2(sourcePosOnScreen.x + dif.x, sourcePosOnScreen.y + dif.y), ImVec2(destinationPosOnScreen.x - dif.x, destinationPosOnScreen.y - dif.y), translationLineColor, 2.f);
+
+         char tmps[512];
+         vec_t deltaInfo = gContext.mModel.v.position - gContext.mMatrixOrigin;
+         int componentInfoIndex = (type - MOVE_X) * 3;
+         ImFormatString(tmps, sizeof(tmps), translationInfoMask[type - MOVE_X], deltaInfo[translationInfoIndex[componentInfoIndex]], deltaInfo[translationInfoIndex[componentInfoIndex + 1]], deltaInfo[translationInfoIndex[componentInfoIndex + 2]]);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), 0xFF000000, tmps);
+         drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), 0xFFFFFFFF, tmps);
+      }
+   }
+
+   static bool CanActivate()
+   {
+      if (ImGui::IsMouseClicked(0) && !ImGui::IsAnyItemHovered() && !ImGui::IsAnyItemActive())
+         return true;
+      return false;
+   }
+
+   static void HandleAndDrawLocalBounds(float *bounds, matrix_t *matrix, float *snapValues, OPERATION operation)
+   {
+       ImGuiIO& io = ImGui::GetIO();
+       ImDrawList* drawList = gContext.mDrawList;
+
+       // compute best projection axis
+       vec_t axesWorldDirections[3];
+       vec_t bestAxisWorldDirection = { 0.0f, 0.0f, 0.0f, 0.0f };
+       int axes[3];
+       unsigned int numAxes = 1;
+       axes[0] = gContext.mBoundsBestAxis;
+       int bestAxis = axes[0];
+       if (!gContext.mbUsingBounds)
+       {
+           numAxes = 0;
+           float bestDot = 0.f;
+           for (unsigned int i = 0; i < 3; i++)
+           {
+               vec_t dirPlaneNormalWorld;
+               dirPlaneNormalWorld.TransformVector(directionUnary[i], gContext.mModelSource);
+               dirPlaneNormalWorld.Normalize();
+
+               float dt = fabsf( Dot(Normalized(gContext.mCameraEye - gContext.mModelSource.v.position), dirPlaneNormalWorld) );
+               if ( dt >= bestDot )
+               {
+                   bestDot = dt;
+                   bestAxis = i;
+                   bestAxisWorldDirection = dirPlaneNormalWorld;
+               }
+
+               if( dt >= 0.1f )
+               {
+                   axes[numAxes] = i;
+                   axesWorldDirections[numAxes] = dirPlaneNormalWorld;
+                   ++numAxes;
+               }
+           }
+       }
+
+       if( numAxes == 0 )
+       {
+            axes[0] = bestAxis;
+            axesWorldDirections[0] = bestAxisWorldDirection;
+            numAxes = 1;
+       }
+       else if( bestAxis != axes[0] )
+       {
+          unsigned int bestIndex = 0;
+          for (unsigned int i = 0; i < numAxes; i++)
+          {
+              if( axes[i] == bestAxis )
+              {
+                  bestIndex = i;
+                  break;
+              }
+          }
+          int tempAxis = axes[0];
+          axes[0] = axes[bestIndex];
+          axes[bestIndex] = tempAxis;
+          vec_t tempDirection = axesWorldDirections[0];
+          axesWorldDirections[0] = axesWorldDirections[bestIndex];
+          axesWorldDirections[bestIndex] = tempDirection;
+       }
+
+       for (unsigned int axisIndex = 0; axisIndex < numAxes; ++axisIndex)
+       {
+           bestAxis = axes[axisIndex];
+           bestAxisWorldDirection = axesWorldDirections[axisIndex];
+
+           // corners
+           vec_t aabb[4];
+
+           int secondAxis = (bestAxis + 1) % 3;
+           int thirdAxis = (bestAxis + 2) % 3;
+
+           for (int i = 0; i < 4; i++)
+           {
+               aabb[i][3] = aabb[i][bestAxis] = 0.f;
+               aabb[i][secondAxis] = bounds[secondAxis + 3 * (i >> 1)];
+               aabb[i][thirdAxis] = bounds[thirdAxis + 3 * ((i >> 1) ^ (i & 1))];
+           }
+
+           // draw bounds
+           unsigned int anchorAlpha = gContext.mbEnable ? 0xFF000000 : 0x80000000;
+
+           matrix_t boundsMVP = gContext.mModelSource * gContext.mViewProjection;
+           for (int i = 0; i < 4;i++)
+           {
+               ImVec2 worldBound1 = worldToPos(aabb[i], boundsMVP);
+               ImVec2 worldBound2 = worldToPos(aabb[(i+1)%4], boundsMVP);
+               if( !IsInContextRect( worldBound1 ) || !IsInContextRect( worldBound2 ) )
+               {
+                   continue;
+               }
+               float boundDistance = sqrtf(ImLengthSqr(worldBound1 - worldBound2));
+               int stepCount = (int)(boundDistance / 10.f);
+               stepCount = min( stepCount, 1000 );
+               float stepLength = 1.f / (float)stepCount;
+               for (int j = 0; j < stepCount; j++)
+               {
+                   float t1 = (float)j * stepLength;
+                   float t2 = (float)j * stepLength + stepLength * 0.5f;
+                   ImVec2 worldBoundSS1 = ImLerp(worldBound1, worldBound2, ImVec2(t1, t1));
+                   ImVec2 worldBoundSS2 = ImLerp(worldBound1, worldBound2, ImVec2(t2, t2));
+                   //drawList->AddLine(worldBoundSS1, worldBoundSS2, 0x000000 + anchorAlpha, 3.f);
+               drawList->AddLine(worldBoundSS1, worldBoundSS2, 0xAAAAAA + anchorAlpha, 2.f);
+               }
+               vec_t midPoint = (aabb[i] + aabb[(i + 1) % 4] ) * 0.5f;
+               ImVec2 midBound = worldToPos(midPoint, boundsMVP);
+               static const float AnchorBigRadius = 8.f;
+               static const float AnchorSmallRadius = 6.f;
+               bool overBigAnchor = ImLengthSqr(worldBound1 - io.MousePos) <= (AnchorBigRadius*AnchorBigRadius);
+               bool overSmallAnchor = ImLengthSqr(midBound - io.MousePos) <= (AnchorBigRadius*AnchorBigRadius);
+
+            int type = NONE;
+            vec_t gizmoHitProportion;
+
+            switch (operation)
+            {
+            case TRANSLATE: type = GetMoveType(&gizmoHitProportion); break;
+            case ROTATE: type = GetRotateType(); break;
+            case SCALE: type = GetScaleType(); break;
+            case BOUNDS: break;
+            }
+            if (type != NONE)
+            {
+               overBigAnchor = false;
+               overSmallAnchor = false;
+            }
+
+
+               unsigned int bigAnchorColor = overBigAnchor ? selectionColor : (0xAAAAAA + anchorAlpha);
+               unsigned int smallAnchorColor = overSmallAnchor ? selectionColor : (0xAAAAAA + anchorAlpha);
+
+               drawList->AddCircleFilled(worldBound1, AnchorBigRadius, 0xFF000000);
+            drawList->AddCircleFilled(worldBound1, AnchorBigRadius-1.2f, bigAnchorColor);
+
+               drawList->AddCircleFilled(midBound, AnchorSmallRadius, 0xFF000000);
+            drawList->AddCircleFilled(midBound, AnchorSmallRadius-1.2f, smallAnchorColor);
+               int oppositeIndex = (i + 2) % 4;
+               // big anchor on corners
+               if (!gContext.mbUsingBounds && gContext.mbEnable && overBigAnchor && CanActivate())
+               {
+                   gContext.mBoundsPivot.TransformPoint(aabb[(i + 2) % 4], gContext.mModelSource);
+                   gContext.mBoundsAnchor.TransformPoint(aabb[i], gContext.mModelSource);
+                   gContext.mBoundsPlan = BuildPlan(gContext.mBoundsAnchor, bestAxisWorldDirection);
+                   gContext.mBoundsBestAxis = bestAxis;
+                   gContext.mBoundsAxis[0] = secondAxis;
+                   gContext.mBoundsAxis[1] = thirdAxis;
+
+                   gContext.mBoundsLocalPivot.Set(0.f);
+                   gContext.mBoundsLocalPivot[secondAxis] = aabb[oppositeIndex][secondAxis];
+                   gContext.mBoundsLocalPivot[thirdAxis] = aabb[oppositeIndex][thirdAxis];
+
+                   gContext.mbUsingBounds = true;
+                   gContext.mBoundsMatrix = gContext.mModelSource;
+               }
+               // small anchor on middle of segment
+               if (!gContext.mbUsingBounds && gContext.mbEnable && overSmallAnchor && CanActivate())
+               {
+                   vec_t midPointOpposite = (aabb[(i + 2) % 4] + aabb[(i + 3) % 4]) * 0.5f;
+                   gContext.mBoundsPivot.TransformPoint(midPointOpposite, gContext.mModelSource);
+                   gContext.mBoundsAnchor.TransformPoint(midPoint, gContext.mModelSource);
+                   gContext.mBoundsPlan = BuildPlan(gContext.mBoundsAnchor, bestAxisWorldDirection);
+                   gContext.mBoundsBestAxis = bestAxis;
+                   int indices[] = { secondAxis , thirdAxis };
+                   gContext.mBoundsAxis[0] = indices[i%2];
+                   gContext.mBoundsAxis[1] = -1;
+
+                   gContext.mBoundsLocalPivot.Set(0.f);
+                   gContext.mBoundsLocalPivot[gContext.mBoundsAxis[0]] = aabb[oppositeIndex][indices[i % 2]];// bounds[gContext.mBoundsAxis[0]] * (((i + 1) & 2) ? 1.f : -1.f);
+
+                   gContext.mbUsingBounds = true;
+                   gContext.mBoundsMatrix = gContext.mModelSource;
+               }
+           }
+
+           if (gContext.mbUsingBounds)
+           {
+               matrix_t scale;
+               scale.SetToIdentity();
+
+               // compute projected mouse position on plan
+               const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mBoundsPlan);
+               vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len;
+
+               // compute a reference and delta vectors base on mouse move
+               vec_t deltaVector = (newPos - gContext.mBoundsPivot).Abs();
+               vec_t referenceVector = (gContext.mBoundsAnchor - gContext.mBoundsPivot).Abs();
+
+               // for 1 or 2 axes, compute a ratio that's used for scale and snap it based on resulting length
+               for (int i = 0; i < 2; i++)
+               {
+                   int axisIndex1 = gContext.mBoundsAxis[i];
+                   if (axisIndex1 == -1)
+                       continue;
+
+                   float ratioAxis = 1.f;
+                   vec_t axisDir = gContext.mBoundsMatrix.component[axisIndex1].Abs();
+
+                   float dtAxis = axisDir.Dot(referenceVector);
+                   float boundSize = bounds[axisIndex1 + 3] - bounds[axisIndex1];
+                   if (dtAxis > FLT_EPSILON)
+                       ratioAxis = axisDir.Dot(deltaVector) / dtAxis;
+
+                   if (snapValues)
+                   {
+                       float length = boundSize * ratioAxis;
+                       ComputeSnap(&length, snapValues[axisIndex1]);
+                       if (boundSize > FLT_EPSILON)
+                           ratioAxis = length / boundSize;
+                   }
+                   scale.component[axisIndex1] *= ratioAxis;
+               }
+
+               // transform matrix
+               matrix_t preScale, postScale;
+               preScale.Translation(-gContext.mBoundsLocalPivot);
+               postScale.Translation(gContext.mBoundsLocalPivot);
+               matrix_t res = preScale * scale * postScale * gContext.mBoundsMatrix;
+               *matrix = res;
+
+               // info text
+               char tmps[512];
+               ImVec2 destinationPosOnScreen = worldToPos(gContext.mModel.v.position, gContext.mViewProjection);
+               ImFormatString(tmps, sizeof(tmps), "X: %.2f Y: %.2f Z:%.2f"
+                   , (bounds[3] - bounds[0]) * gContext.mBoundsMatrix.component[0].Length() * scale.component[0].Length()
+                   , (bounds[4] - bounds[1]) * gContext.mBoundsMatrix.component[1].Length() * scale.component[1].Length()
+                   , (bounds[5] - bounds[2]) * gContext.mBoundsMatrix.component[2].Length() * scale.component[2].Length()
+               );
+               drawList->AddText(ImVec2(destinationPosOnScreen.x + 15, destinationPosOnScreen.y + 15), 0xFF000000, tmps);
+               drawList->AddText(ImVec2(destinationPosOnScreen.x + 14, destinationPosOnScreen.y + 14), 0xFFFFFFFF, tmps);
+            }
+
+           if (!io.MouseDown[0])
+               gContext.mbUsingBounds = false;
+
+           if( gContext.mbUsingBounds )
+               break;
+       }
+   }
+
+   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+   //
+
+   static int GetScaleType()
+   {
+      ImGuiIO& io = ImGui::GetIO();
+      int type = NONE;
+
+      // screen
+      if (io.MousePos.x >= gContext.mScreenSquareMin.x && io.MousePos.x <= gContext.mScreenSquareMax.x &&
+         io.MousePos.y >= gContext.mScreenSquareMin.y && io.MousePos.y <= gContext.mScreenSquareMax.y)
+         type = SCALE_XYZ;
+
+      // compute
+      for (unsigned int i = 0; i < 3 && type == NONE; i++)
+      {
+         vec_t dirPlaneX, dirPlaneY, dirAxis;
+         bool belowAxisLimit, belowPlaneLimit;
+         ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit);
+
+         dirAxis.TransformVector(gContext.mModel);
+       const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, BuildPlan(gContext.mModel.v.position, dirAxis));
+       vec_t posOnPlan = gContext.mRayOrigin + gContext.mRayVector * len;
+
+       const ImVec2 posOnPlanScreen = worldToPos(posOnPlan, gContext.mViewProjection);
+       const ImVec2 axisStartOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor * 0.1f, gContext.mViewProjection);
+       const ImVec2 axisEndOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor, gContext.mViewProjection);
+
+       vec_t closestPointOnAxis = PointOnSegment(makeVect(posOnPlanScreen), makeVect(axisStartOnScreen), makeVect(axisEndOnScreen));
+
+       if ((closestPointOnAxis - makeVect(posOnPlanScreen)).Length() < 12.f) // pixel size
+          type = SCALE_X + i;
+      }
+      return type;
+   }
+
+   static int GetRotateType()
+   {
+      ImGuiIO& io = ImGui::GetIO();
+      int type = NONE;
+
+      vec_t deltaScreen = { io.MousePos.x - gContext.mScreenSquareCenter.x, io.MousePos.y - gContext.mScreenSquareCenter.y, 0.f, 0.f };
+      float dist = deltaScreen.Length();
+      if (dist >= (gContext.mRadiusSquareCenter - 1.0f) && dist < (gContext.mRadiusSquareCenter + 1.0f))
+         type = ROTATE_SCREEN;
+
+      const vec_t planNormals[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir};
+
+      for (unsigned int i = 0; i < 3 && type == NONE; i++)
+      {
+         // pickup plan
+         vec_t pickupPlan = BuildPlan(gContext.mModel.v.position, planNormals[i]);
+
+         const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, pickupPlan);
+         vec_t localPos = gContext.mRayOrigin + gContext.mRayVector * len - gContext.mModel.v.position;
+
+         if (Dot(Normalized(localPos), gContext.mRayVector) > FLT_EPSILON)
+            continue;
+       vec_t idealPosOnCircle = Normalized(localPos);
+       idealPosOnCircle.TransformVector(gContext.mModelInverse);
+       ImVec2 idealPosOnCircleScreen = worldToPos(idealPosOnCircle * gContext.mScreenFactor, gContext.mMVP);
+
+       //gContext.mDrawList->AddCircle(idealPosOnCircleScreen, 5.f, 0xFFFFFFFF);
+       ImVec2 distanceOnScreen = idealPosOnCircleScreen - io.MousePos;
+
+         float distance = makeVect(distanceOnScreen).Length();
+         if (distance < 8.f) // pixel size
+            type = ROTATE_X + i;
+      }
+
+      return type;
+   }
+
+   static int GetMoveType(vec_t *gizmoHitProportion)
+   {
+      ImGuiIO& io = ImGui::GetIO();
+      int type = NONE;
+
+      // screen
+      if (io.MousePos.x >= gContext.mScreenSquareMin.x && io.MousePos.x <= gContext.mScreenSquareMax.x &&
+         io.MousePos.y >= gContext.mScreenSquareMin.y && io.MousePos.y <= gContext.mScreenSquareMax.y)
+         type = MOVE_SCREEN;
+
+      // compute
+      for (unsigned int i = 0; i < 3 && type == NONE; i++)
+      {
+         vec_t dirPlaneX, dirPlaneY, dirAxis;
+         bool belowAxisLimit, belowPlaneLimit;
+         ComputeTripodAxisAndVisibility(i, dirAxis, dirPlaneX, dirPlaneY, belowAxisLimit, belowPlaneLimit);
+       dirAxis.TransformVector(gContext.mModel);
+         dirPlaneX.TransformVector(gContext.mModel);
+         dirPlaneY.TransformVector(gContext.mModel);
+
+         const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, BuildPlan(gContext.mModel.v.position, dirAxis));
+         vec_t posOnPlan = gContext.mRayOrigin + gContext.mRayVector * len;
+
+       const ImVec2 posOnPlanScreen = worldToPos(posOnPlan, gContext.mViewProjection);
+       const ImVec2 axisStartOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor * 0.1f, gContext.mViewProjection);
+       const ImVec2 axisEndOnScreen = worldToPos(gContext.mModel.v.position + dirAxis * gContext.mScreenFactor, gContext.mViewProjection);
+
+       vec_t closestPointOnAxis = PointOnSegment(makeVect(posOnPlanScreen), makeVect(axisStartOnScreen), makeVect(axisEndOnScreen));
+
+       if ((closestPointOnAxis - makeVect(posOnPlanScreen)).Length() < 12.f) // pixel size
+            type = MOVE_X + i;
+
+       const float dx = dirPlaneX.Dot3((posOnPlan - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor));
+       const float dy = dirPlaneY.Dot3((posOnPlan - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor));
+         if (belowPlaneLimit && dx >= quadUV[0] && dx <= quadUV[4] && dy >= quadUV[1] && dy <= quadUV[3])
+            type = MOVE_YZ + i;
+
+         if (gizmoHitProportion)
+            *gizmoHitProportion = makeVect(dx, dy, 0.f);
+      }
+      return type;
+   }
+
+   static void HandleTranslation(float *matrix, float *deltaMatrix, int& type, float *snap)
+   {
+      ImGuiIO& io = ImGui::GetIO();
+      bool applyRotationLocaly = gContext.mMode == LOCAL || type == MOVE_SCREEN;
+
+      // move
+      if (gContext.mbUsing)
+      {
+         ImGui::CaptureMouseFromApp();
+         const float len = fabsf(IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan)); // near plan
+         vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len;
+
+
+
+         // compute delta
+         vec_t newOrigin = newPos - gContext.mRelativeOrigin * gContext.mScreenFactor;
+         vec_t delta = newOrigin - gContext.mModel.v.position;
+
+         // 1 axis constraint
+         if (gContext.mCurrentOperation >= MOVE_X && gContext.mCurrentOperation <= MOVE_Z)
+         {
+            int axisIndex = gContext.mCurrentOperation - MOVE_X;
+            const vec_t& axisValue = *(vec_t*)&gContext.mModel.m[axisIndex];
+            float lengthOnAxis = Dot(axisValue, delta);
+            delta = axisValue * lengthOnAxis;
+         }
+
+         // snap
+         if (snap)
+         {
+            vec_t cumulativeDelta = gContext.mModel.v.position + delta - gContext.mMatrixOrigin;
+            if (applyRotationLocaly)
+            {
+               matrix_t modelSourceNormalized = gContext.mModelSource;
+               modelSourceNormalized.OrthoNormalize();
+               matrix_t modelSourceNormalizedInverse;
+               modelSourceNormalizedInverse.Inverse(modelSourceNormalized);
+               cumulativeDelta.TransformVector(modelSourceNormalizedInverse);
+               ComputeSnap(cumulativeDelta, snap);
+               cumulativeDelta.TransformVector(modelSourceNormalized);
+            }
+            else
+            {
+               ComputeSnap(cumulativeDelta, snap);
+            }
+            delta = gContext.mMatrixOrigin + cumulativeDelta - gContext.mModel.v.position;
+
+         }
+
+         // compute matrix & delta
+         matrix_t deltaMatrixTranslation;
+         deltaMatrixTranslation.Translation(delta);
+         if (deltaMatrix)
+            memcpy(deltaMatrix, deltaMatrixTranslation.m16, sizeof(float) * 16);
+
+
+         matrix_t res = gContext.mModelSource * deltaMatrixTranslation;
+         *(matrix_t*)matrix = res;
+
+         if (!io.MouseDown[0])
+            gContext.mbUsing = false;
+
+         type = gContext.mCurrentOperation;
+      }
+      else
+      {
+         // find new possible way to move
+         vec_t gizmoHitProportion;
+         type = GetMoveType(&gizmoHitProportion);
+         if(type != NONE)
+         {
+            ImGui::CaptureMouseFromApp();
+         }
+       if (CanActivate() && type != NONE)
+       {
+          gContext.mbUsing = true;
+          gContext.mCurrentOperation = type;
+          vec_t movePlanNormal[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir,
+             gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir,
+             -gContext.mCameraDir };
+
+          vec_t cameraToModelNormalized = Normalized(gContext.mModel.v.position - gContext.mCameraEye);
+          for (unsigned int i = 0; i < 3; i++)
+          {
+             vec_t orthoVector = Cross(movePlanNormal[i], cameraToModelNormalized);
+             movePlanNormal[i].Cross(orthoVector);
+             movePlanNormal[i].Normalize();
+          }
+            // pickup plan
+            gContext.mTranslationPlan = BuildPlan(gContext.mModel.v.position, movePlanNormal[type - MOVE_X]);
+            const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan);
+            gContext.mTranslationPlanOrigin = gContext.mRayOrigin + gContext.mRayVector * len;
+            gContext.mMatrixOrigin = gContext.mModel.v.position;
+
+            gContext.mRelativeOrigin = (gContext.mTranslationPlanOrigin - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor);
+         }
+      }
+   }
+
+   static void HandleScale(float *matrix, float *deltaMatrix, int& type, float *snap)
+   {
+      ImGuiIO& io = ImGui::GetIO();
+
+      if (!gContext.mbUsing)
+      {
+         // find new possible way to scale
+         type = GetScaleType();
+         if(type != NONE)
+         {
+            ImGui::CaptureMouseFromApp();
+         }
+         if (CanActivate() && type != NONE)
+         {
+            gContext.mbUsing = true;
+            gContext.mCurrentOperation = type;
+            const vec_t movePlanNormal[] = { gContext.mModel.v.up, gContext.mModel.v.dir, gContext.mModel.v.right, gContext.mModel.v.dir, gContext.mModel.v.up, gContext.mModel.v.right, -gContext.mCameraDir };
+            // pickup plan
+
+            gContext.mTranslationPlan = BuildPlan(gContext.mModel.v.position, movePlanNormal[type - SCALE_X]);
+            const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan);
+            gContext.mTranslationPlanOrigin = gContext.mRayOrigin + gContext.mRayVector * len;
+            gContext.mMatrixOrigin = gContext.mModel.v.position;
+            gContext.mScale.Set(1.f, 1.f, 1.f);
+            gContext.mRelativeOrigin = (gContext.mTranslationPlanOrigin - gContext.mModel.v.position) * (1.f / gContext.mScreenFactor);
+            gContext.mScaleValueOrigin = makeVect(gContext.mModelSource.v.right.Length(), gContext.mModelSource.v.up.Length(), gContext.mModelSource.v.dir.Length());
+            gContext.mSaveMousePosx = io.MousePos.x;
+         }
+      }
+      // scale
+      if (gContext.mbUsing)
+      {
+         ImGui::CaptureMouseFromApp();
+         const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan);
+         vec_t newPos = gContext.mRayOrigin + gContext.mRayVector * len;
+         vec_t newOrigin = newPos - gContext.mRelativeOrigin * gContext.mScreenFactor;
+         vec_t delta = newOrigin - gContext.mModel.v.position;
+
+         // 1 axis constraint
+         if (gContext.mCurrentOperation >= SCALE_X && gContext.mCurrentOperation <= SCALE_Z)
+         {
+            int axisIndex = gContext.mCurrentOperation - SCALE_X;
+            const vec_t& axisValue = *(vec_t*)&gContext.mModel.m[axisIndex];
+            float lengthOnAxis = Dot(axisValue, delta);
+            delta = axisValue * lengthOnAxis;
+
+            vec_t baseVector = gContext.mTranslationPlanOrigin - gContext.mModel.v.position;
+            float ratio = Dot(axisValue, baseVector + delta) / Dot(axisValue, baseVector);
+
+            gContext.mScale[axisIndex] = max(ratio, 0.001f);
+         }
+         else
+         {
+            float scaleDelta = (io.MousePos.x - gContext.mSaveMousePosx)  * 0.01f;
+            gContext.mScale.Set(max(1.f + scaleDelta, 0.001f));
+         }
+
+         // snap
+         if (snap)
+         {
+            float scaleSnap[] = { snap[0], snap[0], snap[0] };
+            ComputeSnap(gContext.mScale, scaleSnap);
+         }
+
+         // no 0 allowed
+         for (int i = 0; i < 3;i++)
+            gContext.mScale[i] = max(gContext.mScale[i], 0.001f);
+
+         // compute matrix & delta
+         matrix_t deltaMatrixScale;
+         deltaMatrixScale.Scale(gContext.mScale * gContext.mScaleValueOrigin);
+
+         matrix_t res = deltaMatrixScale * gContext.mModel;
+         *(matrix_t*)matrix = res;
+
+         if (deltaMatrix)
+         {
+            deltaMatrixScale.Scale(gContext.mScale);
+            memcpy(deltaMatrix, deltaMatrixScale.m16, sizeof(float) * 16);
+         }
+
+         if (!io.MouseDown[0])
+            gContext.mbUsing = false;
+
+         type = gContext.mCurrentOperation;
+      }
+   }
+
+   static void HandleRotation(float *matrix, float *deltaMatrix, int& type, float *snap)
+   {
+      ImGuiIO& io = ImGui::GetIO();
+      bool applyRotationLocaly = gContext.mMode == LOCAL;
+
+      if (!gContext.mbUsing)
+      {
+         type = GetRotateType();
+
+         if(type != NONE)
+         {
+            ImGui::CaptureMouseFromApp();
+         }
+
+         if (type == ROTATE_SCREEN)
+         {
+            applyRotationLocaly = true;
+         }
+
+         if (CanActivate() && type != NONE)
+         {
+            gContext.mbUsing = true;
+            gContext.mCurrentOperation = type;
+            const vec_t rotatePlanNormal[] = { gContext.mModel.v.right, gContext.mModel.v.up, gContext.mModel.v.dir, -gContext.mCameraDir };
+            // pickup plan
+            if (applyRotationLocaly)
+            {
+               gContext.mTranslationPlan = BuildPlan(gContext.mModel.v.position, rotatePlanNormal[type - ROTATE_X]);
+            }
+            else
+            {
+               gContext.mTranslationPlan = BuildPlan(gContext.mModelSource.v.position, directionUnary[type - ROTATE_X]);
+            }
+
+            const float len = IntersectRayPlane(gContext.mRayOrigin, gContext.mRayVector, gContext.mTranslationPlan);
+            vec_t localPos = gContext.mRayOrigin + gContext.mRayVector * len - gContext.mModel.v.position;
+            gContext.mRotationVectorSource = Normalized(localPos);
+            gContext.mRotationAngleOrigin = ComputeAngleOnPlan();
+         }
+      }
+
+      // rotation
+      if (gContext.mbUsing)
+      {
+         ImGui::CaptureMouseFromApp();
+         gContext.mRotationAngle = ComputeAngleOnPlan();
+         if (snap)
+         {
+            float snapInRadian = snap[0] * DEG2RAD;
+            ComputeSnap(&gContext.mRotationAngle, snapInRadian);
+         }
+         vec_t rotationAxisLocalSpace;
+
+         rotationAxisLocalSpace.TransformVector(makeVect(gContext.mTranslationPlan.x, gContext.mTranslationPlan.y, gContext.mTranslationPlan.z, 0.f), gContext.mModelInverse);
+         rotationAxisLocalSpace.Normalize();
+
+         matrix_t deltaRotation;
+         deltaRotation.RotationAxis(rotationAxisLocalSpace, gContext.mRotationAngle - gContext.mRotationAngleOrigin);
+         gContext.mRotationAngleOrigin = gContext.mRotationAngle;
+
+         matrix_t scaleOrigin;
+         scaleOrigin.Scale(gContext.mModelScaleOrigin);
+
+         if (applyRotationLocaly)
+         {
+            *(matrix_t*)matrix = scaleOrigin * deltaRotation * gContext.mModel;
+         }
+         else
+         {
+            matrix_t res = gContext.mModelSource;
+            res.v.position.Set(0.f);
+
+            *(matrix_t*)matrix = res * deltaRotation;
+            ((matrix_t*)matrix)->v.position = gContext.mModelSource.v.position;
+         }
+
+         if (deltaMatrix)
+         {
+            *(matrix_t*)deltaMatrix = gContext.mModelInverse * deltaRotation * gContext.mModel;
+         }
+
+         if (!io.MouseDown[0])
+            gContext.mbUsing = false;
+
+         type = gContext.mCurrentOperation;
+      }
+   }
+
+   void DecomposeMatrixToComponents(const float *matrix, float *translation, float *rotation, float *scale)
+   {
+      matrix_t mat = *(matrix_t*)matrix;
+
+      scale[0] = mat.v.right.Length();
+      scale[1] = mat.v.up.Length();
+      scale[2] = mat.v.dir.Length();
+
+      mat.OrthoNormalize();
+
+      rotation[0] = RAD2DEG * atan2f(mat.m[1][2], mat.m[2][2]);
+      rotation[1] = RAD2DEG * atan2f(-mat.m[0][2], sqrtf(mat.m[1][2] * mat.m[1][2] + mat.m[2][2]* mat.m[2][2]));
+      rotation[2] = RAD2DEG * atan2f(mat.m[0][1], mat.m[0][0]);
+
+      translation[0] = mat.v.position.x;
+      translation[1] = mat.v.position.y;
+      translation[2] = mat.v.position.z;
+   }
+
+   void RecomposeMatrixFromComponents(const float *translation, const float *rotation, const float *scale, float *matrix)
+   {
+      matrix_t& mat = *(matrix_t*)matrix;
+
+      matrix_t rot[3];
+      for (int i = 0; i < 3;i++)
+         rot[i].RotationAxis(directionUnary[i], rotation[i] * DEG2RAD);
+
+      mat = rot[0] * rot[1] * rot[2];
+
+      float validScale[3];
+      for (int i = 0; i < 3; i++)
+      {
+         if (fabsf(scale[i]) < FLT_EPSILON)
+            validScale[i] = 0.001f;
+         else
+            validScale[i] = scale[i];
+      }
+      mat.v.right *= validScale[0];
+      mat.v.up *= validScale[1];
+      mat.v.dir *= validScale[2];
+      mat.v.position.Set(translation[0], translation[1], translation[2], 1.f);
+   }
+
+   void Manipulate(const float *view, const float *projection, OPERATION operation, MODE mode, float *matrix, float *deltaMatrix, float *snap, float *localBounds, float *boundsSnap)
+   {
+      ComputeContext(view, projection, matrix, mode);
+
+      // set delta to identity
+      if (deltaMatrix)
+         ((matrix_t*)deltaMatrix)->SetToIdentity();
+
+      // behind camera
+      vec_t camSpacePosition;
+      camSpacePosition.TransformPoint(makeVect(0.f, 0.f, 0.f), gContext.mMVP);
+      if (!gContext.mIsOrthographic && camSpacePosition.z < 0.001f)
+         return;
+
+      // --
+      int type = NONE;
+      if (gContext.mbEnable)
+      {
+          if (!gContext.mbUsingBounds)
+          {
+              switch (operation)
+              {
+              case ROTATE:
+                  HandleRotation(matrix, deltaMatrix, type, snap);
+                  break;
+              case TRANSLATE:
+                  HandleTranslation(matrix, deltaMatrix, type, snap);
+                  break;
+              case SCALE:
+                  HandleScale(matrix, deltaMatrix, type, snap);
+                  break;
+              case BOUNDS:
+                  break;
+              }
+          }
+      }
+
+      if (localBounds && !gContext.mbUsing)
+          HandleAndDrawLocalBounds(localBounds, (matrix_t*)matrix, boundsSnap, operation);
+
+      if (!gContext.mbUsingBounds)
+      {
+          switch (operation)
+          {
+          case ROTATE:
+              DrawRotationGizmo(type);
+              break;
+          case TRANSLATE:
+              DrawTranslationGizmo(type);
+              break;
+          case SCALE:
+              DrawScaleGizmo(type);
+              break;
+          case BOUNDS:
+              break;
+          }
+      }
+   }
+
+   void DrawCube(const float *view, const float *projection, const float *matrix)
+   {
+      matrix_t viewInverse;
+      viewInverse.Inverse(*(matrix_t*)view);
+      const matrix_t& model = *(matrix_t*)matrix;
+      matrix_t res = *(matrix_t*)matrix * *(matrix_t*)view * *(matrix_t*)projection;
+
+      for (int iFace = 0; iFace < 6; iFace++)
+      {
+         const int normalIndex = (iFace % 3);
+         const int perpXIndex = (normalIndex + 1) % 3;
+         const int perpYIndex = (normalIndex + 2) % 3;
+         const float invert = (iFace > 2) ? -1.f : 1.f;
+
+         const vec_t faceCoords[4] = { directionUnary[normalIndex] + directionUnary[perpXIndex] + directionUnary[perpYIndex],
+            directionUnary[normalIndex] + directionUnary[perpXIndex] - directionUnary[perpYIndex],
+            directionUnary[normalIndex] - directionUnary[perpXIndex] - directionUnary[perpYIndex],
+            directionUnary[normalIndex] - directionUnary[perpXIndex] + directionUnary[perpYIndex],
+         };
+
+         // clipping
+         bool skipFace = false;
+         for (unsigned int iCoord = 0; iCoord < 4; iCoord++)
+         {
+            vec_t camSpacePosition;
+            camSpacePosition.TransformPoint(faceCoords[iCoord] * 0.5f * invert, gContext.mMVP);
+            if (camSpacePosition.z < 0.001f)
+            {
+               skipFace = true;
+               break;
+            }
+         }
+         if (skipFace)
+            continue;
+
+         // 3D->2D
+         ImVec2 faceCoordsScreen[4];
+         for (unsigned int iCoord = 0; iCoord < 4; iCoord++)
+            faceCoordsScreen[iCoord] = worldToPos(faceCoords[iCoord] * 0.5f * invert, res);
+
+         // back face culling
+         vec_t cullPos, cullNormal;
+         cullPos.TransformPoint(faceCoords[0] * 0.5f * invert, model);
+         cullNormal.TransformVector(directionUnary[normalIndex] * invert, model);
+         float dt = Dot(Normalized(cullPos - viewInverse.v.position), Normalized(cullNormal));
+         if (dt>0.f)
+            continue;
+
+         // draw face with lighter color
+         gContext.mDrawList->AddConvexPolyFilled(faceCoordsScreen, 4, directionColor[normalIndex] | 0x808080);
+      }
+   }
+
+   void DrawGrid(const float *view, const float *projection, const float *matrix, const float gridSize)
+   {
+      matrix_t res = *(matrix_t*)matrix * *(matrix_t*)view * *(matrix_t*)projection;
+
+      for (float f = -gridSize; f <= gridSize; f += 1.f)
+      {
+         gContext.mDrawList->AddLine(worldToPos(makeVect(f, 0.f, -gridSize), res), worldToPos(makeVect(f, 0.f, gridSize), res), 0xFF808080);
+         gContext.mDrawList->AddLine(worldToPos(makeVect(-gridSize, 0.f, f), res), worldToPos(makeVect(gridSize, 0.f, f), res), 0xFF808080);
+      }
+   }
+};
+
diff --git a/src/utils/gfx.cpp b/src/utils/gfx.cpp
new file mode 100644
index 0000000..0f82a2f
--- /dev/null
+++ b/src/utils/gfx.cpp
@@ -0,0 +1,909 @@
+#include "gfx.h"
+
+using namespace arma;
+#define PI				3.14159265358979323846
+
+namespace gfx
+{
+
+void init()
+{
+	glewInit();
+}
+
+int checkGlExtension(char const *name)
+{
+	const char *glExtensions = (const char *)glGetString(GL_EXTENSIONS);
+	return (strstr(glExtensions, name) != NULL);
+}
+
+bool getGLError()
+{
+	GLenum err = glGetError();
+	//glClearError();
+	if( err != GL_NO_ERROR )
+	{
+		#if !defined(CM_GLES) && !defined(GFX_OSX)
+			printf("GL ERROR %d %s",(int)err,gluErrorString(err));
+			#endif
+		return true;
+	}
+
+	return false;
+}
+
+
+arma::fmat perspective(float fovy, float aspect, float zNear, float zFar)
+{
+	const float top = zNear * tan(fovy / 2.0f);
+	const float bottom = -top;
+	const float right = top * aspect;
+	const float left = -right;
+
+	arma::fmat projection = arma::zeros<fmat>(4,4);
+
+	projection(0, 0) = 2.0f * zNear / (right - left);
+	projection(0, 2) = (right + left) / (right - left);
+	projection(1, 1) = 2.0f * zNear / (top - bottom);
+	projection(1, 2) = (top + bottom) / (top - bottom);
+	projection(2, 2) = -(zFar + zNear) / (zFar - zNear);
+	projection(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
+	projection(3, 2) = -1.0f;
+
+	return projection;
+}
+
+
+void setPerspectiveProjection(float fovy, float aspect, float zNear, float zFar)
+{
+	glMatrixMode( GL_PROJECTION );
+	glLoadIdentity();
+
+	arma::fmat projection = perspective(fovy, aspect, zNear, zFar);
+
+	// Add to current matrix
+	glMultMatrixf(projection.memptr());
+
+	glMatrixMode( GL_MODELVIEW );
+}
+
+void setOrtho( float w, float h )
+{
+	glMatrixMode( GL_PROJECTION );
+	glLoadIdentity();
+	glOrtho( 0, 0+w, 0+h, 0, -1., 1.);
+	glMatrixMode( GL_MODELVIEW );
+	glLoadIdentity();
+}
+
+void multMatrix( const arma::mat& m_ )
+{
+	arma::mat m = m_;
+	if(m.n_rows==3)
+	{
+		m.insert_cols(2, 1);
+		m.insert_rows(2, 1);
+	}
+	glMultMatrixd(m.memptr());
+}
+
+arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target, const arma::fvec& up)
+{
+	const arma::fvec f(arma::normalise(target - position));
+	const arma::fvec s(arma::normalise(arma::cross(f, up)));
+	const arma::fvec u(arma::cross(s, f));
+
+	arma::fmat result = arma::zeros<fmat>(4,4);
+	result(0, 0) = s(0);
+	result(0, 1) = s(1);
+	result(0, 2) = s(2);
+	result(1, 0) = u(0);
+	result(1, 1) = u(1);
+	result(1, 2) = u(2);
+	result(2, 0) =-f(0);
+	result(2, 1) =-f(1);
+	result(2, 2) =-f(2);
+	result(0, 3) =-arma::dot(s, position);
+	result(1, 3) =-arma::dot(u, position);
+	result(2, 3) = arma::dot(f, position);
+	result(3, 3) = 1.0f;
+
+	return result;
+}
+
+arma::fmat rotate(const arma::fvec& axis, float angle)
+{
+	float rcos = cos(angle);
+	float rsin = sin(angle);
+
+	arma::fmat matrix = arma::zeros<arma::fmat>(4, 4);
+
+	matrix(0, 0) =			  rcos + axis(0) * axis(0) * (1.0f - rcos);
+	matrix(1, 0) =	axis(2) * rsin + axis(1) * axis(0) * (1.0f - rcos);
+	matrix(2, 0) = -axis(1) * rsin + axis(2) * axis(0) * (1.0f - rcos);
+	matrix(0, 1) = -axis(2) * rsin + axis(0) * axis(1) * (1.0f - rcos);
+	matrix(1, 1) =			  rcos + axis(1) * axis(1) * (1.0f - rcos);
+	matrix(2, 1) =	axis(0) * rsin + axis(2) * axis(1) * (1.0f - rcos);
+	matrix(0, 2) =	axis(1) * rsin + axis(0) * axis(2) * (1.0f - rcos);
+	matrix(1, 2) = -axis(0) * rsin + axis(1) * axis(2) * (1.0f - rcos);
+	matrix(2, 2) =			  rcos + axis(2) * axis(2) * (1.0f - rcos);
+	matrix(3, 3) = 1.0f;
+
+	return matrix;
+}
+
+
+///////////////////////////////////////////////////////////////
+
+void drawUVQuad( float x  , float y	 , float w , float h, float maxU, float maxV, bool flip )
+{
+	GLfloat tex_coords[8];
+
+	if(flip)
+	{
+		tex_coords[0] = 0.0f; tex_coords[1] = maxV;
+		tex_coords[2] = 0.0f; tex_coords[3] = 0.0f;
+		tex_coords[4] = maxU; tex_coords[5] =  0.0f;
+		tex_coords[6] = maxU; tex_coords[7] = maxV;
+	}
+	else
+	{
+		tex_coords[0] = 0.0f; tex_coords[1] = 0.0f;
+		tex_coords[2] = 0.0f; tex_coords[3] = maxV;
+		tex_coords[4] = maxU; tex_coords[5] = maxV;
+		tex_coords[6] = maxU; tex_coords[7] = 0.0f;
+	}
+
+	GLfloat verts[] = {
+		x,y,
+		x,y+h,
+		x+w,y+h,
+		x+w,y
+	};
+
+	glEnableClientState( GL_TEXTURE_COORD_ARRAY );
+	glTexCoordPointer(2, GL_FLOAT, 0, tex_coords );
+
+	glEnableClientState(GL_VERTEX_ARRAY);
+	glVertexPointer(2, GL_FLOAT, 0, verts );
+	glDrawArrays( GL_TRIANGLE_FAN, 0, 4 );
+	glDisableClientState( GL_TEXTURE_COORD_ARRAY );
+}
+
+void drawQuad( float x	, float y  , float w , float h )
+{
+	GLfloat verts[] = {
+		x+w,y,
+		x+w,y+h,
+		x,y+h,
+		x,y
+	};
+
+	glBegin(GL_TRIANGLE_FAN);
+	for( int i = 0; i < 8; i+=2 )
+	{
+		glVertex2f(verts[i],verts[i+1]);
+	}
+	glEnd();
+}
+
+
+void enableAntiAliasing( bool aa )
+{
+
+	if ( aa )
+	{
+		glHint( GL_POINT_SMOOTH_HINT, GL_NICEST );
+		glHint( GL_LINE_SMOOTH_HINT, GL_NICEST );
+		glHint( GL_POLYGON_SMOOTH_HINT, GL_NICEST );
+		glEnable( GL_POINT_SMOOTH );
+		glEnable( GL_LINE_SMOOTH );
+		glEnable( GL_POLYGON_SMOOTH );
+		glLineWidth( 0.5 );
+	}	else	{
+		glHint( GL_POINT_SMOOTH_HINT, GL_FASTEST );
+		glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST );
+		glHint( GL_POLYGON_SMOOTH_HINT, GL_FASTEST );
+		glDisable( GL_POINT_SMOOTH );
+		glDisable( GL_LINE_SMOOTH );
+		glDisable( GL_POLYGON_SMOOTH );
+	}
+}
+
+void vertex( const arma::vec& v	 )
+{
+	switch(v.size())
+	{
+		case 2:
+		glVertex2f(v[0], v[1]);
+		break;
+		case 4:
+		glVertex4f(v[0], v[1], v[2], v[3]);
+		break;
+		default:
+		glVertex3f(v[0], v[1], v[2]);
+		break;
+	}
+}
+
+void draw( const arma::mat& P, int prim )
+{
+	if(!P.size())
+		return;
+
+	glBegin(prim);
+	switch(P.n_rows)
+	{
+		case 2:
+		{
+			for( int i = 0; i < P.n_cols; i++ )
+			{
+				glVertex2f(P.at(0,i), P.at(1,i)); //  )P[i][0], P[i][1] );
+			}
+			break;
+		}
+		default:
+		{
+			for( int i = 0; i < P.n_cols; i++ )
+			{
+				glVertex3f(P.at(0, i), P.at(1, i), P.at(2, i));//P[i][0], P[i][1], P[i][2] );
+			}
+		break;
+		}
+	}
+	glEnd();
+}
+
+void drawLine(	const arma::vec&  a,  const arma::vec&	b )
+{
+	glBegin(GL_LINES);
+	vertex(a);
+	vertex(b);
+	glEnd();
+}
+
+
+void drawAxis( const arma::vec& m, float scale	)
+{
+	arma::vec pos = m.submat(0, 3, 2, 3);
+	arma::vec x = pos + m.submat(0, 0, 2, 0)*scale;
+	arma::vec y = pos + m.submat(0, 1, 2, 1)*scale;
+	arma::vec z = pos + m.submat(0, 2, 2, 2)*scale;
+
+	glBegin(GL_LINE_STRIP);
+	glColor4f(1,0,0,1);
+	vertex(pos);
+	vertex(x);
+	glColor4f(0,1,0,1);
+	vertex(pos);
+	vertex(y);
+	glColor4f(0,0,1,1);
+	vertex(pos);
+	vertex(z);
+	glColor4f(1,1,1,1);
+	glEnd();
+}
+
+///////////////////////////////////////////////////////////////
+
+// Handle release
+struct GLObj
+{
+	enum TYPE
+	{
+		TEXTURE = 0,
+		SHADER = 1,
+		PROGRAM,
+		VB,
+		IB
+	};
+
+	GLObj( GLuint glId, TYPE type )
+	:
+	glId(glId),
+	type(type)
+	{
+	}
+
+	GLObj()
+	{
+	}
+
+	GLuint glId;
+	TYPE type;
+};
+
+static void releaseGLObject( GLObj o )
+{
+	switch(o.type)
+	{
+		case GLObj::TEXTURE:
+			glDeleteTextures(1,&o.glId);
+			break;
+
+		case GLObj::SHADER:
+			glDeleteShader(o.glId);
+			break;
+
+		case GLObj::PROGRAM:
+			glDeleteProgram(o.glId);
+			break;
+
+		case GLObj::VB:
+			glDeleteBuffers(1,&o.glId);
+			break;
+
+		case GLObj::IB:
+			glDeleteBuffers(1,&o.glId);
+			break;
+	}
+}
+
+struct Shader : public GLObj
+{
+	Shader( int id=-1 )
+	:
+	GLObj(id, GLObj::SHADER)
+	{
+
+	}
+
+	int refcount=0;
+
+	GLuint glId;
+};
+
+struct ShaderProgram : public GLObj
+{
+	ShaderProgram( int id=-1, int vs=-1, int ps=-1 )
+	:
+	GLObj(id, GLObj::PROGRAM ),
+	ps(ps),
+	vs(vs)
+	{
+
+	}
+
+	int ps=-1;
+	int vs=-1;
+};
+
+typedef std::map<int, Shader> ShaderMap;
+typedef std::map<int, ShaderProgram> ShaderProgramMap;
+
+static ShaderProgramMap shaderProgramMap;
+static ShaderMap shaderMap;
+
+static int curProgram=-1;
+
+static std::string shaderVersion="120";
+
+static void incShaderRefCount(int id)
+{
+	auto it = shaderMap.find(id);
+	if(it != shaderMap.end())
+		it->second.refcount++;
+}
+
+
+void removeShader( int id )
+{
+	auto it = shaderMap.find(id);
+	if(it == shaderMap.end())
+		return;
+	it->second.refcount--;
+	if( it->second.refcount <= 0 )
+	{
+		releaseGLObject(it->second);
+		shaderMap.erase(it);
+	}
+}
+
+void deleteShaderProgram( int id )
+{
+	auto it = shaderProgramMap.find(id);
+	if(it == shaderProgramMap.end())
+		return;
+	releaseGLObject(it->second);
+	removeShader(it->second.vs);
+	removeShader(it->second.ps);
+	shaderProgramMap.erase(it);
+}
+
+void deleteAllShaders()
+{
+	{
+		auto it = shaderMap.begin();
+		while( it != shaderMap.end() )
+		{
+			releaseGLObject(it->second);
+			it++;
+		}
+	}
+
+	{
+		auto it = shaderProgramMap.begin();
+		while( it != shaderProgramMap.end() )
+		{
+			releaseGLObject(it->second);
+			it++;
+		}
+	}
+}
+
+static bool compileShader( GLuint id, const char* shaderStr )
+{
+	char errors[1024];
+
+	glShaderSource(id, 1, (const GLchar**)&shaderStr,NULL);
+
+	GLsizei len;
+	GLint compiled;
+
+	glCompileShader(id);
+	glGetShaderiv(id, GL_COMPILE_STATUS, &compiled);
+
+	if (!compiled)
+	{
+		glGetShaderInfoLog( id, 1024, &len, errors );
+		printf("shader errors: %s\n",errors);
+		return false;
+	}
+
+	return true;
+}
+
+void setShaderVersion( const std::string& version )
+{
+	shaderVersion = version;
+}
+
+static std::string addVersion( const std::string& str )
+{
+	return std::string("\n#version ") + shaderVersion + "\n\n" + str;
+}
+
+int loadVertexShader( const std::string& vs_ )
+{
+	int id = glCreateShader(GL_VERTEX_SHADER);
+	std::string vs = addVersion(vs_);
+	if(!compileShader(id, vs.c_str()))
+	{
+		return -1;
+	}
+
+	shaderMap[id] = Shader(id);
+
+	return id;
+}
+
+int loadPixelShader( const std::string& ps_ )
+{
+	int id = glCreateShader(GL_FRAGMENT_SHADER);
+	std::string ps = addVersion(ps_);
+	if(!compileShader(id, ps.c_str()))
+	{
+		return -1;
+	}
+
+	shaderMap[id] = Shader(id);
+
+	return id;
+}
+
+int linkShader( int vs, int ps )
+{
+	int id = glCreateProgram();
+
+	glAttachShader(id, vs );
+	glAttachShader(id, ps );
+
+
+	// bind attributes
+	/*
+	for( u32 i = 0; i < _vertexAttributes.size() ; i++ )
+	{
+		VertexAttribute * a = _vertexAttributes[i];
+
+		glBindAttribLocation(this->_glId, GENERIC_ATTRIB(a->index), a->name.str);
+
+		if(getGLGfx()->getGLError())
+		{
+			debugPrint("In GLShaderProgram::link");
+			return false;
+		}
+
+		delete _vertexAttributes[i];
+	}
+
+	_vertexAttributes.clear();
+	*/
+
+	glLinkProgram(id);
+
+	GLint linked;
+	char errors[1024];
+	GLsizei len;
+
+	glGetProgramiv(id, GL_LINK_STATUS, &linked);
+	if (!linked)
+	{
+	   glGetProgramInfoLog(id, 1024, &len, errors );
+	   printf("GLSL Shader linker error:%s\n",errors);
+	   assert(0);
+	   return -1;
+	}
+
+	shaderProgramMap[id] = ShaderProgram(id, vs, ps);
+	incShaderRefCount(vs);
+	incShaderRefCount(ps);
+
+	return id;
+}
+
+int loadShader( const std::string& vs, const std::string& ps )
+{
+	int vsid = loadVertexShader(vs);
+	if(vsid < 0)
+		return -1;
+
+	int psid = loadPixelShader(ps);
+	if(psid < 0)
+		return -1;
+
+	return linkShader( vsid, psid );
+}
+
+int reloadShader( int id, const std::string& vs, const std::string& ps )
+{
+	int newid = loadShader(vs, ps);
+	if(newid==-1)
+		return id;
+
+	deleteShaderProgram(id);
+	return newid;
+}
+
+static int getUniformLocation( const std::string& handle )
+{
+	int loc = glGetUniformLocation( curProgram, handle.c_str() );
+	if(loc==-1)
+	{
+		printf("shader error: Could not get uniform %s\n", handle.c_str());
+	}
+	return loc;
+}
+
+bool setTexture( const std::string& handle, int sampler )
+{
+	int id = getUniformLocation(handle);
+	if(id == -1)
+		return false;
+
+	glUniform1i(id, sampler);
+	return true;
+}
+
+void bindShader( int id )
+{
+	glUseProgram(id);
+	curProgram = id;
+}
+
+void unbindShader()
+{
+	glUseProgram(0);
+	curProgram = -1;
+}
+
+bool setInt( const std::string& handle, int v )
+{
+	int id = getUniformLocation(handle);
+	if(id == -1)
+		return false;
+
+	glUniform1i(id, v);
+	return true;
+}
+
+bool setFloat( const std::string& handle, float v )
+{
+	int id = getUniformLocation(handle);
+	if(id == -1)
+		return false;
+	glUniform1f(id, v);
+
+	return true;
+}
+
+bool setVector( const std::string& handle, const arma::vec& v )
+{
+	int id = getUniformLocation(handle);
+	if(id == -1)
+		return false;
+	fmat fv = conv_to<fvec>::from(v);
+	switch(fv.n_rows)
+	{
+		case 2:
+			//glUniform2fv(id, 1, (GLfloat*)fv.memptr());
+			glUniform2f(id, fv[0], fv[1]);//(GLfloat*)fv.memptr());
+			break;
+		case 3:
+			glUniform3fv(id, 1, (GLfloat*)fv.memptr());
+			break;
+		case 4:
+			glUniform4fv(id, 1, (GLfloat*)fv.memptr());
+			break;
+	}
+
+	return true;
+}
+
+bool setMatrix( const std::string& handle, const mat& v )
+{
+	int id = getUniformLocation(handle);
+	if(id == -1)
+		return false;
+
+	fmat fm = conv_to<fmat>::from(v);
+
+	switch(v.n_rows)
+	{
+		case 2:
+			glUniformMatrix2fv(id, 1, GL_FALSE, (GLfloat*)fm.memptr());
+			break;
+		case 3:
+			glUniformMatrix3fv(id, 1, GL_FALSE, (GLfloat*)fm.memptr());
+			break;
+		case 4:
+			glUniformMatrix4fv(id, 1, GL_FALSE, (GLfloat*)fm.memptr());
+			break;
+		default:
+			return false;
+	}
+
+	return true;
+}
+
+/*
+// Hack, limited number of texture samplers, using a larger number will creash
+static int textureSamplers[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+int createTexture(int w, int h, int glFormat, int dataFormat, int dataType )
+{
+	unsigned char * data = new unsigned char[w*h*4*4];
+	int id = createTexture(data, w, h, glFormat, dataFormat, dataType);
+	delete [] data;
+	return id;
+}
+
+int createTexture(void* data, int w, int h, int glFormat, int dataFormat, int dataType )
+{
+	glDisable( GL_TEXTURE_RECTANGLE );
+	glEnable( GL_TEXTURE_2D );
+
+	GLint prevAlignment;
+	glGetIntegerv(GL_UNPACK_ALIGNMENT, &prevAlignment);
+	glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+
+	int id;
+	glGenTextures(1, (GLint*)&id);					// Create 1 Texture
+	glBindTexture(GL_TEXTURE_2D, id);			// Bind The Texture
+
+	// Does not handle mip maps at the moment
+	glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+
+	int hwWidth=width;
+	int hwHeight=height;
+
+	// Check if non power of two
+	if(!checkGlExtension("GL_ARB_texture_non_power_of_two"))
+	{
+		hwWidth = NPOT(width);
+		hwHeight = NPOT(height);
+
+		if(hwWidth != width)
+		{
+			// realign data
+			int hww = info.hwWidth;
+			int hwh = info.hwHeight;
+
+			int sz = glFormatDescs[fmt].bytesPerPixel;
+
+			unsigned char * src = (unsigned char*)data;
+			unsigned char * dst = new unsigned char[ hww*hwh*sz ];
+			data = dst;
+
+			memset( dst, 0, hww*hwh*sz );
+			for( int y = 0; y < h; y++ )
+			{
+				memcpy(&dst[y*hww*sz],&src[y*w*sz],w*sz);
+			}
+		}
+	}
+
+	glTexImage2D(	GL_TEXTURE_2D,
+					 0,
+					 info.glFormat,
+					 info.hwWidth,
+					 info.hwHeight,
+					 0,
+					 info.glDataFormat,
+					 info.glDataType,
+					 data );
+
+	glPixelStorei(GL_UNPACK_ALIGNMENT, prevAlignment);
+	glDisable(GL_TEXTURE_2D);
+
+	if(hwWidth != width)
+	{
+		delete [] (unsigned char*)data;
+	}
+
+	return id;
+}
+
+void bindTexture( int id, int sampler )
+{
+	glActiveTexture( GL_TEXTURE0+sampler );
+	glEnable(GL_TEXTURE_2D);
+	glBindTexture(GL_TEXTURE_2D, id);
+
+	textureSamplers[sampler] = id;
+}
+
+void unbindTexture( int sampler )
+{
+	if(textureSamplers[sampler] > 0)
+	{
+		glActiveTexture( GL_TEXTURE0+sampler);
+		glDisable(GL_TEXTURE_2D);
+		glBindTexture(GL_TEXTURE_2D, 0);
+		textureSamplers[sampler] = 0;
+	}
+}
+
+void grabFrameBuffer( int texId, int w, int h )
+{
+	bindTexture(id);
+	glCopyTexSubImage2D(GL_TEXTURE_2D, //target
+					0, //level
+					0, //xoffset
+					0, //yoffset
+					0, //x
+					0, //y
+					w, //width
+					h //height
+					);
+	unbindTexture();
+}
+*/
+}
+
+arma::mat rot2d( double theta, bool affine )
+{
+	int d = affine?3:2;
+	arma::mat m = arma::eye(d,d);
+
+	double ct = cos(theta);
+	double st = sin(theta);
+
+	m(0,0) = ct; m(0,1) = -st;
+	m(1,0) = st; m(1,1) = ct;
+
+	return m;
+}
+
+arma::mat trans2d( const arma::vec& xy, bool affine )
+{
+	arma::mat m = arma::eye(3,3);
+	m(0,2) = xy[0];
+	m(1,2) = xy[1];
+	return m;
+}
+
+arma::mat trans2d( double x, double y, bool affine )
+{
+	return trans2d( arma::vec({x, y}) );
+}
+
+arma::mat scaling2d( const arma::vec& xy, bool affine )
+{
+	int d = affine?3:2;
+	arma::mat m = arma::eye(d,d);
+
+	m(0,0) = xy[0];
+	m(1,1) = xy[1];
+	return m;
+}
+
+arma::mat scaling2d( double s, bool affine )
+{
+	return scaling2d( arma::vec({s, s}), affine );
+}
+
+arma::mat scaling2d( double x, double y, bool affine )
+{
+	return scaling2d( arma::vec({x, y}), affine );
+}
+
+
+arma::mat rotX3d( double theta, bool affine )
+{
+	int d = affine?4:3;
+	arma::mat m = arma::eye(d,d);
+
+	double ct = cos(theta);
+	double st = sin(theta);
+
+	m(1,1) = ct; m(1,2) = -st;
+	m(2,1) = st; m(2,2) = ct;
+
+	return m;
+}
+
+arma::mat rotY3d( double theta, bool affine )
+{
+	int d = affine?4:3;
+	arma::mat m = arma::eye(d,d);
+
+	double ct = cos(theta);
+	double st = sin(theta);
+
+	m(0,0) = ct; m(0,2) = st;
+	m(2,0) = -st; m(2,2) = ct;
+
+	return m;
+}
+
+arma::mat rotZ3d( double theta, bool affine )
+{
+	int d = affine?4:3;
+	arma::mat m = arma::eye(d,d);
+
+	double ct = cos(theta);
+	double st = sin(theta);
+
+	m(0,0) = ct; m(0,1) = -st;
+	m(1,0) = st; m(1,1) = ct;
+
+	return m;
+}
+
+arma::mat trans3d( const arma::vec& xyz )
+{
+	arma::mat m = arma::eye(4,4);
+	m(0,3) = xyz[0];
+	m(1,3) = xyz[1];
+	m(2,3) = xyz[2];
+	return m;
+}
+
+arma::mat trans3d( double x, double y, double z )
+{
+	return trans3d( arma::vec({x, y, z}) );
+}
+
+arma::mat scaling3d( const arma::vec& xyz, bool affine )
+{
+	int d = affine?4:3;
+	arma::mat m = arma::eye(d,d);
+
+	m(0,0) = xyz[0];
+	m(1,1) = xyz[1];
+	m(2,2) = xyz[2];
+	return m;
+}
+
+arma::mat scaling3d( double s, bool affine )
+{
+	return scaling3d( arma::vec({s, s, s}), affine );
+}
+
+arma::mat scaling3d( double x, double y, double z, bool affine )
+{
+	return scaling3d( arma::vec({x, y, z}), affine );
+}
+
+
+
diff --git a/src/utils/gfx2.cpp b/src/utils/gfx2.cpp
index 28d014e..a229afd 100644
--- a/src/utils/gfx2.cpp
+++ b/src/utils/gfx2.cpp
@@ -1,325 +1,19 @@
 /*
  * gfx3.cpp
  *
- * Rendering utility structures and functions based on OpenGL 3.3+
+ * Rendering utility structures and functions based on OpenGL 2 (no shader)
  *
  * Authors: Philip Abbet
  */
 
 #include <gfx2.h>
 
-namespace gfx2 {
-
-
-/****************************** UTILITY FUNCTIONS ****************************/
-
-void init()
-{
-	glewExperimental = GL_TRUE;
-	glewInit();
-}
-
-//-----------------------------------------------
-
-double deg2rad(double deg)
-{
-	return deg / 360.0 * (2.0 * M_PI);
-}
-
-//-----------------------------------------------
-
-double sin_deg(double deg)
-{
-	return sin(deg2rad(deg));
-}
-
-//-----------------------------------------------
-
-double cos_deg(double deg)
-{
-	return cos(deg2rad(deg));
-}
-
-//-----------------------------------------------
-
-bool is_close(float a, float b, float epsilon)
-{
-	return fabs(a - b) < epsilon;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
-				int fb_width, int fb_height) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) fb_width / (float) win_width;
-	result(1) = ((float) win_height - coords(1)) * (float) fb_height / (float) win_height;
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
-
-	result(1) = ((float) window_size.win_height - coords(1)) *
-				(float) window_size.fb_height / (float) window_size.win_height;
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
-						 int fb_width, int fb_height) {
-	arma::vec result = coords;
-
-	result(0) = (coords(0) - (float) win_width * 0.5f) * (float) fb_width / (float) win_width;
-	result(1) = ((float) win_height * 0.5f - coords(1)) * (float) fb_height / (float) win_height;
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size) {
-	arma::vec result = coords;
+#define GFX_NAMESPACE gfx2
+#include "gfx_common.cpp"
+#undef GFX_NAMESPACE
 
-	result(0) = (coords(0) - (float) window_size.win_width * 0.5f) *
-				(float) window_size.fb_width / (float) window_size.win_width;
-
-	result(1) = ((float) window_size.win_height * 0.5f - coords(1)) *
-				(float) window_size.fb_height / (float) window_size.win_height;
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
-				int fb_width, int fb_height) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) win_width / (float) fb_width;
-	result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height);
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
-	result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
-				(float) window_size.win_height);
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
-						 int fb_width, int fb_height) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) win_width / (float) fb_width + (float) win_width * 0.5f;
-	result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height * 0.5f);
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size) {
-	arma::vec result = coords;
-
-	result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width +
-				(float) window_size.win_width * 0.5f;
-	result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
-				(float) window_size.win_height * 0.5f);
-
-	return result;
-}
-
-
-/************************* PROJECTION & VIEW MATRICES ************************/
-
-arma::fmat perspective(float fovy, float aspect, float zNear, float zFar)
-{
-	const float top = zNear * tan(fovy / 2.0f);
-	const float bottom = -top;
-	const float right = top * aspect;
-	const float left = -right;
-
-	arma::fmat projection = arma::zeros<arma::fmat>(4,4);
-
-	projection(0, 0) = 2.0f * zNear / (right - left);
-	projection(0, 2) = (right + left) / (right - left);
-	projection(1, 1) = 2.0f * zNear / (top - bottom);
-	projection(1, 2) = (top + bottom) / (top - bottom);
-	projection(2, 2) = -(zFar + zNear) / (zFar - zNear);
-	projection(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
-	projection(3, 2) = -1.0f;
-
-	return projection;
-}
-
-//-----------------------------------------------
-
-arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
-				  const arma::fvec& up)
-{
-	const arma::fvec f(arma::normalise(target - position));
-	const arma::fvec s(arma::normalise(arma::cross(f, up)));
-	const arma::fvec u(arma::cross(s, f));
-
-	arma::fmat result = arma::zeros<arma::fmat>(4,4);
-
-	result(0, 0) = s(0);
-	result(0, 1) = s(1);
-	result(0, 2) = s(2);
-	result(1, 0) = u(0);
-	result(1, 1) = u(1);
-	result(1, 2) = u(2);
-	result(2, 0) =-f(0);
-	result(2, 1) =-f(1);
-	result(2, 2) =-f(2);
-	result(0, 3) =-arma::dot(s, position);
-	result(1, 3) =-arma::dot(u, position);
-	result(2, 3) = arma::dot(f, position);
-	result(3, 3) = 1.0f;
-
-	return result;
-}
-
-
-/****************************** TRANSFORMATIONS ******************************/
-
-arma::fmat rotate(const arma::fvec& axis, float angle)
-{
-	float rcos = cos(angle);
-	float rsin = sin(angle);
-
-	arma::fmat matrix = arma::zeros<arma::fmat>(4, 4);
-
-	matrix(0, 0) =			  rcos + axis(0) * axis(0) * (1.0f - rcos);
-	matrix(1, 0) =	axis(2) * rsin + axis(1) * axis(0) * (1.0f - rcos);
-	matrix(2, 0) = -axis(1) * rsin + axis(2) * axis(0) * (1.0f - rcos);
-	matrix(0, 1) = -axis(2) * rsin + axis(0) * axis(1) * (1.0f - rcos);
-	matrix(1, 1) =			  rcos + axis(1) * axis(1) * (1.0f - rcos);
-	matrix(2, 1) =	axis(0) * rsin + axis(2) * axis(1) * (1.0f - rcos);
-	matrix(0, 2) =	axis(1) * rsin + axis(0) * axis(2) * (1.0f - rcos);
-	matrix(1, 2) = -axis(0) * rsin + axis(1) * axis(2) * (1.0f - rcos);
-	matrix(2, 2) =			  rcos + axis(2) * axis(2) * (1.0f - rcos);
-	matrix(3, 3) = 1.0f;
-
-	return matrix;
-}
-
-//-----------------------------------------------
-
-arma::fmat rotation(const arma::fvec& from, const arma::fvec& to)
-{
-	const float dot = arma::dot(from, to);
-	const arma::fvec cross = arma::cross(from, to);
-	const float norm = arma::norm(cross);
-
-	arma::fmat g({
-		{ dot,	-norm, 0.0f },
-		{ norm,	 dot,  0.0f },
-		{ 0.0f,	 0.0f, 1.0f },
-	});
-
-	arma::fmat fi(3, 3);
-	fi.rows(0, 0) = from.t();
-	fi.rows(1, 1) = arma::normalise(to - dot * from).t();
-	fi.rows(2, 2) = arma::cross(to, from).t();
-
-	arma::fmat result = arma::eye<arma::fmat>(4, 4);
-
-	arma::fmat u;
-	if (arma::inv(u, fi))
-	{
-		u = u * g * fi;
-		result.submat(0, 0, 2, 2) = u;
-	}
-
-	return result;
-}
-
-//-----------------------------------------------
-
-void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
-						arma::fmat &rotation, arma::fvec &translation) {
-
-	arma::fvec centroidsA = arma::mean(A, 1);
-	arma::fvec centroidsB = arma::mean(B, 1);
-
-	int n = A.n_cols;
-
-	arma::fmat H = (A - repmat(centroidsA, 1, n)) * (B - repmat(centroidsB, 1, n)).t();
-
-	arma::fmat U, V;
-	arma::fvec s;
-	arma::svd(U, s, V, H);
-
-	rotation = V * U.t();
-
-	if (arma::det(rotation) < 0.0f)
-		rotation.col(2) *= -1.0f;
-
-	translation = -rotation * centroidsA + centroidsB;
-}
-
-//-----------------------------------------------
-
-arma::fmat worldTransforms(const transforms_t* transforms)
-{
-	arma::fmat result = arma::eye<arma::fmat>(4, 4);
-	result(0, 3, arma::size(3, 1)) = worldPosition(transforms);
-
-	result = result * worldRotation(transforms);
-
-	return result;
-}
-
-//-----------------------------------------------
-
-arma::fvec worldPosition(const transforms_t* transforms)
-{
-	if (transforms->parent)
-	{
-		arma::fvec position(4);
-		position.rows(0, 2) = transforms->position;
-		position(3) = 1.0f;
-
-		position = worldRotation(transforms->parent) * position;
-
-		return worldPosition(transforms->parent) + position.rows(0, 2);
-	}
-
-	return transforms->position;
-}
-
-//-----------------------------------------------
-
-arma::fmat worldRotation(const transforms_t* transforms)
-{
-	if (transforms->parent)
-	{
-		arma::fmat result = worldRotation(transforms->parent) * transforms->rotation;
-		return arma::normalise(result);
-	}
-
-	return transforms->rotation;
-}
 
+namespace gfx2 {
 
 /********************************** TEXTURES *********************************/
 
@@ -365,7 +59,7 @@ void destroy(texture_t &texture)
 
 model_t create_rectangle(const arma::fvec& color, float width, float height,
 						 const arma::fvec& position, const arma::fmat& rotation,
-						 transforms_t* parent_transforms)
+						 const transforms_t* parent_transforms)
 {
 	model_t model = { 0 };
 
@@ -421,7 +115,7 @@ model_t create_rectangle(const arma::fvec& color, float width, float height,
 
 model_t create_rectangle(const texture_t& texture, float width, float height,
 						 const arma::fvec& position, const arma::fmat& rotation,
-						 transforms_t* parent_transforms)
+						 const transforms_t* parent_transforms)
 {
 	model_t model = create_rectangle(arma::fvec({1.0f, 1.0f, 1.0f, 1.0f}),
 									 width, height, position, rotation,
@@ -435,7 +129,7 @@ model_t create_rectangle(const texture_t& texture, float width, float height,
 //-----------------------------------------------
 
 model_t create_square(const arma::fvec& color, float size, const arma::fvec& position,
-					  const arma::fmat& rotation, transforms_t* parent_transforms)
+					  const arma::fmat& rotation, const transforms_t* parent_transforms)
 {
 	model_t model = { 0 };
 
@@ -489,7 +183,7 @@ model_t create_square(const arma::fvec& color, float size, const arma::fvec& pos
 //-----------------------------------------------
 
 model_t create_sphere(float radius, const arma::fvec& position,
-					  const arma::fmat& rotation, transforms_t* parent_transforms)
+					  const arma::fmat& rotation, const transforms_t* parent_transforms)
 {
 	model_t model = { 0 };
 
@@ -503,9 +197,9 @@ model_t create_sphere(float radius, const arma::fvec& position,
 	model.transforms.parent = parent_transforms;
 
 	// Material
-	model.ambiant_color = arma::fvec({0.2f, 0.2f, 0.2f});
-	model.diffuse_color = arma::fvec({0.8f, 0.8f, 0.8f});
-	model.specular_color = arma::fvec({0.0f, 0.0f, 0.0f});
+	model.ambiant_color = arma::fvec({0.2f, 0.2f, 0.2f, 1.0f});
+	model.diffuse_color = arma::fvec({0.8f, 0.8f, 0.8f, 1.0f});
+	model.specular_color = arma::fvec({0.0f, 0.0f, 0.0f, 1.0f});
 	model.specular_power = 5;
 
 	// Create the mesh
@@ -580,7 +274,7 @@ model_t create_sphere(float radius, const arma::fvec& position,
 
 model_t create_line(const arma::fvec& color, const arma::mat& points,
 					const arma::fvec& position, const arma::fmat& rotation,
-					transforms_t* parent_transforms, bool line_strip)
+					const transforms_t* parent_transforms, bool line_strip)
 {
 	model_t model = { 0 };
 
@@ -622,7 +316,7 @@ model_t create_line(const arma::fvec& color, const arma::mat& points,
 
 model_t create_line(const arma::fvec& color, const std::vector<arma::vec>& points,
 					const arma::fvec& position, const arma::fmat& rotation,
-					transforms_t* parent_transforms, bool line_strip)
+					const transforms_t* parent_transforms, bool line_strip)
 {
 	arma::mat points_mat(points[0].n_rows, points.size());
 
@@ -636,7 +330,7 @@ model_t create_line(const arma::fvec& color, const std::vector<arma::vec>& point
 
 model_t create_mesh(const arma::fvec& color, const arma::mat& vertices,
 					const arma::fvec& position, const arma::fmat& rotation,
-					transforms_t* parent_transforms)
+					const transforms_t* parent_transforms)
 {
 	model_t model = { 0 };
 
@@ -679,7 +373,7 @@ model_t create_mesh(const arma::fvec& color, const arma::mat& vertices,
 model_t create_gaussian_background(const arma::fvec& color, const arma::vec& mu,
 								   const arma::mat& sigma,
 								   const arma::fvec& position, const arma::fmat& rotation,
-								   transforms_t* parent_transforms)
+								   const transforms_t* parent_transforms)
 {
 	arma::mat vertices = get_gaussian_background_vertices(mu, sigma, 60);
 
@@ -695,7 +389,7 @@ model_t create_gaussian_background(const arma::fvec& color, const arma::vec& mu,
 model_t create_gaussian_border(const arma::fvec& color, const arma::vec& mu,
 							   const arma::mat& sigma,
 							   const arma::fvec& position, const arma::fmat& rotation,
-							   transforms_t* parent_transforms)
+							   const transforms_t* parent_transforms)
 {
 	arma::mat pts = get_gaussian_border_vertices(mu, sigma, 60, true);
 
@@ -749,7 +443,9 @@ bool draw(const model_t& model, const light_list_t& lights)
 
 		glEnable(GL_LIGHT0);
 		glLightfv(GL_LIGHT0, GL_POSITION, lights[0].transforms.position.memptr());
-		glLightfv(GL_LIGHT0, GL_DIFFUSE, lights[0].color.memptr());
+		glLightfv(GL_LIGHT0, GL_AMBIENT, lights[0].ambient_color.memptr());
+		glLightfv(GL_LIGHT0, GL_DIFFUSE, lights[0].diffuse_color.memptr());
+		glLightfv(GL_LIGHT0, GL_SPECULAR, lights[0].specular_color.memptr());
 	}
 
 	// Set vertex data
@@ -840,6 +536,8 @@ bool draw(const model_t& model, const light_list_t& lights)
 	glDisableClientState(GL_NORMAL_ARRAY);
 	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
 
+	glDisable(GL_LIGHTING);
+
 	if (model.texture.width > 0)
 		glDisable(GL_TEXTURE_2D);
 
@@ -966,142 +664,48 @@ bool draw_gaussian(const arma::fvec& color, const arma::vec& mu, const arma::mat
 	return result;
 }
 
-
-/******************************** RAY CASTING ********************************/
-
-ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
-				 const arma::fmat& view, const arma::fmat& projection,
-				 int window_width, int window_height)
-{
-	ray_t ray;
-
-	ray.origin = origin;
-
-	// Compute the ray in homogeneous clip coordinates (range [-1:1, -1:1, -1:1, -1:1])
-	arma::fvec ray_clip(4);
-	ray_clip(0) = (2.0f * mouse_x) / window_width - 1.0f;
-	ray_clip(1) = 1.0f - (2.0f * mouse_y) / window_height;
-	ray_clip(2) = -1.0f;
-	ray_clip(3) = 1.0f;
-
-	// Compute the ray in camera coordinates
-	arma::fvec ray_eye = arma::inv(projection) * ray_clip;
-	ray_eye(2) = -1.0f;
-	ray_eye(3) = 0.0f;
-
-	// Compute the ray in world coordinates
-	arma::fvec ray_world = arma::inv(view) * ray_eye;
-	ray.direction = arma::fvec(arma::normalise(ray_world)).rows(0, 2);
-
-	return ray;
-}
-
 //-----------------------------------------------
 
-bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
-				arma::fvec &result)
-{
-	arma::fvec O_C = ray.origin - center;
-	float b = arma::dot(ray.direction, O_C);
-	float c = arma::dot(O_C, O_C) - radius * radius;
-
-	float det = b * b - c;
-
-	if (det < 0.0f)
-		return false;
-
-	float t;
-
-	if (det > 0.0f)
-	{
-		float t1 = -b + sqrtf(det);
-		float t2 = -b - sqrtf(det);
-
-		t = (t1 < t2 ? t1 : t2);
-	}
-	else
-	{
-		t = -b + sqrtf(det);
-	}
-
-	result = ray.origin + ray.direction * t;
-
-	return true;
-}
-
-
-/********************************** OTHERS ***********************************/
-
-arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
-									 	   int nb_points)
+bool draw_gaussian_3D(const arma::fvec& color, const arma::vec& mu,
+					  const arma::mat& sigma)
 {
-	arma::mat pts = get_gaussian_border_vertices(mu, sigma, nb_points, true);
+	gfx2::model_t sphere = gfx2::create_sphere();
+	sphere.lightning_enabled = false;
+	sphere.use_one_minus_src_alpha_blending = true;
 
-	arma::mat vertices(2, nb_points * 3);
+	sphere.diffuse_color(3) = 0.1f;
+	sphere.specular_power = 0.0f;
 
-	// We need to ensure that the vertices will be in a counter-clockwise order
-	arma::vec v1 = pts(arma::span::all, 0) - mu(arma::span(0, 1));
-	arma::vec v2 = pts(arma::span::all, 1) - mu(arma::span(0, 1));
+	sphere.ambiant_color(arma::span(0, 2)) = color;
+	sphere.diffuse_color(arma::span(0, 2)) = color;
 
-	if (atan2(v1(1), v1(0)) - atan2(v2(1), v2(0)) > 0.0) {
-		for (int i = 0; i < nb_points - 1; ++i)
-		{
-			vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
-			vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
-			vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
-		}
+	arma::mat V;
+	arma::vec d;
+	arma::eig_sym(d, V, sigma);
+	arma::mat VD = V * arma::diagmat(sqrt(d));
 
-		vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
-		vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, 0);
-		vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, nb_points - 1);
-	} else {
-		for (int i = 0; i < nb_points - 1; ++i)
-		{
-			vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
-			vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i);
-			vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i + 1);
-		}
-
-		vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
-		vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, nb_points - 1);
-		vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, 0);
-	}
-
-	return vertices;
-}
+	sphere.transforms.position = arma::conv_to<arma::fmat>::from(mu);
+	arma::fmat rot_scale = arma::conv_to<arma::fmat>::from(VD);
 
-//-----------------------------------------------
-
-arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
-								 	   int nb_points, bool line_strip)
-{
-	arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points)),
-									 arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points))
-	);
+	bool result = false;
 
-	arma::vec eigval(2);
-	arma::mat eigvec(2, 2);
-	eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+	for (int j = 1; j <= 20; ++j) {
+		glClear(GL_DEPTH_BUFFER_BIT);
 
-	arma::mat R = eigvec * diagmat(sqrt(eigval));
+		arma::fmat scaling = arma::eye<arma::fmat>(3, 3) * (float) j / 20.0f;
 
-	arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, nb_points);
+		arma::fmat scaled_transforms = arma::eye<arma::fmat>(4, 4);
 
-	if (line_strip)
-		return pts;
+		scaled_transforms(arma::span(0, 2), arma::span(0, 2)) = scaling * rot_scale;
 
-	arma::mat vertices(2, nb_points * 2);
+		sphere.transforms.rotation = scaled_transforms;
 
-	for (int i = 0; i < nb_points - 1; ++i)
-	{
-		vertices(arma::span::all, i * 2) = pts(arma::span::all, i);;
-		vertices(arma::span::all, i * 2 + 1) = pts(arma::span::all, i + 1);
+		result &= gfx2::draw(sphere);
 	}
 
-	vertices(arma::span::all, (nb_points - 1) * 2) = pts(arma::span::all, 0);;
-	vertices(arma::span::all, (nb_points - 1) * 2 + 1) = pts(arma::span::all, nb_points - 1);
+	gfx2::destroy(sphere);
 
-	return vertices;
+	return result;
 }
 
 }
diff --git a/src/utils/gfx3.cpp b/src/utils/gfx3.cpp
new file mode 100644
index 0000000..d068c78
--- /dev/null
+++ b/src/utils/gfx3.cpp
@@ -0,0 +1,2036 @@
+/*
+ * gfx3.cpp
+ *
+ * Rendering utility structures and functions based on OpenGL 3.3+
+ *
+ * Authors: Philip Abbet
+ */
+
+#include <gfx3.h>
+
+#define GFX_NAMESPACE gfx3
+#include "gfx_common.cpp"
+#undef GFX_NAMESPACE
+
+
+namespace gfx3 {
+
+/****************************** UTILITY FUNCTIONS ****************************/
+
+arma::vec ui2shader(const arma::vec& coords, int win_width, int win_height,
+					int fb_width, int fb_height, float sh_left, float sh_top,
+					float sh_right, float sh_bottom) {
+	arma::vec result = ui2fb_centered(coords, win_width, win_height, fb_width, fb_height);
+
+	result(0) = result(0) * (sh_right - sh_left) / (float) fb_width + (1.0f + sh_left);
+	result(1) = result(1) * (sh_top - sh_bottom) / (float) fb_height + (1.0f + sh_bottom);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2shader(const arma::vec& coords, const window_size_t& window_size,
+					float sh_left, float sh_top, float sh_right, float sh_bottom) {
+	arma::vec result = ui2fb_centered(coords, window_size);
+
+	result(0) = result(0) * (sh_right - sh_left) / (float) window_size.fb_width + (1.0f + sh_left);
+	result(1) = result(1) * (sh_top - sh_bottom) / (float) window_size.fb_height + (1.0f + sh_bottom);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2shader_centered(const arma::vec& coords, const window_size_t& window_size,
+							 float sh_left, float sh_top, float sh_right, float sh_bottom) {
+	arma::vec result(2);
+
+	result(0) = coords(0) * (sh_right - sh_left) / (float) window_size.fb_width + (1.0f + sh_left);
+	result(1) = coords(1) * (sh_top - sh_bottom) / (float) window_size.fb_height + (1.0f + sh_bottom);
+
+	return result;
+}
+
+
+/********************************** SHADERS **********************************/
+
+void shader_t::setUniform(const std::string& name, const arma::fmat& value)
+{
+	auto iter = fmat_uniforms.find(name);
+
+	if (iter == fmat_uniforms.end())
+	{
+		shader_fmat_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = value;
+		fmat_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = value;
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, const arma::mat& value)
+{
+	auto iter = fmat_uniforms.find(name);
+
+	if (iter == fmat_uniforms.end())
+	{
+		shader_fmat_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = arma::conv_to<arma::fmat>::from(value);
+		fmat_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = arma::conv_to<arma::fmat>::from(value);
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, const arma::fvec& value)
+{
+	auto iter = fvec_uniforms.find(name);
+
+	if (iter == fvec_uniforms.end())
+	{
+		shader_fvec_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = value;
+		fvec_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = value;
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, const arma::vec& value)
+{
+	auto iter = fvec_uniforms.find(name);
+
+	if (iter == fvec_uniforms.end())
+	{
+		shader_fvec_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = arma::conv_to<arma::fvec>::from(value);
+		fvec_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = arma::conv_to<arma::fvec>::from(value);
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, float value)
+{
+	auto iter = float_uniforms.find(name);
+
+	if (iter == float_uniforms.end())
+	{
+		shader_float_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = value;
+		float_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = value;
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, bool value)
+{
+	auto iter = bool_uniforms.find(name);
+
+	if (iter == bool_uniforms.end())
+	{
+		shader_bool_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = value;
+		bool_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = value;
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setUniform(const std::string& name, int value)
+{
+	auto iter = int_uniforms.find(name);
+
+	if (iter == int_uniforms.end())
+	{
+		shader_int_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.value = value;
+		int_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.value = value;
+	}
+}
+
+//-----------------------------------------------
+
+void shader_t::setTexture(const std::string& name, GLuint texture, int nb_dimensions)
+{
+	auto iter = texture_uniforms.find(name);
+
+	if (iter == texture_uniforms.end())
+	{
+		shader_texture_uniform_t entry;
+		entry.handle = glGetUniformLocation(this->id, name.c_str());
+		if (entry.handle == -1)
+			return;
+
+		entry.texture = texture;
+		entry.nb_dimensions = nb_dimensions;
+		texture_uniforms[name] = entry;
+	}
+	else
+	{
+		iter->second.texture = texture;
+	}
+}
+
+//-----------------------------------------------
+
+GLuint compileShader(GLenum shader_type, const char* shader_source)
+{
+	GLuint id = glCreateShader(shader_type);
+	GLint compiled;
+
+	glShaderSource(id, 1, (const GLchar**) &shader_source, NULL);
+	glCompileShader(id);
+	glGetShaderiv(id, GL_COMPILE_STATUS, &compiled);
+
+	if (!compiled)
+	{
+		char errors[1024];
+		GLsizei len;
+
+		glGetShaderInfoLog(id, 1024, &len, errors);
+		printf("shader errors: %s\n", errors);
+		return 0;
+	}
+
+	return id;
+}
+
+//-----------------------------------------------
+
+GLuint linkShader(GLuint vertex_shader, GLuint fragment_shader)
+{
+	GLuint id = glCreateProgram();
+
+	glAttachShader(id, vertex_shader);
+	glAttachShader(id, fragment_shader);
+	glLinkProgram(id);
+
+	GLint linked;
+
+	glGetProgramiv(id, GL_LINK_STATUS, &linked);
+	if (!linked)
+	{
+		char errors[1024];
+		GLsizei len;
+
+		glGetProgramInfoLog(id, 1024, &len, errors);
+		printf("GLSL Shader linker error:%s\n",errors);
+		return 0;
+	}
+
+	return id;
+}
+
+//-----------------------------------------------
+
+shader_t loadShader(const std::string& vertex_shader,
+					const std::string& fragment_shader,
+					const std::string& version)
+{
+	shader_t shader = { 0 };
+	shader.id = 0;
+
+	// Compile the vertex shader
+	GLuint vs_id = compileShader(
+		GL_VERTEX_SHADER, (std::string("#version ") + version + "\n\n" + vertex_shader).c_str());
+
+	if (vs_id == 0)
+		return shader;
+
+	// Compile the fragment shader
+	GLuint fg_id = compileShader(
+		GL_FRAGMENT_SHADER, (std::string("#version ") + version + "\n\n" + fragment_shader).c_str());
+
+	if (fg_id == 0)
+		return shader;
+
+	// Link the shaders into a GLSL program
+	shader.id = linkShader(vs_id, fg_id);
+
+	// Transformation matrices-related uniforms
+	shader.model_matrix_handle		= glGetUniformLocation(shader.id, "ModelMatrix");
+	shader.view_matrix_handle		= glGetUniformLocation(shader.id, "ViewMatrix");
+	shader.projection_matrix_handle = glGetUniformLocation(shader.id, "ProjectionMatrix");
+
+	// Material-related uniforms
+	shader.ambiant_color_handle		= glGetUniformLocation(shader.id, "AmbiantColor");
+	shader.diffuse_color_handle		= glGetUniformLocation(shader.id, "DiffuseColor");
+	shader.specular_color_handle	= glGetUniformLocation(shader.id, "SpecularColor");
+	shader.specular_power_handle	= glGetUniformLocation(shader.id, "SpecularPower");
+
+	// Texture-related uniforms
+	shader.diffuse_texture_handle	= glGetUniformLocation(shader.id, "DiffuseTexture");
+
+	// Light-related uniforms
+	shader.light_position_handle	= glGetUniformLocation(shader.id, "LightPosition");
+	shader.light_color_handle		= glGetUniformLocation(shader.id, "LightColor");
+	shader.light_power_handle		= glGetUniformLocation(shader.id, "LightPower");
+
+	// Determine if lightning is used by the shaders
+	shader.use_lightning = (shader.light_position_handle != -1);
+
+	// Backbuffer
+	shader.backbuffer_handle = glGetUniformLocation(shader.id, "BackBuffer");
+
+	return shader;
+}
+
+//-----------------------------------------------
+
+void sendApplicationUniforms(const shader_t* shader)
+{
+	// Send the application-specific uniforms to the shader
+	for (auto iter = shader->fmat_uniforms.begin(), iterEnd = shader->fmat_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		if (iter->second.value.n_rows == 4)
+		{
+			if (iter->second.value.n_cols == 4)
+				glUniformMatrix4fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+			else if (iter->second.value.n_cols == 3)
+				glUniformMatrix2x3fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+			else if (iter->second.value.n_cols == 2)
+				glUniformMatrix2x4fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+		}
+		else if (iter->second.value.n_rows == 2)
+		{
+			if (iter->second.value.n_cols == 2)
+				glUniformMatrix2fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+			else if (iter->second.value.n_cols == 3)
+				glUniformMatrix3x2fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+			else if (iter->second.value.n_cols == 4)
+				glUniformMatrix4x2fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+		}
+		else if ((iter->second.value.n_rows == 3) && (iter->second.value.n_cols == 3)) {
+			glUniformMatrix3fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
+		}
+	}
+
+	for (auto iter = shader->fvec_uniforms.begin(), iterEnd = shader->fvec_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		if (iter->second.value.n_rows == 4)
+			glUniform4fv(iter->second.handle, 1, iter->second.value.memptr());
+		else if (iter->second.value.n_rows == 3)
+			glUniform3fv(iter->second.handle, 1, iter->second.value.memptr());
+		else if (iter->second.value.n_rows == 2)
+			glUniform2fv(iter->second.handle, 1, iter->second.value.memptr());
+	}
+
+	for (auto iter = shader->float_uniforms.begin(), iterEnd = shader->float_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		glUniform1f(iter->second.handle, iter->second.value);
+	}
+
+	for (auto iter = shader->bool_uniforms.begin(), iterEnd = shader->bool_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		glUniform1i(iter->second.handle, iter->second.value);
+	}
+
+	for (auto iter = shader->int_uniforms.begin(), iterEnd = shader->int_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		glUniform1i(iter->second.handle, iter->second.value);
+	}
+}
+
+
+//-----------------------------------------------
+
+void sendTextures(const shader_t* shader, GLuint next_texture = GL_TEXTURE0)
+{
+	if (shader->backbuffer_handle >= 0)
+		++next_texture;
+
+	for (auto iter = shader->texture_uniforms.begin(), iterEnd = shader->texture_uniforms.end();
+		 iter != iterEnd; ++iter)
+	{
+		glActiveTexture(next_texture);
+
+		if (iter->second.nb_dimensions == 2)
+			glBindTexture(GL_TEXTURE_2D, iter->second.texture);
+		else
+			glBindTexture(GL_TEXTURE_3D, iter->second.texture);
+
+		glUniform1i(iter->second.handle, next_texture - GL_TEXTURE0);
+		++next_texture;
+	}
+}
+
+//-----------------------------------------------
+
+const char* VERTEX_SHADER_ONE_LIGHT = STRINGIFY(
+	// Input vertex data
+	layout(location = 0) in vec3 vertex_position;
+	layout(location = 1) in vec3 normal;
+
+	// Values that stay constant for the whole mesh
+	uniform mat4 ModelMatrix;
+	uniform mat4 ViewMatrix;
+	uniform mat4 ProjectionMatrix;
+	uniform vec3 LightPosition;
+
+	// Output data ; will be interpolated for each fragment
+	out vec3 position_worldspace;
+	out vec3 eye_direction_cameraspace;
+	out vec3 light_direction_cameraspace;
+	out vec3 normal_cameraspace;
+
+	void main() {
+		// Position of the vertex, in clip space
+		gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
+
+		// Position of the vertex, in worldspace
+		position_worldspace = (ModelMatrix * vec4(vertex_position, 1)).xyz;
+
+		// Vector that goes from the vertex to the camera, in camera space.
+		// In camera space, the camera is at the origin (0,0,0).
+		vec3 vertex_position_cameraspace = (ViewMatrix * vec4(position_worldspace, 1)).xyz;
+		eye_direction_cameraspace = vec3(0,0,0) - vertex_position_cameraspace;
+
+		// Vector that goes from the vertex to the light, in camera space
+		vec3 light_position_cameraspace = (ViewMatrix * vec4(LightPosition, 1)).xyz;
+		light_direction_cameraspace = light_position_cameraspace + eye_direction_cameraspace;
+
+		// Normal of the vertex, in camera space (Only correct if ModelMatrix does
+		// not scale the model)
+		normal_cameraspace = (ViewMatrix * ModelMatrix * vec4(normal, 0)).xyz;
+	}
+);
+
+//-----------------------------------------------
+
+const char* FRAGMENT_SHADER_ONE_LIGHT = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec3 AmbiantColor;
+	uniform vec4 DiffuseColor;
+	uniform vec3 SpecularColor;
+	uniform float SpecularPower;
+	uniform vec3 LightPosition;
+	uniform vec3 LightColor;
+	uniform float LightPower;
+
+	// Interpolated values from the vertex shaders
+	in vec3 position_worldspace;
+	in vec3 eye_direction_cameraspace;
+	in vec3 light_direction_cameraspace;
+	in vec3 normal_cameraspace;
+
+	// Output data
+	out vec4 color;
+
+	void main() {
+		// Normal of the computed fragment, in camera space
+		vec3 n = normalize(normal_cameraspace);
+
+		// Direction of the light (from the fragment to the light)
+		vec3 l = normalize(light_direction_cameraspace);
+
+		// Eye vector (towards the camera)
+		vec3 E = normalize(eye_direction_cameraspace);
+
+		// Direction in which the triangle reflects the light
+		vec3 R = reflect(-l, n);
+
+		// Cosine of the angle between the normal and the light direction,
+		// clamped above 0
+		//	- light is at the vertical of the triangle -> 1
+		//	- light is perpendicular to the triangle -> 0
+		//	- light is behind the triangle -> 0
+		float cos_theta = clamp(dot(n, l), 0, 1);
+
+		// Cosine of the angle between the Eye vector and the Reflect vector,
+		// clamped to 0
+		//	- Looking into the reflection -> 1
+		//	- Looking elsewhere -> < 1
+		float cos_alpha = clamp(dot(E, R), 0, 1);
+
+		// Distance to the light
+		float distance = length(LightPosition - position_worldspace);
+
+		// Computation of the color of the fragment
+		color.rgb = AmbiantColor +
+					DiffuseColor.rgb * LightColor * LightPower * cos_theta / (distance * distance) +
+					SpecularColor * LightColor * LightPower * pow(cos_alpha, SpecularPower) / (distance * distance);
+
+		color.a = DiffuseColor.a;
+	}
+);
+
+//-----------------------------------------------
+
+const char* VERTEX_SHADER_COLORED = STRINGIFY(
+	// Input vertex data
+	layout(location = 0) in vec3 vertex_position;
+
+	// Values that stay constant for the whole mesh
+	uniform mat4 ModelMatrix;
+	uniform mat4 ViewMatrix;
+	uniform mat4 ProjectionMatrix;
+
+	void main() {
+		// Position of the vertex, in clip space
+		gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
+	}
+);
+
+//-----------------------------------------------
+
+const char* FRAGMENT_SHADER_COLORED = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec4 DiffuseColor;
+
+	// Output data
+	out vec4 color;
+
+	void main() {
+		color = DiffuseColor;
+	}
+);
+
+//-----------------------------------------------
+
+const char* VERTEX_SHADER_TEXTURED = STRINGIFY(
+	// Input vertex data
+	layout(location = 0) in vec3 vertex_position;
+	layout(location = 2) in vec2 vertex_UV;
+
+	// Values that stay constant for the whole mesh
+	uniform mat4 ModelMatrix;
+	uniform mat4 ViewMatrix;
+	uniform mat4 ProjectionMatrix;
+
+	// Output data ; will be interpolated for each fragment
+	out vec2 UV;
+
+	void main() {
+		// Position of the vertex, in clip space
+		gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
+
+		UV = vertex_UV;
+	}
+);
+
+//-----------------------------------------------
+
+const char* FRAGMENT_SHADER_ONE_TEXTURE = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform sampler2D DiffuseTexture;
+
+	// Input data
+	in vec2 UV;
+
+	// Output data
+	out vec3 color;
+
+	void main() {
+		color = texture(DiffuseTexture, UV).rgb;
+	}
+);
+
+//-----------------------------------------------
+
+char const* FRAGMENT_SHADER_GAUSSIAN = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec2 Mu;
+	uniform mat2 InvSigma;
+	uniform vec4 GaussianColor;
+
+	// Input data
+	in vec2 UV;
+
+	// Output data
+	out vec4 color;
+
+	void main() {
+		vec2 e = UV - Mu;
+
+		float p = clamp(exp(-(InvSigma[0][0] * e.x * e.x + 2 * InvSigma[0][1] * e.x * e.y +
+							  InvSigma[1][1] * e.y * e.y)), 0, 0.5f);
+
+		color = vec4(GaussianColor.x, GaussianColor.y, GaussianColor.z,
+					 clamp(GaussianColor.a * 2 * p, 0.0, 1.0));
+	}
+);
+
+//-----------------------------------------------
+
+const char* VERTEX_SHADER_3D_GAUSSIAN = STRINGIFY(
+	// Input vertex data
+	layout(location = 0) in vec3 vertex_position;
+
+	// Values that stay constant for the whole mesh
+	uniform mat4 ModelMatrix;
+	uniform mat4 ViewMatrix;
+	uniform mat4 ProjectionMatrix;
+
+	// Output data ; will be interpolated for each fragment
+	out vec3 vertex_position_cameraspace;
+
+	void main() {
+		// Position of the vertex, in clip space
+		gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
+
+		// Position of the vertex, in worldspace
+		vec3 position_worldspace = (ModelMatrix * vec4(vertex_position, 1)).xyz;
+
+		// Vector that goes from the vertex to the camera, in camera space.
+		// In camera space, the camera is at the origin (0,0,0).
+		vertex_position_cameraspace = (ViewMatrix * vec4(position_worldspace, 1)).xyz;
+	}
+);
+
+//-----------------------------------------------
+
+char const* FRAGMENT_SHADER_3D_GAUSSIAN = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec3 Mu;
+	uniform mat3 InvSigma;
+	uniform vec4 DiffuseColor;
+
+	// Interpolated values from the vertex shaders
+	in vec3 vertex_position_cameraspace;
+
+	// Output data
+	out vec4 color;
+
+	void main() {
+		vec3 eye_dir = normalize(vertex_position_cameraspace);
+
+		vec3 dir_step = eye_dir * 0.01;
+
+		vec3 position = vertex_position_cameraspace;
+
+		color.a = 0.0f;
+
+		for (int i = 0; i < 200; ++i) {
+			vec3 e = position - Mu;
+
+			float alpha = clamp(exp(-(InvSigma[0][0] * e.x * e.x + 2 * InvSigma[0][1] * e.x * e.y +
+									  InvSigma[1][1] * e.y * e.y + 2 * InvSigma[0][2] * e.x * e.z +
+									  InvSigma[2][2] * e.z * e.z + 2 * InvSigma[1][2] * e.y * e.z)),
+								0, 1.0f
+			);
+
+			if (alpha > color.a)
+				color.a = alpha;
+
+			// Stop when the alpha becomes significantly smaller than the maximum
+			// value seen so far
+			else if (alpha < color.a * 0.9f)
+				break;
+
+			// Stop when the alpha becomes very large
+			if (color.a >= 0.999f)
+				break;
+
+			position = position + dir_step;
+		}
+
+		color.rgb = DiffuseColor.rgb;
+		color.a = color.a * DiffuseColor.a;
+	}
+);
+
+//-----------------------------------------------
+
+const char* RTT_VERTEX_SHADER = STRINGIFY(
+	// Input vertex data
+	layout(location = 0) in vec3 vertex_position;
+
+	// Output data ; will be interpolated for each fragment
+	out vec2 coords;
+
+	void main() {
+		gl_Position = vec4(vertex_position, 1);
+		coords = vertex_position.xy;
+	}
+);
+
+//-----------------------------------------------
+
+char const* RTT_FRAGMENT_SHADER_GAUSSIAN = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec2 Scale;
+	uniform vec2 Mu;
+	uniform mat2 InvSigma;
+	uniform vec3 GaussianColor;
+	uniform vec3 BackgroundColor;
+
+	// Input data
+	in vec2 coords;
+
+	// Output data
+	layout(location = 0) out vec3 color;
+
+	void main() {
+		vec2 e = coords * Scale - Mu;
+		float a = clamp(exp(-(InvSigma[0][0] * e.x * e.x + 2 * InvSigma[0][1] * e.x * e.y +
+							  InvSigma[1][1] * e.y * e.y)), 0, 1.0f);
+
+		color = GaussianColor * a + (1.0f - a) * BackgroundColor;
+	}
+);
+
+//-----------------------------------------------
+
+char const* RTT_FRAGMENT_SHADER_TRANSPARENT_GAUSSIAN = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec2 Scale;
+	uniform vec2 Mu;
+	uniform mat2 InvSigma;
+	uniform vec3 GaussianColor;
+
+	// Input data
+	in vec2 coords;
+
+	// Output data
+	layout(location = 0) out vec4 color;
+
+	void main() {
+		vec2 e = coords * Scale - Mu;
+		float a = clamp(exp(-(InvSigma[0][0] * e.x * e.x + 2 * InvSigma[0][1] * e.x * e.y +
+							  InvSigma[1][1] * e.y * e.y)), 0, 1.0f);
+
+		color.rgb = GaussianColor;
+		color.a = a;
+	}
+);
+
+//-----------------------------------------------
+
+const char* RTT_FRAGMENT_SHADER_FLOW_FIELD = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec2 Target;
+	uniform mat2 InvSigma;
+
+	// Input data
+	in vec2 coords;
+
+	// Output data
+	layout(location = 0) out vec2 color;
+
+	void main() {
+		vec2 dTarget = -InvSigma * (coords - Target);
+		dTarget = dTarget / length(dTarget);
+
+		color = dTarget;
+	}
+);
+
+//-----------------------------------------------
+
+const char* RTT_FRAGMENT_SHADER_LIC = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform vec2 Resolution;
+	uniform float Time;
+	uniform sampler2D FlowField;
+	uniform sampler2D BackBuffer;
+
+	// Input data
+	in vec2 coords;
+
+	// Output data
+	layout(location = 0) out vec3 color;
+
+	// Constants
+	const int Length = 10;
+	const int nbPass = 5;
+
+	void main() {
+		vec2 uvUnit = 1.0 / Resolution.xy;
+		vec2 uv = (coords.xy + 1.0) * 0.5;
+
+		color = vec3(0);
+
+		// Random noise
+		color += vec3(fract(sin(dot(uv.xy, vec2(12.9898, 78.233) * sin(Time))) * 43758.5453));
+
+		for (int n = 0; n < nbPass; n++) {
+			for (int i = 0; i < Length; i++) {
+				color += texture(BackBuffer, uv - texture(FlowField, uv).rg * uvUnit * float(i + 4)).rgb;
+			}
+		}
+		color /= float(Length) * float(nbPass) + 1.0;
+	}
+);
+
+//-----------------------------------------------
+
+const char* RTT_FRAGMENT_SHADER_LIC_COLORED_MEAN = STRINGIFY(
+	// Values that stay constant for the whole mesh
+	uniform sampler2D FlowField;
+	uniform sampler2D Image1;
+	uniform sampler2D Image2;
+	uniform sampler2D Image3;
+	uniform sampler2D Image4;
+	uniform sampler2D Image5;
+
+	// Input data
+	in vec2 coords;
+
+	// Output data
+	layout(location = 0) out vec3 color;
+
+	void main() {
+		const float pi = 3.1415926538;
+
+		vec2 uv = (coords.xy + 1.0) * 0.5;
+
+		color = (texture(Image1, uv).rgb + texture(Image2, uv).rgb +
+				 texture(Image3, uv).rgb + texture(Image4, uv).rgb +
+				 texture(Image5, uv).rgb) / 5.0;
+
+		color = clamp((color - 0.49) * 20.0 + 0.49, 0, 1);
+
+		vec2 dir = texture(FlowField, uv).rg;
+		float angle = atan(dir.y, dir.x);
+
+		const float inc = 2 * pi / 6;
+
+		vec3 start;
+		vec3 end;
+
+		if (angle < -pi + inc) {
+			start = vec3(0.9, 0.1, 0.1);
+			end = vec3(0.9, 0.9, 0.1);
+			angle = (angle + pi) / inc;
+		}
+		else if (angle < -pi + 2 * inc) {
+			start = vec3(0.9, 0.9, 0.1);
+			end = vec3(0.1, 0.9, 0.1);
+			angle = (angle + pi - inc) / inc;
+		}
+		else if (angle < -pi + 3 * inc) {
+			start = vec3(0.1, 0.9, 0.1);
+			end = vec3(0.1, 0.9, 0.9);
+			angle = (angle + pi - 2 * inc) / inc;
+		}
+		else if (angle < -pi + 4 * inc) {
+			start = vec3(0.1, 0.9, 0.9);
+			end = vec3(0.1, 0.1, 0.9);
+			angle = (angle + pi - 3 * inc) / inc;
+		}
+		else if (angle < -pi + 5 * inc) {
+			start = vec3(0.1, 0.1, 0.9);
+			end = vec3(0.9, 0.1, 0.9);
+			angle = (angle + pi - 4 * inc) / inc;
+		}
+		else {
+			start = vec3(0.9, 0.1, 0.9);
+			end = vec3(0.9, 0.1, 0.1);
+			angle = (angle + pi - 5 * inc) / inc;
+		}
+
+		color.r += mix(start.r, end.r, angle) * 0.4;
+		color.g += mix(start.g, end.g, angle) * 0.4;
+		color.b += mix(start.b, end.b, angle) * 0.4;
+	}
+);
+
+
+/********************************** TEXTURE **********************************/
+
+texture_t create_texture(unsigned int width, unsigned int height,
+						 unsigned int nb_channels, const unsigned char* pixels)
+{
+	texture_t texture = { 0 };
+	texture.width = width;
+	texture.height = height;
+	texture.depth = 1;
+
+	glActiveTexture(GL_TEXTURE0);
+	glGenTextures(1, &texture.id);
+	glBindTexture(GL_TEXTURE_2D, texture.id);
+
+	if (nb_channels == 4)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels);
+	else if (nb_channels == 3)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, pixels);
+	else if (nb_channels == 2)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, width, height, 0, GL_RG, GL_UNSIGNED_BYTE, pixels);
+	else
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, width, height, 0, GL_RED, GL_UNSIGNED_BYTE, pixels);
+
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+	return texture;
+}
+
+//-----------------------------------------------
+
+texture_t create_texture(unsigned int width, unsigned int height,
+						 unsigned int nb_channels, const float* pixels)
+{
+	texture_t texture = { 0 };
+	texture.width = width;
+	texture.height = height;
+	texture.depth = 1;
+
+	glActiveTexture(GL_TEXTURE0);
+	glGenTextures(1, &texture.id);
+	glBindTexture(GL_TEXTURE_2D, texture.id);
+
+	if (nb_channels == 4)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA32F, width, height, 0, GL_RGBA, GL_FLOAT, pixels);
+	else if (nb_channels == 3)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB32F, width, height, 0, GL_RGB, GL_FLOAT, pixels);
+	else if (nb_channels == 2)
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RG32F, width, height, 0, GL_RG, GL_FLOAT, pixels);
+	else
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, width, height, 0, GL_RED, GL_FLOAT, pixels);
+
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+	return texture;
+}
+
+//-----------------------------------------------
+
+texture_t create_texture(unsigned int width, unsigned int height,
+						 unsigned int depth, unsigned int nb_channels,
+						 const float* pixels)
+{
+	texture_t texture = { 0 };
+	texture.width = width;
+	texture.height = height;
+	texture.depth = depth;
+
+	glActiveTexture(GL_TEXTURE0);
+	glGenTextures(1, &texture.id);
+	glBindTexture(GL_TEXTURE_3D, texture.id);
+
+	if (nb_channels == 4)
+		glTexImage3D(GL_TEXTURE_3D, 0, GL_RGBA32F, width, height, depth, 0, GL_RGBA, GL_FLOAT, pixels);
+	else if (nb_channels == 3)
+		glTexImage3D(GL_TEXTURE_3D, 0, GL_RGB32F, width, height, depth, 0, GL_RGB, GL_FLOAT, pixels);
+	else if (nb_channels == 2)
+		glTexImage3D(GL_TEXTURE_3D, 0, GL_RG32F, width, height, depth, 0, GL_RG, GL_FLOAT, pixels);
+	else
+		glTexImage3D(GL_TEXTURE_3D, 0, GL_R32F, width, height, depth, 0, GL_RED, GL_FLOAT, pixels);
+
+	glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+	return texture;
+}
+
+//-----------------------------------------------
+
+void destroy(const texture_t& texture)
+{
+	glDeleteTextures(1, &texture.id);
+}
+
+
+/***************************** RENDER-TO-TEXTURE *****************************/
+
+render_to_texture_t initRTT(const shader_t& shader, unsigned int width,
+							unsigned int height, unsigned int nb_buffers)
+{
+	render_to_texture_t rtt;
+	rtt.width = width;
+	rtt.height = height;
+
+	rtt.shader = &shader;
+
+	rtt.current_buffer = 0;
+
+	if (nb_buffers > 1)
+		rtt.nb_buffers = nb_buffers;
+	else
+		rtt.nb_buffers = (shader.backbuffer_handle >= 0 ? 2 : 1);
+
+	// The rectangular mesh on which the shader is applied
+	rtt.nb_vertices = 6;
+
+	const GLfloat vertex_buffer_data[] = {
+		-1.0f, -1.0f, 0.0f,
+		 1.0f, -1.0f, 0.0f,
+		-1.0f,  1.0f, 0.0f,
+		-1.0f,  1.0f, 0.0f,
+		 1.0f, -1.0f, 0.0f,
+		 1.0f,  1.0f, 0.0f,
+	};
+
+	// Vertex buffer
+	glGenBuffers(1, &rtt.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, rtt.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, rtt.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	glBindFramebuffer(GL_FRAMEBUFFER, 0);
+
+	return rtt;
+}
+
+//-----------------------------------------------
+
+render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+							  unsigned int height, const arma::fvec& color,
+							  unsigned int nb_buffers, unsigned int channel_size)
+{
+	render_to_texture_t rtt;
+
+	int nb_channels = color.n_elem;
+
+	if (channel_size == 8)
+	{
+		// Preparation of the initial color buffer
+		unsigned char* pixels = new unsigned char[width * height * nb_channels];
+		unsigned char* pDst = pixels;
+		for (unsigned int y = 0; y < height; ++y)
+		{
+			for (unsigned int x = 0; x < width; ++x)
+			{
+				pDst[0] = (unsigned char) (color(0) * 255);
+
+				if (nb_channels >= 2)
+				{
+					pDst[1] = (unsigned char) (color(1) * 255);
+
+					if (nb_channels >= 3)
+					{
+						pDst[2] = (unsigned char) (color(2) * 255);
+
+						if (nb_channels == 4)
+							pDst[3] = (unsigned char) (color(3) * 255);
+					}
+				}
+
+				pDst += nb_channels;
+			}
+		}
+
+		rtt = createRTT(shader, width, height, nb_channels, pixels, nb_buffers);
+
+		delete[] pixels;
+	}
+	else  if (channel_size == 32)
+	{
+		// Preparation of the initial color buffer
+		float* pixels = new float[width * height * nb_channels];
+		float* pDst = pixels;
+		for (unsigned int y = 0; y < height; ++y)
+		{
+			for (unsigned int x = 0; x < width; ++x)
+			{
+				pDst[0] = color(0);
+
+				if (nb_channels >= 2)
+				{
+					pDst[1] = color(1);
+
+					if (nb_channels >= 3)
+					{
+						pDst[2] = color(2);
+
+						if (nb_channels == 4)
+							pDst[3] = color(3);
+					}
+				}
+
+				pDst += nb_channels;
+			}
+		}
+
+		rtt = createRTT(shader, width, height, nb_channels, pixels, nb_buffers);
+
+		delete[] pixels;
+	}
+
+	return rtt;
+}
+
+//-----------------------------------------------
+
+render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+							  unsigned int height, unsigned int nb_channels,
+							  const unsigned char* pixels,
+							  unsigned int nb_buffers)
+{
+	render_to_texture_t rtt = initRTT(shader, width, height, nb_buffers);
+
+	for (unsigned int i = 0; i < rtt.nb_buffers; ++i) {
+		render_to_texture_buffer_t buffer;
+
+		// Creates the framebuffer
+		glGenFramebuffers(1, &buffer.framebuffer);
+		glBindFramebuffer(GL_FRAMEBUFFER, buffer.framebuffer);
+
+		// Creates the texture we're going to render to
+		buffer.texture = create_texture(width, height, nb_channels, pixels);
+
+		// Link the two
+		glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, buffer.texture.id, 0);
+
+		rtt.buffers.push_back(buffer);
+	}
+
+	return rtt;
+}
+
+//-----------------------------------------------
+
+render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
+							  unsigned int height, unsigned int nb_channels,
+							  const float* pixels, unsigned int nb_buffers)
+{
+	render_to_texture_t rtt = initRTT(shader, width, height, nb_buffers);
+
+	for (unsigned int i = 0; i < rtt.nb_buffers; ++i) {
+		render_to_texture_buffer_t buffer;
+
+		// Creates the framebuffer
+		glGenFramebuffers(1, &buffer.framebuffer);
+		glBindFramebuffer(GL_FRAMEBUFFER, buffer.framebuffer);
+
+		// Creates the texture we're going to render to
+		buffer.texture = create_texture(width, height, nb_channels, pixels);
+
+		// Link the two
+		glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, buffer.texture.id, 0);
+
+		rtt.buffers.push_back(buffer);
+	}
+
+	return rtt;
+}
+
+//-----------------------------------------------
+
+bool draw(render_to_texture_t &rtt)
+{
+	// Various checks
+	if (rtt.shader->id == 0)
+		return false;
+
+	// Retrieve the current viewport
+	GLint previous_viewport[4];
+	glGetIntegerv(GL_VIEWPORT, previous_viewport);
+
+	// Activate the GLSL program
+	glUseProgram(rtt.shader->id);
+
+	// Backbuffer management
+	if (rtt.nb_buffers > 1) {
+		unsigned int previous_buffer = rtt.current_buffer;
+
+		rtt.current_buffer++;
+		if (rtt.current_buffer >= rtt.nb_buffers)
+			rtt.current_buffer = 0;
+
+		if (rtt.shader->backbuffer_handle >= 0)
+		{
+			glActiveTexture(GL_TEXTURE0);
+			glBindTexture(GL_TEXTURE_2D, rtt.buffers[previous_buffer].texture.id);
+			glUniform1i(rtt.shader->backbuffer_handle, 0);
+		}
+	}
+
+	// Send the textures to the shader
+	sendTextures(rtt.shader);
+
+	glBindFramebuffer(GL_FRAMEBUFFER, rtt.buffers[rtt.current_buffer].framebuffer);
+
+	glViewport(0, 0, rtt.width, rtt.height);
+
+	// Send the application-specific uniforms to the shader
+	sendApplicationUniforms(rtt.shader);
+
+	// Specify the vertices for the shader
+	glEnableVertexAttribArray(0);
+	glBindBuffer(GL_ARRAY_BUFFER, rtt.vertex_buffer);
+	glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
+
+	// Set the list of draw buffers.
+	GLenum draw_buffers[1] = { GL_COLOR_ATTACHMENT0 };
+	glDrawBuffers(1, draw_buffers);
+
+	// Draw the mesh
+	glDrawArrays(GL_TRIANGLES, 0, rtt.nb_vertices);
+
+	// Cleanup
+	glDisableVertexAttribArray(0);
+	glBindFramebuffer(GL_FRAMEBUFFER, 0);
+
+	// Restore the previous viewport
+	glViewport(previous_viewport[0], previous_viewport[1], previous_viewport[2],
+			   previous_viewport[3]);
+
+	return true;
+}
+
+//-----------------------------------------------
+
+void destroy(const render_to_texture_t& rtt)
+{
+	for (auto iter = rtt.buffers.begin(); iter != rtt.buffers.end(); ++iter)
+	{
+		glDeleteFramebuffers(1, &iter->framebuffer);
+		destroy(iter->texture);
+	}
+
+	glDeleteBuffers(1, &rtt.vertex_buffer);
+}
+
+
+/*********************************** MESHES **********************************/
+
+model_t create_rectangle(const shader_t& shader, const arma::fvec& color,
+						 float width, float height, const arma::fvec& position,
+						 const arma::fmat& rotation, const transforms_t* parent_transforms)
+{
+	model_t model;
+
+	if (shader.use_lightning)
+		return model;
+
+	model.mode = GL_TRIANGLES;
+	model.shader = &shader;
+
+	// Position & rotation
+	model.transforms.position = position;
+	model.transforms.rotation = rotation;
+	model.transforms.parent = parent_transforms;
+
+	// Material
+	model.diffuse_color = color;
+
+	// Create the mesh
+	model.nb_vertices = 6;
+
+	//-- Vertex buffer
+	float half_width = 0.5f * width;
+	float half_height = 0.5f * height;
+
+	const GLfloat vertex_buffer_data[] = {
+		 half_width,  half_height, 0.0f,
+		-half_width,  half_height, 0.0f,
+		-half_width, -half_height, 0.0f,
+		-half_width, -half_height, 0.0f,
+		 half_width, -half_height, 0.0f,
+		 half_width,  half_height, 0.0f,
+	};
+
+	glGenBuffers(1, &model.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	//-- UVs buffer
+	const GLfloat uv_buffer_data[] = {
+		1.0f,  1.0f,
+		0.0f,  1.0f,
+		0.0f,  0.0f,
+		0.0f,  0.0f,
+		1.0f,  0.0f,
+		1.0f,  1.0f,
+	};
+
+	glGenBuffers(1, &model.uv_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 2 * sizeof(GLfloat),
+				 uv_buffer_data, GL_STATIC_DRAW);
+
+	return model;
+}
+
+//-----------------------------------------------
+
+model_t create_square(const shader_t& shader, const arma::fvec& color, float size,
+					  const arma::fvec& position, const arma::fmat& rotation,
+					  const transforms_t* parent_transforms)
+{
+	model_t model;
+
+	if (shader.use_lightning)
+		return model;
+
+	model.mode = GL_TRIANGLES;
+	model.shader = &shader;
+
+	// Position & rotation
+	model.transforms.position = position;
+	model.transforms.rotation = rotation;
+	model.transforms.parent = parent_transforms;
+
+	// Material
+	model.diffuse_color = color;
+
+	// Create the mesh
+	model.nb_vertices = 6;
+
+	//-- Vertex buffer
+	float half_size = 0.5f * size;
+
+	const GLfloat vertex_buffer_data[] = {
+		 half_size,	 half_size, 0.0f,
+		-half_size,	 half_size, 0.0f,
+		-half_size, -half_size, 0.0f,
+		-half_size, -half_size, 0.0f,
+		 half_size, -half_size, 0.0f,
+		 half_size,	 half_size, 0.0f,
+	};
+
+	glGenBuffers(1, &model.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	//-- UVs buffer
+	const GLfloat uv_buffer_data[] = {
+		1.0f,  1.0f,
+		0.0f,  1.0f,
+		0.0f,  0.0f,
+		0.0f,  0.0f,
+		1.0f,  0.0f,
+		1.0f,  1.0f,
+	};
+
+	glGenBuffers(1, &model.uv_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 2 * sizeof(GLfloat),
+				 uv_buffer_data, GL_STATIC_DRAW);
+
+	return model;
+}
+
+//-----------------------------------------------
+
+model_t create_sphere(const shader_t& shader, float radius, const arma::fvec& position,
+					  const arma::fmat& rotation, const transforms_t* parent_transforms)
+{
+	model_t model;
+
+	model.mode = GL_TRIANGLES;
+	model.shader = &shader;
+
+	// Position & rotation
+	model.transforms.position = position;
+	model.transforms.rotation = rotation;
+	model.transforms.parent = parent_transforms;
+
+	// Material
+	model.ambiant_color = arma::fvec({0.2f, 0.2f, 0.2f});
+	model.diffuse_color = arma::fvec({0.8f, 0.8f, 0.8f});
+	model.specular_color = arma::fvec({0.0f, 0.0f, 0.0f});
+	model.specular_power = 5;
+
+	// Create the mesh
+	const int NB_STEPS = 72;
+	const float STEP_SIZE = 360.0f / NB_STEPS;
+
+	model.nb_vertices = NB_STEPS / 2 * NB_STEPS * 6;
+
+	GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
+
+	GLfloat* normal_buffer_data = (shader.use_lightning ?
+										new GLfloat[model.nb_vertices * 3] : 0);
+
+	GLfloat* dst_vertex = vertex_buffer_data;
+	GLfloat* dst_normal = normal_buffer_data;
+
+	for (int i = 0; i < NB_STEPS / 2; ++i)
+	{
+		GLfloat latitude_lo = (float) i * STEP_SIZE;
+		GLfloat latitude_hi = latitude_lo + STEP_SIZE;
+
+		for (int j = 0; j < NB_STEPS; ++j)
+		{
+			GLfloat longitude_lo = (float) j * STEP_SIZE;
+			GLfloat longitude_hi = longitude_lo + STEP_SIZE;
+
+			arma::fvec vert_ne(3);
+			arma::fvec vert_nw(3);
+			arma::fvec vert_sw(3);
+			arma::fvec vert_se(3);
+
+			// Assign each X,Z with sin,cos values scaled by latitude radius indexed by longitude.
+			vert_ne(1) = vert_nw(1) = (float) -cos_deg(latitude_hi) * radius;
+			vert_sw(1) = vert_se(1) = (float) -cos_deg(latitude_lo) * radius;
+
+			vert_nw(0) = (float) cos_deg(longitude_lo) * (radius * (float) sin_deg(latitude_hi));
+			vert_sw(0) = (float) cos_deg(longitude_lo) * (radius * (float) sin_deg(latitude_lo));
+			vert_ne(0) = (float) cos_deg(longitude_hi) * (radius * (float) sin_deg(latitude_hi));
+			vert_se(0) = (float) cos_deg(longitude_hi) * (radius * (float) sin_deg(latitude_lo));
+
+			vert_nw(2) = (float) -sin_deg(longitude_lo) * (radius * (float) sin_deg(latitude_hi));
+			vert_sw(2) = (float) -sin_deg(longitude_lo) * (radius * (float) sin_deg(latitude_lo));
+			vert_ne(2) = (float) -sin_deg(longitude_hi) * (radius * (float) sin_deg(latitude_hi));
+			vert_se(2) = (float) -sin_deg(longitude_hi) * (radius * (float) sin_deg(latitude_lo));
+
+			dst_vertex[0] = vert_ne(0); dst_vertex[1] = vert_ne(1); dst_vertex[2] = vert_ne(2); dst_vertex += 3;
+			dst_vertex[0] = vert_nw(0); dst_vertex[1] = vert_nw(1); dst_vertex[2] = vert_nw(2); dst_vertex += 3;
+			dst_vertex[0] = vert_sw(0); dst_vertex[1] = vert_sw(1); dst_vertex[2] = vert_sw(2); dst_vertex += 3;
+
+			dst_vertex[0] = vert_sw(0); dst_vertex[1] = vert_sw(1); dst_vertex[2] = vert_sw(2); dst_vertex += 3;
+			dst_vertex[0] = vert_se(0); dst_vertex[1] = vert_se(1); dst_vertex[2] = vert_se(2); dst_vertex += 3;
+			dst_vertex[0] = vert_ne(0); dst_vertex[1] = vert_ne(1); dst_vertex[2] = vert_ne(2); dst_vertex += 3;
+
+			if (shader.use_lightning)
+			{
+				arma::fvec normal_ne = arma::normalise(vert_ne);
+				arma::fvec normal_nw = arma::normalise(vert_nw);
+				arma::fvec normal_sw = arma::normalise(vert_sw);
+				arma::fvec normal_se = arma::normalise(vert_se);
+
+				dst_normal[0] = normal_ne(0); dst_normal[1] = normal_ne(1); dst_normal[2] = normal_ne(2); dst_normal += 3;
+				dst_normal[0] = normal_nw(0); dst_normal[1] = normal_nw(1); dst_normal[2] = normal_nw(2); dst_normal += 3;
+				dst_normal[0] = normal_sw(0); dst_normal[1] = normal_sw(1); dst_normal[2] = normal_sw(2); dst_normal += 3;
+
+				dst_normal[0] = normal_sw(0); dst_normal[1] = normal_sw(1); dst_normal[2] = normal_sw(2); dst_normal += 3;
+				dst_normal[0] = normal_se(0); dst_normal[1] = normal_se(1); dst_normal[2] = normal_se(2); dst_normal += 3;
+				dst_normal[0] = normal_ne(0); dst_normal[1] = normal_ne(1); dst_normal[2] = normal_ne(2); dst_normal += 3;
+			}
+		}
+	}
+
+	// Vertex buffer
+	glGenBuffers(1, &model.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	// Normal buffer
+	if (shader.use_lightning)
+	{
+		glGenBuffers(1, &model.normal_buffer);
+		glBindBuffer(GL_ARRAY_BUFFER, model.normal_buffer);
+
+		glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+					 normal_buffer_data, GL_STATIC_DRAW);
+	}
+
+	// Cleanup
+	delete[] vertex_buffer_data;
+
+	if (shader.use_lightning)
+		delete[] normal_buffer_data;
+
+	return model;
+}
+
+//-----------------------------------------------
+
+model_t create_line(const shader_t& shader, const arma::fvec& color,
+					const arma::mat& points, const arma::fvec& position,
+					const arma::fmat& rotation, const transforms_t* parent_transforms,
+					bool line_strip)
+{
+	model_t model;
+
+	if (shader.use_lightning)
+		return model;
+
+	model.mode = (line_strip ? GL_LINE_STRIP : GL_LINES);
+	model.shader = &shader;
+
+	// Position & rotation
+	model.transforms.position = position;
+	model.transforms.rotation = rotation;
+	model.transforms.parent = parent_transforms;
+
+	// Material
+	model.diffuse_color = color;
+
+	// Create the mesh
+	model.nb_vertices = points.n_cols;
+
+	GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
+
+	GLfloat* dst = vertex_buffer_data;
+
+	for (int i = 0; i < points.n_cols; ++i) {
+		dst[0] = (float) points(0, i);
+		dst[1] = (float) points(1, i);
+
+		if (points.n_rows == 3)
+			dst[2] = (float) points(2, i);
+		else
+			dst[2] = 0.0f;
+
+		dst += 3;
+	}
+
+	// Vertex buffer
+	glGenBuffers(1, &model.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	// Cleanup
+	delete[] vertex_buffer_data;
+
+	return model;
+}
+
+//-----------------------------------------------
+
+model_t create_line(const shader_t& shader, const arma::fvec& color,
+					const arma::mat& points, float width, const arma::fvec& position,
+					const arma::fmat& rotation, const transforms_t* parent_transforms)
+{
+	arma::mat vertices(2, (points.n_cols - 1) * 12 - 6);
+
+	int n = 0;
+	for (int i = 0; i < points.n_cols - 1; ++i)
+	{
+		arma::vec p1 = points.col(i);
+		arma::vec p2 = points.col(i + 1);
+
+		float dx = p2(0) - p1(0);
+		float dy = p2(1) - p1(1);
+
+		arma::vec normal = arma::normalise(arma::vec({ -dy, dx }));
+
+		arma::vec v1 = p1 + normal * width / 2;
+		arma::vec v2 = p2 + normal * width / 2;
+		arma::vec v3 = p1 - normal * width / 2;
+		arma::vec v4 = p2 - normal * width / 2;
+
+		if (n > 0)
+		{
+			arma::vec previous_v2 = vertices.col(n - 1);
+			arma::vec previous_v4 = vertices.col(n - 2);
+
+			vertices.col(n) = previous_v2;
+			vertices.col(n + 1) = previous_v4;
+			vertices.col(n + 2) = v1;
+			vertices.col(n + 3) = previous_v4;
+			vertices.col(n + 4) = v3;
+			vertices.col(n + 5) = v1;
+
+			n += 6;
+		}
+
+		vertices.col(n) = v1;
+		vertices.col(n + 1) = v3;
+		vertices.col(n + 2) = v2;
+		vertices.col(n + 3) = v3;
+		vertices.col(n + 4) = v4;
+		vertices.col(n + 5) = v2;
+
+		n += 6;
+	}
+
+	return create_mesh(shader, color, vertices, position, rotation, parent_transforms);
+}
+
+//-----------------------------------------------
+
+model_t create_line(const shader_t& shader, const arma::fvec& color,
+					const std::vector<arma::vec>& points, const arma::fvec& position,
+					const arma::fmat& rotation, const transforms_t* parent_transforms,
+					bool line_strip)
+{
+	arma::mat points_mat(points[0].n_rows, points.size());
+
+	for (size_t i = 0; i < points.size(); ++i)
+		points_mat.col(i) = points[i];
+
+	return create_line(shader, color, points_mat, position, rotation,
+					   parent_transforms, line_strip);
+}
+
+//-----------------------------------------------
+
+model_t create_line(const shader_t& shader, const arma::fvec& color,
+					const std::vector<arma::vec>& points, float width,
+					const arma::fvec& position, const arma::fmat& rotation,
+					const transforms_t* parent_transforms)
+{
+	arma::mat points_mat(points[0].n_rows, points.size());
+
+	for (size_t i = 0; i < points.size(); ++i)
+		points_mat.col(i) = points[i];
+
+	return create_line(shader, color, points_mat, width, position, rotation,
+					   parent_transforms);
+}
+
+//-----------------------------------------------
+
+model_t create_mesh(const shader_t& shader, const arma::fvec& color,
+					const arma::mat& vertices,
+					const arma::fvec& position, const arma::fmat& rotation,
+					const transforms_t* parent_transforms)
+{
+	model_t model;
+
+	if (shader.use_lightning)
+		return model;
+
+	model.mode = GL_TRIANGLES;
+	model.shader = &shader;
+
+	// Position & rotation
+	model.transforms.position = position;
+	model.transforms.rotation = rotation;
+	model.transforms.parent = parent_transforms;
+
+	// Material
+	model.diffuse_color = color;
+
+	// Create the mesh
+	model.nb_vertices = vertices.n_cols;
+
+	GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
+
+	GLfloat* dst = vertex_buffer_data;
+
+	for (int i = 0; i < vertices.n_cols; ++i) {
+		dst[0] = (float) vertices(0, i);
+		dst[1] = (float) vertices(1, i);
+
+		if (vertices.n_rows == 3)
+			dst[2] = (float) vertices(2, i);
+		else
+			dst[2] = 0.0f;
+
+		dst += 3;
+	}
+
+	// Vertex buffer
+	glGenBuffers(1, &model.vertex_buffer);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+
+	glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
+				 vertex_buffer_data, GL_STATIC_DRAW);
+
+	// Cleanup
+	delete[] vertex_buffer_data;
+
+	return model;
+}
+
+//-----------------------------------------------
+
+model_t create_gaussian_border(const shader_t& shader, const arma::fvec& color,
+							   const arma::vec& mu, const arma::mat& sigma,
+							   const arma::fvec& position, const arma::fmat& rotation,
+							   const transforms_t* parent_transforms)
+{
+	arma::mat pts = get_gaussian_border_vertices(mu, sigma, 60, true);
+
+	return create_line(shader, color, pts, position, rotation, parent_transforms);
+}
+
+
+//-----------------------------------------------
+
+model_t create_gaussian_plane(const shader_t& shader, const camera_t& camera)
+{
+	model_t plane = create_rectangle(
+		shader, arma::fvec({ 1.0f, 0.0f, 0.0f, 1.0f }), 1.0f, 1.0f,
+		arma::fvec({ 0.0f, 0.0f, 1.5f }),
+		rotate(arma::fvec({ 0.0f, 1.0f, 0.0f }), deg2rad(0.0f)),
+		&camera.rotator
+	);
+
+	plane.use_transparency = true;
+
+	return plane;
+}
+
+//-----------------------------------------------
+
+void destroy(const model_t& model)
+{
+	if (model.vertex_buffer)
+		glDeleteBuffers(1, &model.vertex_buffer);
+
+	if (model.normal_buffer)
+		glDeleteBuffers(1, &model.normal_buffer);
+
+	if (model.uv_buffer)
+		glDeleteBuffers(1, &model.uv_buffer);
+}
+
+
+/********************************** RENDERING ********************************/
+
+inline arma::fvec check_color(const arma::fvec& color) {
+
+	if (color.n_rows < 4) {
+		arma::fvec color4({ 0.0, 0.0, 0.0, 1.0 });
+		color4(arma::span(0, 2)) = color;
+		return color4;
+	}
+
+	return color;
+}
+
+//-----------------------------------------------
+
+bool draw(const model_t& model, const arma::fmat& view,
+		  const arma::fmat& projection, const light_list_t& lights)
+{
+	// Various checks
+	if (model.nb_vertices == 0)
+		return false;
+
+	if (!model.shader || (model.shader->id == 0))
+		return false;
+
+	if (model.shader->use_lightning && lights.empty())
+		return false;
+
+	// Activate the GLSL program
+	glUseProgram(model.shader->id);
+
+	// Send the model, view and projection matrices to the shader
+	arma::fmat model_matrix = worldTransforms(&model.transforms);
+
+	glUniformMatrix4fv(model.shader->model_matrix_handle, 1, GL_FALSE, model_matrix.memptr());
+	glUniformMatrix4fv(model.shader->view_matrix_handle, 1, GL_FALSE, view.memptr());
+	glUniformMatrix4fv(model.shader->projection_matrix_handle, 1, GL_FALSE, projection.memptr());
+
+	// Send the material parameters to the shader
+	glUniform4fv(model.shader->diffuse_color_handle, 1, check_color(model.diffuse_color).memptr());
+
+	if (model.shader->use_lightning) {
+		glUniform3fv(model.shader->ambiant_color_handle, 1, model.ambiant_color.memptr());
+		glUniform3fv(model.shader->specular_color_handle, 1, model.specular_color.memptr());
+		glUniform1f(model.shader->specular_power_handle, model.specular_power);
+	}
+
+	// Send the texture parameters to the shader
+	if (model.shader->diffuse_texture_handle > -1) {
+		glActiveTexture(GL_TEXTURE0);
+		glBindTexture(GL_TEXTURE_2D, model.diffuse_texture);
+		glUniform1i(model.shader->diffuse_texture_handle, 0);
+	}
+
+	// Send the light parameters to the shader
+	if (model.shader->use_lightning) {
+		glUniform3fv(model.shader->light_position_handle, 1, lights[0].transforms.position.memptr());
+		glUniform3fv(model.shader->light_color_handle, 1, lights[0].color.memptr());
+		glUniform1f(model.shader->light_power_handle, lights[0].power);
+	}
+
+	// Send the textures to the shader
+	sendTextures(model.shader, (model.shader->diffuse_texture_handle > -1 ? GL_TEXTURE1 : GL_TEXTURE0));
+
+	// Send the application-specific uniforms to the shader
+	sendApplicationUniforms(model.shader);
+
+	// Specify the vertices for the shader
+	glEnableVertexAttribArray(0);
+	glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
+	glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
+
+	// Specify the normals for the shader
+	if (model.shader->use_lightning) {
+		glEnableVertexAttribArray(1);
+		glBindBuffer(GL_ARRAY_BUFFER, model.normal_buffer);
+		glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
+	}
+
+	// Specify the UVs for the shader
+	if (model.uv_buffer) {
+		glEnableVertexAttribArray(2);
+		glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
+		glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 0, (void*) 0);
+	}
+
+	// Specify the UVWs for the shader
+	if (model.uvw_buffer) {
+		glEnableVertexAttribArray(2);
+		glBindBuffer(GL_ARRAY_BUFFER, model.uvw_buffer);
+		glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
+	}
+
+	if (model.use_transparency) {
+		glDisable(GL_DEPTH_TEST);
+		glEnable(GL_BLEND);
+	}
+
+	// Draw the mesh
+	glDrawArrays(model.mode, 0, model.nb_vertices);
+
+	if (model.use_transparency) {
+		glDisable(GL_BLEND);
+		glEnable(GL_DEPTH_TEST);
+	}
+
+	glDisableVertexAttribArray(0);
+
+	if (model.shader->use_lightning)
+		glDisableVertexAttribArray(1);
+
+	if (model.uv_buffer)
+		glDisableVertexAttribArray(2);
+
+	if (model.uvw_buffer)
+		glDisableVertexAttribArray(2);
+
+	return true;
+}
+
+//-----------------------------------------------
+
+bool draw_rectangle(const shader_t& shader, const arma::fvec& color, float width,
+					float height, const arma::fmat& view, const arma::fmat& projection,
+					const arma::fvec& position, const arma::fmat& rotation)
+{
+	model_t rect = create_rectangle(shader, color, width, height, position, rotation);
+
+	bool result = draw(rect, view, projection);
+
+	destroy(rect);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_line(const shader_t& shader, const arma::fvec& color, const arma::mat& points,
+			   const arma::fmat& view, const arma::fmat& projection,
+			   const arma::fvec& position, const arma::fmat& rotation, bool line_strip)
+{
+	model_t line = create_line(shader, color, points, position, rotation, 0, line_strip);
+
+	bool result = draw(line, view, projection);
+
+	destroy(line);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_line(const shader_t& shader, const arma::fvec& color, const arma::mat& points,
+			   float width, const arma::fmat& view, const arma::fmat& projection,
+			   const arma::fvec& position, const arma::fmat& rotation)
+{
+	model_t line = create_line(shader, color, points, width, position, rotation);
+
+	bool result = draw(line, view, projection);
+
+	destroy(line);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_line(const shader_t& shader, const arma::fvec& color,
+			   const std::vector<arma::vec>& points,
+			   const arma::fmat& view, const arma::fmat& projection,
+			   const arma::fvec& position, const arma::fmat& rotation,
+			   bool line_strip)
+{
+	model_t line = create_line(shader, color, points, position, rotation, 0, line_strip);
+
+	bool result = draw(line, view, projection);
+
+	destroy(line);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_line(const shader_t& shader, const arma::fvec& color,
+			   const std::vector<arma::vec>& points, float width,
+			   const arma::fmat& view, const arma::fmat& projection,
+			   const arma::fvec& position, const arma::fmat& rotation)
+{
+	model_t line = create_line(shader, color, points, width, position, rotation);
+
+	bool result = draw(line, view, projection);
+
+	destroy(line);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_mesh(const shader_t& shader, const arma::fvec& color, const arma::mat& vertices,
+			   const arma::fmat& view, const arma::fmat& projection,
+			   const arma::fvec& position, const arma::fmat& rotation)
+{
+	model_t mesh = create_mesh(shader, color, vertices, position, rotation);
+
+	bool result = draw(mesh, view, projection);
+
+	destroy(mesh);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_gaussian(shader_t* shader, const arma::fvec& color,
+				   const arma::vec& mu, const arma::mat& sigma,
+				   const arma::fmat& view, const arma::fmat& projection,
+				   float viewport_width, float viewport_height)
+{
+	float square_size = fmax(viewport_width, viewport_height) +
+						fmax(fabs(fabs(mu(0)) - fabs(view(0, 3))),
+							 fabs(fabs(mu(1)) - fabs(view(1, 3)))) * 2;
+
+	gfx3::model_t square = gfx3::create_rectangle(*shader, color, square_size, square_size);
+
+	square.transforms.position = { (float) mu(0), (float) mu(1), 0.0f };
+
+	glDisable(GL_DEPTH_TEST);
+	glEnable(GL_BLEND);
+
+	arma::mat scaling({
+		{ 1.0 / square_size, 0.0 },
+		{ 0.0, 1.0 / square_size }
+	});
+
+	arma::mat scaled_sigma = scaling * sigma(arma::span(0, 1), arma::span(0, 1)) * scaling.t();
+
+	arma::vec gaussian_color;
+	if (color.n_rows == 4)
+		gaussian_color = arma::conv_to<arma::vec>::from(color);
+	else
+		gaussian_color = arma::vec({ color(0), color(1), color(2), 0.5 });
+
+	shader->setUniform("Mu", arma::vec{0.5, 0.5});
+	shader->setUniform("InvSigma", arma::mat(arma::inv(scaled_sigma)));
+	shader->setUniform("GaussianColor", gaussian_color);
+
+	bool result = gfx3::draw(square, view, projection);
+
+	glDisable(GL_BLEND);
+	glEnable(GL_DEPTH_TEST);
+
+	gfx3::destroy(square);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
+						  const arma::vec& mu, const arma::mat& sigma,
+						  const arma::fmat& view, const arma::fmat& projection,
+						  float viewport_width, float viewport_height)
+{
+	arma::mat pts = get_gaussian_border_vertices(mu, sigma, 60, true);
+
+	arma::fvec gaussian_color = color(arma::span(0, 2));
+
+	return draw_line(*shader, gaussian_color, pts, view, projection);
+}
+
+//-----------------------------------------------
+
+bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
+						  const arma::vec& mu, const arma::mat& sigma,
+						  float width, const arma::fmat& view,
+						  const arma::fmat& projection,
+						  float viewport_width, float viewport_height)
+{
+	arma::mat pts = get_gaussian_border_vertices(mu, sigma, 60, true);
+
+	arma::fvec gaussian_color = color(arma::span(0, 2));
+
+	return draw_line(*shader, gaussian_color, pts, width, view, projection);
+}
+
+//-----------------------------------------------
+
+bool draw_gaussian_3D(model_t* plane, shader_t* shader,
+					  const arma::fvec& color,
+					  const arma::vec& mu, const arma::mat& sigma,
+					  const arma::fmat& view, const arma::fmat& projection)
+{
+	plane->diffuse_color = color;
+
+	// We need to compute mu and sigma in camera space
+	arma::vec mu_cameraspace(4);
+	mu_cameraspace(arma::span(0, 2)) = mu;
+	mu_cameraspace(3) = 1.0;
+
+	mu_cameraspace = view * mu_cameraspace;
+
+	arma::mat sigma_cameraspace =
+		view(arma::span(0, 2), arma::span(0, 2)) * sigma * view(arma::span(0, 2), arma::span(0, 2)).t();
+
+	shader->setUniform("Mu", arma::vec(mu_cameraspace(arma::span(0, 2))));
+	shader->setUniform("InvSigma", arma::mat(arma::inv(sigma_cameraspace)));
+
+
+	glClear(GL_DEPTH_BUFFER_BIT);
+
+	return gfx3::draw(*plane, view, projection);
+}
+
+}
diff --git a/src/utils/gfx_common.cpp b/src/utils/gfx_common.cpp
new file mode 100644
index 0000000..c4ec7aa
--- /dev/null
+++ b/src/utils/gfx_common.cpp
@@ -0,0 +1,652 @@
+/*
+ * gfx_common.cpp
+ *
+ * Common functions usable with both gfx2 (OpenGL 2) and gfx3 (OpenGL 3.3+)
+ *
+ * Authors: Philip Abbet
+ */
+
+#ifndef GFX_NAMESPACE
+#error "Don't compile 'gfx_common.cpp' yourself, it is included by 'gfx2.cpp' or 'gfx3.cpp'"
+#endif
+
+
+namespace GFX_NAMESPACE {
+
+GLFWmonitor* get_current_monitor(GLFWwindow* window)
+{
+	int win_x, win_y, win_width, win_height;
+	glfwGetWindowPos(window, &win_x, &win_y);
+	glfwGetWindowSize(window, &win_width, &win_height);
+
+	int nb_monitors;
+	GLFWmonitor** monitors;
+	monitors = glfwGetMonitors(&nb_monitors);
+
+	int best_overlap = 0;
+	GLFWmonitor* best_monitor = 0;
+
+	for (int i = 0; i < nb_monitors; ++i) {
+		const GLFWvidmode* mode = glfwGetVideoMode(monitors[i]);
+
+		int monitor_x, monitor_y;
+		glfwGetMonitorPos(monitors[i], &monitor_x, &monitor_y);
+
+		int monitor_width = mode->width;
+		int monitor_height = mode->height;
+
+		int overlap =
+			std::max(0, std::min(win_x + win_width, monitor_x + monitor_width) - std::max(win_x, monitor_x)) *
+			std::max(0, std::min(win_y + win_height, monitor_y + monitor_height) - std::max(win_y, monitor_y)
+		);
+
+		if (best_overlap < overlap) {
+			best_overlap = overlap;
+			best_monitor = monitors[i];
+		}
+	}
+
+	return best_monitor;
+}
+
+//-----------------------------------------------
+
+GLFWwindow* create_window_at_optimal_size(const char* title, int &width, int &height) {
+
+	// First create a window of the desired size
+	GLFWwindow* window = glfwCreateWindow(
+		width, height, title, NULL, NULL
+	);
+
+	// Next determine on which monitor the window is located
+	GLFWmonitor* monitor = get_current_monitor(window);
+
+	// Then compute the "optimal" size of the window on that monitor
+	const GLFWvidmode* mode = glfwGetVideoMode(monitor);
+
+	int original_width = width;
+	int original_height = height;
+
+	if (mode->height >= 2000)
+		height = 1800;
+	else if (mode->height >= 1440)
+		height = 1200;
+	else if (mode->height >= 1200)
+		height = 1000;
+	else if (mode->height >= 1000)
+		height = 900;
+	else if (mode->height >= 900)
+		height = 800;
+	else
+		height = 600;
+
+	width = original_width * height / original_height;
+
+	if (width >= mode->width)
+	{
+		width = mode->width - 100;
+		height = original_height * width / original_width;
+	}
+
+	// Finally, resize the window and center it
+	glfwSetWindowSize(window, width, height);
+	glfwSetWindowPos(window, (mode->width - width) / 2, (mode->height - height) / 2);
+
+	return window;
+}
+
+//-----------------------------------------------
+
+window_result_t handle_window_resizing(GLFWwindow* window, window_size_t* window_size,
+									   window_size_t* previous_size) {
+
+	// Retrieve the current size of the window
+	int width, height;
+	glfwGetWindowSize(window, &width, &height);
+
+	if ((width == -1) || (height == -1))
+		return INVALID_SIZE;
+
+	// Detect when the window was resized
+	if ((width != window_size->win_width) || (height != window_size->win_height) ||
+		(window_size->fb_width == -1)) {
+
+		bool first = (window_size->win_width == -1) || (window_size->fb_width == -1);
+
+		if (previous_size) {
+			previous_size->win_width  = window_size->win_width;
+			previous_size->win_height = window_size->win_height;
+			previous_size->fb_width   = window_size->fb_width;
+			previous_size->fb_height  = window_size->fb_height;
+		}
+
+		window_size->win_width = width;
+		window_size->win_height = height;
+
+		// Retrieve the new framebuffer size
+		glfwGetFramebufferSize(window, &window_size->fb_width, &window_size->fb_height);
+
+#if __APPLE__
+		// Workaround for a bug in macOS 10.14 (Mojave): the window is black
+		// until moved or resized
+		if (first) {
+			int window_x, window_y;
+			glfwGetWindowPos(window, &window_x, &window_y);
+			glfwSetWindowPos(window, window_x - 1, window_y);
+		}
+#endif
+
+		return (first ? WINDOW_READY : WINDOW_RESIZED);
+	}
+
+	if (window_size->fb_width == -1)
+		return INVALID_SIZE;
+
+	return NO_CHANGE;
+}
+
+
+/****************************** UTILITY FUNCTIONS ****************************/
+
+void init()
+{
+	glewExperimental = GL_TRUE;
+	glewInit();
+}
+
+//-----------------------------------------------
+
+double deg2rad(double deg)
+{
+	return deg / 360.0 * (2.0 * M_PI);
+}
+
+//-----------------------------------------------
+
+double sin_deg(double deg)
+{
+	return sin(deg2rad(deg));
+}
+
+//-----------------------------------------------
+
+double cos_deg(double deg)
+{
+	return cos(deg2rad(deg));
+}
+
+//-----------------------------------------------
+
+bool is_close(float a, float b, float epsilon)
+{
+	return fabs(a - b) < epsilon;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
+				int fb_width, int fb_height) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) fb_width / (float) win_width;
+	result(1) = ((float) win_height - coords(1)) * (float) fb_height / (float) win_height;
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
+
+	result(1) = ((float) window_size.win_height - coords(1)) *
+				(float) window_size.fb_height / (float) window_size.win_height;
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
+						 int fb_width, int fb_height) {
+	arma::vec result = coords;
+
+	result(0) = (coords(0) - (float) win_width * 0.5f) * (float) fb_width / (float) win_width;
+	result(1) = ((float) win_height * 0.5f - coords(1)) * (float) fb_height / (float) win_height;
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size) {
+	arma::vec result = coords;
+
+	result(0) = (coords(0) - (float) window_size.win_width * 0.5f) *
+				(float) window_size.fb_width / (float) window_size.win_width;
+
+	result(1) = ((float) window_size.win_height * 0.5f - coords(1)) *
+				(float) window_size.fb_height / (float) window_size.win_height;
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
+				int fb_width, int fb_height) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) win_width / (float) fb_width;
+	result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
+	result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
+				(float) window_size.win_height);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
+						 int fb_width, int fb_height) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) win_width / (float) fb_width + (float) win_width * 0.5f;
+	result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height * 0.5f);
+
+	return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size) {
+	arma::vec result = coords;
+
+	result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width +
+				(float) window_size.win_width * 0.5f;
+	result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
+				(float) window_size.win_height * 0.5f);
+
+	return result;
+}
+
+
+/************************* PROJECTION & VIEW MATRICES ************************/
+
+arma::fmat perspective(float fovy, float aspect, float zNear, float zFar)
+{
+	const float top = zNear * tan(fovy / 2.0f);
+	const float bottom = -top;
+	const float right = top * aspect;
+	const float left = -right;
+
+	arma::fmat projection = arma::zeros<arma::fmat>(4,4);
+
+	projection(0, 0) = 2.0f * zNear / (right - left);
+	projection(0, 2) = (right + left) / (right - left);
+	projection(1, 1) = 2.0f * zNear / (top - bottom);
+	projection(1, 2) = (top + bottom) / (top - bottom);
+	projection(2, 2) = -(zFar + zNear) / (zFar - zNear);
+	projection(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
+	projection(3, 2) = -1.0f;
+
+	return projection;
+}
+
+//-----------------------------------------------
+
+arma::fmat orthographic(float width, float height, float zNear, float zFar)
+{
+	const float top = height / 2.0f;
+	const float bottom = -top;
+	const float right = width / 2.0f;
+	const float left = -right;
+
+	arma::fmat projection = arma::zeros<arma::fmat>(4,4);
+
+	projection(0, 0) = 2.0f / (right - left);
+	projection(0, 3) = -(right + left) / (right - left);
+	projection(1, 1) = 2.0f / (top - bottom);
+	projection(1, 3) = -(top + bottom) / (top - bottom);
+	projection(2, 2) = -2.0f / (zFar - zNear);
+	projection(2, 3) = -(zFar + zNear) / (zFar - zNear);
+	projection(3, 3) = 1.0f;
+
+	return projection;
+}
+
+//-----------------------------------------------
+
+arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
+				  const arma::fvec& up)
+{
+	const arma::fvec f(arma::normalise(target - position));
+	const arma::fvec s(arma::normalise(arma::cross(f, up)));
+	const arma::fvec u(arma::cross(s, f));
+
+	arma::fmat result = arma::zeros<arma::fmat>(4,4);
+
+	result(0, 0) = s(0);
+	result(0, 1) = s(1);
+	result(0, 2) = s(2);
+	result(1, 0) = u(0);
+	result(1, 1) = u(1);
+	result(1, 2) = u(2);
+	result(2, 0) =-f(0);
+	result(2, 1) =-f(1);
+	result(2, 2) =-f(2);
+	result(0, 3) =-arma::dot(s, position);
+	result(1, 3) =-arma::dot(u, position);
+	result(2, 3) = arma::dot(f, position);
+	result(3, 3) = 1.0f;
+
+	return result;
+}
+
+
+/****************************** TRANSFORMATIONS ******************************/
+
+arma::fmat rotate(const arma::fvec& axis, float angle)
+{
+	float rcos = cos(angle);
+	float rsin = sin(angle);
+
+	arma::fmat matrix = arma::zeros<arma::fmat>(4, 4);
+
+	matrix(0, 0) =			  rcos + axis(0) * axis(0) * (1.0f - rcos);
+	matrix(1, 0) =	axis(2) * rsin + axis(1) * axis(0) * (1.0f - rcos);
+	matrix(2, 0) = -axis(1) * rsin + axis(2) * axis(0) * (1.0f - rcos);
+	matrix(0, 1) = -axis(2) * rsin + axis(0) * axis(1) * (1.0f - rcos);
+	matrix(1, 1) =			  rcos + axis(1) * axis(1) * (1.0f - rcos);
+	matrix(2, 1) =	axis(0) * rsin + axis(2) * axis(1) * (1.0f - rcos);
+	matrix(0, 2) =	axis(1) * rsin + axis(0) * axis(2) * (1.0f - rcos);
+	matrix(1, 2) = -axis(0) * rsin + axis(1) * axis(2) * (1.0f - rcos);
+	matrix(2, 2) =			  rcos + axis(2) * axis(2) * (1.0f - rcos);
+	matrix(3, 3) = 1.0f;
+
+	return matrix;
+}
+
+//-----------------------------------------------
+
+arma::fmat rotation(const arma::fvec& from, const arma::fvec& to)
+{
+	const float dot = arma::dot(from, to);
+	const arma::fvec cross = arma::cross(from, to);
+	const float norm = arma::norm(cross);
+
+	arma::fmat g({
+		{ dot,	-norm, 0.0f },
+		{ norm,	 dot,  0.0f },
+		{ 0.0f,	 0.0f, 1.0f },
+	});
+
+	arma::fmat fi(3, 3);
+	fi.rows(0, 0) = from.t();
+	fi.rows(1, 1) = arma::normalise(to - dot * from).t();
+	fi.rows(2, 2) = arma::cross(to, from).t();
+
+	arma::fmat result = arma::eye<arma::fmat>(4, 4);
+
+	arma::fmat u;
+	if (arma::inv(u, fi))
+	{
+		u = u * g * fi;
+		result.submat(0, 0, 2, 2) = u;
+	}
+
+	return result;
+}
+
+//-----------------------------------------------
+
+void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
+						arma::fmat &rotation, arma::fvec &translation) {
+
+	arma::fvec centroidsA = arma::mean(A, 1);
+	arma::fvec centroidsB = arma::mean(B, 1);
+
+	int n = A.n_cols;
+
+	arma::fmat H = (A - repmat(centroidsA, 1, n)) * (B - repmat(centroidsB, 1, n)).t();
+
+	arma::fmat U, V;
+	arma::fvec s;
+	arma::svd(U, s, V, H);
+
+	rotation = V * U.t();
+
+	if (arma::det(rotation) < 0.0f)
+		rotation.col(2) *= -1.0f;
+
+	translation = -rotation * centroidsA + centroidsB;
+}
+
+//-----------------------------------------------
+
+arma::fmat worldTransforms(const transforms_t* transforms)
+{
+	arma::fmat result = arma::eye<arma::fmat>(4, 4);
+	result(0, 3, arma::size(3, 1)) = worldPosition(transforms);
+
+	result = result * worldRotation(transforms);
+
+	return result;
+}
+
+
+//-----------------------------------------------
+
+arma::fvec worldPosition(const transforms_t* transforms)
+{
+	if (transforms->parent)
+	{
+		arma::fvec position(4);
+		position.rows(0, 2) = transforms->position;
+		position(3) = 1.0f;
+
+		position = worldRotation(transforms->parent) * position;
+
+		return worldPosition(transforms->parent) + position.rows(0, 2);
+	}
+
+	return transforms->position;
+}
+
+
+//-----------------------------------------------
+
+arma::fmat worldRotation(const transforms_t* transforms)
+{
+	if (transforms->parent)
+	{
+		arma::fmat result = worldRotation(transforms->parent) * transforms->rotation;
+		return arma::normalise(result);
+	}
+
+	return transforms->rotation;
+}
+
+
+/*********************************** CAMERAS *********************************/
+
+void yaw(camera_t &camera, float delta) {
+	camera.target.rotation = rotate(arma::fvec({ 0.0f, 1.0f, 0.0f }), delta) * camera.target.rotation;
+}
+
+//-----------------------------------------------
+
+void pitch(camera_t &camera, float delta) {
+	camera.rotator.rotation = rotate(arma::fvec({ 1.0f, 0.0f, 0.0f }), delta) * camera.rotator.rotation;
+}
+
+//-----------------------------------------------
+
+arma::fmat view_matrix(const camera_t& camera) {
+
+	arma::fvec target_position = worldPosition(&camera.target);
+
+	arma::fvec position = worldPosition(&camera.transforms);
+
+	arma::fmat rotation = worldRotation(&camera.transforms);
+
+	arma::fmat view = lookAt(
+		position,               // Position of the camera
+		target_position,        // Look at the origin
+		arma::fvec(rotation * arma::fvec({0, 1, 0, 1}))(arma::span(0, 2))   // Head is up
+	);
+
+	return view;
+}
+
+
+/******************************** RAY CASTING ********************************/
+
+ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
+				 const arma::fmat& view, const arma::fmat& projection,
+				 int window_width, int window_height)
+{
+	ray_t ray;
+
+	ray.origin = origin;
+
+	// Compute the ray in homogeneous clip coordinates (range [-1:1, -1:1, -1:1, -1:1])
+	arma::fvec ray_clip(4);
+	ray_clip(0) = (2.0f * mouse_x) / window_width - 1.0f;
+	ray_clip(1) = 1.0f - (2.0f * mouse_y) / window_height;
+	ray_clip(2) = -1.0f;
+	ray_clip(3) = 1.0f;
+
+	// Compute the ray in camera coordinates
+	arma::fvec ray_eye = arma::inv(projection) * ray_clip;
+	ray_eye(2) = -1.0f;
+	ray_eye(3) = 0.0f;
+
+	// Compute the ray in world coordinates
+	arma::fvec ray_world = arma::inv(view) * ray_eye;
+	ray.direction = arma::fvec(arma::normalise(ray_world)).rows(0, 2);
+
+	return ray;
+}
+
+//-----------------------------------------------
+
+bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
+				arma::fvec &result)
+{
+	arma::fvec O_C = ray.origin - center;
+	float b = arma::dot(ray.direction, O_C);
+	float c = arma::dot(O_C, O_C) - radius * radius;
+
+	float det = b * b - c;
+
+	if (det < 0.0f)
+		return false;
+
+	float t;
+
+	if (det > 0.0f)
+	{
+		float t1 = -b + sqrtf(det);
+		float t2 = -b - sqrtf(det);
+
+		t = (t1 < t2 ? t1 : t2);
+	}
+	else
+	{
+		t = -b + sqrtf(det);
+	}
+
+	result = ray.origin + ray.direction * t;
+
+	return true;
+}
+
+
+/********************************** OTHERS ***********************************/
+
+arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
+										   int nb_points)
+{
+	arma::mat pts = get_gaussian_border_vertices(mu, sigma, nb_points, true);
+
+	arma::mat vertices(2, nb_points * 3);
+
+	// We need to ensure that the vertices will be in a counter-clockwise order
+	arma::vec v1 = pts(arma::span::all, 0) - mu(arma::span(0, 1));
+	arma::vec v2 = pts(arma::span::all, 1) - mu(arma::span(0, 1));
+
+	if (atan2(v1(1), v1(0)) - atan2(v2(1), v2(0)) > 0.0) {
+		for (int i = 0; i < nb_points - 1; ++i)
+		{
+			vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+			vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
+			vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
+		}
+
+		vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
+		vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, 0);
+		vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, nb_points - 1);
+	} else {
+		for (int i = 0; i < nb_points - 1; ++i)
+		{
+			vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+			vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i);
+			vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i + 1);
+		}
+
+		vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
+		vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, nb_points - 1);
+		vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, 0);
+	}
+
+	return vertices;
+}
+
+//-----------------------------------------------
+
+arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
+									   int nb_points, bool line_strip)
+{
+	arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points)),
+									 arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points))
+	);
+
+	arma::vec eigval(2);
+	arma::mat eigvec(2, 2);
+	eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+
+	arma::mat R = eigvec * diagmat(sqrt(eigval));
+
+	arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, nb_points);
+
+	if (line_strip)
+		return pts;
+
+	arma::mat vertices(2, nb_points * 2);
+
+	for (int i = 0; i < nb_points - 1; ++i)
+	{
+		vertices(arma::span::all, i * 2) = pts(arma::span::all, i);;
+		vertices(arma::span::all, i * 2 + 1) = pts(arma::span::all, i + 1);
+	}
+
+	vertices(arma::span::all, (nb_points - 1) * 2) = pts(arma::span::all, 0);;
+	vertices(arma::span::all, (nb_points - 1) * 2 + 1) = pts(arma::span::all, nb_points - 1);
+
+	return vertices;
+}
+
+}
diff --git a/src/utils/gl2ps.c b/src/utils/gl2ps.c
index c6c6d41..5c17ec0 100644
--- a/src/utils/gl2ps.c
+++ b/src/utils/gl2ps.c
@@ -1,6 +1,6 @@
 /*
  * GL2PS, an OpenGL to PostScript Printing Library
- * Copyright (C) 1999-2015 C. Geuzaine
+ * Copyright (C) 1999-2017 C. Geuzaine
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of either:
@@ -65,20 +65,6 @@
 #define GL2PS_ZOFFSET_LARGE 20.0F
 #define GL2PS_ZERO(arg)     (fabs(arg) < 1.e-20)
 
-/* Primitive types */
-
-#define GL2PS_NO_TYPE          -1
-#define GL2PS_TEXT             1
-#define GL2PS_POINT            2
-#define GL2PS_LINE             3
-#define GL2PS_QUADRANGLE       4
-#define GL2PS_TRIANGLE         5
-#define GL2PS_PIXMAP           6
-#define GL2PS_IMAGEMAP         7
-#define GL2PS_IMAGEMAP_WRITTEN 8
-#define GL2PS_IMAGEMAP_VISIBLE 9
-#define GL2PS_SPECIAL          10
-
 /* BSP tree primitive comparison */
 
 #define GL2PS_COINCIDENT  1
@@ -101,14 +87,16 @@
 #define GL2PS_BEGIN_STIPPLE_TOKEN  5
 #define GL2PS_END_STIPPLE_TOKEN    6
 #define GL2PS_POINT_SIZE_TOKEN     7
-#define GL2PS_LINE_WIDTH_TOKEN     8
-#define GL2PS_BEGIN_BLEND_TOKEN    9
-#define GL2PS_END_BLEND_TOKEN      10
-#define GL2PS_SRC_BLEND_TOKEN      11
-#define GL2PS_DST_BLEND_TOKEN      12
-#define GL2PS_IMAGEMAP_TOKEN       13
-#define GL2PS_DRAW_PIXELS_TOKEN    14
-#define GL2PS_TEXT_TOKEN           15
+#define GL2PS_LINE_CAP_TOKEN       8
+#define GL2PS_LINE_JOIN_TOKEN      9
+#define GL2PS_LINE_WIDTH_TOKEN     10
+#define GL2PS_BEGIN_BLEND_TOKEN    11
+#define GL2PS_END_BLEND_TOKEN      12
+#define GL2PS_SRC_BLEND_TOKEN      13
+#define GL2PS_DST_BLEND_TOKEN      14
+#define GL2PS_IMAGEMAP_TOKEN       15
+#define GL2PS_DRAW_PIXELS_TOKEN    16
+#define GL2PS_TEXT_TOKEN           17
 
 typedef enum {
   T_UNDEFINED    = -1,
@@ -119,7 +107,6 @@ typedef enum {
   T_VAR_ALPHA    = 1<<4
 } GL2PS_TRIANGLE_PROPERTY;
 
-typedef GLfloat GL2PSxyz[3];
 typedef GLfloat GL2PSplane[4];
 
 typedef struct _GL2PSbsptree2d GL2PSbsptree2d;
@@ -142,11 +129,6 @@ struct _GL2PSbsptree {
   GL2PSbsptree *front, *back;
 };
 
-typedef struct {
-  GL2PSxyz xyz;
-  GL2PSrgba rgba;
-} GL2PSvertex;
-
 typedef struct {
   GL2PSvertex vertex[3];
   int prop;
@@ -182,7 +164,7 @@ typedef struct {
   GLshort type, numverts;
   GLushort pattern;
   char boundary, offset, culled;
-  GLint factor;
+  GLint factor, linecap, linejoin;
   GLfloat width, ofactor, ounits;
   GL2PSvertex *verts;
   union {
@@ -209,6 +191,7 @@ typedef struct{
 typedef struct {
   /* General */
   GLint format, sort, options, colorsize, colormode, buffersize;
+  GLint lastlinecap, lastlinejoin;
   char *title, *producer, *filename;
   GLboolean boundary, blending;
   GLfloat *feedback, lastlinewidth;
@@ -220,6 +203,8 @@ typedef struct {
   FILE *stream;
   GL2PScompress *compress;
   GLboolean header;
+  GL2PSvertex rasterpos;
+  GLboolean forcerasterpos;
 
   /* BSP-specific */
   GLint maxbestroot;
@@ -878,13 +863,20 @@ static GLint gl2psAddText(GLint type, const char *str, const char *fontname,
 
   if(gl2ps->options & GL2PS_NO_TEXT) return GL2PS_SUCCESS;
 
-  glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &valid);
-  if(GL_FALSE == valid) return GL2PS_SUCCESS; /* the primitive is culled */
-
-  glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
+  if (gl2ps->forcerasterpos) {
+    pos[0] = gl2ps->rasterpos.xyz[0];
+    pos[1] = gl2ps->rasterpos.xyz[1];
+    pos[2] = gl2ps->rasterpos.xyz[2];
+    pos[3] = 1.f;
+  }
+  else {
+    glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &valid);
+    if(GL_FALSE == valid) return GL2PS_SUCCESS; /* the primitive is culled */
+    glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
+  }
 
   prim = (GL2PSprimitive*)gl2psMalloc(sizeof(GL2PSprimitive));
-  prim->type = type;
+  prim->type = (GLshort)type;
   prim->boundary = 0;
   prim->numverts = 1;
   prim->verts = (GL2PSvertex*)gl2psMalloc(sizeof(GL2PSvertex));
@@ -898,10 +890,23 @@ static GLint gl2psAddText(GLint type, const char *str, const char *fontname,
   prim->pattern = 0;
   prim->factor = 0;
   prim->width = 1;
-  if (color)
+  prim->linecap = 0;
+  prim->linejoin = 0;
+
+  if (color) {
     memcpy(prim->verts[0].rgba, color, 4 * sizeof(float));
-  else
-    glGetFloatv(GL_CURRENT_RASTER_COLOR, prim->verts[0].rgba);
+  }
+  else {
+    if (gl2ps->forcerasterpos) {
+      prim->verts[0].rgba[0] = gl2ps->rasterpos.rgba[0];
+      prim->verts[0].rgba[1] = gl2ps->rasterpos.rgba[1];
+      prim->verts[0].rgba[2] = gl2ps->rasterpos.rgba[2];
+      prim->verts[0].rgba[3] = gl2ps->rasterpos.rgba[3];
+    }
+    else {
+      glGetFloatv(GL_CURRENT_RASTER_COLOR, prim->verts[0].rgba);
+    }
+  }
   prim->data.text = (GL2PSstring*)gl2psMalloc(sizeof(GL2PSstring));
   prim->data.text->str = (char*)gl2psMalloc((strlen(str)+1)*sizeof(char));
   strcpy(prim->data.text->str, str);
@@ -911,8 +916,16 @@ static GLint gl2psAddText(GLint type, const char *str, const char *fontname,
   prim->data.text->alignment = alignment;
   prim->data.text->angle = angle;
 
-  gl2psListAdd(gl2ps->auxprimitives, &prim);
-  glPassThrough(GL2PS_TEXT_TOKEN);
+  gl2ps->forcerasterpos = GL_FALSE;
+
+  /* If no OpenGL context, just add directly to primitives */
+  if (gl2ps->options & GL2PS_NO_OPENGL_CONTEXT) {
+    gl2psListAdd(gl2ps->primitives, &prim);
+  }
+  else {
+    gl2psListAdd(gl2ps->auxprimitives, &prim);
+    glPassThrough(GL2PS_TEXT_TOKEN);
+  }
 
   return GL2PS_SUCCESS;
 }
@@ -1031,6 +1044,12 @@ static void gl2psInitTriangle(GL2PStriangle *t)
 
 /* Miscellaneous helper routines */
 
+static void gl2psResetLineProperties(void)
+{
+  gl2ps->lastlinewidth = 0.;
+  gl2ps->lastlinecap = gl2ps->lastlinejoin = 0;
+}
+
 static GL2PSprimitive *gl2psCopyPrimitive(GL2PSprimitive *p)
 {
   GL2PSprimitive *prim;
@@ -1052,6 +1071,8 @@ static GL2PSprimitive *gl2psCopyPrimitive(GL2PSprimitive *p)
   prim->factor = p->factor;
   prim->culled = p->culled;
   prim->width = p->width;
+  prim->linecap = p->linecap;
+  prim->linejoin = p->linejoin;
   prim->verts = (GL2PSvertex*)gl2psMalloc(p->numverts*sizeof(GL2PSvertex));
   memcpy(prim->verts, p->verts, p->numverts * sizeof(GL2PSvertex));
 
@@ -1250,6 +1271,8 @@ static void gl2psCreateSplitPrimitive(GL2PSprimitive *parent, GL2PSplane plane,
   child->pattern = parent->pattern;
   child->factor = parent->factor;
   child->width = parent->width;
+  child->linecap = parent->linecap;
+  child->linejoin = parent->linejoin;
   child->numverts = numverts;
   child->verts = (GL2PSvertex*)gl2psMalloc(numverts * sizeof(GL2PSvertex));
 
@@ -1388,6 +1411,8 @@ static void gl2psDivideQuad(GL2PSprimitive *quad,
   (*t1)->pattern = (*t2)->pattern = quad->pattern;
   (*t1)->factor = (*t2)->factor = quad->factor;
   (*t1)->width = (*t2)->width = quad->width;
+  (*t1)->linecap = (*t2)->linecap = quad->linecap;
+  (*t1)->linejoin = (*t2)->linejoin = quad->linejoin;
   (*t1)->verts = (GL2PSvertex*)gl2psMalloc(3 * sizeof(GL2PSvertex));
   (*t2)->verts = (GL2PSvertex*)gl2psMalloc(3 * sizeof(GL2PSvertex));
   (*t1)->verts[0] = quad->verts[0];
@@ -1929,6 +1954,8 @@ static GL2PSprimitive *gl2psCreateSplitPrimitive2D(GL2PSprimitive *parent,
   child->pattern = parent->pattern;
   child->factor = parent->factor;
   child->width = parent->width;
+  child->linecap = parent->linecap;
+  child->linejoin = parent->linejoin;
   child->numverts = numverts;
   child->verts = (GL2PSvertex*)gl2psMalloc(numverts * sizeof(GL2PSvertex));
   for(i = 0; i < numverts; i++){
@@ -2120,6 +2147,8 @@ static void gl2psAddBoundaryInList(GL2PSprimitive *prim, GL2PSlist *list)
       b->factor = prim->factor;
       b->culled = prim->culled;
       b->width = prim->width;
+      b->linecap = prim->linecap;
+      b->linejoin = prim->linejoin;
       b->boundary = 0;
       b->numverts = 2;
       b->verts = (GL2PSvertex*)gl2psMalloc(2 * sizeof(GL2PSvertex));
@@ -2185,11 +2214,12 @@ static void gl2psBuildPolygonBoundary(GL2PSbsptree *tree)
  *
  *********************************************************************/
 
-static void gl2psAddPolyPrimitive(GLshort type, GLshort numverts,
-                                  GL2PSvertex *verts, GLint offset,
-                                  GLfloat ofactor, GLfloat ounits,
-                                  GLushort pattern, GLint factor,
-                                  GLfloat width, char boundary)
+GL2PSDLL_API void gl2psAddPolyPrimitive(GLshort type, GLshort numverts,
+                                        GL2PSvertex *verts, GLint offset,
+                                        GLfloat ofactor, GLfloat ounits,
+                                        GLushort pattern, GLint factor,
+                                        GLfloat width, GLint linecap,
+                                        GLint linejoin,char boundary)
 {
   GL2PSprimitive *prim;
 
@@ -2199,12 +2229,14 @@ static void gl2psAddPolyPrimitive(GLshort type, GLshort numverts,
   prim->verts = (GL2PSvertex*)gl2psMalloc(numverts * sizeof(GL2PSvertex));
   memcpy(prim->verts, verts, numverts * sizeof(GL2PSvertex));
   prim->boundary = boundary;
-  prim->offset = offset;
+  prim->offset = (char)offset;
   prim->ofactor = ofactor;
   prim->ounits = ounits;
   prim->pattern = pattern;
   prim->factor = factor;
   prim->width = width;
+  prim->linecap = linecap;
+  prim->linejoin = linejoin;
   prim->culled = 0;
 
   /* FIXME: here we should have an option to split stretched
@@ -2244,6 +2276,7 @@ static void gl2psParseFeedbackBuffer(GLint used)
   GLushort pattern = 0;
   GLboolean boundary;
   GLint i, sizeoffloat, count, v, vtot, offset = 0, factor = 0, auxindex = 0;
+  GLint lcap = 0, ljoin = 0;
   GLfloat lwidth = 1.0F, psize = 1.0F, ofactor, ounits;
   GLfloat *current;
   GL2PSvertex vertices[3];
@@ -2265,7 +2298,7 @@ static void gl2psParseFeedbackBuffer(GLint used)
       current += i;
       used    -= i;
       gl2psAddPolyPrimitive(GL2PS_POINT, 1, vertices, 0, 0.0, 0.0,
-                            pattern, factor, psize, 0);
+                            pattern, factor, psize, lcap, ljoin, 0);
       break;
     case GL_LINE_TOKEN :
     case GL_LINE_RESET_TOKEN :
@@ -2278,7 +2311,7 @@ static void gl2psParseFeedbackBuffer(GLint used)
       current += i;
       used    -= i;
       gl2psAddPolyPrimitive(GL2PS_LINE, 2, vertices, 0, 0.0, 0.0,
-                            pattern, factor, lwidth, 0);
+                            pattern, factor, lwidth, lcap, ljoin, 0);
       break;
     case GL_POLYGON_TOKEN :
       count = (GLint)current[1];
@@ -2302,7 +2335,8 @@ static void gl2psParseFeedbackBuffer(GLint used)
           else
             flag = 0;
           gl2psAddPolyPrimitive(GL2PS_TRIANGLE, 3, vertices, offset, ofactor,
-                                ounits, pattern, factor, 1, flag);
+                                ounits, pattern, factor, 1, lcap, ljoin,
+                                flag);
           vertices[1] = vertices[2];
         }
         else
@@ -2336,7 +2370,7 @@ static void gl2psParseFeedbackBuffer(GLint used)
         break;
       case GL2PS_BEGIN_BOUNDARY_TOKEN : boundary = GL_TRUE; break;
       case GL2PS_END_BOUNDARY_TOKEN : boundary = GL_FALSE; break;
-      case GL2PS_END_STIPPLE_TOKEN : pattern = factor = 0; break;
+      case GL2PS_END_STIPPLE_TOKEN : pattern = 0; factor = 0; break;
       case GL2PS_BEGIN_BLEND_TOKEN : gl2ps->blending = GL_TRUE; break;
       case GL2PS_END_BLEND_TOKEN : gl2ps->blending = GL_FALSE; break;
       case GL2PS_BEGIN_STIPPLE_TOKEN :
@@ -2362,6 +2396,16 @@ static void gl2psParseFeedbackBuffer(GLint used)
         used -= 2;
         psize = current[1];
         break;
+      case GL2PS_LINE_CAP_TOKEN :
+        current += 2;
+        used -= 2;
+        lcap = current[1];
+        break;
+      case GL2PS_LINE_JOIN_TOKEN :
+        current += 2;
+        used -= 2;
+        ljoin = current[1];
+        break;
       case GL2PS_LINE_WIDTH_TOKEN :
         current += 2;
         used -= 2;
@@ -2475,7 +2519,7 @@ static void gl2psPrintPostScriptPixmap(GLfloat x, GLfloat y, GL2PSimage *im)
 {
   GLuint nbhex, nbyte, nrgb, nbits;
   GLuint row, col, ibyte, icase;
-  GLfloat dr, dg, db, fgrey;
+  GLfloat dr = 0., dg = 0., db = 0., fgrey;
   unsigned char red = 0, green = 0, blue = 0, b, grey;
   GLuint width = (GLuint)im->width;
   GLuint height = (GLuint)im->height;
@@ -2757,7 +2801,6 @@ static void gl2psPrintPostScriptHeader(void)
 
   gl2psPrintf("%%%%BeginProlog\n"
               "/gl2psdict 64 dict def gl2psdict begin\n"
-              "0 setlinecap 0 setlinejoin\n"
               "/tryPS3shading %s def %% set to false to force subdivision\n"
               "/rThreshold %g def %% red component subdivision threshold\n"
               "/gThreshold %g def %% green component subdivision threshold\n"
@@ -2768,7 +2811,9 @@ static void gl2psPrintPostScriptHeader(void)
   gl2psPrintf("/BD { bind def } bind def\n"
               "/C  { setrgbcolor } BD\n"
               "/G  { 0.082 mul exch 0.6094 mul add exch 0.3086 mul add neg 1.0 add setgray } BD\n"
-              "/W  { setlinewidth } BD\n");
+              "/W  { setlinewidth } BD\n"
+              "/LC  { setlinecap } BD\n"
+              "/LJ  { setlinejoin } BD\n");
 
   gl2psPrintf("/FC { findfont exch /SH exch def SH scalefont setfont } BD\n"
               "/SW { dup stringwidth pop } BD\n"
@@ -3043,6 +3088,8 @@ static void gl2psPrintPostScriptPrimitive(void *data)
     if(!gl2psSamePosition(gl2ps->lastvertex.xyz, prim->verts[0].xyz) ||
        !gl2psSameColor(gl2ps->lastrgba, prim->verts[0].rgba) ||
        gl2ps->lastlinewidth != prim->width ||
+       gl2ps->lastlinecap != prim->linecap ||
+       gl2ps->lastlinejoin != prim->linejoin ||
        gl2ps->lastpattern != prim->pattern ||
        gl2ps->lastfactor != prim->factor){
       /* End the current line if the new segment does not start where
@@ -3061,6 +3108,14 @@ static void gl2psPrintPostScriptPrimitive(void *data)
       gl2ps->lastlinewidth = prim->width;
       gl2psPrintf("%g W\n", gl2ps->lastlinewidth);
     }
+    if(gl2ps->lastlinecap != prim->linecap){
+      gl2ps->lastlinecap = prim->linecap;
+      gl2psPrintf("%d LC\n", gl2ps->lastlinecap);
+    }
+    if(gl2ps->lastlinejoin != prim->linejoin){
+      gl2ps->lastlinejoin = prim->linejoin;
+      gl2psPrintf("%d LJ\n", gl2ps->lastlinejoin);
+    }
     gl2psPrintPostScriptDash(prim->pattern, prim->factor, "setdash");
     gl2psPrintPostScriptColor(prim->verts[0].rgba);
     gl2psPrintf("%g %g %s\n", prim->verts[0].xyz[0], prim->verts[0].xyz[1],
@@ -3182,6 +3237,9 @@ static void gl2psPrintPostScriptBeginViewport(GLint viewport[4])
     gl2ps->header = GL_FALSE;
   }
 
+  gl2psResetPostScriptColor();
+  gl2psResetLineProperties();
+
   gl2psPrintf("gsave\n"
               "1.0 1.0 scale\n");
 
@@ -3369,6 +3427,8 @@ static void gl2psPrintTeXBeginViewport(GLint viewport[4])
   (void) viewport;  /* not used */
   glRenderMode(GL_FEEDBACK);
 
+  gl2psResetLineProperties();
+
   if(gl2ps->header){
     gl2psPrintTeXHeader();
     gl2ps->header = GL_FALSE;
@@ -3456,6 +3516,22 @@ static int gl2psPrintPDFLineWidth(GLfloat lw)
     return gl2psPrintf("%g w\n", lw);
 }
 
+static int gl2psPrintPDFLineCap(GLint lc)
+{
+  if(gl2ps->lastlinecap == lc)
+    return 0;
+  else
+    return gl2psPrintf("%d J\n", lc);
+}
+
+static int gl2psPrintPDFLineJoin(GLint lj)
+{
+  if(gl2ps->lastlinejoin == lj)
+    return 0;
+  else
+    return gl2psPrintf("%d j\n", lj);
+}
+
 static void gl2psPutPDFText(GL2PSstring *text, int cnt, GLfloat x, GLfloat y)
 {
   GLfloat rad, crad, srad;
@@ -3483,8 +3559,9 @@ static void gl2psPutPDFText(GL2PSstring *text, int cnt, GLfloat x, GLfloat y)
   }
 }
 
-static void gl2psPutPDFSpecial(GL2PSstring *text)
+static void gl2psPutPDFSpecial(int prim, int sec, GL2PSstring *text)
 {
+  gl2ps->streamlength += gl2psPrintf("/GS%d%d gs\n", prim, sec);
   gl2ps->streamlength += gl2psPrintf("%s\n", text->str);
 }
 
@@ -3495,7 +3572,8 @@ static void gl2psPutPDFImage(GL2PSimage *image, int cnt, GLfloat x, GLfloat y)
      "%d 0 0 %d %f %f cm\n"
      "/Im%d Do\n"
      "Q\n",
-     (int)image->width, (int)image->height, x, y, cnt);
+     (int)(image->zoom_x * image->width), (int)(image->zoom_y * image->height),
+     x, y, cnt);
 }
 
 static void gl2psPDFstacksInit(void)
@@ -3532,6 +3610,8 @@ static void gl2psPDFgroupListInit(void)
   GLushort lastpattern = 0;
   GLint lastfactor = 0;
   GLfloat lastwidth = 1;
+  GLint lastlinecap = 0;
+  GLint lastlinejoin = 0;
   GL2PStriangle lastt, tmpt;
   int lastTriangleWasNotSimpleWithSameColor = 0;
 
@@ -3560,6 +3640,7 @@ static void gl2psPDFgroupListInit(void)
       break;
     case GL2PS_LINE:
       if(lasttype != p->type || lastwidth != p->width ||
+         lastlinecap != p->linecap || lastlinejoin != p->linejoin ||
          lastpattern != p->pattern || lastfactor != p->factor ||
          !gl2psSameColor(p->verts[0].rgba, lastrgba)){
         gl2psPDFgroupObjectInit(&gro);
@@ -3573,6 +3654,8 @@ static void gl2psPDFgroupListInit(void)
       lastpattern = p->pattern;
       lastfactor = p->factor;
       lastwidth = p->width;
+      lastlinecap = p->linecap;
+      lastlinejoin = p->linejoin;
       lastrgba[0] = p->verts[0].rgba[0];
       lastrgba[1] = p->verts[0].rgba[1];
       lastrgba[2] = p->verts[0].rgba[2];
@@ -3680,7 +3763,7 @@ static void gl2psSortOutTrianglePDFgroup(GL2PSpdfgroup *gro)
 
 static void gl2psPDFgroupListWriteMainStream(void)
 {
-  int i, j, lastel;
+  int i, j, lastel, count;
   GL2PSprimitive *prim = NULL, *prev = NULL;
   GL2PSpdfgroup *gro;
   GL2PStriangle t;
@@ -3688,7 +3771,9 @@ static void gl2psPDFgroupListWriteMainStream(void)
   if(!gl2ps->pdfgrouplist)
     return;
 
-  for(i = 0; i < gl2psListNbr(gl2ps->pdfgrouplist); ++i){
+  count = gl2psListNbr(gl2ps->pdfgrouplist);
+
+  for(i = 0; i < count; ++i){
     gro = (GL2PSpdfgroup*)gl2psListPointer(gl2ps->pdfgrouplist, i);
 
     lastel = gl2psListNbr(gro->ptrlist) - 1;
@@ -3717,6 +3802,8 @@ static void gl2psPDFgroupListWriteMainStream(void)
          order to get nice stippling even when the individual segments
          are smaller than the stipple */
       gl2ps->streamlength += gl2psPrintPDFLineWidth(prim->width);
+      gl2ps->streamlength += gl2psPrintPDFLineCap(prim->linecap);
+      gl2ps->streamlength += gl2psPrintPDFLineJoin(prim->linejoin);
       gl2ps->streamlength += gl2psPrintPDFStrokeColor(prim->verts[0].rgba);
       gl2ps->streamlength += gl2psPrintPostScriptDash(prim->pattern, prim->factor, "d");
       /* start new path */
@@ -3859,9 +3946,13 @@ static void gl2psPDFgroupListWriteMainStream(void)
       }
       break;
     case GL2PS_SPECIAL:
+      lastel = gl2psListNbr(gro->ptrlist) - 1;
+      if(lastel < 0)
+        continue;
+
       for(j = 0; j <= lastel; ++j){
         prim = *(GL2PSprimitive**)gl2psListPointer(gro->ptrlist, j);
-        gl2psPutPDFSpecial(prim->data.text);
+        gl2psPutPDFSpecial(i, j, prim->data.text);
       }
     default:
       break;
@@ -3913,7 +4004,6 @@ static int gl2psPDFgroupListWriteShaderResources(void)
 }
 
 /* Images & Mask Shader XObject names */
-
 static int gl2psPDFgroupListWriteXObjectResources(void)
 {
   int i;
@@ -4502,8 +4592,8 @@ static int gl2psPrintPDFShaderMask(int obj, int childobj)
                   (int)gl2ps->viewport[2], (int)gl2ps->viewport[3]);
 
   len = (childobj>0)
-    ? strlen("/TrSh sh\n") + (int)log10((double)childobj)+1
-    : strlen("/TrSh0 sh\n");
+    ? (int)strlen("/TrSh sh\n") + (int)log10((double)childobj)+1
+    : (int)strlen("/TrSh0 sh\n");
 
   offs += fprintf(gl2ps->stream,
                   "/Length %d\n"
@@ -4837,6 +4927,8 @@ static void gl2psPrintPDFBeginViewport(GLint viewport[4])
 
   glRenderMode(GL_FEEDBACK);
 
+  gl2psResetLineProperties();
+
   if(gl2ps->header){
     gl2psPrintPDFHeader();
     gl2ps->header = GL_FALSE;
@@ -4978,7 +5070,7 @@ static void gl2psPrintSVGHeader(void)
   }
 
   /* group all the primitives and disable antialiasing */
-  gl2psPrintf("<g shape-rendering=\"crispEdges\">\n");
+  gl2psPrintf("<g>\n");
 }
 
 static void gl2psPrintSVGSmoothTriangle(GL2PSxyz xyz[3], GL2PSrgba rgba[3])
@@ -4996,6 +5088,7 @@ static void gl2psPrintSVGSmoothTriangle(GL2PSxyz xyz[3], GL2PSrgba rgba[3])
     gl2psSVGGetColorString(rgba[0], col);
     gl2psPrintf("<polygon fill=\"%s\" ", col);
     if(rgba[0][3] < 1.0F) gl2psPrintf("fill-opacity=\"%g\" ", rgba[0][3]);
+    gl2psPrintf("shape-rendering=\"crispEdges\" ");
     gl2psPrintf("points=\"%g,%g %g,%g %g,%g\"/>\n", xyz[0][0], xyz[0][1],
                 xyz[1][0], xyz[1][1], xyz[2][0], xyz[2][1]);
   }
@@ -5121,6 +5214,7 @@ static void gl2psPrintSVGPrimitive(void *data)
   GL2PSxyz xyz[4];
   GL2PSrgba rgba[4];
   char col[32];
+  char lcap[7], ljoin[7];
   int newline;
 
   prim = *(GL2PSprimitive**)data;
@@ -5146,6 +5240,8 @@ static void gl2psPrintSVGPrimitive(void *data)
     if(!gl2psSamePosition(gl2ps->lastvertex.xyz, prim->verts[0].xyz) ||
        !gl2psSameColor(gl2ps->lastrgba, prim->verts[0].rgba) ||
        gl2ps->lastlinewidth != prim->width ||
+       gl2ps->lastlinecap != prim->linecap ||
+       gl2ps->lastlinejoin != prim->linejoin ||
        gl2ps->lastpattern != prim->pattern ||
        gl2ps->lastfactor != prim->factor){
       /* End the current line if the new segment does not start where
@@ -5161,12 +5257,38 @@ static void gl2psPrintSVGPrimitive(void *data)
     gl2ps->lastvertex = prim->verts[1];
     gl2psSetLastColor(prim->verts[0].rgba);
     gl2ps->lastlinewidth = prim->width;
+    gl2ps->lastlinecap = prim->linecap;
+    gl2ps->lastlinejoin = prim->linejoin;
     gl2ps->lastpattern = prim->pattern;
     gl2ps->lastfactor = prim->factor;
     if(newline){
       gl2psSVGGetColorString(rgba[0], col);
       gl2psPrintf("<polyline fill=\"none\" stroke=\"%s\" stroke-width=\"%g\" ",
                   col, prim->width);
+      switch (prim->linecap){
+      case GL2PS_LINE_CAP_BUTT:
+        sprintf (lcap, "%s", "butt");
+        break;
+      case GL2PS_LINE_CAP_ROUND:
+        sprintf (lcap, "%s", "round");
+        break;
+      case GL2PS_LINE_CAP_SQUARE:
+        sprintf (lcap, "%s", "square");
+        break;
+      }
+      switch (prim->linejoin){
+      case GL2PS_LINE_JOIN_MITER:
+        sprintf (ljoin, "%s", "miter");
+        break;
+      case GL2PS_LINE_JOIN_ROUND:
+        sprintf (ljoin, "%s", "round");
+        break;
+      case GL2PS_LINE_JOIN_BEVEL:
+        sprintf (ljoin, "%s", "bevel");
+        break;
+      }
+      gl2psPrintf("stroke-linecap=\"%s\" stroke-linejoin=\"%s\" ",
+                  lcap, ljoin);
       if(rgba[0][3] < 1.0F) gl2psPrintf("stroke-opacity=\"%g\" ", rgba[0][3]);
       gl2psPrintSVGDash(prim->pattern, prim->factor);
       gl2psPrintf("points=\"%g,%g ", xyz[0][0], xyz[0][1]);
@@ -5193,38 +5315,38 @@ static void gl2psPrintSVGPrimitive(void *data)
                   -prim->data.text->angle, xyz[0][0], xyz[0][1]);
     switch(prim->data.text->alignment){
     case GL2PS_TEXT_C:
-      gl2psPrintf("text-anchor=\"middle\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize / 2);
+      gl2psPrintf("text-anchor=\"middle\" dy=\"%d\" ",
+                  prim->data.text->fontsize / 2);
       break;
     case GL2PS_TEXT_CL:
-      gl2psPrintf("text-anchor=\"start\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize / 2);
+      gl2psPrintf("text-anchor=\"start\" dy=\"%d\" ",
+                  prim->data.text->fontsize / 2);
       break;
     case GL2PS_TEXT_CR:
-      gl2psPrintf("text-anchor=\"end\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize / 2);
+      gl2psPrintf("text-anchor=\"end\" dy=\"%d\" ",
+                  prim->data.text->fontsize / 2);
       break;
     case GL2PS_TEXT_B:
-      gl2psPrintf("text-anchor=\"middle\" baseline-shift=\"0\" ");
+      gl2psPrintf("text-anchor=\"middle\" dy=\"0\" ");
       break;
     case GL2PS_TEXT_BR:
-      gl2psPrintf("text-anchor=\"end\" baseline-shift=\"0\" ");
+      gl2psPrintf("text-anchor=\"end\" dy=\"0\" ");
       break;
     case GL2PS_TEXT_T:
-      gl2psPrintf("text-anchor=\"middle\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize);
+      gl2psPrintf("text-anchor=\"middle\" dy=\"%d\" ",
+                  prim->data.text->fontsize);
       break;
     case GL2PS_TEXT_TL:
-      gl2psPrintf("text-anchor=\"start\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize);
+      gl2psPrintf("text-anchor=\"start\" dy=\"%d\" ",
+                  prim->data.text->fontsize);
       break;
     case GL2PS_TEXT_TR:
-      gl2psPrintf("text-anchor=\"end\" baseline-shift=\"%d\" ",
-                  -prim->data.text->fontsize);
+      gl2psPrintf("text-anchor=\"end\" dy=\"%d\" ",
+                  prim->data.text->fontsize);
       break;
     case GL2PS_TEXT_BL:
     default: /* same as GL2PS_TEXT_BL */
-      gl2psPrintf("text-anchor=\"start\" baseline-shift=\"0\" ");
+      gl2psPrintf("text-anchor=\"start\" dy=\"0\" ");
       break;
     }
     if(!strcmp(prim->data.text->fontname, "Times-Roman"))
@@ -5279,6 +5401,8 @@ static void gl2psPrintSVGBeginViewport(GLint viewport[4])
 
   glRenderMode(GL_FEEDBACK);
 
+  gl2psResetLineProperties();
+
   if(gl2ps->header){
     gl2psPrintSVGHeader();
     gl2ps->header = GL_FALSE;
@@ -5296,11 +5420,12 @@ static void gl2psPrintSVGBeginViewport(GLint viewport[4])
       rgba[3] = 1.0F;
     }
     gl2psSVGGetColorString(rgba, col);
-    gl2psPrintf("<polygon fill=\"%s\" points=\"%d,%d %d,%d %d,%d %d,%d\"/>\n", col,
+    gl2psPrintf("<polygon fill=\"%s\" points=\"%d,%d %d,%d %d,%d %d,%d\" ", col,
                 x, gl2ps->viewport[3] - y,
                 x + w, gl2ps->viewport[3] - y,
                 x + w, gl2ps->viewport[3] - (y + h),
                 x, gl2ps->viewport[3] - (y + h));
+    gl2psPrintf("shape-rendering=\"crispEdges\"/>\n");
   }
 
   gl2psPrintf("<clipPath id=\"cp%d%d%d%d\">\n", x, y, w, h);
@@ -5443,6 +5568,34 @@ static void gl2psPrintPGFPrimitive(void *data)
       gl2ps->lastlinewidth = prim->width;
       fprintf(gl2ps->stream, "\\pgfsetlinewidth{%fpt}\n", gl2ps->lastlinewidth);
     }
+    if(gl2ps->lastlinecap != prim->linecap){
+      gl2ps->lastlinecap = prim->linecap;
+      switch (prim->linecap){
+      case GL2PS_LINE_CAP_BUTT:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "buttcap");
+        break;
+      case GL2PS_LINE_CAP_ROUND:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "roundcap");
+        break;
+      case GL2PS_LINE_CAP_SQUARE:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "rectcap");
+        break;
+      }
+    }
+    if(gl2ps->lastlinejoin != prim->linejoin){
+      gl2ps->lastlinejoin = prim->linejoin;
+      switch (prim->linejoin){
+      case GL2PS_LINE_JOIN_MITER:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "miterjoin");
+        break;
+      case GL2PS_LINE_JOIN_ROUND:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "roundjoin");
+        break;
+      case GL2PS_LINE_JOIN_BEVEL:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "beveljoin");
+        break;
+      }
+    }
     gl2psPrintPGFDash(prim->pattern, prim->factor);
     fprintf(gl2ps->stream,
             "\\pgfpathmoveto{\\pgfpoint{%fpt}{%fpt}}\n"
@@ -5456,6 +5609,34 @@ static void gl2psPrintPGFPrimitive(void *data)
       gl2ps->lastlinewidth = 0;
       fprintf(gl2ps->stream, "\\pgfsetlinewidth{0.01pt}\n");
     }
+    if(gl2ps->lastlinecap != prim->linecap){
+      gl2ps->lastlinecap = prim->linecap;
+      switch (prim->linecap){
+      case GL2PS_LINE_CAP_BUTT:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "buttcap");
+        break;
+      case GL2PS_LINE_CAP_ROUND:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "roundcap");
+        break;
+      case GL2PS_LINE_CAP_SQUARE:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "rectcap");
+        break;
+      }
+    }
+    if(gl2ps->lastlinejoin != prim->linejoin){
+      gl2ps->lastlinejoin = prim->linejoin;
+      switch (prim->linejoin){
+      case GL2PS_LINE_JOIN_MITER:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "miterjoin");
+        break;
+      case GL2PS_LINE_JOIN_ROUND:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "roundjoin");
+        break;
+      case GL2PS_LINE_JOIN_BEVEL:
+        fprintf(gl2ps->stream, "\\pgfset%s\n", "beveljoin");
+        break;
+      }
+    }
     gl2psPrintPGFColor(prim->verts[0].rgba);
     fprintf(gl2ps->stream,
             "\\pgfpathmoveto{\\pgfpoint{%fpt}{%fpt}}\n"
@@ -5513,6 +5694,8 @@ static void gl2psPrintPGFBeginViewport(GLint viewport[4])
 
   glRenderMode(GL_FEEDBACK);
 
+  gl2psResetLineProperties();
+
   if(gl2ps->header){
     gl2psPrintPGFHeader();
     gl2ps->header = GL_FALSE;
@@ -5611,9 +5794,11 @@ static GLint gl2psPrintPrimitives(void)
 {
   GL2PSbsptree *root;
   GL2PSxyz eye = {0.0F, 0.0F, 100.0F * GL2PS_ZSCALE};
-  GLint used;
+  GLint used = 0;
 
-  used = glRenderMode(GL_RENDER);
+  if ((gl2ps->options & GL2PS_NO_OPENGL_CONTEXT) == GL2PS_NONE) {
+    used = glRenderMode(GL_RENDER);
+  }
 
   if(used < 0){
     gl2psMsg(GL2PS_INFO, "OpenGL feedback buffer overflow");
@@ -5681,6 +5866,34 @@ static GLint gl2psPrintPrimitives(void)
   return GL2PS_SUCCESS;
 }
 
+static GLboolean gl2psCheckOptions(GLint options, GLint colormode)
+{
+  if (options & GL2PS_NO_OPENGL_CONTEXT) {
+    if (options & GL2PS_DRAW_BACKGROUND) {
+      gl2psMsg(GL2PS_ERROR, "Options GL2PS_NO_OPENGL_CONTEXT and "
+                            "GL2PS_DRAW_BACKGROUND are incompatible.");
+      return GL_FALSE;
+    }
+    if (options & GL2PS_USE_CURRENT_VIEWPORT) {
+      gl2psMsg(GL2PS_ERROR, "Options GL2PS_NO_OPENGL_CONTEXT and "
+                            "GL2PS_USE_CURRENT_VIEWPORT are incompatible.");
+      return GL_FALSE;
+    }
+    if ((options & GL2PS_NO_BLENDING) == GL2PS_NONE) {
+      gl2psMsg(GL2PS_ERROR, "Option GL2PS_NO_OPENGL_CONTEXT requires "
+                            "option GL2PS_NO_BLENDING.");
+      return GL_FALSE;
+    }
+    if (colormode != GL_RGBA) {
+      gl2psMsg(GL2PS_ERROR, "Option GL2PS_NO_OPENGL_CONTEXT requires colormode "
+                            "to be GL_RGBA.");
+      return GL_FALSE;
+    }
+  }
+
+  return GL_TRUE;
+}
+
 /*********************************************************************
  *
  * Public routines
@@ -5704,6 +5917,13 @@ GL2PSDLL_API GLint gl2psBeginPage(const char *title, const char *producer,
 
   gl2ps = (GL2PScontext*)gl2psMalloc(sizeof(GL2PScontext));
 
+  /* Validate options */
+  if (gl2psCheckOptions(options, colormode) == GL_FALSE) {
+    gl2psFree(gl2ps);
+    gl2ps = NULL;
+    return GL2PS_ERROR;
+  }
+
   if(format >= 0 && format < (GLint)(sizeof(gl2psbackends) / sizeof(gl2psbackends[0]))){
     gl2ps->format = format;
   }
@@ -5738,6 +5958,7 @@ GL2PSDLL_API GLint gl2psBeginPage(const char *title, const char *producer,
   }
 
   gl2ps->header = GL_TRUE;
+  gl2ps->forcerasterpos = GL_FALSE;
   gl2ps->maxbestroot = 10;
   gl2ps->options = options;
   gl2ps->compress = NULL;
@@ -5775,6 +5996,8 @@ GL2PSDLL_API GLint gl2psBeginPage(const char *title, const char *producer,
     gl2ps->lastrgba[i] = -1.0F;
   }
   gl2ps->lastlinewidth = -1.0F;
+  gl2ps->lastlinecap = 0;
+  gl2ps->lastlinejoin = 0;
   gl2ps->lastpattern = 0;
   gl2ps->lastfactor = 0;
   gl2ps->imagetree = NULL;
@@ -5786,14 +6009,22 @@ GL2PSDLL_API GLint gl2psBeginPage(const char *title, const char *producer,
 
   /* get default blending mode from current OpenGL state (enabled by
      default for SVG) */
-  gl2ps->blending = (gl2ps->format == GL2PS_SVG) ? GL_TRUE : glIsEnabled(GL_BLEND);
-  glGetIntegerv(GL_BLEND_SRC, &gl2ps->blendfunc[0]);
-  glGetIntegerv(GL_BLEND_DST, &gl2ps->blendfunc[1]);
+  if ((gl2ps->options & GL2PS_NO_BLENDING) == GL2PS_NONE) {
+    gl2ps->blending = (gl2ps->format == GL2PS_SVG) ? GL_TRUE
+                                                   : glIsEnabled(GL_BLEND);
+    glGetIntegerv(GL_BLEND_SRC, &gl2ps->blendfunc[0]);
+    glGetIntegerv(GL_BLEND_DST, &gl2ps->blendfunc[1]);
+  }
+  else {
+    gl2ps->blending = GL_FALSE;
+  }
 
   if(gl2ps->colormode == GL_RGBA){
     gl2ps->colorsize = 0;
     gl2ps->colormap = NULL;
-    glGetFloatv(GL_COLOR_CLEAR_VALUE, gl2ps->bgcolor);
+    if ((gl2ps->options & GL2PS_NO_OPENGL_CONTEXT) == GL2PS_NONE) {
+      glGetFloatv(GL_COLOR_CLEAR_VALUE, gl2ps->bgcolor);
+    }
   }
   else if(gl2ps->colormode == GL_COLOR_INDEX){
     if(!colorsize || !colormap){
@@ -5847,9 +6078,16 @@ GL2PSDLL_API GLint gl2psBeginPage(const char *title, const char *producer,
 
   gl2ps->primitives = gl2psListCreate(500, 500, sizeof(GL2PSprimitive*));
   gl2ps->auxprimitives = gl2psListCreate(100, 100, sizeof(GL2PSprimitive*));
-  gl2ps->feedback = (GLfloat*)gl2psMalloc(gl2ps->buffersize * sizeof(GLfloat));
-  glFeedbackBuffer(gl2ps->buffersize, GL_3D_COLOR, gl2ps->feedback);
-  glRenderMode(GL_FEEDBACK);
+
+  if ((gl2ps->options & GL2PS_NO_OPENGL_CONTEXT) == GL2PS_NONE) {
+    gl2ps->feedback = (GLfloat*)gl2psMalloc(gl2ps->buffersize * sizeof(GLfloat));
+    glFeedbackBuffer(gl2ps->buffersize, GL_3D_COLOR, gl2ps->feedback);
+    glRenderMode(GL_FEEDBACK);
+  }
+  else {
+    gl2ps->feedback = NULL;
+    gl2ps->buffersize = 0;
+  }
 
   return GL2PS_SUCCESS;
 }
@@ -5899,7 +6137,7 @@ GL2PSDLL_API GLint gl2psEndViewport(void)
   res = (gl2psbackends[gl2ps->format]->endViewport)();
 
   /* reset last used colors, line widths */
-  gl2ps->lastlinewidth = -1.0F;
+  gl2psResetLineProperties();
 
   return res;
 }
@@ -5929,6 +6167,11 @@ GL2PSDLL_API GLint gl2psSpecial(GLint format, const char *str)
   return gl2psAddText(GL2PS_SPECIAL, str, "", 0, format, 0.0F, NULL);
 }
 
+GL2PSDLL_API GLint gl2psSpecialColor(GLint format, const char *str, GL2PSrgba rgba)
+{
+  return gl2psAddText(GL2PS_SPECIAL, str, "", 0, format, 0.0F, rgba);
+}
+
 GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
                                    GLint xorig, GLint yorig,
                                    GLenum format, GLenum type,
@@ -5952,12 +6195,22 @@ GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
     return GL2PS_ERROR;
   }
 
-  glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &valid);
-  if(GL_FALSE == valid) return GL2PS_SUCCESS; /* the primitive is culled */
-
-  glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
-  glGetFloatv(GL_ZOOM_X, &zoom_x);
-  glGetFloatv(GL_ZOOM_Y, &zoom_y);
+  if (gl2ps->forcerasterpos) {
+    pos[0] = gl2ps->rasterpos.xyz[0];
+    pos[1] = gl2ps->rasterpos.xyz[1];
+    pos[2] = gl2ps->rasterpos.xyz[2];
+    pos[3] = 1.f;
+    /* Hardcode zoom factors (for now?) */
+    zoom_x = 1.f;
+    zoom_y = 1.f;
+  }
+  else {
+    glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &valid);
+    if(GL_FALSE == valid) return GL2PS_SUCCESS; /* the primitive is culled */
+    glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
+    glGetFloatv(GL_ZOOM_X, &zoom_x);
+    glGetFloatv(GL_ZOOM_Y, &zoom_y);
+  }
 
   prim = (GL2PSprimitive*)gl2psMalloc(sizeof(GL2PSprimitive));
   prim->type = GL2PS_PIXMAP;
@@ -5974,7 +6227,15 @@ GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
   prim->pattern = 0;
   prim->factor = 0;
   prim->width = 1;
-  glGetFloatv(GL_CURRENT_RASTER_COLOR, prim->verts[0].rgba);
+  if (gl2ps->forcerasterpos) {
+    prim->verts[0].rgba[0] = gl2ps->rasterpos.rgba[0];
+    prim->verts[0].rgba[1] = gl2ps->rasterpos.rgba[1];
+    prim->verts[0].rgba[2] = gl2ps->rasterpos.rgba[2];
+    prim->verts[0].rgba[3] = gl2ps->rasterpos.rgba[3];
+  }
+  else {
+    glGetFloatv(GL_CURRENT_RASTER_COLOR, prim->verts[0].rgba);
+  }
   prim->data.image = (GL2PSimage*)gl2psMalloc(sizeof(GL2PSimage));
   prim->data.image->width = width;
   prim->data.image->height = height;
@@ -5983,6 +6244,8 @@ GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
   prim->data.image->format = format;
   prim->data.image->type = type;
 
+  gl2ps->forcerasterpos = GL_FALSE;
+
   switch(format){
   case GL_RGBA:
     if(gl2ps->options & GL2PS_NO_BLENDING || !gl2ps->blending){
@@ -6011,8 +6274,14 @@ GL2PSDLL_API GLint gl2psDrawPixels(GLsizei width, GLsizei height,
     break;
   }
 
-  gl2psListAdd(gl2ps->auxprimitives, &prim);
-  glPassThrough(GL2PS_DRAW_PIXELS_TOKEN);
+  /* If no OpenGL context, just add directly to primitives */
+  if ((gl2ps->options & GL2PS_NO_OPENGL_CONTEXT) == GL2PS_NONE) {
+    gl2psListAdd(gl2ps->auxprimitives, &prim);
+    glPassThrough(GL2PS_DRAW_PIXELS_TOKEN);
+  }
+  else {
+    gl2psListAdd(gl2ps->primitives, &prim);
+  }
 
   return GL2PS_SUCCESS;
 }
@@ -6113,6 +6382,26 @@ GL2PSDLL_API GLint gl2psPointSize(GLfloat value)
   return GL2PS_SUCCESS;
 }
 
+GL2PSDLL_API GLint gl2psLineCap(GLint value)
+{
+  if(!gl2ps) return GL2PS_UNINITIALIZED;
+
+  glPassThrough(GL2PS_LINE_CAP_TOKEN);
+  glPassThrough(value);
+
+  return GL2PS_SUCCESS;
+}
+
+GL2PSDLL_API GLint gl2psLineJoin(GLint value)
+{
+  if(!gl2ps) return GL2PS_UNINITIALIZED;
+
+  glPassThrough(GL2PS_LINE_JOIN_TOKEN);
+  glPassThrough(value);
+
+  return GL2PS_SUCCESS;
+}
+
 GL2PSDLL_API GLint gl2psLineWidth(GLfloat value)
 {
   if(!gl2ps) return GL2PS_UNINITIALIZED;
@@ -6142,6 +6431,10 @@ GL2PSDLL_API GLint gl2psSetOptions(GLint options)
 {
   if(!gl2ps) return GL2PS_UNINITIALIZED;
 
+  if(gl2psCheckOptions(options, gl2ps->colormode) == GL_FALSE) {
+    return GL2PS_ERROR;
+  }
+
   gl2ps->options = options;
 
   return GL2PS_SUCCESS;
@@ -6179,3 +6472,22 @@ GL2PSDLL_API GLint gl2psGetFileFormat()
 {
   return gl2ps->format;
 }
+
+GL2PSDLL_API GLint gl2psForceRasterPos(GL2PSvertex *vert)
+{
+
+  if(!gl2ps) {
+    return GL2PS_UNINITIALIZED;
+  }
+
+  gl2ps->forcerasterpos = GL_TRUE;
+  gl2ps->rasterpos.xyz[0] = vert->xyz[0];
+  gl2ps->rasterpos.xyz[1] = vert->xyz[1];
+  gl2ps->rasterpos.xyz[2] = vert->xyz[2];
+  gl2ps->rasterpos.rgba[0] = vert->rgba[0];
+  gl2ps->rasterpos.rgba[1] = vert->rgba[1];
+  gl2ps->rasterpos.rgba[2] = vert->rgba[2];
+  gl2ps->rasterpos.rgba[3] = vert->rgba[3];
+
+  return GL2PS_SUCCESS;
+}
diff --git a/src/utils/imgui_impl_glfw_gl3.cpp b/src/utils/imgui_impl_glfw_gl3.cpp
new file mode 100644
index 0000000..9620870
--- /dev/null
+++ b/src/utils/imgui_impl_glfw_gl3.cpp
@@ -0,0 +1,530 @@
+// ImGui GLFW binding with OpenGL3 + shaders
+// (GLFW is a cross-platform general purpose library for handling windows, inputs, OpenGL/Vulkan graphics context creation, etc.)
+// (GL3W is a helper library to access OpenGL functions since there is no standard header to access modern OpenGL functions easily. Alternatives are GLEW, Glad, etc.)
+
+// Implemented features:
+//  [X] User texture binding. Cast 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID in imgui.cpp.
+//  [X] Gamepad navigation mapping. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'.
+
+// You can copy and use unmodified imgui_impl_* files in your project. See main.cpp for an example of using this.
+// If you use this binding you'll need to call 4 functions: ImGui_ImplXXXX_Init(), ImGui_ImplXXXX_NewFrame(), ImGui::Render() and ImGui_ImplXXXX_Shutdown().
+// If you are new to ImGui, see examples/README.txt and documentation at the top of imgui.cpp.
+// https://github.com/ocornut/imgui
+
+// CHANGELOG
+// (minor and older changes stripped away, please see git history for details)
+//  2018-03-20: Misc: Setup io.BackendFlags ImGuiBackendFlags_HasMouseCursors and ImGuiBackendFlags_HasSetMousePos flags + honor ImGuiConfigFlags_NoMouseCursorChange flag.
+//  2018-03-06: OpenGL: Added const char* glsl_version parameter to ImGui_ImplGlfwGL3_Init() so user can override the GLSL version e.g. "#version 150".
+//  2018-02-23: OpenGL: Create the VAO in the render function so the setup can more easily be used with multiple shared GL context.
+//  2018-02-20: Inputs: Added support for mouse cursors (ImGui::GetMouseCursor() value and WM_SETCURSOR message handling).
+//  2018-02-20: Inputs: Renamed GLFW callbacks exposed in .h to not include GL3 in their name.
+//  2018-02-16: Misc: Obsoleted the io.RenderDrawListsFn callback and exposed ImGui_ImplGlfwGL3_RenderDrawData() in the .h file so you can call it yourself.
+//  2018-02-06: Misc: Removed call to ImGui::Shutdown() which is not available from 1.60 WIP, user needs to call CreateContext/DestroyContext themselves.
+//  2018-02-06: Inputs: Added mapping for ImGuiKey_Space.
+//  2018-01-25: Inputs: Added gamepad support if ImGuiConfigFlags_NavEnableGamepad is set.
+//  2018-01-25: Inputs: Honoring the io.WantSetMousePos flag by repositioning the mouse (ImGuiConfigFlags_NavEnableSetMousePos is set).
+//  2018-01-20: Inputs: Added Horizontal Mouse Wheel support.
+//  2018-01-18: Inputs: Added mapping for ImGuiKey_Insert.
+//  2018-01-07: OpenGL: Changed GLSL shader version from 330 to 150. (Also changed GL context from 3.3 to 3.2 in example's main.cpp)
+//  2017-09-01: OpenGL: Save and restore current bound sampler. Save and restore current polygon mode.
+//  2017-08-25: Inputs: MousePos set to -FLT_MAX,-FLT_MAX when mouse is unavailable/missing (instead of -1,-1).
+//  2017-05-01: OpenGL: Fixed save and restore of current blend function state.
+//  2016-10-15: Misc: Added a void* user_data parameter to Clipboard function handlers.
+//  2016-09-05: OpenGL: Fixed save and restore of current scissor rectangle.
+//  2016-04-30: OpenGL: Fixed save and restore of current GL_ACTIVE_TEXTURE.
+
+#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
+#define _CRT_SECURE_NO_WARNINGS
+#endif
+
+#include "imgui.h"
+#include "imgui_impl_glfw_gl3.h"
+
+// GL3W/GLFW
+#include <GL/glew.h>
+#include <GLFW/glfw3.h>
+#ifdef _WIN32
+#undef APIENTRY
+#define GLFW_EXPOSE_NATIVE_WIN32
+#define GLFW_EXPOSE_NATIVE_WGL
+#include <GLFW/glfw3native.h>
+#endif
+
+// GLFW data
+static GLFWwindow*  g_Window = NULL;
+static double       g_Time = 0.0f;
+static bool         g_MouseJustPressed[3] = { false, false, false };
+static GLFWcursor*  g_MouseCursors[ImGuiMouseCursor_COUNT] = { 0 };
+
+// OpenGL3 data
+static char         g_GlslVersion[32] = "#version 150";
+static GLuint       g_FontTexture = 0;
+static int          g_ShaderHandle = 0, g_VertHandle = 0, g_FragHandle = 0;
+static int          g_AttribLocationTex = 0, g_AttribLocationProjMtx = 0;
+static int          g_AttribLocationPosition = 0, g_AttribLocationUV = 0, g_AttribLocationColor = 0;
+static unsigned int g_VboHandle = 0, g_ElementsHandle = 0;
+
+// OpenGL3 Render function.
+// (this used to be set in io.RenderDrawListsFn and called by ImGui::Render(), but you can now call this directly from your main loop)
+// Note that this implementation is little overcomplicated because we are saving/setting up/restoring every OpenGL state explicitly, in order to be able to run within any OpenGL engine that doesn't do so. 
+void ImGui_ImplGlfwGL3_RenderDrawData(ImDrawData* draw_data)
+{
+    // Avoid rendering when minimized, scale coordinates for retina displays (screen coordinates != framebuffer coordinates)
+    ImGuiIO& io = ImGui::GetIO();
+    int fb_width = (int)(io.DisplaySize.x * io.DisplayFramebufferScale.x);
+    int fb_height = (int)(io.DisplaySize.y * io.DisplayFramebufferScale.y);
+    if (fb_width == 0 || fb_height == 0)
+        return;
+    draw_data->ScaleClipRects(io.DisplayFramebufferScale);
+
+    // Backup GL state
+    GLenum last_active_texture; glGetIntegerv(GL_ACTIVE_TEXTURE, (GLint*)&last_active_texture);
+    glActiveTexture(GL_TEXTURE0);
+    GLint last_program; glGetIntegerv(GL_CURRENT_PROGRAM, &last_program);
+    GLint last_texture; glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
+    GLint last_sampler; glGetIntegerv(GL_SAMPLER_BINDING, &last_sampler);
+    GLint last_array_buffer; glGetIntegerv(GL_ARRAY_BUFFER_BINDING, &last_array_buffer);
+    GLint last_element_array_buffer; glGetIntegerv(GL_ELEMENT_ARRAY_BUFFER_BINDING, &last_element_array_buffer);
+    GLint last_vertex_array; glGetIntegerv(GL_VERTEX_ARRAY_BINDING, &last_vertex_array);
+    GLint last_polygon_mode[2]; glGetIntegerv(GL_POLYGON_MODE, last_polygon_mode);
+    GLint last_viewport[4]; glGetIntegerv(GL_VIEWPORT, last_viewport);
+    GLint last_scissor_box[4]; glGetIntegerv(GL_SCISSOR_BOX, last_scissor_box);
+    GLenum last_blend_src_rgb; glGetIntegerv(GL_BLEND_SRC_RGB, (GLint*)&last_blend_src_rgb);
+    GLenum last_blend_dst_rgb; glGetIntegerv(GL_BLEND_DST_RGB, (GLint*)&last_blend_dst_rgb);
+    GLenum last_blend_src_alpha; glGetIntegerv(GL_BLEND_SRC_ALPHA, (GLint*)&last_blend_src_alpha);
+    GLenum last_blend_dst_alpha; glGetIntegerv(GL_BLEND_DST_ALPHA, (GLint*)&last_blend_dst_alpha);
+    GLenum last_blend_equation_rgb; glGetIntegerv(GL_BLEND_EQUATION_RGB, (GLint*)&last_blend_equation_rgb);
+    GLenum last_blend_equation_alpha; glGetIntegerv(GL_BLEND_EQUATION_ALPHA, (GLint*)&last_blend_equation_alpha);
+    GLboolean last_enable_blend = glIsEnabled(GL_BLEND);
+    GLboolean last_enable_cull_face = glIsEnabled(GL_CULL_FACE);
+    GLboolean last_enable_depth_test = glIsEnabled(GL_DEPTH_TEST);
+    GLboolean last_enable_scissor_test = glIsEnabled(GL_SCISSOR_TEST);
+
+    // Setup render state: alpha-blending enabled, no face culling, no depth testing, scissor enabled, polygon fill
+    glEnable(GL_BLEND);
+    glBlendEquation(GL_FUNC_ADD);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+    glDisable(GL_CULL_FACE);
+    glDisable(GL_DEPTH_TEST);
+    glEnable(GL_SCISSOR_TEST);
+    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+
+    // Setup viewport, orthographic projection matrix
+    glViewport(0, 0, (GLsizei)fb_width, (GLsizei)fb_height);
+    const float ortho_projection[4][4] =
+    {
+        { 2.0f/io.DisplaySize.x, 0.0f,                   0.0f, 0.0f },
+        { 0.0f,                  2.0f/-io.DisplaySize.y, 0.0f, 0.0f },
+        { 0.0f,                  0.0f,                  -1.0f, 0.0f },
+        {-1.0f,                  1.0f,                   0.0f, 1.0f },
+    };
+    glUseProgram(g_ShaderHandle);
+    glUniform1i(g_AttribLocationTex, 0);
+    glUniformMatrix4fv(g_AttribLocationProjMtx, 1, GL_FALSE, &ortho_projection[0][0]);
+    glBindSampler(0, 0); // Rely on combined texture/sampler state.
+
+    // Recreate the VAO every time 
+    // (This is to easily allow multiple GL contexts. VAO are not shared among GL contexts, and we don't track creation/deletion of windows so we don't have an obvious key to use to cache them.)
+    GLuint vao_handle = 0;
+    glGenVertexArrays(1, &vao_handle);
+    glBindVertexArray(vao_handle);
+    glBindBuffer(GL_ARRAY_BUFFER, g_VboHandle);
+    glEnableVertexAttribArray(g_AttribLocationPosition);
+    glEnableVertexAttribArray(g_AttribLocationUV);
+    glEnableVertexAttribArray(g_AttribLocationColor);
+    glVertexAttribPointer(g_AttribLocationPosition, 2, GL_FLOAT, GL_FALSE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, pos));
+    glVertexAttribPointer(g_AttribLocationUV, 2, GL_FLOAT, GL_FALSE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, uv));
+    glVertexAttribPointer(g_AttribLocationColor, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, col));
+
+    // Draw
+    for (int n = 0; n < draw_data->CmdListsCount; n++)
+    {
+        const ImDrawList* cmd_list = draw_data->CmdLists[n];
+        const ImDrawIdx* idx_buffer_offset = 0;
+
+        glBindBuffer(GL_ARRAY_BUFFER, g_VboHandle);
+        glBufferData(GL_ARRAY_BUFFER, (GLsizeiptr)cmd_list->VtxBuffer.Size * sizeof(ImDrawVert), (const GLvoid*)cmd_list->VtxBuffer.Data, GL_STREAM_DRAW);
+
+        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, g_ElementsHandle);
+        glBufferData(GL_ELEMENT_ARRAY_BUFFER, (GLsizeiptr)cmd_list->IdxBuffer.Size * sizeof(ImDrawIdx), (const GLvoid*)cmd_list->IdxBuffer.Data, GL_STREAM_DRAW);
+
+        for (int cmd_i = 0; cmd_i < cmd_list->CmdBuffer.Size; cmd_i++)
+        {
+            const ImDrawCmd* pcmd = &cmd_list->CmdBuffer[cmd_i];
+            if (pcmd->UserCallback)
+            {
+                pcmd->UserCallback(cmd_list, pcmd);
+            }
+            else
+            {
+                glBindTexture(GL_TEXTURE_2D, (GLuint)(intptr_t)pcmd->TextureId);
+                glScissor((int)pcmd->ClipRect.x, (int)(fb_height - pcmd->ClipRect.w), (int)(pcmd->ClipRect.z - pcmd->ClipRect.x), (int)(pcmd->ClipRect.w - pcmd->ClipRect.y));
+                glDrawElements(GL_TRIANGLES, (GLsizei)pcmd->ElemCount, sizeof(ImDrawIdx) == 2 ? GL_UNSIGNED_SHORT : GL_UNSIGNED_INT, idx_buffer_offset);
+            }
+            idx_buffer_offset += pcmd->ElemCount;
+        }
+    }
+    glDeleteVertexArrays(1, &vao_handle);
+
+    // Restore modified GL state
+    glUseProgram(last_program);
+    glBindTexture(GL_TEXTURE_2D, last_texture);
+    glBindSampler(0, last_sampler);
+    glActiveTexture(last_active_texture);
+    glBindVertexArray(last_vertex_array);
+    glBindBuffer(GL_ARRAY_BUFFER, last_array_buffer);
+    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, last_element_array_buffer);
+    glBlendEquationSeparate(last_blend_equation_rgb, last_blend_equation_alpha);
+    glBlendFuncSeparate(last_blend_src_rgb, last_blend_dst_rgb, last_blend_src_alpha, last_blend_dst_alpha);
+    if (last_enable_blend) glEnable(GL_BLEND); else glDisable(GL_BLEND);
+    if (last_enable_cull_face) glEnable(GL_CULL_FACE); else glDisable(GL_CULL_FACE);
+    if (last_enable_depth_test) glEnable(GL_DEPTH_TEST); else glDisable(GL_DEPTH_TEST);
+    if (last_enable_scissor_test) glEnable(GL_SCISSOR_TEST); else glDisable(GL_SCISSOR_TEST);
+    glPolygonMode(GL_FRONT_AND_BACK, (GLenum)last_polygon_mode[0]);
+    glViewport(last_viewport[0], last_viewport[1], (GLsizei)last_viewport[2], (GLsizei)last_viewport[3]);
+    glScissor(last_scissor_box[0], last_scissor_box[1], (GLsizei)last_scissor_box[2], (GLsizei)last_scissor_box[3]);
+}
+
+static const char* ImGui_ImplGlfwGL3_GetClipboardText(void* user_data)
+{
+    return glfwGetClipboardString((GLFWwindow*)user_data);
+}
+
+static void ImGui_ImplGlfwGL3_SetClipboardText(void* user_data, const char* text)
+{
+    glfwSetClipboardString((GLFWwindow*)user_data, text);
+}
+
+void ImGui_ImplGlfw_MouseButtonCallback(GLFWwindow*, int button, int action, int /*mods*/)
+{
+    if (action == GLFW_PRESS && button >= 0 && button < 3)
+        g_MouseJustPressed[button] = true;
+}
+
+void ImGui_ImplGlfw_ScrollCallback(GLFWwindow*, double xoffset, double yoffset)
+{
+    ImGuiIO& io = ImGui::GetIO();
+    io.MouseWheelH += (float)xoffset;
+    io.MouseWheel += (float)yoffset;
+}
+
+void ImGui_ImplGlfw_KeyCallback(GLFWwindow*, int key, int, int action, int mods)
+{
+    ImGuiIO& io = ImGui::GetIO();
+    if (action == GLFW_PRESS)
+        io.KeysDown[key] = true;
+    if (action == GLFW_RELEASE)
+        io.KeysDown[key] = false;
+
+    (void)mods; // Modifiers are not reliable across systems
+    io.KeyCtrl = io.KeysDown[GLFW_KEY_LEFT_CONTROL] || io.KeysDown[GLFW_KEY_RIGHT_CONTROL];
+    io.KeyShift = io.KeysDown[GLFW_KEY_LEFT_SHIFT] || io.KeysDown[GLFW_KEY_RIGHT_SHIFT];
+    io.KeyAlt = io.KeysDown[GLFW_KEY_LEFT_ALT] || io.KeysDown[GLFW_KEY_RIGHT_ALT];
+    io.KeySuper = io.KeysDown[GLFW_KEY_LEFT_SUPER] || io.KeysDown[GLFW_KEY_RIGHT_SUPER];
+}
+
+void ImGui_ImplGlfw_CharCallback(GLFWwindow*, unsigned int c)
+{
+    ImGuiIO& io = ImGui::GetIO();
+    if (c > 0 && c < 0x10000)
+        io.AddInputCharacter((unsigned short)c);
+}
+
+bool ImGui_ImplGlfwGL3_CreateFontsTexture()
+{
+    // Build texture atlas
+    ImGuiIO& io = ImGui::GetIO();
+    unsigned char* pixels;
+    int width, height;
+    io.Fonts->GetTexDataAsRGBA32(&pixels, &width, &height);   // Load as RGBA 32-bits (75% of the memory is wasted, but default font is so small) because it is more likely to be compatible with user's existing shaders. If your ImTextureId represent a higher-level concept than just a GL texture id, consider calling GetTexDataAsAlpha8() instead to save on GPU memory.
+
+    // Upload texture to graphics system
+    GLint last_texture;
+    glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
+    glGenTextures(1, &g_FontTexture);
+    glBindTexture(GL_TEXTURE_2D, g_FontTexture);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+    glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels);
+
+    // Store our identifier
+    io.Fonts->TexID = (void *)(intptr_t)g_FontTexture;
+
+    // Restore state
+    glBindTexture(GL_TEXTURE_2D, last_texture);
+
+    return true;
+}
+
+bool ImGui_ImplGlfwGL3_CreateDeviceObjects()
+{
+    // Backup GL state
+    GLint last_texture, last_array_buffer, last_vertex_array;
+    glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
+    glGetIntegerv(GL_ARRAY_BUFFER_BINDING, &last_array_buffer);
+    glGetIntegerv(GL_VERTEX_ARRAY_BINDING, &last_vertex_array);
+
+    const GLchar* vertex_shader =
+        "uniform mat4 ProjMtx;\n"
+        "in vec2 Position;\n"
+        "in vec2 UV;\n"
+        "in vec4 Color;\n"
+        "out vec2 Frag_UV;\n"
+        "out vec4 Frag_Color;\n"
+        "void main()\n"
+        "{\n"
+        "	Frag_UV = UV;\n"
+        "	Frag_Color = Color;\n"
+        "	gl_Position = ProjMtx * vec4(Position.xy,0,1);\n"
+        "}\n";
+
+    const GLchar* fragment_shader =
+        "uniform sampler2D Texture;\n"
+        "in vec2 Frag_UV;\n"
+        "in vec4 Frag_Color;\n"
+        "out vec4 Out_Color;\n"
+        "void main()\n"
+        "{\n"
+        "	Out_Color = Frag_Color * texture( Texture, Frag_UV.st);\n"
+        "}\n";
+
+    const GLchar* vertex_shader_with_version[2] = { g_GlslVersion, vertex_shader };
+    const GLchar* fragment_shader_with_version[2] = { g_GlslVersion, fragment_shader };
+
+    g_ShaderHandle = glCreateProgram();
+    g_VertHandle = glCreateShader(GL_VERTEX_SHADER);
+    g_FragHandle = glCreateShader(GL_FRAGMENT_SHADER);
+    glShaderSource(g_VertHandle, 2, vertex_shader_with_version, NULL);
+    glShaderSource(g_FragHandle, 2, fragment_shader_with_version, NULL);
+    glCompileShader(g_VertHandle);
+    glCompileShader(g_FragHandle);
+    glAttachShader(g_ShaderHandle, g_VertHandle);
+    glAttachShader(g_ShaderHandle, g_FragHandle);
+    glLinkProgram(g_ShaderHandle);
+
+    g_AttribLocationTex = glGetUniformLocation(g_ShaderHandle, "Texture");
+    g_AttribLocationProjMtx = glGetUniformLocation(g_ShaderHandle, "ProjMtx");
+    g_AttribLocationPosition = glGetAttribLocation(g_ShaderHandle, "Position");
+    g_AttribLocationUV = glGetAttribLocation(g_ShaderHandle, "UV");
+    g_AttribLocationColor = glGetAttribLocation(g_ShaderHandle, "Color");
+
+    glGenBuffers(1, &g_VboHandle);
+    glGenBuffers(1, &g_ElementsHandle);
+
+    ImGui_ImplGlfwGL3_CreateFontsTexture();
+
+    // Restore modified GL state
+    glBindTexture(GL_TEXTURE_2D, last_texture);
+    glBindBuffer(GL_ARRAY_BUFFER, last_array_buffer);
+    glBindVertexArray(last_vertex_array);
+
+    return true;
+}
+
+void    ImGui_ImplGlfwGL3_InvalidateDeviceObjects()
+{
+    if (g_VboHandle) glDeleteBuffers(1, &g_VboHandle);
+    if (g_ElementsHandle) glDeleteBuffers(1, &g_ElementsHandle);
+    g_VboHandle = g_ElementsHandle = 0;
+
+    if (g_ShaderHandle && g_VertHandle) glDetachShader(g_ShaderHandle, g_VertHandle);
+    if (g_VertHandle) glDeleteShader(g_VertHandle);
+    g_VertHandle = 0;
+
+    if (g_ShaderHandle && g_FragHandle) glDetachShader(g_ShaderHandle, g_FragHandle);
+    if (g_FragHandle) glDeleteShader(g_FragHandle);
+    g_FragHandle = 0;
+
+    if (g_ShaderHandle) glDeleteProgram(g_ShaderHandle);
+    g_ShaderHandle = 0;
+
+    if (g_FontTexture)
+    {
+        glDeleteTextures(1, &g_FontTexture);
+        ImGui::GetIO().Fonts->TexID = 0;
+        g_FontTexture = 0;
+    }
+}
+
+static void ImGui_ImplGlfw_InstallCallbacks(GLFWwindow* window)
+{
+    glfwSetMouseButtonCallback(window, ImGui_ImplGlfw_MouseButtonCallback);
+    glfwSetScrollCallback(window, ImGui_ImplGlfw_ScrollCallback);
+    glfwSetKeyCallback(window, ImGui_ImplGlfw_KeyCallback);
+    glfwSetCharCallback(window, ImGui_ImplGlfw_CharCallback);
+}
+
+bool    ImGui_ImplGlfwGL3_Init(GLFWwindow* window, bool install_callbacks, const char* glsl_version)
+{
+    g_Window = window;
+
+    // Store GL version string so we can refer to it later in case we recreate shaders.
+    if (glsl_version == NULL)
+        glsl_version = "#version 150";
+    IM_ASSERT((int)strlen(glsl_version) + 2 < IM_ARRAYSIZE(g_GlslVersion));
+    strcpy(g_GlslVersion, glsl_version);
+    strcat(g_GlslVersion, "\n");
+
+    // Setup back-end capabilities flags
+    ImGuiIO& io = ImGui::GetIO();
+    io.BackendFlags |= ImGuiBackendFlags_HasMouseCursors;   // We can honor GetMouseCursor() values (optional)
+    io.BackendFlags |= ImGuiBackendFlags_HasSetMousePos;    // We can honor io.WantSetMousePos requests (optional, rarely used)
+
+    // Keyboard mapping. ImGui will use those indices to peek into the io.KeyDown[] array.
+    io.KeyMap[ImGuiKey_Tab] = GLFW_KEY_TAB;
+    io.KeyMap[ImGuiKey_LeftArrow] = GLFW_KEY_LEFT;
+    io.KeyMap[ImGuiKey_RightArrow] = GLFW_KEY_RIGHT;
+    io.KeyMap[ImGuiKey_UpArrow] = GLFW_KEY_UP;
+    io.KeyMap[ImGuiKey_DownArrow] = GLFW_KEY_DOWN;
+    io.KeyMap[ImGuiKey_PageUp] = GLFW_KEY_PAGE_UP;
+    io.KeyMap[ImGuiKey_PageDown] = GLFW_KEY_PAGE_DOWN;
+    io.KeyMap[ImGuiKey_Home] = GLFW_KEY_HOME;
+    io.KeyMap[ImGuiKey_End] = GLFW_KEY_END;
+    io.KeyMap[ImGuiKey_Insert] = GLFW_KEY_INSERT;
+    io.KeyMap[ImGuiKey_Delete] = GLFW_KEY_DELETE;
+    io.KeyMap[ImGuiKey_Backspace] = GLFW_KEY_BACKSPACE;
+    io.KeyMap[ImGuiKey_Space] = GLFW_KEY_SPACE;
+    io.KeyMap[ImGuiKey_Enter] = GLFW_KEY_ENTER;
+    io.KeyMap[ImGuiKey_Escape] = GLFW_KEY_ESCAPE;
+    io.KeyMap[ImGuiKey_A] = GLFW_KEY_A;
+    io.KeyMap[ImGuiKey_C] = GLFW_KEY_C;
+    io.KeyMap[ImGuiKey_V] = GLFW_KEY_V;
+    io.KeyMap[ImGuiKey_X] = GLFW_KEY_X;
+    io.KeyMap[ImGuiKey_Y] = GLFW_KEY_Y;
+    io.KeyMap[ImGuiKey_Z] = GLFW_KEY_Z;
+
+    io.SetClipboardTextFn = ImGui_ImplGlfwGL3_SetClipboardText;
+    io.GetClipboardTextFn = ImGui_ImplGlfwGL3_GetClipboardText;
+    io.ClipboardUserData = g_Window;
+#ifdef _WIN32
+    io.ImeWindowHandle = glfwGetWin32Window(g_Window);
+#endif
+
+    // Load cursors
+    // FIXME: GLFW doesn't expose suitable cursors for ResizeAll, ResizeNESW, ResizeNWSE. We revert to arrow cursor for those.
+    g_MouseCursors[ImGuiMouseCursor_Arrow] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_TextInput] = glfwCreateStandardCursor(GLFW_IBEAM_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_ResizeAll] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_ResizeNS] = glfwCreateStandardCursor(GLFW_VRESIZE_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_ResizeEW] = glfwCreateStandardCursor(GLFW_HRESIZE_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_ResizeNESW] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
+    g_MouseCursors[ImGuiMouseCursor_ResizeNWSE] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
+
+    if (install_callbacks)
+        ImGui_ImplGlfw_InstallCallbacks(window);
+
+    return true;
+}
+
+void ImGui_ImplGlfwGL3_Shutdown()
+{
+    // Destroy GLFW mouse cursors
+    for (ImGuiMouseCursor cursor_n = 0; cursor_n < ImGuiMouseCursor_COUNT; cursor_n++)
+        glfwDestroyCursor(g_MouseCursors[cursor_n]);
+    memset(g_MouseCursors, 0, sizeof(g_MouseCursors));
+
+    // Destroy OpenGL objects
+    ImGui_ImplGlfwGL3_InvalidateDeviceObjects();
+}
+
+void ImGui_ImplGlfwGL3_NewFrame()
+{
+    if (!g_FontTexture)
+        ImGui_ImplGlfwGL3_CreateDeviceObjects();
+
+    ImGuiIO& io = ImGui::GetIO();
+
+    // Setup display size (every frame to accommodate for window resizing)
+    int w, h;
+    int display_w, display_h;
+    glfwGetWindowSize(g_Window, &w, &h);
+    glfwGetFramebufferSize(g_Window, &display_w, &display_h);
+    io.DisplaySize = ImVec2((float)w, (float)h);
+    io.DisplayFramebufferScale = ImVec2(w > 0 ? ((float)display_w / w) : 0, h > 0 ? ((float)display_h / h) : 0);
+
+    // Setup time step
+    double current_time =  glfwGetTime();
+    io.DeltaTime = g_Time > 0.0 ? (float)(current_time - g_Time) : (float)(1.0f/60.0f);
+    g_Time = current_time;
+
+    // Setup inputs
+    // (we already got mouse wheel, keyboard keys & characters from glfw callbacks polled in glfwPollEvents())
+    if (glfwGetWindowAttrib(g_Window, GLFW_FOCUSED))
+    {
+        // Set OS mouse position if requested (only used when ImGuiConfigFlags_NavEnableSetMousePos is enabled by user)
+        if (io.WantSetMousePos)
+        {
+            glfwSetCursorPos(g_Window, (double)io.MousePos.x, (double)io.MousePos.y);
+        }
+        else
+        {
+            double mouse_x, mouse_y;
+            glfwGetCursorPos(g_Window, &mouse_x, &mouse_y);
+            io.MousePos = ImVec2((float)mouse_x, (float)mouse_y);
+        }
+    }
+    else
+    {
+        io.MousePos = ImVec2(-FLT_MAX,-FLT_MAX);
+    }
+
+    for (int i = 0; i < 3; i++)
+    {
+        // If a mouse press event came, always pass it as "mouse held this frame", so we don't miss click-release events that are shorter than 1 frame.
+        io.MouseDown[i] = g_MouseJustPressed[i] || glfwGetMouseButton(g_Window, i) != 0;
+        g_MouseJustPressed[i] = false;
+    }
+
+    // Update OS/hardware mouse cursor if imgui isn't drawing a software cursor
+    if ((io.ConfigFlags & ImGuiConfigFlags_NoMouseCursorChange) == 0 && glfwGetInputMode(g_Window, GLFW_CURSOR) != GLFW_CURSOR_DISABLED)
+    {
+        ImGuiMouseCursor cursor = ImGui::GetMouseCursor();
+        if (io.MouseDrawCursor || cursor == ImGuiMouseCursor_None)
+        {
+            glfwSetInputMode(g_Window, GLFW_CURSOR, GLFW_CURSOR_HIDDEN);
+        }
+        else
+        {
+            glfwSetCursor(g_Window, g_MouseCursors[cursor] ? g_MouseCursors[cursor] : g_MouseCursors[ImGuiMouseCursor_Arrow]);
+            glfwSetInputMode(g_Window, GLFW_CURSOR, GLFW_CURSOR_NORMAL);
+        }
+    }
+
+    // Gamepad navigation mapping [BETA]
+    memset(io.NavInputs, 0, sizeof(io.NavInputs));
+    if (io.ConfigFlags & ImGuiConfigFlags_NavEnableGamepad)
+    {
+        // Update gamepad inputs
+        #define MAP_BUTTON(NAV_NO, BUTTON_NO)       { if (buttons_count > BUTTON_NO && buttons[BUTTON_NO] == GLFW_PRESS) io.NavInputs[NAV_NO] = 1.0f; }
+        #define MAP_ANALOG(NAV_NO, AXIS_NO, V0, V1) { float v = (axes_count > AXIS_NO) ? axes[AXIS_NO] : V0; v = (v - V0) / (V1 - V0); if (v > 1.0f) v = 1.0f; if (io.NavInputs[NAV_NO] < v) io.NavInputs[NAV_NO] = v; }
+        int axes_count = 0, buttons_count = 0;
+        const float* axes = glfwGetJoystickAxes(GLFW_JOYSTICK_1, &axes_count);
+        const unsigned char* buttons = glfwGetJoystickButtons(GLFW_JOYSTICK_1, &buttons_count);
+        MAP_BUTTON(ImGuiNavInput_Activate,   0);     // Cross / A
+        MAP_BUTTON(ImGuiNavInput_Cancel,     1);     // Circle / B
+        MAP_BUTTON(ImGuiNavInput_Menu,       2);     // Square / X
+        MAP_BUTTON(ImGuiNavInput_Input,      3);     // Triangle / Y
+        MAP_BUTTON(ImGuiNavInput_DpadLeft,   13);    // D-Pad Left
+        MAP_BUTTON(ImGuiNavInput_DpadRight,  11);    // D-Pad Right
+        MAP_BUTTON(ImGuiNavInput_DpadUp,     10);    // D-Pad Up
+        MAP_BUTTON(ImGuiNavInput_DpadDown,   12);    // D-Pad Down
+        MAP_BUTTON(ImGuiNavInput_FocusPrev,  4);     // L1 / LB
+        MAP_BUTTON(ImGuiNavInput_FocusNext,  5);     // R1 / RB
+        MAP_BUTTON(ImGuiNavInput_TweakSlow,  4);     // L1 / LB
+        MAP_BUTTON(ImGuiNavInput_TweakFast,  5);     // R1 / RB
+        MAP_ANALOG(ImGuiNavInput_LStickLeft, 0,  -0.3f,  -0.9f);
+        MAP_ANALOG(ImGuiNavInput_LStickRight,0,  +0.3f,  +0.9f);
+        MAP_ANALOG(ImGuiNavInput_LStickUp,   1,  +0.3f,  +0.9f);
+        MAP_ANALOG(ImGuiNavInput_LStickDown, 1,  -0.3f,  -0.9f);
+        #undef MAP_BUTTON
+        #undef MAP_ANALOG
+        if (axes_count > 0 && buttons_count > 0) 
+            io.BackendFlags |= ImGuiBackendFlags_HasGamepad; 
+        else 
+            io.BackendFlags &= ~ImGuiBackendFlags_HasGamepad;
+    }
+
+    // Start the frame. This call will update the io.WantCaptureMouse, io.WantCaptureKeyboard flag that you can use to dispatch inputs (or not) to your application.
+    ImGui::NewFrame();
+}
-- 
GitLab