<|start_filename|>Chapter02/Code/CH15Demo.cpp<|end_filename|>
#include "CH15Demo.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void CH15Demo::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);

	physicsSystem.RenderRandomColors = true;
	physicsSystem.ImpulseIteration = 8;
	physicsSystem.DoLinearProjection = true;
	size_imgui_window = true;

	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(12.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

void CH15Demo::ResetDemo() {
	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();

	bodies.clear();
	bodies.resize(2);

	bodies[0].type = RIGIDBODY_TYPE_BOX;
	bodies[0].position = vec3(0.5f, 6, 0);
#ifndef LINEAR_ONLY
	bodies[0].orientation = vec3(0.0f, 0.0f, 0.4f);
#endif

	bodies[1].type = RIGIDBODY_TYPE_BOX;
	bodies[1].position = vec3(0, 1, 0);
	bodies[1].mass = 5.0f;

	groundBox = RigidbodyVolume(RIGIDBODY_TYPE_BOX);
	groundBox.position = vec3(0, -0.5f, 0) * vec3(1, 0.5f, 1);
	groundBox.box.size = vec3(50, 1, 50) * 0.25f;
	groundBox.mass = 0.0f;
	groundBox.SynchCollisionVolumes();

	for (int i = 0; i < bodies.size(); ++i) {
		bodies[i].SynchCollisionVolumes();
		physicsSystem.AddRigidbody(&bodies[i]);
	}
	physicsSystem.AddRigidbody(&groundBox);
}

void CH15Demo::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 75));
	}

	ImGui::Begin("Chapter 15 Demo", 0, ImGuiWindowFlags_NoResize);

	ImGui::PushItemWidth(55);
	ImGui::SliderFloat("Porjection", &physicsSystem.LinearProjectionPercent, 0.2f, 0.8f);
	ImGui::SameLine();
	ImGui::PushItemWidth(55);
	ImGui::SliderFloat("Slop", &physicsSystem.PenetrationSlack, 0.01f, 0.1f);
	ImGui::SameLine();
	ImGui::PushItemWidth(55);
	ImGui::SliderInt("Iteration", &physicsSystem.ImpulseIteration, 1, 20);

	if (ImGui::Button("Reset")) {
		ResetDemo();
	}
	ImGui::SameLine();
	ImGui::Checkbox("Debug Render", &physicsSystem.DebugRender);
	ImGui::SameLine();
	ImGui::Checkbox("Linear Projection", &physicsSystem.DoLinearProjection);

	ImGui::End();
}

void CH15Demo::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();
}

void CH15Demo::Update(float dt) {
	DemoBase::Update(dt);


	physicsSystem.Update(dt);
}

<|start_filename|>Chapter01/Code/CH16Demo.h<|end_filename|>
#ifndef _H_CH16DEMO_
#define _H_CH16DEMO_

#include "DemoBase.h"
#include "Geometry3D.h"
#include "PhysicsSystem.h"
#include "Cloth.h"

class CH16Demo : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	Cloth cloth;
	OBB ground;
	std::vector<OBB> renderObjects;

	int num_part;
	float part_dist;
	float k;
	float d;

	bool size_imgui_window;
protected:
	void ResetDemo();
	float Random(float min, float max);
public:
	inline CH16Demo() : DemoBase(), size_imgui_window(true) { }

	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};

#endif

<|start_filename|>Chapter01/Code/Spring.h<|end_filename|>
#ifndef _H_SPRING_
#define _H_SPRING_

#include "Particle.h"


// F = -kx-bv
// k - spring "tightness" constant [negative to 0] rigid at 0
// x - displacement of spring from equalibrium
// b - constant (coefficient) dampening
// v - realtive velocity of points of spring

class Spring {
public:
	Particle* p1;
	Particle* p2;

	float k; // [-x to 0] higher = stiff sprint, lower = loose spring
	float restingLength;
	float b;
public:
	Spring(float _k, float _b, float len);
	void SetParticles(Particle* _p1, Particle* _p2);
	Particle* GetP1();
	Particle* GetP2();
	void SetConstants(float _k, float _b);
	void ApplyForce(float dt);
};

#endif

<|start_filename|>Chapter02/Code/FixedFunctionPrimitives.h<|end_filename|>
#ifndef _H_FIXED_FUNCTION_PRIMITIVES_
#define _H_FIXED_FUNCTION_PRIMITIVES_

#include "Geometry2D.h"
#include "Geometry3D.h"
#include "ObjLoader.h"
#include "Scene.h"
#include "matrices.h"

// 2D Geometry
void Render(const Point2D& point); 
void Render(const Line2D& line);
void Render(const Circle& circle);
void Render(const Rectangle2D& rect); 
void Render(const OrientedRectangle& rect);

// 3D Geometry
void Render(const Point& point);
void Render(const Line& line);
void Render(const Ray& ray);
void Render(const Sphere& sphere);
void Render(const AABB& aabb);
void RenderWithQuads(const AABB& aabb);
void Render(const OBB& obb);
void Render(const Plane& plane);
void Render(const Plane& plane, float scale);
void Render(const Triangle& triangle);
void Render(const Frustum& frustum);
void RenderNormals(const Frustum& frustum);
void Render(const CollisionManifold& manifold);

void Render(const Mesh& mesh);
void Render(const BVHNode& bvh);
void Render(const Model& model);

void Render(const std::vector<Line>& edges);

// Generic Primitives (All pointing up on Y axis, must be rotated)
void FixedFunctionSphere(int numDivisions, float radius);
void FixedFunctionCube(float extentsX, float extentsY, float extentsZ);
void FixedFunctionTorus(int majorSegments, int minorSegments, float majorRadius, float minorRadius);
void FixedFunctionCylinder(int slices, float height, float radius);
void FixedFunctionPlane(float size, int subdivs);
void FixedFunctionOrigin(bool depthTest, bool twoSided);
void FixedFunctionCone(int subdiv, float height, float radius);
void FixedFunctionCubeQuads(float extentsX, float extentsY, float extentsZ);

// Conveniance function
void FixedFunctionTorus(float outerRaidus, float innerRadius);
void FixedFunctionCylinder(float height, float radius);
void FixedFunctionCylinder(float height);
void FixedFunctionPlane(float size);
void FixedFunctionOrigin(bool depthTest);
void FixedFunctionCone(float height, float radius);
void FixedFunctionCone();
void FixedFunctionOrigin();
void FixedFunctionTorus();
void FixedFunctionSphere();
void FixedFunctionCube();
void FixedFunctionCylinder();
void FixedFunctionPlane();

// Helper functions
void FixedFunctionSubdivTetrahedron(float* a, float* b, float* c, int div, float r);
void FixedFunctionSubdivCone(float *v1, float *v2, int subdiv, float height, float radius);

#endif


<|start_filename|>Chapter01/Code/main-win32.cpp<|end_filename|>
#if _DEBUG
	#define _CRTDBG_MAP_ALLOC
	#include <stdlib.h>
	#include <crtdbg.h>

	#include <float.h>
	unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
#endif

// If you don't want a console window define this

// Only change if you know what you are doing
#define MAIN_WIN_32_WINDOW_CLASS L"Win32GLSandbox"
#define ENABLE_RESIZE

// Should not need to edit below this line
// -------------------------------------------------- \\

#define WIN32_LEAN_AND_MEAN 
#define WIN32_EXTRA_LEAN
#include <windows.h>
#include <windowsx.h>

#include "glad/glad.h"

#include <string>
#include <iostream>
#include <cstdio>
#include <conio.h>

#include "IWindow.h"
#include "matrices.h"
#include "imgui/imgui.h"
#include "imgui/imgui_implementation.h"
#include "imgui/ImGuizmo.h"

// I set all of these settings in the "Project Settings" of visual studio
// #pragma comment(lib, "opengl32.lib") 
#if _DEBUG
#pragma comment(linker,"/SUBSYSTEM:CONSOLE")
#else 
#pragma comment(linker,"/SUBSYSTEM:WINDOWS")
#endif

RECT windowRect;
RECT clientRect;
RECT borderRect;

LRESULT CALLBACK WndProc(HWND hwnd, UINT iMsg, WPARAM wParam, LPARAM lParam);
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR szCmdLine, int iCmdShow);
bool CheckIfAlreadyRunning(IWindow* pWindowInstance);
HWND MakeNewWindow(IWindow* pWindowInstance, HINSTANCE hInstance, LPSTR szCmdLine);
HGLRC OpenGLBindContext(HDC hdc);
void OpenGLUnbindContext(HWND hwnd, HDC hdc, HGLRC hglrc);
void UpdateFullscreen(IWindow* pWindowInstance, HWND hwnd, HDC hdc);
void SetDisplayMode(int width, int height, int bpp, int refreshRate);
int WParamToKeydef(WPARAM param, bool shiftDown);
double GetMilliseconds();

#define NO_RESIZE_STYLE (WS_OVERLAPPED | WS_CAPTION | WS_SYSMENU | WS_MINIMIZEBOX | WS_MAXIMIZEBOX)
#define NORMAL_STYLE (WS_VISIBLE | WS_OVERLAPPEDWINDOW)

// https://www.opengl.org/archives/resources/code/samples/win32_tutorial/
// https://msdn.microsoft.com/en-us/library/bb384843.aspx?f=255&MSPPError=-2147217396
// https://gist.github.com/gszauer/5718416
// Glad loader: http://glad.dav1d.de/
// Glad tutorial: https://github.com/Dav1dde/glad
// Gl3w loader: https://github.com/skaslev/gl3w
// GLLoadGen loader: https://bitbucket.org/alfonse/glloadgen/wiki/Home

int main(int argc, const char** argv) {
	IWindow* pWindowInstance = IWindow::GetInstance();

	if (pWindowInstance->GetQuitFlag()) {
		std::cout << "Something went wrong, could not initialize!\n";
		getch();
	}
	else if (CheckIfAlreadyRunning(pWindowInstance)) {
		std::cout << "Error, only one instance of application is allowed to run\n";
		getch();
	}
	else {
		WinMain(GetModuleHandle(NULL), NULL, GetCommandLineA(), SW_SHOWDEFAULT);
	}
	return 0;
}

HWND MakeNewWindow(IWindow* pWindowInstance, HINSTANCE hInstance, LPSTR szCmdLine) {
	std::wstring MAIN_WIN32_WINDOW_NAME;
	pWindowInstance->GetAndResetTitleDirtyFlag();
	std::string title(pWindowInstance->GetTitle());
	MAIN_WIN32_WINDOW_NAME = std::wstring(title.begin(), title.end());

	return CreateWindowEx(0, MAIN_WIN_32_WINDOW_CLASS, MAIN_WIN32_WINDOW_NAME.c_str(),
#ifdef ENABLE_RESIZE
		NORMAL_STYLE
#else
		NO_RESIZE_STYLE
#endif
		, windowRect.left, windowRect.top,
		windowRect.right - windowRect.left, windowRect.bottom - windowRect.top,
		NULL, NULL, hInstance, szCmdLine);
}

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR szCmdLine, int iCmdShow) {
	IWindow* pWindowInstance = IWindow::GetInstance();

	if (CheckIfAlreadyRunning(pWindowInstance)) {
		MessageBox(NULL, L"Only one instance of application is allowed to run", NULL, NULL);
		return FALSE;
	}
	
	int width = pWindowInstance->GetWidth();
	int height = pWindowInstance->GetHeight();

	HINSTANCE hinstance = hInstance;

	WNDCLASSEX wndclass;
	ZeroMemory(&wndclass, sizeof(WNDCLASSEX));
	wndclass.cbSize = sizeof(WNDCLASSEX);
	wndclass.style = CS_HREDRAW | CS_VREDRAW;
	wndclass.lpfnWndProc = WndProc;
	wndclass.cbClsExtra = 0;
	wndclass.cbWndExtra = 0;
	wndclass.hInstance = hInstance;
	wndclass.hIcon = LoadIcon(NULL, IDI_APPLICATION);
	wndclass.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
	wndclass.hCursor = LoadCursor(NULL, IDC_ARROW);
	wndclass.hbrBackground = (HBRUSH)(COLOR_BTNFACE + 1);
	wndclass.lpszMenuName = 0;
	wndclass.lpszClassName = MAIN_WIN_32_WINDOW_CLASS;
	RegisterClassEx(&wndclass);

	SetRect(&windowRect, (GetSystemMetrics(SM_CXSCREEN) / 2) - (width / 2), 
						 (GetSystemMetrics(SM_CYSCREEN) / 2) - (height / 2), 
						 (GetSystemMetrics(SM_CXSCREEN) / 2) + (width / 2), 
						 (GetSystemMetrics(SM_CYSCREEN) / 2) + (height / 2));
	AdjustWindowRectEx(&windowRect, 
#ifdef ENABLE_RESIZE
		NORMAL_STYLE
#else
		NO_RESIZE_STYLE
#endif
		, FALSE, 0);

	HWND hwnd = MakeNewWindow(pWindowInstance, hInstance, szCmdLine);

	HDC hdc = GetDC(hwnd);
	HGLRC hglrc = OpenGLBindContext(hdc);

	if (!gladLoadGL()) {
		std::cout << "Could not instantiate GLAD OpenGL 2.1 context\n";
		exit(-1);
	}
	else if (GLVersion.major < 2) {
		std::cout << "Your system doesn't support OpenGL >= 2!\n";
		return -1;
	}

	std::cout << "OpenGL Context: " << GLVersion.major << ", " << GLVersion.minor << "\n";
	std::cout << "OpenGL version: " << glGetString(GL_VERSION) << "\n";
	std::cout << "GLSL Version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << "\n";

	ImGui_Implementation_Init(hwnd);
	pWindowInstance->OnInitialize();

	bool fullscreen = pWindowInstance->GetFullScreen();
	if (fullscreen) {
		UpdateFullscreen(pWindowInstance, hwnd, hdc);
	}
	else {
		pWindowInstance->OnResize(width, height);
	}
	
	ShowWindow(hwnd, SW_SHOW);
	UpdateWindow(hwnd);
	GetClientRect(hwnd, &clientRect);
	GetWindowRect(hwnd, &windowRect);
	GetWindowRect(hwnd, &borderRect);

	MSG msg;
	DWORD next_game_tick = GetTickCount();
	int sleep_time = 0;
	double lastTime = GetMilliseconds();
	double fixed_millis = pWindowInstance->GetFixedFPS() / 1000.0; 
	double fixed_ellapsed = 0.0;

	pWindowInstance->MarkAsShown();

	while (!pWindowInstance->GetQuitFlag()) {
		// If or while? Not sure if all messages should process at once or not
		while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE)) {
			if (msg.message == WM_QUIT) {
				break;
			}
			TranslateMessage(&msg);
			DispatchMessage(&msg);
		}
		ImGui_Implementation_NewFrame();
		ImGuizmo::BeginFrame();

		// Potentially change title
		if (pWindowInstance->GetAndResetTitleDirtyFlag()) {
			std::wstring MAIN_WIN32_WINDOW_NAME;
			std::string title(pWindowInstance->GetTitle());
			MAIN_WIN32_WINDOW_NAME = std::wstring(title.begin(), title.end());
			SetWindowText(hwnd, MAIN_WIN32_WINDOW_NAME.c_str());
		}

		// Potentially toggle full-screen
		if (fullscreen != pWindowInstance->GetFullScreen()) {
			UpdateFullscreen(pWindowInstance, hwnd, hdc);
			fullscreen = pWindowInstance->GetFullScreen();
		}

		if (!fullscreen) {
			int windowWidth = borderRect.right - borderRect.left;
			int windowHeight = borderRect.bottom - borderRect.top;
			int clientWidth = clientRect.right - clientRect.left;
			int clientHeight = clientRect.bottom - clientRect.top;
			int borderWidth = windowWidth - clientWidth;
			int borderHeight = windowHeight - clientHeight;

			if (clientWidth != pWindowInstance->GetWidth() || clientHeight != pWindowInstance->GetHeight()) {
				SetWindowPos(hwnd, 0, windowRect.left, windowRect.top, pWindowInstance->GetWidth() + borderWidth, pWindowInstance->GetHeight() + borderHeight, SWP_NOZORDER);
			}
		}

		double time = GetMilliseconds();
		float deltaTime = float(time - lastTime) * 0.001f;
		lastTime = time;

		pWindowInstance->OnUpdate(deltaTime);

		fixed_ellapsed += deltaTime;
		while (fixed_ellapsed > fixed_millis) {
			pWindowInstance->OnFixedUpdate(fixed_millis);
			fixed_ellapsed -= fixed_millis;
		}

		pWindowInstance->OnRender();
		ImGui::Render();
		SwapBuffers(hdc);

		// Regulate FPS
		int SKIP_TICKS = 1000 / pWindowInstance->GetTargetFPS();
		next_game_tick += SKIP_TICKS;
		sleep_time = next_game_tick - GetTickCount();
		if (sleep_time >= 0) {
			Sleep(sleep_time);
		}
	}

	pWindowInstance->OnShutdown();
	ImGui_Implementation_Shutdown();
	OpenGLUnbindContext(hwnd, hdc, hglrc);
	
	CleanupMemory(pWindowInstance);

#if _DEBUG
	_CrtDumpMemoryLeaks();
#endif

	return (int)msg.wParam;
}

LRESULT CALLBACK WndProc(HWND hwnd, UINT iMsg, WPARAM wParam, LPARAM lParam) {
	ImGui_Implementation_WndProcHandler(hwnd, iMsg, wParam, lParam);

	IWindow* pWindowInstance = IWindow::GetInstance();
	int width, height;

	static bool shiftDown = false;
	static bool capsOn = false;

	switch (iMsg) {
	// Window Lifecycle events
	case WM_CLOSE:
		DestroyWindow(hwnd);
		pWindowInstance->Close();
		break;
	case WM_DESTROY:
		PostQuitMessage(0);
		break;
	// Handle resize event
	case WM_SIZE:
		width = LOWORD(lParam);
		height = HIWORD(lParam);

		GetClientRect(hwnd, &clientRect);
		GetWindowRect(hwnd, &borderRect);
		pWindowInstance->Resize(width, height);
		if (pWindowInstance->WasWindowShown()) {
			pWindowInstance->OnResize(width, height);
		}

		break;
	// Handle Mouse Events
	case WM_MOUSEMOVE:
		if (!ImGui_Implementation_HasMouse()) {
			pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
		}
		break;
	case WM_LBUTTONDOWN:
		if (!ImGui_Implementation_HasMouse()) {
			//pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			if (!ImGuizmo::IsUsing() && !ImGuizmo::IsOver()) {
				pWindowInstance->OnMouseDown(MOUSE_LEFT);
			}
		}
		break;
	case WM_LBUTTONUP:
		if (!ImGui_Implementation_HasMouse()) {
			//pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			if (!ImGuizmo::IsUsing()) {
				pWindowInstance->OnMouseUp(MOUSE_LEFT);
			}
		}
		break;
	case WM_RBUTTONDOWN:
		if (!ImGui_Implementation_HasMouse()) {
			//pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			if (!ImGuizmo::IsUsing() && !ImGuizmo::IsOver()) {
				pWindowInstance->OnMouseDown(MOUSE_RIGHT);
			}
		}
		break;
	case WM_RBUTTONUP:
		if (!ImGui_Implementation_HasMouse()) {
			//pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			if (!ImGuizmo::IsUsing()) {
				pWindowInstance->OnMouseUp(MOUSE_RIGHT);
			}
		}
		break;
	case WM_MBUTTONDOWN:
		if (!ImGui_Implementation_HasMouse()) {
			//pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			if (!ImGuizmo::IsUsing() && !ImGuizmo::IsOver()) {
				pWindowInstance->OnMouseDown(MOUSE_MIDDLE);
			}
		}
		break;
	case WM_MBUTTONUP:
		if (!ImGui_Implementation_HasMouse()) {
			if (!ImGuizmo::IsOver()) {
				pWindowInstance->OnMouseMove(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
			}
			if (!ImGuizmo::IsUsing()) {
				pWindowInstance->OnMouseUp(MOUSE_MIDDLE);
			}
		}
		break;
	// Handle keyboard events
	case WM_SYSKEYUP:
		break;
	case WM_SYSKEYDOWN:
		break;
	case WM_KEYDOWN:
		if (wParam == VK_SHIFT || wParam == VK_LSHIFT || wParam == VK_RSHIFT)
			shiftDown = true;

		if (wParam == VK_CAPITAL)
			capsOn = !capsOn;

		if (!ImGui_Implementation_HasKeyboard()) {
			pWindowInstance->OnKeyDown(WParamToKeydef(wParam, shiftDown ^ capsOn));
		}
		break;
	case WM_KEYUP:
		if (wParam == VK_SHIFT || wParam == VK_LSHIFT || wParam == VK_RSHIFT)
			shiftDown = false;

		if (!ImGui_Implementation_HasKeyboard()) {
			pWindowInstance->OnKeyUp(WParamToKeydef(wParam, shiftDown ^ capsOn));
		}
		break;
	}
	
	return DefWindowProc(hwnd, iMsg, wParam, lParam);
}

bool CheckIfAlreadyRunning(IWindow* pWindowInstance) {
	std::wstring MAIN_WIN32_WINDOW_NAME;
	std::string title(pWindowInstance->GetTitle());
	MAIN_WIN32_WINDOW_NAME = std::wstring(title.begin(), title.end());

	HWND hWnd = FindWindow(MAIN_WIN_32_WINDOW_CLASS, MAIN_WIN32_WINDOW_NAME.c_str());

	if (hWnd) {
		if (IsIconic(hWnd)) {
			ShowWindow(hWnd, SW_RESTORE);
		}
		SetForegroundWindow(hWnd);
		return true;
	}

	return false;
}

HGLRC OpenGLBindContext(HDC hdc) {
	PIXELFORMATDESCRIPTOR pfd;
	ZeroMemory(&pfd, sizeof(PIXELFORMATDESCRIPTOR));

	pfd.nSize = sizeof(PIXELFORMATDESCRIPTOR);
	pfd.nVersion = 1;
	pfd.dwFlags = PFD_SUPPORT_OPENGL | PFD_DRAW_TO_WINDOW | PFD_DOUBLEBUFFER;
	pfd.iPixelType = PFD_TYPE_RGBA;
	pfd.cColorBits = 24;
	pfd.cDepthBits = 32;
	pfd.cStencilBits = 8;
	pfd.iLayerType = PFD_MAIN_PLANE;

	int pixelFormat = ChoosePixelFormat(hdc, &pfd);
	SetPixelFormat(hdc, pixelFormat, &pfd);

	HGLRC context = wglCreateContext(hdc);
	wglMakeCurrent(hdc, context);
	return context;
}

void OpenGLUnbindContext(HWND hwnd, HDC hdc, HGLRC hglrc) {
	wglMakeCurrent(NULL, NULL);
	wglDeleteContext(hglrc);
	ReleaseDC(hwnd, hdc);
}

void UpdateFullscreen(IWindow* pWindowInstance, HWND hwnd, HDC hdc) {
	if (pWindowInstance->GetFullScreen()) {
		GetWindowRect(hwnd, &windowRect);
		SetDisplayMode(GetSystemMetrics(SM_CXSCREEN), GetSystemMetrics(SM_CYSCREEN), GetDeviceCaps(hdc, BITSPIXEL), GetDeviceCaps(hdc, VREFRESH));
		SetWindowLongPtr(hwnd, GWL_STYLE, WS_POPUP);
		SetWindowPos(hwnd, HWND_TOPMOST, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_SHOWWINDOW | SWP_NOMOVE | SWP_NOSIZE);
		// Not 100% sure why, but setting height to GetSystemMetrics(SM_CYSCREEN) causes flickering!
		// Adding 1 to it seems to solve it. Maybe something to do with running in a scaled WM???
		SetWindowPos(hwnd, 0, 0, 0, GetSystemMetrics(SM_CXSCREEN), GetSystemMetrics(SM_CYSCREEN) + 1, SWP_NOZORDER);
		ShowCursor(FALSE);
	}
	else {
		SetDisplayMode(0, 0, 0, 0);
		SetWindowLongPtr(hwnd, GWL_STYLE, 
#ifdef ENABLE_RESIZE
			NORMAL_STYLE
#else
			NO_RESIZE_STYLE
#endif	
		);
		SetWindowPos(hwnd, HWND_NOTOPMOST, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_SHOWWINDOW | SWP_NOMOVE | SWP_NOSIZE);
		int iWindowWidth = windowRect.right - windowRect.left;
		int iWindowHeight = windowRect.bottom - windowRect.top;
		SetWindowPos(hwnd, 0, windowRect.left, windowRect.top, iWindowWidth, iWindowHeight, SWP_NOZORDER);
		ShowCursor(TRUE);
	}

	GetClientRect(hwnd, &clientRect);
	GetWindowRect(hwnd, &borderRect);
}

void SetDisplayMode(int width, int height, int bpp, int refreshRate) {
	if (width == 0 && height == 0 && bpp == 0 && refreshRate == 0) {
		ChangeDisplaySettings(NULL, 0);
		return;
	}

	DEVMODE dm;
	ZeroMemory(&dm, sizeof(DEVMODE));
	dm.dmSize = sizeof(DEVMODE);

	int i = 0;
	while (EnumDisplaySettings(NULL, i++, &dm)) {
		if (dm.dmPelsWidth == width && dm.dmPelsHeight == height &&
			dm.dmBitsPerPel == bpp && dm.dmDisplayFrequency == refreshRate) {
			if (ChangeDisplaySettings(&dm, CDS_TEST) == DISP_CHANGE_SUCCESSFUL) {
				ChangeDisplaySettings(&dm, CDS_FULLSCREEN);
				return;
			}
		}
	}
}

#define REMAP(VIR_KEY, NEW_KEY) \
	case VIR_KEY: return NEW_KEY
#define SHIFTMAP(VIR_KEY, SHIFT_DOWN, SHIFT_UP) \
	case VIR_KEY: return shiftDown? SHIFT_DOWN :  SHIFT_UP

int WParamToKeydef(WPARAM param, bool shiftDown) {
	switch (param)
	{
		REMAP(VK_PAUSE, KEY_PAUSE);
		REMAP(VK_SCROLL, KEY_SCROLL_LOCK);
		REMAP(VK_PRINT, KEY_PRINT);
		REMAP(VK_ESCAPE, KEY_ESCAPE);
		case VK_BACK: 
		case VK_F16: // CTRL + BACKSPACE
			return KEY_BACKSPACE;
		SHIFTMAP(VK_TAB, KEY_BACK_TAB, KEY_TAB);
		REMAP(VK_RETURN, KEY_RETURN);
		REMAP(VK_CAPITAL, KEY_CAPS_LOCK);
		REMAP(VK_SHIFT, KEY_SHIFT);
		REMAP(VK_LSHIFT, KEY_SHIFT);
		REMAP(VK_RSHIFT, KEY_SHIFT);
		REMAP(VK_CONTROL, KEY_CTRL);
		REMAP(VK_RCONTROL, KEY_CTRL);
		REMAP(VK_LCONTROL, KEY_CTRL);
		REMAP(VK_MENU, KEY_ALT);
		REMAP(VK_LMENU, KEY_ALT);
		REMAP(VK_RMENU, KEY_ALT);
		REMAP(VK_APPS, KEY_MENU);
		REMAP(VK_LWIN, KEY_HYPER);
		REMAP(VK_RWIN, KEY_HYPER);
		REMAP(VK_BROWSER_SEARCH, KEY_SEARCH);
		REMAP(VK_INSERT, KEY_INSERT);
		REMAP(VK_HOME, KEY_HOME);
		REMAP(VK_PRIOR, KEY_PG_UP);
		REMAP(VK_DELETE, KEY_DELETE);
		REMAP(VK_END, KEY_END);
		REMAP(VK_NEXT, KEY_PG_DOWN);
		REMAP(VK_LEFT, KEY_LEFT_ARROW);
		REMAP(VK_RIGHT, KEY_RIGHT_ARROW);
		REMAP(VK_UP, KEY_UP_ARROW);
		REMAP(VK_DOWN, KEY_DOWN_ARROW);
		REMAP(VK_NUMLOCK, KEY_NUM_LOCK);
		REMAP(VK_ADD, KEY_KP_PLUS);
		REMAP(VK_SUBTRACT, KEY_KP_MINUS);
		REMAP(VK_MULTIPLY, KEY_KP_MULTIPLY);
		REMAP(VK_DIVIDE, KEY_KP_DIVIDE);
		REMAP(VK_NUMPAD0, KEY_KP_INSERT);
		REMAP(VK_NUMPAD1, KEY_KP_END);
		REMAP(VK_NUMPAD2, KEY_KP_DOWN);
		REMAP(VK_NUMPAD3, KEY_KP_PG_DOWN);
		REMAP(VK_NUMPAD4, KEY_KP_LEFT);
		REMAP(VK_NUMPAD5, KEY_KP_FIVE);
		REMAP(VK_NUMPAD6, KEY_KP_RIGHT);
		REMAP(VK_NUMPAD7, KEY_KP_HOME);
		REMAP(VK_NUMPAD8, KEY_KP_UP);
		REMAP(VK_NUMPAD9, KEY_KP_PG_UP);
		REMAP(VK_DECIMAL, KEY_KP_DELETE);
		REMAP(VK_F1, KEY_F1);
		REMAP(VK_F2, KEY_F2);
		REMAP(VK_F3, KEY_F3);
		REMAP(VK_F4, KEY_F3);
		REMAP(VK_F5, KEY_F3);
		REMAP(VK_F6, KEY_F3);
		REMAP(VK_F7, KEY_F3);
		REMAP(VK_F8, KEY_F4);
		REMAP(VK_F9, KEY_F5);
		REMAP(VK_F10, KEY_F10);
		REMAP(VK_F11, KEY_F11);
		REMAP(VK_F12, KEY_F12);
		REMAP(VK_SPACE, KEY_SPACE);
		SHIFTMAP(0x30, KEY_RIGHT_PARENTHESIS, KEY_ZERO);
		SHIFTMAP(0x31, KEY_EXCLAM, KEY_ONE);
		SHIFTMAP(0x32, KEY_AT, KEY_TWO);
		SHIFTMAP(0x33, KEY_NUMBER, KEY_THREE);
		SHIFTMAP(0x34, KEY_DOLLAR, KEY_FOUR);
		SHIFTMAP(0x35, KEY_PERCENT, KEY_FIVE);
		SHIFTMAP(0x36, KEY_CIRCUMFLEX, KEY_SIX);
		SHIFTMAP(0x37, KEY_AMPERSAND, KEY_SEVEN);
		SHIFTMAP(0x38, KEY_ASTERISK, KEY_EIGHT);
		SHIFTMAP(0x39, KEY_LEFT_PARENTHESIS, KEY_NINE);
		SHIFTMAP(VK_OEM_PLUS, KEY_EQUAL, KEY_PLUS);
		SHIFTMAP(VK_OEM_COMMA, KEY_LESS_THAN, KEY_COMMA);
		SHIFTMAP(VK_OEM_MINUS, KEY_UNDERSCORE, KEY_MINUS);
		SHIFTMAP(VK_OEM_PERIOD, KEY_GREATER_THAN, KEY_PERIOD);
		SHIFTMAP(VK_OEM_1, KEY_COLON, KEY_SEMICOLON);
		SHIFTMAP(VK_OEM_2, KEY_QUESTION, KEY_SLASH);
		SHIFTMAP(VK_OEM_3, KEY_TILDE, KEY_GRAVE);
		SHIFTMAP(VK_OEM_4, KEY_LEFT_BRACE, KEY_LEFT_BRACKET);
		SHIFTMAP(VK_OEM_5, KEY_BAR, KEY_BACK_SLASH);
		SHIFTMAP(VK_OEM_6, KEY_RIGHT_BRACE, KEY_RIGHT_BRACKET);
		SHIFTMAP(VK_OEM_7, KEY_QUOTE, KEY_APOSTROPHE);
		SHIFTMAP(0x41, KEY_CAPITAL_A, KEY_A);
		SHIFTMAP(0x42, KEY_CAPITAL_B, KEY_B);
		SHIFTMAP(0x43, KEY_CAPITAL_C, KEY_C);
		SHIFTMAP(0x44, KEY_CAPITAL_D, KEY_D);
		SHIFTMAP(0x45, KEY_CAPITAL_E, KEY_E);
		SHIFTMAP(0x46, KEY_CAPITAL_F, KEY_F);
		SHIFTMAP(0x47, KEY_CAPITAL_G, KEY_G);
		SHIFTMAP(0x48, KEY_CAPITAL_H, KEY_H);
		SHIFTMAP(0x49, KEY_CAPITAL_I, KEY_I);
		SHIFTMAP(0x4A, KEY_CAPITAL_J, KEY_J);
		SHIFTMAP(0x4B, KEY_CAPITAL_K, KEY_K);
		SHIFTMAP(0x4C, KEY_CAPITAL_L, KEY_L);
		SHIFTMAP(0x4D, KEY_CAPITAL_M, KEY_M);
		SHIFTMAP(0x4E, KEY_CAPITAL_N, KEY_N);
		SHIFTMAP(0x4F, KEY_CAPITAL_O, KEY_O);
		SHIFTMAP(0x50, KEY_CAPITAL_P, KEY_P);
		SHIFTMAP(0x51, KEY_CAPITAL_Q, KEY_Q);
		SHIFTMAP(0x52, KEY_CAPITAL_R, KEY_R);
		SHIFTMAP(0x53, KEY_CAPITAL_S, KEY_S);
		SHIFTMAP(0x54, KEY_CAPITAL_T, KEY_T);
		SHIFTMAP(0x55, KEY_CAPITAL_U, KEY_U);
		SHIFTMAP(0x56, KEY_CAPITAL_V, KEY_V);
		SHIFTMAP(0x57, KEY_CAPITAL_W, KEY_W);
		SHIFTMAP(0x58, KEY_CAPITAL_X, KEY_X);
		SHIFTMAP(0x59, KEY_CAPITAL_Y, KEY_Y);
		SHIFTMAP(0x5A, KEY_CAPITAL_Z, KEY_Z);
	}

	return KEY_NONE;
}


double GetMilliseconds() {
	static LARGE_INTEGER s_frequency;
	static BOOL s_use_qpc = QueryPerformanceFrequency(&s_frequency);
	if (s_use_qpc) {
		LARGE_INTEGER now;
		QueryPerformanceCounter(&now);
		return (double)((1000LL * now.QuadPart) / s_frequency.QuadPart);
	}
	else {
		return GetTickCount();
	}
}


<|start_filename|>Chapter05/Physics/Geometry3D.h<|end_filename|>
#ifndef _H_GEOMETRY_3D_
#define _H_GEOMETRY_3D_

#include "vectors.h"
#include "matrices.h"

#ifndef NO_EXTRAS
#include <ostream>
#endif 

typedef vec3 Point;

typedef struct Line {
	Point start;
	Point end;

	inline Line() {}
	inline Line(const Point& s, const Point& e) :
		start(s), end(e) { }
} Line;

typedef struct Ray {
	Point origin;
	vec3 direction;

	inline Ray() : direction(0.0f, 0.0f, 1.0f) {}
	inline Ray(const Point& o, const vec3& d) :
		origin(o), direction(d) {
		NormalizeDirection();
	}
	inline void NormalizeDirection() {
		Normalize(direction);
	}
} Ray;

typedef struct Sphere {
	Point position;
	float radius;

	inline Sphere() : radius(1.0f) { }
	inline Sphere(const Point& p, float r) :
		position(p), radius(r) { }
} Sphere;

typedef struct AABB {
	Point position;
	vec3 size; // HALF SIZE!

	inline AABB() : size(1, 1, 1) { }
	inline AABB(const Point& p, const vec3& s) :
		position(p), size(s) { }
} AABB;

typedef struct OBB {
	Point position;
	vec3 size; // HALF SIZE!
	mat3 orientation;

	inline OBB() : size(1, 1, 1) { }
	inline OBB(const Point& p, const vec3& s) :
		position(p), size(s) { }
	inline OBB(const Point& p, const vec3& s, const mat3& o) :
		position(p), size(s), orientation(o) { }
} OBB;

typedef struct Plane {
	vec3 normal;
	float distance;

	inline Plane() : normal(1, 0, 0) { }
	inline Plane(const vec3& n, float d) :
		normal(n), distance(d) { }
} Plane;

typedef struct Triangle {
	union {
		struct {
			Point a;
			Point b;
			Point c;
		};
#ifndef NO_EXTRAS
		struct {
			Point p1;
			Point p2;
			Point p3;
		};
#endif 

		Point points[3];
		float values[9];
	};
	
	inline Triangle() { }
	inline Triangle(const Point& _p1, const Point& _p2, const Point& _p3) :
		a(_p1), b(_p2), c(_p3) { }
} Triangle;

typedef struct BVHNode {
	AABB bounds;
	BVHNode* children;
	int numTriangles;
	int* triangles;

	BVHNode() : children(0), numTriangles(0), triangles(0) {}
} BVHNode;

typedef struct Mesh {
	int numTriangles;
	union {
		Triangle* triangles;
		Point* vertices;
		float* values;
	};
	BVHNode* accelerator;

	Mesh() : numTriangles(0), values(0), accelerator(0) {}
} Mesh;

class Model {
protected:
	Mesh* content;
	AABB bounds;
public:
	vec3 position;
	vec3 rotation;
	
	Model* parent;

	inline Model() : parent(0), content(0) { }
	inline Mesh* GetMesh() const {
		return content;
	}
	inline AABB GetBounds() const {
		return bounds;
	}

	void SetContent(Mesh* mesh);
};

typedef struct Interval {
	float min;
	float max;
} Interval;

#ifndef NO_EXTRAS
typedef vec3 Point3D;
typedef Line Line3D;
typedef Ray Ray3D;
typedef AABB Rectangle3D;
typedef Interval Interval3D;
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape);
std::ostream& operator<<(std::ostream& os, const Ray& shape);
std::ostream& operator<<(std::ostream& os, const Sphere& shape);
std::ostream& operator<<(std::ostream& os, const AABB& shape);
std::ostream& operator<<(std::ostream& os, const OBB& shape);
std::ostream& operator<<(std::ostream& os, const Plane& shape);
std::ostream& operator<<(std::ostream& os, const Triangle& shape);
#endif 

float Length(const Line& line);
float LengthSq(const Line& line);
Ray FromPoints(const Point& from, const Point& to);
vec3 GetMin(const AABB& aabb);
vec3 GetMax(const AABB& aabb);
AABB FromMinMax(const vec3& min, const vec3& max);
float PlaneEquation(const Point& point, const Plane& plane);
#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point);
#endif

bool PointInSphere(const Point& point, const Sphere& sphere);
bool PointInAABB(const Point& point, const AABB& aabb);
bool PointInOBB(const Point& point, const OBB& obb);
bool PointOnPlane(const Point& point, const Plane& plane);
bool PointOnLine(const Point& point, const Line& line);
bool PointOnRay(const Point& point, const Ray& ray);

#ifndef NO_EXTRAS
bool PointInPlane(const Point& point, const Plane& plane);
bool PointInLine(const Point& point, const Line& line);
bool PointInRay(const Point& point, const Ray& ray);
bool ContainsPoint(const Sphere& sphere, const Point& point);
bool ContainsPoint(const Point& point, const Sphere& sphere);
bool ContainsPoint(const AABB& aabb, const Point& point);
bool ContainsPoint(const Point& point, const AABB& aabb);
bool ContainsPoint(const Point& point, const OBB& obb);
bool ContainsPoint(const OBB& obb, const Point& point);
bool ContainsPoint(const Point& point, const Plane& plane);
bool ContainsPoint(const Plane& plane, const Point& point);
bool ContainsPoint(const Point& point, const Line& line);
bool ContainsPoint(const Line& line, const Point& point);
bool ContainsPoint(const Point& point, const Ray& ray);
bool ContainsPoint(const Ray& ray, const Point& point);
#endif

Point ClosestPoint(const Sphere& sphere, const Point& point);
Point ClosestPoint(const AABB& aabb, const Point& point);
Point ClosestPoint(const OBB& obb, const Point& point);
Point ClosestPoint(const Plane& plane, const Point& point);
Point ClosestPoint(const Line& line, const Point& point);
Point ClosestPoint(const Ray& ray, const Point& point);

#ifndef NO_EXTRAS
Point ClosestPoint(const Point& point, const Sphere& sphere);
Point ClosestPoint(const Point& point, const AABB& aabb);
Point ClosestPoint(const Point& point, const OBB& obb);
Point ClosestPoint(const Point& point, const Plane& plane);
Point ClosestPoint(const Point& point, const Line& line);
Point ClosestPoint(const Point& point, const Ray& ray);
Point ClosestPoint(const Point& p, const Triangle& t);
#endif

Interval GetInterval(const AABB& aabb, const vec3& axis);
Interval GetInterval(const OBB& obb, const vec3& axis);
Interval GetInterval(const Triangle& triangle, const vec3& axis);

bool OverlapOnAxis(const AABB& aabb, const OBB& obb, const vec3& axis);
bool OverlapOnAxis(const OBB& obb1, const OBB& obb2, const vec3& axis);
bool OverlapOnAxis(const AABB& aabb, const Triangle& triangle, const vec3& axis);
bool OverlapOnAxis(const OBB& obb, const Triangle& triangle, const vec3& axis);
bool OverlapOnAxis(const Triangle& t1, const Triangle& t2, const vec3& axis);

bool SphereSphere(const Sphere& s1, const Sphere& s2);
bool SphereAABB(const Sphere& sphere, const AABB& aabb);
bool SphereOBB(const Sphere& sphere, const OBB& obb);
bool SpherePlane(const Sphere& sphere, const Plane& plane);
bool AABBAABB(const AABB& aabb1, const AABB& aabb2);
bool AABBOBB(const AABB& aabb, const OBB& obb);
bool AABBPlane(const AABB& aabb, const Plane& plane);
bool OBBOBB(const OBB& obb1, const OBB& obb2);
bool OBBPlane(const OBB& obb, const Plane& plane);
bool PlanePlane(const Plane& plane1, const Plane& plane2);

#define AABBSphere(aabb, sphere) \
	SphereAABB(Sphere, AABB)
#define OBBSphere(obb, sphere) \
	SphereOBB(sphere, obb)
#define PlaneSphere(plane, sphere) \
	SpherePlane(sphere, plane)
#define OBBAABB(obb, aabb) \
	AABBOBB(aabb, obb)
#define PlaneAABB(plane, aabb) \
	AABBPlane(aabb, plane)
#define PlaneOBB(plane, obb) \
	OBBPlane(obb, plane)


float Raycast(const Sphere& sphere, const Ray& ray);
float Raycast(const AABB& aabb, const Ray& ray);
float Raycast(const OBB& obb, const Ray& ray);
float Raycast(const Plane& plane, const Ray& ray);
float Raycast(const Triangle& triangle, const Ray& ray);

bool Linetest(const Sphere& sphere, const Line& line);
bool Linetest(const AABB& aabb, const Line& line);
bool Linetest(const OBB& obb, const Line& line);
bool Linetest(const Plane& plane, const Line& line);
bool Linetest(const Triangle& triangle, const Line& line);

#ifndef NO_EXTRAS
float Raycast(const Ray& ray, const Sphere& sphere);
float Raycast(const Ray& ray, const AABB& aabb);
float Raycast(const Ray& ray, const OBB& obb);
float Raycast(const Ray& ray, const Plane& plane);
bool Linetest(const Line& line, const Sphere& sphere);
bool Linetest(const Line& line, const AABB& aabb);
bool Linetest(const Line& line, const OBB& obb);
bool Linetest(const Line& line, const Plane& plane);
#endif

#ifndef NO_EXTRAS
vec3 BarycentricOptimized(const Point& p, const Triangle& t);
#endif

bool PointInTriangle(const Point& p, const Triangle& t);
Plane FromTriangle(const Triangle& t);
Point ClosestPoint(const Triangle& t, const Point& p);
bool TriangleSphere(const Triangle& t, const Sphere& s);
bool TriangleAABB(const Triangle& t, const AABB& a);
bool TriangleOBB(const Triangle& t, const OBB& o);
bool TriangleTriangle(const Triangle& t1, const Triangle& t2);
bool TriangleTriangleRobust(const Triangle& t1, const Triangle& t2);
bool TrianglePlane(const Triangle& t, const Plane& p); 

#define SphereTriangle(s, t) \
	TriangleSphere(t, s)
#define AABBTriangle(a, t) \
	TriangleAABB(t, a)
#define OBBTriangle(o, t) \
	TriangleOBB(t, o)
#define PlaneTriangle(p, t) \
	TrianglePlane(t, p)

// A - Edge 0, Point 0
// B - Edge 0, Point 1
// C - Edge 1, Point 0
// D - Edge 1, Point 1
vec3 SatCrossEdge(const vec3& a, const vec3& b, const vec3& c, const vec3& d);
vec3 Barycentric(const Point& p, const Triangle& t);

void AccelerateMesh(Mesh& mesh);
void SplitBVHNode(BVHNode* node, const Mesh& model, int depth);
void FreeBVHNode(BVHNode* node);

bool Linetest(const Mesh& mesh, const Line& line);
bool MeshSphere(const Mesh& mesh, const Sphere& sphere);
bool MeshAABB(const Mesh& mesh, const AABB& aabb);
bool MeshOBB(const Mesh& mesh, const OBB& obb);
bool MeshPlane(const Mesh& mesh, const Plane& plane);
bool MeshTriangle(const Mesh& mesh, const Triangle& triangle);
float MeshRay(const Mesh& mesh, const Ray& ray);
#ifndef NO_EXTRAS
float Raycast(const Mesh& mesh, const Ray& ray);
float Raycast(const Model& mesh, const Ray& ray);
#endif 

mat4 GetWorldMatrix(const Model& model);
OBB GetOBB(const Model& model);

float ModelRay(const Model& model, const Ray& ray);
bool Linetest(const Model& model, const Line& line);
bool ModelSphere(const Model& model, const Sphere& sphere);
bool ModelAABB(const Model& model, const AABB& aabb);
bool ModelOBB(const Model& model, const OBB& obb);
bool ModelPlane(const Model& model, const Plane& plane);
bool ModelTriangle(const Model& model, const Triangle& triangle);

#endif

<|start_filename|>Chapter01/Code/Cloth.h<|end_filename|>
#ifndef _H_CLOTH_
#define _H_CLOTH_

#include "Particle.h"
#include "Spring.h"
#include <vector>

class Cloth {
protected:
	std::vector<Particle> verts;
	std::vector<Spring> structural;
	std::vector<Spring> shear;
	std::vector<Spring> bend;
	float clothSize;
public:
	// Public API
	void Initialize(int gridSize, float distance, const vec3& position);

	void SetStructuralSprings(float k, float b);
	void SetShearSprings(float k, float b);
	void SetBendSprings(float k, float b);
	void SetParticleMass(float mass);

	// For Physics System
	void ApplyForces();
	void Update(float dt);
	void SolveConstraints(const std::vector<OBB>& constraints);
	void ApplySpringForces(float dt);
	void Render(bool debug);
};

#endif

<|start_filename|>Chapter05/Physics/ObjLoader.cpp<|end_filename|>
#define TINYOBJLOADER_IMPLEMENTATION 
#include "tiny_obj_loader.h"
#include "ObjLoader.h"

bool LoadMesh(const char* szFilePath, Mesh* mesh) {
	if (szFilePath == 0 || mesh == 0) {
		return false;
	}

	tinyobj::attrib_t attrib;
	std::vector<tinyobj::shape_t> shapes;
	std::vector<tinyobj::material_t> materials;
	std::string err;

	if (!tinyobj::LoadObj(&attrib, &shapes, &materials, &err, szFilePath)) {
		return false;
	}

	// Reset mesh, assume it's fresh!
	mesh->numTriangles = 0;
	mesh->triangles = 0;

	for (int s = 0; s < shapes.size(); s++) { // Loop over shapes
		int index_offset = 0; // Loop over faces(polygon)
		for (int f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++) {
			int fv = shapes[s].mesh.num_face_vertices[f];
			if (fv != 3) { // Not triangle!
				return false;
			}
			mesh->numTriangles += 1;
		}
	}

	mesh->triangles = new Triangle[mesh->numTriangles];
	int vi = 0;

	for (int s = 0; s < shapes.size(); s++) { // Loop over shapes
		int index_offset = 0; // Loop over faces(polygon)
		for (int f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++) {
			int fv = shapes[s].mesh.num_face_vertices[f];

			for (int v = 0; v < fv; v++) { // Loop over vertices in the face.
				tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v];
				float vx = attrib.vertices[3 * idx.vertex_index + 0];
				float vy = attrib.vertices[3 * idx.vertex_index + 1];
				float vz = attrib.vertices[3 * idx.vertex_index + 2];

				mesh->vertices[vi++] = vec3(vx, vy, vz);
			}
			index_offset += fv;

			// per-face material
			shapes[s].mesh.material_ids[f];
		}
	}


	return true;
}

void FreeMesh(Mesh* mesh) {
	if (mesh->triangles != 0) {
		delete[] mesh->triangles;
	}

	mesh->triangles = 0;
	mesh->numTriangles = 0;
}

<|start_filename|>Chapter10/Physics/Geometry3D.h<|end_filename|>
#ifndef _H_GEOMETRY_3D_
#define _H_GEOMETRY_3D_

#include "vectors.h"
#include "matrices.h"

#ifndef NO_EXTRAS
#include <ostream>
#endif 

typedef vec3 Point;

typedef struct Line {
	Point start;
	Point end;

	inline Line() {}
	inline Line(const Point& s, const Point& e) :
		start(s), end(e) { }
} Line;

typedef struct Ray {
	Point origin;
	vec3 direction;

	inline Ray() : direction(0.0f, 0.0f, 1.0f) {}
	inline Ray(const Point& o, const vec3& d) :
		origin(o), direction(d) {
		NormalizeDirection();
	}
	inline void NormalizeDirection() {
		Normalize(direction);
	}
} Ray;

typedef struct Sphere {
	Point position;
	float radius;

	inline Sphere() : radius(1.0f) { }
	inline Sphere(const Point& p, float r) :
		position(p), radius(r) { }
} Sphere;

typedef struct AABB {
	Point position;
	vec3 size;

	inline AABB() : size(1, 1, 1) { }
	inline AABB(const Point& p, const vec3& s) :
		position(p), size(s) { }
} AABB;

typedef struct OBB {
	Point position;
	vec3 size;
	mat3 orientation;

	inline OBB() : size(1, 1, 1) { }
	inline OBB(const Point& p, const vec3& s) :
		position(p), size(s) { }
	inline OBB(const Point& p, const vec3& s, const mat3& o) :
		position(p), size(s), orientation(o) { }
} OBB;

typedef struct Plane {
	vec3 normal;
	float distance;

	inline Plane() : normal(1, 0, 0) { }
	inline Plane(const vec3& n, float d) :
		normal(n), distance(d) { }
} Plane;

typedef struct Triangle {
	union {
		struct {
			Point a;
			Point b;
			Point c;
		};
#ifndef NO_EXTRAS
		struct {
			Point p1;
			Point p2;
			Point p3;
		};
#endif 

		Point points[3];
		float values[9];
	};
	
	inline Triangle() { }
	inline Triangle(const Point& _p1, const Point& _p2, const Point& _p3) :
		a(_p1), b(_p2), c(_p3) { }
} Triangle;

#ifndef NO_EXTRAS
typedef vec3 Point3D;
typedef Line Line3D;
typedef Ray Ray3D;
typedef AABB Rectangle3D;
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape);
std::ostream& operator<<(std::ostream& os, const Ray& shape);
std::ostream& operator<<(std::ostream& os, const Sphere& shape);
std::ostream& operator<<(std::ostream& os, const AABB& shape);
std::ostream& operator<<(std::ostream& os, const OBB& shape);
std::ostream& operator<<(std::ostream& os, const Plane& shape);
std::ostream& operator<<(std::ostream& os, const Triangle& shape);
#endif 

float Length(const Line& line);
float LengthSq(const Line& line);
Ray FromPoints(const Point& from, const Point& to);
vec3 GetMin(const AABB& aabb);
vec3 GetMax(const AABB& aabb);
AABB FromMinMax(const vec3& min, const vec3& max);
float PlaneEquation(const Point& point, const Plane& plane);
#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point);
#endif

#endif

<|start_filename|>Chapter02/Code/ConservationOfMomentum.cpp<|end_filename|>
#include "ConservationOfMomentum.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void ConservationOfMomentum::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);

	physicsSystem.RenderRandomColors = true;
	physicsSystem.ImpulseIteration = 20;

	size_imgui_window = true;

	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(12.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

void ConservationOfMomentum::ResetDemo() {
	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();

	bodies.clear();
	bodies.resize(5);

	for (int i = 0; i < 5; ++i) {
		bodies[i].type = RIGIDBODY_TYPE_SPHERE;
		float pos = (float)i * (bodies[i].sphere.radius + 0.00001f) * 2.0f;
		if (i == 4) {
			pos = (float)i * (bodies[i].sphere.radius + 0.00001f) * 3.0f;
		}
		bodies[i].position = vec3(pos, 1.16, 0.0f);
		physicsSystem.AddRigidbody(&bodies[i]);
	}
	vec3 impulse = vec3(-bodies[4].position.x, 0.0f, 0.0f);
	Normalize(impulse);
	bodies[4].AddLinearImpulse(impulse * 20.0f);

	groundBox = RigidbodyVolume(RIGIDBODY_TYPE_BOX);
	groundBox.box.size = vec3(15.0f, 0.15f, 15.0f);
	groundBox.mass = 0.0f;

	physicsSystem.AddRigidbody(&groundBox);
}

float ConservationOfMomentum::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

vec3 ConservationOfMomentum::Random(vec3 min, vec3 max) {
	vec3 result;
	result.x = Random(min.x, max.x);
	result.y = Random(min.y, max.y);
	result.z = Random(min.z, max.z);
	return result;
}

void ConservationOfMomentum::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 100));
	}

	ImGui::Begin("Conservation Demo", 0, ImGuiWindowFlags_NoResize);

	if (ImGui::Button("Reset")) {
		ResetDemo();
	}

	ImGui::End();
}

void ConservationOfMomentum::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();
}

void ConservationOfMomentum::Update(float dt) {
	DemoBase::Update(dt);

	physicsSystem.Update(dt);
}

<|start_filename|>Chapter02/Code/Particle.cpp<|end_filename|>
#include "Particle.h"
#include "Geometry3D.h"
#include "FixedFunctionPrimitives.h"

Particle::Particle() {
	type = RIGIDBODY_TYPE_PARTICLE;
	friction = 0.95f;
	bounce = 0.7f;
	gravity = vec3(0.0f, -9.82f, 0.0f);

#ifdef EULER_INTEGRATION
	mass = 1.0f;
#endif
}

void Particle::Update(float deltaTime) {
#ifdef EULER_INTEGRATION
	oldPosition = position;
	vec3 acceleration = forces * (1.0f / mass);
#ifdef ACCURATE_EULER_INTEGRATION
	vec3 oldVelocity = velocity;
	velocity = velocity * friction + acceleration * deltaTime;
	position = position + (oldVelocity + velocity) * 0.5f * deltaTime;
#else
	velocity = velocity * friction + acceleration * deltaTime;
	position = position + velocity * deltaTime;
#endif
#else
	vec3 velocity = position - oldPosition;
	oldPosition = position;
	float deltaSquare = deltaTime * deltaTime;
	position = position + (velocity * friction + forces * deltaSquare);
#endif
}

void Particle::Render() {
	Sphere visual(position, 0.1f);
	::Render(visual);
}

void Particle::ApplyForces() {
	forces = gravity;
}

void Particle::SolveConstraints(const std::vector<OBB>& constraints) {
	int size = constraints.size();
	for (int i = 0; i < size; ++i) {
		Line traveled(oldPosition, position);
		if (Linetest(constraints[i], traveled)) {
			//if (PointInOBB(position, constraints[i])) {
#ifndef EULER_INTEGRATION
			vec3 velocity = position - oldPosition;
#endif
			vec3 direction = Normalized(velocity);
			Ray ray(oldPosition, direction);
			RaycastResult result;

			if (Raycast(constraints[i], ray, &result)) {
				// Place object just a little above collision result
				position = result.point + result.normal * 0.003f;

				vec3 vn = result.normal * Dot(result.normal, velocity);
				vec3 vt = velocity - vn;

#ifdef EULER_INTEGRATION
				oldPosition = position;
				velocity = vt - vn * bounce;
#else
				oldPosition = position - (vt - vn * bounce);
#endif
				break;
			}
		}
	}
}

void Particle::SetPosition(const vec3& pos) {
	position = pos;
	oldPosition = pos;
}

vec3 Particle::GetPosition() {
	return position;
}

void Particle::SetBounce(float b) {
	bounce = b;
}

float Particle::GetBounce() {
	return bounce;
}

<|start_filename|>Chapter09/Physics/Geometry3D.cpp<|end_filename|>
#include "Geometry3D.h"
#include <cmath>
#include <cfloat>

#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

float Length(const Line& line) {
	return Magnitude(line.start - line.end);
}

float LengthSq(const Line& line) {
	return MagnitudeSq(line.start - line.end);
}

Ray FromPoints(const Point& from, const Point& to) {
	return Ray(
		from,
		Normalized(to - from)
	);
}

vec3 GetMin(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fminf(p1.x, p2.x), fminf(p1.y, p2.y), fminf(p1.z, p2.z));
}
vec3 GetMax(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y), fmaxf(p1.z, p2.z));
}

AABB FromMinMax(const vec3& min, const vec3& max) {
	return AABB((min + max) * 0.5f, (max - min) * 0.5f);
}

float PlaneEquation(const Point& point, const Plane& plane) {
	return Dot(point, plane.normal) - plane.distance;
}

#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point) {
	return Dot(point, plane.normal) - plane.distance;
}
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << ", " << shape.start.z << "), end: (";
	os << shape.end.x << ", " << shape.end.y << ", " << shape.end.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Ray& shape) {
	os << "origin: (" << shape.origin.x << ", " << shape.origin.y << ", " << shape.origin.z << "), ";
	os << "direction: (" << shape.direction.x << ", " << shape.direction.y << ", " << shape.direction.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Sphere& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "radius: " << shape.radius;
	return os;
}

std::ostream& operator<<(std::ostream& os, const AABB& shape) {
	vec3 min = GetMin(shape);
	vec3 max = GetMax(shape);
	os << "min: (" << min.x << ", " << min.y << ", " << min.z << "), ";
	os << "max: (" << max.x << ", " << max.y << ", " << max.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Plane& shape) {
	os << "normal: (" << shape.normal.x << ", " << shape.normal.y << ", " << shape.normal.z << "), ";
	os << "distance: " << shape.distance;
	return os;
}

std::ostream& operator<<(std::ostream& os, const Triangle& shape) {
	os << "a: (" << shape.a.x << ", " << shape.a.y << ", " << shape.a.z << "), ";
	os << "b: (" << shape.b.x << ", " << shape.b.y << ", " << shape.b.z << "), ";
	os << "c: (" << shape.c.x << ", " << shape.c.y << ", " << shape.c.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const OBB& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "size:" << shape.size.x << ", " << shape.size.y << ", " << shape.size.z << "), ";
	os << "x basis:" << shape.orientation._11 << ", " << shape.orientation._21 << ", " << shape.orientation._31 << "), ";
	os << "y basis:" << shape.orientation._12 << ", " << shape.orientation._22 << ", " << shape.orientation._32 << "), ";
	os << "z basis:" << shape.orientation._13 << ", " << shape.orientation._23 << ", " << shape.orientation._33 << ")";
	return os;
}
#endif 

bool PointInSphere(const Point& point, const Sphere& sphere) {
	return MagnitudeSq(point - sphere.position) < sphere.radius * sphere.radius;
}

bool PointOnPlane(const Point& point, const Plane& plane) {
	// This should probably use an epsilon!
	//return Dot(point, plane.normal) - plane.distance == 0.0f;

	return CMP(Dot(point, plane.normal) - plane.distance, 0.0f);
}

bool PointInAABB(const Point& point, const AABB& aabb) {
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	if (point.x < min.x || point.x < min.y || point.z < min.z) {
		return false;
	}
	if (point.x > max.x || point.y > max.y || point.z > max.z) {
		return false;
	}

	return true;
}

bool PointInOBB(const Point& point, const OBB& obb) {
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			return false;
		}
		if (distance < -obb.size.asArray[i]) {
			return false;
		}
	}

	return true;
}

Point ClosestPoint(const Sphere& sphere, const Point& point) {
	vec3 sphereToPoint = point - sphere.position;
	Normalize(sphereToPoint);
	sphereToPoint = sphereToPoint * sphere.radius;
	return sphereToPoint + sphere.position;
}

Point ClosestPoint(const AABB& aabb, const Point& point) {
	Point result = point;
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	result.x = (result.x < min.x) ? min.x : result.x;
	result.y = (result.y < min.x) ? min.y : result.y;
	result.z = (result.z < min.x) ? min.z : result.z;

	result.x = (result.x > max.x) ? max.x : result.x;
	result.y = (result.y > max.x) ? max.y : result.y;
	result.z = (result.z > max.x) ? max.z : result.z;

	return result;
}

Point ClosestPoint(const OBB& obb, const Point& point) {
	Point result = obb.position;
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			distance = obb.size.asArray[i];
		}
		if (distance < -obb.size.asArray[i]) {
			distance = -obb.size.asArray[i];
		}

		result = result + (axis * distance);
	}

	return result;
}

Point ClosestPoint(const Plane& plane, const Point& point) {
	// This works assuming plane.Normal is normalized, which it should be
	float distance = Dot(plane.normal, point) - plane.distance;
	// If the plane normal wasn't normalized, we'd need this:
	// distance = distance / DOT(plane.Normal, plane.Normal);

	return point - plane.normal * distance;
}

bool PointOnLine(const Point& point, const Line& line) {
	Point closest = ClosestPoint(line, point);
	float distanceSq = MagnitudeSq(closest - point);
	return CMP(distanceSq, 0.0f);
}

Point ClosestPoint(const Line& line, const Point& point) {
	vec3 lVec = line.end - line.start; // Line Vector
	// Project "point" onto the "Line Vector", computing:
	// closest(t) = start + t * (end - start)
	// T is how far along the line the projected point is
	float t = Dot(point - line.start, lVec) / Dot(lVec, lVec);
	// Clamp t to the 0 to 1 range
	t = fmaxf(t, 0.0f);
	t = fminf(t, 1.0f);
	// Return projected position of t
	return line.start + lVec * t;
}

bool PointOnRay(const Point& point, const Ray& ray) {
	if (point == ray.origin) {
		return true;
	}

	vec3 norm = point - ray.origin;
	Normalize(norm);
	float diff = Dot(norm, ray.direction); // Direction is normalized
	// If BOTH vectors point in the same direction, diff should be 1
	return CMP(diff, 1.0f);
}

Point ClosestPoint(const Ray& ray, const Point& point) {
	// Project point onto ray, 
	float t = Dot(point - ray.origin, ray.direction);
	// Not needed if direction is normalized!
	// t /= Dot(ray.direction, ray.direction);

	// We only want to clamp t in the positive direction.
	// The ray extends infinatley in this direction!
	t = fmaxf(t, 0.0f);

	// Compute the projected position from the clamped t
	// Notice we multiply r.Normal by t, not AB.
	// This is becuase we want the ray in the direction 
	// of the normal, which technically the line segment is
	// but this is much more explicit and easy to read.
	return Point(ray.origin + ray.direction * t);
}

<|start_filename|>Chapter03/Code/IMGUIWindow.cpp<|end_filename|>
#include "IMGUIWindow.h"

<|start_filename|>Chapter03/Code/IMGUIWindow.h<|end_filename|>
#ifndef _H_IMGUI_WINDOW_
#define _H_IMGUI_WINDOW_

// TODO: Implement DearImgui as a standard, sub-classable window
// url: https://github.com/ocornut/imgui

#include "GLWindow.h"

#endif


<|start_filename|>Chapter03/Code/Geometry3D.cpp<|end_filename|>
#include "Geometry3D.h"
#include <cmath>
#include <cfloat>
#include <list>

#ifdef DO_SANITY_TESTS
#include <iostream>
#endif

#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

float Length(const Line& line) {
	return Magnitude(line.start - line.end);
}

float LengthSq(const Line& line) {
	return MagnitudeSq(line.start - line.end);
}

Ray FromPoints(const Point& from, const Point& to) {
	return Ray(
		from,
		Normalized(to - from)
	);
}

vec3 GetMin(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fminf(p1.x, p2.x), fminf(p1.y, p2.y), fminf(p1.z, p2.z));
}
vec3 GetMax(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y), fmaxf(p1.z, p2.z));
}

AABB FromMinMax(const vec3& min, const vec3& max) {
	return AABB((min + max) * 0.5f, (max - min) * 0.5f);
}

float PlaneEquation(const Point& point, const Plane& plane) {
	return Dot(point, plane.normal) - plane.distance;
}

#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point) {
	return Dot(point, plane.normal) - plane.distance;
}
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << ", " << shape.start.z << "), end: (";
	os << shape.end.x << ", " << shape.end.y << ", " << shape.end.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Ray& shape) {
	os << "origin: (" << shape.origin.x << ", " << shape.origin.y << ", " << shape.origin.z << "), ";
	os << "direction: (" << shape.direction.x << ", " << shape.direction.y << ", " << shape.direction.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Sphere& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "radius: " << shape.radius;
	return os;
}

std::ostream& operator<<(std::ostream& os, const AABB& shape) {
	vec3 min = GetMin(shape);
	vec3 max = GetMax(shape);
	os << "min: (" << min.x << ", " << min.y << ", " << min.z << "), ";
	os << "max: (" << max.x << ", " << max.y << ", " << max.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Plane& shape) {
	os << "normal: (" << shape.normal.x << ", " << shape.normal.y << ", " << shape.normal.z << "), ";
	os << "distance: " << shape.distance;
	return os;
}

std::ostream& operator<<(std::ostream& os, const Triangle& shape) {
	os << "a: (" << shape.a.x << ", " << shape.a.y << ", " << shape.a.z << "), ";
	os << "b: (" << shape.b.x << ", " << shape.b.y << ", " << shape.b.z << "), ";
	os << "c: (" << shape.c.x << ", " << shape.c.y << ", " << shape.c.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const OBB& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "size:" << shape.size.x << ", " << shape.size.y << ", " << shape.size.z << "), ";
	os << "x basis:" << shape.orientation._11 << ", " << shape.orientation._21 << ", " << shape.orientation._31 << "), ";
	os << "y basis:" << shape.orientation._12 << ", " << shape.orientation._22 << ", " << shape.orientation._32 << "), ";
	os << "z basis:" << shape.orientation._13 << ", " << shape.orientation._23 << ", " << shape.orientation._33 << ")";
	return os;
}
#endif 

bool PointInSphere(const Point& point, const Sphere& sphere) {
	return MagnitudeSq(point - sphere.position) < sphere.radius * sphere.radius;
}

bool PointOnPlane(const Point& point, const Plane& plane) {
	// This should probably use an epsilon!
	//return Dot(point, plane.normal) - plane.distance == 0.0f;

	return CMP(Dot(point, plane.normal) - plane.distance, 0.0f);
}

bool PointInAABB(const Point& point, const AABB& aabb) {
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	if (point.x < min.x || point.x < min.y || point.z < min.z) {
		return false;
	}
	if (point.x > max.x || point.y > max.y || point.z > max.z) {
		return false;
	}

	return true;
}

bool PointInOBB(const Point& point, const OBB& obb) {
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			return false;
		}
		if (distance < -obb.size.asArray[i]) {
			return false;
		}
	}

	return true;
}

Point ClosestPoint(const Sphere& sphere, const Point& point) {
	vec3 sphereToPoint = point - sphere.position;
	Normalize(sphereToPoint);
	sphereToPoint = sphereToPoint * sphere.radius;
	return sphereToPoint + sphere.position;
}

Point ClosestPoint(const AABB& aabb, const Point& point) {
	Point result = point;
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	result.x = (result.x < min.x) ? min.x : result.x;
	result.y = (result.y < min.x) ? min.y : result.y;
	result.z = (result.z < min.x) ? min.z : result.z;

	result.x = (result.x > max.x) ? max.x : result.x;
	result.y = (result.y > max.x) ? max.y : result.y;
	result.z = (result.z > max.x) ? max.z : result.z;

	return result;
}

Point ClosestPoint(const OBB& obb, const Point& point) {
	Point result = obb.position;
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			distance = obb.size.asArray[i];
		}
		if (distance < -obb.size.asArray[i]) {
			distance = -obb.size.asArray[i];
		}

		result = result + (axis * distance);
	}

	return result;
}

Point ClosestPoint(const Plane& plane, const Point& point) {
	// This works assuming plane.Normal is normalized, which it should be
	float distance = Dot(plane.normal, point) - plane.distance;
	// If the plane normal wasn't normalized, we'd need this:
	// distance = distance / DOT(plane.Normal, plane.Normal);

	return point - plane.normal * distance;
}

bool PointOnLine(const Point& point, const Line& line) {
	Point closest = ClosestPoint(line, point);
	float distanceSq = MagnitudeSq(closest - point);
	return CMP(distanceSq, 0.0f);
}

Point ClosestPoint(const Line& line, const Point& point) {
	vec3 lVec = line.end - line.start; // Line Vector
	// Project "point" onto the "Line Vector", computing:
	// closest(t) = start + t * (end - start)
	// T is how far along the line the projected point is
	float t = Dot(point - line.start, lVec) / Dot(lVec, lVec);
	// Clamp t to the 0 to 1 range
	t = fmaxf(t, 0.0f);
	t = fminf(t, 1.0f);
	// Return projected position of t
	return line.start + lVec * t;
}

bool PointOnRay(const Point& point, const Ray& ray) {
	if (point == ray.origin) {
		return true;
	}

	vec3 norm = point - ray.origin;
	Normalize(norm);
	float diff = Dot(norm, ray.direction); // Direction is normalized
	// If BOTH vectors point in the same direction, diff should be 1
	return CMP(diff, 1.0f);
}

Point ClosestPoint(const Ray& ray, const Point& point) {
	// Project point onto ray, 
	float t = Dot(point - ray.origin, ray.direction);
	// Not needed if direction is normalized!
	// t /= Dot(ray.direction, ray.direction);

	// We only want to clamp t in the positive direction.
	// The ray extends infinatley in this direction!
	t = fmaxf(t, 0.0f);

	// Compute the projected position from the clamped t
	// Notice we multiply r.Normal by t, not AB.
	// This is becuase we want the ray in the direction 
	// of the normal, which technically the line segment is
	// but this is much more explicit and easy to read.
	return Point(ray.origin + ray.direction * t);
}

#ifndef NO_EXTRAS
bool PointInPlane(const Point& point, const Plane& plane) {
	return PointOnPlane(point, plane);
}
bool PointInLine(const Point& point, const Line& line) {
	return PointOnLine(point, line);
}
bool PointInRay(const Point& point, const Ray& ray) {
	return PointOnRay(point, ray);
}
bool ContainsPoint(const Sphere& sphere, const Point& point) {
	return PointInSphere(point, sphere);
}
bool ContainsPoint(const Point& point, const Sphere& sphere) {
	return PointInSphere(point, sphere);
}
bool ContainsPoint(const AABB& aabb, const Point& point) {
	return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const AABB& aabb) {
	return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const OBB& obb) {
	return PointInOBB(point, obb);
}
bool ContainsPoint(const OBB& obb, const Point& point) {
	return PointInOBB(point, obb);
}
bool ContainsPoint(const Point& point, const Plane& plane) {
	return PointOnPlane(point, plane);
}
bool ContainsPoint(const Plane& plane, const Point& point) {
	return PointOnPlane(point, plane);
}
bool ContainsPoint(const Point& point, const Line& line) {
	return PointOnLine(point, line);
}
bool ContainsPoint(const Line& line, const Point& point) {
	return PointOnLine(point, line);
}
bool ContainsPoint(const Point& point, const Ray& ray) {
	return PointOnRay(point, ray);
}
bool ContainsPoint(const Ray& ray, const Point& point) {
	return PointOnRay(point, ray);
}
Point ClosestPoint(const Point& point, const Sphere& sphere) {
	return ClosestPoint(sphere, point);
}
Point ClosestPoint(const Point& point, const AABB& aabb) {
	return ClosestPoint(aabb, point);
}
Point ClosestPoint(const Point& point, const OBB& obb) {
	return ClosestPoint(obb, point);
}
Point ClosestPoint(const Point& point, const Plane& plane) {
	return ClosestPoint(plane, point);
}
Point ClosestPoint(const Point& point, const Line& line) {
	return ClosestPoint(line, point);
}
Point ClosestPoint(const Point& point, const Ray& ray) {
	return ClosestPoint(ray, point);
}
Point ClosestPoint(const Point& p, const Triangle& t) {
	return ClosestPoint(t, p);
}
#endif

bool SphereSphere(const Sphere& s1, const Sphere& s2) {
	float radiiSum = s1.radius + s2.radius;
	float sqDistance = MagnitudeSq(s1.position - s2.position);
	return sqDistance < radiiSum * radiiSum;
}

bool SphereAABB(const Sphere& sphere, const AABB& aabb) {
	Point closestPoint = ClosestPoint(aabb, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool SphereOBB(const Sphere& sphere, const OBB& obb) {
	Point closestPoint = ClosestPoint(obb, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool SpherePlane(const Sphere& sphere, const Plane& plane) {
	Point closestPoint = ClosestPoint(plane, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool AABBAABB(const AABB& aabb1, const AABB& aabb2) {
	Point aMin = GetMin(aabb1);
	Point aMax = GetMax(aabb1);
	Point bMin = GetMin(aabb2);
	Point bMax = GetMax(aabb2);

	return	(aMin.x <= bMax.x && aMax.x >= bMin.x) &&
			(aMin.y <= bMax.y && aMax.y >= bMin.y) &&
			(aMin.z <= bMax.z && aMax.z >= bMin.z);
}

bool AABBOBB(const AABB& aabb, const OBB& obb) {
	const float* o = obb.orientation.asArray;

	vec3 test[15] = {
		vec3(1, 0, 0), // AABB axis 1
		vec3(0, 1, 0), // AABB axis 2
		vec3(0, 0, 1), // AABB axis 3
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8])
	};

	for (int i = 0; i < 3; ++i) { // Fill out rest of axis
		test[6 + i * 3 + 0] = Cross(test[i], test[0]);
		test[6 + i * 3 + 1] = Cross(test[i], test[1]);
		test[6 + i * 3 + 2] = Cross(test[i], test[2]);
	}

	for (int i = 0; i < 15; ++i) {
		if (!OverlapOnAxis(aabb, obb, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool OverlapOnAxis(const AABB& aabb, const OBB& obb, const vec3& axis) {
	Interval a = GetInterval(aabb, axis);
	Interval b = GetInterval(obb, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OverlapOnAxis(const OBB& obb1, const OBB& obb2, const vec3& axis) {
	Interval a = GetInterval(obb1, axis);
	Interval b = GetInterval(obb1, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OverlapOnAxis(const AABB& aabb, const Triangle& triangle, const vec3& axis) {
	Interval a = GetInterval(aabb, axis);
	Interval b = GetInterval(triangle, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OverlapOnAxis(const OBB& obb, const Triangle& triangle, const vec3& axis) {
	Interval a = GetInterval(obb, axis);
	Interval b = GetInterval(triangle, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OverlapOnAxis(const Triangle& t1, const Triangle& t2, const vec3& axis) {
	Interval a = GetInterval(t1, axis);
	Interval b = GetInterval(t2, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

Interval GetInterval(const Triangle& triangle, const vec3& axis) {
	Interval result;

	result.min = Dot(axis, triangle.points[0]);
	result.max = result.min;
	for (int i = 1; i < 3; ++i) {
		float value = Dot(axis, triangle.points[i]);
		result.min = fminf(result.min, value);
		result.max = fmaxf(result.max, value);
	}

	return result;
}

Interval GetInterval(const OBB& obb, const vec3& axis) {
	vec3 vertex[8];

	vec3 C = obb.position;	// OBB Center
	vec3 E = obb.size;		// OBB Extents
	const float* o = obb.orientation.asArray;
	vec3 A[] = {			// OBB Axis
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8]),
	};

	vertex[0] = C + A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
	vertex[1] = C - A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
	vertex[2] = C + A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
	vertex[3] = C + A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
	vertex[4] = C - A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
	vertex[5] = C + A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
	vertex[6] = C - A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
	vertex[7] = C - A[0] * E[0] - A[1] * E[1] + A[2] * E[2];

	Interval result;
	result.min = result.max = Dot(axis, vertex[0]);

	for (int i = 1; i < 8; ++i) {
		float projection = Dot(axis, vertex[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}

Interval GetInterval(const AABB& aabb, const vec3& axis) {
	vec3 i = GetMin(aabb);
	vec3 a = GetMax(aabb);

	vec3 vertex[8] = {
		vec3(i.x, a.y, a.z),
		vec3(i.x, a.y, i.z),
		vec3(i.x, i.y, a.z),
		vec3(i.x, i.y, i.z),
		vec3(a.x, a.y, a.z),
		vec3(a.x, a.y, i.z),
		vec3(a.x, i.y, a.z),
		vec3(a.x, i.y, i.z)
	};

	Interval result;
	result.min = result.max = Dot(axis, vertex[0]);

	for (int i = 1; i < 8; ++i) {
		float projection = Dot(axis, vertex[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}

bool AABBPlane(const AABB& aabb, const Plane& plane) {
	// Project the half extents of the AABB onto the plane normal
	float pLen =aabb.size.x * fabsf(plane.normal.x) +
				aabb.size.y * fabsf(plane.normal.y) +
				aabb.size.z * fabsf(plane.normal.z);
	// Find the distance from the center of the AABB to the plane
	float dist = Dot(plane.normal, aabb.position) - plane.distance;
	// Intersection occurs if the distance falls within the projected side
	return fabsf(dist) <= pLen;
}

bool OBBOBB(const OBB& obb1, const OBB& obb2) {
	const float* o1 = obb1.orientation.asArray;
	const float* o2 = obb2.orientation.asArray;

	vec3 test[15] = {
		vec3(o1[0], o1[1], o1[2]),
		vec3(o1[3], o1[4], o1[5]),
		vec3(o1[6], o1[7], o1[8]),
		vec3(o2[0], o2[1], o2[2]),
		vec3(o2[3], o2[4], o2[5]),
		vec3(o2[6], o2[7], o2[8])
	};

	for (int i = 0; i < 3; ++i) { // Fill out rest of axis
		test[6 + i * 3 + 0] = Cross(test[i], test[0]);
		test[6 + i * 3 + 1] = Cross(test[i], test[1]);
		test[6 + i * 3 + 2] = Cross(test[i], test[2]);
	}

	for (int i = 0; i < 15; ++i) {
		if (!OverlapOnAxis(obb1, obb2, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool OBBPlane(const OBB& obb, const Plane& plane) {
	// Local variables for readability only
	const float* o = obb.orientation.asArray;
	vec3 rot[] = { // rotation / orientation
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8]),
	};
	vec3 normal = plane.normal;

	// Project the half extents of the AABB onto the plane normal
	float pLen =obb.size.x * fabsf(Dot(normal, rot[0])) +
				obb.size.y * fabsf(Dot(normal, rot[1])) +
				obb.size.z * fabsf(Dot(normal, rot[2]));
	// Find the distance from the center of the OBB to the plane
	float dist = Dot(plane.normal, obb.position) - plane.distance;
	// Intersection occurs if the distance falls within the projected side
	return fabsf(dist) <= pLen;
}

bool PlanePlane(const Plane& plane1, const Plane& plane2) {
	// Compute direction of intersection line
	vec3 d = Cross(plane1.normal, plane2.normal);

	// Check the length of the direction line
	// if the length is 0, no intersection happened
	return !(CMP(Dot(d, d), 0));

	// We could have used the dot product here, instead of the cross product
}

bool Raycast(const Sphere& sphere, const Ray& ray, RaycastResult* outResult) {
	ResetRaycastResult(outResult);

	vec3 e = sphere.position - ray.origin;
	float rSq = sphere.radius * sphere.radius;

	float eSq = MagnitudeSq(e);
	float a = Dot(e, ray.direction); // ray.direction is assumed to be normalized
	float bSq = /*sqrtf(*/eSq - (a * a)/*)*/;
	float f = sqrt((rSq)- /*(b * b)*/bSq);

	// Assume normal intersection!
	float t = a - f;

	// No collision has happened
	if (rSq - (eSq - a * a) < 0.0f) {
		return false;
	}
	// Ray starts inside the sphere
	else if (eSq < rSq) {
		// Just reverse direction
		t = a + f;
	}
	if (outResult != 0) {
		outResult->t = t;
		outResult->hit = true;
		outResult->point = ray.origin + ray.direction * t;
		outResult->normal = Normalized(outResult->point - sphere.position);
	}
	return true;
}

bool Raycast(const OBB& obb, const Ray& ray, RaycastResult* outResult) {
	ResetRaycastResult(outResult);

	const float* o = obb.orientation.asArray;
	const float* size = obb.size.asArray;

	vec3 p = obb.position - ray.origin;

	vec3 X(o[0], o[1], o[2]);
	vec3 Y(o[3], o[4], o[5]);
	vec3 Z(o[6], o[7], o[8]);

	vec3 f(
		Dot(X, ray.direction),
		Dot(Y, ray.direction),
		Dot(Z, ray.direction)
	);

	vec3 e(
		Dot(X, p),
		Dot(Y, p),
		Dot(Z, p)
	);

#if 1
	float t[6] = { 0, 0, 0, 0, 0, 0 };
	for (int i = 0; i < 3; ++i) {
		if (CMP(f[i], 0)) {
			if (-e[i] - size[i] > 0 || -e.x + size[i] < 0) {
				return false;
			}
			f[i] = 0.00001f; // Avoid div by 0!
		}

		t[i * 2 + 0] = (e[i] + size[i]) / f[i]; // tmin[x, y, z]
		t[i * 2 + 1] = (e[i] - size[i]) / f[i]; // tmax[x, y, z]
	}

	float tmin = fmaxf(fmaxf(fminf(t[0], t[1]), fminf(t[2], t[3])), fminf(t[4], t[5]));
	float tmax = fminf(fminf(fmaxf(t[0], t[1]), fmaxf(t[2], t[3])), fmaxf(t[4], t[5]));
#else 
	// The above loop simplifies the below if statements
	// this is done to make sure the sample fits into the book
	if (CMP(f.x, 0)) {
		if (-e.x - obb.size.x > 0 || -e.x + obb.size.x < 0) {
			return -1;
		}
		f.x = 0.00001f; // Avoid div by 0!
	}
	else if (CMP(f.y, 0)) {
		if (-e.y - obb.size.y > 0 || -e.y + obb.size.y < 0) {
			return -1;
		}
		f.y = 0.00001f; // Avoid div by 0!
	}
	else if (CMP(f.z, 0)) {
		if (-e.z - obb.size.z > 0 || -e.z + obb.size.z < 0) {
			return -1;
		}
		f.z = 0.00001f; // Avoid div by 0!
	}

	float t1 = (e.x + obb.size.x) / f.x;
	float t2 = (e.x - obb.size.x) / f.x;
	float t3 = (e.y + obb.size.y) / f.y;
	float t4 = (e.y - obb.size.y) / f.y;
	float t5 = (e.z + obb.size.z) / f.z;
	float t6 = (e.z - obb.size.z) / f.z;

	float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
	float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));
#endif

	// if tmax < 0, ray is intersecting AABB
	// but entire AABB is behing it's origin
	if (tmax < 0) {
		return false;
	}

	// if tmin > tmax, ray doesn't intersect AABB
	if (tmin > tmax) {
		return false;
	}

	// If tmin is < 0, tmax is closer
	float t_result = tmin;

	if (tmin < 0.0f) {
		t_result = tmax;
	}

	if (outResult != 0) {
		outResult->hit = true;
		outResult->t = t_result;
		outResult->point = ray.origin + ray.direction * t_result;

		vec3 normals[] = {
			X,			// +x
			X * -1.0f,	// -x
			Y,			// +y
			Y * -1.0f,	// -y
			Z,			// +z
			Z * -1.0f	// -z
		};

		for (int i = 0; i < 6; ++i) {
			if (CMP(t_result, t[i])) {
				outResult->normal = Normalized(normals[i]);
			}
		}
	}
	return true;
}

void ResetRaycastResult(RaycastResult* outResult) {
	if (outResult != 0) {
		outResult->t = -1;
		outResult->hit = false;
		outResult->normal = vec3(0, 0, 1);
		outResult->point = vec3(0, 0, 0);
	}
}

bool Raycast(const AABB& aabb, const Ray& ray, RaycastResult* outResult) {
	ResetRaycastResult(outResult);

	vec3 min = GetMin(aabb);
	vec3 max = GetMax(aabb);

	// Any component of direction could be 0!
	// Address this by using a small number, close to
	// 0 in case any of directions components are 0
	float t1 = (min.x - ray.origin.x) / (CMP(ray.direction.x, 0.0f) ? 0.00001f : ray.direction.x);
	float t2 = (max.x - ray.origin.x) / (CMP(ray.direction.x, 0.0f) ? 0.00001f : ray.direction.x);
	float t3 = (min.y - ray.origin.y) / (CMP(ray.direction.y, 0.0f) ? 0.00001f : ray.direction.y);
	float t4 = (max.y - ray.origin.y) / (CMP(ray.direction.y, 0.0f) ? 0.00001f : ray.direction.y);
	float t5 = (min.z - ray.origin.z) / (CMP(ray.direction.z, 0.0f) ? 0.00001f : ray.direction.z);
	float t6 = (max.z - ray.origin.z) / (CMP(ray.direction.z, 0.0f) ? 0.00001f : ray.direction.z);

	float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
	float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));

	// if tmax < 0, ray is intersecting AABB
	// but entire AABB is behing it's origin
	if (tmax < 0) {
		return false;
	}

	// if tmin > tmax, ray doesn't intersect AABB
	if (tmin > tmax) {
		return false;
	}

	float t_result = tmin;

	// If tmin is < 0, tmax is closer
	if (tmin < 0.0f) {
		t_result = tmax;
	}

	if (outResult != 0) {
		outResult->t = t_result;
		outResult->hit = true;
		outResult->point = ray.origin + ray.direction * t_result;

		vec3 normals[] = {
			vec3(-1, 0, 0),
			vec3(1, 0, 0),
			vec3(0, -1, 0),
			vec3(0, 1, 0),
			vec3(0, 0, -1),
			vec3(0, 0, 1)
		};
		float t[] = { t1, t2, t3, t4, t5, t6 };

		for (int i = 0; i < 6; ++i) {
			if (CMP(t_result, t[i])) {
				outResult->normal = normals[i];
			}
		}
	}

	return true;
}

bool Raycast(const Plane& plane, const Ray& ray, RaycastResult* outResult) {
	ResetRaycastResult(outResult);

	float nd = Dot(ray.direction, plane.normal);
	float pn = Dot(ray.origin, plane.normal);

	// nd must be negative, and not 0
	// if nd is positive, the ray and plane normals
	// point in the same direction. No intersection.
	if (nd >= 0.0f) {
		return false;
	}

	float t = (plane.distance - pn) / nd;

	// t must be positive
	if (t >= 0.0f) {
		if (outResult != 0) {
			outResult->t = t;
			outResult->hit = true;
			outResult->point = ray.origin + ray.direction * t;
			outResult->normal = Normalized(plane.normal);
		}
		return true;
	}

	return false;
}

bool Linetest(const Sphere& sphere, const Line& line) {
	Point closest = ClosestPoint(line, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closest);
	return distSq <= (sphere.radius * sphere.radius);
}

bool Linetest(const Plane& plane, const Line& line) {
	vec3 ab = line.end - line.start;

	float nA = Dot(plane.normal, line.start);
	float nAB = Dot(plane.normal, ab);

	if (CMP(nAB, 0)) {
		return false;
	}

	float t = (plane.distance - nA) / nAB;
	return t >= 0.0f && t <= 1.0f;
}

bool Linetest(const AABB& aabb, const Line& line) {
	Ray ray;
	ray.origin = line.start;
	ray.direction = Normalized(line.end - line.start);
	RaycastResult raycast;
	if (!Raycast(aabb, ray, &raycast)) {
		return false;
	}
	float t = raycast.t;

	return t >= 0 && t * t <= LengthSq(line);
}

bool Linetest(const OBB& obb, const Line& line) {
	Ray ray;
	ray.origin = line.start;
	ray.direction = Normalized(line.end - line.start);
	RaycastResult result;
	if (!Raycast(obb, ray, &result)) {
		return false;
	}
	float t = result.t;

	return t >= 0 && t * t <= LengthSq(line);
}

#ifndef NO_EXTRAS
bool Raycast(const Ray& ray, const Sphere& sphere, RaycastResult* outResult) {
	return Raycast(sphere, ray, outResult);
}

bool Raycast(const Ray& ray, const AABB& aabb, RaycastResult* outResult) {
	return Raycast(aabb, ray, outResult);
}

bool Raycast(const Ray& ray, const OBB& obb, RaycastResult* outResult) {
	return Raycast(obb, ray, outResult);
}

bool Raycast(const Ray& ray, const Plane& plane, RaycastResult* outResult) {
	return Raycast(plane, ray, outResult);
}

bool Linetest(const Line& line, const Sphere& sphere) {
	return Linetest(sphere, line);
}

bool Linetest(const Line& line, const AABB& aabb) {
	return Linetest(aabb, line);
}

bool Linetest(const Line& line, const OBB& obb) {
	return Linetest(obb, line);
}

bool Linetest(const Line& line, const Plane& plane) {
	return Linetest(plane, line);
}
#endif

bool PointInTriangle(const Point& p, const Triangle& t) {
	// Move the triangle so that the point is  
	// now at the origin of the triangle
	vec3 a = t.a - p;
	vec3 b = t.b - p;
	vec3 c = t.c - p;

	// The point should be moved too, so they are both
	// relative, but because we don't use p in the
	// equation anymore, we don't need it!
	// p -= p; // This would just equal the zero vector!

	vec3 normPBC = Cross(b, c); // Normal of PBC (u)
	vec3 normPCA = Cross(c, a); // Normal of PCA (v)
	vec3 normPAB = Cross(a, b); // Normal of PAB (w)

	// Test to see if the normals are facing 
	// the same direction, return false if not
	if (Dot(normPBC, normPCA) < 0.0f) {
		return false;
	}
	else if (Dot(normPBC, normPAB) < 0.0f) {
		return false;
	}

	// All normals facing the same way, return true
	return true;
}

#ifndef NO_EXTRAS
vec3 BarycentricOptimized(const Point& p, const Triangle& t) {
	vec3 v0 = t.b - t.a;
	vec3 v1 = t.c - t.a;
	vec3 v2 = p - t.a;

	float d00 = Dot(v0, v0);
	float d01 = Dot(v0, v1);
	float d11 = Dot(v1, v1);
	float d20 = Dot(v2, v0);
	float d21 = Dot(v2, v1);
	float denom = d00 * d11 - d01 * d01;

	if (CMP(denom, 0.0f)) {
		return vec3();
	}

	vec3 result;
	result.y = (d11 * d20 - d01 * d21) / denom;
	result.z = (d00 * d21 - d01 * d20) / denom;
	result.x = 1.0f - result.y - result.z;

	return result;
}
#endif

vec3 Barycentric(const Point& p, const Triangle& t) {
	vec3 ap = p - t.a;
	vec3 bp = p - t.b;
	vec3 cp = p - t.c;

	vec3 ab = t.b - t.a;
	vec3 ac = t.c - t.a;
	vec3 bc = t.c - t.b;
	vec3 cb = t.b - t.c;
	vec3 ca = t.a - t.c;

	vec3 v = ab - Project(ab, cb);
	float a = 1.0f - (Dot(v, ap) / Dot(v, ab));

	v = bc - Project(bc, ac);
	float b = 1.0f - (Dot(v, bp) / Dot(v, bc));

	v = ca - Project(ca, ab);
	float c = 1.0f - (Dot(v, cp) / Dot(v, ca));

#ifdef DO_SANITY_TESTS
#ifndef NO_EXTRAS
	vec3 barycentric = BarycentricOptimized(p, t);
	if (fabsf(a - barycentric.x) > 0.1f) {
		std::cout << "Expected a: " << a << " to be: " << barycentric.x << "\n";
	}
	if (fabsf(b - barycentric.y) > 0.1f) {
		std::cout << "Expected b: " << b << " to be: " << barycentric.y << "\n";
	}
	if (fabsf(c - barycentric.z) > 0.1f) {
		std::cout << "Expected c: " << c << " to be: " << barycentric.z << "\n";
	}
#endif
#endif

	return vec3(a, b, c);
}

Plane FromTriangle(const Triangle& t) {
	Plane result;
	result.normal = Normalized(Cross(t.b - t.a, t.c - t.a));
	result.distance = Dot(result.normal, t.a);
	return result;
}

Point ClosestPoint(const Triangle& t, const Point& p) {
	Plane plane = FromTriangle(t);
	Point closest = ClosestPoint(plane, p);

	// Closest point was inside triangle
	if (PointInTriangle(closest, t)) {
		return closest;
	}

	Point c1 = ClosestPoint(Line(t.a, t.b), closest); // Line AB
	Point c2 = ClosestPoint(Line(t.b, t.c), closest); // Line BC
	Point c3 = ClosestPoint(Line(t.c, t.a), closest); // Line CA

	float magSq1 = MagnitudeSq(closest - c1);
	float magSq2 = MagnitudeSq(closest - c2);
	float magSq3 = MagnitudeSq(closest - c3);

	if (magSq1 < magSq2 && magSq1 < magSq3) {
		return c1;
	}
	else if (magSq2 < magSq1 && magSq2 < magSq3) {
		return c2;
	}

	return c3;
}

bool TriangleSphere(const Triangle& t, const Sphere& s) {
	Point closest = ClosestPoint(t, s.position);
	float magSq = MagnitudeSq(closest - s.position);
	return magSq <= s.radius * s.radius;
}

bool TriangleAABB(const Triangle& t, const AABB& a) {
	// Compute the edge vectors of the triangle  (ABC)
	vec3 f0 = t.b - t.a; 
	vec3 f1 = t.c - t.b; 
	vec3 f2 = t.a - t.c; 

	// Compute the face normals of the AABB
	vec3 u0(1.0f, 0.0f, 0.0f);
	vec3 u1(0.0f, 1.0f, 0.0f);
	vec3 u2(0.0f, 0.0f, 1.0f);

	vec3 test[13] = {
		// 3 Normals of AABB
		u0, // AABB Axis 1
		u1, // AABB Axis 2
		u2, // AABB Axis 3
		// 1 Normal of the Triangle
		Cross(f0, f1),
		// 9 Axis, cross products of all edges
		Cross(u0, f0),
		Cross(u0, f1),
		Cross(u0, f2),
		Cross(u1, f0),
		Cross(u1, f1),
		Cross(u1, f2),
		Cross(u2, f0),
		Cross(u2, f1),
		Cross(u2, f2)
	};

	for (int i = 0; i < 13; ++i) {
		if (!OverlapOnAxis(a, t, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool TriangleOBB(const Triangle& t, const OBB& o) {
	// Compute the edge vectors of the triangle  (ABC)
	vec3 f0 = t.b - t.a;
	vec3 f1 = t.c - t.b;
	vec3 f2 = t.a - t.c;

	// Compute the face normals of the AABB
	const float* orientation = o.orientation.asArray;
	vec3 u0(orientation[0], orientation[1], orientation[2]);
	vec3 u1(orientation[3], orientation[4], orientation[5]);
	vec3 u2(orientation[6], orientation[7], orientation[8]);

	vec3 test[13] = {
		// 3 Normals of AABB
		u0, // AABB Axis 1
		u1, // AABB Axis 2
		u2, // AABB Axis 3
		// 1 Normal of the Triangle
		Cross(f0, f1),
		// 9 Axis, cross products of all edges
		Cross(u0, f0),
		Cross(u0, f1),
		Cross(u0, f2),
		Cross(u1, f0),
		Cross(u1, f1),
		Cross(u1, f2),
		Cross(u2, f0),
		Cross(u2, f1),
		Cross(u2, f2)
	};

	for (int i = 0; i < 13; ++i) {
		if (!OverlapOnAxis(o, t, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool TriangleTriangle(const Triangle& t1, const Triangle& t2) {
#if 0
	vec3 axisToTest[] = {
		// Triangle 1, Normal
		SatCrossEdge(t1.a, t1.b, t1.b, t1.c),
		// Triangle 2, Normal
		SatCrossEdge(t2.a, t2.b, t2.b, t2.c),

		// Cross Product of edges
		SatCrossEdge(t2.a, t2.b, t1.a, t1.b),
		SatCrossEdge(t2.a, t2.b, t1.b, t1.c),
		SatCrossEdge(t2.a, t2.b, t1.c, t1.a),

		SatCrossEdge(t2.b, t2.c, t1.a, t1.b),
		SatCrossEdge(t2.b, t2.c, t1.b, t1.c),
		SatCrossEdge(t2.b, t2.c, t1.c, t1.a),

		SatCrossEdge(t2.c, t2.a, t1.a, t1.b),
		SatCrossEdge(t2.c, t2.a, t1.b, t1.c),
		SatCrossEdge(t2.c, t2.a, t1.c, t1.a),
	};
#else 
	vec3 t1_f0 = t1.b - t1.a; // Edge 0
	vec3 t1_f1 = t1.c - t1.b; // Edge 1
	vec3 t1_f2 = t1.a - t1.c; // Edge 2

	vec3 t2_f0 = t2.b - t2.a; // Edge 0
	vec3 t2_f1 = t2.c - t2.b; // Edge 1
	vec3 t2_f2 = t2.a - t2.c; // Edge 2

	vec3 axisToTest[] = {
		// Triangle 1, Normal
		Cross(t1_f0, t1_f1),
		// Triangle 2, Normal
		Cross(t2_f0, t2_f1),

		// Cross Product of edges
		Cross(t2_f0, t1_f0),
		Cross(t2_f0, t1_f1),
		Cross(t2_f0, t1_f2),

		Cross(t2_f1, t1_f0),
		Cross(t2_f1, t1_f1),
		Cross(t2_f1, t1_f2),

		Cross(t2_f2, t1_f0),
		Cross(t2_f2, t1_f1),
		Cross(t2_f2, t1_f2),
	};
#endif

	for (int i = 0; i < 11; ++i) {
		if (!OverlapOnAxis(t1, t2, axisToTest[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool TriangleTriangleRobust(const Triangle& t1, const Triangle& t2) {
	vec3 axisToTest[] = {
		// Triangle 1, Normal
		SatCrossEdge(t1.a, t1.b, t1.b, t1.c),
		// Triangle 2, Normal
		SatCrossEdge(t2.a, t2.b, t2.b, t2.c),

		// Cross Product of edges
		SatCrossEdge(t2.a, t2.b, t1.a, t1.b),
		SatCrossEdge(t2.a, t2.b, t1.b, t1.c),
		SatCrossEdge(t2.a, t2.b, t1.c, t1.a),

		SatCrossEdge(t2.b, t2.c, t1.a, t1.b),
		SatCrossEdge(t2.b, t2.c, t1.b, t1.c),
		SatCrossEdge(t2.b, t2.c, t1.c, t1.a),

		SatCrossEdge(t2.c, t2.a, t1.a, t1.b),
		SatCrossEdge(t2.c, t2.a, t1.b, t1.c),
		SatCrossEdge(t2.c, t2.a, t1.c, t1.a),
	};

	for (int i = 0; i < 11; ++i) {
		if (!OverlapOnAxis(t1, t2, axisToTest[i])) {
			if (!CMP(MagnitudeSq(axisToTest[i]), 0)) {
				return false; // Seperating axis found
			}
		}
	}

	return true; // Seperating axis not found
}

vec3 SatCrossEdge(const vec3& a, const vec3& b, const vec3& c, const vec3& d) {
	vec3 ab = b - a;
	vec3 cd = d - c;

	vec3 result = Cross(ab, cd);
	if (!CMP(MagnitudeSq(result), 0)) { // Is ab and cd parallel?
		return result; // Not parallel!
	}
	else { // ab and cd are parallel
		// Get an axis perpendicular to AB
		vec3 axis = Cross(ab, c - a);
		result = Cross(ab, axis);
		if (!CMP(MagnitudeSq(result), 0)) { // Still parallel?
			return result; // Not parallel
		}
	}
	// New axis being tested is parallel too.
	// This means that a, b, c and d are on a line
	// Nothing we can do!
	return vec3();
}

Point debugRaycastResult;

bool Raycast(const Triangle& triangle, const Ray& ray, RaycastResult* outResult) {
	ResetRaycastResult(outResult);
	Plane plane = FromTriangle(triangle);

	RaycastResult planeResult;
	if (!Raycast(plane, ray, &planeResult)) {
		return false;
	}
	float t = planeResult.t;

	Point result = ray.origin + ray.direction * t;
	
	vec3 barycentric = Barycentric(result, triangle);
	if (barycentric.x >= 0.0f && barycentric.x <= 1.0f &&
		barycentric.y >= 0.0f && barycentric.y <= 1.0f &&
		barycentric.z >= 0.0f && barycentric.z <= 1.0f) {

#ifdef DO_SANITY_TESTS
		if (!PointInTriangle(result, triangle)) {
			std::cout << "Point in triangle and barycentric coordinates don't match!\n";
		}
#endif
		if (outResult != 0) {
			outResult->t = t;
			outResult->hit = true;
			outResult->point = ray.origin + ray.direction * t;
			outResult->normal = plane.normal;
		}

		return true;
	}
#ifdef DO_SANITY_TESTS
	else if (PointInTriangle(result, triangle)) {
		std::cout << "Point in triangle and barycentric coordinates don't match!\n";
	}
#endif

	return false;
}

bool Linetest(const Triangle& triangle, const Line& line) {
	Ray ray;
	ray.origin = line.start;
	ray.direction = Normalized(line.end - line.start);
	RaycastResult raycast;
	if (!Raycast(triangle, ray, &raycast)) {
		return false;
	}
	float t = raycast.t;

	return t >= 0 && t * t <= LengthSq(line);
}

void AccelerateMesh(Mesh& mesh) {
	if (mesh.accelerator != 0) {
		return;
	}

	vec3 min = mesh.vertices[0];
	vec3 max = mesh.vertices[0];

	for (int i = 1; i < mesh.numTriangles * 3; ++i) {
		min.x = fminf(mesh.vertices[i].x, min.x);
		min.y = fminf(mesh.vertices[i].y, min.y);
		min.z = fminf(mesh.vertices[i].z, min.z);
	
		max.x = fmaxf(mesh.vertices[i].x, max.x);
		max.y = fmaxf(mesh.vertices[i].y, max.y);
		max.z = fmaxf(mesh.vertices[i].z, max.z);
	}

	mesh.accelerator = new BVHNode();
	mesh.accelerator->bounds = FromMinMax(min, max);
	mesh.accelerator->children = 0;
	mesh.accelerator->numTriangles = mesh.numTriangles;
	mesh.accelerator->triangles = new int[mesh.numTriangles];
	for (int i = 0; i < mesh.numTriangles; ++i) {
		mesh.accelerator->triangles[i] = i;
	}

	SplitBVHNode(mesh.accelerator, mesh, 3);
}

void SplitBVHNode(BVHNode* node, const Mesh& model, int depth) {
	if (depth-- <= 0) { // Decrements depth
		return;
	}

	if (node->children == 0) {
		// Only split if this node contains triangles
		if (node->numTriangles > 0) {
			node->children = new BVHNode[8];

			vec3 c = node->bounds.position;
			vec3 e = node->bounds.size *0.5f;

			node->children[0].bounds = AABB(c + vec3(-e.x, +e.y, -e.z), e);
			node->children[1].bounds = AABB(c + vec3(+e.x, +e.y, -e.z), e);
			node->children[2].bounds = AABB(c + vec3(-e.x, +e.y, +e.z), e);
			node->children[3].bounds = AABB(c + vec3(+e.x, +e.y, +e.z), e);
			node->children[4].bounds = AABB(c + vec3(-e.x, -e.y, -e.z), e);
			node->children[5].bounds = AABB(c + vec3(+e.x, -e.y, -e.z), e);
			node->children[6].bounds = AABB(c + vec3(-e.x, -e.y, +e.z), e);
			node->children[7].bounds = AABB(c + vec3(+e.x, -e.y, +e.z), e);

		}
	}

	// If this node was just split
	if (node->children != 0 && node->numTriangles > 0) {
		for (int i = 0; i < 8; ++i) { // For each child
			// Count how many triangles each child will contain
			node->children[i].numTriangles = 0;
			for (int j = 0; j < node->numTriangles; ++j) {
				Triangle t = model.triangles[node->triangles[j]];
				if (TriangleAABB(t, node->children[i].bounds)) {
					node->children[i].numTriangles += 1;
				}
			}
			if (node->children[i].numTriangles == 0) {
				continue;
			}
			node->children[i].triangles = new int[node->children[i].numTriangles];
			int index = 0; // Add the triangles in the new child arrau
			for (int j = 0; j < node->numTriangles; ++j) {
				Triangle t = model.triangles[node->triangles[j]];
				if (TriangleAABB(t, node->children[i].bounds)) {
					node->children[i].triangles[index++] = node->triangles[j];
				}
			}
		}

		node->numTriangles = 0;
		delete[] node->triangles;
		node->triangles = 0;

		// Recurse
		for (int i = 0; i < 8; ++i) {
			SplitBVHNode(&node->children[i], model, depth);
		}
	}
}

void FreeBVHNode(BVHNode* node) {
	if (node->children != 0) {
		for (int i = 0; i < 8; ++i) {
			FreeBVHNode(&node->children[i]);
		}
		delete[] node->children;
		node->children = 0;
	}

	if (node->numTriangles != 0 || node->triangles != 0) {
		delete[] node->triangles;
		node->triangles = 0;
		node->numTriangles = 0;
	}
}

bool MeshAABB(const Mesh& mesh, const AABB& aabb) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (TriangleAABB(mesh.triangles[i], aabb)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (TriangleAABB(mesh.triangles[iterator->triangles[i]], aabb)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (AABBAABB(iterator->children[i].bounds, aabb)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

bool Linetest(const Mesh& mesh, const Line& line) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (Linetest(mesh.triangles[i], line)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (Linetest(mesh.triangles[iterator->triangles[i]], line)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (Linetest(iterator->children[i].bounds, line)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

bool MeshSphere(const Mesh& mesh, const Sphere& sphere) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (TriangleSphere(mesh.triangles[i], sphere)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (TriangleSphere(mesh.triangles[iterator->triangles[i]], sphere)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (SphereAABB(sphere, iterator->children[i].bounds)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

bool MeshOBB(const Mesh& mesh, const OBB& obb) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (TriangleOBB(mesh.triangles[i], obb)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (TriangleOBB(mesh.triangles[iterator->triangles[i]], obb)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (AABBOBB(iterator->children[i].bounds, obb)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

bool MeshPlane(const Mesh& mesh, const Plane& plane) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (TrianglePlane(mesh.triangles[i], plane)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (TrianglePlane(mesh.triangles[iterator->triangles[i]], plane)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (AABBPlane(iterator->children[i].bounds, plane)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

bool MeshTriangle(const Mesh& mesh, const Triangle& triangle) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			if (TriangleTriangle(mesh.triangles[i], triangle)) {
				return true;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					if (TriangleTriangle(mesh.triangles[iterator->triangles[i]], triangle)) {
						return true;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					if (TriangleAABB(triangle, iterator->children[i].bounds)) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return false;
}

#ifndef NO_EXTRAS
float Raycast(const Mesh& mesh, const Ray& ray) {
	return MeshRay(mesh, ray);
}
float Raycast(const Model& mesh, const Ray& ray) {
	return ModelRay(mesh, ray);
}
#endif 

float MeshRay(const Mesh& mesh, const Ray& ray) {
	if (mesh.accelerator == 0) {
		for (int i = 0; i < mesh.numTriangles; ++i) {
			RaycastResult raycast;
			Raycast(mesh.triangles[i], ray, &raycast);
			float result = raycast.t;
			if (result >= 0) {
				return result;
			}
		}
	}
	else {
		std::list<BVHNode*> toProcess;
		toProcess.push_front(mesh.accelerator);

		// Recursivley walk the BVH tree
		while (!toProcess.empty()) {
			BVHNode* iterator = *(toProcess.begin());
			toProcess.erase(toProcess.begin());

			if (iterator->numTriangles >= 0) {
				// Iterate trough all triangles of the node
				for (int i = 0; i < iterator->numTriangles; ++i) {
					// Triangle indices in BVHNode index the mesh
					RaycastResult raycast;
					Raycast(mesh.triangles[iterator->triangles[i]], ray, &raycast);
					float r = raycast.t;
					if (r >= 0) {
						return r;
					}
				}
			}

			if (iterator->children != 0) {
				for (int i = 8 - 1; i >= 0; --i) {
					// Only push children whos bounds intersect the test geometry
					RaycastResult raycast;
					Raycast(iterator->children[i].bounds, ray, &raycast);
					if (raycast.t >= 0) {
						toProcess.push_front(&iterator->children[i]);
					}
				}
			}
		}
	}
	return -1;
}

bool TrianglePlane(const Triangle& t, const Plane& p) {
	float side1 = PlaneEquation(t.a, p);
	float side2 = PlaneEquation(t.b, p);
	float side3 = PlaneEquation(t.c, p);

	// On Plane
	if (CMP(side1, 0) && CMP(side2, 0) && CMP(side3, 0)) {
		return true;
	}

	// Triangle in front of plane
	if (side1 > 0 && side2 > 0 && side3 > 0) {
		return false;
	}

	// Triangle behind plane
	if (side1 < 0 && side2 < 0 && side3 < 0) {
		return false;
	}

	return true; // Intersection
}

void Model::SetContent(Mesh* mesh) {
	content = mesh;
	if (content != 0) {
		vec3 min = mesh->vertices[0];
		vec3 max = mesh->vertices[0];

		for (int i = 1; i < mesh->numTriangles * 3; ++i) {
			min.x = fminf(mesh->vertices[i].x, min.x);
			min.y = fminf(mesh->vertices[i].y, min.y);
			min.z = fminf(mesh->vertices[i].z, min.z);

			max.x = fmaxf(mesh->vertices[i].x, max.x);
			max.y = fmaxf(mesh->vertices[i].y, max.y);
			max.z = fmaxf(mesh->vertices[i].z, max.z);
		}
		bounds = FromMinMax(min, max);
	}
}

mat4 GetWorldMatrix(const Model& model) {
	mat4 translation = Translation(model.position);
	mat4 rotation = Rotation(model.rotation.x, model.rotation.y, model.rotation.z);
	mat4 localMat = /* Scale * */ rotation * translation;
	
	mat4 parentMat;
	if (model.parent != 0) {
		parentMat = GetWorldMatrix(*model.parent);
	}

	return localMat * parentMat;
}

OBB GetOBB(const Model& model) {
	mat4 world = GetWorldMatrix(model);
	AABB aabb = model.GetBounds();
	OBB obb;

	obb.size = aabb.size;
	obb.position = MultiplyPoint(aabb.position, world);
	obb.orientation = Cut(world, 3, 3);

	return obb;
}

float ModelRay(const Model& model, const Ray& ray) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	Ray local;
	local.origin = MultiplyPoint(ray.origin, inv);
	local.direction = MultiplyVector(ray.direction, inv);
	local.NormalizeDirection();
	if (model.GetMesh() != 0) {
		return MeshRay(*(model.GetMesh()), local);
	}
	return -1;
}

bool Linetest(const Model& model, const Line& line) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	Line local;
	local.start = MultiplyPoint(line.start, inv);
	local.end = MultiplyPoint(line.end, inv);
	if (model.GetMesh() != 0) {
		return Linetest(*(model.GetMesh()), local);
	}
	return false;
}

bool ModelSphere(const Model& model, const Sphere& sphere) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	Sphere local;
	local.position = MultiplyPoint(sphere.position, inv);
	if (model.GetMesh() != 0) {
		return MeshSphere(*(model.GetMesh()), local);
	}
	return false;
}

bool ModelAABB(const Model& model, const AABB& aabb) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	OBB local;
	local.size = aabb.size;
	local.position = MultiplyPoint(aabb.position, inv);
	local.orientation = Cut(inv, 3, 3);
	if (model.GetMesh() != 0) {
		return MeshOBB(*(model.GetMesh()), local);
	}
	return false;
}

bool ModelOBB(const Model& model, const OBB& obb) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	OBB local;
	local.size = obb.size;
	local.position = MultiplyPoint(obb.position, inv);
	local.orientation = obb.orientation * Cut(inv, 3, 3);
	if (model.GetMesh() != 0) {
		return MeshOBB(*(model.GetMesh()), local);
	}
	return false;
}

bool ModelPlane(const Model& model, const Plane& plane) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	Plane local;
	local.normal = MultiplyVector(plane.normal, inv);
	local.distance = plane.distance;
	if (model.GetMesh() != 0) {
		return MeshPlane(*(model.GetMesh()), local);
	}
	return false;
}

bool ModelTriangle(const Model& model, const Triangle& triangle) {
	mat4 world = GetWorldMatrix(model);
	mat4 inv = Inverse(world);
	Triangle local;
	local.a = MultiplyPoint(triangle.a, inv);
	local.b = MultiplyPoint(triangle.b, inv);
	local.c = MultiplyPoint(triangle.c, inv);
	if (model.GetMesh() != 0) {
		return MeshTriangle(*(model.GetMesh()), local);
	}
	return false;
}

Point Intersection(Plane p1, Plane p2, Plane p3) {
	/*return ((Cross(p2.normal, p3.normal) * -p1.distance) +
		(Cross(p3.normal, p1.normal) * -p2.distance) +
		(Cross(p1.normal, p2.normal) * -p3.distance)) /
		(Dot(p1.normal, Cross(p2.normal, p3.normal)));*/
		
#if 1
	mat3 D(
		p1.normal.x, p2.normal.x, p3.normal.x,
		p1.normal.y, p2.normal.y, p3.normal.y,
		p1.normal.z, p2.normal.z, p3.normal.z
	);
	vec3 A(-p1.distance, -p2.distance, -p3.distance);

	mat3 Dx = D, Dy = D, Dz = D;
	Dx._11 = A.x; Dx._12 = A.y; Dx._13 = A.z;
	Dy._21 = A.x; Dy._22 = A.y; Dy._23 = A.z;
	Dz._31 = A.x; Dz._32 = A.y; Dz._33 = A.z;

	float detD = Determinant(D);

	if (CMP(detD, 0)) {
		return Point();
	}

	float detDx = Determinant(Dx);
	float detDy = Determinant(Dy);
	float detDz = Determinant(Dz);

	return Point(detDx / detD, detDy / detD, detDz / detD);
#else 
	vec3 m1(p1.normal.x, p2.normal.x, p3.normal.x);
	vec3 m2(p1.normal.y, p2.normal.y, p3.normal.y);
	vec3 m3(p1.normal.z, p2.normal.z, p3.normal.z);
	vec3 d(-p1.distance, -p2.distance, -p3.distance);
	
	vec3 u = Cross(m2, m3);
	vec3 v = Cross(m1, d);
	float denom = Dot(m1, u);

	if (CMP(denom, 0.0f)) {
		return Point();
	}

	Point result;
	result.x = Dot(d, u) / denom;
	result.y = Dot(m3, v) / denom;
	result.z = -Dot(m2, v) / denom;
	return result;
#endif
}

void GetCorners(const Frustum& f, vec3* outCorners) {
	outCorners[0] = Intersection(f.near, f.top,    f.left);
	outCorners[1] = Intersection(f.near, f.top,    f.right);
	outCorners[2] = Intersection(f.near, f.bottom, f.left);
	outCorners[3] = Intersection(f.near, f.bottom, f.right);
	outCorners[4] = Intersection(f.far,  f.top,    f.left);
	outCorners[5] = Intersection(f.far,  f.top,    f.right);
	outCorners[6] = Intersection(f.far,  f.bottom, f.left);
	outCorners[7] = Intersection(f.far,  f.bottom, f.right);
}

bool Intersects(const Frustum& f, const Point& p) {
	for (int i = 0; i < 6; ++i) {
		vec3 normal = f.planes[i].normal;
		float dist = f.planes[i].distance;
		float side = Dot(p, normal) + dist;
		if (side < 0.0f) {
			return false;
		}
	}

	return true;
}

bool Intersects(const Frustum& f, const Sphere& s) {
	for (int i = 0; i < 6; ++i) {
		vec3 normal = f.planes[i].normal;
		float dist = f.planes[i].distance;
		float side = Dot(s.position, normal) + dist;
		if (side < -s.radius) {
			return false;
		}
	}

	return true;
}

float Classify(const AABB& aabb, const Plane& plane) {
	// maximum extent in direction of plane normal 
	float r = fabsf(aabb.size.x * plane.normal.x)
		+ fabsf(aabb.size.y * plane.normal.y)
		+ fabsf(aabb.size.z * plane.normal.z);

	// signed distance between box center and plane
	//float d = plane.Test(mCenter);
	float d = Dot(plane.normal, aabb.position) + plane.distance;

	// return signed distance
	if (fabsf(d) < r) {
		return 0.0f;
	}
	else if (d < 0.0f) {
		return d + r;
	}
	return d - r;
}

float Classify(const OBB& obb, const Plane& plane) {
	vec3 normal = MultiplyVector(plane.normal, obb.orientation);

	// maximum extent in direction of plane normal 
	float r = fabsf(obb.size.x * normal.x)
		+ fabsf(obb.size.y * normal.y)
		+ fabsf(obb.size.z * normal.z);

	// signed distance between box center and plane
	//float d = plane.Test(mCenter);
	float d = Dot(plane.normal, obb.position) + plane.distance;

	// return signed distance
	if (fabsf(d) < r) {
		return 0.0f;
	}
	else if (d < 0.0f) {
		return d + r;
	}
	return d - r;
}

bool Intersects(const Frustum& f, const OBB& obb) {
	for (int i = 0; i < 6; ++i) {
		float side = Classify(obb, f.planes[i]);
		if (side < 0) {
			return false;
		}
	}
	return true;
}

bool Intersects(const Frustum& f, const AABB& aabb) {
	for (int i = 0; i < 6; ++i) {
		float side = Classify(aabb, f.planes[i]);
		if (side < 0) {
			return false;
		}
	}
	return true;
}

vec3 Unproject(const vec3& viewportPoint, const vec2& viewportOrigin, const vec2& viewportSize, const mat4& view, const mat4& projection) {
	// Step 1, Normalize the input vector to the view port
	float normalized[4] = {
		(viewportPoint.x - viewportOrigin.x) / viewportSize.x,
		(viewportPoint.y - viewportOrigin.y) / viewportSize.y,
		viewportPoint.z,
		1.0f
	};

	// Step 2, Translate into NDC space
	float ndcSpace[4] = {
		normalized[0], normalized[1],
		normalized[2], normalized[3]
	};
	// X Range: -1 to 1
	ndcSpace[0] = ndcSpace[0] * 2.0f - 1.0f;
	// Y Range: -1 to 1, our Y axis is flipped!
	ndcSpace[1] = 1.0f - ndcSpace[1] * 2.0f;
	// Z Range: 0 to 1
	if (ndcSpace[2] < 0.0f) {
		ndcSpace[2] = 0.0f;
	}
	if (ndcSpace[2] > 1.0f) {
		ndcSpace[2] = 1.0f;
	}

	// Step 3, NDC to Eye Space
	mat4 invProjection = Inverse(projection);
	float eyeSpace[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
	// eyeSpace = MultiplyPoint(ndcSpace, invProjection);
	Multiply(eyeSpace, ndcSpace, 1, 4, invProjection.asArray, 4, 4);

	// Step 4, Eye Space to World Space
	mat4 invView = Inverse(view);
	float worldSpace[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
	// worldSpace = MultiplyPoint(eyeSpace, invView);
	Multiply(worldSpace, eyeSpace, 1, 4, invView.asArray, 4, 4);

	// Step 5, Undo perspective divide!
	if (!CMP(worldSpace[3], 0.0f)) {
		worldSpace[0] /= worldSpace[3];
		worldSpace[1] /= worldSpace[3];
		worldSpace[2] /= worldSpace[3];
	}

	// Return the resulting world space point
	return vec3(worldSpace[0], worldSpace[1], worldSpace[2]);
}

Ray GetPickRay(const vec2& viewportPoint, const vec2& viewportOrigin, const vec2& viewportSize, const mat4& view, const mat4& projection) {
	vec3 nearPoint(viewportPoint.x, viewportPoint.y, 0.0f);
	vec3 farPoint(viewportPoint.x, viewportPoint.y, 1.0f);

	vec3 pNear = Unproject(nearPoint, viewportOrigin, viewportSize, view, projection);
	vec3 pFar = Unproject(farPoint, viewportOrigin, viewportSize, view, projection);

	vec3 normal = Normalized(pFar - pNear);
	vec3 origin = pNear;

	return Ray(origin, normal);
}

<|start_filename|>Chapter03/Code/IWindow.h<|end_filename|>
#ifndef _H_I_WINDOW_
#define _H_I_WINDOW_

class IWindow {
	friend void CleanupMemory(IWindow* window);
private:
	IWindow(const IWindow&) {}
	IWindow& operator=(const IWindow&) {}
#if ENABLE_USER_DATA
	void InitGenericIntegers();
#endif
protected:
	static IWindow* g_pSingleton;
	bool m_bQuitFlag;
	bool m_bFullscreenFlag;
	int m_nWidth;
	int m_nHeight;
	bool b_isVisible;

	char* m_szTitle;
	bool m_bTitleDirty;

#if ENABLE_USER_DATA
	int* m_vGenericIntegerValues;
	char** m_vGenericIntegerNames;
	int m_nGenericIntegerCount; // Active
	int m_nGenericIntegerLength; // Total
#endif

	int m_nTargetFPS;
	int m_nFixedFPS;
public:
	inline static IWindow* GetInstance() {
		return g_pSingleton;
	}

	IWindow();
	IWindow(const char* title);
	IWindow(const char* title, int width, int height);

	virtual ~IWindow();

	virtual void OnInitialize() { }
	virtual void OnUpdate(float deltaTime) { }
	virtual void OnFixedUpdate(float fixedDeltaTime) { } 
	virtual void OnRender() { }
	virtual void OnShutdown() { }

	virtual void OnResize(int width, int height) { } 
	
	virtual void OnMouseMove(int x, int y) { } 
	virtual void OnMouseDown(int mouseCode) { }
	virtual void OnMouseUp(int mouseCode) { }

	virtual void OnKeyDown(int keyCode) { }
	virtual void OnKeyUp(int keyCode) { }

	void SetTitle(const char* title);
	bool GetAndResetTitleDirtyFlag();
	const char* GetTitle();

	void SetFullScreen(bool value);
	bool GetFullScreen();
	
	void Resize(int width, int height);
	int GetWidth();
	int GetHeight();
	
	void Close();
	bool GetQuitFlag();

	void SetTargetFPS(int target);
	int GetTargetFPS();
	int GetFixedFPS();

	void MarkAsShown();
	bool WasWindowShown();

#if ENABLE_USER_DATA
	void SetInt(const char* name, int value);
	int GetInt(const char* name, int default);
	bool HasInt(const char* name);
#endif
};

void CleanupMemory(IWindow* window);
int KeyIndex(int keyCode);

#define MOUSE_LEFT		1
#define MOUSE_MIDDLE	2
#define MOUSE_RIGHT		3

#define KEY_NONE              0
#define KEY_PAUSE             0x0013
#define KEY_SCROLL_LOCK       0x1014
#define KEY_PRINT             0x1061
#define KEY_SYSREQ            0x106A
#define KEY_BREAK             0x106B
#define KEY_ESCAPE            0x001B
#define KEY_BACKSPACE         0x0008
#define KEY_TAB               0x0009
#define KEY_BACK_TAB          0x0089
#define KEY_RETURN            0x000D
#define KEY_CAPS_LOCK         0x00E5
#define KEY_SHIFT             0x00E1
#define KEY_CTRL              0x00E3
#define KEY_ALT               0x00E9
#define KEY_MENU              0x1067
#define KEY_HYPER             0x10ED
#define KEY_INSERT            0x1063
#define KEY_HOME              0x1050
#define KEY_PG_UP             0x1055
#define KEY_DELETE            0x10FF
#define KEY_END               0x1057
#define KEY_PG_DOWN           0x1056
#define KEY_LEFT_ARROW        0x1051
#define KEY_RIGHT_ARROW       0x1053
#define KEY_UP_ARROW          0x1052
#define KEY_DOWN_ARROW        0x1054
#define KEY_NUM_LOCK          0x107F
#define KEY_KP_PLUS           0x10AB
#define KEY_KP_MINUS          0x10AD
#define KEY_KP_MULTIPLY       0x10AA
#define KEY_KP_DIVIDE         0x10AF
#define KEY_KP_ENTER          0x108D
#define KEY_KP_HOME           0x10B7
#define KEY_KP_UP             0x10B8
#define KEY_KP_PG_UP          0x10B9
#define KEY_KP_LEFT           0x10B4
#define KEY_KP_FIVE           0x10B5
#define KEY_KP_RIGHT          0x10B6
#define KEY_KP_END            0x10B1
#define KEY_KP_DOWN           0x10B2
#define KEY_KP_PG_DOWN        0x10B3
#define KEY_KP_INSERT         0x10B0
#define KEY_KP_DELETE         0x10AE
#define KEY_F1                0x00BE
#define KEY_F2                0x00BF
#define KEY_F3                0x00C0
#define KEY_F4                0x00C1
#define KEY_F5                0x00C2
#define KEY_F6                0x00C3
#define KEY_F7                0x00C4
#define KEY_F8                0x00C5
#define KEY_F9                0x00C6
#define KEY_F10               0x00C7
#define KEY_F11               0x00C8
#define KEY_F12               0x00C9
#define KEY_SPACE             ' '
#define KEY_EXCLAM            '!'
#define KEY_QUOTE             '"'
#define KEY_NUMBER            '#'
#define KEY_DOLLAR            '$'
#define KEY_PERCENT           '%'
#define KEY_CIRCUMFLEX        '^'
#define KEY_AMPERSAND         '&'
#define KEY_APOSTROPHE        '\''
#define KEY_LEFT_PARENTHESIS  '('
#define KEY_RIGHT_PARENTHESIS ')'
#define KEY_ASTERISK          '*'
#define KEY_PLUS              '+'
#define KEY_COMMA             ','
#define KEY_MINUS             '-'
#define KEY_PERIOD            '.'
#define KEY_SLASH             '/'
#define KEY_ZERO              '0'
#define KEY_ONE               '1'
#define KEY_TWO               '2'
#define KEY_THREE             '3'
#define KEY_FOUR              '4'
#define KEY_FIVE              '5'
#define KEY_SIX               '6'
#define KEY_SEVEN             '7'
#define KEY_EIGHT             '8'
#define KEY_NINE              '9'
#define KEY_0				  '0'
#define KEY_1                 '1'
#define KEY_2                 '2'
#define KEY_3				  '3'
#define KEY_4				  '4'
#define KEY_5				  '5'
#define KEY_6				  '6'
#define KEY_7				  '7'
#define KEY_8				  '8'
#define KEY_9				  '9'
#define KEY_COLON             ':'
#define KEY_SEMICOLON         ';'
#define KEY_LESS_THAN         '<'
#define KEY_EQUAL             '='
#define KEY_GREATER_THAN      '>'
#define KEY_QUESTION          '?'
#define KEY_AT                '@'
#define KEY_CAPITAL_A         'A'
#define KEY_CAPITAL_B         'B'
#define KEY_CAPITAL_C         'C'
#define KEY_CAPITAL_D         'D'
#define KEY_CAPITAL_E         'E'
#define KEY_CAPITAL_F         'F'
#define KEY_CAPITAL_G         'G'
#define KEY_CAPITAL_H         'H'
#define KEY_CAPITAL_I         'I'
#define KEY_CAPITAL_J         'J'
#define KEY_CAPITAL_K         'K'
#define KEY_CAPITAL_L         'L'
#define KEY_CAPITAL_M         'M'
#define KEY_CAPITAL_N         'N'
#define KEY_CAPITAL_O         'O'
#define KEY_CAPITAL_P         'P'
#define KEY_CAPITAL_Q         'Q'
#define KEY_CAPITAL_R         'R'
#define KEY_CAPITAL_S         'S'
#define KEY_CAPITAL_T         'T'
#define KEY_CAPITAL_U         'U'
#define KEY_CAPITAL_V         'V'
#define KEY_CAPITAL_W         'W'
#define KEY_CAPITAL_X         'X'
#define KEY_CAPITAL_Y         'Y'
#define KEY_CAPITAL_Z         'Z'
#define KEY_LEFT_BRACKET      '['
#define KEY_BACK_SLASH        '\\'
#define KEY_RIGHT_BRACKET     ']'
#define KEY_UNDERSCORE        '_'
#define KEY_GRAVE             '`'
#define KEY_A                 'a'
#define KEY_B                 'b'
#define KEY_C                 'c'
#define KEY_D                 'd'
#define KEY_E                 'e'
#define KEY_F                 'f'
#define KEY_G                 'g'
#define KEY_H                 'h'
#define KEY_I                 'i'
#define KEY_J                 'j'
#define KEY_K                 'k'
#define KEY_L                 'l'
#define KEY_M                 'm'
#define KEY_N                 'n'
#define KEY_O                 'o'
#define KEY_P                 'p'
#define KEY_Q                 'q'
#define KEY_R                 'r'
#define KEY_S                 's'
#define KEY_T                 't'
#define KEY_U                 'u'
#define KEY_V                 'v'
#define KEY_W                 'w'
#define KEY_X                 'x'
#define KEY_Y                 'y'
#define KEY_Z                 'z'
#define KEY_LEFT_BRACE        '{'
#define KEY_BAR               '|'
#define KEY_RIGHT_BRACE       '}'
#define KEY_TILDE             '~'
#define KEY_EURO              0x20AC
#define KEY_POUND             0x00A3
#define KEY_YEN               0x00A5
#define KEY_MIDDLE_DOT        0x0095
#define KEY_SEARCH            0xFFAA

#endif

<|start_filename|>Chapter02/Code/imgui/imgui_implementation.h<|end_filename|>
#ifndef _H_IMGUI_IMPLEMENTATION_
#define _H_IMGUI_IMPLEMENTATION_

#define WIN32_LEAN_AND_MEAN 
#define WIN32_EXTRA_LEAN
#include <windows.h>

// Only the WinMain file should have access to this
// Implementation of this file will probably be per platform

bool ImGui_Implementation_Init(void* hwnd);
void ImGui_Implementation_Shutdown();
bool ImGui_Implementation_CreateDeviceObjects();
void ImGui_Implementation_InvalidateDeviceObjects();
void ImGui_Implementation_RenderDrawLists(struct ImDrawData* draw_data);
void ImGui_Implementation_NewFrame();
// Will need to wrap this function in a #define for the platform being used
void ImGui_Implementation_WndProcHandler(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam);
bool ImGui_Implementation_HasMouse();
bool ImGui_Implementation_HasKeyboard();

#endif


<|start_filename|>Chapter01/Code/Particle.h<|end_filename|>
#ifndef _H_PARTICLE_
#define _H_PARTICLE_

#include "Rigidbody.h";

// The particles will either use euler or verlet integration
// If EULER_INTEGRATION is defined, euler integration will
// be used. Otherwise, verlet integration is the default

// If ACCURATE_EULER_INTEGRATION is defined, a slightly more
// accurate integration model is used. This should help 
// keep the simulation stable over long periods of time

#define EULER_INTEGRATION
//#define ACCURATE_EULER_INTEGRATION

class Particle : public Rigidbody {
	vec3 position;
	vec3 oldPosition;
	vec3 forces;

	vec3 gravity;
	float friction;
	float bounce;

#ifdef EULER_INTEGRATION
	vec3 velocity;
#endif
	float mass;
public:
	Particle();

	void Update(float deltaTime);
	void Render();
	void ApplyForces();
	void SolveConstraints(const std::vector<OBB>& constraints);

	void SetPosition(const vec3& pos);
	vec3 GetPosition();

	void SetBounce(float b);
	float GetBounce();

	void AddImpulse(const vec3& impulse);
	float InvMass();
	void SetMass(float m);
	vec3 GetVelocity();
	void SetFriction(float f);
};

#endif


<|start_filename|>Chapter15/Physics/matrices.h<|end_filename|>
#ifndef _H_MATH_MATRICES_
#define _H_MATH_MATRICES_

#include "vectors.h"

typedef struct mat2 {
	union {
		struct {
			float	_11, _12,
					_21, _22;
		};
		float asArray[4];
	};

	inline mat2() { 
		_11 = _22 = 1.0f;
		_12 = _21 = 0.0f;
	}

	inline mat2(float f11, float f12,
				float f21, float f22) {
		_11 = f11; _12 = f12;
		_21 = f21; _22 = f22;
	}

	inline float* operator[](int i) {
		return &(asArray[i * 2]);
	}
} mat2;

typedef struct mat3 {
	union {
		struct {
			float	_11, _12, _13,
					_21, _22, _23,
					_31, _32, _33;
		};
		float asArray[9];
	};

	inline mat3() {
		_11 = _22 = _33 = 1.0f;
		_12 = _13 = _21 = 0.0f;
		_23 = _32 = _32 = 0.0f;
	}

	inline mat3(float f11, float f12, float f13,
				float f21, float f22, float f23,
				float f31, float f32, float f33) {
		_11 = f11; _12 = f12; _13 = f13;
		_21 = f21; _22 = f22; _23 = f23;
		_31 = f31; _32 = f32; _33 = f33;
	}

	inline float* operator[](int i) {
		return &(asArray[i * 3]);
	}
} mat3;

typedef struct mat4 {
	union {
		struct {
			float	_11, _12, _13, _14,
					_21, _22, _23, _24,
					_31, _32, _33, _34,
					_41, _42, _43, _44;
		};
		float asArray[16];
	};

	inline mat4() {
		_11 = _22 = _33 = _44 = 1.0f;
		_12 = _13 = _14 = _21 = 0.0f;
		_23 = _24 = _31 = _32 = 0.0f;
		_34 = _41 = _42 = _43 = 0.0f;
	}

	inline mat4(float f11, float f12, float f13, float f14,
				float f21, float f22, float f23, float f24,
				float f31, float f32, float f33, float f34,
				float f41, float f42, float f43, float f44) {
		_11 = f11; _12 = f12; _13 = f13; _14 = f14;
		_21 = f21; _22 = f22; _23 = f23; _24 = f24;
		_31 = f31; _32 = f32; _33 = f33; _34 = f34;
		_41 = f41; _42 = f42; _43 = f43; _44 = f44;
	}

	inline float* operator[](int i) {
		return &(asArray[i * 4]);
	}
} mat4;

#ifndef NO_EXTRAS
	bool operator==(const mat2& l, const mat2& r);
	bool operator==(const mat3& l, const mat3& r);
	bool operator==(const mat4& l, const mat4& r);

	bool operator!=(const mat2& l, const mat2& r);
	bool operator!=(const mat3& l, const mat3& r);
	bool operator!=(const mat4& l, const mat4& r);
#endif 

void Transpose(const float *srcMat, float *dstMat,  int srcRows,  int srcCols);
mat2 Transpose(const mat2& matrix);
mat3 Transpose(const mat3& matrix);
mat4 Transpose(const mat4& matrix);

mat2 operator*(const mat2& matrix, float scalar);
mat3 operator*(const mat3& matrix, float scalar);
mat4 operator*(const mat4& matrix, float scalar);

bool Multiply(float* out, const float* matA, int aRows, int aCols, const float* matB, int bRows, int bCols);
mat2 operator*(const mat2& matrixA, const mat2& matrixB);
mat3 operator*(const mat3& matrixA, const mat3& matrixB);
mat4 operator*(const mat4& matrixA, const mat4& matrixB);

mat3 Cut(const mat4& mat, int row, int col);
mat2 Cut(const mat3& mat, int row, int col);
void Cofactor(float* out, const float* minor, int rows, int cols);

mat2 Minor(const mat2& mat);
mat2 Cofactor(const mat2& mat);
float Determinant(const mat2& matrix);
mat2 Adjugate(const mat2& mat);
mat2 Inverse(const mat2& mat);

mat3 Minor(const mat3& mat);
mat3 Cofactor(const mat3& mat);
float Determinant(const mat3& mat);
mat3 Adjugate(const mat3& mat);
mat3 Inverse(const mat3& mat);

mat4 Minor(const mat4& mat);
mat4 Cofactor(const mat4& mat);
float Determinant(const mat4& mat);
mat4 Adjugate(const mat4& mat);
mat4 Inverse(const mat4& mat);

#endif

<|start_filename|>Chapter13/Physics/Geometry2D.cpp<|end_filename|>
#include "Geometry2D.h"
#include "matrices.h"
#include <cmath>
#include <cfloat>

#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

float Length(const Line2D& line) {
	return Magnitude(line.end - line.start);
}

float LengthSq(const Line2D& line) {
	return MagnitudeSq(line.end - line.start);
}

vec2 GetMin(const Rectangle2D& rect) {
	vec2 p1 = rect.origin;
	vec2 p2 = rect.origin + rect.size;

	return vec2(fminf(p1.x, p2.x), fminf(p1.y, p2.y));
}

vec2 GetMax(const Rectangle2D& rect) {
	vec2 p1 = rect.origin;
	vec2 p2 = rect.origin + rect.size;

	return vec2(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y));
}

Rectangle2D FromMinMax(const vec2& min, const vec2& max) {
	return Rectangle2D(min, max - min);
}

bool PointOnLine(const Point2D& point, const Line2D& line) {
	// Find the slope
	float M = (line.end.y - line.start.y) / (line.end.x - line.start.x);
	// Find the Y-Intercept
	float B = line.start.y - M * line.start.x;
	// Check line equation
	return CMP(point.y, M * point.x + B);
}

bool PointInCircle(const Point2D& point, const Circle& circle) {
	Line2D line(point, circle.position);
	if (LengthSq(line) < circle.radius * circle.radius) {
		return true;
	}
	return false;
}

bool PointInRectangle(const Point2D& point, const Rectangle2D& rectangle) {
	vec2 min = GetMin(rectangle);
	vec2 max = GetMax(rectangle);

	return	min.x <= point.x && min.y <= point.y &&
			point.x <= max.x && point.y <= max.y;
}

bool PointInOrientedRectangle(const Point2D& point, const OrientedRectangle& rectangle) {
	Rectangle2D localRectangle(Point2D(), rectangle.halfExtents * 2.0f);

	// First, we get a vector from the center of the rectangle
	// pointing to the point we want to test!
	vec2 rotVector = point - rectangle.position;

	// Second, we rotate that vector by the inverse of the rectangles
	// rotation (On the Z axis in 3D). We can either invert a matrix,
	// or construct a matrix from the negative rotation angle
	float theta = -DEG2RAD(rectangle.rotation);
	float zRotation2x2[] = { // Construct matrix
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	// Rotate vector
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);

	// Now, the rotVector is in the local space of the world oriented rectangle.
	// But remember, it's a vector, not a point. Assume the rectangle is at 0,0
	// this means the center of the rotated rectangle is at 0,0. And rotVector points
	// to a point inside of this oriented box

	// Remember, the regular Rectangle class has 0,0 at bottom left! So, when we 
	// constructed the local space version of the oriented rectangle, we essentially
	// made an oriented rectangle with no orientation that was offset by half it's
	// size, so that bottom left is at origin. We have to apply this same offset
	// to the vector being rotated

	// Third we offset the point pointed to by the local roation vector by
	// half of the oriented bounding boxes extents. We do this because the
	// non oriented bounding box has its origin at bottom left, while the
	// oriented bounding box has its origin at center
	vec2 localPoint = rotVector + rectangle.halfExtents;

	// Finally, perform the local space point rectangle test!
	return PointInRectangle(localPoint, localRectangle);

	/* Alternate second step, Verbose
	vec3 pointToRotate(localPoint.x, localPoint.y, 0.0f);
	mat3 rotation = ZRotation3x3(-rectangle.rotation);
	vec3 result = MultiplyVector(pointToRotate, rotation);
	localPoint = vec2(result.x, result.y);*/

	// Alternate second step, With extras
	//localPoint = RotateVector(localPoint, -rectangle.rotation);
}

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line2D& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << "), end: (" << shape.end.x << ", " << shape.end.y << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Circle& shape) {
	os << "position: (" << shape.position.x << ", " << shape.position.y << "), radius: " << shape.radius;
	return os;
}
std::ostream& operator<<(std::ostream& os, const Rectangle2D& shape) {
	vec2 min = GetMin(shape);
	vec2 max = GetMax(shape);

	os << "min: (" << min.x << ", " << min.y << "), max: (" << max.x << ", " << max.y << ")";
	return os;
}
std::ostream& operator<<(std::ostream& os, const OrientedRectangle& shape) {
	os << "position: (" << shape.position.x << ", " << shape.position.y << "), half size: (";
	os << shape.halfExtents.x << ", " << shape.halfExtents.y << "), rotation: " << shape.rotation;
	return os;
}
#endif 

bool LineCircle(const Line2D& line, const Circle& circle) {
	// Turn line into vector
	vec2 ab = line.end - line.start;

	// Project point (circle position) onto ab (line segment), computing the 
	// paramaterized position d(t) = a + t * (b - a)
	float t = Dot(circle.position - line.start, ab) / Dot(ab, ab);

	// Clamp T to a 0-1 range. If t was < 0 or > 1
	// then the closest point was outside the segment!
	if (t < 0.0f || t > 1.0f) {
		return false;
	}
	// We do not need to clamp t
	//t = (t < 0.0f) ? 0.0f : ((t > 1.0f)? 1.0f : t);

	// Find the closest point on the line segment
	Point2D closestPoint = line.start + ab * t;

	Line2D circleToClosest(circle.position, closestPoint);
	return LengthSq(circleToClosest) < circle.radius * circle.radius;
}

#if 1
bool LineRectangle(const Line2D& line, const Rectangle2D& rect) {
	if (PointInRectangle(line.start, rect) || PointInRectangle(line.end, rect)) {
		return true;
	}
	
	vec2 norm = Normalized(line.end - line.start);
	norm.x = (norm.x != 0) ? 1.0f / norm.x : 0;
	norm.y = (norm.y != 0) ? 1.0f / norm.y : 0;
	vec2 min = (GetMin(rect) - line.start) * norm;
	vec2 max = (GetMax(rect) - line.start) * norm;

	float tmin = fmaxf(fminf(min.x, max.x), fminf(min.y, max.y));
	float tmax = fminf(fmaxf(min.x, max.x), fmaxf(min.y, max.y));
	if (tmax < 0 || tmin > tmax) {
		return false;
	}
	float t = (tmin < 0.0f) ? tmax : tmin;
	return t > 0.0f && t * t < LengthSq(line);
}
#else // Same code, just a bit easyer to read!
bool LineRectangle(const Line2D& line, const Rectangle2D& rect) {
	// If either point of the line is in the rectangle, we have collision!
	if (PointInRectangle(line.start, rect) || PointInRectangle(line.end, rect)) {
		return true;
	}

	// Now we do a raycast
	vec2 min = GetMin(rect);
	vec2 max = GetMax(rect);
	vec2 norm = Normalized(line.end - line.start);

	float t1 = (min.x - line.start.x) / norm.x;
	float t2 = (max.x - line.start.x) / norm.x;
	float t3 = (min.y - line.start.y) / norm.y;
	float t4 = (max.y - line.start.y) / norm.y;

	float tmin = fmaxf(fminf(t1, t2), fminf(t3, t4));
	float tmax = fminf(fmaxf(t1, t2), fmaxf(t3, t4));

	// if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
	// So the line segment is not intersecting
	if (tmax < 0) {
		return false;
	}

	// if tmin > tmax, ray doesn't intersect AABB
	if (tmin > tmax) {
		return false;
	}

	float t = tmin;

	if (tmin < 0.0f) {
		// Intersection point is:
		// line.start + norm * tmax
		t = tmax;
	}

	// Intersection point is:
	// line.start + norm * tmin
	return t > 0.0f && t * t < LengthSq(line);

	

	/* This tests a line (not segment) against a Rectangle

	// First get the corners of the box
	Point2D c[] = { // corners
		rect.origin,
		rect.origin + rect.size,
		Point2D(rect.origin.x + rect.size.x, rect.origin.y),
		Point2D(rect.origin.x, rect.origin.y + rect.size.y)
	};

	// These variables are just to keep the determinant caluclations readable
	float xDelta = line.end.x - line.start.x;
	float yDelta = line.end.y - line.start.y;
	float sx = line.start.x;
	float sy = line.start.y;

	// Find the determinant of
	// | x2 - x1 |  x3 - x1 |
	// | y2 - y1 |  y3 - y1 |
	// for every corner. The result of the determinant for that matrix
	// will be positive if the point is on one side, negative if its
	// on the other side and 0 if the point is on the line.
	float d1 = (c[0].x - sx) * (yDelta) - (c[0].y - sy) * (xDelta);
	float d2 = (c[1].x - sx) * (yDelta) - (c[1].y - sy) * (xDelta);
	float d3 = (c[2].x - sx) * (yDelta) - (c[2].y - sy) * (xDelta);
	float d4 = (c[3].x - sx) * (yDelta) - (c[3].y - sy) * (xDelta);

	// If all points are on the same side of the line, we have a collision
	// We take advantage of the fact that multiplying two positive numbers
	// is always positive and multiplying two negative numbers is always
	// negative. The only way multiplying two determinant results is negative
	// is if two of the numbers where on the same side.
	return (!d1 * d2 <= 0 || d2 * d3 <= 0 || d3 * d4 <= 0); */
}
#endif

bool LineOrientedRectangle(const Line2D& line, const OrientedRectangle& rectangle) {
	float theta = -DEG2RAD(rectangle.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Line2D localLine;

	vec2 rotVector = line.start - rectangle.position;
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	localLine.start  = rotVector + rectangle.halfExtents;
	
	rotVector = line.end - rectangle.position;
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	localLine.end = rotVector + rectangle.halfExtents;
	
	Rectangle2D localRectangle(Point2D(), rectangle.halfExtents * 2.0f);
	return LineRectangle(localLine, localRectangle);
}

<|start_filename|>Chapter01/Code/CH16Demo.cpp<|end_filename|>
#include "CH16Demo.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void CH16Demo::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	size_imgui_window = true;

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(9.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	num_part = 15;
	part_dist = 0.2f;
	k = -3.0f;
	d = 0.0f;
	ground.size = vec3(10.0f, 0.1f, 10.0f);

	ResetDemo();
}

float CH16Demo::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

void CH16Demo::ResetDemo() {
	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();
	physicsSystem.ClearSprings();
	physicsSystem.ClearCloths();
	renderObjects.clear();

	cloth.Initialize(num_part, part_dist, vec3(0, 6, 0));
	cloth.SetStructuralSprings(k, d);
	cloth.SetBendSprings(k, d);
	cloth.SetShearSprings(k, d);
	physicsSystem.AddCloth(&cloth);

	physicsSystem.AddConstraint(ground);
	renderObjects.resize(4);

	float d = 0.5f;
	renderObjects[0].position = vec3( d, 2.4f,  d);
	renderObjects[1].position = vec3(-d, 2.3f,  d);
	renderObjects[2].position = vec3(-d, 2.4f, -d);
	renderObjects[3].position = vec3( d, 2.3f, -d);

	for (int i = 0; i < 4; ++i) {
		renderObjects[i].size = vec3(0.3f, 0.5f, 0.3f);
		physicsSystem.AddConstraint(renderObjects[i]);
		renderObjects[i].size = vec3(0.1f, 0.5f, 0.1f);
	}
}

void CH16Demo::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 75));
	}

	ImGui::Begin("Chapter 16 Demo", 0, ImGuiWindowFlags_NoResize);
	
	
	ImGui::PushItemWidth(50);
	ImGui::SliderInt("Count", &num_part, 5, 30);
	ImGui::SameLine();
	ImGui::PushItemWidth(50);
	ImGui::SliderFloat("Distance", &part_dist, 0.1f, 1.0f);
	ImGui::SameLine();
	ImGui::PushItemWidth(50);
	ImGui::SliderFloat("Spring k", &k, -5.0f, 0.0f);

	if (ImGui::Button("Reset")) {
		ResetDemo();
	}
	

	ImGui::End();
}

void CH16Demo::Render() {
	DemoBase::Render();

	// Mesh bending is not implemented, so we render the 
	// obstacle which the cloth is resting on with a cheat

	static const float constraintDiffuse[]{ 0.0f, 200.0f / 255.0f, 0.0f, 0.0f };
	static const float constraintAmbient[]{ 50.0f / 255.0f, 200.0f / 255.0f, 50.0f / 255.0f, 0.0f };
	static const float groundDiffuse[]{ 0.0f, 0.0f, 200.0f / 255.0f, 0.0f };
	static const float groundAmbient[]{ 50.0f / 255.0f, 50.0f / 255.0f, 200.0f / 255.0f, 0.0f };
	static const float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	glColor3f(constraintDiffuse[0], constraintDiffuse[1], constraintDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, constraintAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, constraintDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	for (int i = 0; i < renderObjects.size(); ++i) {
		::Render(renderObjects[i]);
	}

	glColor3f(groundDiffuse[0], groundDiffuse[1], groundDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, groundAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, groundDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	::Render(ground);
	
	cloth.Render(physicsSystem.DebugRender);

	//physicsSystem.Render();
	
	/*Line l(vec3(0, -8, 0), vec3(0, 8, 0));
	::Render(l);*/
}

void CH16Demo::Update(float dt) {
	DemoBase::Update(dt);

	physicsSystem.Update(dt);
}

<|start_filename|>Chapter03/Code/Camera.cpp<|end_filename|>
#include "Camera.h"
#include "GLWindow.h"
#include "Compare.h"

#include <iostream>

Camera::Camera() {
	m_nFov = 60.0f;
	m_nAspect = 1.3f;
	m_nNear = 0.01f;
	m_nFar = 1000.0f;
	m_nWidth = 1.0;
	m_nHeight = 1.0f;

	m_matWorld = mat4();
	m_matProj = Projection(m_nFov, m_nAspect, m_nNear, m_nFar);
	m_nProjectionMode = 0;
}

mat4 Camera::GetWorldMatrix() {
	return m_matWorld;
	/*mat3 r = Rotation3x3(rotation.x, rotation.y, rotation.z);

	return mat4(
		r._11, r._12, r._13, 0.0f,
		r._21, r._22, r._23, 0.0f,
		r._31, r._32, r._33, 0.0f,
		Dot(position, vec3(r._11, r._12, r._13)),
		Dot(position, vec3(r._21, r._22, r._23)),
		Dot(position, vec3(r._31, r._32, r._33)),
		1.0f
	);*/
}

bool Camera::IsOrthoNormal() {
	vec3 right = vec3(m_matWorld._11, m_matWorld._12, m_matWorld._13);
	vec3 up = vec3(m_matWorld._21, m_matWorld._22, m_matWorld._23);
	vec3 forward = vec3(m_matWorld._31, m_matWorld._32, m_matWorld._33);

	if (!CMP(Dot(right, right), 1.0f)) {
		return false; // X axis is not normal
	}

	if (!CMP(Dot(up, up), 1.0f)) {
		return false; // Y axis is not normal
	}

	if (!CMP(Dot(forward, forward), 1.0f)) {
		return false; // Z axis is not normal
	}

	if (!CMP(Dot(forward, up), 0.0f)) {
		return false; // Not perpendicular
	}

	if (!CMP(Dot(forward, right), 0.0f)) {
		return false; // Not perpendicular
	}

	if (!CMP(Dot(right, up), 0.0f)) {
		return false; // Not perpendicular
	}

	/*mat3 orientation = Cut(m_matWorld, 3, 3);
	mat3 transposed = Transpose(orientation);
	mat3 result = orientation * transposed;
	mat3 identity;
	
	for (int i = 0; i < 9; ++i) {
		if (!CMP(result.asArray[i], identity.asArray[i])) {
			return false;
		}
	}*/
	return true;
}

void Camera::OrthoNormalize() {
	vec3 right = vec3(m_matWorld._11, m_matWorld._12, m_matWorld._13);
	vec3 up = vec3(m_matWorld._21, m_matWorld._22, m_matWorld._23);
	vec3 forward = vec3(m_matWorld._31, m_matWorld._32, m_matWorld._33);

	vec3 f = Normalized(forward);
	vec3 r = Normalized(Cross(up, f));
	vec3 u = Cross(f, r);

	m_matWorld = mat4(
		r.x, r.y, r.z, 0.0f,
		u.x, u.y, u.z, 0.0f,
		f.x, f.y, f.z, 0.0f,
		m_matWorld._41, m_matWorld._42, m_matWorld._43, 1.0f
	);
}

mat4 Camera::GetViewMatrix() {
	//return Inverse(m_matWorld);

	if (!IsOrthoNormal()) {
		OrthoNormalize();
	}

	mat4 inverse = Transpose(m_matWorld);
	inverse._41 = inverse._14 = 0.0f;
	inverse._42 = inverse._24 = 0.0f;
	inverse._43 = inverse._34 = 0.0f;

	vec3 right = vec3(m_matWorld._11, m_matWorld._12, m_matWorld._13);
	vec3 up = vec3(m_matWorld._21, m_matWorld._22, m_matWorld._23);
	vec3 forward = vec3(m_matWorld._31, m_matWorld._32, m_matWorld._33);
	vec3 position = vec3(m_matWorld._41, m_matWorld._42, m_matWorld._43);

	inverse._41 = -Dot(right, position);
	inverse._42 = -Dot(up, position);
	inverse._43 = -Dot(forward, position);

#ifdef DO_SANITY_TESTS
#ifndef NO_EXTRAS
	if (inverse != Inverse(m_matWorld)) {
		std::cout << "View matrix is not what was expected!\n";
	}
#endif
#endif

	return inverse;
}

float Camera::GetAspect() {
	return m_nAspect;
}

mat4 Camera::GetProjectionMatrix() {
	return m_matProj;
}

void Camera::Resize(int width, int height) {
	m_nAspect = (float)width / (float)height;

	if (m_nProjectionMode == 0) { // Perspective
		m_matProj = Projection(m_nFov, m_nAspect, m_nNear, m_nFar);
	}
	else if (m_nProjectionMode == 1) { // Ortho
		m_nWidth = (float)width;
		m_nHeight = (float)height;

		float halfW = m_nWidth * 0.5f;
		float halfH = m_nHeight * 0.5f;

		m_matProj = Ortho(-halfW, halfW, halfH, -halfH, m_nNear, m_nFar);
	}
	// m_nProjectionMode == 2
		// User defined
}

bool Camera::IsOrthographic() {
	return m_nProjectionMode == 1;
}

bool Camera::IsPerspective() {
	return m_nProjectionMode == 0;
}

void Camera::Perspective(float fov, float aspect, float zNear, float zFar) {
	m_nFov = fov;
	m_nAspect = aspect;
	m_nNear = zNear;
	m_nFar = zFar;

	m_matProj = Projection(fov, aspect, zNear, zFar);
	m_nProjectionMode = 0;
}

void Camera::Orthographic(float width, float height, float zNear, float zFar) {
	m_nWidth = width;
	m_nHeight = height;
	m_nNear = zNear;
	m_nFar = zFar;

	float halfW = width * 0.5f;
	float halfH = height * 0.5f;

	m_matProj = Ortho(-halfW, halfW, halfH, -halfH, zNear, zFar);
	m_nProjectionMode = 1;
}

void Camera::SetProjection(const mat4& projection) {
	m_matProj = projection;
	m_nProjectionMode = 2;
}

void Camera::SetWorld(const mat4& view) {
	m_matWorld = view;
}

Camera CreatePerspective(float fieldOfView, float aspectRatio, float nearPlane, float farPlane) {
	Camera result;
	result.Perspective(fieldOfView, aspectRatio, nearPlane, farPlane);
	return result;
}

Camera CreateOrthographic(float width, float height, float nearPlane, float farPlane) {
	Camera result;
	result.Orthographic(width, height, nearPlane, farPlane);
	return result;
}

OrbitCamera::OrbitCamera() {
	target = vec3(0, 0, 0);
	zoomDistance = 10.0f;
	zoomSpeed = 200.0f;
	rotationSpeed = vec2(250.0f, 120.0f);
	yRotationLimit = vec2(-20.0f, 80.0f);
	zoomDistanceLimit = vec2(3.0f, 15.0f);
	currentRotation = vec2(0, 0);
	panSpeed = vec2(180.0f, 180.0f);
}

void OrbitCamera::Rotate(const vec2& deltaRot, float deltaTime) {
	currentRotation.x += deltaRot.x * rotationSpeed.x * zoomDistance* deltaTime;
	currentRotation.y += deltaRot.y * rotationSpeed.y * zoomDistance * deltaTime;

	currentRotation.x = ClampAngle(currentRotation.x, -360, 360);
	currentRotation.y = ClampAngle(currentRotation.y, yRotationLimit.x, yRotationLimit.y);
}

void OrbitCamera::Zoom(float deltaZoom, float deltaTime) {
	zoomDistance = zoomDistance + deltaZoom  * zoomSpeed * deltaTime;
	if (zoomDistance < zoomDistanceLimit.x) {
		zoomDistance = zoomDistanceLimit.x;
	}
	if (zoomDistance > zoomDistanceLimit.y) {
		zoomDistance = zoomDistanceLimit.y;
	}
}

void OrbitCamera::Pan(const vec2& delataPan, float deltaTime) {
	vec3 right(m_matWorld._11, m_matWorld._12, m_matWorld._13);

	// Pan X axis in local space
	target = target - (right * (delataPan.x * panSpeed.x * deltaTime));
	// Pan Y Axis in global space
	target = target + (vec3(0, 1, 0) * (delataPan.y * panSpeed.y * deltaTime));

	// Reset zoom to allow infinate zooming after a motion
	// This part of the code is not in the book!
	float midZoom = zoomDistanceLimit.x + (zoomDistanceLimit.y - zoomDistanceLimit.x) * 0.5f;
	zoomDistance = midZoom - zoomDistance;
	vec3 rotation = vec3(currentRotation.y, currentRotation.x, 0);
	mat3 orient = Rotation3x3(rotation.x, rotation.y, rotation.z);
	vec3 dir = MultiplyVector( vec3(0.0, 0.0, -zoomDistance), orient);
	target = target - dir;
	zoomDistance = midZoom;
}

void OrbitCamera::Update(float dt) {
	vec3 rotation = vec3(currentRotation.y, currentRotation.x, 0);
	mat3 orient = Rotation3x3(rotation.x, rotation.y, rotation.z);
	vec3 dir = MultiplyVector( vec3(0.0, 0.0, -zoomDistance), orient);
	vec3 position = /*rotation * vec3(0.0, 0.0, -distance)*/dir + target;
#ifndef NO_EXTRAS
	m_matWorld = FastInverse(LookAt(position, target, vec3(0, 1, 0)));
#else
	m_matWorld = Inverse(LookAt(position, target, vec3(0, 1, 0)));
#endif
}

float OrbitCamera::ClampAngle(float angle, float min, float max) {
	while (angle < -360) {
		angle += 360;
	}
	while (angle > 360) {
		angle -= 360;
	}
	if (angle < min) {
		angle = min;
	}
	if (angle > max) {
		angle = max;
	}
	return angle;
}

Frustum Camera::GetFrustum() {
	Frustum result;

	mat4 vp = GetViewMatrix() * GetProjectionMatrix();

	vec3 col1(vp._11, vp._21, vp._31);//, vp._41 };
	vec3 col2(vp._12, vp._22, vp._32);//, vp._42 };
	vec3 col3(vp._13, vp._23, vp._33);//, vp._43 };
	vec3 col4(vp._14, vp._24, vp._34);//, vp._44 };
					
	// Find plane magnitudes
	result.left.normal	= col4 + col1;
	result.right.normal = col4 - col1;
	result.bottom.normal= col4 + col2;
	result.top.normal	= col4 - col2;
	result.near.normal	= /*col4 +*/ col3;
	result.far.normal	= col4 - col3;

	// Find plane distances
	result.left.distance	= vp._44 + vp._41;
	result.right.distance	= vp._44 - vp._41;
	result.bottom.distance	= vp._44 + vp._42;
	result.top.distance		= vp._44 - vp._42;
	result.near.distance	= /*vp._44 +*/ vp._43;
	result.far.distance		= vp._44 - vp._43;

	// Normalize all 6 planes
	for (int i = 0; i < 6; ++i) {
		float mag = 1.0f / Magnitude(result.planes[i].normal);
		Normalize(result.planes[i].normal);
		result.planes[i].distance *= mag;
	}

	return result;		   
}

void OrbitCamera::PrintDebug() {
	std::cout << "Target: (" << target.x << ", " << target.y << ", " << target.z << ")\n";
	std::cout << "Zoom distance: " << zoomDistance << "\n";
	std::cout << "Rotation: (" << currentRotation.x << ", " << currentRotation.y << ")\n";
}

void OrbitCamera::SetTarget(const vec3& newTarget) {
	target = newTarget;
}

void OrbitCamera::SetZoom(float zoom) {
	zoomDistance = zoom;
}

void OrbitCamera::SetRotation(const vec2& rotation) {
	currentRotation = rotation;
}

<|start_filename|>Chapter02/Code/imgui/imgui_implementation.cpp<|end_filename|>
#include "imgui_implementation.h"
#include "imgui.h"

#include "../glad/glad.h"

static HWND g_imgui_HWND = 0;
static INT64 g_imgui_time = 0;
static INT64 g_imgui_ticks_per_second = 0;
static GLuint g_imgui_font_texture = 0;

bool ImGui_Implementation_Init(void* hwnd) {
	g_imgui_HWND = (HWND)hwnd;

	if (!QueryPerformanceFrequency((LARGE_INTEGER *)&g_imgui_ticks_per_second))
		return false;
	if (!QueryPerformanceCounter((LARGE_INTEGER *)&g_imgui_time))
		return false;

	ImGuiIO& io = ImGui::GetIO();
	io.KeyMap[ImGuiKey_Tab] = VK_TAB;                       // Keyboard mapping. ImGui will use those indices to peek into the io.KeyDown[] array that we will update during the application lifetime.
	io.KeyMap[ImGuiKey_LeftArrow] = VK_LEFT;
	io.KeyMap[ImGuiKey_RightArrow] = VK_RIGHT;
	io.KeyMap[ImGuiKey_UpArrow] = VK_UP;
	io.KeyMap[ImGuiKey_DownArrow] = VK_DOWN;
	io.KeyMap[ImGuiKey_PageUp] = VK_PRIOR;
	io.KeyMap[ImGuiKey_PageDown] = VK_NEXT;
	io.KeyMap[ImGuiKey_Home] = VK_HOME;
	io.KeyMap[ImGuiKey_End] = VK_END;
	io.KeyMap[ImGuiKey_Delete] = VK_DELETE;
	io.KeyMap[ImGuiKey_Backspace] = VK_BACK;
	io.KeyMap[ImGuiKey_Enter] = VK_RETURN;
	io.KeyMap[ImGuiKey_Escape] = VK_ESCAPE;
	io.KeyMap[ImGuiKey_A] = 'A';
	io.KeyMap[ImGuiKey_C] = 'C';
	io.KeyMap[ImGuiKey_V] = 'V';
	io.KeyMap[ImGuiKey_X] = 'X';
	io.KeyMap[ImGuiKey_Y] = 'Y';
	io.KeyMap[ImGuiKey_Z] = 'Z';

	io.RenderDrawListsFn = ImGui_Implementation_RenderDrawLists;   // Alternatively you can set this to NULL and call ImGui::GetDrawData() after ImGui::Render() to get the same ImDrawData pointer.
	io.ImeWindowHandle = g_imgui_HWND;

	return true;
}

bool ImGui_Implementation_CreateDeviceObjects() {
	// 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_imgui_font_texture);
	glBindTexture(GL_TEXTURE_2D, g_imgui_font_texture);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	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_imgui_font_texture;

	// Restore state
	glBindTexture(GL_TEXTURE_2D, last_texture);

	return true;
}

void ImGui_Implementation_Shutdown() {
	ImGui_Implementation_InvalidateDeviceObjects();
	ImGui::Shutdown();
	g_imgui_HWND = 0;
}

void ImGui_Implementation_InvalidateDeviceObjects() {
	if (g_imgui_font_texture) {
		glDeleteTextures(1, &g_imgui_font_texture);
		ImGui::GetIO().Fonts->TexID = 0;
		g_imgui_font_texture = 0;
	}
}

void ImGui_Implementation_RenderDrawLists(struct 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);

	// We are using the OpenGL fixed pipeline to make the example code simpler to read!
	// Setup render state: alpha-blending enabled, no face culling, no depth testing, scissor enabled, vertex/texcoord/color pointers.
	GLint last_texture; glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
	GLint last_viewport[4]; glGetIntegerv(GL_VIEWPORT, last_viewport);
	GLint last_scissor_box[4]; glGetIntegerv(GL_SCISSOR_BOX, last_scissor_box);
	glPushAttrib(GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT | GL_TRANSFORM_BIT | GL_POLYGON_BIT | GL_LIGHTING_BIT);
	glDisable(GL_LIGHTING);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glDisable(GL_CULL_FACE);
	glDisable(GL_DEPTH_TEST);
	glEnable(GL_SCISSOR_TEST);
	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	glEnableClientState(GL_COLOR_ARRAY);
	glEnable(GL_TEXTURE_2D);
	//glUseProgram(0); // You may want this if using this code in an OpenGL 3+ context

	// Setup viewport, orthographic projection matrix
	glViewport(0, 0, (GLsizei)fb_width, (GLsizei)fb_height);
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0.0f, io.DisplaySize.x, io.DisplaySize.y, 0.0f, -1.0f, +1.0f);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();

	// Render command lists
#define OFFSETOF(TYPE, ELEMENT) ((size_t)&(((TYPE *)0)->ELEMENT))
	for (int n = 0; n < draw_data->CmdListsCount; n++)
	{
		const ImDrawList* cmd_list = draw_data->CmdLists[n];
		const ImDrawVert* vtx_buffer = cmd_list->VtxBuffer.Data;
		const ImDrawIdx* idx_buffer = cmd_list->IdxBuffer.Data;
		glVertexPointer(2, GL_FLOAT, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, pos)));
		glTexCoordPointer(2, GL_FLOAT, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, uv)));
		glColorPointer(4, GL_UNSIGNED_BYTE, sizeof(ImDrawVert), (const GLvoid*)((const char*)vtx_buffer + OFFSETOF(ImDrawVert, col)));

		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);
			}
			idx_buffer += pcmd->ElemCount;
		}
	}
#undef OFFSETOF

	// Restore modified state
	glDisableClientState(GL_COLOR_ARRAY);
	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
	glDisableClientState(GL_VERTEX_ARRAY);
	glBindTexture(GL_TEXTURE_2D, (GLuint)last_texture);
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glPopAttrib();
	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]);


}

void ImGui_Implementation_NewFrame() {
	if (!g_imgui_font_texture) {
		ImGui_Implementation_CreateDeviceObjects();
	}

	ImGuiIO& io = ImGui::GetIO();

	// Setup display size (every frame to accommodate for window resizing)
	RECT rect;
	GetClientRect(g_imgui_HWND, &rect);
	io.DisplaySize = ImVec2((float)(rect.right - rect.left), (float)(rect.bottom - rect.top));

	// Setup time step
	INT64 current_time;
	QueryPerformanceCounter((LARGE_INTEGER *)&current_time);
	io.DeltaTime = (float)(current_time - g_imgui_time) / g_imgui_ticks_per_second;
	g_imgui_time = current_time;

	// Read keyboard modifiers inputs
	io.KeyCtrl = (GetKeyState(VK_CONTROL) & 0x8000) != 0;
	io.KeyShift = (GetKeyState(VK_SHIFT) & 0x8000) != 0;
	io.KeyAlt = (GetKeyState(VK_MENU) & 0x8000) != 0;
	io.KeySuper = false;
	// io.KeysDown : filled by WM_KEYDOWN/WM_KEYUP events
	// io.MousePos : filled by WM_MOUSEMOVE events
	// io.MouseDown : filled by WM_*BUTTON* events
	// io.MouseWheel : filled by WM_MOUSEWHEEL events

	// Hide OS mouse cursor if ImGui is drawing it
	if (io.MouseDrawCursor) {
		SetCursor(NULL);
	}

	// Start the frame
	ImGui::NewFrame();
}

bool ImGui_Implementation_HasMouse() {
	ImGuiIO& io = ImGui::GetIO();
	return io.WantCaptureMouse;
}

bool ImGui_Implementation_HasKeyboard() {
	ImGuiIO& io = ImGui::GetIO();
	return io.WantCaptureKeyboard;
}

void ImGui_Implementation_WndProcHandler(HWND, UINT msg, WPARAM wParam, LPARAM lParam) {
	ImGuiIO& io = ImGui::GetIO();
	switch (msg)
	{
	case WM_LBUTTONDOWN:
		io.MouseDown[0] = true;
		break;
	case WM_LBUTTONUP:
		io.MouseDown[0] = false;
		break;
	case WM_RBUTTONDOWN:
		io.MouseDown[1] = true;
		break;
	case WM_RBUTTONUP:
		io.MouseDown[1] = false;
		break;
	case WM_MBUTTONDOWN:
		io.MouseDown[2] = true;
		break;
	case WM_MBUTTONUP:
		io.MouseDown[2] = false;
		break;
	case WM_MOUSEWHEEL:
		io.MouseWheel += GET_WHEEL_DELTA_WPARAM(wParam) > 0 ? +1.0f : -1.0f;
		break;
	case WM_MOUSEMOVE:
		io.MousePos.x = (signed short)(lParam);
		io.MousePos.y = (signed short)(lParam >> 16);
		break;
	case WM_KEYDOWN:
		if (wParam < 256) {
			io.KeysDown[wParam] = 1;
		}
		break;
	case WM_KEYUP:
		if (wParam < 256) {
			io.KeysDown[wParam] = 0;
		}
		break;
	case WM_CHAR:
		// You can also use ToAscii()+GetKeyboardState() to retrieve characters.
		if (wParam > 0 && wParam < 0x10000) {
			io.AddInputCharacter((unsigned short)wParam);
		}
		break;
	}
}


<|start_filename|>Chapter01/Code/RotationalForce.h<|end_filename|>
#ifndef _H_ROTATIONAL_FORCE_
#define _H_ROTATIONAL_FORCE_

#include "DemoBase.h"
#include "Geometry3D.h"
#include "PhysicsSystem.h"

class RotationalForce : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	std::vector<Rigidbody> bodies;
	Rigidbody groundBox;
	Ray ray;
	float force;
	bool allowForceAdd;
	bool step;
	bool allowed;

	bool size_imgui_window;
protected:
	void ResetDemo();
	float Random(float min, float max);
	vec3 Random(vec3 min, vec3 max);
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};


#endif


<|start_filename|>Chapter01/Code/RaycastDemo.h<|end_filename|>
#ifndef _H_RAYCAST_DEMO_
#define _H_RAYCAST_DEMO_

#include "DemoBase.h"
#include "Geometry3D.h"

class RaycastDemo : public DemoBase {
protected:
	RaycastResult raycastResult;
	Sphere sphere;
	AABB aabb;
	OBB obb;
	Plane plane;
	Triangle triangle;
	Ray screenRay;
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
};

#endif


<|start_filename|>Chapter03/Code/Geometry2D.cpp<|end_filename|>
#include "Geometry2D.h"
#include "matrices.h"
#include <cmath>
#include <cfloat>

#ifdef DO_SANITY_TESTS
	#include <iostream>
	#include "Compare.h"
#else
	#define CMP(x, y) \
		(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))
#endif

#define CLAMP(number, minimum, maximum) \
	number = (number < minimum) ? minimum : \
		((number > maximum) ? maximum : number)

#define OVERLAP(minA, minB, maxA, maxB) \
	((minB <= maxA) && (minA <= maxB))

float Length(const Line2D& line) {
	return Magnitude(line.end - line.start);
}

float LengthSq(const Line2D& line) {
	return MagnitudeSq(line.end - line.start);
}

vec2 GetMin(const Rectangle2D& rect) {
	vec2 p1 = rect.origin;
	vec2 p2 = rect.origin + rect.size;

	return vec2(fminf(p1.x, p2.x), fminf(p1.y, p2.y));
}

vec2 GetMax(const Rectangle2D& rect) {
	vec2 p1 = rect.origin;
	vec2 p2 = rect.origin + rect.size;

	return vec2(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y));
}

Rectangle2D FromMinMax(const vec2& min, const vec2& max) {
	return Rectangle2D(min, max - min);
}

bool PointOnLine(const Point2D& point, const Line2D& line) {
	// Find the slope
	float M = (line.end.y - line.start.y) / (line.end.x - line.start.x);
	// Find the Y-Intercept
	float B = line.start.y - M * line.start.x;
	// Check line equation
	return CMP(point.y, M * point.x + B);
}

bool PointInCircle(const Point2D& point, const Circle& circle) {
	Line2D line(point, circle.position);
	if (LengthSq(line) < circle.radius * circle.radius) {
		return true;
	}
	return false;
}

bool PointInRectangle(const Point2D& point, const Rectangle2D& rectangle) {
	vec2 min = GetMin(rectangle);
	vec2 max = GetMax(rectangle);

	return	min.x <= point.x && min.y <= point.y &&
			point.x <= max.x && point.y <= max.y;
}

bool PointInOrientedRectangle(const Point2D& point, const OrientedRectangle& rectangle) {
	Rectangle2D localRectangle(Point2D(), rectangle.halfExtents * 2.0f);

	// First, we get a vector from the center of the rectangle
	// pointing to the point we want to test!
	vec2 rotVector = point - rectangle.position;

	// Second, we rotate that vector by the inverse of the rectangles
	// rotation (On the Z axis in 3D). We can either invert a matrix,
	// or construct a matrix from the negative rotation angle
	float theta = -DEG2RAD(rectangle.rotation);
	float zRotation2x2[] = { // Construct matrix
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	// Rotate vector
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);

	// Now, the rotVector is in the local space of the world oriented rectangle.
	// But remember, it's a vector, not a point. Assume the rectangle is at 0,0
	// this means the center of the rotated rectangle is at 0,0. And rotVector points
	// to a point inside of this oriented box

	// Remember, the regular Rectangle class has 0,0 at bottom left! So, when we 
	// constructed the local space version of the oriented rectangle, we essentially
	// made an oriented rectangle with no orientation that was offset by half it's
	// size, so that bottom left is at origin. We have to apply this same offset
	// to the vector being rotated

	// Third we offset the point pointed to by the local roation vector by
	// half of the oriented bounding boxes extents. We do this because the
	// non oriented bounding box has its origin at bottom left, while the
	// oriented bounding box has its origin at center
	vec2 localPoint = rotVector + rectangle.halfExtents;

	// Finally, perform the local space point rectangle test!
	return PointInRectangle(localPoint, localRectangle);

	/* Alternate second step, Verbose
	vec3 pointToRotate(localPoint.x, localPoint.y, 0.0f);
	mat3 rotation = ZRotation3x3(-rectangle.rotation);
	vec3 result = MultiplyVector(pointToRotate, rotation);
	localPoint = vec2(result.x, result.y);*/

	// Alternate second step, With extras
	//localPoint = RotateVector(localPoint, -rectangle.rotation);
}

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line2D& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << "), end: (" << shape.end.x << ", " << shape.end.y << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Circle& shape) {
	os << "position: (" << shape.position.x << ", " << shape.position.y << "), radius: " << shape.radius;
	return os;
}
std::ostream& operator<<(std::ostream& os, const Rectangle2D& shape) {
	vec2 min = GetMin(shape);
	vec2 max = GetMax(shape);

	os << "min: (" << min.x << ", " << min.y << "), max: (" << max.x << ", " << max.y << ")";
	return os;
}
std::ostream& operator<<(std::ostream& os, const OrientedRectangle& shape) {
	os << "position: (" << shape.position.x << ", " << shape.position.y << "), half size: (";
	os << shape.halfExtents.x << ", " << shape.halfExtents.y << "), rotation: " << shape.rotation;
	return os;
}
#endif 

bool LineCircle(const Line2D& line, const Circle& circle) {
	// Turn line into vector
	vec2 ab = line.end - line.start;

	// Project point (circle position) onto ab (line segment), computing the 
	// paramaterized position d(t) = a + t * (b - a)
	float t = Dot(circle.position - line.start, ab) / Dot(ab, ab);

	// Clamp T to a 0-1 range. If t was < 0 or > 1
	// then the closest point was outside the segment!
	if (t < 0.0f || t > 1.0f) {
		return false;
	}
	// We do not need to clamp t
	//t = (t < 0.0f) ? 0.0f : ((t > 1.0f)? 1.0f : t);

	// Find the closest point on the line segment
	Point2D closestPoint = line.start + ab * t;

	Line2D circleToClosest(circle.position, closestPoint);
	return LengthSq(circleToClosest) < circle.radius * circle.radius;
}

#if 1
bool LineRectangle(const Line2D& line, const Rectangle2D& rect) {
	if (PointInRectangle(line.start, rect) || PointInRectangle(line.end, rect)) {
		return true;
	}
	
	vec2 norm = Normalized(line.end - line.start);
	norm.x = (norm.x != 0) ? 1.0f / norm.x : 0;
	norm.y = (norm.y != 0) ? 1.0f / norm.y : 0;
	vec2 min = (GetMin(rect) - line.start) * norm;
	vec2 max = (GetMax(rect) - line.start) * norm;

	float tmin = fmaxf(fminf(min.x, max.x), fminf(min.y, max.y));
	float tmax = fminf(fmaxf(min.x, max.x), fmaxf(min.y, max.y));
	if (tmax < 0 || tmin > tmax) {
		return false;
	}
	float t = (tmin < 0.0f) ? tmax : tmin;
	return t > 0.0f && t * t < LengthSq(line);
}
#else // Same code, just a bit easyer to read!
bool LineRectangle(const Line2D& line, const Rectangle2D& rect) {
	// If either point of the line is in the rectangle, we have collision!
	if (PointInRectangle(line.start, rect) || PointInRectangle(line.end, rect)) {
		return true;
	}

	// Now we do a raycast
	vec2 min = GetMin(rect);
	vec2 max = GetMax(rect);
	vec2 norm = Normalized(line.end - line.start);

	float t1 = (min.x - line.start.x) / norm.x;
	float t2 = (max.x - line.start.x) / norm.x;
	float t3 = (min.y - line.start.y) / norm.y;
	float t4 = (max.y - line.start.y) / norm.y;

	float tmin = fmaxf(fminf(t1, t2), fminf(t3, t4));
	float tmax = fminf(fmaxf(t1, t2), fmaxf(t3, t4));

	// if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
	// So the line segment is not intersecting
	if (tmax < 0) {
		return false;
	}

	// if tmin > tmax, ray doesn't intersect AABB
	if (tmin > tmax) {
		return false;
	}

	float t = tmin;

	if (tmin < 0.0f) {
		// Intersection point is:
		// line.start + norm * tmax
		t = tmax;
	}

	// Intersection point is:
	// line.start + norm * tmin
	return t > 0.0f && t * t < LengthSq(line);

	

	/* This tests a line (not segment) against a Rectangle

	// First get the corners of the box
	Point2D c[] = { // corners
		rect.origin,
		rect.origin + rect.size,
		Point2D(rect.origin.x + rect.size.x, rect.origin.y),
		Point2D(rect.origin.x, rect.origin.y + rect.size.y)
	};

	// These variables are just to keep the determinant caluclations readable
	float xDelta = line.end.x - line.start.x;
	float yDelta = line.end.y - line.start.y;
	float sx = line.start.x;
	float sy = line.start.y;

	// Find the determinant of
	// | x2 - x1 |  x3 - x1 |
	// | y2 - y1 |  y3 - y1 |
	// for every corner. The result of the determinant for that matrix
	// will be positive if the point is on one side, negative if its
	// on the other side and 0 if the point is on the line.
	float d1 = (c[0].x - sx) * (yDelta) - (c[0].y - sy) * (xDelta);
	float d2 = (c[1].x - sx) * (yDelta) - (c[1].y - sy) * (xDelta);
	float d3 = (c[2].x - sx) * (yDelta) - (c[2].y - sy) * (xDelta);
	float d4 = (c[3].x - sx) * (yDelta) - (c[3].y - sy) * (xDelta);

	// If all points are on the same side of the line, we have a collision
	// We take advantage of the fact that multiplying two positive numbers
	// is always positive and multiplying two negative numbers is always
	// negative. The only way multiplying two determinant results is negative
	// is if two of the numbers where on the same side.
	return (!d1 * d2 <= 0 || d2 * d3 <= 0 || d3 * d4 <= 0); */
}
#endif

bool LineOrientedRectangle(const Line2D& line, const OrientedRectangle& rectangle) {
	float theta = -DEG2RAD(rectangle.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Line2D localLine;

	vec2 rotVector = line.start - rectangle.position;
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	localLine.start  = rotVector + rectangle.halfExtents;
	
	rotVector = line.end - rectangle.position;
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	localLine.end = rotVector + rectangle.halfExtents;
	
	Rectangle2D localRectangle(Point2D(), rectangle.halfExtents * 2.0f);
	return LineRectangle(localLine, localRectangle);
}

bool CircleCircle(const Circle& circle1, const Circle& circle2) {
	Line2D line(circle1.position, circle2.position);
	float radiiSum = circle1.radius + circle2.radius;
	return LengthSq(line) <= radiiSum * radiiSum;
}

bool CircleRectangle(const Circle& circle, const Rectangle2D& rect) {
	Point2D closestPoint = circle.position;
	vec2 min = GetMin(rect);
	vec2 max = GetMax(rect);
	closestPoint.x = (closestPoint.x < min.x) ? min.x :
		(closestPoint.x > max.x) ? max.x : closestPoint.x;
	closestPoint.y = (closestPoint.y < min.y) ? min.y :
		(closestPoint.y > max.y) ? max.y : closestPoint.y;

	Line2D line(circle.position, closestPoint);
	return LengthSq(line) <= circle.radius * circle.radius;
}

bool CircleOrientedRectangle(const Circle& circle, const OrientedRectangle& rect) {
	vec2 rotVector = circle.position - rect.position;
	float theta = -DEG2RAD(rect.rotation);
	float zRotation2x2[] = { 
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	
	Circle localCircle(rotVector + rect.halfExtents, circle.radius);
	Rectangle2D localRectangle(Point2D(), rect.halfExtents * 2.0f);

	return CircleRectangle(localCircle, localRectangle);
}

bool RectangleRectangle(const Rectangle2D& rect1, const Rectangle2D& rect2) {
	vec2 aMin = GetMin(rect1);
	vec2 aMax = GetMax(rect1);
	vec2 bMin = GetMin(rect2);
	vec2 bMax = GetMax(rect2);

	bool xOverlap = ((bMin.x <= aMax.x) && (aMin.x <= bMax.x));
	bool yOverlap = ((bMin.y <= aMax.y) && (aMin.y <= bMax.y));

	return xOverlap && yOverlap;
}

Interval2D GetInterval(const Rectangle2D& rect, const vec2& axis) {
	vec2 min = GetMin(rect);
	vec2 max = GetMax(rect);

	vec2 verts[] = { // Get all vertices of rect
		vec2(min.x, min.y),
		vec2(min.x, max.y),
		vec2(max.x, max.y),
		vec2(max.x, min.y)
	};

	// Set interval first projected vertex
	Interval2D result;
	result.min = Dot(axis, verts[0]);
	result.max = result.min;

	// For all other verts
	for (int i = 1; i < 4; ++i) {
		// Project vertex
		float projection = Dot(axis, verts[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}

bool OverlapOnAxis(const Rectangle2D& rect1, const Rectangle2D& rect2, const vec2& axis) {
	Interval2D a = GetInterval(rect1, axis);
	Interval2D b = GetInterval(rect2, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool RectangleRectangleSAT(const Rectangle2D& rect1, const Rectangle2D& rect2) {
	vec2 axisToTest[] = {
		vec2(1, 0),
		vec2(0, 1)
	};

	for (int i = 0; i < 2; ++i) {
		if (!OverlapOnAxis(rect1, rect2, axisToTest[i])) {
			// Intervals don't overlap, seperating axis found
			return false; // No collision has taken place
		}
	}

	// All intervals overlapped, seperating axis not found
	return true; // We have a collision
}

Interval2D GetInterval(const OrientedRectangle& rect, const vec2& axis) {
	Rectangle2D nonOrientedRect = Rectangle2D(Point2D(rect.position - rect.halfExtents), rect.halfExtents * 2.0f);
	vec2 min = GetMin(nonOrientedRect); 
	vec2 max = GetMax(nonOrientedRect);
	vec2 verts[] = { min, max, vec2(min.x, max.y), vec2(max.x, min.y) };

	float theta = DEG2RAD(rect.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };

	for (int i = 0; i < 4; ++i) {
		vec2 rotVector = verts[i] - rect.position;
		Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
		verts[i] = rotVector + rect.position;
	}

	// Set interval first projected vertex
	Interval2D result;
	result.min = result.max = Dot(axis, verts[0]);

	// For all other verts
	for (int i = 1; i < 4; ++i) {
		// Project vertex
		float projection = Dot(axis, verts[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}


bool OverlapOnAxis(const Rectangle2D& rect1, const OrientedRectangle& rect2, const vec2& axis) {
	Interval2D a = GetInterval(rect1, axis);
	Interval2D b = GetInterval(rect2, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool RectangleOrientedRectangle(const Rectangle2D& rect1, const OrientedRectangle& rect2) {
	vec2 axisToTest[]{
		vec2(1, 0),
		vec2(0, 1),
		Normalized(vec2(rect2.halfExtents.x, 0)),
		Normalized(vec2(0, rect2.halfExtents.y))
	};

	float theta = DEG2RAD(rect2.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Multiply(axisToTest[2].asArray, Normalized(vec2(rect2.halfExtents.x, 0)).asArray, 1, 2, zRotation2x2, 2, 2);
	Multiply(axisToTest[3].asArray, Normalized(vec2(0, rect2.halfExtents.y)).asArray, 1, 2, zRotation2x2, 2, 2);

	for (int i = 0; i < 4; ++i) {
		if (!OverlapOnAxis(rect1, rect2, axisToTest[i])) {
			// Intervals don't overlap, seperating axis found
			return false; // No collision has taken place
		}
	}

	// All intervals overlapped, seperating axis not found
	return true; // We have a collision
}

bool OverlapOnAxis(const OrientedRectangle& rect1, const OrientedRectangle& rect2, const vec2& axis) {
	Interval2D a = GetInterval(rect1, axis);
	Interval2D b = GetInterval(rect2, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OrientedRectangleOrientedRectangleSAT(const OrientedRectangle& rect1, const OrientedRectangle& rect2) {
	vec2 axisToTest[]{
		vec2(1, 0),
		vec2(0, 1),
		vec2(), // TEMP
		vec2(), // TEMP
		vec2(), // TEMP
		vec2() // TEMP
	};

	// Collision axis of rect 2
	float theta = DEG2RAD(rect2.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Multiply(axisToTest[2].asArray, Normalized(vec2(rect2.halfExtents.x, 0)).asArray, 1, 2, zRotation2x2, 2, 2);
	Multiply(axisToTest[3].asArray, Normalized(vec2(0, rect2.halfExtents.y)).asArray, 1, 2, zRotation2x2, 2, 2);

	// Collision axis of rect 1
	theta = DEG2RAD(rect1.rotation);
	zRotation2x2[0] = cosf(theta);
	zRotation2x2[1] = sinf(theta);
	zRotation2x2[2] = -sinf(theta);
	zRotation2x2[3] = cosf(theta);
	Multiply(axisToTest[4].asArray, Normalized(vec2(rect1.halfExtents.x, 0)).asArray, 1, 2, zRotation2x2, 2, 2);
	Multiply(axisToTest[5].asArray, Normalized(vec2(0, rect1.halfExtents.y)).asArray, 1, 2, zRotation2x2, 2, 2);

	for (int i = 0; i < 6; ++i) {
		if (!OverlapOnAxis(rect1, rect2, axisToTest[i])) {
			// Intervals don't overlap, seperating axis found
			return false; // No collision has taken place
		}
	}

	// All intervals overlapped, seperating axis not found
	return true; // We have a collision
}

bool OrientedRectangleOrientedRectangle(const OrientedRectangle& r1, const OrientedRectangle& r2) {
	//return OrientedRectangleOrientedRectangleSAT(rect1, rect2);
	Rectangle2D localRect1(Point2D(), r1.halfExtents * 2.0f);
	OrientedRectangle localRect2(r2.position, r2.halfExtents, r2.rotation);

	localRect2.rotation = r2.rotation - r1.rotation;
	vec2 rotVector = r2.position - r1.position;
	float theta = -DEG2RAD(r1.rotation);
	float zRotation2x2[] = {
		cosf(theta), sinf(theta),
		-sinf(theta), cosf(theta) };
	Multiply(rotVector.asArray, vec2(rotVector.x, rotVector.y).asArray, 1, 2, zRotation2x2, 2, 2);
	localRect2.position = rotVector + r1.halfExtents;

	return RectangleOrientedRectangle(localRect1, localRect2);
}

Circle ContainingCircle(Point2D* pointArray, int arrayCount) {
	Point2D center; 
	for (int i = 0; i < arrayCount; ++i) {
		center = center + pointArray[i];
	}
	center = center * (1.0f / (float)arrayCount);

	Circle result(center, 1.0f);

	// Find the squared radius
	result.radius = MagnitudeSq(center - pointArray[0]);
	for (int i = 1; i < arrayCount; ++i) {
		float distance = MagnitudeSq(center - pointArray[i]);
		if (distance > result.radius) {
			result.radius = distance;
		}
	}

	// This has been squared until now
	result.radius = sqrtf(result.radius);
	return result;
}

Circle ContainingCircleAlt(Point2D* pointArray, int arrayCount) {
	vec2 min = pointArray[0];
	vec2 max = pointArray[0];

	for (int i = 0; i < arrayCount; ++i) {
		min.x = pointArray[i].x < min.x ? pointArray[i].x : min.x;
		min.y = pointArray[i].y < min.y ? pointArray[i].y : min.y;
		max.x = pointArray[i].x > max.x ? pointArray[i].x : max.x;
		max.y = pointArray[i].y > max.y ? pointArray[i].y : max.y;
	}

	return Circle((min + max) * 0.5f, Magnitude(max - min) * 0.5f);
}

Rectangle2D ContainingRectangle(Point2D* pointArray, int arrayCount) {
	vec2 min = pointArray[0];
	vec2 max = pointArray[0];

	for (int i = 0; i < arrayCount; ++i) {
		min.x = pointArray[i].x < min.x ? pointArray[i].x : min.x;
		min.y = pointArray[i].y < min.y ? pointArray[i].y : min.y;
		max.x = pointArray[i].x > max.x ? pointArray[i].x : max.x;
		max.y = pointArray[i].y > max.y ? pointArray[i].y : max.y;
	}

	return FromMinMax(min, max);
}




bool PointInShape(const BoundingShape& shape, const Point2D& point) {
	for (int i = 0; i < shape.numCircles; ++i) {
		if (PointInCircle(point, shape.circles[i])) {
			return true;
		}
	}
	for (int i = 0; i < shape.numRectangles; ++i) {
		if (PointInRectangle(point, shape.rectangles[i])) {
			return true;
		}
	}
	return false;
}

#ifndef NO_EXTRAS
bool LineShape(const Line2D& line, const BoundingShape& shape) {
	for (int i = 0; i < shape.numCircles; ++i) {
		if (LineCircle(line, shape.circles[i])) {
			return true;
		}
	}
	for (int i = 0; i < shape.numRectangles; ++i) {
		if (LineRectangle(line, shape.rectangles[i])) {
			return true;
		}
	}
	return false;
}

bool CircleShape(const Circle& circle, const BoundingShape& shape) {
	for (int i = 0; i < shape.numCircles; ++i) {
		if (CircleCircle(circle, shape.circles[i])) {
			return true;
		}
	}
	for (int i = 0; i < shape.numRectangles; ++i) {
		if (CircleRectangle(circle, shape.rectangles[i])) {
			return true;
		}
	}
	return false;
}

bool RectangleShape(const Rectangle2D& rectangle, const BoundingShape& shape) {
	for (int i = 0; i < shape.numCircles; ++i) {
		if (RectangleCircle(rectangle, shape.circles[i])) {
			return true;
		}
	}
	for (int i = 0; i < shape.numRectangles; ++i) {
		if (RectangleRectangle(rectangle, shape.rectangles[i])) {
			return true;
		}
	}
	return false;
}

bool OrientedRectangleShape(const OrientedRectangle& rect, const BoundingShape& shape) {
	for (int i = 0; i < shape.numCircles; ++i) {
		if (OrientedRectangleCircle(rect, shape.circles[i])) {
			return true;
		}
	}
	for (int i = 0; i < shape.numRectangles; ++i) {
		if (OrientedRectangleRectangle(rect, shape.rectangles[i])) {
			return true;
		}
	}
	return false;
}

#endif

<|start_filename|>Chapter10/Physics/Geometry3D.cpp<|end_filename|>
#include "Geometry3D.h"
#include <cmath>
#include <cfloat>

float Length(const Line& line) {
	return Magnitude(line.start - line.end);
}

float LengthSq(const Line& line) {
	return MagnitudeSq(line.start - line.end);
}

Ray FromPoints(const Point& from, const Point& to) {
	return Ray(
		from,
		Normalized(to - from)
	);
}

vec3 GetMin(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fminf(p1.x, p2.x), fminf(p1.y, p2.y), fminf(p1.z, p2.z));
}
vec3 GetMax(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y), fmaxf(p1.z, p2.z));
}

AABB FromMinMax(const vec3& min, const vec3& max) {
	return AABB((min + max) * 0.5f, (max - min) * 0.5f);
}

float PlaneEquation(const Point& point, const Plane& plane) {
	return Dot(point, plane.normal) - plane.distance;
}

#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point) {
	return Dot(point, plane.normal) - plane.distance;
}
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << ", " << shape.start.z << "), end: (";
	os << shape.end.x << ", " << shape.end.y << ", " << shape.end.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Ray& shape) {
	os << "origin: (" << shape.origin.x << ", " << shape.origin.y << ", " << shape.origin.z << "), ";
	os << "direction: (" << shape.direction.x << ", " << shape.direction.y << ", " << shape.direction.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Sphere& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "radius: " << shape.radius;
	return os;
}

std::ostream& operator<<(std::ostream& os, const AABB& shape) {
	vec3 min = GetMin(shape);
	vec3 max = GetMax(shape);
	os << "min: (" << min.x << ", " << min.y << ", " << min.z << "), ";
	os << "max: (" << max.x << ", " << max.y << ", " << max.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Plane& shape) {
	os << "normal: (" << shape.normal.x << ", " << shape.normal.y << ", " << shape.normal.z << "), ";
	os << "distance: " << shape.distance;
	return os;
}

std::ostream& operator<<(std::ostream& os, const Triangle& shape) {
	os << "a: (" << shape.a.x << ", " << shape.a.y << ", " << shape.a.z << "), ";
	os << "b: (" << shape.b.x << ", " << shape.b.y << ", " << shape.b.z << "), ";
	os << "c: (" << shape.c.x << ", " << shape.c.y << ", " << shape.c.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const OBB& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "size:" << shape.size.x << ", " << shape.size.y << ", " << shape.size.z << "), ";
	os << "x basis:" << shape.orientation._11 << ", " << shape.orientation._21 << ", " << shape.orientation._31 << "), ";
	os << "y basis:" << shape.orientation._12 << ", " << shape.orientation._22 << ", " << shape.orientation._32 << "), ";
	os << "z basis:" << shape.orientation._13 << ", " << shape.orientation._23 << ", " << shape.orientation._33 << ")";
	return os;
}
#endif 

<|start_filename|>Chapter07/Physics/Geometry3D.h<|end_filename|>
#ifndef _H_GEOMETRY_3D_
#define _H_GEOMETRY_3D_

#include "vectors.h"
#include "matrices.h"

#ifndef NO_EXTRAS
#include <ostream>
#endif 

typedef vec3 Point;

typedef struct Line {
	Point start;
	Point end;

	inline Line() {}
	inline Line(const Point& s, const Point& e) :
		start(s), end(e) { }
} Line;

typedef struct Ray {
	Point origin;
	vec3 direction;

	inline Ray() : direction(0.0f, 0.0f, 1.0f) {}
	inline Ray(const Point& o, const vec3& d) :
		origin(o), direction(d) {
		NormalizeDirection();
	}
	inline void NormalizeDirection() {
		Normalize(direction);
	}
} Ray;

typedef struct Sphere {
	Point position;
	float radius;

	inline Sphere() : radius(1.0f) { }
	inline Sphere(const Point& p, float r) :
		position(p), radius(r) { }
} Sphere;

typedef struct AABB {
	Point position;
	vec3 size; // HALF SIZE!

	inline AABB() : size(1, 1, 1) { }
	inline AABB(const Point& p, const vec3& s) :
		position(p), size(s) { }
} AABB;

typedef struct OBB {
	Point position;
	vec3 size; // HALF SIZE!
	mat3 orientation;

	inline OBB() : size(1, 1, 1) { }
	inline OBB(const Point& p, const vec3& s) :
		position(p), size(s) { }
	inline OBB(const Point& p, const vec3& s, const mat3& o) :
		position(p), size(s), orientation(o) { }
} OBB;

typedef struct Plane {
	vec3 normal;
	float distance;

	inline Plane() : normal(1, 0, 0) { }
	inline Plane(const vec3& n, float d) :
		normal(n), distance(d) { }
} Plane;

typedef struct Triangle {
	union {
		struct {
			Point a;
			Point b;
			Point c;
		};
#ifndef NO_EXTRAS
		struct {
			Point p1;
			Point p2;
			Point p3;
		};
#endif 

		Point points[3];
		float values[9];
	};
	
	inline Triangle() { }
	inline Triangle(const Point& _p1, const Point& _p2, const Point& _p3) :
		a(_p1), b(_p2), c(_p3) { }
} Triangle;

typedef struct Interval {
	float min;
	float max;
} Interval;

#ifndef NO_EXTRAS
typedef vec3 Point3D;
typedef Line Line3D;
typedef Ray Ray3D;
typedef AABB Rectangle3D;
typedef Interval Interval3D;
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape);
std::ostream& operator<<(std::ostream& os, const Ray& shape);
std::ostream& operator<<(std::ostream& os, const Sphere& shape);
std::ostream& operator<<(std::ostream& os, const AABB& shape);
std::ostream& operator<<(std::ostream& os, const OBB& shape);
std::ostream& operator<<(std::ostream& os, const Plane& shape);
std::ostream& operator<<(std::ostream& os, const Triangle& shape);
#endif 

float Length(const Line& line);
float LengthSq(const Line& line);
Ray FromPoints(const Point& from, const Point& to);
vec3 GetMin(const AABB& aabb);
vec3 GetMax(const AABB& aabb);
AABB FromMinMax(const vec3& min, const vec3& max);
float PlaneEquation(const Point& point, const Plane& plane);
#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point);
#endif

bool PointInSphere(const Point& point, const Sphere& sphere);
bool PointInAABB(const Point& point, const AABB& aabb);
bool PointInOBB(const Point& point, const OBB& obb);
bool PointOnPlane(const Point& point, const Plane& plane);
bool PointOnLine(const Point& point, const Line& line);
bool PointOnRay(const Point& point, const Ray& ray);

#ifndef NO_EXTRAS
bool PointInPlane(const Point& point, const Plane& plane);
bool PointInLine(const Point& point, const Line& line);
bool PointInRay(const Point& point, const Ray& ray);
bool ContainsPoint(const Sphere& sphere, const Point& point);
bool ContainsPoint(const Point& point, const Sphere& sphere);
bool ContainsPoint(const AABB& aabb, const Point& point);
bool ContainsPoint(const Point& point, const AABB& aabb);
bool ContainsPoint(const Point& point, const OBB& obb);
bool ContainsPoint(const OBB& obb, const Point& point);
bool ContainsPoint(const Point& point, const Plane& plane);
bool ContainsPoint(const Plane& plane, const Point& point);
bool ContainsPoint(const Point& point, const Line& line);
bool ContainsPoint(const Line& line, const Point& point);
bool ContainsPoint(const Point& point, const Ray& ray);
bool ContainsPoint(const Ray& ray, const Point& point);
#endif

Point ClosestPoint(const Sphere& sphere, const Point& point);
Point ClosestPoint(const AABB& aabb, const Point& point);
Point ClosestPoint(const OBB& obb, const Point& point);
Point ClosestPoint(const Plane& plane, const Point& point);
Point ClosestPoint(const Line& line, const Point& point);
Point ClosestPoint(const Ray& ray, const Point& point);

#ifndef NO_EXTRAS
Point ClosestPoint(const Point& point, const Sphere& sphere);
Point ClosestPoint(const Point& point, const AABB& aabb);
Point ClosestPoint(const Point& point, const OBB& obb);
Point ClosestPoint(const Point& point, const Plane& plane);
Point ClosestPoint(const Point& point, const Line& line);
Point ClosestPoint(const Point& point, const Ray& ray);
#endif

Interval GetInterval(const AABB& aabb, const vec3& axis);
Interval GetInterval(const OBB& obb, const vec3& axis);
bool OverlapOnAxis(const AABB& aabb, const OBB& obb, const vec3& axis);
bool OverlapOnAxis(const OBB& obb1, const OBB& obb2, const vec3& axis);

bool SphereSphere(const Sphere& s1, const Sphere& s2);
bool SphereAABB(const Sphere& sphere, const AABB& aabb);
bool SphereOBB(const Sphere& sphere, const OBB& obb);
bool SpherePlane(const Sphere& sphere, const Plane& plane);
bool AABBAABB(const AABB& aabb1, const AABB& aabb2);
bool AABBOBB(const AABB& aabb, const OBB& obb);
bool AABBPlane(const AABB& aabb, const Plane& plane);
bool OBBOBB(const OBB& obb1, const OBB& obb2);
bool OBBPlane(const OBB& obb, const Plane& plane);
bool PlanePlane(const Plane& plane1, const Plane& plane2);

#define AABBSphere(aabb, sphere) \
	SphereAABB(Sphere, AABB)
#define OBBSphere(obb, sphere) \
	SphereOBB(sphere, obb)
#define PlaneSphere(plane, sphere) \
	SpherePlane(sphere, plane)
#define OBBAABB(obb, aabb) \
	AABBOBB(aabb, obb)
#define PlaneAABB(plane, aabb) \
	AABBPlane(aabb, plane)
#define PlaneOBB(plane, obb) \
	OBBPlane(obb, plane)


float Raycast(const Sphere& sphere, const Ray& ray);
float Raycast(const AABB& aabb, const Ray& ray);
float Raycast(const OBB& obb, const Ray& ray);
float Raycast(const Plane& plane, const Ray& ray);

bool Linetest(const Sphere& sphere, const Line& line);
bool Linetest(const AABB& aabb, const Line& line);
bool Linetest(const OBB& obb, const Line& line);
bool Linetest(const Plane& plane, const Line& line);

#ifndef NO_EXTRAS
float Raycast(const Ray& ray, const Sphere& sphere);
float Raycast(const Ray& ray, const AABB& aabb);
float Raycast(const Ray& ray, const OBB& obb);
float Raycast(const Ray& ray, const Plane& plane);
bool Linetest(const Line& line, const Sphere& sphere);
bool Linetest(const Line& line, const AABB& aabb);
bool Linetest(const Line& line, const OBB& obb);
bool Linetest(const Line& line, const Plane& plane);
#endif

#endif


<|start_filename|>Chapter01/Code/LinearImpulse.cpp<|end_filename|>
#include "LinearImpulse.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void LinearImpulse::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);

	physicsSystem.RenderRandomColors = true;

	size_imgui_window = true;
	isPaused = true;
	numSteps = 0;
	stepSize = 1;

	use_spheres = false;
	drop = false;

	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(9.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

#define sphere0 bodies[0]
#define sphere1 bodies[1]
#define box0 bodies[2]
#define box1 bodies[3]

void LinearImpulse::ResetDemo() {
	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();

	bodies.clear();
	bodies.resize(4);

	sphere0.type = sphere1.type = RIGIDBODY_TYPE_SPHERE;
	box0.type = box1.type = RIGIDBODY_TYPE_BOX;

	if (!drop) {
		sphere0.position = vec3(-10, 1.5f, -5);
		sphere1.position = vec3(5, 1.5f, 2.5f);
		box0.position = vec3(-10, 1.5f, -5);
		box1.position = vec3(5, 1.5f, 2.5f);

		vec3 impulseDirection = vec3(-10, 1.5f, -5) - vec3(5, 1.5f, 2.5f);
		sphere1.AddLinearImpulse(impulseDirection + vec3(0, 5, 0));
		box1.AddLinearImpulse(impulseDirection + vec3(0, 5, 0));

		impulseDirection = vec3(5, 1.5f, 2.5f) - vec3(-10, 1.5f, -5);
		sphere0.AddLinearImpulse(impulseDirection + vec3(0, 5, 0));
		box0.AddLinearImpulse(impulseDirection + vec3(0, 5, 0));
	}
	else {
		sphere0.position = vec3(0, 1.2, 0);
		sphere1.position = vec3(0, 4, 0);
		box0.position = vec3(0, 1.2, 0);
		box1.position = vec3(0, 4, 0);
	}

	groundBox = RigidbodyVolume(RIGIDBODY_TYPE_BOX);
	groundBox.box.size = vec3(10.0f, 0.15f, 10.0f);
	groundBox.mass = 0.0f;

	if (use_spheres) {
		physicsSystem.AddRigidbody(&sphere0);
		physicsSystem.AddRigidbody(&sphere1);
	}
	else {
		physicsSystem.AddRigidbody(&box0);
		physicsSystem.AddRigidbody(&box1);
	}

	physicsSystem.AddRigidbody(&groundBox);
}

float LinearImpulse::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

vec3 LinearImpulse::Random(vec3 min, vec3 max) {
	vec3 result;
	result.x = Random(min.x, max.x);
	result.y = Random(min.y, max.y);
	result.z = Random(min.z, max.z);
	return result;
}

void LinearImpulse::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 100));
	}

	ImGui::Begin("Linear Impulse Demo", 0, ImGuiWindowFlags_NoResize);

	if (ImGui::Button("Step")) {
		numSteps = stepSize;
	}
	ImGui::SameLine();
	ImGui::PushItemWidth(100);
	ImGui::InputInt("Frames", &stepSize);

	ImGui::SameLine();
	if (ImGui::Button("Reset")) {
		ResetDemo();
	}
	ImGui::SameLine();
	ImGui::Checkbox("Pause", &isPaused);

	bool wasUsingSpheres = use_spheres;
	bool wasDropping = drop;

	ImGui::Checkbox("Spheres", &use_spheres);
	ImGui::SameLine();
	ImGui::Checkbox("Drop", &drop);

	if (wasUsingSpheres != use_spheres || wasDropping != drop) {
		ResetDemo();
	}

	ImGui::End();
}

void LinearImpulse::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();
}

void LinearImpulse::Update(float dt) {
	DemoBase::Update(dt);

	if (!isPaused) {
		physicsSystem.Update(dt);
	}
	else if (numSteps > 0) {
		numSteps -= 1;
		physicsSystem.Update(dt);
	}
}

<|start_filename|>Chapter01/Code/PhysicsSystem.h<|end_filename|>
#ifndef _H_PHYSICS_SYSTEM_
#define _H_PHYSICS_SYSTEM_

#include "Rigidbody.h"
#include "Spring.h"
#include "Cloth.h"

class PhysicsSystem {
protected:
	std::vector<Rigidbody*> bodies;
	std::vector<Cloth*> cloths;
	std::vector<OBB> constraints;
	std::vector<Spring> springs;

	std::vector<Rigidbody*> colliders1;
	std::vector<Rigidbody*> colliders2;
	std::vector<CollisionManifold> results;
public:
	float LinearProjectionPercent; // [0.2 to 0.8], Smaller = less jitter / more penetration
	float PenetrationSlack; // [0.01 to 0.1],  Samller = more accurate
	int ImpulseIteration;

	// Not in book, just for debug purposes
	bool DebugRender;
	bool DoLinearProjection;
	bool RenderRandomColors;

	PhysicsSystem();

	void Update(float deltaTime);
	void Render();
	
	void AddRigidbody(Rigidbody* body);
	void AddCloth(Cloth* cloth);
	void AddSpring(const Spring& spring);
	void AddConstraint(const OBB& constraint);

	void ClearRigidbodys();
	void ClearConstraints();
	void ClearSprings();
	void ClearCloths();
};

#endif

<|start_filename|>Chapter01/Code/FixedFunctionPrimitives.cpp<|end_filename|>
#define WIN32_LEAN_AND_MEAN 
#define WIN32_EXTRA_LEAN
#include <windows.h>
#include <windowsx.h>
#include "glad/glad.h"
#include <cmath>
#include "Compare.h"
#include "FixedFunctionPrimitives.h"

#ifndef  M_PI
	#define M_PI 3.14159265358979323846f
#endif

void Render(const Mesh& mesh) {
	glBegin(GL_TRIANGLES);

	for (int i = 0; i < mesh.numTriangles; ++i) {
		vec3 normal = Normalized(Cross(mesh.triangles[i].c - mesh.triangles[i].a, mesh.triangles[i].b - mesh.triangles[i].a));
		glNormal3fv(normal.asArray);

		glVertex3fv(mesh.triangles[i].a.asArray);
		glVertex3fv(mesh.triangles[i].b.asArray);
		glVertex3fv(mesh.triangles[i].c.asArray);
	}

	glEnd();
}

void Render(const Model& model) {
	glPushMatrix();

	mat4 world = GetWorldMatrix(model);
	glMultMatrixf(world.asArray);
	if (model.GetMesh() != 0) {
		Render(*(model.GetMesh()));
	}

	glPopMatrix();
}

void Render(const CollisionManifold& manifold) {
	if (!manifold.colliding) {
		return;
	}

	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_POINTS);
	for (int i = 0; i < manifold.contacts.size(); ++i) {
		glVertex3f(manifold.contacts[i].x, manifold.contacts[i].y, manifold.contacts[i].z);
	}
	glEnd();

	glColor3f(0.0f, 1.0f, 0.0f);
	glBegin(GL_LINES);
	vec3 center = vec3();
	for (int i = 0; i < manifold.contacts.size(); ++i) {
		vec3 start = manifold.contacts[i];
		vec3 end = start + manifold.normal * manifold.depth;
		center = center + start;

		glVertex3fv(start.asArray);
		glVertex3fv(end.asArray);
	}
	glEnd();

	if (manifold.contacts.size() == 0) {
		return;
	}
	float denom = 1.0f / (float)manifold.contacts.size();
	center = center * denom;

	glColor3f(0.0f, 0.0f, 1.0f);
	glBegin(GL_LINES);
	vec3 start = center;
	vec3 end = center + manifold.normal;

	glVertex3fv(start.asArray);
	glVertex3fv(end.asArray);
	glEnd();

}

void RenderNormals(const Frustum& frustum) {
	vec3 NTL = Intersection(frustum._near, frustum.top, frustum.left);
	vec3 NTR = Intersection(frustum._near, frustum.top, frustum.right);
	vec3 NBL = Intersection(frustum._near, frustum.bottom, frustum.left);
	vec3 NBR = Intersection(frustum._near, frustum.bottom, frustum.right);

	vec3 FTL = Intersection(frustum._far, frustum.top, frustum.left);
	vec3 FTR = Intersection(frustum._far, frustum.top, frustum.right);
	vec3 FBL = Intersection(frustum._far, frustum.bottom, frustum.left);
	vec3 FBR = Intersection(frustum._far, frustum.bottom, frustum.right);

	vec3 centers[] = {
		(NTR + NTL + FTR + FTL) * 0.25f, // Top
		(NBR + NBL + FBR + FBL) * 0.25f, // Bottom
		(NTL + NBL + FTL + FBL) * 0.25f,// Left
		(NTR + NBR + FTR + FBR) * 0.25f,// Right
		(NTL + NTR + NBL + NBR) * 0.25f,// Near
		(FTL + FTR + FBL + FBR) * 0.25f// Far
	};

	/*vec3 centers[] = {
		frustum.planes[0].normal,
		frustum.planes[1].normal,
		frustum.planes[2].normal,
		frustum.planes[3].normal,
		frustum.planes[4].normal,
		frustum.planes[5].normal,
	};*/

	glBegin(GL_LINES);
	for (int i = 0; i < 6; ++i) {
		vec3 p1 = centers[i] + frustum.planes[i].normal * 0.5f;
		glVertex3fv(centers[i].asArray);
		glVertex3fv(p1.asArray);
	}
	glEnd();

	for (int i = 0; i < 6; ++i) {
		vec3 p1 = centers[i] + frustum.planes[i].normal * 0.5f;
		vec3 p2 = p1 + frustum.planes[i].normal * 0.25f;

#ifndef NO_EXTRAS
		mat4 orient = FastInverse(LookAt(p1, p2, vec3(0, 1, 0)));
#else
		mat4 orient = Inverse(LookAt(p1, p2, vec3(0, 1, 0)));
#endif
		mat4 rotate = Rotation(90.0f, 0.0f, 0.0f);

		glPushMatrix();
		glMultMatrixf((rotate * orient).asArray);
		FixedFunctionCone(3, 3.0f, 0.1f);
		glPopMatrix();
	}
}

void Render(const Frustum& frustum) {
	vec3 NTL = Intersection(frustum._near, frustum.top, frustum.left);
	vec3 NTR = Intersection(frustum._near, frustum.top, frustum.right);
	vec3 NBL = Intersection(frustum._near, frustum.bottom, frustum.left);
	vec3 NBR = Intersection(frustum._near, frustum.bottom, frustum.right);

	vec3 FTL = Intersection(frustum._far, frustum.top, frustum.left);
	vec3 FTR = Intersection(frustum._far, frustum.top, frustum.right);
	vec3 FBL = Intersection(frustum._far, frustum.bottom, frustum.left);
	vec3 FBR = Intersection(frustum._far, frustum.bottom, frustum.right);
	
	glBegin(GL_LINES);

	// Near
	glVertex3fv(NTL.asArray);
	glVertex3fv(NTR.asArray);
	glVertex3fv(NTR.asArray);
	glVertex3fv(NBR.asArray);
	glVertex3fv(NBR.asArray);
	glVertex3fv(NBL.asArray);
	glVertex3fv(NBL.asArray);
	glVertex3fv(NTL.asArray);

	// Far
	glVertex3fv(FTL.asArray);
	glVertex3fv(FTR.asArray);
	glVertex3fv(FTR.asArray);
	glVertex3fv(FBR.asArray);
	glVertex3fv(FBR.asArray);
	glVertex3fv(FBL.asArray);
	glVertex3fv(FBL.asArray);
	glVertex3fv(FTL.asArray);

	// Edges
	glVertex3fv(NTL.asArray);
	glVertex3fv(FTL.asArray);
	glVertex3fv(NTR.asArray);
	glVertex3fv(FTR.asArray);
	glVertex3fv(NBL.asArray);
	glVertex3fv(FBL.asArray);
	glVertex3fv(NBR.asArray);
	glVertex3fv(FBR.asArray);


	glEnd();
}

void Render(const Plane& plane, float scale) {
	glPushMatrix();
	glScalef(scale, scale, scale);
	Render(plane);
	glPopMatrix();
}

void Render(const Plane& plane) {
	vec3 forward = plane.normal;
	vec3 up = vec3(0, 1, 0);
	if (CMP(MagnitudeSq(Cross(up, plane.normal)), 0)) {
		up = vec3(1, 0, 0);
		if (CMP(MagnitudeSq(Cross(up, plane.normal)), 0)) {
			up = vec3(0, 0, 1);
		}
	}
	vec3 right = Cross(up, forward);
	up = Cross(forward, right);

	vec3 n = Normalized(plane.normal);
	float d = plane.distance;
	vec3 tx = Normalized(right);
	vec3 ty = Normalized(up);

	glBegin(GL_QUADS);
		glVertex3f((tx.x + ty.x) + n.x * d, (tx.y + ty.y) + n.y * d, (tx.z + ty.z) + n.z * d);
		glVertex3f((tx.x - ty.x) + n.x * d, (tx.y - ty.y) + n.y * d, (tx.z - ty.z) + n.z * d);
		glVertex3f((-tx.x - ty.x) + n.x * d, (-tx.y - ty.y) + n.y * d, (-tx.z - ty.z) + n.z * d);
		glVertex3f((-tx.x + ty.x) + n.x * d, (-tx.y + ty.y) + n.y * d, (-tx.z + ty.z) + n.z * d);

		glVertex3f((tx.x + ty.x) + n.x * d, (tx.y + ty.y) + n.y * d, (tx.z + ty.z) + n.z * d);
		glVertex3f((-tx.x + ty.x) + n.x * d, (-tx.y + ty.y) + n.y * d, (-tx.z + ty.z) + n.z * d);
		glVertex3f((-tx.x - ty.x) + n.x * d, (-tx.y - ty.y) + n.y * d, (-tx.z - ty.z) + n.z * d);
		glVertex3f((tx.x - ty.x) + n.x * d, (tx.y - ty.y) + n.y * d, (tx.z - ty.z) + n.z * d);
	glEnd();

	float currentColor[4];
	glGetFloatv(GL_CURRENT_COLOR, currentColor);

	glColor3f(0.0f, 0.0f, 0.0f);
	glBegin(GL_LINES);
	vec3 start = n * d;
	vec3 end = start + n;
	glVertex3f(start.x, start.y, start.z);
	glVertex3f(end.x, end.y, end.z);
	glEnd();

	glColor4fv(currentColor);
}

void Render(const Triangle& triangle) {
	Render(triangle, false);
}

void Render(const Triangle& triangle, bool oneSided) {
	glBegin(GL_TRIANGLES);

	if (!oneSided) {
		glVertex3f(triangle.a.x, triangle.a.y, triangle.a.z);
		glVertex3f(triangle.c.x, triangle.c.y, triangle.c.z);
		glVertex3f(triangle.b.x, triangle.b.y, triangle.b.z);
	}
	else {
		vec3 ab = triangle.a - triangle.b;
		vec3 ac = triangle.a - triangle.c;
		vec3 norm = Normalized(Cross(ac, ab));
		glNormal3f(norm.x, norm.y, norm.z);
	}

	glVertex3f(triangle.a.x, triangle.a.y, triangle.a.z);
	glVertex3f(triangle.b.x, triangle.b.y, triangle.b.z);
	glVertex3f(triangle.c.x, triangle.c.y, triangle.c.z);

	glEnd();
}

void Render(const Circle& circle) {
	glBegin(GL_LINE_LOOP);

	for (int i = 0; i < 360; ++i) {
		glVertex2f(circle.position.x + cosf(DEG2RAD(i)) * circle.radius, circle.position.y + sinf(DEG2RAD(i)) * circle.radius);
	}

	glEnd();
}

void Render(const Line2D& line) {
	glBegin(GL_LINES);
	glVertex3f(line.start.x, line.start.y, 0.0f);
	glVertex3f(line.end.x, line.end.y, 0.0f);
	glEnd();
}

void Render(const Line& line) {
	glBegin(GL_LINES);
	glVertex3f(line.start.x, line.start.y, line.start.z);
	glVertex3f(line.end.x, line.end.y, line.end.z);
	glEnd();
}

void Render(const std::vector<Line>& edges) {
	glBegin(GL_LINES);
	for (int i = 0; i < edges.size(); ++i) {
		vec3 p1 = edges[i].start;
		vec3 p2 = edges[i].end;
		glVertex3f(p1.x, p1.y, p1.z);
		glVertex3f(p2.x, p2.y, p2.z);
	}
	glEnd();
}

void Render(const Point2D& point) {
	glBegin(GL_POINTS);
	glVertex3f(point.x, point.y, 0.0f);
	glEnd();
}

void Render(const Point& point) {
	glBegin(GL_POINTS);
	glVertex3f(point.x, point.y, point.z);
	glEnd();
}

void Render(const Ray& ray) {
	// Bit of an ugly hack, just make a really long line
	Line line;
	line.start = ray.origin;
	line.end = ray.origin + ray.direction * 50000.0f;
	// And then render that line :(
	glBegin(GL_LINES);
	glVertex3f(line.start.x, line.start.y, line.start.z);
	glVertex3f(line.end.x, line.end.y, line.end.z);
	glEnd();
	// At some point i'm going to fix this
}

void Render(const Sphere& sphere) {
	glPushMatrix();

	glTranslatef(sphere.position.x, sphere.position.y, sphere.position.z);
	FixedFunctionSphere(2, sphere.radius);

	glPopMatrix();
}

void Render(const OBB& obb) {
	glPushMatrix();

	mat4 scale = Scale(obb.size);
	mat4 rotation = FromMat3(obb.orientation);
	mat4 translation = Translation(obb.position);

	// SRT: Scale First, Rotate Second, Translate Last
	// orientation = roll * pitch * yaw;
	mat4 transform = scale * rotation * translation;
	
	// Using GL pipe stuff:
	// glTranslate(obb.position.x, obb.position.y, obb.position.z);
	// Orientation = yaw * pitch * roll
	// glRotate??? Probably do a matrix multiplication instead
	// glScale(obb.size.x, obb.size.y, obb.size.z);

	glMultMatrixf(transform.asArray);
	FixedFunctionCube();
	glPopMatrix();
}

void Render(const AABB& aabb) {
	glPushMatrix();

	glTranslatef(aabb.position.x, aabb.position.y, aabb.position.z);
	FixedFunctionCube(aabb.size.x, aabb.size.y, aabb.size.z);

	glPopMatrix();
}

void RenderWithQuads(const AABB& aabb) {
	glPushMatrix();

	glTranslatef(aabb.position.x, aabb.position.y, aabb.position.z);
	FixedFunctionCubeQuads(aabb.size.x, aabb.size.y, aabb.size.z);

	glPopMatrix();
}

void Render(const Rectangle2D& rect) {
	vec2 min = GetMin(rect);
	vec2 max = GetMax(rect);

	glBegin(GL_LINES); 
	glVertex3f(min[0], min[1], 0.0f);
	glVertex3f(min[0], max[1], 0.0f);
	glVertex3f(min[0], max[1], 0.0f);
	glVertex3f(max[0], max[1], 0.0f);
	glVertex3f(max[0], max[1], 0.0f);
	glVertex3f(max[0], min[1], 0.0f);
	glVertex3f(max[0], min[1], 0.0f);
	glVertex3f(min[0], min[1], 0.0f);
	glEnd();
}

void Render(const BVHNode& bvh) {
	if (bvh.children == 0) {
		RenderWithQuads(bvh.bounds);
	}
	else {
		for (int i = 0; i < 8; ++i) {
			Render(bvh.children[i]);
		}
	}
}

void Render(const OrientedRectangle& rect) {
	glPushMatrix();
	glTranslatef(rect.position.x, rect.position.y, 0.0f);
	glRotatef(rect.rotation, 0.0f, 0.0f, 1.0f);

	vec2 min = vec2(-rect.halfExtents.x, -rect.halfExtents.y);
	vec2 max = vec2(+rect.halfExtents.x, +rect.halfExtents.y);
	
	glBegin(GL_LINES);
	glVertex3f(min[0], min[1], 0.0f);
	glVertex3f(min[0], max[1], 0.0f);
	glVertex3f(min[0], max[1], 0.0f);
	glVertex3f(max[0], max[1], 0.0f);
	glVertex3f(max[0], max[1], 0.0f);
	glVertex3f(max[0], min[1], 0.0f);
	glVertex3f(max[0], min[1], 0.0f);
	glVertex3f(min[0], min[1], 0.0f);
	glEnd();

	glPopMatrix();
}

void FixedFunctionSubdivTetrahedron(float* a, float* b, float* c, int div, float r) {
	if (div <= 0) {
		glNormal3fv(a);
		glVertex3f(a[0] * r, a[1] * r, a[2] * r);
		
		glNormal3fv(b);
		glVertex3f(b[0] * r, b[1] * r, b[2] * r);
		
		glNormal3fv(c);
		glVertex3f(c[0] * r, c[1] * r, c[2] * r);
	}
	else {
		float ab[3], ac[3], bc[3];

		ab[0] = (a[0] + b[0]) / 2.0f;
		ac[0] = (a[0] + c[0]) / 2.0f;
		bc[0] = (b[0] + c[0]) / 2.0f;

		ab[1] = (a[1] + b[1]) / 2.0f;
		ac[1] = (a[1] + c[1]) / 2.0f;
		bc[1] = (b[1] + c[1]) / 2.0f;
		
		ab[2] = (a[2] + b[2]) / 2.0f;
		ac[2] = (a[2] + c[2]) / 2.0f;
		bc[2] = (b[2] + c[2]) / 2.0f;

		// Normalize ab
		float d = sqrtf(ab[0] * ab[0] + ab[1] * ab[1] + ab[2] * ab[2]);
		ab[0] /= d; ab[1] /= d; ab[2] /= d;
		// Normalize ac
		d = sqrtf(ac[0] * ac[0] + ac[1] * ac[1] + ac[2] * ac[2]);
		ac[0] /= d; ac[1] /= d; ac[2] /= d;
		// Normalize bc
		d = sqrtf(bc[0] * bc[0] + bc[1] * bc[1] + bc[2] * bc[2]);
		bc[0] /= d; bc[1] /= d; bc[2] /= d;

		FixedFunctionSubdivTetrahedron(a, ab, ac, div - 1, r);
		FixedFunctionSubdivTetrahedron(b, bc, ab, div - 1, r);
		FixedFunctionSubdivTetrahedron(c, ac, bc, div - 1, r);
		FixedFunctionSubdivTetrahedron(ab, bc, ac, div - 1, r); 
	}
}

void FixedFunctionSphere() {
	FixedFunctionSphere(2, 1.0f);
}

void FixedFunctionSphere(int numDivisions, float radius) {
	static float X = 0.525731112119133606f;
	static float Y = 0.0f;
	static float Z = 0.850650808352039932f;

	static float vdata[12][3] = {
		{ -X,Y,Z },{ X,Y,Z },{ -X,Y,-Z },{ X,Y,-Z },
		{ Y,Z,X },{ Y,Z,-X },{ Y,-Z,X },{ Y,-Z,-X },
		{ Z,X,Y },{ -Z,X,Y },{ Z,-X,Y },{ -Z,-X,Y } 
	};
	static int tindices[20][3] = { 
		{ 0,4,1 },{ 0,9,4 },{ 9,5,4 },{ 4,5,8 },{ 4,8,1 },
		{ 8,10,1 },{ 8,3,10 },{ 5,3,8 },{ 5,2,3 },{ 2,7,3 },
		{ 7,10,3 },{ 7,6,10 },{ 7,11,6 },{ 11,0,6 },{ 0,1,6 },
		{ 6,1,10 },{ 9,0,11 },{ 9,11,2 },{ 9,2,5 },{ 7,2,11 } 
	};

	glBegin(GL_TRIANGLES);

	for (int i = 0; i < 20; ++i) {
		FixedFunctionSubdivTetrahedron(
			vdata[tindices[i][0]], vdata[tindices[i][1]], vdata[tindices[i][2]], numDivisions, radius
		);
	}

	glEnd();
}

void FixedFunctionCube() {
	FixedFunctionCube(1.0f, 1.0f, 1.0f);
}

void FixedFunctionCubeQuads(float extentsX, float extentsY, float extentsZ) {
	float min[] = { -extentsX, -extentsY, -extentsZ };
	float max[] = { +extentsX, +extentsY, +extentsZ };

	glBegin(GL_QUADS);

#if 1
	// Top!
	glNormal3f(0.0f, 1.0f, 0.0f);
	/*0*/glVertex3f(min[0], max[1], min[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*2*/glVertex3f(max[0], max[1], max[2]);
	/*3*/glVertex3f(min[0], max[1], max[2]);
#endif

#if 1
	// Front!
	glNormal3f(0.0f, 0.0f, 1.0f);
	/*0*/glVertex3f(min[0], max[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], max[2]);
	/*2*/glVertex3f(max[0], min[1], max[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
#endif

#if 1
	// Left
	glNormal3f(-1.0f, 0.0f, 0.0f);
	/*0*/glVertex3f(min[0], max[1], max[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*2*/glVertex3f(min[0], min[1], min[2]);
	/*1*/glVertex3f(min[0], max[1], min[2]);
#endif

#if 1
	// Bottom!
	glNormal3f(0.0f, -1.0f, 0.0f);
	/*0*/glVertex3f(min[0], min[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*2*/glVertex3f(max[0], min[1], max[2]);
	/*1*/glVertex3f(max[0], min[1], min[2]);
#endif

#if 1
	// Back!
	glNormal3f(0.0f, 0.0f, -1.0f);
	/*0*/glVertex3f(min[0], max[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], min[2]);
	/*2*/glVertex3f(max[0], min[1], min[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
#endif

#if 1
	// Right
	glNormal3f(1.0f, 0.0f, 0.0f);
	/*0*/glVertex3f(max[0], max[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*2*/glVertex3f(max[0], min[1], min[2]);
	/*3*/glVertex3f(max[0], min[1], max[2]);
#endif

	glEnd();
}

void FixedFunctionCube(float extentsX, float extentsY, float extentsZ) {
	float min[] = { -extentsX, -extentsY, -extentsZ };
	float max[] = { +extentsX, +extentsY, +extentsZ };

	glBegin(GL_TRIANGLES);

#if 1
	// Top!
	glNormal3f(0.0f, 1.0f, 0.0f);
	/*0*/glVertex3f(min[0], max[1], min[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*3*/glVertex3f(min[0], max[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*2*/glVertex3f(max[0], max[1], max[2]);
	/*3*/glVertex3f(min[0], max[1], max[2]);
#endif

#if 1
	// Front!
	glNormal3f(0.0f, 0.0f, 1.0f);
	/*0*/glVertex3f(min[0], max[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], max[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], max[2]);
	/*2*/glVertex3f(max[0], min[1], max[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
#endif

#if 1
	// Left
	glNormal3f(-1.0f, 0.0f, 0.0f);
	/*0*/glVertex3f(min[0], max[1], max[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*1*/glVertex3f(min[0], max[1], min[2]);
	/*1*/glVertex3f(min[0], max[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*2*/glVertex3f(min[0], min[1], min[2]);
#endif

#if 1
	// Bottom!
	glNormal3f(0.0f, -1.0f, 0.0f);
	/*0*/glVertex3f(min[0], min[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*1*/glVertex3f(max[0], min[1], min[2]);
	/*1*/glVertex3f(max[0], min[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], max[2]);
	/*2*/glVertex3f(max[0], min[1], max[2]);
#endif

#if 1
	// Back!
	glNormal3f(0.0f, 0.0f, -1.0f);
	/*0*/glVertex3f(min[0], max[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], min[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*3*/glVertex3f(min[0], min[1], min[2]);
	/*2*/glVertex3f(max[0], min[1], min[2]);
#endif

#if 1
	// Right
	glNormal3f(1.0f, 0.0f, 0.0f);
	/*0*/glVertex3f(max[0], max[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*3*/glVertex3f(max[0], min[1], max[2]);
	/*1*/glVertex3f(max[0], max[1], min[2]);
	/*2*/glVertex3f(max[0], min[1], min[2]);
	/*3*/glVertex3f(max[0], min[1], max[2]);
#endif

	glEnd();
}

void FixedFunctionTorus(float outerRaidus, float innerRadius) {
	float diff = (outerRaidus - innerRadius);
	float minorRad = diff * 0.5f;
	float majorRad = outerRaidus - minorRad;
	FixedFunctionTorus(15, 10, majorRad, minorRad);
}

void FixedFunctionTorus() {
	FixedFunctionTorus(1.0f, 0.5f);
}

void FixedFunctionCylinder(float height, float radius) {
	FixedFunctionCylinder(15, height, radius);
}

void FixedFunctionCylinder(float height) {
	FixedFunctionCylinder(15, height, 1.0f);
}

void FixedFunctionCylinder() {
	FixedFunctionCylinder(15, 1.0f, 1.0f);
}

void FixedFunctionCylinder(int slices, float height, float radius) {
	// Modified from http://math.hws.edu/graphicsbook/c4/s2.html
	height *= 0.5f;
	float twopi_slices = (2.0f * M_PI) / (float)slices;

	// HULL
	glBegin(GL_TRIANGLE_STRIP);
	for (int i = 0; i <= slices; i++) {
		double angle = twopi_slices * (float)i;  // i 16-ths of a full circle
		double x = cos(angle);
		double z = sin(angle);

		float norm[] = { (float)x, 0.0f, (float)z };
		float d = sqrtf(norm[0] * norm[0] + norm[1] * norm[1] + norm[2] * norm[2]);
		norm[0] /= d; norm[1] /= d; norm[2] /= d;

		x *= radius;
		z *= radius;

		glNormal3f(norm[0], norm[1], norm[2]); 
		glVertex3f((float)x, (float)height, (float)z);
		glVertex3f((float)x, (float)(-height), (float)z);
	}
	glEnd();

	// TOP CAP
	glNormal3f(0.0f, 1.0f, 0.0f);
	glBegin(GL_TRIANGLE_FAN); 
	for (int i = 0; i <= slices; i++) {
		double angle = twopi_slices * (float)i;
		double x = cos(angle) * radius;
		double z = sin(angle) * radius;
		glVertex3f((float)x, (float)height, (float)z);
	}
	glEnd();

	// BOTTOM CAP
	glNormal3f(0.0f, -1.0f, 0.0f);
	glBegin(GL_TRIANGLE_FAN); 
	for (int i = slices; i >= 0; i--) {
		double angle = twopi_slices * (float)i;
		double x = cos(angle) * radius;
		double z = sin(angle) * radius;
		glVertex3f((float)x, (float)(-height), (float)z);
	}
	glEnd();
}

/* From the opengl super bible! */
// Description of what the arguments mean is here: 
// http://www.povray.org/documentation/images/reference/mimxrtor.png
void FixedFunctionTorus(int TORUS_MAJOR_RES, int TORUS_MINOR_RES, float TORUS_MAJOR, float TORUS_MINOR) {
	int    i, j, k;
	double s, t, x, y, z, nx, ny, nz, scale, twopi;

	twopi = 2.0 * M_PI;
	for (i = 0; i < TORUS_MINOR_RES; i++)
	{
		glBegin(GL_QUAD_STRIP);
		for (j = 0; j <= TORUS_MAJOR_RES; j++)
		{
			for (k = 1; k >= 0; k--)
			{
				s = (i + k) % TORUS_MINOR_RES + 0.5;
				t = j % TORUS_MAJOR_RES;

				// Calculate point on surface
				x = (TORUS_MAJOR + TORUS_MINOR * cos(s * twopi / TORUS_MINOR_RES)) * cos(t * twopi / TORUS_MAJOR_RES);
				y = TORUS_MINOR * sin(s * twopi / TORUS_MINOR_RES);
				z = (TORUS_MAJOR + TORUS_MINOR * cos(s * twopi / TORUS_MINOR_RES)) * sin(t * twopi / TORUS_MAJOR_RES);

				// Calculate surface normal
				nx = x - TORUS_MAJOR * cos(t * twopi / TORUS_MAJOR_RES);
				ny = y;
				nz = z - TORUS_MAJOR * sin(t * twopi / TORUS_MAJOR_RES);
				scale = 1.0 / sqrt(nx*nx + ny*ny + nz*nz);
				nx *= scale;
				ny *= scale;
				nz *= scale;

				glNormal3f((float)nx, (float)ny, (float)nz);
				glVertex3f((float)x, (float)y, (float)z);
			}
		}

		glEnd();
	}
}

/* https://www.opengl.org/archives/resources/code/samples/redbook/torus.c
void FixedFunctionTorus(int numc, int numt) {
	int i, j, k;
	double s, t, x, y, z, twopi;

	twopi = 2 * M_PI;
	for (i = 0; i < numc; i++) {
		glBegin(GL_QUAD_STRIP);
		for (j = 0; j <= numt; j++) {
			for (k = 1; k >= 0; k--) {
				s = (i + k) % numc + 0.5;
				t = j % numt;

				x = (1 + .1*cos(s*twopi / numc))*cos(t*twopi / numt);
				y = (1 + .1*cos(s*twopi / numc))*sin(t*twopi / numt);
				z = .1 * sin(s * twopi / numc);
				glVertex3f(x, y, z);
			}
		}
		glEnd();
	}
} */

void FixedFunctionPlane(float size, int subdivs) {
	glBegin(GL_TRIANGLES);
	glNormal3f(0.0f, 1.0f, 0.0f);

	float slice = size / (float)subdivs;

	for (int i = 0; i < subdivs; ++i) {
		for (int j = 0; j < subdivs; ++j) {
			float x = ((float)i * slice) - (size * 0.5f);
			float z = ((float)j * slice) - (size * 0.5f);

			glVertex3f(x,         0.0f, z);
			glVertex3f(x + slice, 0.0f, z);
			glVertex3f(x,         0.0f, z + slice);
			glVertex3f(x + slice, 0.0f, z);
			glVertex3f(x + slice, 0.0f, z + slice);
			glVertex3f(x,         0.0f, z + slice);
		}
	}

	glEnd();
}

void FixedFunctionPlane(float size) {
	FixedFunctionPlane(size, 10);
}

void FixedFunctionPlane() {
	FixedFunctionPlane(10, 10);
}

void FixedFunctionOrigin(bool depthTest, bool twoSided) {
	bool isLit = glIsEnabled(GL_LIGHTING) == GL_TRUE;
	bool depthOn = glIsEnabled(GL_DEPTH_TEST) == GL_TRUE;

	if (isLit) {
		glDisable(GL_LIGHTING);
	}
	if (depthOn && !depthTest) {
		glDisable(GL_DEPTH_TEST);
	} else if (depthTest) {
		glEnable(GL_DEPTH_TEST);
	}

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);
	if (twoSided) {
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(-1.0f, 0.0f, 0.0f);
	}

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);
	if (twoSided) {
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, -1.0f, 0.0f);
	}

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	if (twoSided) {
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, -1.0f);
	}
	glEnd();

	if (isLit) {
		glEnable(GL_LIGHTING);
	}
	if (depthOn) {
		glEnable(GL_DEPTH_TEST);
	}
}

void FixedFunctionOrigin(bool depthTest) {
	FixedFunctionOrigin(depthTest, false);
}

void FixedFunctionOrigin() {
	FixedFunctionOrigin(false, false);
}

void FixedFunctionSubdivCone(float *v1, float *v2, int subdiv, float height, float radius) {
	float v0[3] = { 0, 0, 0 };

	if (subdiv == 0) {
		// Bottom cover of the cone
		glNormal3f(0.0f, -1.0f, 0.0f);
		glVertex3f(v0[0] * radius, v0[1] * radius, v0[2] * radius);
		glVertex3f(v2[0] * radius, v2[1] * radius, v2[2] * radius);
		glVertex3f(v1[0] * radius, v1[1] * radius, v1[2] * radius);

		v0[1] = height; // height of the cone, the tip on y axis
		// Side cover of the cone
		float e1[] = { v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2] };
		float e2[] = { v0[0] - v2[0], v0[1] - v2[1], v0[2] - v2[2] };
		float n[] = {
			e1[1] * e2[2] - e1[2] * e2[1],
			e1[2] * e2[0] - e1[0] * e2[2],
			e1[0] * e2[1] - e1[1] * e2[0],
		};
		float d = 1.0f / sqrtf(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
		
		glNormal3f(n[0] * d, n[1] * d, n[2] * d);
		glVertex3f(v0[0] * radius, v0[1] * radius, v0[2] * radius);
		
		glNormal3fv(v1);
		glVertex3f(v1[0] * radius, v1[1] * radius, v1[2] * radius);
		
		glNormal3fv(v2);
		glVertex3f(v2[0] * radius, v2[1] * radius, v2[2] * radius);

		return;
	}

	float v12[3]{
		v1[0] + v2[0],
		v1[1] + v2[1],
		v1[2] + v2[2],
	};

	// Normalize
	float d = 1.0f / sqrtf(v12[0] * v12[0] + v12[1] * v12[1] + v12[2] * v12[2]);
	v12[0] *= d; v12[1] *= d; v12[2] *= d;

	FixedFunctionSubdivCone(v1, v12, subdiv - 1, height, radius);
	FixedFunctionSubdivCone(v12, v2, subdiv - 1, height, radius);
}


// Adapted from: http://cs.gmu.edu/~jchen/graphics/book/examples/2.5.cone.c
void FixedFunctionCone(int subdiv, float height, float radius) {
	static float vdata[4][3] = {
		{  1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f,  1.0f },
		{ -1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, -1.0f }
	};

	glBegin(GL_TRIANGLES);
	FixedFunctionSubdivCone(vdata[0], vdata[1], subdiv, height, radius);
	FixedFunctionSubdivCone(vdata[1], vdata[2], subdiv, height, radius);
	FixedFunctionSubdivCone(vdata[2], vdata[3], subdiv, height, radius);
	FixedFunctionSubdivCone(vdata[3], vdata[0], subdiv, height, radius);
	glEnd();
}

void FixedFunctionCone(float height, float radius) {
	FixedFunctionCone(3, height, radius);
}

void FixedFunctionCone() {
	FixedFunctionCone(3, 1.0f, 1.0f);
}

<|start_filename|>Chapter02/Code/DemoBase.cpp<|end_filename|>
#include "DemoBase.h"
#include "imgui/imgui.h"
#include "FixedFunctionPrimitives.h"

DemoBase::DemoBase() {
	show_help = false;
	mouseLeftDown = false;
	mouseRightDown = false;
	mouseMiddleDown = false;
	mouseDelta = vec2(0.0f, 0.0f);
	mousePos = vec2(0.0f, 0.0f);
	size = vec2(0.0f, 0.0f);
	configHelpWindow = true;
}

void DemoBase::Initialize(int width, int height) {
	Resize(width, height);
}

void DemoBase::Resize(int width, int height) {
	camera.Perspective(60.0f, (float)width / (float)height, 0.01f, 1000.0f);
	size = vec2(width, height);
}

void DemoBase::Update(float dt) {
	if (mouseRightDown) {
		camera.Rotate(mouseDelta, dt);
	}
	else if (mouseMiddleDown) {
		camera.Zoom(mouseDelta.y, dt);
	}
	else if (mouseLeftDown) {
		camera.Pan(mouseDelta, dt);
	}

	// Update the camera position
	camera.Update(dt);
}

void DemoBase::ImGUI() {
	if (show_help) {
		if (configHelpWindow) {
			configHelpWindow = false;

			ImGui::SetNextWindowPos(ImVec2(400, 90));
			ImGui::SetNextWindowSize(ImVec2(370, 100));
		}
		ImGui::Begin("How to use", &show_help, ImGuiWindowFlags_NoResize);
		ImGui::Text("A 3 button mouse is needed to navigate the scene");
		ImGui::Text("Press the left mouse and move the mouse to pan");
		ImGui::Text("Press the middle mouse and move the mouse to zoom");
		ImGui::Text("Press the right mouse and move the mouse to rotate");
		ImGui::End();
	}
}

void DemoBase::Render() {
	FixedFunctionOrigin();
}

void DemoBase::SetMouseState(bool left, bool middle, bool right, const vec2& delta, const vec2& mouse) {
	mouseLeftDown = left;
	mouseMiddleDown = middle;
	mouseRightDown = right;
	mouseDelta = delta;
	mousePos = mouse;
}

<|start_filename|>Chapter02/Code/DemoBase.h<|end_filename|>
#ifndef _H_DEMO_BASE_
#define _H_DEMO_BASE_

#include "Camera.h"

class DemoBase {
private: // Disable
	DemoBase(const DemoBase&);
	DemoBase& operator=(const DemoBase);
protected:
	bool show_help;
	bool mouseLeftDown;
	bool mouseRightDown;
	bool mouseMiddleDown;
	vec2 mouseDelta;
	vec2 mousePos;
	vec2 size;
	bool configHelpWindow;
public:
	OrbitCamera camera;

	DemoBase();
	inline virtual ~DemoBase() { }

	virtual void Initialize(int width, int height);
	virtual void Resize(int width, int height);
	virtual void Render();
	virtual void Update(float dt);
	virtual void ImGUI();
	inline virtual void Shutdown() { }

	void SetMouseState(bool left, bool middle, bool right, const vec2& delta, const vec2& mouse);
};

#endif 

<|start_filename|>Chapter02/Code/GLWindow.cpp<|end_filename|>
#define WIN32_LEAN_AND_MEAN 
#define WIN32_EXTRA_LEAN
#include <windows.h>
#include <windowsx.h>

#include "glad/glad.h"

#include "GLWindow.h"

#define Clamp01(x) \
	(x < 0.0f? 0.0f : (x > 1.0f? 1.0f : x))

#define Bit(number) (1 << number)
#define BitValue(variable, bitmask) (variable & bitmask)
#define BitOn(variable, bitmask) (variable |= bitmask)
#define BitOff(variable, bitmask) (variable &= ~bitmask)
#define BitToggle(variable, bitmask) (variable ^ bitmask)

GLWindow::GLWindow(const char* title, int width, int height) 
	: IWindow(title, width, height) {
	mouseButtonState = 0;
	memset(keyboardState, false, sizeof(bool) * 256);
	m_vecMousePos = vec2(0.0f, 0.0f);
}

void GLWindow::SetClearColor(float r, float g, float b) {
	r = Clamp01(r);
	b = Clamp01(b);
	g = Clamp01(g);
	glClearColor(r, g, b, 1.0f);
}

void GLWindow::OnInitialize() {
	glClearColor(0.5f, 0.6f, 0.7f, 1.0f);

	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);
	
	// Set default OpenGL States
	OnResize(m_nWidth, m_nHeight);
}

void GLWindow::OnRender() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
}

void GLWindow::OnResize(int width, int height) {
	glViewport(0, 0, width, height);
}

void GLWindow::OnMouseMove(int x, int y) { 
	m_vecMousePos.x = x;
	m_vecMousePos.y = y;
}

void GLWindow::OnMouseDown(int mouseCode) { 
	BitOn(mouseButtonState, Bit(mouseCode));
}

void GLWindow::OnMouseUp(int mouseCode) {
	BitOff(mouseButtonState, Bit(mouseCode));
}

void GLWindow::OnKeyDown(int keyCode) {
	keyboardState[KeyIndex(keyCode)] = true;
}

void GLWindow::OnKeyUp(int keyCode) {
	keyboardState[KeyIndex(keyCode)] = false;
}

vec2 GLWindow::GetMousePosition() {
	return m_vecMousePos;
}

bool GLWindow::MouseButonDown(int button) {
	if (button <= 0 || button >= 4) {
		return false;
	}

	return BitValue(mouseButtonState, Bit(button));
}

bool GLWindow::KeyDown(int keyCode) {
	keyCode = KeyIndex(keyCode);
	if (keyCode < 0 || keyCode >= 256) {
		return false;
	}
	return keyboardState[keyCode];
}

void GLWindow::SetGLProjection(float* projectionArray) {
	glMatrixMode(GL_PROJECTION);
	glLoadMatrixf(projectionArray);
}

void GLWindow::SetGLModelView(float* mvArray) {
	glMatrixMode(GL_MODELVIEW);
	glLoadMatrixf(mvArray);
}

<|start_filename|>Chapter03/Code/GLWindow.h<|end_filename|>
#ifndef _H_GL_WINDOW_
#define _H_GL_WINDOW_

#include "IWindow.h"
#include "matrices.h"

class GLWindow : public IWindow {
protected:
	vec2 m_vecMousePos;
	unsigned int mouseButtonState;
	bool keyboardState[256];
public:
	GLWindow(const char* title, int width, int height);
	virtual ~GLWindow() { }
	void SetClearColor(float r, float g, float b);

	virtual void OnInitialize();
	virtual void OnRender();
	virtual void OnResize(int width, int height);

	virtual void OnMouseMove(int x, int y);
	virtual void OnMouseDown(int mouseCode);
	virtual void OnMouseUp(int mouseCode);

	virtual void OnKeyDown(int keyCode);
	virtual void OnKeyUp(int keyCode);

	vec2 GetMousePosition();
	bool MouseButonDown(int button);
	bool KeyDown(int key);

	void SetGLProjection(float* projectionArray);
	void SetGLModelView(float* mvArray);
};

#endif


<|start_filename|>Chapter01/Code/RaycastDemo.cpp<|end_filename|>
#include "RaycastDemo.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include <iostream>

void DebugRaycastResult(RaycastResult* result) {
	if (result == 0) {
		return;
	}

	std::cout << "Hit: " << result->hit << "\n";
	std::cout << "t: " << result->t << "\n";
	std::cout << "Point: " << result->point.x << ", " << result->point.y << ", " << result->point.z << "\n";
	std::cout << "Normal: " << result->normal.x << ", " << result->normal.y << ", " << result->normal.z << "\n";
}

void RaycastDemo::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);

	ResetRaycastResult(&raycastResult);

	sphere.position = vec3(-4, 2, 0);
	aabb.position = vec3(4, 2, 0);
	obb.position = vec3(-4, -2, 0);
	obb.orientation = Rotation3x3(30.0f, 20.0f, 0.0f);
	plane = FromTriangle(Triangle(
		Point(4, -1, 0),
		Point(3, -3, 0),
		Point(3, -3, 1)
	));
	triangle.a = vec3(0, 3, 0);
	triangle.b = vec3(-1, 1, 0);
	triangle.c = vec3(1, 1, 0);

	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);
}

void RaycastDemo::Render() {
	DemoBase::Render();

	static const float position[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };
	static const float ambient[] = { 0.0f, 0.0f, 0.5f, 0.0f };
	static const float diffuse[] = { 0.0f, 0.0f, 0.5f, 0.0f };

	glLightfv(GL_LIGHT0, GL_POSITION, position);
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);

	::Render(sphere);
	::Render(aabb);
	::Render(obb);
	::Render(plane);
	::Render(triangle);

	glDisable(GL_LIGHTING);
	if (mouseLeftDown) {
		screenRay = GetPickRay(mousePos, vec2(), size, camera.GetViewMatrix(), camera.GetProjectionMatrix());
		screenRay.NormalizeDirection();

		Raycast(sphere, screenRay, 0);
		Raycast(aabb, screenRay, 0);
		Raycast(obb, screenRay, 0);
		Raycast(plane, screenRay, 0);
		Raycast(triangle, screenRay, 0);

		RaycastResult allCasts[5];
		Raycast(sphere, screenRay, &allCasts[0]);
		Raycast(aabb, screenRay, &allCasts[1]);
		Raycast(obb, screenRay, &allCasts[2]);
		Raycast(plane, screenRay, &allCasts[3]);
		Raycast(triangle, screenRay, &allCasts[4]);

		raycastResult = allCasts[0];
		for (int i = 0; i < 5; ++i) {
			if (!allCasts[i].hit) {
				if (allCasts[i].t >= 0) {
					DebugRaycastResult(&allCasts[i]);
				}
			}

			if (allCasts[i].hit) {
				if (!raycastResult.hit) {
					raycastResult = allCasts[i];
				}
				else if (allCasts[i].t < raycastResult.t) {
					raycastResult = allCasts[i];
				}
			}
		}
	}

	if (raycastResult.hit) {
		glColor3f(1.0f, 0.0f, 0.0f);
		::Render(raycastResult.point);

		glColor3f(0.0f, 1.0f, 0.0f);
		Line normal(raycastResult.point, raycastResult.point + raycastResult.normal);
		::Render(normal);
	}
	glColor3f(0.0f, 0.0f, 1.0f);
	::Render(screenRay);
	glEnable(GL_LIGHTING);
}

void RaycastDemo::Update(float dt) {
	DemoBase::Update(dt);
}

<|start_filename|>Chapter01/Code/RigidbodyVolume.h<|end_filename|>
#ifndef _H_MASS_RIGIDBODY_
#define _H_MASS_RIGIDBODY_

#include "Rigidbody.h"

#define GRAVITY_CONST vec3(0.0f, -9.82f, 0.0f)

class RigidbodyVolume : public Rigidbody {
public:
	vec3 position;
	vec3 velocity;

#ifndef LINEAR_ONLY
	vec3 orientation;
	vec3 angVel;
#endif

	vec3 forces; // sumForces
#ifndef LINEAR_ONLY
	vec3 torques; // Sum torques
#endif

				  //vec3 inertia;
	float mass;
	float cor; // Coefficient of restitution
#ifdef DYNAMIC_FRICTION
	float staticFriction;
	float dynamicFriction;
#else
	float friction;
#endif

	OBB box;
	Sphere sphere;
public:

	inline RigidbodyVolume() :
		cor(0.5f), mass(1.0f),
#ifdef DYNAMIC_FRICTION
		staticFriction(0.5f),
		dynamicFriction(0.3f)
#else
		friction(0.6f)
#endif
		{
		type = RIGIDBODY_TYPE_BASE;
	}

	inline RigidbodyVolume(int bodyType) :
		cor(0.5f), mass(1.0f),
#ifdef DYNAMIC_FRICTION
		staticFriction(0.5f),
		dynamicFriction(0.3f)
#else
		friction(0.6f)
#endif
		{
			type = bodyType;
	}

	virtual ~RigidbodyVolume() { }

	virtual void Render();
	virtual void Update(float dt); // Update Position

	float InvMass();
#ifndef LINEAR_ONLY
	mat4 InvTensor();
#endif

	virtual void ApplyForces();
	void SynchCollisionVolumes();

	virtual void AddLinearImpulse(const vec3& impulse);
#ifndef LINEAR_ONLY
	virtual void AddRotationalImpulse(const vec3& point, const vec3& impulse);
#endif
};

CollisionManifold FindCollisionFeatures(RigidbodyVolume& ra, RigidbodyVolume& rb);
void ApplyImpulse(RigidbodyVolume& A, RigidbodyVolume& B, const CollisionManifold& M, int c);

#endif

<|start_filename|>Chapter05/Physics/ObjLoader.h<|end_filename|>
#ifndef _H_OBJ_LOADER_
#define _H_OBJ_LOADER_

#include "Geometry3D.h"

bool LoadMesh(const char* szFilePath, Mesh* mesh);
void FreeMesh(Mesh* mesh);

#endif 


<|start_filename|>Chapter08/Physics/Geometry3D.cpp<|end_filename|>
#include "Geometry3D.h"
#include <cmath>
#include <cfloat>

#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

float Length(const Line& line) {
	return Magnitude(line.start - line.end);
}

float LengthSq(const Line& line) {
	return MagnitudeSq(line.start - line.end);
}

Ray FromPoints(const Point& from, const Point& to) {
	return Ray(
		from,
		Normalized(to - from)
	);
}

vec3 GetMin(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fminf(p1.x, p2.x), fminf(p1.y, p2.y), fminf(p1.z, p2.z));
}
vec3 GetMax(const AABB& aabb) {
	vec3 p1 = aabb.position + aabb.size;
	vec3 p2 = aabb.position - aabb.size;

	return vec3(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y), fmaxf(p1.z, p2.z));
}

AABB FromMinMax(const vec3& min, const vec3& max) {
	return AABB((min + max) * 0.5f, (max - min) * 0.5f);
}

float PlaneEquation(const Point& point, const Plane& plane) {
	return Dot(point, plane.normal) - plane.distance;
}

#ifndef NO_EXTRAS
float PlaneEquation(const Plane& plane, const Point& point) {
	return Dot(point, plane.normal) - plane.distance;
}
#endif

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line& shape) {
	os << "start: (" << shape.start.x << ", " << shape.start.y << ", " << shape.start.z << "), end: (";
	os << shape.end.x << ", " << shape.end.y << ", " << shape.end.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Ray& shape) {
	os << "origin: (" << shape.origin.x << ", " << shape.origin.y << ", " << shape.origin.z << "), ";
	os << "direction: (" << shape.direction.x << ", " << shape.direction.y << ", " << shape.direction.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Sphere& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "radius: " << shape.radius;
	return os;
}

std::ostream& operator<<(std::ostream& os, const AABB& shape) {
	vec3 min = GetMin(shape);
	vec3 max = GetMax(shape);
	os << "min: (" << min.x << ", " << min.y << ", " << min.z << "), ";
	os << "max: (" << max.x << ", " << max.y << ", " << max.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const Plane& shape) {
	os << "normal: (" << shape.normal.x << ", " << shape.normal.y << ", " << shape.normal.z << "), ";
	os << "distance: " << shape.distance;
	return os;
}

std::ostream& operator<<(std::ostream& os, const Triangle& shape) {
	os << "a: (" << shape.a.x << ", " << shape.a.y << ", " << shape.a.z << "), ";
	os << "b: (" << shape.b.x << ", " << shape.b.y << ", " << shape.b.z << "), ";
	os << "c: (" << shape.c.x << ", " << shape.c.y << ", " << shape.c.z << ")";
	return os;
}

std::ostream& operator<<(std::ostream& os, const OBB& shape) {
	os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
	os << "size:" << shape.size.x << ", " << shape.size.y << ", " << shape.size.z << "), ";
	os << "x basis:" << shape.orientation._11 << ", " << shape.orientation._21 << ", " << shape.orientation._31 << "), ";
	os << "y basis:" << shape.orientation._12 << ", " << shape.orientation._22 << ", " << shape.orientation._32 << "), ";
	os << "z basis:" << shape.orientation._13 << ", " << shape.orientation._23 << ", " << shape.orientation._33 << ")";
	return os;
}
#endif 

bool PointInSphere(const Point& point, const Sphere& sphere) {
	return MagnitudeSq(point - sphere.position) < sphere.radius * sphere.radius;
}

bool PointOnPlane(const Point& point, const Plane& plane) {
	// This should probably use an epsilon!
	//return Dot(point, plane.normal) - plane.distance == 0.0f;

	return CMP(Dot(point, plane.normal) - plane.distance, 0.0f);
}

bool PointInAABB(const Point& point, const AABB& aabb) {
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	if (point.x < min.x || point.x < min.y || point.z < min.z) {
		return false;
	}
	if (point.x > max.x || point.y > max.y || point.z > max.z) {
		return false;
	}

	return true;
}

bool PointInOBB(const Point& point, const OBB& obb) {
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			return false;
		}
		if (distance < -obb.size.asArray[i]) {
			return false;
		}
	}

	return true;
}

Point ClosestPoint(const Sphere& sphere, const Point& point) {
	vec3 sphereToPoint = point - sphere.position;
	Normalize(sphereToPoint);
	sphereToPoint = sphereToPoint * sphere.radius;
	return sphereToPoint + sphere.position;
}

Point ClosestPoint(const AABB& aabb, const Point& point) {
	Point result = point;
	Point min = GetMin(aabb);
	Point max = GetMax(aabb);

	result.x = (result.x < min.x) ? min.x : result.x;
	result.y = (result.y < min.x) ? min.y : result.y;
	result.z = (result.z < min.x) ? min.z : result.z;

	result.x = (result.x > max.x) ? max.x : result.x;
	result.y = (result.y > max.x) ? max.y : result.y;
	result.z = (result.z > max.x) ? max.z : result.z;

	return result;
}

Point ClosestPoint(const OBB& obb, const Point& point) {
	Point result = obb.position;
	vec3 dir = point - obb.position;

	for (int i = 0; i < 3; ++i) {
		const float* orientation = &obb.orientation.asArray[i * 3];
		vec3 axis(orientation[0], orientation[1], orientation[2]);

		float distance = Dot(dir, axis);

		if (distance > obb.size.asArray[i]) {
			distance = obb.size.asArray[i];
		}
		if (distance < -obb.size.asArray[i]) {
			distance = -obb.size.asArray[i];
		}

		result = result + (axis * distance);
	}

	return result;
}

Point ClosestPoint(const Plane& plane, const Point& point) {
	// This works assuming plane.Normal is normalized, which it should be
	float distance = Dot(plane.normal, point) - plane.distance;
	// If the plane normal wasn't normalized, we'd need this:
	// distance = distance / DOT(plane.Normal, plane.Normal);

	return point - plane.normal * distance;
}

bool PointOnLine(const Point& point, const Line& line) {
	Point closest = ClosestPoint(line, point);
	float distanceSq = MagnitudeSq(closest - point);
	return CMP(distanceSq, 0.0f);
}

Point ClosestPoint(const Line& line, const Point& point) {
	vec3 lVec = line.end - line.start; // Line Vector
	// Project "point" onto the "Line Vector", computing:
	// closest(t) = start + t * (end - start)
	// T is how far along the line the projected point is
	float t = Dot(point - line.start, lVec) / Dot(lVec, lVec);
	// Clamp t to the 0 to 1 range
	t = fmaxf(t, 0.0f);
	t = fminf(t, 1.0f);
	// Return projected position of t
	return line.start + lVec * t;
}

bool PointOnRay(const Point& point, const Ray& ray) {
	if (point == ray.origin) {
		return true;
	}

	vec3 norm = point - ray.origin;
	Normalize(norm);
	float diff = Dot(norm, ray.direction); // Direction is normalized
	// If BOTH vectors point in the same direction, diff should be 1
	return CMP(diff, 1.0f);
}

Point ClosestPoint(const Ray& ray, const Point& point) {
	// Project point onto ray, 
	float t = Dot(point - ray.origin, ray.direction);
	// Not needed if direction is normalized!
	// t /= Dot(ray.direction, ray.direction);

	// We only want to clamp t in the positive direction.
	// The ray extends infinatley in this direction!
	t = fmaxf(t, 0.0f);

	// Compute the projected position from the clamped t
	// Notice we multiply r.Normal by t, not AB.
	// This is becuase we want the ray in the direction 
	// of the normal, which technically the line segment is
	// but this is much more explicit and easy to read.
	return Point(ray.origin + ray.direction * t);
}

#ifndef NO_EXTRAS
bool PointInPlane(const Point& point, const Plane& plane) {
	return PointOnPlane(point, plane);
}
bool PointInLine(const Point& point, const Line& line) {
	return PointOnLine(point, line);
}
bool PointInRay(const Point& point, const Ray& ray) {
	return PointOnRay(point, ray);
}
bool ContainsPoint(const Sphere& sphere, const Point& point) {
	return PointInSphere(point, sphere);
}
bool ContainsPoint(const Point& point, const Sphere& sphere) {
	return PointInSphere(point, sphere);
}
bool ContainsPoint(const AABB& aabb, const Point& point) {
	return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const AABB& aabb) {
	return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const OBB& obb) {
	return PointInOBB(point, obb);
}
bool ContainsPoint(const OBB& obb, const Point& point) {
	return PointInOBB(point, obb);
}
bool ContainsPoint(const Point& point, const Plane& plane) {
	return PointOnPlane(point, plane);
}
bool ContainsPoint(const Plane& plane, const Point& point) {
	return PointOnPlane(point, plane);
}
bool ContainsPoint(const Point& point, const Line& line) {
	return PointOnLine(point, line);
}
bool ContainsPoint(const Line& line, const Point& point) {
	return PointOnLine(point, line);
}
bool ContainsPoint(const Point& point, const Ray& ray) {
	return PointOnRay(point, ray);
}
bool ContainsPoint(const Ray& ray, const Point& point) {
	return PointOnRay(point, ray);
}
Point ClosestPoint(const Point& point, const Sphere& sphere) {
	return ClosestPoint(sphere, point);
}
Point ClosestPoint(const Point& point, const AABB& aabb) {
	return ClosestPoint(aabb, point);
}
Point ClosestPoint(const Point& point, const OBB& obb) {
	return ClosestPoint(obb, point);
}
Point ClosestPoint(const Point& point, const Plane& plane) {
	return ClosestPoint(plane, point);
}
Point ClosestPoint(const Point& point, const Line& line) {
	return ClosestPoint(line, point);
}
Point ClosestPoint(const Point& point, const Ray& ray) {
	return ClosestPoint(ray, point);
}
#endif

bool SphereSphere(const Sphere& s1, const Sphere& s2) {
	float radiiSum = s1.radius + s2.radius;
	float sqDistance = MagnitudeSq(s1.position - s2.position);
	return sqDistance < radiiSum * radiiSum;
}

bool SphereAABB(const Sphere& sphere, const AABB& aabb) {
	Point closestPoint = ClosestPoint(aabb, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool SphereOBB(const Sphere& sphere, const OBB& obb) {
	Point closestPoint = ClosestPoint(obb, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool SpherePlane(const Sphere& sphere, const Plane& plane) {
	Point closestPoint = ClosestPoint(plane, sphere.position);
	float distSq = MagnitudeSq(sphere.position - closestPoint);
	float radiusSq = sphere.radius * sphere.radius;
	return distSq < radiusSq;
}

bool AABBAABB(const AABB& aabb1, const AABB& aabb2) {
	Point aMin = GetMin(aabb1);
	Point aMax = GetMax(aabb1);
	Point bMin = GetMin(aabb2);
	Point bMax = GetMax(aabb2);

	return	(aMin.x <= bMax.x && aMax.x >= bMin.x) &&
			(aMin.y <= bMax.y && aMax.y >= bMin.y) &&
			(aMin.z <= bMax.z && aMax.z >= bMin.z);
}

bool AABBOBB(const AABB& aabb, const OBB& obb) {
	const float* o = obb.orientation.asArray;

	vec3 test[15] = {
		vec3(1, 0, 0), // AABB axis 1
		vec3(0, 1, 0), // AABB axis 2
		vec3(0, 0, 1), // AABB axis 3
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8])
	};

	for (int i = 0; i < 3; ++i) { // Fill out rest of axis
		test[6 + i * 3 + 0] = Cross(test[i], test[0]);
		test[6 + i * 3 + 1] = Cross(test[i], test[1]);
		test[6 + i * 3 + 2] = Cross(test[i], test[2]);
	}

	for (int i = 0; i < 15; ++i) {
		if (!OverlapOnAxis(aabb, obb, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool OverlapOnAxis(const AABB& aabb, const OBB& obb, const vec3& axis) {
	Interval a = GetInterval(aabb, axis);
	Interval b = GetInterval(obb, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

bool OverlapOnAxis(const OBB& obb1, const OBB& obb2, const vec3& axis) {
	Interval a = GetInterval(obb1, axis);
	Interval b = GetInterval(obb1, axis);
	return ((b.min <= a.max) && (a.min <= b.max));
}

Interval GetInterval(const OBB& obb, const vec3& axis) {
	vec3 vertex[8];

	vec3 C = obb.position;	// OBB Center
	vec3 E = obb.size;		// OBB Extents
	const float* o = obb.orientation.asArray;
	vec3 A[] = {			// OBB Axis
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8]),
	};

	vertex[0] = C + A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
	vertex[1] = C - A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
	vertex[2] = C + A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
	vertex[3] = C + A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
	vertex[4] = C - A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
	vertex[5] = C + A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
	vertex[6] = C - A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
	vertex[7] = C - A[0] * E[0] - A[1] * E[1] + A[2] * E[2];

	Interval result;
	result.min = result.max = Dot(axis, vertex[0]);

	for (int i = 1; i < 8; ++i) {
		float projection = Dot(axis, vertex[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}

Interval GetInterval(const AABB& aabb, const vec3& axis) {
	vec3 i = GetMin(aabb);
	vec3 a = GetMax(aabb);

	vec3 vertex[8] = {
		vec3(i.x, a.y, a.z),
		vec3(i.x, a.y, i.z),
		vec3(i.x, i.y, a.z),
		vec3(i.x, i.y, i.z),
		vec3(a.x, a.y, a.z),
		vec3(a.x, a.y, i.z),
		vec3(a.x, i.y, a.z),
		vec3(a.x, i.y, i.z)
	};

	Interval result;
	result.min = result.max = Dot(axis, vertex[0]);

	for (int i = 1; i < 8; ++i) {
		float projection = Dot(axis, vertex[i]);
		result.min = (projection < result.min) ? projection : result.min;
		result.max = (projection > result.max) ? projection : result.max;
	}

	return result;
}

bool AABBPlane(const AABB& aabb, const Plane& plane) {
	// Project the half extents of the AABB onto the plane normal
	float pLen =aabb.size.x * fabsf(plane.normal.x) +
				aabb.size.y * fabsf(plane.normal.y) +
				aabb.size.z * fabsf(plane.normal.z);
	// Find the distance from the center of the AABB to the plane
	float dist = Dot(plane.normal, aabb.position) - plane.distance;
	// Intersection occurs if the distance falls within the projected side
	return fabsf(dist) <= pLen;
}

bool OBBOBB(const OBB& obb1, const OBB& obb2) {
	const float* o1 = obb1.orientation.asArray;
	const float* o2 = obb2.orientation.asArray;

	vec3 test[15] = {
		vec3(o1[0], o1[1], o1[2]),
		vec3(o1[3], o1[4], o1[5]),
		vec3(o1[6], o1[7], o1[8]),
		vec3(o2[0], o2[1], o2[2]),
		vec3(o2[3], o2[4], o2[5]),
		vec3(o2[6], o2[7], o2[8])
	};

	for (int i = 0; i < 3; ++i) { // Fill out rest of axis
		test[6 + i * 3 + 0] = Cross(test[i], test[0]);
		test[6 + i * 3 + 1] = Cross(test[i], test[1]);
		test[6 + i * 3 + 2] = Cross(test[i], test[2]);
	}

	for (int i = 0; i < 15; ++i) {
		if (!OverlapOnAxis(obb1, obb2, test[i])) {
			return false; // Seperating axis found
		}
	}

	return true; // Seperating axis not found
}

bool OBBPlane(const OBB& obb, const Plane& plane) {
	// Local variables for readability only
	const float* o = obb.orientation.asArray;
	vec3 rot[] = { // rotation / orientation
		vec3(o[0], o[1], o[2]),
		vec3(o[3], o[4], o[5]),
		vec3(o[6], o[7], o[8]),
	};
	vec3 normal = plane.normal;

	// Project the half extents of the AABB onto the plane normal
	float pLen =obb.size.x * fabsf(Dot(normal, rot[0])) +
				obb.size.y * fabsf(Dot(normal, rot[1])) +
				obb.size.z * fabsf(Dot(normal, rot[2]));
	// Find the distance from the center of the OBB to the plane
	float dist = Dot(plane.normal, obb.position) - plane.distance;
	// Intersection occurs if the distance falls within the projected side
	return fabsf(dist) <= pLen;
}

bool PlanePlane(const Plane& plane1, const Plane& plane2) {
	// Compute direction of intersection line
	vec3 d = Cross(plane1.normal, plane2.normal);

	// Check the length of the direction line
	// if the length is 0, no intersection happened
	return !(CMP(Dot(d, d), 0));

	// We could have used the dot product here, instead of the cross product
}

<|start_filename|>Chapter03/Code/PhysicsSystem.h<|end_filename|>
#ifndef _H_PHYSICS_SYSTEM_
#define _H_PHYSICS_SYSTEM_

#include "Rigidbody.h"

class PhysicsSystem {
protected:
	std::vector<Rigidbody*> bodies;
	std::vector<OBB> constraints;
public:
	void Update(float deltaTime);
	void Render();
	
	void AddRigidbody(Rigidbody* body);
	void AddConstraint(const OBB& constraint);

	void ClearRigidbodys();
	void ClearConstraints();
};

#endif

<|start_filename|>Chapter05/Physics/Scene.cpp<|end_filename|>
#include "Scene.h"
#include <algorithm>

void Scene::AddModel(Model* model) {
	if (std::find(objects.begin(), objects.end(), model) != objects.end()) {
		// Duplicate object, don't add
		return;
	}
	objects.push_back(model);
}

void Scene::RemoveModel(Model* model) {
	objects.erase(std::remove(objects.begin(), objects.end(), model), objects.end());
}

void Scene::UpdateModel(Model* model) {
	// Stub
}

std::vector<Model*> Scene::FindChildren(const Model* model) {
	std::vector<Model*> result;

	for (int i = 0, size = objects.size(); i < size; ++i) {
		if (objects[i] == 0 || objects[i] == model) {
			continue;
		}

		Model* iterator = objects[i]->parent;
		if (iterator != 0) {
			if (iterator == model) {
				result.push_back(objects[i]);
				continue;
			}
			iterator = iterator->parent;
		}
	}

	return result;
}

Model* Scene::Raycast(const Ray& ray) {
	if (octree != 0) {
		// :: lets the compiler know to look outside class scope
		return ::Raycast(octree, ray);
	}

	Model* result = 0;
	float result_t = -1;

	for (int i = 0, size = objects.size(); i < size; ++i) {
		float t = ModelRay(*objects[i], ray);
		if (result == 0 && t >= 0) {
			result = objects[i];
			result_t = t;
		}
		else if (result != 0 && t < result_t) {
			result = objects[i];
			result_t = t;
		}
	}

	return result;
}

std::vector<Model*> Scene::Query(const Sphere& sphere) {
	if (octree != 0) {
		// :: lets the compiler know to look outside class scope
		return ::Query(octree, sphere);
	}

	std::vector<Model*> result;
	for (int i = 0, size = objects.size(); i < size; ++i) {
		OBB bounds = GetOBB(*objects[i]);
		if (SphereOBB(sphere, bounds)) {
			result.push_back(objects[i]);
		}
	}
	return result;
}

std::vector<Model*> Scene::Query(const AABB& aabb) {
	if (octree != 0) {
		// :: lets the compiler know to look outside class scope
		return ::Query(octree, aabb);
	}

	std::vector<Model*> result;
	for (int i = 0, size = objects.size(); i < size; ++i) {
		OBB bounds = GetOBB(*objects[i]);
		if (AABBOBB(aabb, bounds)) {
			result.push_back(objects[i]);
		}
	}
	return result;
}

void SplitTree(OctreeNode* node, int depth) {
	if (depth-- <= 0) { // Decrements depth
		return;
	}

	if (node->children == 0) {
		node->children = new OctreeNode[8];

		vec3 c = node->bounds.position;
		vec3 e = node->bounds.size *0.5f;

		node->children[0].bounds = AABB(c + vec3(-e.x, +e.y, -e.z), e);
		node->children[1].bounds = AABB(c + vec3(+e.x, +e.y, -e.z), e);
		node->children[2].bounds = AABB(c + vec3(-e.x, +e.y, +e.z), e);
		node->children[3].bounds = AABB(c + vec3(+e.x, +e.y, +e.z), e);
		node->children[4].bounds = AABB(c + vec3(-e.x, -e.y, -e.z), e);
		node->children[5].bounds = AABB(c + vec3(+e.x, -e.y, -e.z), e);
		node->children[6].bounds = AABB(c + vec3(-e.x, -e.y, +e.z), e);
		node->children[7].bounds = AABB(c + vec3(+e.x, -e.y, +e.z), e);
	}

	if (node->children != 0 && node->models.size() > 0) {
		for (int i = 0; i < 8; ++i) { // For each child
			for (int j = 0, size = node->models.size(); j < size; ++j) {
				OBB bounds = GetOBB(*node->models[j]);
				if (AABBOBB(node->children[i].bounds, bounds)) {
					node->children[i].models.push_back(node->models[j]);
				}
			}
		}
		node->models.clear();

		// Recurse
		for (int i = 0; i < 8; ++i) {
			SplitTree(&(node->children[i]), depth);
		}
	}
}

void Insert(OctreeNode* node, Model* model) {
	OBB bounds = GetOBB(*model);
	if (AABBOBB(node->bounds, bounds)) {
		if (node->children == 0) {
			node->models.push_back(model);
		}
		else {
			for (int i = 0; i < 8; ++i) {
				Insert(&(node->children[i]), model);
			}
		}
	}
}

void Remove(OctreeNode* node, Model* model) {
	if (node->children == 0) {
		std::vector<Model*>::iterator it = std::find(node->models.begin(), node->models.end(), model);
		if (it != node->models.end()) {
			node->models.erase(it);
		}
	}
	else {
		for (int i = 0; i < 8; ++i) {
			Remove(&(node->children[i]), model);
		}
	}
}

void Update(OctreeNode* node, Model* model) {
	Remove(node, model);
	Insert(node, model);
}

Model* FindClosest(const std::vector<Model*>& set, const Ray& ray) {
	if (set.size() == 0) {
		return 0;
	}

	Model* closest = 0;
	float closest_t = -1;

	for (int i = 0, size = set.size(); i < size; ++i) {
		float this_t = ModelRay(*set[i], ray);

		if (this_t < 0) {
			continue;
		}

		if (closest_t < 0 || this_t < closest_t) {
			closest_t = this_t;
			closest = set[i];
		}
	}

	return closest;
}

Model* Raycast(OctreeNode* node, const Ray& ray) {
	float t = Raycast(node->bounds, ray);

	if (t >= 0) {
		if (node->children == 0) {
			return FindClosest(node->models, ray);
		}
		else {
			std::vector<Model*> results;
			for (int i = 0; i < 8; ++i) {
				Model* result = Raycast(&(node->children[i]), ray);
				if (result != 0) {
					results.push_back(result);
				}
			}
			return FindClosest(results, ray);
		}
	}

	return 0;
}

std::vector<Model*> Query(OctreeNode* node, const Sphere& sphere) {
	std::vector<Model*> result;

	if (SphereAABB(sphere, node->bounds)) {
		if (node->children == 0) {
			for (int i = 0, size = node->models.size(); i < size; ++i) {
				OBB bounds = GetOBB(*(node->models[i]));
				if (SphereOBB(sphere, bounds)) {
					result.push_back(node->models[i]);
				}
			}
		}
		else {
			for (int i = 0; i < 8; ++i) {
				std::vector<Model*> child = Query(&(node->children[i]), sphere);
				if (child.size() > 0) {
					result.insert(result.end(), child.begin(), child.end());
				}
			}
		}
	}

	return result;
}

std::vector<Model*> Query(OctreeNode* node, const AABB& aabb) {
	std::vector<Model*> result;

	if (AABBAABB(aabb, node->bounds)) {
		if (node->children == 0) {
			for (int i = 0, size = node->models.size(); i < size; ++i) {
				OBB bounds = GetOBB(*(node->models[i]));
				if (AABBOBB(aabb, bounds)) {
					result.push_back(node->models[i]);
				}
			}
		}
		else {
			for (int i = 0; i < 8; ++i) {
				std::vector<Model*> child = Query(&(node->children[i]), aabb);
				if (child.size() > 0) {
					result.insert(result.end(), child.begin(), child.end());
				}
			}
		}
	}

	return result;
}

bool Scene::Accelerate(const vec3& position, float size) {
	if (octree != 0) {
		return false;
	}

	vec3 min(position.x - size, position.y - size, position.z - size);
	vec3 max(position.x + size, position.y + size, position.z + size);

	// Construct tree root
	octree = new OctreeNode();
	octree->bounds = FromMinMax(min, max);
	octree->children = 0;
	for (int i = 0, size = objects.size(); i < size; ++i) {
		octree->models.push_back(objects[i]);
	}

	// Create tree
	SplitTree(octree, 5);
	return true;
}

<|start_filename|>Chapter01/Code/Cloth.cpp<|end_filename|>
#include "Cloth.h"
#include "glad/glad.h"
#include "FixedFunctionPrimitives.h"

void Cloth::Initialize(int gridSize, float distance, const vec3& position) {
	float k = -1.0f;
	float b = 0.0f;
	clothSize = gridSize;

	verts.clear();
	structural.clear();
	shear.clear();
	bend.clear();

	verts.resize(gridSize * gridSize);
	float halfSize = (float)(gridSize - 1) * 0.5f;

	if (gridSize < 3) {
		gridSize = 3;
	}

	// Create vertices
	for (int x = 0; x < gridSize; ++x) {
		for (int z = 0; z < gridSize; ++z) {
			int i = z * gridSize + x;

			float x_pos = ((float)x + position.x - halfSize) * distance;
			float z_pos = ((float)z + position.z - halfSize) * distance;


			verts[i].SetPosition(vec3(x_pos, position.y, z_pos));
			verts[i].SetMass(1.0f);
			verts[i].SetBounce(0.0f);
			verts[i].SetFriction(0.9f);
		}
	}

	// Create left to right structural springs
	for (int x = 0; x < gridSize; ++x) {
		for (int z = 0; z < gridSize - 1; ++z) {
			int i = z * gridSize + x;
			int j = (z + 1) * gridSize + x;

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			structural.push_back(spring);
		}
	}

	// Create up and down structural springs
	for (int x = 0; x < gridSize - 1; ++x) {
		for (int z = 0; z < gridSize; ++z) {
			int i = z * gridSize + x;
			int j = z * gridSize + (x + 1);

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			structural.push_back(spring);
		}
	}

	// Create left to right shear springs
	for (int x = 0; x < gridSize - 1; ++x) {
		for (int z = 0; z < gridSize - 1; ++z) {
			int i = z * gridSize + x;
			int j = (z + 1) * gridSize + (x + 1);

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			shear.push_back(spring);
		}
	}

	// Create right to left shear springs
	for (int x = 1; x < gridSize; ++x) {
		for (int z = 0; z < gridSize - 1; ++z) {
			int i = z * gridSize + x;
			int j = (z + 1) * gridSize + (x - 1);

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			shear.push_back(spring);
		}
	}
	
	// Create left to right bend springs
	for (int x = 0; x < gridSize; ++x) {
		for (int z = 0; z < gridSize - 2; ++z) {
			int i = z * gridSize + x;
			int j = (z + 2) * gridSize + x;

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			bend.push_back(spring);
		}
	}

	// Create up and down bend springs
	for (int x = 0; x < gridSize - 2; ++x) {
		for (int z = 0; z < gridSize; ++z) {
			int i = z * gridSize + x;
			int j = z * gridSize + (x + 2);

			float rest = Magnitude(verts[i].GetPosition() - verts[j].GetPosition());
			Spring spring(k, b, rest);
			spring.SetParticles(&verts[i], &verts[j]);
			bend.push_back(spring);
		}
	}
}

void Cloth::SetStructuralSprings(float k, float b) {
	for (int i = 0, size = structural.size(); i < size; ++i) {
		structural[i].SetConstants(k, b);
	}
}

void Cloth::SetShearSprings(float k, float b) {
	for (int i = 0, size = shear.size(); i < size; ++i) {
		shear[i].SetConstants(k, b);
	}
}

void Cloth::SetBendSprings(float k, float b) {
	for (int i = 0, size = bend.size(); i < size; ++i) {
		bend[i].SetConstants(k, b);
	}
}

void Cloth::SetParticleMass(float mass) {
	for (int i = 0, size = verts.size(); i < size; ++i) {
		verts[i].SetMass(mass);
	}
}

void Cloth::ApplyForces() {
	for (int i = 0, size = verts.size(); i < size; ++i) {
		verts[i].ApplyForces();
	}
}

void Cloth::Update(float dt) {
	for (int i = 0, size = verts.size(); i < size; ++i) {
		verts[i].Update(dt);
	}
}

void Cloth::SolveConstraints(const std::vector<OBB>& constraints) {
	for (int i = 0, size = verts.size(); i < size; ++i) {
		verts[i].SolveConstraints(constraints);
	}
}

void Cloth::ApplySpringForces(float dt) {
	for (int i = 0, size = structural.size(); i < size; ++i) {
		structural[i].ApplyForce(dt);
	}
	for (int i = 0, size = shear.size(); i < size; ++i) {
		shear[i].ApplyForce(dt);
	}
	for (int i = 0, size = bend.size(); i < size; ++i) {
		bend[i].ApplyForce(dt);
	}
}

void Cloth::Render(bool debug) {
	static const float redDiffuse[]{ 200.0f / 255.0f, 0.0f, 0.0f, 0.0f };
	static const float redAmbient[]{ 200.0f / 255.0f, 50.0f / 255.0f, 50.0f / 255.0f, 0.0f };
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };

	glColor3f(redDiffuse[0], redDiffuse[1], redDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, redAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, redDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);

	if (debug) {
		for (int i = 0, size = verts.size(); i < size; ++i) {
			verts[i].Render();
		}

		GLboolean status;
		glGetBooleanv(GL_LIGHTING, &status);
		glDisable(GL_LIGHTING);
		
		glColor3f(1.0f, 0.0f, 1.0f);
		for (int i = 0, size = structural.size(); i < size; ++i) {
			if (structural[i].GetP1() == 0 || structural[i].GetP2() == 0) {
				continue;
			}

			Line l(structural[i].GetP1()->GetPosition(), structural[i].GetP2()->GetPosition());
			::Render(l);
		}

		glColor3f(1.0f, 1.0f, 0.0f);
		for (int i = 0, size = shear.size(); i < size; ++i) {
			if (shear[i].GetP1() == 0 || shear[i].GetP2() == 0) {
				continue;
			}

			Line l(shear[i].GetP1()->GetPosition(), shear[i].GetP2()->GetPosition());
			::Render(l);
		}

		glColor3f(0.0f, 1.0f, 1.0f);
		for (int i = 0, size = bend.size(); i < size; ++i) {
			if (bend[i].GetP1() == 0 || bend[i].GetP2() == 0) {
				continue;
			}

			vec3 p1 = bend[i].GetP1()->GetPosition();
			vec3 p2 = bend[i].GetP2()->GetPosition();

			// Visualization
			/*p1.y += 0.1f;
			p2.y += 0.1f;
			for (int j = i; j >= 0; --j) {
				p1.y += 0.1f;
				p2.y += 0.1f;
			}*/

			Line l(p1, p2);
			::Render(l);
		}

		if (status) {
			glEnable(GL_LIGHTING);
		}
	}
	else {
		for (int x = 0; x < clothSize - 1; ++x) {
			for (int z = 0; z < clothSize - 1; ++z) {
				int tl = z * clothSize + x;
				int bl = (z + 1) * clothSize + x;
				int tr = z * clothSize + (x + 1);
				int br = (z + 1) * clothSize + (x + 1);

				Triangle t1(verts[tl].GetPosition(), verts[br].GetPosition(), verts[bl].GetPosition());
				Triangle t2(verts[tl].GetPosition(), verts[tr].GetPosition(), verts[br].GetPosition());

				::Render(t1, true);
				::Render(t2, true);
			}
		}
	}
}

<|start_filename|>Chapter01/Code/SimpleSprings.h<|end_filename|>
#ifndef _H_SIMPLE_SPRINGS_
#define _H_SIMPLE_SPRINGS_

#include "DemoBase.h"

#include "PhysicsSystem.h"
#include "Particle.h"
#include "Spring.h"
#include <vector>
#include <list>

class SimpleSprings : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	std::vector<Particle> particles;
	std::list<float> deltaTimes;

	float lastFrameTime;
	bool size_imgui_window;
protected:
	void ResetDemo();
public:
	inline SimpleSprings() : DemoBase(), size_imgui_window(true) { }

	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();

	float Random(float min, float max);
	vec3 Random(vec3 min, vec3 max);
};

#endif 


<|start_filename|>Chapter01/Code/RotationalForce.cpp<|end_filename|>
#include "RotationalForce.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void RotationalForce::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);

	physicsSystem.RenderRandomColors = true;
	physicsSystem.ImpulseIteration = 20;

	size_imgui_window = true;
	force = 5;
	allowForceAdd = false;

	step = true;
	allowed = false;

	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(12.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

void RotationalForce::ResetDemo() {
	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();


	bodies.clear();
	bodies.resize(5);

	bodies.resize(2);
	bodies[0].type = RIGIDBODY_TYPE_BOX;
	bodies[0].position = vec3(0, 10, 0);

	bodies[1].type = RIGIDBODY_TYPE_BOX;
	bodies[1].position = vec3(1.6, 2, 0);


	for (int i = 0; i < bodies.size(); ++i) {
		physicsSystem.AddRigidbody(&bodies[i]);
	}

	groundBox = Rigidbody(RIGIDBODY_TYPE_BOX);
	groundBox.box.size = vec3(15.0f, 0.15f, 15.0f);
	groundBox.mass = 0.0f;

	physicsSystem.AddRigidbody(&groundBox);
}

float RotationalForce::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

vec3 RotationalForce::Random(vec3 min, vec3 max) {
	vec3 result;
	result.x = Random(min.x, max.x);
	result.y = Random(min.y, max.y);
	result.z = Random(min.z, max.z);
	return result;
}

void RotationalForce::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 100));
	}

	ImGui::Begin("Conservation Demo", 0, ImGuiWindowFlags_NoResize);

	ImGui::Checkbox("Step", &step);
	ImGui::SameLine();
	if (ImGui::Button("Go")) {
		allowed = true;
	}
	ImGui::SameLine();
	ImGui::Checkbox("Allow force", &allowForceAdd);
	ImGui::SameLine();
	ImGui::DragFloat("Force", &force, 1.0f, 0.0f, 10.0f);


	if (ImGui::Button("Reset")) {
		ResetDemo();
	}

	ImGui::End();
}

void RotationalForce::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();

	glDisable(GL_LIGHTING);
	glColor3f(1.0f, 0.0f, 1.0f);
	::Render(ray);
	glEnable(GL_LIGHTING);
}

void RotationalForce::Update(float dt) {
	DemoBase::Update(dt);

	if (step) {
		if (allowed) {
			physicsSystem.Update(dt);
			allowed = false;
		}
	}
	else {
		physicsSystem.Update(dt);
		if (mouseLeftDown) {
			ray = GetPickRay(mousePos, vec2(), size, camera.GetViewMatrix(), camera.GetProjectionMatrix());
			ray.NormalizeDirection();
		}

		RaycastResult result;
		if (allowForceAdd && Raycast(bodies[0].box, ray, &result)) {
			bodies[0].AddRotationalImpulse(result.point, ray.direction * force);
			allowForceAdd = false;
		}
	}
}

<|start_filename|>Chapter02/Code/ConservationOfMomentum.h<|end_filename|>
#ifndef _H_CONSERVATION_OF_MOMENTUM_
#define _H_CONSERVATION_OF_MOMENTUM_

#include "DemoBase.h"
#include "Geometry3D.h"
#include "PhysicsSystem.h"
#include "RigidbodyVolume.h"

class ConservationOfMomentum : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	std::vector<RigidbodyVolume> bodies;
	RigidbodyVolume groundBox;

	bool size_imgui_window;
protected:
	void ResetDemo();
	float Random(float min, float max);
	vec3 Random(vec3 min, vec3 max);
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};


#endif


<|start_filename|>Chapter10/Physics/Geometry2D.h<|end_filename|>
#ifndef _H_GEOMETRY_2D
#define _H_GEOMETRY_2D

#include "vectors.h"

#ifndef NO_EXTRAS
#include <ostream>
#endif 

typedef vec2 Point2D;

typedef struct Line2D {
	Point2D start;
	Point2D end;

	inline Line2D() {}
	inline Line2D(const Point2D& s, const Point2D& e) :
		start(s), end(e) {}
} Line2D;

typedef struct Circle {
	Point2D position;
	float radius;

	inline Circle() : radius(1.0f) {}
	inline Circle(const Point2D& p, float r):
		position(p), radius(r) {}
} Circle;

typedef struct Rectangle2D {
	Point2D origin;
	vec2 size;

	inline Rectangle2D() : size(1, 1) { }
	inline Rectangle2D(const Point2D& o, const vec2& s) :
		origin(o), size(s) { }
} Rectangle2D;

typedef struct OrientedRectangle {
	Point2D position;
	vec2 halfExtents;
	float rotation;

	inline OrientedRectangle() :
		halfExtents(1.0f, 1.0f), rotation(0.0f) { }
	inline OrientedRectangle(const Point2D& pos, const vec2& ext) :
		position(pos), halfExtents(ext), rotation(0.0f) { }
	inline OrientedRectangle(const Point2D& pos, const vec2& ext, float rot) :
		position(pos), halfExtents(ext), rotation(rot) { }
} OrientedRectangle;

typedef struct BoundingShape {
	int numCircles;
	Circle* circles;
	int numRectangles;
	Rectangle2D* rectangles;

	inline BoundingShape() :
		numCircles(0), circles(0),
		numRectangles(0), rectangles(0) { }
};

typedef struct Interval2D {
	float min;
	float max;
} Interval2D;

float Length(const Line2D& line);
float LengthSq(const Line2D& line);
vec2 GetMin(const Rectangle2D& rect);
vec2 GetMax(const Rectangle2D& rect);
Rectangle2D FromMinMax(const vec2& min, const vec2& max);

bool PointOnLine(const Point2D& point, const Line2D& line);
bool PointInCircle(const Point2D& point, const Circle& circle);
bool PointInRectangle(const Point2D& point, const Rectangle2D& rectangle);
bool PointInOrientedRectangle(const Point2D& point, const OrientedRectangle& rectangle);

#ifndef NO_EXTRAS
#define PointCircle(point, circle) \
	PointInCircle(point, circle)
#define CirclePoint(circle, point) \
	PointInCircle(point, circle)
#define PointRectangle(point, rect) \
	PointInRectangle(point, rect)
#define RectanglePoint(rect, point) \
	PointInRectangle(point, rect)
#define PointOrientedRectangle(point, rect) \
	PointInOrientedRectangle(point, rect)
#define OrientedRectanglePoint(rect, point) \
	PointInOrientedRectangle(point, rect)
#endif

bool LineCircle(const Line2D& line, const Circle& circle);
bool LineRectangle(const Line2D& line, const Rectangle2D& rectangle);
bool LineOrientedRectangle(const Line2D& line, const OrientedRectangle& rectangle);

#define PointLine(point, line) PointOnLine(point, line)
#define LinePoint(line, point) PointOnLine(point, line)
#define CircleLine(circle, line) LineCircle(line, circle)
#define RectangleLine(rectangle, line) \
	LineRectangle(line, rectangle);
#define OrientedRectangleLine(rectangle, line) \
	LineOrientedRectangle(line, rectangle);

#ifndef NO_EXTRAS
std::ostream& operator<<(std::ostream& os, const Line2D& shape);
std::ostream& operator<<(std::ostream& os, const Circle& shape);
std::ostream& operator<<(std::ostream& os, const Rectangle2D& shape);
std::ostream& operator<<(std::ostream& os, const OrientedRectangle& shape);
#endif 

bool CircleCircle(const Circle& circle1, const Circle& circle2);
bool CircleRectangle(const Circle& circle, const Rectangle2D& rect);
#define RectangleCircle(rectangle, circle) \
	CircleRectangle(circle, rectangle)
bool CircleOrientedRectangle(const Circle& circle, const OrientedRectangle& rect);
#define OrientedRectangleCircle(rectangle, circle) \
	CircleOrientedRectangle(circle, rectangle)
bool RectangleRectangle(const Rectangle2D& rect1, const Rectangle2D& rect2);

Interval2D GetInterval(const Rectangle2D& rect, const vec2& axis);
bool OverlapOnAxis(const Rectangle2D& rect1, const Rectangle2D& rect2, const vec2& axis);
bool RectangleRectangleSAT(const Rectangle2D& rect1, const Rectangle2D& rect2);

Interval2D GetInterval(const OrientedRectangle& rect, const vec2& axis);
bool OverlapOnAxis(const Rectangle2D& rect1, const OrientedRectangle& rect2, const vec2& axis);
bool RectangleOrientedRectangle(const Rectangle2D& rect1, const OrientedRectangle& rect2);
#define OrientedRectangleRectangle(oriented, regular) \
	RectangleOrientedRectangle(regular, oriented)

bool OverlapOnAxis(const OrientedRectangle& rect1, const OrientedRectangle& rect2, const vec2& axis);
bool OrientedRectangleOrientedRectangleSAT(const OrientedRectangle& rect1, const OrientedRectangle& rect2);
bool OrientedRectangleOrientedRectangle(const OrientedRectangle& rect1, const OrientedRectangle& rect2);

Circle ContainingCircle(Point2D* pointArray, int arrayCount);
#ifndef NO_EXTRAS
Circle ContainingCircleAlt(Point2D* pointArray, int arrayCount);
#endif
Rectangle2D ContainingRectangle(Point2D* pointArray, int arrayCount);


bool PointInShape(const BoundingShape& shape, const Point2D& point);
#ifndef NO_EXTRAS
bool LineShape(const Line2D& line, const BoundingShape& shape);
bool CircleShape(const Circle& circle, const BoundingShape& shape);
bool RectangleShape(const Rectangle2D& rectangle, const BoundingShape& shape);
bool OrientedRectangleShape(const OrientedRectangle& rect, const BoundingShape& shape);

#define ShapeLine(shape, line) \
	LineShape(line, shape)
#define ShapeCircle(shape, circle) \
	CircleShape(circle, shape)
#define ShapeRectangle(shape, rectangle) \
	RectangleShape(rectangle, shape)
#define ShapeOrientedRectangle(shape, rect) \
	OrientedRectangleShape(rect, shape)
#endif

#endif

<|start_filename|>Chapter10/Physics/QuadTree.cpp<|end_filename|>
#include "QuadTree.h"
#include <queue>

int QuadTreeNode::maxDepth = 5;
int QuadTreeNode::maxObjectsPerNode = 15;

bool QuadTreeNode::IsLeaf() {
	return children.size() == 0;
}

int QuadTreeNode::NumObjects() {
	Reset();

	int objectCount = contents.size();
	for (int i = 0, size = contents.size(); i < size; ++i) {
		contents[i]->flag = true;
	}

	std::queue<QuadTreeNode*> process;
	process.push(this);

	while (process.size() > 0) {
		QuadTreeNode* processing = process.back();

		if (!processing->IsLeaf()) {
			for (int i = 0, size = processing->children.size(); i < size; ++i) {
				process.push(&processing->children[i]);
			}
		}
		else {
			for (int i = 0, size = processing->contents.size(); i < size; ++i) {
				if (!processing->contents[i]->flag) {
					objectCount += 1;
					processing->contents[i]->flag = true;
				}
			}
		}

		process.pop();
	}

	Reset();
	return objectCount;
}

void QuadTreeNode::Insert(QuadTreeData& data) {
	if (!RectangleRectangle(data.bounds, nodeBounds)) {
		return; // The object does not fit into this node
	}

	if (IsLeaf() && contents.size() + 1 > maxObjectsPerNode) {
		Split(); // Try splitting!
	}

	if (IsLeaf()) {
		contents.push_back(&data);
	}
	else {
		for (int i = 0, size = children.size(); i < size; ++i) {
			children[i].Insert(data);
		}
	}
}

void QuadTreeNode::Remove(QuadTreeData& data) {
	if (IsLeaf()) {
		int removeIndex = -1;
		for (int i = 0, size = contents.size(); i < size; ++i) {
			if (contents[i]->object == data.object) {
				removeIndex = i;
				break;
			}
		}

		if (removeIndex != -1) {
			contents.erase(contents.begin() + 1);
		}
	}
	else {
		for (int i = 0, size = children.size(); i < size; ++i) {
			children[i].Remove(data);
		}
	}

	Shake();
}

void QuadTreeNode::Update(QuadTreeData& data) {
	Remove(data);
	Insert(data);
}

void QuadTreeNode::Shake() {
	// Cant shake a leaf
	if (!IsLeaf()) {
		int numObjects = NumObjects();
		// None of the children contain any objects, collapse
		if (numObjects == 0) {
			children.clear();
		}
		// Children combined have less objects than maximum for the node, collapse!
		else if (numObjects < maxObjectsPerNode) {
			// Loop trough down to leaf nodes, non-recursivley
			std::queue<QuadTreeNode*> process;
			process.push(this);

			while (process.size() > 0) {
				QuadTreeNode* processing = process.back();

				if (!processing->IsLeaf()) {
					// Not a leaf node, add children to list to process!
					for (int i = 0, size = processing->children.size(); i < size; ++i) {
						process.push(&processing->children[i]);
					}
				}
				else {
					// Was a leaf node, add all children to our list!
					contents.insert(contents.end(), processing->contents.begin(), processing->contents.end());
				}

				process.pop();
			}

			children.clear();
		}
	}
}

void QuadTreeNode::Reset() {
	if (IsLeaf()) {
		for (int i = 0, size = contents.size(); i < size; ++i) {
			contents[i]->flag = false;
		}
	}
	else {
		for (int i = 0, size = children.size(); i < size; ++i) {
			children[i].Reset();
		}
	}
}

void QuadTreeNode::Split() {
	if (currentDepth + 1 >= maxDepth) {
		return;
	}

	vec2 min = GetMin(nodeBounds);
	vec2 max = GetMax(nodeBounds);
	vec2 center = min + ((max - min) * 0.5f);

	Rectangle2D childAreas[] = {
		Rectangle2D(FromMinMax(vec2(min.x, min.y), vec2(center.x, center.y))),
		Rectangle2D(FromMinMax(vec2(center.x, min.y), vec2(max.x, center.y))),
		Rectangle2D(FromMinMax(vec2(center.x, center.y), vec2(max.x, max.y))),
		Rectangle2D(FromMinMax(vec2(min.x, center.y), vec2(center.x, max.y))),
	};
	for (int i = 0; i < 4; ++i) {
		children.push_back(QuadTreeNode(childAreas[i]));
		children[i].currentDepth = currentDepth + 1;
	}

	for (int i = 0, size = contents.size(); i < size; ++i) {
		children[i].Insert(*contents[i]);
	}

	contents.clear();
}

std::vector<QuadTreeData*> QuadTreeNode::Query(const Rectangle2D& area) {
	std::vector<QuadTreeData*> result;
	if (!RectangleRectangle(area, nodeBounds)) {
		return result;
	}

	if (IsLeaf()) {
		for (int i = 0, size = contents.size(); i < size; ++i) {
			if (RectangleRectangle(contents[i]->bounds, area)) {
				result.push_back(contents[i]);
			}
		}
	}
	else {
		for (int i = 0, size = children.size(); i < size; ++i) {
			std::vector<QuadTreeData*> recurse = children[i].Query(area);
			if (recurse.size() > 0) {
				result.insert(result.end(), recurse.begin(), recurse.end());
			}
		}
	}
	return result;
}

<|start_filename|>Chapter02/Code/CollisionFeature.cpp<|end_filename|>
#include "CollisionFeature.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include "imgui/ImGuizmo.h"
#include <iostream>

void CollisionFeature::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);
	size_imgui_window = true;

	manipulator = manTranslation.asArray;
	manipulating = -1;
	transformWorld = false;

	obb[0].position = vec3(4, 2, 0);
	obb[1].position = vec3(-4, -2, 0);
	obb[1].orientation = Rotation3x3(30.0f, 20.0f, 0.0f);

	sphere[0].position = vec3(-4, 2, 0);
	sphere[1].position = vec3(4, -2, 0);
	sphere[1].radius = 0.5f;

	
	glPointSize(5.0f);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);
}

void CollisionFeature::ImGUI() {
	DemoBase::ImGUI();

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 75));
	}

	ImGui::Begin("SAT Test Demo", 0, ImGuiWindowFlags_NoResize);
	if (ImGui::RadioButton("Translate", manipulator == manTranslation.asArray)) {
		manipulator = manTranslation.asArray;
	}
	ImGui::SameLine();
	if (ImGui::RadioButton("Rotate", manipulator == manRotation.asArray)) {
		manipulator = manRotation.asArray;
	}
	ImGui::SameLine();
	if (ImGui::RadioButton("Scale", manipulator == manScale.asArray)) {
		manipulator = manScale.asArray;
	}

	if (ImGui::RadioButton("Local", transformWorld == false)) {
		transformWorld = false;
	}
	ImGui::SameLine();
	if (ImGui::RadioButton("World", transformWorld == true)) {
		transformWorld = true;
	}
	ImGui::End();

	if (manipulating != -1) {
		ImGuizmo::MODE currentGizmoMode = transformWorld ? ImGuizmo::WORLD : ImGuizmo::LOCAL;
		ImGuizmo::OPERATION currentGizmoOperation = ImGuizmo::ROTATE;
		
		if (manipulator == manTranslation.asArray) {
			currentGizmoOperation = ImGuizmo::TRANSLATE;
			ImGuizmo::Enable(true);
		}
		if (manipulator == manRotation.asArray) {
			currentGizmoOperation = ImGuizmo::ROTATE;
			ImGuizmo::Enable(true);// manipulating == 0 || manipulating == 1);
		}
		if (manipulator == manScale.asArray) {
			currentGizmoOperation = ImGuizmo::SCALE;
			ImGuizmo::Enable(true);
		}

		ImGuizmo::Manipulate(
			camera.GetViewMatrix().asArray,
			camera.GetProjectionMatrix().asArray, 
			currentGizmoOperation, currentGizmoMode, manipulator);
		
		float matrixTranslation[3], matrixRotation[3], matrixScale[3];
		ImGuizmo::DecomposeMatrixToComponents(manipulator, matrixTranslation, matrixRotation, matrixScale);

		if (manipulating == 0 || manipulating == 1) {
			if (manipulator == manTranslation.asArray) {
				obb[manipulating].position.x = matrixTranslation[0];
				obb[manipulating].position.y = matrixTranslation[1];
				obb[manipulating].position.z = matrixTranslation[2];
			}
			else if (manipulator == manScale.asArray) {
				obb[manipulating].size.x = matrixScale[0];
				obb[manipulating].size.y = matrixScale[1];
				obb[manipulating].size.z = matrixScale[2];
			}
			else if (manipulator == manRotation.asArray) {
				obb[manipulating].orientation = Cut(manRotation, 3, 3);
			}
		}
		else if (manipulating == 2 || manipulating == 3) {
			if (manipulator == manTranslation.asArray) {
				sphere[manipulating - 2].position.x = matrixTranslation[0];
				sphere[manipulating - 2].position.y = matrixTranslation[1];
				sphere[manipulating - 2].position.z = matrixTranslation[2];
			}
			else if (manipulator == manScale.asArray) {
				float diff0 = fabsf(sphere[manipulating - 2].radius - matrixScale[0]);
				float diff1 = fabsf(sphere[manipulating - 2].radius - matrixScale[1]);
				float diff2 = fabsf(sphere[manipulating - 2].radius - matrixScale[2]);
				
				if (diff0 > diff1 && diff0 > diff2) {
					sphere[manipulating - 2].radius = matrixScale[0];
				}
				else if (diff1 > diff0 && diff1 > diff2) {
					sphere[manipulating - 2].radius = matrixScale[1];
				}
				else if (diff2 > diff0 && diff2 > diff1) {
					sphere[manipulating - 2].radius = matrixScale[2];
				}
			}
		}

	}
}

void CollisionFeature::Render() {
	DemoBase::Render();

	static const float position[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };
	static const float ambient[] = { 0.0f, 0.0f, 0.5f, 0.0f };
	static const float diffuse[] = { 0.0f, 0.0f, 0.5f, 0.0f };

	glLightfv(GL_LIGHT0, GL_POSITION, position);
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);

	CollisionManifold r1 = FindCollisionFeatures(obb[0], obb[1]);
	CollisionManifold r2 = FindCollisionFeatures(obb[0], sphere[0]);
	CollisionManifold r3 = FindCollisionFeatures(obb[1], sphere[0]);
	CollisionManifold r4 = FindCollisionFeatures(sphere[0], sphere[1]);
	CollisionManifold r5 = FindCollisionFeatures(obb[0], sphere[1]);
	CollisionManifold r6 = FindCollisionFeatures(obb[1], sphere[1]);

	if (!r1.colliding && !r2.colliding && !r5.colliding) {
		::Render(obb[0]);
	}
	if (!r1.colliding && !r3.colliding && !r6.colliding) {
		::Render(obb[1]);
	}
	if (!r2.colliding && !r3.colliding && !r4.colliding) {
		::Render(sphere[0]);
	}
	if (!r4.colliding && !r5.colliding && !r6.colliding) {
		::Render(sphere[1]);
	}

	glDisable(GL_LIGHTING);
	glColor3f(0.0f, 0.0f, 1.0f);
	if (r1.colliding || r2.colliding || r5.colliding) {
		::Render(GetEdges(obb[0]));
	}
	if (r1.colliding || r3.colliding || r6.colliding) {
		::Render(GetEdges(obb[1]));
	}
	if (r2.colliding || r3.colliding || r4.colliding) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		::Render(sphere[0]);
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}
	if (r4.colliding || r5.colliding || r6.colliding) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		::Render(sphere[1]);
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}

	/*glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	Sphere s1(obb[0].position, Magnitude(obb[0].size));
	::Render(s1);
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);*/

	glColor3f(1.0f, 0.0f, 0.0f);
	if (r1.colliding) {
		for (int i = 0; i < r1.contacts.size(); ++i) {
			::Render(r1.contacts[i]);
		}
	}
	if (r2.colliding) {
		for (int i = 0; i < r2.contacts.size(); ++i) {
			::Render(r2.contacts[i]);
		}
	}
	if (r3.colliding) {
		for (int i = 0; i < r3.contacts.size(); ++i) {
			::Render(r3.contacts[i]);
		}
	}
	if (r4.colliding) {
		for (int i = 0; i < r4.contacts.size(); ++i) {
			::Render(r4.contacts[i]);
		}
	}
	glEnable(GL_LIGHTING);
}

void CollisionFeature::Update(float dt) {
	DemoBase::Update(dt);

	if (mouseLeftDown) {
		Ray screenRay = GetPickRay(mousePos, vec2(), size, camera.GetViewMatrix(), camera.GetProjectionMatrix());
		screenRay.NormalizeDirection();

		std::vector<RaycastResult> allCasts;
		allCasts.resize(4);
		Raycast(obb[0], screenRay, &allCasts[0]);
		Raycast(obb[1], screenRay, &allCasts[1]);
		Raycast(sphere[0], screenRay, &allCasts[2]);
		Raycast(sphere[1], screenRay, &allCasts[3]);

		RaycastResult raycastResult;
		ResetRaycastResult(&raycastResult);
		int wasManipulating = manipulating;

		for (int i = 0; i < allCasts.size(); ++i) {
			if (allCasts[i].hit) {
				if (!raycastResult.hit || allCasts[i].t < raycastResult.t) {
					raycastResult = allCasts[i];
					manipulating = i;
				}
			}
		}

		bool trans = manipulator == manTranslation.asArray;
		bool rot = manipulator == manRotation.asArray;
		bool scal = manipulator == manScale.asArray;

		if (!raycastResult.hit) {
			manipulating = -1;
		}
		else if (manipulating < 2 && wasManipulating != manipulating) {
			manTranslation = Translation(obb[manipulating].position.x, obb[manipulating].position.y, obb[manipulating].position.z);
			manRotation = FromMat3(obb[manipulating].orientation) * manTranslation;
			manTranslation = manRotation;
			manScale = Scale(obb[manipulating].size.x, obb[manipulating].size.y, obb[manipulating].size.z) * manTranslation;
		}
		else if (manipulating >= 2 && manipulating < 4 && wasManipulating != manipulating) {
			manTranslation = Translation(sphere[manipulating - 2].position.x, sphere[manipulating - 2].position.y, sphere[manipulating - 2].position.z);
			manRotation = manTranslation;
			manScale = Scale(sphere[manipulating - 2].radius, sphere[manipulating - 2].radius, sphere[manipulating - 2].radius) * manTranslation;
		}

		if (trans) {
			manipulator = manTranslation.asArray;
		}
		else if (rot) {
			manipulator = manRotation.asArray;
		}
		else if (scal) {
			manipulator = manScale.asArray;
		}
	}
}

<|start_filename|>Chapter02/Code/CollisionFeature.h<|end_filename|>
#ifndef _H_GENERIC_SAT_DEMO_
#define _H_GENERIC_SAT_DEMO_

#include "DemoBase.h"
#include "Geometry3D.h"

class CollisionFeature : public DemoBase {
protected:
	Sphere sphere[2];
	OBB obb[2];

	float* manipulator;
	mat4 manTranslation;
	mat4 manRotation;
	mat4 manScale;
	int manipulating;
	bool transformWorld;
	bool size_imgui_window;
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};

#endif

<|start_filename|>Chapter15/Physics/matrices.cpp<|end_filename|>
#include "matrices.h"
#include <cmath>
#include <cfloat>
#define SANITY_TEST_MAT4_INVERSE

#ifdef SANITY_TEST_MAT4_INVERSE
#include <iostream>
#endif 


// http://realtimecollisiondetection.net/pubs/Tolerances/
#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

#ifndef NO_EXTRAS
bool operator==(const mat2& l, const mat2& r) {
	for (int i = 0; i < /* 2 * 2 = */4; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}

bool operator==(const mat3& l, const mat3& r) {
	for (int i = 0; i < /* 3 * 3 = */ 9; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}
bool operator==(const mat4& l, const mat4& r) {
	for (int i = 0; i < /* 4 * 4 = */ 16; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}

bool operator!=(const mat2& l, const mat2& r) {
	return !(l == r);
}

bool operator!=(const mat3& l, const mat3& r) {
	return !(l == r);
}

bool operator!=(const mat4& l, const mat4& r) {
	return !(l == r);
}
#endif 

void Transpose(const float *srcMat, float *dstMat, int srcRows, int srcCols) {
	for (int i = 0; i < srcRows * srcCols; i++) {
		int row = i / srcRows;
		int col = i % srcRows;
		dstMat[i] = srcMat[srcCols * col + row];
	}
}

mat2 Transpose(const mat2& matrix) {
	mat2 result;
	Transpose(matrix.asArray, result.asArray, 2, 2);
	return result;
}

mat3 Transpose(const mat3& matrix) {
	mat3 result;
	Transpose(matrix.asArray, result.asArray, 3, 3);
	return result;
}

mat4 Transpose(const mat4& matrix) {
	mat4 result;
	Transpose(matrix.asArray, result.asArray, 4, 4);
	return result;
}

mat2 operator*(const mat2& matrix, float scalar) {
	mat2 result;
	for (int i = 0; i < 4; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

mat3 operator*(const mat3& matrix, float scalar) {
	mat3 result;
	for (int i = 0; i < 9; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

mat4 operator*(const mat4& matrix, float scalar) {
	mat4 result;
	for (int i = 0; i < 16; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

bool Multiply(float* out, const float* matA, int aRows, int aCols, const float* matB, int bRows, int bCols) {
	if (aCols != bRows) {
		return false;
	}

	for (int i = 0; i < aRows; ++i) {
		for (int j = 0; j < bCols; ++j) {
			out[bCols * i + j] = 0.0f;
			for (int k = 0; k < bRows; ++k) {
				out[bCols * i + j] += matA[aCols * i + k] * matB[bCols * k + j];
			}
		}
	}

	return true;
}

mat2 operator*(const mat2& matrixA, const mat2& matrixB) {
	mat2 result;
	Multiply(result.asArray, matrixA.asArray, 2, 2, matrixB.asArray, 2, 2);
	return result;
}

mat3 operator*(const mat3& matrixA, const mat3& matrixB) {
	mat3 result;
	Multiply(result.asArray, matrixA.asArray, 3, 3, matrixB.asArray, 3, 3);
	return result;
}

mat4 operator*(const mat4& matrixA, const mat4& matrixB) {
	mat4 result;
	Multiply(result.asArray, matrixA.asArray, 4, 4, matrixB.asArray, 4, 4);
	return result;
}

float Determinant(const mat2& matrix) {
	return matrix._11 * matrix._22 - matrix._12 * matrix._21;
}

mat2 Cut(const mat3& mat, int row, int col) {
	mat2 result;
	int index = 0;

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			if (i == row || j == col) {
				continue;
			}
			result.asArray[index++] = mat.asArray[3 * i + j];
		}
	}

	return result;
}

mat3 Cut(const mat4& mat, int row, int col) {
	mat3 result;
	int index = 0;

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			if (i == row || j == col) {
				continue;
			}
			result.asArray[index++] = mat.asArray[4 * i + j];
		}
	}

	return result;
}

mat3 Minor(const mat3& mat) {
	mat3 result;

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			result[i][j] = Determinant(Cut(mat, i, j));
		}
	}
	
	return result;
}

mat2 Minor(const mat2& mat) {
	return mat2(
		mat._22, mat._21,
		mat._12, mat._11
	);
}

void Cofactor(float* out, const float* minor, int rows, int cols) {
	for (int i = 0; i < rows; ++i) {
		for (int j = 0; j < cols; ++j) {
			out[cols * j + i] = minor[cols * j + i] * powf(-1.0f, i + j);
		}
	}
}

mat2 Cofactor(const mat2& mat) {
	mat2 result;
	Cofactor(result.asArray, Minor(mat).asArray, 2, 2);
	return result;
}

mat3 Cofactor(const mat3& mat) {
	mat3 result;
	Cofactor(result.asArray, Minor(mat).asArray, 3, 3);
	return result;
}

float Determinant(const mat3& mat) {
	float result = 0.0f;

	/*float A = mat.asArray[3 * 0 + 0] * Determinant(Cut(mat, 0, 0));
	float B = mat.asArray[3 * 0 + 1] * Determinant(Cut(mat, 0, 1));
	float C = mat.asArray[3 * 0 + 2] * Determinant(Cut(mat, 0, 2));
	result = A - B + C;*/

	/*for (int j = 0; j < 3; ++j) {
		result += mat.asArray[3 * 0 + j] * Determinant(Cut(mat, 0, j)) * powf(-1, 0 + j);
	}*/

	mat3 cofactor = Cofactor(mat);
	for (int j = 0; j < 3; ++j) {
		result += mat.asArray[3 * 0 + j] * cofactor[0][j];
	}

	return result;
}

mat4 Minor(const mat4& mat) {
	mat4 result;

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			result[i][j] = Determinant(Cut(mat, i, j));
		}
	}

	return result;
}

mat4 Cofactor(const mat4& mat) {
	mat4 result;
	Cofactor(result.asArray, Minor(mat).asArray, 4, 4);
	return result;
}

float Determinant(const mat4& mat) {
	float result = 0.0f;

	mat4 cofactor = Cofactor(mat);
	for (int j = 0; j < 4; ++j) {
		result += mat.asArray[4 * 0 + j] * cofactor[0][j];
	}

	return result;
}

mat2 Adjugate(const mat2& mat) {
	return Transpose(Cofactor(mat));
}

mat3 Adjugate(const mat3& mat) {
	return Transpose(Cofactor(mat));
}

mat4 Adjugate(const mat4& mat) {
	return Transpose(Cofactor(mat));
}

mat2 Inverse(const mat2& mat) {
	float det = Determinant(mat);
	if (CMP(det, 0.0f)) { return mat2(); }
	return Adjugate(mat) * (1.0f / det);

	/*float det = mat._11 * mat._22 - mat._12 * mat._21;
	if (CMP(det, 0.0f)) { 
		return mat2(); 
	}
	float i_det = 1.0f / det;
	mat2 result;
	result._11 =  mat._22 * i_det;
	result._12 = -mat._12 * i_det;
	result._21 = -mat._21 * i_det;
	result._22 =  mat._11 * i_det;
	return result;*/
}

mat3 Inverse(const mat3& mat) {
	float det = Determinant(mat);
	if (CMP(det, 0.0f)) { return mat3(); }
	return Adjugate(mat) * (1.0f / det);
}

mat4 Inverse(const mat4& m) {
	/*float det = Determinant(m);
	if (CMP(det, 0.0f)) { return mat4(); }
	return Adjugate(m) * (1.0f / det);*/

	// The code below is the expanded form of the above equation.
	// This optimization avoids loops and function calls

	float det 
		= m._11 * m._22 * m._33 * m._44 + m._11 * m._23 * m._34 * m._42 + m._11 * m._24 * m._32 * m._43
		+ m._12 * m._21 * m._34 * m._43 + m._12 * m._23 * m._31 * m._44 + m._12 * m._24 * m._33 * m._41
		+ m._13 * m._21 * m._32 * m._44 + m._13 * m._22 * m._34 * m._41 + m._13 * m._24 * m._31 * m._42
		+ m._14 * m._21 * m._33 * m._42 + m._14 * m._22 * m._31 * m._43 + m._14 * m._23 * m._32 * m._41
		- m._11 * m._22 * m._34 * m._43 - m._11 * m._23 * m._32 * m._44 - m._11 * m._24 * m._33 * m._42
		- m._12 * m._21 * m._33 * m._44 - m._12 * m._23 * m._34 * m._41 - m._12 * m._24 * m._31 * m._43
		- m._13 * m._21 * m._34 * m._42 - m._13 * m._22 * m._31 * m._44 - m._13 * m._24 * m._32 * m._41
		- m._14 * m._21 * m._32 * m._43 - m._14 * m._22 * m._33 * m._41 - m._14 * m._23 * m._31 * m._42;

	if (CMP(det, 0.0f)) { 
		return mat4(); 
	}
	float i_det = 1.0f / det;

	mat4 result;
	result._11 = (m._22 * m._33 * m._44 + m._23 * m._34 * m._42 + m._24 * m._32 * m._43 - m._22 * m._34 * m._43 - m._23 * m._32 * m._44 - m._24 * m._33 * m._42) * i_det;
	result._12 = (m._12 * m._34 * m._43 + m._13 * m._32 * m._44 + m._14 * m._33 * m._42 - m._12 * m._33 * m._44 - m._13 * m._34 * m._42 - m._14 * m._32 * m._43) * i_det;
	result._13 = (m._12 * m._23 * m._44 + m._13 * m._24 * m._42 + m._14 * m._22 * m._43 - m._12 * m._24 * m._43 - m._13 * m._22 * m._44 - m._14 * m._23 * m._42) * i_det;
	result._14 = (m._12 * m._24 * m._33 + m._13 * m._22 * m._34 + m._14 * m._23 * m._32 - m._12 * m._23 * m._34 - m._13 * m._24 * m._32 - m._14 * m._22 * m._33) * i_det;
	result._21 = (m._21 * m._34 * m._43 + m._23 * m._31 * m._44 + m._24 * m._33 * m._41 - m._21 * m._33 * m._44 - m._23 * m._34 * m._41 - m._24 * m._31 * m._43) * i_det;
	result._22 = (m._11 * m._33 * m._44 + m._13 * m._34 * m._41 + m._14 * m._31 * m._43 - m._11 * m._34 * m._43 - m._13 * m._31 * m._44 - m._14 * m._33 * m._41) * i_det;
	result._23 = (m._11 * m._24 * m._43 + m._13 * m._21 * m._44 + m._14 * m._23 * m._41 - m._11 * m._23 * m._44 - m._13 * m._24 * m._41 - m._14 * m._21 * m._43) * i_det;
	result._24 = (m._11 * m._23 * m._34 + m._13 * m._24 * m._31 + m._14 * m._21 * m._33 - m._11 * m._24 * m._33 - m._13 * m._21 * m._34 - m._14 * m._23 * m._31) * i_det;
	result._31 = (m._21 * m._32 * m._44 + m._22 * m._34 * m._41 + m._24 * m._31 * m._42 - m._21 * m._34 * m._42 - m._22 * m._31 * m._44 - m._24 * m._32 * m._41) * i_det;
	result._32 = (m._11 * m._34 * m._42 + m._12 * m._31 * m._44 + m._14 * m._32 * m._41 - m._11 * m._32 * m._44 - m._12 * m._34 * m._41 - m._14 * m._31 * m._42) * i_det;
	result._33 = (m._11 * m._22 * m._44 + m._12 * m._24 * m._41 + m._14 * m._21 * m._42 - m._11 * m._24 * m._42 - m._12 * m._21 * m._44 - m._14 * m._22 * m._41) * i_det;
	result._34 = (m._11 * m._24 * m._32 + m._12 * m._21 * m._34 + m._14 * m._22 * m._31 - m._11 * m._22 * m._34 - m._12 * m._24 * m._31 - m._14 * m._21 * m._32) * i_det;
	result._41 = (m._21 * m._33 * m._42 + m._22 * m._31 * m._43 + m._23 * m._32 * m._41 - m._21 * m._32 * m._43 - m._22 * m._33 * m._41 - m._23 * m._31 * m._42) * i_det;
	result._42 = (m._11 * m._32 * m._43 + m._12 * m._33 * m._41 + m._13 * m._31 * m._42 - m._11 * m._33 * m._42 - m._12 * m._31 * m._43 - m._13 * m._32 * m._41) * i_det;
	result._43 = (m._11 * m._23 * m._42 + m._12 * m._21 * m._43 + m._13 * m._22 * m._41 - m._11 * m._22 * m._43 - m._12 * m._23 * m._41 - m._13 * m._21 * m._42) * i_det;
	result._44 = (m._11 * m._22 * m._33 + m._12 * m._23 * m._31 + m._13 * m._21 * m._32 - m._11 * m._23 * m._32 - m._12 * m._21 * m._33 - m._13 * m._22 * m._31) * i_det;

#ifdef  SANITY_TEST_MAT4_INVERSE
	if (result * m != mat4()) {
		std::cout << "ERROR! Expecting matrix x inverse to equal identity!\n";
	}
#endif


	return result;
}

<|start_filename|>Chapter04/Physics/QuadTree.h<|end_filename|>
#ifndef _H_QUAD_TREE_
#define _H_QUAD_TREE_

#include "Geometry2D.h"
#include <vector>

using std::vector;

struct QuadTreeData {
	// I don't know what you want to store
	// figure it out, change the void* to store that
	void* object;
	Rectangle2D bounds;
	bool flag;

	inline QuadTreeData(void* o, const Rectangle2D& b) :
		object(o), bounds(b), flag(false) { }
};

class QuadTreeNode {
protected:
	std::vector<QuadTreeNode> children;
	vector<QuadTreeData*> contents;
	int currentDepth;
	static int maxDepth;
	static int maxObjectsPerNode;
	Rectangle2D nodeBounds;
public:
	inline QuadTreeNode(const Rectangle2D& bounds) :
		nodeBounds(bounds), currentDepth(0) { }

	bool IsLeaf();

	int NumObjects();

	void Insert(QuadTreeData& data);
	void Remove(QuadTreeData& data);
	void Update(QuadTreeData& data);

	void Shake();
	void Split();
	void Reset();
	std::vector<QuadTreeData*> Query(const Rectangle2D& area);
};

typedef QuadTreeNode QuadTree;

#endif

<|start_filename|>Chapter01/Code/Rigidbody.h<|end_filename|>
#ifndef _H_RIGIDBODY_
#define _H_RIGIDBODY_

#include <vector>
#include "Geometry3D.h"

#define RIGIDBODY_TYPE_BASE		0
#define RIGIDBODY_TYPE_PARTICLE	1
#define RIGIDBODY_TYPE_SPHERE	2
#define RIGIDBODY_TYPE_BOX		3

class Rigidbody {
public:
	int type;
public:
	inline Rigidbody() {
		type = RIGIDBODY_TYPE_BASE;
	}
	virtual inline ~Rigidbody() { }

	virtual inline void Update(float deltaTime) { }
	virtual inline void Render() { }
	virtual inline void ApplyForces() { }
	virtual inline void SolveConstraints(const std::vector<OBB>& constraints) { }

	inline bool HasVolume() {
		return type == RIGIDBODY_TYPE_SPHERE || type == RIGIDBODY_TYPE_BOX;
	}
};

#endif

<|start_filename|>Chapter01/Code/PhysicsSystem.cpp<|end_filename|>
#include "PhysicsSystem.h"
#include "RigidbodyVolume.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include <iostream>

PhysicsSystem::PhysicsSystem() {
	LinearProjectionPercent = 0.45f;
	PenetrationSlack = 0.01f;
	ImpulseIteration = 5;

	DebugRender = false;
	DoLinearProjection = true;
	RenderRandomColors = false;

	colliders1.reserve(100);
	colliders2.reserve(100);
	results.reserve(100);
}

void PhysicsSystem::Update(float deltaTime) {
	colliders1.clear();
	colliders2.clear();
	results.clear();

	{ // Find objects whom are colliding
	  // First, build a list of colliding objects
		CollisionManifold result;
		for (int i = 0, size = bodies.size(); i < size; ++i) {
			for (int j = i; j < size; ++j) {
				if (i == j) {
					continue;
				}
				ResetCollisionManifold(&result);
				if (bodies[i]->HasVolume() && bodies[j]->HasVolume()) {
					RigidbodyVolume* m1 = (RigidbodyVolume*)bodies[i];
					RigidbodyVolume* m2 = (RigidbodyVolume*)bodies[j];
					result = FindCollisionFeatures(*m1, *m2);
				}
				if (result.colliding) {
#if 0 
					bool isDuplicate = false;
					for (int k = 0, kSize = colliders1.size(); k < kSize; ++k) {
						if (colliders1[k] == bodies[i] || colliders1[k] == bodies[j]) {
							if (colliders2[k] == bodies[i] || colliders2[k] == bodies[j]) {
								isDuplicate = true;
								break;
							}
						}
					}

					if (!isDuplicate) {
						for (int k = 0, kSize = colliders2.size(); k < kSize; ++k) {
							if (colliders2[k] == bodies[i] || colliders2[k] == bodies[j]) {
								if (colliders1[k] == bodies[i] || colliders1[k] == bodies[j]) {
									isDuplicate = true;
									break;
								}
							}
						}
					}
					if (!isDuplicate)
#endif

					{
						colliders1.push_back(bodies[i]);
						colliders2.push_back(bodies[j]);
						results.push_back(result);
					}
				}
			}
		}
	}

	// Calculate foces acting on the object
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->ApplyForces();
	}

	// Same as above, calculate forces acting on cloths
	for (int i = 0, size = cloths.size(); i < size; ++i) {
		cloths[i]->ApplyForces();
	}

	// Apply impulses to resolve collisions
	for (int k = 0; k < ImpulseIteration; ++k) { // Apply impulses
		for (int i = 0, size = results.size(); i < size; ++i) {
			for (int j = 0, jSize = results[i].contacts.size(); j < jSize; ++j) {
				if (colliders1[i]->HasVolume() && colliders2[i]->HasVolume()) {
					RigidbodyVolume* m1 = (RigidbodyVolume*)colliders1[i];
					RigidbodyVolume* m2 = (RigidbodyVolume*)colliders2[i];
					ApplyImpulse(*m1, *m2, results[i], j);
				}
			}
		}
	}

	// Integrate velocity and impulse of objects
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->Update(deltaTime);
	}

	// Same as above, integrate velocity and impulse of cloths
	for (int i = 0, size = cloths.size(); i < size; ++i) {
		cloths[i]->Update(deltaTime);
	}

	// Correct position to avoid sinking!
	if (DoLinearProjection) {
		for (int i = 0, size = results.size(); i < size; ++i) {
			if (!colliders1[i]->HasVolume() && !colliders2[i]->HasVolume()) {
				continue;
			}

			RigidbodyVolume* m1 = (RigidbodyVolume*)colliders1[i];
			RigidbodyVolume* m2 = (RigidbodyVolume*)colliders2[i];
			float totalMass = m1->InvMass() + m2->InvMass();

			if (totalMass == 0.0f) {
				continue;
			}

			float depth = fmaxf(results[i].depth - PenetrationSlack, 0.0f);
			float scalar = (totalMass == 0.0f) ? 0.0f : depth / totalMass;
			vec3 correction = results[i].normal * scalar * LinearProjectionPercent;

			m1->position = m1->position - correction * m1->InvMass();
			m2->position = m2->position + correction * m2->InvMass();

			m1->SynchCollisionVolumes();
			m2->SynchCollisionVolumes();
		}
	}

	// Apply spring forces
	for (int i = 0, size = springs.size(); i < size; ++i) {
		springs[i].ApplyForce(deltaTime);
	}

	// Same as above, apply spring forces for cloths
	for (int i = 0, size = cloths.size(); i < size; ++i) {
		cloths[i]->ApplySpringForces(deltaTime);
	}

	// Solve constraints
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->SolveConstraints(constraints);
	}

	// Same as above, solve cloth constraints
	for (int i = 0, size = cloths.size(); i < size; ++i) {
		cloths[i]->SolveConstraints(constraints);
	}
}

void PhysicsSystem::Render() {
	if (DebugRender) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	}
	static const float rigidbodyDiffuse[]{ 200.0f / 255.0f, 0.0f, 0.0f, 0.0f };
	static const float rigidbodyAmbient[]{ 200.0f / 255.0f, 50.0f / 255.0f, 50.0f / 255.0f, 0.0f };

	static const float groundDiffuse[]{ 0.0f, 0.0f, 200.0f / 255.0f, 0.0f };
	static const float groundAmbient[]{ 50.0f / 255.0f, 50.0f / 255.0f, 200.0f / 255.0f, 0.0f };

	static const float constraintDiffuse[]{ 0.0f, 200.0f / 255.0f, 0.0f, 0.0f };
	static const float constraintAmbient[]{ 50.0f / 255.0f, 200.0f / 255.0f, 50.0f / 255.0f, 0.0f };
	
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };

	std::vector<const float*> ambient;
	std::vector<const float*> diffuse;
	if (RenderRandomColors) {
		ambient.push_back(rigidbodyAmbient);
		ambient.push_back(groundAmbient);
		ambient.push_back(constraintAmbient);
		diffuse.push_back(rigidbodyDiffuse);
		diffuse.push_back(groundDiffuse);
		diffuse.push_back(constraintDiffuse);
	}

	glColor3f(rigidbodyDiffuse[0], rigidbodyDiffuse[1], rigidbodyDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, rigidbodyAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, rigidbodyDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		if (RenderRandomColors) {
			int a_i = i % ambient.size();
			int d_i = i % diffuse.size();
			glColor3f(diffuse[d_i][0], diffuse[d_i][1], diffuse[d_i][2]);
			glLightfv(GL_LIGHT0, GL_AMBIENT, ambient[a_i]);
			glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse[d_i]);
			glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
		}
		if (DebugRender && bodies[i]->type == RIGIDBODY_TYPE_BOX) {
			RigidbodyVolume* mb = (RigidbodyVolume*)bodies[i];
			mb->SynchCollisionVolumes();
			::Render(GetEdges(mb->box));
		}
		else {
			bodies[i]->Render();
		}
	}

	// First constraint is usually the ground
	// Rendering it diferent color is just a hack to make visualization easyer
	// Normally, you wouldn't even render physics geo!
	if (constraints.size() > 0) {
		glColor3f(groundDiffuse[0], groundDiffuse[1], groundDiffuse[2]);
		glLightfv(GL_LIGHT0, GL_AMBIENT, groundAmbient);
		glLightfv(GL_LIGHT0, GL_DIFFUSE, groundDiffuse);
		glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
		::Render(constraints[0]);
	}

	glColor3f(constraintDiffuse[0], constraintDiffuse[1], constraintDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, constraintAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, constraintDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	for (int i = 1, size = constraints.size(); i < size; ++i) {
		::Render(constraints[i]);
	}
	if (DebugRender) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

		GLboolean status;
		glGetBooleanv(GL_LIGHTING, &status);

		glDisable(GL_LIGHTING);
		for (int i = 0; i < results.size(); ++i) {
			::Render(results[i]);
		}
		if (status) {
			glEnable(GL_LIGHTING);
		}
	}

	// Render springs
	GLboolean status;
	glGetBooleanv(GL_LIGHTING, &status);
	for (int i = 0, size = springs.size(); i < size; ++i) {
		for (int i = 0, size = springs.size(); i < size; ++i) {
			if (springs[i].GetP1() == 0 || springs[i].GetP2() == 0) {
				continue;
			}

			Line l(springs[i].GetP1()->GetPosition(), springs[i].GetP2()->GetPosition());
			::Render(l);
		}
	}
	if (status) {
		glEnable(GL_LIGHTING);
	}

	// Render all cloths
	for (int i = 0, size = cloths.size(); i < size; ++i) {
		cloths[i]->Render(DebugRender);
	}
}

void PhysicsSystem::AddRigidbody(Rigidbody* body) {
	bodies.push_back(body);
}

void PhysicsSystem::AddConstraint(const OBB& obb) {
	constraints.push_back(obb);
}

void PhysicsSystem::ClearRigidbodys() {
	bodies.clear();
}

void PhysicsSystem::ClearConstraints() {
	constraints.clear();
}

void PhysicsSystem::AddSpring(const Spring& spring) {
	springs.push_back(spring);
}

void PhysicsSystem::ClearSprings() {
	springs.clear();
}

void PhysicsSystem::AddCloth(Cloth* cloth) {
	cloths.push_back(cloth);
}

void PhysicsSystem::ClearCloths() {
	cloths.clear();
}

<|start_filename|>Chapter04/Physics/Camera.h<|end_filename|>
#ifndef _H_CAMERA_
#define _H_CAMERA_

#include "matrices.h"
#include "Geometry3D.h"

class Camera {
protected:
	float m_nFov;
	float m_nAspect;
	float m_nNear;
	float m_nFar;
	float m_nWidth;
	float m_nHeight;

	mat4 m_matWorld; // World Transform
	// View Transform = Inverse(World Transform)
	mat4 m_matProj;
	int m_nProjectionMode; // 0 - Perspective, 1 - Ortho, 2 - User
public:
	Camera();
	// Default copy constructor / assignment operator will do!
	inline virtual ~Camera() { }

	mat4 GetWorldMatrix();
	mat4 GetViewMatrix(); // Inverse of world!
	mat4 GetProjectionMatrix();

	float GetAspect();
	bool IsOrthographic();
	bool IsPerspective();

	bool IsOrthoNormal();
	void OrthoNormalize();

	void Resize(int width, int height);
	
	void Perspective(float fov, float aspect, float zNear, float zFar);
	void Orthographic(float width, float height, float zNear, float zFar);
	
	void SetProjection(const mat4& projection);
	void SetWorld(const mat4& view);

	Frustum GetFrustum();
};

Camera CreatePerspective(float fieldOfView, float aspectRatio, float nearPlane, float farPlane);
Camera CreateOrthographic(float width, float height, float nearPlane, float farPlane);

class OrbitCamera : public Camera {
protected:
	vec3 target;
	vec2 panSpeed;

	float zoomDistance;
	vec2 zoomDistanceLimit; // x = min, y = max;
	float zoomSpeed;

	vec2 rotationSpeed;
	vec2 yRotationLimit; // x = min, y = max
	vec2 currentRotation;
public:
	OrbitCamera();
	inline virtual ~OrbitCamera() { }

	void Rotate(const vec2& deltaRot, float deltaTime);
	void Zoom(float deltaZoom, float deltaTime);
	void Pan(const vec2& delataPan, float deltaTime);

	void Update(float dt);
	float ClampAngle(float angle, float min, float max);
};

#endif 


<|start_filename|>Chapter03/Code/IWindow.cpp<|end_filename|>
#include <iostream>
#include <cstring>
#include "IWindow.h"

IWindow* IWindow::g_pSingleton = 0;

#if ENABLE_USER_DATA
	#define I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH 5 
	#define I_WINDOW_CPP_GENERIC_EXPAND_BY 10
	#define I_WINDOW_CPP_DEFAULTS(w, h) \
		m_bQuitFlag(false), m_bFullscreenFlag(false), m_nWidth(w), m_nHeight(h), \
		m_szTitle(0), m_bTitleDirty(false), m_vGenericIntegerValues(0), \
		m_vGenericIntegerNames(0),m_nGenericIntegerCount(0), m_nGenericIntegerLength(0), \
		m_nTargetFPS(30), m_nFixedFPS(30), b_isVisible(false)
#else 
	#define I_WINDOW_CPP_DEFAULTS(w, h) \
		m_bQuitFlag(false), m_bFullscreenFlag(false), m_nWidth(w), m_nHeight(h), \
		m_szTitle(0), m_bTitleDirty(false), m_nTargetFPS(30), m_nFixedFPS(30), b_isVisible(false)
#endif

IWindow::IWindow() : I_WINDOW_CPP_DEFAULTS(800, 600) {
	if (g_pSingleton != 0) {
		std::cout << "Error, two instances of the window interface exist!\n";
		m_bQuitFlag = true;
	}

	SetTitle("Platform Window");
	m_bTitleDirty = true;

#if ENABLE_USER_DATA
	InitGenericIntegers();
#endif

	g_pSingleton = this;
};

IWindow::IWindow(const char* title) : I_WINDOW_CPP_DEFAULTS(800, 600) {
	if (g_pSingleton != 0) {
		std::cout << "Error, two instances of the window interface exist!\n";
		m_bQuitFlag = true;
	}

	SetTitle(title);
	m_bTitleDirty = true;

#if ENABLE_USER_DATA
	InitGenericIntegers();
#endif

	g_pSingleton = this;
}

IWindow::IWindow(const char* title, int width, int height) : I_WINDOW_CPP_DEFAULTS(width, height) {
	if (g_pSingleton != 0) {
		std::cout << "Error, two instances of the window interface exist!\n";
		m_bQuitFlag = true;
	}

	SetTitle(title);
	m_bTitleDirty = false;

#if ENABLE_USER_DATA
	InitGenericIntegers();
#endif

	g_pSingleton = this;
}

void IWindow::SetTitle(const char* title) {
	if (m_szTitle != 0) {
		free(m_szTitle);
	}

	int len = strlen(title);
	m_szTitle = (char*)malloc(sizeof(char) * len + 1);
	strcpy(m_szTitle, title);
	m_szTitle[len] = '\0';

	m_bTitleDirty = true;
}

bool IWindow::GetAndResetTitleDirtyFlag() {
	bool value = m_bTitleDirty;
	m_bTitleDirty = false;
	return value;
}

const char* IWindow::GetTitle() {
	return m_szTitle;
}

void CleanupMemory(IWindow* window) {
	if (window->m_szTitle != 0) {
		free(window->m_szTitle);
	}
	window->m_szTitle = 0;

#if ENABLE_USER_DATA
	if (window->m_vGenericIntegerValues != 0) {
		free(window->m_vGenericIntegerValues);
	}
	window->m_vGenericIntegerValues = 0;

	if (window->m_vGenericIntegerNames != 0) {
		for (int i = 0; i < window->m_nGenericIntegerCount; ++i) {
			free(window->m_vGenericIntegerNames[i]);
			window->m_vGenericIntegerNames[i] = 0;
		}
		free(window->m_vGenericIntegerNames);
	}
	window->m_vGenericIntegerNames = 0;
#endif
}

IWindow::~IWindow() {
	CleanupMemory(this);
}

void IWindow::Close() {
	m_bQuitFlag = true;
}

bool IWindow::GetQuitFlag() {
	return m_bQuitFlag;
}

int IWindow::GetWidth() {
	return m_nWidth;
}

int IWindow::GetHeight() {
	return m_nHeight;
}

void IWindow::Resize(int width, int height) {
	m_nWidth = width;
	m_nHeight = height;
}

void IWindow::SetFullScreen(bool value) {
	m_bFullscreenFlag = value;
}

bool IWindow::GetFullScreen() {
	return m_bFullscreenFlag;
}

#if ENABLE_USER_DATA
void IWindow::InitGenericIntegers() {
	m_nGenericIntegerCount = 0;
	m_nGenericIntegerLength = I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH;

	m_vGenericIntegerValues = (int*)malloc(sizeof(int) * I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH);
	memset(m_vGenericIntegerValues, 0, sizeof(int) * I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH);
	
	m_vGenericIntegerNames = (char**)malloc(sizeof(char*) * I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH);
	memset(m_vGenericIntegerNames, 0, sizeof(char*) * I_WINDOW_CPP_GENERIC_DEFAULT_LENGTH);
}

void IWindow::SetInt(const char* name, int value) {
	if (m_vGenericIntegerNames == 0) {
		InitGenericIntegers();
	}

	// Update
	for (int i = 0; i < m_nGenericIntegerCount; ++i) {
		if (strcmp(m_vGenericIntegerNames[i], name) == 0) {
			m_vGenericIntegerValues[i] = value;
			return;
		}
	}

	// Expand
	if (m_nGenericIntegerCount == m_nGenericIntegerLength) {
		int newSize = m_nGenericIntegerLength + I_WINDOW_CPP_GENERIC_EXPAND_BY;

		int* oldIntegerValues = m_vGenericIntegerValues;
		char** oldIntegerNames = m_vGenericIntegerNames;

		m_vGenericIntegerValues = (int*)malloc(sizeof(int) * newSize);
		memset(m_vGenericIntegerValues, 0, sizeof(int) * newSize);
		memcpy(m_vGenericIntegerValues, oldIntegerValues, sizeof(int) * m_nGenericIntegerLength);

		m_vGenericIntegerNames = (char**)malloc(sizeof(char*) * newSize);
		memset(m_vGenericIntegerNames, 0, sizeof(char*) * newSize);
		memcpy(m_vGenericIntegerNames, oldIntegerNames, sizeof(char*) * m_nGenericIntegerLength);

		free(oldIntegerValues);
		for (int i = 0; i < m_nGenericIntegerCount; ++i) {
			free(oldIntegerNames[i]);
		}
		free(oldIntegerNames);
		m_nGenericIntegerLength += I_WINDOW_CPP_GENERIC_EXPAND_BY;
	}

	// Insert
	int length = strlen(name) + 1;
	m_vGenericIntegerValues[m_nGenericIntegerCount] = value;
	m_vGenericIntegerNames[m_nGenericIntegerCount] = (char*)malloc(sizeof(char) * length);
	strcpy(m_vGenericIntegerNames[m_nGenericIntegerCount], name);
	m_vGenericIntegerNames[m_nGenericIntegerCount][length - 1] = '\0';
	m_nGenericIntegerCount += 1;
}

int IWindow::GetInt(const char* name, int default) {
	if (m_vGenericIntegerNames == 0) {
		return default;
	}

	for (int i = 0; i < m_nGenericIntegerCount; ++i) {
		if (strcmp(m_vGenericIntegerNames[i], name) == 0) {
			return m_vGenericIntegerValues[i];
		}
	}
	return default;
}

bool IWindow::HasInt(const char* name) {
	if (m_vGenericIntegerNames == 0) {
		return false;
	}

	for (int i = 0; i < m_nGenericIntegerCount; ++i) {
		if (strcmp(m_vGenericIntegerNames[i], name) == 0) {
			return true;
		}
	}
	return false;
}
#endif

void IWindow::SetTargetFPS(int target) {
	m_nTargetFPS = target;
	if (m_nTargetFPS < 15) {
		m_nTargetFPS = 15;
	}
	if (m_nTargetFPS > 120) {
		m_nTargetFPS = 120;
	}
}

int IWindow::GetFixedFPS() {
	return m_nFixedFPS;
}

void IWindow::MarkAsShown() {
	b_isVisible = true;
}

bool IWindow::WasWindowShown() {
	return b_isVisible;
}

int IWindow::GetTargetFPS() {
	return m_nTargetFPS;
}

int KeyIndex(int keyCode) {
	switch (keyCode) {
		case KEY_NONE:              return 0;
		case KEY_PAUSE:             return 1;
		case KEY_SCROLL_LOCK:       return 2;
		case KEY_PRINT:             return 3;
		case KEY_SYSREQ:            return 4;
		case KEY_BREAK:             return 5;
		case KEY_ESCAPE:            return 6;
		case KEY_BACKSPACE:         return 7;
		case KEY_TAB:               return 8;
		case KEY_BACK_TAB:          return 9;
		case KEY_RETURN:            return 10;
		case KEY_CAPS_LOCK:         return 11;
		case KEY_SHIFT:             return 12;
		case KEY_CTRL:              return 13;
		case KEY_ALT:               return 14;
		case KEY_MENU:              return 15;
		case KEY_HYPER:             return 16;
		case KEY_INSERT:            return 17;
		case KEY_HOME:              return 18;
		case KEY_PG_UP:             return 19;
		case KEY_DELETE:            return 20;
		case KEY_END:               return 21;
		case KEY_PG_DOWN:           return 22;
		case KEY_LEFT_ARROW:        return 23;
		case KEY_RIGHT_ARROW:       return 24;
		case KEY_UP_ARROW:          return 25;
		case KEY_DOWN_ARROW:        return 26;
		case KEY_NUM_LOCK:          return 27;
		case KEY_KP_PLUS:           return 28;
		case KEY_KP_MINUS:          return 29;
		case KEY_KP_MULTIPLY:       return 30;
		case KEY_KP_DIVIDE:         return 31;
		case KEY_KP_ENTER:          return 32;
		case KEY_KP_HOME:           return 33;
		case KEY_KP_UP:             return 34;
		case KEY_KP_PG_UP:          return 35;
		case KEY_KP_LEFT:           return 36;
		case KEY_KP_FIVE:           return 37;
		case KEY_KP_RIGHT:          return 38;
		case KEY_KP_END:            return 39;
		case KEY_KP_DOWN:           return 40;
		case KEY_KP_PG_DOWN:        return 41;
		case KEY_KP_INSERT:         return 42;
		case KEY_KP_DELETE:         return 43;
		case KEY_F1:                return 44;
		case KEY_F2:                return 45;
		case KEY_F3:                return 46;
		case KEY_F4:                return 47;
		case KEY_F5:                return 48;
		case KEY_F6:                return 49;
		case KEY_F7:                return 50;
		case KEY_F8:                return 51;
		case KEY_F9:                return 52;
		case KEY_F10:               return 53;
		case KEY_F11:               return 54;
		case KEY_F12:               return 55;
		case KEY_SPACE:             return 56;
		case KEY_EXCLAM:            return 57;
		case KEY_QUOTE:             return 58;
		case KEY_NUMBER:            return 59;
		case KEY_DOLLAR:            return 60;
		case KEY_PERCENT:           return 61;
		case KEY_CIRCUMFLEX:        return 62;
		case KEY_AMPERSAND:         return 63;
		case KEY_APOSTROPHE:        return 64;
		case KEY_LEFT_PARENTHESIS:  return 65;
		case KEY_RIGHT_PARENTHESIS: return 66;
		case KEY_ASTERISK:          return 67;
		case KEY_PLUS:              return 68;
		case KEY_COMMA:             return 69;
		case KEY_MINUS:             return 70;
		case KEY_PERIOD:            return 71;
		case KEY_SLASH:             return 72;
		case KEY_ZERO:              return 73;
		case KEY_ONE:               return 74;
		case KEY_TWO:               return 75;
		case KEY_THREE:             return 76;
		case KEY_FOUR:              return 77;
		case KEY_FIVE:              return 78;
		case KEY_SIX:               return 79;
		case KEY_SEVEN:             return 80;
		case KEY_EIGHT:             return 81;
		case KEY_NINE:              return 82;
		case KEY_COLON:             return 83;
		case KEY_SEMICOLON:         return 84;
		case KEY_LESS_THAN:         return 85;
		case KEY_EQUAL:             return 86;
		case KEY_GREATER_THAN:      return 87;
		case KEY_QUESTION:          return 88;
		case KEY_AT:                return 89;
		case KEY_CAPITAL_A:         return 90;
		case KEY_CAPITAL_B:         return 91;
		case KEY_CAPITAL_C:         return 92;
		case KEY_CAPITAL_D:         return 93;
		case KEY_CAPITAL_E:         return 94;
		case KEY_CAPITAL_F:         return 95;
		case KEY_CAPITAL_G:         return 96;
		case KEY_CAPITAL_H:         return 97;
		case KEY_CAPITAL_I:         return 98;
		case KEY_CAPITAL_J:         return 99;
		case KEY_CAPITAL_K:         return 100;
		case KEY_CAPITAL_L:         return 101;
		case KEY_CAPITAL_M:         return 102;
		case KEY_CAPITAL_N:         return 103;
		case KEY_CAPITAL_O:         return 104;
		case KEY_CAPITAL_P:         return 105;
		case KEY_CAPITAL_Q:         return 106;
		case KEY_CAPITAL_R:         return 107;
		case KEY_CAPITAL_S:         return 108;
		case KEY_CAPITAL_T:         return 109;
		case KEY_CAPITAL_U:         return 110;
		case KEY_CAPITAL_V:         return 111;
		case KEY_CAPITAL_W:         return 112;
		case KEY_CAPITAL_X:         return 113;
		case KEY_CAPITAL_Y:         return 114;
		case KEY_CAPITAL_Z:         return 115;
		case KEY_LEFT_BRACKET:      return 116;
		case KEY_BACK_SLASH:        return 117;
		case KEY_RIGHT_BRACKET:     return 118;
		case KEY_UNDERSCORE:        return 119;
		case KEY_GRAVE:             return 120;
		case KEY_A:                 return 121;
		case KEY_B:                 return 122;
		case KEY_C:                 return 123;
		case KEY_D:                 return 124;
		case KEY_E:                 return 125;
		case KEY_F:                 return 126;
		case KEY_G:                 return 127;
		case KEY_H:                 return 128;
		case KEY_I:                 return 129;
		case KEY_J:                 return 130;
		case KEY_K:                 return 131;
		case KEY_L:                 return 132;
		case KEY_M:                 return 133;
		case KEY_N:                 return 134;
		case KEY_O:                 return 135;
		case KEY_P:                 return 136;
		case KEY_Q:                 return 137;
		case KEY_R:                 return 138;
		case KEY_S:                 return 139;
		case KEY_T:                 return 140;
		case KEY_U:                 return 141;
		case KEY_V:                 return 142;
		case KEY_W:                 return 143;
		case KEY_X:                 return 144;
		case KEY_Y:                 return 145;
		case KEY_Z:                 return 146;
		case KEY_LEFT_BRACE:        return 147;
		case KEY_BAR:               return 148;
		case KEY_RIGHT_BRACE:       return 149;
		case KEY_TILDE:             return 150;
		case KEY_EURO:              return 151;
		case KEY_POUND:             return 152;
		case KEY_YEN:               return 153;
		case KEY_MIDDLE_DOT:        return 154;
		case KEY_SEARCH:            return 155;
	}
	return 0;
}

<|start_filename|>Chapter02/Code/CH15Demo.h<|end_filename|>
#ifndef _H_ANGULAR_VELOCITY_
#define _H_ANGULAR_VELOCITY_

#include "DemoBase.h"
#include "Geometry3D.h"
#include "PhysicsSystem.h"
#include "RigidbodyVolume.h"

class CH15Demo : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	std::vector<RigidbodyVolume> bodies;
	RigidbodyVolume groundBox;

	bool size_imgui_window;
protected:
	void ResetDemo();
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};


#endif


<|start_filename|>Chapter05/Physics/Scene.h<|end_filename|>
#ifndef _H_SCENE_
#define _H_SCENE_

#include "Geometry3D.h"
#include <vector>

typedef struct OctreeNode {
	AABB bounds;
	OctreeNode* children;
	std::vector<Model*> models;

	inline OctreeNode() : children(0) { }
	inline ~OctreeNode() {
		if (children != 0) {
			delete[] children;
		}
	}
} OctreeNode;

class Scene {
protected:
	std::vector<Model*> objects;
	OctreeNode* octree;
private:
	Scene(const Scene&);
	Scene& operator=(const Scene&);
public:
	inline Scene() : octree(0) { } 
	inline ~Scene() {
		if (octree != 0) {
			delete octree;
		}
	}

	void AddModel(Model* model);
	void RemoveModel(Model* model);
	void UpdateModel(Model* model);
	std::vector<Model*> FindChildren(const Model* model);

	Model* Raycast(const Ray& ray);
	std::vector<Model*> Query(const Sphere& sphere);
	std::vector<Model*> Query(const AABB& aabb);

	bool Accelerate(const vec3& position, float size); 
};

void SplitTree(OctreeNode* node, int depth);

void Insert(OctreeNode* node, Model* model);
void Remove(OctreeNode* node, Model* model);
void Update(OctreeNode* node, Model* model);

Model* FindClosest(const std::vector<Model*>& set, const Ray& ray);
Model* Raycast(OctreeNode* node, const Ray& ray);
std::vector<Model*> Query(OctreeNode* node, const Sphere& sphere);
std::vector<Model*> Query(OctreeNode* node, const AABB& aabb);

#endif

<|start_filename|>Chapter01/Code/JointDemo.cpp<|end_filename|>
#include "JointDemo.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include <iostream>

extern double GetMilliseconds();

void JointDemo::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	numParticles = 50;
	size_imgui_window = true;
	lastFrameTime = GetMilliseconds();

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(9.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

void JointDemo::ResetDemo() {
	particles.clear();
	joints.clear();
	joints.reserve(numParticles / 2 + 1);
	particles.reserve(numParticles + 1);

	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();


	OBB ground;
	ground.size = vec3(10.0f, 0.15f, 10.0f);

	OBB obb1;
	obb1.position = vec3(0.0f, 1.86f, -1.92f);
	obb1.orientation = Rotation3x3(30.716f, 0.0f, 0.0f);
	obb1.size = vec3(2.0f, 0.15f, 2.0f);

	OBB obb2;
	obb2.position = vec3(-1.0f, 3.6f, 1.2f);
	obb2.orientation = Rotation3x3(-33.964f, -24.233f, 9.128f);
	obb2.size = vec3(2.0f, 0.15f, 2.0f);

	OBB obb3;
	obb3.position = vec3(0.0f, 3.93f, -2.27f);
	obb3.orientation = Rotation3x3(24.702f, 0.0f, 0.0f);
	obb3.size = vec3(2.0f, 0.15f, 0.7817011f);

	physicsSystem.AddConstraint(ground);
	physicsSystem.AddConstraint(obb1);
	physicsSystem.AddConstraint(obb2);
	physicsSystem.AddConstraint(obb3);

	vec3 spawnPos = vec3(-0.5f, 6.5f, -1.01f);
	vec3 spawnSize = vec3(3.8505f, 2, 4.034834f);
	vec3 spawnMin = spawnPos - spawnSize;
	vec3 spawnMax = spawnPos + spawnSize;

	int createCount = numParticles;
	if (numParticles % 2 != 0) {
		numParticles += 1;
	}
	int numJoints = createCount / 2;

	for (int i = 0; i < createCount; ++i) {
		particles.push_back(Particle());
		particles[i].SetPosition(Random(spawnMin, spawnMax));
		particles[i].SetBounce(Random(0, 1));
	}

	for (int i = 0; i < createCount; ++i) {
		physicsSystem.AddRigidbody(&particles[i]);
	}

	for (int i = 0; i < numJoints; ++i) {
		joints.push_back(DistanceJoint());
		joints[i].Initialize(&particles[i], &particles[i + numJoints], Random(0.5f, 1.0f));
		physicsSystem.AddRigidbody(&joints[i]);
	}
}

void JointDemo::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();
}

void JointDemo::ImGUI() {
	DemoBase::ImGUI();

	float avgTime = 0;
	std::list<float>::iterator iterator = deltaTimes.begin();
	for (; iterator != deltaTimes.end(); iterator++) {
		avgTime += *iterator;
	}
	if (deltaTimes.size() > 0) {
		avgTime /= deltaTimes.size();
	}

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 75));
	}

	ImGui::Begin("Joint Demo", 0, ImGuiWindowFlags_NoResize);

	ImGui::Text("Particle count");
	ImGui::SameLine();
	ImGui::PushItemWidth(100);
	ImGui::SliderInt("", &numParticles, 2, 100);
	ImGui::SameLine();
	if (ImGui::Button("Reset")) {
		ResetDemo();
	}
	ImGui::SameLine();
	if (ImGui::Button("Show Help")) {
		show_help = true;
	}
	ImGui::End();
}

void JointDemo::Update(float dt) {
	DemoBase::Update(dt);

	double thisFrameTime = GetMilliseconds();
	deltaTimes.push_back(float(thisFrameTime - lastFrameTime));
	while (deltaTimes.size() > 120) {
		deltaTimes.pop_front();
	}
	lastFrameTime = thisFrameTime;

	physicsSystem.Update(dt);

	// This next part isn't really needed. It's just in place 
	// in case the simulation becomes unstable and something 
	// falls below the floor of the world!
	for (int i = 0; i < particles.size(); ++i) {
		vec3 position = particles[i].GetPosition();
		if (position.y < -5.0f) {
			position.y = -5.0f;
			particles[i].SetPosition(position);
		}
	}
}

float JointDemo::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

vec3 JointDemo::Random(vec3 min, vec3 max) {
	vec3 result;
	result.x = Random(min.x, max.x);
	result.y = Random(min.y, max.y);
	result.z = Random(min.z, max.z);
	return result;
}

<|start_filename|>Chapter01/Code/DistanceJoint.cpp<|end_filename|>
#include "DistanceJoint.h"
#include "FixedFunctionPrimitives.h"

void DistanceJoint::Initialize(Particle* _p1, Particle* _p2, float len) {
	p1 = _p1;
	p2 = _p2;
	length = len;
}

void DistanceJoint::SolveConstraints(const std::vector<OBB>& constraints) {
	vec3 delta = p2->GetPosition() - p1->GetPosition();
	float distance = Magnitude(delta);
	float correction = (distance - length) / distance;
	
	p1->SetPosition(p1->GetPosition() + delta * 0.5f * correction);
	p2->SetPosition(p2->GetPosition() - delta * 0.5f * correction);

	p1->SolveConstraints(constraints);
	p2->SolveConstraints(constraints);
}

void DistanceJoint::Render() {
	vec3 pos1 = p1->GetPosition();
	vec3 pos2 = p2->GetPosition();
	Line l(pos1, pos2);
	::Render(l);
}

<|start_filename|>Chapter01/Code/RigidbodyVolume.cpp<|end_filename|>
#include "RigidbodyVolume.h"
#include "Compare.h"
#include "FixedFunctionPrimitives.h"

void RigidbodyVolume::ApplyForces() {
	forces = GRAVITY_CONST * mass;
}

#ifndef LINEAR_ONLY
void  RigidbodyVolume::AddRotationalImpulse(const vec3& point, const vec3& impulse) {
	vec3 centerOfMass = position;
	vec3 torque = Cross(point - centerOfMass, impulse);

	vec3 angAccel = MultiplyVector(torque, InvTensor());
	angVel = angVel + angAccel;
}
#endif

void RigidbodyVolume::AddLinearImpulse(const vec3& impulse) {
	velocity = velocity + impulse;
}

float RigidbodyVolume::InvMass() {
	if (mass == 0.0f) {
		return 0.0f;
	}
	return 1.0f / mass;
}

void RigidbodyVolume::SynchCollisionVolumes() {
	sphere.position = position;
	box.position = position;

#ifndef LINEAR_ONLY
	box.orientation = Rotation3x3(
		RAD2DEG(orientation.x),
		RAD2DEG(orientation.y),
		RAD2DEG(orientation.z)
	);
#endif
}

void RigidbodyVolume::Render() {
	SynchCollisionVolumes();

	if (type == RIGIDBODY_TYPE_SPHERE) {
		::Render(sphere);
	}
	else if (type == RIGIDBODY_TYPE_BOX) {
		::Render(box);
	}
}

#ifndef LINEAR_ONLY
mat4 RigidbodyVolume::InvTensor() {
	if (mass == 0) {
		return mat4(
			0, 0, 0, 0,
			0, 0, 0, 0,
			0, 0, 0, 0,
			0, 0, 0, 0
		);
	}
	float ix = 0.0f;
	float iy = 0.0f;
	float iz = 0.0f;
	float iw = 0.0f;

	if (mass != 0 && type == RIGIDBODY_TYPE_SPHERE) {
		float r2 = sphere.radius * sphere.radius;
		float fraction = (2.0f / 5.0f);

		ix = r2 * mass * fraction;
		iy = r2 * mass * fraction;
		iz = r2 * mass * fraction;
		iw = 1.0f;
	}
	else if (mass != 0 && type == RIGIDBODY_TYPE_BOX) {
		vec3 size = box.size * 2.0f;
		float fraction = (1.0f / 12.0f);

		float x2 = size.x * size.x;
		float y2 = size.y * size.y;
		float z2 = size.z * size.z;

		ix = (y2 + z2) * mass * fraction;
		iy = (x2 + z2) * mass * fraction;
		iz = (x2 + y2) * mass * fraction;
		iw = 1.0f;
	}

	return Inverse(mat4(
		ix, 0, 0, 0,
		0, iy, 0, 0,
		0, 0, iz, 0,
		0, 0, 0, iw));
}
#endif

void RigidbodyVolume::Update(float dt) {
	// Integrate velocity
	const float damping = 0.98f;

	vec3 acceleration = forces * InvMass();
	velocity = velocity + acceleration * dt;
	velocity = velocity * damping;

	if (fabsf(velocity.x) < 0.001f) {
		velocity.x = 0.0f;
	}
	if (fabsf(velocity.y) < 0.001f) {
		velocity.y = 0.0f;
	}
	if (fabsf(velocity.z) < 0.001f) {
		velocity.z = 0.0f;
	}

#ifndef LINEAR_ONLY
	if (type == RIGIDBODY_TYPE_BOX) {
		vec3 angAccel = MultiplyVector(torques, InvTensor());
		angVel = angVel + angAccel * dt;
		angVel = angVel *  damping;

		if (fabsf(angVel.x) < 0.001f) {
			angVel.x = 0.0f;
		}
		if (fabsf(angVel.y) < 0.001f) {
			angVel.y = 0.0f;
		}
		if (fabsf(angVel.z) < 0.001f) {
			angVel.z = 0.0f;
		}
	}
#endif

	// Integrate position
	position = position + velocity * dt;

#ifndef LINEAR_ONLY
	if (type == RIGIDBODY_TYPE_BOX) {
		orientation = orientation + angVel * dt;
	}
#endif

	SynchCollisionVolumes();
}

CollisionManifold FindCollisionFeatures(RigidbodyVolume& ra, RigidbodyVolume& rb) {
	CollisionManifold result;
	ResetCollisionManifold(&result);

	if (ra.type == RIGIDBODY_TYPE_SPHERE) {
		if (rb.type == RIGIDBODY_TYPE_SPHERE) {
			result = FindCollisionFeatures(ra.sphere, rb.sphere);
		}
		else if (rb.type == RIGIDBODY_TYPE_BOX) {
			result = FindCollisionFeatures(rb.box, ra.sphere);
			result.normal = result.normal * -1.0f;
		}
	}
	else if (ra.type == RIGIDBODY_TYPE_BOX) {
		if (rb.type == RIGIDBODY_TYPE_BOX) {
			result = FindCollisionFeatures(ra.box, rb.box);
		}
		else if (rb.type == RIGIDBODY_TYPE_SPHERE) {
			result = FindCollisionFeatures(ra.box, rb.sphere);
		}
	}


	return result;
}

void ApplyImpulse(RigidbodyVolume& A, RigidbodyVolume& B, const CollisionManifold& M, int c) {
	// Linear impulse
	float invMass1 = A.InvMass();
	float invMass2 = B.InvMass();
	float invMassSum = invMass1 + invMass2;

	if (invMassSum == 0.0f) {
		return; // Both objects have infinate mass!
	}

#ifndef LINEAR_ONLY
	vec3 r1 = M.contacts[c] - A.position;
	vec3 r2 = M.contacts[c] - B.position;
	mat4 i1 = A.InvTensor();
	mat4 i2 = B.InvTensor();
#endif

	// Relative velocity
#ifndef LINEAR_ONLY
	vec3 relativeVel = (B.velocity + Cross(B.angVel, r2)) - (A.velocity + Cross(A.angVel, r1));
#else
	vec3 relativeVel = B.velocity - A.velocity;
#endif
	// Relative collision normal
	vec3 relativeNorm = M.normal;
	Normalize(relativeNorm);

	// Moving away from each other? Do nothing!
	if (Dot(relativeVel, relativeNorm) > 0.0f) {
		return;
	}

	float e = fminf(A.cor, B.cor);

	float numerator = (-(1.0f + e) * Dot(relativeVel, relativeNorm));
	float d1 = invMassSum;
#ifndef LINEAR_ONLY
	vec3 d2 = Cross(MultiplyVector(Cross(r1, relativeNorm), i1), r1);
	vec3 d3 = Cross(MultiplyVector(Cross(r2, relativeNorm), i2), r2);
	float denominator = d1 + Dot(relativeNorm, d2 + d3);
#else
	float denominator = d1;
#endif

	float j = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	if (M.contacts.size() > 0.0f && j != 0.0f) {
		j /= (float)M.contacts.size();
	}

	vec3 impulse = relativeNorm * j;
	A.velocity = A.velocity - impulse *  invMass1;
	B.velocity = B.velocity + impulse *  invMass2;

#ifndef LINEAR_ONLY
	A.angVel = A.angVel - MultiplyVector(Cross(r1, impulse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, impulse), i2);
#endif

	// Friction
	vec3 t = relativeVel - (relativeNorm * Dot(relativeVel, relativeNorm));
	if (CMP(MagnitudeSq(t), 0.0f)) {
		return;
	}
	Normalize(t);

	numerator = -Dot(relativeVel, t);
	d1 = invMassSum;
#ifndef LINEAR_ONLY
	d2 = Cross(MultiplyVector(Cross(r1, t), i1), r1);
	d3 = Cross(MultiplyVector(Cross(r2, t), i2), r2);
	denominator = d1 + Dot(t, d2 + d3);
#else
	denominator = d1;
#endif

	float jt = (denominator == 0.0f) ? 0.0f : numerator / denominator;
	if (M.contacts.size() > 0.0f && jt != 0.0f) {
		jt /= (float)M.contacts.size();
	}

	if (CMP(jt, 0.0f)) {
		return;
	}

	vec3 tangentImpuse;
#ifdef DYNAMIC_FRICTION
	float sf = sqrtf(A.staticFriction * B.staticFriction);
	float df = sqrtf(A.dynamicFriction * B.dynamicFriction);
	if (fabsf(jt) < j * sf) {
		tangentImpuse = t * jt;
	}
	else {
		tangentImpuse = t * -j * df;
	}
#else
	float friction = sqrtf(A.friction * B.friction);
	if (jt > j * friction) {
		jt = j * friction;
	}
	else if (jt < -j * friction) {
		jt = -j * friction;
	}
	tangentImpuse = t * jt;
#endif

	A.velocity = A.velocity - tangentImpuse *  invMass1;
	B.velocity = B.velocity + tangentImpuse *  invMass2;

#ifndef LINEAR_ONLY
	A.angVel = A.angVel - MultiplyVector(Cross(r1, tangentImpuse), i1);
	B.angVel = B.angVel + MultiplyVector(Cross(r2, tangentImpuse), i2);
#endif
}

<|start_filename|>Chapter02/Code/CH14Demo.cpp<|end_filename|>
#include "CH14Demo.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"
#include "imgui/imgui.h"
#include <iostream>

extern double GetMilliseconds();
 
void CH14Demo::Initialize(int width, int height) {
	DemoBase::Initialize(width, height);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	float val[] = { 0.5f, 1.0f, -1.5f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	numParticles = 50;
	size_imgui_window = true;
	lastFrameTime = GetMilliseconds();

	camera.SetTarget(vec3(3.75622f, 2.98255f, 0.0f));
	camera.SetZoom(9.0f);
	camera.SetRotation(vec2(-67.9312f, 19.8f));

	ResetDemo();
}

void CH14Demo::ResetDemo() {
	particles.clear();
	particles.reserve(numParticles);

	physicsSystem.ClearRigidbodys();
	physicsSystem.ClearConstraints();


	OBB ground;
	ground.size = vec3(10.0f, 0.15f, 10.0f);

	OBB obb1;
	obb1.position = vec3(0.0f, 1.86f, -1.92f);
	obb1.orientation = Rotation3x3(30.716f, 0.0f, 0.0f);
	obb1.size = vec3(2.0f, 0.15f, 2.0f);

	OBB obb2;
	obb2.position = vec3(-1.0f, 3.6f, 1.2f);
	obb2.orientation = Rotation3x3(-33.964f, -24.233f, 9.128f);
	obb2.size = vec3(2.0f, 0.15f, 2.0f);

	OBB obb3;
	obb3.position = vec3(0.0f, 3.93f, -2.27f);
	obb3.orientation = Rotation3x3(24.702f, 0.0f, 0.0f);
	obb3.size = vec3(2.0f, 0.15f, 0.7817011f);

	physicsSystem.AddConstraint(ground);
	physicsSystem.AddConstraint(obb1);
	physicsSystem.AddConstraint(obb2);
	physicsSystem.AddConstraint(obb3);

	vec3 spawnPos = vec3(-0.5f, 6.5f, -1.01f);
	vec3 spawnSize = vec3(3.8505f, 2, 4.034834f);
	vec3 spawnMin = spawnPos - spawnSize;
	vec3 spawnMax = spawnPos + spawnSize;

	for (int i = 0; i < numParticles; ++i) {
		particles.push_back(Particle());
		particles[i].SetPosition(Random(spawnMin, spawnMax));
		particles[i].SetBounce(Random(0, 1));
		physicsSystem.AddRigidbody(&particles[i]);
	}

	/* Debug
	particles.push_back(Particle());
	particles[0].SetPosition(vec3(-1.07645, 7.29192, 0.292606));
	physicsSystem.AddRigidbody(&particles[0]);
	*/
}

void CH14Demo::Render() {
	DemoBase::Render();

	float val[] = { 0.0f, 1.0f, 0.0f, 0.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, val);

	physicsSystem.Render();
}

void CH14Demo::ImGUI() {
	DemoBase::ImGUI();

	float avgTime = 0;
	std::list<float>::iterator iterator = deltaTimes.begin();
	for (; iterator != deltaTimes.end(); iterator++) {
		avgTime += *iterator;
	}
	if (deltaTimes.size() > 0) {
		avgTime /= deltaTimes.size();
	}

	if (size_imgui_window) {
		size_imgui_window = false;
		ImGui::SetNextWindowPos(ImVec2(400, 10));
		ImGui::SetNextWindowSize(ImVec2(370, 75));
	}

	ImGui::Begin("Chapter 14 Demo", 0, ImGuiWindowFlags_NoResize);
	ImGui::Text("Simulation delta: %.3f ms/frame", avgTime);

	ImGui::Text("Particle count");
	ImGui::SameLine();
	ImGui::PushItemWidth(100);
	ImGui::SliderInt("", &numParticles, 1, 300);
	ImGui::SameLine();
	if (ImGui::Button("Reset")) {
		ResetDemo();
	}
	ImGui::SameLine();
	if (ImGui::Button("Show Help")) {
		show_help = true;
	}
	ImGui::End();
}

void CH14Demo::Update(float dt) {
	DemoBase::Update(dt);

	double thisFrameTime = GetMilliseconds();
	deltaTimes.push_back(float(thisFrameTime - lastFrameTime));
	while (deltaTimes.size() > 120) {
		deltaTimes.pop_front();
	}
	lastFrameTime = thisFrameTime;

	physicsSystem.Update(dt);

	// This next part isn't really needed. It's just in place 
	// in case the simulation becomes unstable and something 
	// falls below the floor of the world!
	for (int i = 0; i < particles.size(); ++i) {
		vec3 position = particles[i].GetPosition();
		if (position.y < -5.0f) {
			position.y = -5.0f;
			particles[i].SetPosition(position);
		}
	}
}

float CH14Demo::Random(float min, float max) {
	if (max < min) {
		float t = min;
		min = max;
		max = t;
	}

	float random = ((float)rand()) / (float)RAND_MAX;

	float range = max - min;
	return (random*range) + min;
}

vec3 CH14Demo::Random(vec3 min, vec3 max) {
	vec3 result;
	result.x = Random(min.x, max.x);
	result.y = Random(min.y, max.y);
	result.z = Random(min.z, max.z);
	return result;
}

<|start_filename|>Chapter03/Code/DemoWindow.h<|end_filename|>
#ifndef _H_DEMO_WINDOW_
#define _H_DEMO_WINDOW_

#include "GLWindow.h"
#include "vectors.h"
#include "DemoBase.h"

class DemoWindow : public GLWindow {
protected:
	vec2 m_prevMousePos;
	int m_selectedDemo; // -1: None, 0: CH14, 1: CH15, 2: CH16
	DemoBase* m_pDemo;
	bool imgui_init;
protected:
	void ApplyDemoCamera();
public:

	DemoWindow(const char* title, int width, int height)
		: GLWindow(title, width, height) {
		m_nFixedFPS = 30.0f;
		m_nTargetFPS = 60.0f;
	}
	virtual ~DemoWindow() { }

	virtual void OnRender();
	virtual void OnResize(int width, int height);
	virtual void OnInitialize();
	virtual void OnFixedUpdate(float deltaTime);
	virtual void OnUpdate(float deltaTime);
	virtual void OnShutdown();

	void StopDemo();
	void Start14();
	void Start15();
	void Start16();
};

// Static instance defined in .cpp file!

#endif 

<|start_filename|>Chapter15/Physics/main.cpp<|end_filename|>
#include <iostream>
#include <cstdio>
#include "vectors.h"
#include "matrices.h"
#include <stdlib.h>

// http://realtimecollisiondetection.net/pubs/Tolerances/
#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))


void DebugPrintMatrixArray(mat4& m) {
	std::cout << "[0] :"  << m.asArray[0]  << ", [1] :" << m.asArray[1]  << ", [2] :"  << m.asArray[2]  << ", [3] :" << m.asArray[3]  << ",\n";
	std::cout << "[4] :"  << m.asArray[4] <<  ", [5] :" << m.asArray[5]  << ", [6] :"  << m.asArray[6]  << ", [7] :" << m.asArray[7]  << ",\n";
	std::cout << "[8] :"  << m.asArray[8]  << ", [9] :" << m.asArray[9]  << ", [10]:" << m.asArray[10]  << ", [11]:" << m.asArray[11] << ",\n";
	std::cout << "[12]: " << m.asArray[12] << ", [13]:" << m.asArray[13] << ", [14]:" << m.asArray[14] << ", [15]: " << m.asArray[15] << "\n";
}

void DebugPrintColMatrix(float* col) {
	std::cout << "[0]: " << col[0] << ", [1]" << col[1] << "\n";
	std::cout << "[2]: " << col[2] << ", [3]" << col[3] << "\n";
	std::cout << "[4]: " << col[4] << ", [5]" << col[5] << "\n";
	std::cout << "[6]: " << col[6] << ", [7]" << col[7] << "\n";
}

void DebugPrintRowMatrix(float* row) {
	std::cout << "[0] :" << row[0] << ", [1] :" << row[1] << ", [2] :" << row[2] << ", [3] :" << row[3] << ",\n";
	std::cout << "[4] :" << row[4] << ", [5] :" << row[5] << ", [6] :" << row[6] << ", [7] :" << row[7] << ",\n";
}

void PrintMat2(mat2& m) {
	std::cout << "[0] :" << m.asArray[0] << ", [1] :" << m.asArray[1] << ",\n";
	std::cout << "[2] :" << m.asArray[2] << ", [3] :" << m.asArray[3] << "\n";

}

bool ArrCompare(float* a, float* b, int s) {
	for (int i = 0; i < s; ++i) {
		if (!CMP(a[i], b[i])) {
			std::cout << "bad index: " << i << "\n";
			return false;
		}
	}
	return true;
}



int main(int argc, char** argv) {
	mat4 M1(-4, 0, 0, 0,
		-1, 2, 0, 0,
		-4.0f, 4.0f, 4.0f, 0.0f,
		- 1.0f, -9.0f, -1.0f, 1.0f);
	float det = Determinant(M1);
	//mat4 x = Minor(M1);
	std::cout << "det: " << det << " / -32\n";
	mat4 A1(8, 0, 0, 0,
		4, -16, 0, 0,
		4, 16, -8, 0,
		48, -128, -8, -32);
	mat4 E1(-8.0f / 32.0f, 0.0f, 0.0f, 0.0f,
		-4.0f / 32.0f, 16.0f / 32.0f, 0.0f, 0.0f,
		-4.0f / 32.0f, -16.0f / 32.0f, 8.0f / 32.0f, 0.0f,
		-48.0f / 32.0f, 128.0f / 32.0f, 8.0f / 32.0f, 32.0f / 32.0f);
	if (Adjugate(M1) != A1) {
		std::cout << "ERROR adjugate 1\n";
	}
	if (Inverse(M1) != E1) {
		std::cout << "ERROR inverse 1\n";
	}
	mat3 M2(1, 2, 3,
		2, 5, 3,
		1, 0, 8);
	mat3 E2(-40, 16, 9,
		13, -5, -3,
		5, -2, -1);
	if (Inverse(M2) != E2) {
		std::cout << "ERROR inverse 2\n";
	}
	mat3 M3(4, 2, 1,
		6, 3, 4,
		2, 1, 0);
	mat3 E3(-4, -8, 0, -1, -2, 0, 5, 10, 0);
	if (Minor(M3) != E3) {
		std::cout << "ERROR minor 1\n";
	}
	mat4 M4(1, 0, 0, 1,
		0, 2, 1, 2,
		2, 1, 0, 1,
		2, 0, 1, 4);
	mat4 E4(
		-2, -0.5f, 1, 0.5f, 
		1, 0.5f, 0, -0.5f,
		-8, -1, 2, 2,
		3, 0.5f, -1, -0.5f
	);
	if (Inverse(M4) != E4) {
		std::cout << "ERROR inverse 3\n";
	}

	std::cout << "Press return to exit...";

	getchar();

	return 0;
}

<|start_filename|>Chapter02/Code/DemoWindow.cpp<|end_filename|>
#include "DemoWindow.h"
#include "imgui/imgui.h"

#include "CH14Demo.h"
#include "RaycastDemo.h"
#include "CollisionFeature.h"
#include "LinearImpulse.h"
#include "ConservationOfMomentum.h"
#include "CH15Demo.h"

#include <cstdlib>

// This is the global instance of the IWindow singleton!
static DemoWindow g_WindowInstance("Physics Sandbox", 800, 600);

void DemoWindow::OnInitialize() {
	GLWindow::OnInitialize();

	m_prevMousePos = vec2(0, 0);
	m_selectedDemo = -1;
	m_pDemo = 0;
	imgui_init = true;

	select_all = false;
}

DemoWindow::~DemoWindow() {
	StopDemo();
}

void DemoWindow::OnResize(int width, int height) {
	GLWindow::OnResize(width, height);
	if (m_pDemo != 0) {
		m_pDemo->Resize(width, height);
	}
	ApplyDemoCamera();
}

void DemoWindow::OnRender() {
	GLWindow::OnRender();

	if (m_pDemo != 0) {
		mat4 view = m_pDemo->camera.GetViewMatrix();
		SetGLModelView(view.asArray);

		m_pDemo->Render();
	}
}

void DemoWindow::ApplyDemoCamera() {
	if (m_pDemo == 0) {
		return;
	}

	mat4 projection = m_pDemo->camera.GetProjectionMatrix();
	mat4 view = m_pDemo->camera.GetViewMatrix();

	SetGLProjection(projection.asArray);
	SetGLModelView(view.asArray);
}

void DemoWindow::OnUpdate(float deltaTime) {
	GLWindow::OnUpdate(deltaTime);

	if (imgui_init) {
		ImGui::SetNextWindowPos(ImVec2(10, 10));
		imgui_init = false;
	}
	ImGui::SetNextWindowSize(ImVec2(370, (select_all)? 145 : 75));
	ImGui::Begin("Physics Demo", 0, ImGuiWindowFlags_NoResize);
	ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);

	if (select_all) {
		const char* listbox_items[] = { "Raycast Demo", "Chapter 14", "Collision Features", "Linear Impulse", "Conservation Of Momentum", "Chapter 15" };
		int lastSelected = m_selectedDemo;
		ImGui::ListBox("", &m_selectedDemo, listbox_items, 6, 4);

		if (m_selectedDemo != lastSelected) {
			StopDemo();

			switch (m_selectedDemo) {
				case 0: m_pDemo = new RaycastDemo(); break;
				case 1: m_pDemo = new CH14Demo(); break;
				case 2: m_pDemo = new CollisionFeature(); break;
				case 3: m_pDemo = new LinearImpulse(); break;
				case 4: m_pDemo = new ConservationOfMomentum(); break;
				case 5: m_pDemo = new CH15Demo(); break;
			}
			
			m_pDemo->Initialize(GetWidth(), GetHeight());
			ApplyDemoCamera();
		}

		if (ImGui::Button("Clear Console")) {
			system("cls");
		}
	}
	else {
		if (m_selectedDemo == 0) {
			if (ImGui::Button("Stop Chapter 14")) {
				m_selectedDemo = -1;
				StopDemo();
			}
		}
		else {
			if (ImGui::Button(" Run Chapter 14")) {
				m_selectedDemo = 0;
				Start14();
			}
		}
		ImGui::SameLine();
		if (m_selectedDemo == 1) {
			if (ImGui::Button("Stop Chapter 15")) {
				m_selectedDemo = -1;
				StopDemo();
			}
		}
		else {
			if (ImGui::Button(" Run Chapter 15")) {
				m_selectedDemo = 1;
				Start15();
			}
		}
		/*ImGui::SameLine();
		if (m_selectedDemo == 2) {
			if (ImGui::Button("Stop Chapter 16")) {
				m_selectedDemo = -1;
				StopDemo();
			}
		}
		else {
			if (ImGui::Button(" Run Chapter 16")) {
				m_selectedDemo = 2;
				Start16();
			}
		}*/
	}

	ImGui::End();

	if (m_pDemo != 0) {
		m_pDemo->ImGUI();
	}
}

void DemoWindow::OnFixedUpdate(float deltaTime) {
	GLWindow::OnFixedUpdate(deltaTime);
	
	bool leftDown = MouseButonDown(MOUSE_LEFT);
	bool middleDown = MouseButonDown(MOUSE_MIDDLE);
	bool rightDown = MouseButonDown(MOUSE_RIGHT);

	vec2 mousePos = GetMousePosition();
	vec2 mouseDelta = mousePos - m_prevMousePos;
	mouseDelta.x /= (float)GetWidth();
	mouseDelta.y /= (float)GetHeight();

	if (m_pDemo != 0) {
		m_pDemo->SetMouseState(leftDown, middleDown, rightDown, mouseDelta, mousePos);
		m_pDemo->Update(deltaTime);
	}

	m_prevMousePos = mousePos;
}

void DemoWindow::OnShutdown() {
	GLWindow::OnShutdown();
	StopDemo();
}

void DemoWindow::StopDemo() {
	if (m_pDemo != 0) {
		m_pDemo->Shutdown();
		delete m_pDemo;
	}
	m_pDemo = 0;
}

void DemoWindow::Start14() {
	StopDemo();
	m_pDemo = new CH14Demo();
	m_pDemo->Initialize(GetWidth(), GetHeight());
	ApplyDemoCamera();
}

void DemoWindow::Start15() {
	StopDemo();
	m_pDemo = new CH15Demo();
	m_pDemo->Initialize(GetWidth(), GetHeight());
	ApplyDemoCamera();
}

void DemoWindow::Start16() {
	StopDemo();
	// TODO
}

<|start_filename|>Chapter05/Physics/matrices.cpp<|end_filename|>
#include "matrices.h"
#include <cmath>
#include <cfloat>

#ifdef DO_SANITY_TESTS
	#include <iostream>

	#ifdef FLT_EPSILON
		#undef FLT_EPSILON
	#endif
	#define FLT_EPSILON 0.0000005f
#endif 

// http://realtimecollisiondetection.net/pubs/Tolerances/
#define CMP(x, y) \
	(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))

#ifndef NO_EXTRAS
bool operator==(const mat2& l, const mat2& r) {
	for (int i = 0; i < /* 2 * 2 = */4; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}

bool operator==(const mat3& l, const mat3& r) {
	for (int i = 0; i < /* 3 * 3 = */ 9; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}
bool operator==(const mat4& l, const mat4& r) {
	for (int i = 0; i < /* 4 * 4 = */ 16; ++i) {
		if (!CMP(l.asArray[i], r.asArray[i])) {
			return false;
		}
	}
	return true;
}

bool operator!=(const mat2& l, const mat2& r) {
	return !(l == r);
}

bool operator!=(const mat3& l, const mat3& r) {
	return !(l == r);
}

bool operator!=(const mat4& l, const mat4& r) {
	return !(l == r);
}

std::ostream& operator<<(std::ostream& os, const mat2& m) {
	os << m._11 << ", " << m._12 << "\n";
	os << m._21 << ", " << m._22;
	return os;
}

std::ostream& operator<<(std::ostream& os, const mat3& m) {
	os << m._11 << ", " << m._12 << ", " << m._13 << "\n";
	os << m._21 << ", " << m._22 << ", " << m._23 << "\n";
	os << m._31 << ", " << m._32 << ", " << m._33;
	return os;
}

std::ostream& operator<<(std::ostream& os, const mat4& m) {
	os << m._11 << ", " << m._12 << ", " << m._13 << ", " << m._14 << "\n";
	os << m._21 << ", " << m._22 << ", " << m._23 << ", " << m._24 << "\n";
	os << m._31 << ", " << m._32 << ", " << m._33 << ", " << m._34 << "\n";
	os << m._41 << ", " << m._42 << ", " << m._43 << ", " << m._44;
	return os;
}

mat4 FromMat3(const mat3& mat) {
	mat4 result;

	result._11 = mat._11;
	result._12 = mat._12;
	result._13 = mat._13;

	result._21 = mat._21;
	result._22 = mat._22;
	result._23 = mat._23;

	result._31 = mat._31;
	result._32 = mat._32;
	result._33 = mat._33;

	return result;
}
#endif 

void Transpose(const float *srcMat, float *dstMat, int srcRows, int srcCols) {
	for (int i = 0; i < srcRows * srcCols; i++) {
		int row = i / srcRows;
		int col = i % srcRows;
		dstMat[i] = srcMat[srcCols * col + row];
	}
}

mat2 Transpose(const mat2& matrix) {
	mat2 result;
	Transpose(matrix.asArray, result.asArray, 2, 2);
	return result;
}

mat3 Transpose(const mat3& matrix) {
	mat3 result;
	Transpose(matrix.asArray, result.asArray, 3, 3);
	return result;
}

mat4 Transpose(const mat4& matrix) {
	mat4 result;
	Transpose(matrix.asArray, result.asArray, 4, 4);
	return result;
}

mat2 operator*(const mat2& matrix, float scalar) {
	mat2 result;
	for (int i = 0; i < 4; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

mat3 operator*(const mat3& matrix, float scalar) {
	mat3 result;
	for (int i = 0; i < 9; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

mat4 operator*(const mat4& matrix, float scalar) {
	mat4 result;
	for (int i = 0; i < 16; ++i) {
		result.asArray[i] = matrix.asArray[i] * scalar;
	}
	return result;
}

bool Multiply(float* out, const float* matA, int aRows, int aCols, const float* matB, int bRows, int bCols) {
	if (aCols != bRows) {
		return false;
	}

	for (int i = 0; i < aRows; ++i) {
		for (int j = 0; j < bCols; ++j) {
			out[bCols * i + j] = 0.0f;
			for (int k = 0; k < bRows; ++k) {
				out[bCols * i + j] += matA[aCols * i + k] * matB[bCols * k + j];
			}
		}
	}

	return true;
}

mat2 operator*(const mat2& matrixA, const mat2& matrixB) {
	mat2 result;
	Multiply(result.asArray, matrixA.asArray, 2, 2, matrixB.asArray, 2, 2);
	return result;
}

mat3 operator*(const mat3& matrixA, const mat3& matrixB) {
	mat3 result;
	Multiply(result.asArray, matrixA.asArray, 3, 3, matrixB.asArray, 3, 3);
	return result;
}

mat4 operator*(const mat4& matrixA, const mat4& matrixB) {
	mat4 result;
	Multiply(result.asArray, matrixA.asArray, 4, 4, matrixB.asArray, 4, 4);
	return result;
}

float Determinant(const mat2& matrix) {
	return matrix._11 * matrix._22 - matrix._12 * matrix._21;
}

mat2 Cut(const mat3& mat, int row, int col) {
	mat2 result;
	int index = 0;

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			if (i == row || j == col) {
				continue;
			}
			result.asArray[index++] = mat.asArray[3 * i + j];
		}
	}

	return result;
}

mat3 Cut(const mat4& mat, int row, int col) {
	mat3 result;
	int index = 0;

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			if (i == row || j == col) {
				continue;
			}
			result.asArray[index++] = mat.asArray[4 * i + j];
		}
	}

	return result;
}

mat3 Minor(const mat3& mat) {
	mat3 result;

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			result[i][j] = Determinant(Cut(mat, i, j));
		}
	}
	
	return result;
}

mat2 Minor(const mat2& mat) {
	return mat2(
		mat._22, mat._21,
		mat._12, mat._11
	);
}

void Cofactor(float* out, const float* minor, int rows, int cols) {
	for (int i = 0; i < rows; ++i) {
		for (int j = 0; j < cols; ++j) {
			out[cols * j + i] = minor[cols * j + i] * powf(-1.0f, i + j);
		}
	}
}

mat2 Cofactor(const mat2& mat) {
	mat2 result;
	Cofactor(result.asArray, Minor(mat).asArray, 2, 2);
	return result;
}

mat3 Cofactor(const mat3& mat) {
	mat3 result;
	Cofactor(result.asArray, Minor(mat).asArray, 3, 3);
	return result;
}

float Determinant(const mat3& mat) {
	float result = 0.0f;

	/*float A = mat.asArray[3 * 0 + 0] * Determinant(Cut(mat, 0, 0));
	float B = mat.asArray[3 * 0 + 1] * Determinant(Cut(mat, 0, 1));
	float C = mat.asArray[3 * 0 + 2] * Determinant(Cut(mat, 0, 2));
	result = A - B + C;*/

	/*for (int j = 0; j < 3; ++j) {
		result += mat.asArray[3 * 0 + j] * Determinant(Cut(mat, 0, j)) * powf(-1, 0 + j);
	}*/

	mat3 cofactor = Cofactor(mat);
	for (int j = 0; j < 3; ++j) {
		result += mat.asArray[3 * 0 + j] * cofactor[0][j];
	}

	return result;
}

mat4 Minor(const mat4& mat) {
	mat4 result;

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			result[i][j] = Determinant(Cut(mat, i, j));
		}
	}

	return result;
}

mat4 Cofactor(const mat4& mat) {
	mat4 result;
	Cofactor(result.asArray, Minor(mat).asArray, 4, 4);
	return result;
}

float Determinant(const mat4& mat) {
	float result = 0.0f;

	mat4 cofactor = Cofactor(mat);
	for (int j = 0; j < 4; ++j) {
		result += mat.asArray[4 * 0 + j] * cofactor[0][j];
	}

	return result;
}

mat2 Adjugate(const mat2& mat) {
	return Transpose(Cofactor(mat));
}

mat3 Adjugate(const mat3& mat) {
	return Transpose(Cofactor(mat));
}

mat4 Adjugate(const mat4& mat) {
	return Transpose(Cofactor(mat));
}

mat2 Inverse(const mat2& mat) {
	float det = Determinant(mat);
	if (CMP(det, 0.0f)) { return mat2(); }
	return Adjugate(mat) * (1.0f / det);

	/*float det = mat._11 * mat._22 - mat._12 * mat._21;
	if (CMP(det, 0.0f)) { 
		return mat2(); 
	}
	float i_det = 1.0f / det;
	mat2 result;
	result._11 =  mat._22 * i_det;
	result._12 = -mat._12 * i_det;
	result._21 = -mat._21 * i_det;
	result._22 =  mat._11 * i_det;
	return result;*/
}

mat3 Inverse(const mat3& mat) {
	float det = Determinant(mat);
	if (CMP(det, 0.0f)) { return mat3(); }
	return Adjugate(mat) * (1.0f / det);
}

mat4 Inverse(const mat4& m) {
	/*float det = Determinant(m);
	if (CMP(det, 0.0f)) { return mat4(); }
	return Adjugate(m) * (1.0f / det);*/

	// The code below is the expanded form of the above equation.
	// This optimization avoids loops and function calls

	float det 
		= m._11 * m._22 * m._33 * m._44 + m._11 * m._23 * m._34 * m._42 + m._11 * m._24 * m._32 * m._43
		+ m._12 * m._21 * m._34 * m._43 + m._12 * m._23 * m._31 * m._44 + m._12 * m._24 * m._33 * m._41
		+ m._13 * m._21 * m._32 * m._44 + m._13 * m._22 * m._34 * m._41 + m._13 * m._24 * m._31 * m._42
		+ m._14 * m._21 * m._33 * m._42 + m._14 * m._22 * m._31 * m._43 + m._14 * m._23 * m._32 * m._41
		- m._11 * m._22 * m._34 * m._43 - m._11 * m._23 * m._32 * m._44 - m._11 * m._24 * m._33 * m._42
		- m._12 * m._21 * m._33 * m._44 - m._12 * m._23 * m._34 * m._41 - m._12 * m._24 * m._31 * m._43
		- m._13 * m._21 * m._34 * m._42 - m._13 * m._22 * m._31 * m._44 - m._13 * m._24 * m._32 * m._41
		- m._14 * m._21 * m._32 * m._43 - m._14 * m._22 * m._33 * m._41 - m._14 * m._23 * m._31 * m._42;

	if (CMP(det, 0.0f)) { 
		return mat4(); 
	}
	float i_det = 1.0f / det;

	mat4 result;
	result._11 = (m._22 * m._33 * m._44 + m._23 * m._34 * m._42 + m._24 * m._32 * m._43 - m._22 * m._34 * m._43 - m._23 * m._32 * m._44 - m._24 * m._33 * m._42) * i_det;
	result._12 = (m._12 * m._34 * m._43 + m._13 * m._32 * m._44 + m._14 * m._33 * m._42 - m._12 * m._33 * m._44 - m._13 * m._34 * m._42 - m._14 * m._32 * m._43) * i_det;
	result._13 = (m._12 * m._23 * m._44 + m._13 * m._24 * m._42 + m._14 * m._22 * m._43 - m._12 * m._24 * m._43 - m._13 * m._22 * m._44 - m._14 * m._23 * m._42) * i_det;
	result._14 = (m._12 * m._24 * m._33 + m._13 * m._22 * m._34 + m._14 * m._23 * m._32 - m._12 * m._23 * m._34 - m._13 * m._24 * m._32 - m._14 * m._22 * m._33) * i_det;
	result._21 = (m._21 * m._34 * m._43 + m._23 * m._31 * m._44 + m._24 * m._33 * m._41 - m._21 * m._33 * m._44 - m._23 * m._34 * m._41 - m._24 * m._31 * m._43) * i_det;
	result._22 = (m._11 * m._33 * m._44 + m._13 * m._34 * m._41 + m._14 * m._31 * m._43 - m._11 * m._34 * m._43 - m._13 * m._31 * m._44 - m._14 * m._33 * m._41) * i_det;
	result._23 = (m._11 * m._24 * m._43 + m._13 * m._21 * m._44 + m._14 * m._23 * m._41 - m._11 * m._23 * m._44 - m._13 * m._24 * m._41 - m._14 * m._21 * m._43) * i_det;
	result._24 = (m._11 * m._23 * m._34 + m._13 * m._24 * m._31 + m._14 * m._21 * m._33 - m._11 * m._24 * m._33 - m._13 * m._21 * m._34 - m._14 * m._23 * m._31) * i_det;
	result._31 = (m._21 * m._32 * m._44 + m._22 * m._34 * m._41 + m._24 * m._31 * m._42 - m._21 * m._34 * m._42 - m._22 * m._31 * m._44 - m._24 * m._32 * m._41) * i_det;
	result._32 = (m._11 * m._34 * m._42 + m._12 * m._31 * m._44 + m._14 * m._32 * m._41 - m._11 * m._32 * m._44 - m._12 * m._34 * m._41 - m._14 * m._31 * m._42) * i_det;
	result._33 = (m._11 * m._22 * m._44 + m._12 * m._24 * m._41 + m._14 * m._21 * m._42 - m._11 * m._24 * m._42 - m._12 * m._21 * m._44 - m._14 * m._22 * m._41) * i_det;
	result._34 = (m._11 * m._24 * m._32 + m._12 * m._21 * m._34 + m._14 * m._22 * m._31 - m._11 * m._22 * m._34 - m._12 * m._24 * m._31 - m._14 * m._21 * m._32) * i_det;
	result._41 = (m._21 * m._33 * m._42 + m._22 * m._31 * m._43 + m._23 * m._32 * m._41 - m._21 * m._32 * m._43 - m._22 * m._33 * m._41 - m._23 * m._31 * m._42) * i_det;
	result._42 = (m._11 * m._32 * m._43 + m._12 * m._33 * m._41 + m._13 * m._31 * m._42 - m._11 * m._33 * m._42 - m._12 * m._31 * m._43 - m._13 * m._32 * m._41) * i_det;
	result._43 = (m._11 * m._23 * m._42 + m._12 * m._21 * m._43 + m._13 * m._22 * m._41 - m._11 * m._22 * m._43 - m._12 * m._23 * m._41 - m._13 * m._21 * m._42) * i_det;
	result._44 = (m._11 * m._22 * m._33 + m._12 * m._23 * m._31 + m._13 * m._21 * m._32 - m._11 * m._23 * m._32 - m._12 * m._21 * m._33 - m._13 * m._22 * m._31) * i_det;

#ifdef  DO_SANITY_TESTS
	if (result * m != mat4()) {
		std::cout << "ERROR! Expecting matrix x inverse to equal identity!\n";
	}
#endif

	return result;
}

mat4 ToColumnMajor(const mat4& mat) {
	return Transpose(mat);
}

mat3 ToColumnMajor(const mat3& mat) {
	return Transpose(mat);
}

mat4 FromColumnMajor(const mat4& mat) {
	return Transpose(mat);
}

mat3 FromColumnMajor(const mat3& mat) {
	return Transpose(mat);
}

mat4 FromColumnMajor(const float* mat) {
	return Transpose(mat4(
		mat[0],  mat[1],  mat[2],  mat[3],
		mat[4],  mat[5],  mat[6],  mat[7],
		mat[8],  mat[9],  mat[10], mat[11],
		mat[12], mat[13], mat[14], mat[15]
	));
}

mat4 Translation(float x, float y, float z) {
	return mat4(
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, 1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		   x,    y,    z, 1.0f
	);
}
mat4 Translation(const vec3& pos) {
	return mat4(
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, 1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		pos.x,pos.y,pos.z,1.0f
	);
}

vec3 GetTranslation(const mat4& mat) {
	return vec3(mat._41, mat._42, mat._43);
}

mat4 Scale(float x, float y, float z) {
	return mat4(
		   x, 0.0f, 0.0f, 0.0f,
		0.0f,    y, 0.0f, 0.0f,
		0.0f, 0.0f,    z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

mat4 Scale(const vec3& vec) {
	return mat4(
		vec.x,0.0f, 0.0f, 0.0f,
		0.0f, vec.y,0.0f, 0.0f,
		0.0f, 0.0f, vec.z,0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

vec3 GetScale(const mat4& mat) {
	return vec3(mat._11, mat._22, mat._33);
}

mat4 Rotation(float pitch, float yaw, float roll) {
	return  ZRotation(roll) * XRotation(pitch) * YRotation(yaw);
}

mat3 Rotation3x3(float pitch, float yaw, float roll) {
	return ZRotation3x3(roll) * XRotation3x3(pitch) * YRotation3x3(yaw);
}

#ifndef NO_EXTRAS
mat2 Rotation2x2(float angle) {
	return mat2(
		cosf(angle), sinf(angle),
		-sinf(angle), cosf(angle)
		);
}

mat4 YawPitchRoll(float yaw, float pitch, float roll) {
	yaw = DEG2RAD(yaw);
	pitch = DEG2RAD(pitch);
	roll = DEG2RAD(roll);

	mat4 out; // z * x * y
	out._11 = (cosf(roll) * cosf(yaw)) + (sinf(roll) * sinf(pitch) * sinf(yaw));
	out._12 = (sinf(roll) * cosf(pitch));
	out._13 = (cosf(roll) * -sinf(yaw)) + (sinf(roll) * sinf(pitch) * cosf(yaw));
	out._21 = (-sinf(roll) * cosf(yaw)) + (cosf(roll) * sinf(pitch) * sinf(yaw));
	out._22 = (cosf(roll) * cosf(pitch));
	out._23 = (sinf(roll) * sinf(yaw)) + (cosf(roll) * sinf(pitch) * cosf(yaw));
	out._31 = (cosf(pitch) * sinf(yaw));
	out._32 = -sinf(pitch);
	out._33 = (cosf(pitch) * cosf(yaw));
	out._44 = 1;
	return out;
}
#endif

mat4 XRotation(float angle) {
	angle = DEG2RAD(angle);
	return mat4(
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, cosf(angle), sinf(angle), 0.0f,
		0.0f, -sinf(angle), cosf(angle), 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

mat3 XRotation3x3(float angle) {
	angle = DEG2RAD(angle);
	return mat3(
		1.0f, 0.0f, 0.0f,
		0.0f, cosf(angle), sinf(angle),
		0.0f, -sinf(angle), cosf(angle)
	);
}

mat4 YRotation(float angle) {
	angle = DEG2RAD(angle);
	return mat4(
		cosf(angle), 0.0f, -sinf(angle), 0.0f,
		0.0f, 1.0f, 0.0f, 0.0f,
		sinf(angle), 0.0f, cosf(angle), 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

mat3 YRotation3x3(float angle) {
	angle = DEG2RAD(angle);
	return mat3(
		cosf(angle), 0.0f, -sinf(angle),
		0.0f, 1.0f, 0.0f,
		sinf(angle), 0.0f, cosf(angle)
	);
}

mat4 ZRotation(float angle) {
	angle = DEG2RAD(angle);
	return mat4(
		cosf(angle), sinf(angle), 0.0f, 0.0f,
		-sinf(angle), cosf(angle), 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

mat3 ZRotation3x3(float angle) {
	angle = DEG2RAD(angle);
	return mat3(
		cosf(angle), sinf(angle), 0.0f,
		-sinf(angle), cosf(angle), 0.0f,
		0.0f, 0.0f, 1.0f
	);
}

#ifndef NO_EXTRAS
mat4 Orthogonalize(const mat4& mat) {
	vec3 xAxis(mat._11, mat._12, mat._13);
	vec3 yAxis(mat._21, mat._22, mat._23);
	vec3 zAxis = Cross(xAxis, yAxis);

	xAxis = Cross(yAxis, zAxis);
	yAxis = Cross(zAxis, xAxis);
	zAxis = Cross(xAxis, yAxis);

	return mat4(
		xAxis.x, xAxis.y, xAxis.z, mat._14,
		yAxis.x, yAxis.y, yAxis.z, mat._24,
		zAxis.x, zAxis.y, zAxis.z, mat._34,
		mat._41, mat._42, mat._43, mat._44
	);
}

mat3 Orthogonalize(const mat3& mat) {
	vec3 xAxis(mat._11, mat._12, mat._13);
	vec3 yAxis(mat._21, mat._22, mat._23);
	vec3 zAxis = Cross(xAxis, yAxis);

	xAxis = Cross(yAxis, zAxis);
	yAxis = Cross(zAxis, xAxis);
	zAxis = Cross(xAxis, yAxis);

	return mat3(
		xAxis.x, xAxis.y, xAxis.z,
		yAxis.x, yAxis.y, yAxis.z,
		zAxis.x, zAxis.y, zAxis.z
	);
}
#endif

mat4 AxisAngle(const vec3& axis, float angle) {
	angle = DEG2RAD(angle);
	float c = cosf(angle);
	float s = sinf(angle);
	float t = 1.0f - cosf(angle);

	float x = axis.x, y = axis.y, z = axis.z;
	if (!CMP(MagnitudeSq(axis), 1.0f)) {
		float inv_len = 1.0f / Magnitude(axis);
		x *= inv_len;
		y *= inv_len;
		z *= inv_len;
	}

	return mat4(
		t * (x * x) + c, t * x * y + s * z, t * x * z - s * y, 0.0f,
		t * x * y - s * z, t * (y * y) + c, t * y * z + s * x, 0.0f,
		t * x * z + s * y, t * y * z - s * x, t * (z * z) + c, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);
}

mat3 AxisAngle3x3(const vec3& axis, float angle) {
	angle = DEG2RAD(angle);
	float c = cosf(angle);
	float s = sinf(angle);
	float t = 1.0f - cosf(angle);

	float x = axis.x;
	float y = axis.y;
	float z = axis.z;
	if (!CMP(MagnitudeSq(axis), 1.0f)) {
		float inv_len = 1.0f / Magnitude(axis);
		x *= inv_len;
		y *= inv_len;
		z *= inv_len;
	}

	return mat3(
		t * (x * x) + c, t * x * y + s * z, t * x * z - s * y, 
		t * x * y - s * z, t * (y * y) + c, t * y * z + s * x, 
		t * x * z + s * y, t * y * z - s * x, t * (z * z) + c
	);
}

vec3 MultiplyPoint(const vec3& vec, const mat4& mat) {
	vec3 result;
	result.x = vec.x * mat._11 + vec.y * mat._21 + vec.z * mat._31 + 1.0f * mat._41;
	result.y = vec.x * mat._12 + vec.y * mat._22 + vec.z * mat._32 + 1.0f * mat._42;
	result.z = vec.x * mat._13 + vec.y * mat._23 + vec.z * mat._33 + 1.0f * mat._43;
	return result;
}

vec3 MultiplyVector(const vec3& vec, const mat4& mat) {
	vec3 result;
	result.x = vec.x * mat._11 + vec.y * mat._21 + vec.z * mat._31 + 0.0f * mat._41;
	result.y = vec.x * mat._12 + vec.y * mat._22 + vec.z * mat._32 + 0.0f * mat._42;
	result.z = vec.x * mat._13 + vec.y * mat._23 + vec.z * mat._33 + 0.0f * mat._43;
	return result;
}

vec3 MultiplyVector(const vec3& vec, const mat3& mat) {
	vec3 result;
	result.x = Dot(vec, vec3{ mat._11, mat._21, mat._31 });
	result.y = Dot(vec, vec3{ mat._12, mat._22, mat._32 });
	result.z = Dot(vec, vec3{ mat._13, mat._23, mat._33 });
	return result;
}

mat4 Transform(const vec3& scale, const vec3& eulerRotation, const vec3& translate) {
	return Scale(scale) *
		Rotation(eulerRotation.x, eulerRotation.y, eulerRotation.z) *
		Translation(translate);
}

mat4 Transform(const vec3& scale, const vec3& rotationAxis, float rotationAngle, const vec3& translate) {
	return Scale(scale) * 
		AxisAngle(rotationAxis, rotationAngle) * 
		Translation(translate);
}

mat4 LookAt(const vec3& position, const vec3& target, const vec3& up) {
	vec3 forward = Normalized(target - position);
	vec3 right = Normalized(Cross(up, forward));
	vec3 newUp = Cross(forward, right);

#ifdef DO_SANITY_TESTS
	mat4 viewPosition = Translation(position);
	mat4 viewOrientation = mat4(
		right.x, right.y, right.z, 0.0f,
		newUp.x, newUp.y, newUp.z, 0.0f,
		forward.x, forward.y, forward.z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	);

	mat4 view = Inverse(viewPosition) * Inverse(viewOrientation);
	mat4 result =
#else
	return
#endif
		mat4(
		right.x, newUp.x, forward.x, 0.0f,
		right.y, newUp.y, forward.y, 0.0f,
		right.z, newUp.z, forward.z, 0.0f,
		-Dot(right, position), -Dot(newUp, position), -Dot(forward, position), 1.0f
	);
#ifdef DO_SANITY_TESTS
	if (result != view) {
		std::cout << "Error, result and view do not match in an expected manner!\n";
		std::cout << "view: \n" << view << "\n\n";
		std::cout << "result: \n" << result << "\n\n";
	}
	return result;
#endif
}

mat4 Projection(float fov, float aspect, float zNear, float zFar) {
	float tanHalfFov = tanf(DEG2RAD((fov * 0.5f)));

	mat4 result; // There are MANY different ways to derive a projection matrix!

#if 0
		result._11 = 1.0f / (aspect * tanHalfFov);
		result._22 = 1.0f / tanHalfFov;
		result._33 = (-zNear - zFar) / (zNear - zFar);
		result._44 = 0.0f;
		result._34 = 1.0f;
		result._43 = (2.0f * zFar * zNear) / (zNear - zFar); 
#else
		float fovY = 1.0f / tanHalfFov; // cot(fov/2)
		float fovX = fovY / aspect; // cot(fov/2) / aspect

		result._11 = fovX;
		result._22 = fovY;
		result._33 = zFar / (zFar - zNear); // far / range
		result._34 = 1.0f;
		result._43 = -zNear * result._33; // - near * (far / range)
		result._44 = 0.0f;
#endif

	return result;
}

mat4 Ortho(float left, float right, float bottom, float top, float zNear, float zFar) {
	float w = right - left;
	float h = bottom - top;

	return mat4(
		w * 0.5f, 0.0f, 0.0f, 0.0f,
		0.0f, h * 0.5f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f / (zFar - zNear), 0.0f,
		0.0f, 0.0f, zNear / (zNear - zFar), 1.0f
	);
}

<|start_filename|>Chapter01/Code/DistanceJoint.h<|end_filename|>
#ifndef _H_DISTANCE_JOINT
#define _H_DISTANCE_JOINT

#include "Particle.h"

class DistanceJoint : public Rigidbody {
protected:
	Particle* p1;
	Particle* p2;
	float length;
public:
	void Initialize(Particle* _p1, Particle* _p2, float len);
	void SolveConstraints(const std::vector<OBB>& constraints);
	void Render();
};

#endif


<|start_filename|>Chapter01/Code/LinearImpulse.h<|end_filename|>
#ifndef _H_LINEAR_IMPULSE_
#define _H_LINEAR_IMPULSE_

#include "DemoBase.h"
#include "Geometry3D.h"
#include "PhysicsSystem.h"
#include "RigidbodyVolume.h"

class LinearImpulse : public DemoBase {
protected:
	PhysicsSystem physicsSystem;
	std::vector<RigidbodyVolume> bodies;
	RigidbodyVolume groundBox;

	bool size_imgui_window;
	bool isPaused;
	int numSteps;
	int stepSize;
	bool use_spheres;
	bool drop;
protected:
	void ResetDemo();
	float Random(float min, float max);
	vec3 Random(vec3 min, vec3 max);
public:
	void Initialize(int width, int height);
	void Render();
	void Update(float dt);
	void ImGUI();
};


#endif


<|start_filename|>Chapter03/Code/PhysicsSystem.cpp<|end_filename|>
#include "PhysicsSystem.h"
#include "FixedFunctionPrimitives.h"
#include "glad/glad.h"

void PhysicsSystem::Update(float deltaTime) {
	// Add forces
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->ApplyForces();
	}

	// Update object positions
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->Update(deltaTime);
	}

	// Solve constraints
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->SolveConstraints(constraints);
	}
}

void PhysicsSystem::Render() {
	static const float rigidbodyDiffuse[]{ 200.0f / 255.0f, 0.0f, 0.0f, 0.0f };
	static const float rigidbodyAmbient[]{ 200.0f / 255.0f, 50.0f / 255.0f, 50.0f / 255.0f, 0.0f };

	static const float groundDiffuse[]{ 0.0f, 0.0f, 200.0f / 255.0f, 0.0f };
	static const float groundAmbient[]{ 50.0f / 255.0f, 50.0f / 255.0f, 200.0f / 255.0f, 0.0f };

	static const float constraintDiffuse[]{ 0.0f, 200.0f / 255.0f, 0.0f, 0.0f };
	static const float constraintAmbient[]{ 50.0f / 255.0f, 200.0f / 255.0f, 50.0f / 255.0f, 0.0f };
	
	static const float zero[] = { 0.0f, 0.0f, 0.0f, 0.0f };


	glColor3f(rigidbodyDiffuse[0], rigidbodyDiffuse[1], rigidbodyDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, rigidbodyAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, rigidbodyDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	for (int i = 0, size = bodies.size(); i < size; ++i) {
		bodies[i]->Render();
	}

	// First constraint is usually the ground
	// Rendering it diferent color is just a hack to make visualization easyer
	// Normally, you wouldn't even render physics geo!
	if (constraints.size() > 0) {
		glColor3f(groundDiffuse[0], groundDiffuse[1], groundDiffuse[2]);
		glLightfv(GL_LIGHT0, GL_AMBIENT, groundAmbient);
		glLightfv(GL_LIGHT0, GL_DIFFUSE, groundDiffuse);
		glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
		::Render(constraints[0]);
	}

	glColor3f(constraintDiffuse[0], constraintDiffuse[1], constraintDiffuse[2]);
	glLightfv(GL_LIGHT0, GL_AMBIENT, constraintAmbient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, constraintDiffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, zero);
	for (int i = 1, size = constraints.size(); i < size; ++i) {
		::Render(constraints[i]);
	}
}

void PhysicsSystem::AddRigidbody(Rigidbody* body) {
	bodies.push_back(body);
}

void PhysicsSystem::AddConstraint(const OBB& obb) {
	constraints.push_back(obb);
}

void PhysicsSystem::ClearRigidbodys() {
	bodies.clear();
}

void PhysicsSystem::ClearConstraints() {
	constraints.clear();
}

<|start_filename|>Chapter03/Code/Rigidbody.h<|end_filename|>
#ifndef _H_RIGIDBODY_
#define _H_RIGIDBODY_

#include <vector>
#include "Geometry3D.h"

class Rigidbody {
public:
	Rigidbody() { }
	virtual ~Rigidbody() { }

	virtual void Update(float deltaTime) { }
	virtual void Render() { }
	virtual void ApplyForces() { }
	virtual void SolveConstraints(const std::vector<OBB>& constraints) { }
};

#endif


<|start_filename|>Chapter01/Code/Spring.cpp<|end_filename|>
#include "Spring.h"

Spring::Spring(float _k, float _b, float len) {
	k = _k;
	b = _b;
	restingLength = len;
}

void Spring::SetParticles(Particle* _p1, Particle* _p2) {
	p1 = _p1;
	p2 = _p2;
}

Particle* Spring::GetP1() {
	return p1;
}

Particle* Spring::GetP2() {
	return p2;
}

void Spring::SetConstants(float _k, float _b) {
	k = _k;
	b = _b;
}

void Spring::ApplyForce(float dt) {
	vec3 relPos = p2->GetPosition() - p1->GetPosition();
	vec3 relVel = p2->GetVelocity() - p1->GetVelocity();

	// Prevent underflow
	for (int i = 0; i < 3; ++i) {
		relPos[i] = (fabsf(relPos[i]) < 0.0000001f) ? 0.0f : relPos[i];
		relVel[i] = (fabsf(relVel[i]) < 0.0000001f) ? 0.0f : relVel[i];
	}

	float x = Magnitude(relPos) - restingLength;
	float v = Magnitude(relVel);

	float F = (-k * x) + (-b * v);

	vec3 impulse = Normalized(relPos) * F;
	p1->AddImpulse(impulse * p1->InvMass());
	p2->AddImpulse(impulse*  -1.0f * p2->InvMass());
}