Physics Code:

Box Class cpp

#include "BoxClass.h"

BoxClass::BoxClass(glm::vec3 position, glm::vec3 extents, glm::vec3 velocity, float mass, glm::vec4 colour)
{
	m_extents = extents;
	m_Position = position;
	m_Mass = mass;
	m_Velocity = velocity;
	m_ShapeID = BOX;
}

BoxClass::BoxClass() {};

void BoxClass::makeGizmo()
{
	if (isObjectStatic == false)
	{
		Gizmos::addAABBFilled(m_Position, m_extents, glm::vec4(0, 0.5f, 0, 1));
	}
	else if (isObjectStatic == true)
	{
		Gizmos::addAABBFilled(m_Position, m_extents, glm::vec4(1, 0, 0, 1));
	}
}

BoxClass::~BoxClass() {}

Box Class Header

#pragma once
#include "RigidBodyClass.h"
#include "PhysicObjs.h"
#include "Gizmos.h"

class BoxClass : public RigidBodyClass
{
public:
	BoxClass(glm::vec3 position, glm::vec3 extents, glm::vec3 velocity, float mass, glm::vec4 colour);
	BoxClass();
	~BoxClass();

	glm::vec3 GetExtents() const { return m_extents; }
	void virtual makeGizmo() override;

private:
	glm::vec3 m_extents;
};

Camera cpp

#include "Camera.h"

#include "GLFW/glfw3.h"

#include "glm/ext.hpp"

Camera::Camera(float aspect)
{
	view = glm::lookAt(vec3(0,0,0), vec3(0,0,1), vec3(0, 1, 0));
	world = glm::inverse(view);
	proj = glm::perspective(glm::radians(60.0f), aspect, 0.1f, 1000.0f);
	view_proj = proj * view;
}

void Camera::updateViewProj()
{
	view_proj = proj * view;
}

void Camera::setPerspective(float fov, float aspect, float near, float far)
{
	proj = glm::perspective(fov, aspect, near, far);
	updateViewProj();
}

void Camera::setLookAt(vec3 pos, vec3 center, vec3 up)
{
	view = glm::lookAt(pos, center, up);
	world = glm::inverse(view);
	updateViewProj();
}

void Camera::setPosition(vec3 pos)
{
	world[3] = vec4(pos, 1);
	view = glm::inverse(world);
	updateViewProj();
}

vec3 Camera::pickAgainstPlane(float x, float y, vec4 plane)
{
    float nxPos = x / 1280.0f; //replace these with your screen width and height
    float nyPos = y / 720.0f;

    float sxPos = nxPos - 0.5f;
    float syPos = nyPos - 0.5f;

    float fxPos = sxPos * 2;
    float fyPos = syPos * -2;

    mat4 inv_viewproj = glm::inverse(view_proj); //view_proj is the memeber variable

    vec4 mouse_pos(fxPos, fyPos, 1, 1);
    vec4 world_pos = inv_viewproj * mouse_pos;

    world_pos /= world_pos.w;

    vec3 cam_pos = world[3].xyz(); //world is the member variable
    vec3 dir = world_pos.xyz() - cam_pos;

    float t = -(glm::dot(cam_pos, plane.xyz()) + plane.w)
        / (glm::dot(dir, plane.xyz()));

    vec3 result = cam_pos + dir * t;

    return result;
}

FlyCamera::FlyCamera(float aspect, float new_speed) : Camera(aspect)
{
	this->m_speed = new_speed;
	m_clicked_down = false;
	yaw = 0;
	pitch = 0;
}

void FlyCamera::update(float dt)
{
	GLFWwindow* curr_window = glfwGetCurrentContext();

	float speed = m_speed;

	if (glfwGetKey(curr_window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS)
	{
		speed *= 3.0f;
	}

	if (glfwGetKey(curr_window, GLFW_KEY_W) == GLFW_PRESS)
	{
		world[3] -= world[2] * speed * dt;
	}
	if (glfwGetKey(curr_window, GLFW_KEY_S) == GLFW_PRESS)
	{
		world[3] += world[2] * speed * dt;
	}
	if (glfwGetKey(curr_window, GLFW_KEY_A) == GLFW_PRESS)
	{
		world[3] -= world[0] * speed * dt;
	}
	if (glfwGetKey(curr_window, GLFW_KEY_D) == GLFW_PRESS)
	{
		world[3] += world[0] * speed * dt;
	}
	if (glfwGetKey(curr_window, GLFW_KEY_Q) == GLFW_PRESS)
	{
		world[3] -= world[1] * speed * dt;
	}
	if (glfwGetKey(curr_window, GLFW_KEY_E) == GLFW_PRESS)
	{
		world[3] += world[1] * speed * dt;
	}
	

	if (glfwGetMouseButton(curr_window, 1) == GLFW_PRESS)
	{
		double x_delta, y_delta;



		glfwGetCursorPos(curr_window, &x_delta, &y_delta);

		glfwSetCursorPos(curr_window, 1280.f / 2.f, 720.f / 2.f);

		double post_set_x, post_set_y;

		glfwGetCursorPos(curr_window, &post_set_x, &post_set_y);
		if (post_set_x == 1280.0f / 2.0f && post_set_y == 720.0f / 2.0f)
		{
			if (m_clicked_down)
			{
				x_delta -= (1280.f / 2.f);
				y_delta -= (720.f / 2.f);

				x_delta /= (1280.f / 2.f);
				y_delta /= (720.f / 2.f);

				x_delta *= -sensitivity;
				y_delta *= -sensitivity;

				yaw += (float)x_delta;
				pitch += (float)y_delta;

				if (pitch >= glm::radians(89.f))
				{
					pitch = glm::radians(89.f);
				}
				if (pitch <= glm::radians(-89.f))
				{
					pitch = glm::radians(-89.f);
				}

					mat4 pitch_mat = glm::rotate((pitch), vec3(1, 0, 0));
					mat4 yaw_mat = glm::rotate((yaw), vec3(0, 1, 0));

					mat4 transform = yaw_mat * pitch_mat;

					transform[3] = world[3];
					world = transform;
					view = glm::inverse(world);
					view_proj = proj * view;
			}
			m_clicked_down = true;
		}
	}
	else
	{
		m_clicked_down = false;
	}

	view = glm::inverse(world);
	updateViewProj();
}

Camera Header

#ifndef CAMERA_H_
#define CAMERA_H_

#define GLM_SWIZZLE
#include "glm/glm.hpp"

using glm::vec3;
using glm::vec4;
using glm::mat4;


class Camera
{
public:
	Camera(){}
	Camera(float aspect);

	void updateViewProj();
	void setPosition(vec3 pos);
	void setPerspective(float fov, float aspect, float near, float far);
	void setLookAt(vec3 pos, vec3 center, vec3 up);

	virtual void update(float dt) = 0;

    vec3 pickAgainstPlane(float x, float y, vec4 plane);

	mat4 world;
	mat4 view;
	mat4 proj;
	mat4 view_proj;
};

class FlyCamera : public Camera
{
public:
	FlyCamera(){}
	FlyCamera(float aspect, float speed);
	virtual void update(float dt);

	float m_speed;
	float sensitivity;

	float yaw;
	float pitch;

	bool m_clicked_down;
};

#endif //CAMERA_H_

Particle Emitter cpp

#include "ParticleEmitter.h"
#include <iostream>
#include<vector>

#define GLM_SWIZZLE
#include "glm/glm.hpp"
#include "glm/ext.hpp"
#include "Gizmos.h"


//constructor
ParticleEmitter::ParticleEmitter(int _maxParticles, PxVec3 _position,PxParticleSystem* _ps,float _releaseDelay)
{
	m_releaseDelay		= _releaseDelay;
	//maximum number of particles our emitter can handle
	m_maxParticles		= _maxParticles; 

	//array of particle structs
	m_activeParticles	= new Particle[m_maxParticles]; 
	
	m_time				= 0; //time system has been running
	m_respawnTime		= 0; //time for next respawn
	m_position			= _position;
	m_ps				=_ps; //pointer to the physX particle system
	m_particleMaxAge	= 8; //maximum time in seconds that a particle can live for
	
	//initialize the buffer
	for(int index=0;index<m_maxParticles;index++)
	{
		m_activeParticles[index].active = false;
		m_activeParticles[index].maxTime = 0;
	}

	m_minVelocity = PxVec3(-10.0f, 0, -10.0f);
	m_maxVelocity = PxVec3(10.0f, 0, 10.0f);
}

//destructure
ParticleEmitter::~ParticleEmitter()
{
	//remove all the active particles
	delete m_activeParticles;
}

void ParticleEmitter::setStartVelocityRange(float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
{
	m_minVelocity.x = minX;
	m_minVelocity.y = minY;
	m_minVelocity.z = minZ;
	m_maxVelocity.x = maxX;
	m_maxVelocity.y = maxY;
	m_maxVelocity.z = maxZ;
}

//find the next free particle, mark it as used and return it's index.  If it can't allocate a particle: returns minus one
int ParticleEmitter::getNextFreeParticle()
{
	//find particle, this is a very inefficient way to do this.  A better way would be to keep a list of free particles so we can quickly find the first free one
	for(int index=0;index<m_maxParticles;index++)
	{
		//when we find a particle which is free
		if(!m_activeParticles[index].active)
		{
			m_activeParticles[index].active = true; //mark it as not free
			m_activeParticles[index].maxTime = m_time+m_particleMaxAge;  //record when the particle was created so we know when to remove it
			return index;
		}
	}
	return -1; //returns minus if a particle was not allocated
}


//releast a particle from the system using it's index to ID it
void ParticleEmitter::releaseParticle(int index)
{
	if(index >= 0 && index < m_maxParticles)
		m_activeParticles[index].active = false;
}

//returns true if a particle age is greater than it's maximum allowed age
bool ParticleEmitter::tooOld(int index)
{
	if(index >= 0 && index < m_maxParticles && m_time > m_activeParticles[index].maxTime)
		return true;
	return false;
}

//add particle to PhysX System
bool ParticleEmitter::addPhysXParticle(int particleIndex)
{

	//set up the data
	//set up the buffers
	PxU32 myIndexBuffer[] = {(PxU32)particleIndex};
	PxVec3 startPos = m_position;
	PxVec3 startVel(0,0,0);
	//randomize starting velocity.
	float fT = (rand() % (RAND_MAX + 1)) / (float)RAND_MAX;
	startVel.x += m_minVelocity.x + (fT * (m_maxVelocity.x - m_minVelocity.x));
	fT = (rand() % (RAND_MAX + 1)) / (float)RAND_MAX;
	startVel.y += m_minVelocity.y + (fT * (m_maxVelocity.y - m_minVelocity.y));
	fT = (rand() % (RAND_MAX + 1)) / (float)RAND_MAX;
	startVel.z += m_minVelocity.z + (fT * (m_maxVelocity.z - m_minVelocity.z));

	//we can change starting position tos get different emitter shapes
	PxVec3 myPositionBuffer[] = {startPos};
	PxVec3 myVelocityBuffer[] =  {startVel};

	//reserve space for data
	PxParticleCreationData particleCreationData;
	particleCreationData.numParticles = 1;  //spawn one particle at a time,  this is inefficient and we could improve this by passing in the list of particles.
	particleCreationData.indexBuffer = PxStrideIterator<const PxU32>(myIndexBuffer);
	particleCreationData.positionBuffer = PxStrideIterator<const PxVec3>(myPositionBuffer);
	particleCreationData.velocityBuffer = PxStrideIterator<const PxVec3>(myVelocityBuffer);
	// create particles in *PxParticleSystem* ps
	return m_ps->createParticles(particleCreationData);
}

//updateParticle
void ParticleEmitter::update(float delta)
{
	//tick the emitter
	m_time += delta;
	m_respawnTime+= delta;
	int numberSpawn = 0;
	//if respawn time is greater than our release delay then we spawn at least one particle so work out how many to spawn
	if(m_respawnTime>m_releaseDelay)
	{
		numberSpawn = (int)(m_respawnTime/m_releaseDelay);
		m_respawnTime -= (numberSpawn * m_releaseDelay);
	}
	// spawn the required number of particles 
	for(int count = 0;count < numberSpawn;count++)
	{
		//get the next free particle
		int particleIndex = getNextFreeParticle();
		if(particleIndex >=0)
		{
			//if we got a particle ID then spawn it
			addPhysXParticle(particleIndex);
		}
	}
	//check to see if we need to release particles because they are either too old or have hit the particle sink
	//lock the particle buffer so we can work on it and get a pointer to read data
	PxParticleReadData* rd = m_ps->lockParticleReadData();
	// access particle data from PxParticleReadData was OK
	if (rd)
	{
		vector<PxU32> particlesToRemove; //we need to build a list of particles to remove so we can do it all in one go
		PxStrideIterator<const PxParticleFlags> flagsIt(rd->flagsBuffer);

		for (unsigned i = 0; i < rd->validParticleRange; ++i, ++flagsIt)
		{
			if (*flagsIt & PxParticleFlag::eVALID)
				{
					//if particle is either too old or has hit the sink then mark it for removal.  We can't remove it here because we buffer is locked
					if (*flagsIt & PxParticleFlag::eCOLLISION_WITH_DRAIN || tooOld(i))
					{
						//mark our local copy of the particle free
						releaseParticle(i);
						//add to our list of particles to remove
						particlesToRemove.push_back(i);
					}
				}
		}
		// return ownership of the buffers back to the SDK
		rd->unlock();
		//if we have particles to release then pass the particles to remove to PhysX so it can release them
		if(particlesToRemove.size()>0)
		{
			//create a buffer of particle indicies which we are going to remove
			PxStrideIterator<const PxU32> indexBuffer(&particlesToRemove[0]);
			//free particles from the physics system
			m_ps->releaseParticles(particlesToRemove.size(), indexBuffer);
		}
	}
}

//simple routine to render our particles
void ParticleEmitter::renderParticles()
{
	// lock SDK buffers of *PxParticleSystem* ps for reading
	PxParticleReadData* rd = m_ps->lockParticleReadData();
	// access particle data from PxParticleReadData
	if (rd)
	{
		PxStrideIterator<const PxParticleFlags> flagsIt(rd->flagsBuffer);
		PxStrideIterator<const PxVec3> positionIt(rd->positionBuffer);

		for (unsigned i = 0; i < rd->validParticleRange; ++i, ++flagsIt, ++positionIt)
		{
				if (*flagsIt & PxParticleFlag::eVALID)
				{
						//convert physx vector to a glm vec3
						glm::vec3 pos(positionIt->x,positionIt->y,positionIt->z);
						//use a gizmo box to visualize particle.  This would be much better done using a facing quad preferably done using the geometry shader
						Gizmos::addAABBFilled(pos,glm::vec3(.1,.1,.1),glm::vec4(1,0,1,1));
				}
		}
		// return ownership of the buffers back to the SDK
		rd->unlock();
	}
}

Particle Emitter Header

#pragma once
#include <PxPhysicsAPI.h>
using namespace physx;
using namespace std;

//simple struct for our particles

struct Particle
{
	bool active;
	float maxTime;
};


//simple class for particle emitter.  For a real system we would make this a base class and derive different emitters from it by making functions virtual and overloading them.
class ParticleEmitter
{
public:
	ParticleEmitter(int _maxParticles,PxVec3 _position,PxParticleSystem* _ps,float _releaseDelay);
	~ParticleEmitter();
	

	void setStartVelocityRange(float minX, float minY, float minZ, float maxX, float maxY, float maxZ);
	void update(float delta);
	void releaseParticle(int);
	bool tooOld(int);
	void renderParticles();

private:

	int getNextFreeParticle();
	bool addPhysXParticle(int particleIndex);

	int			m_maxParticles;
	Particle*	m_activeParticles;
	float		m_releaseDelay;
	int			m_numberActiveParticles;

	float		m_time;
	float		m_respawnTime;
	float		m_particleMaxAge;
	PxVec3		m_position;
	
	PxParticleSystem* m_ps;
	
	
	PxVec3 m_minVelocity;
	PxVec3 m_maxVelocity;
};

Physics cpp

//============================================//
#include "Physics.h"
#include "gl_core_4_4.h"
#include "GLFW/glfw3.h"
#include "Gizmos.h"
#include "glm/ext.hpp"
#include "glm/gtc/quaternion.hpp"
#include "PlaneClass.h"
#include "BoxClass.h"
#include "SpringClass.h"
//============================================//
#include <PxPhysicsAPI.h>
#include <PxScene.h>
#include "RagDolls.h"
//============================================//
#define Assert(val) if (val){}else {*((char*)0) = 0;}
#define ArrayCount(val) (sizeof(val)/sizeof(val[0]))
ParticleFluidEmitter *m_particleEmitter;
bool ChangeColour;
//============================================//
bool Physics::startup()
{
	physicScene = new PhysicScene();
    if (Application::startup() == false) {
        return false; }
	
	PhysicsSceneFirst();
    glClearColor(0.3f, 0.3f, 0.3f, 1.0f);
    glEnable(GL_DEPTH_TEST);
    Gizmos::create();
	// Camera
    m_camera = FlyCamera(1280.0f / 720.0f, 10.0f);
    m_camera.setLookAt(vec3(20, 50, 15), vec3(0, 0, 15), vec3(0, 1, 0));
    m_camera.sensitivity = 3;
	// Gun Bool
	isReadyToShoot = false;
	//---------------------------------------------------//
	ChangeColour = false;
	isPressed = false;
	// Physx
	setUpPhysx();
	setUpVisualDebug();
	setupTransforms();
	//Collision CallBack
	PxSimulationEventCallback* myCollisionCallBack = new CollisionCallBack();
	m_PhysicsScene->setSimulationEventCallback(myCollisionCallBack);
	m_particleEmitter->Colour = glm::vec4(1, 0, 1, 1);
	//---------------------------------------------------//
	SphereHeight = 1.3f;
	m_renderer = new Renderer();
    return true;
}

void Physics::shutdown()
{
	//------------------------------//
	// Physx
	m_PhysicsScene->release();
	m_Physics->release();
	m_PhysicsFoundation->release();
	//------------------------------//
	delete m_renderer;
    Gizmos::destroy();
    Application::shutdown();
}

void Physics::PhysicsSceneFirst()
{
	physicScene->gravity = glm::vec3(0, -10, 0);
	//============================================//
	// Spring Spheres 
	//-----------------------// Set 1
	SphereObjClass* ball1;
	ball1 = new SphereObjClass(glm::vec3(0, 14, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	ball1->isObjectStatic = true;
	physicScene->addActor(ball1);
	SphereObjClass* ball2;
	ball2 = new SphereObjClass(glm::vec3(0, 10, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball2);
	//-----------------------// Set 2
	SphereObjClass* ball4;
	ball4 = new SphereObjClass(glm::vec3(10, 14, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	ball4->isObjectStatic = true;
	physicScene->addActor(ball4);
	SphereObjClass* ball5;
	ball5 = new SphereObjClass(glm::vec3(10, 10, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball5);
	//-----------------------// Set 3
	SphereObjClass* ball6;
	ball6 = new SphereObjClass(glm::vec3(-10, 14, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	ball6->isObjectStatic = true;
	physicScene->addActor(ball6);
	SphereObjClass* ball7;
	ball7 = new SphereObjClass(glm::vec3(-10, 10, 0), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball7);
	//-----------------------// Set 4
	SphereObjClass* ball9;
	ball9 = new SphereObjClass(glm::vec3(0, 14, 10), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	ball9->isObjectStatic = true;
	physicScene->addActor(ball9);
	SphereObjClass* ball10;
	ball10 = new SphereObjClass(glm::vec3(0, 10, 10), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball10);
	//-----------------------// Set 5
	SphereObjClass* ball11;
	ball11 = new SphereObjClass(glm::vec3(0, 14, -10), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	ball11->isObjectStatic = true;
	physicScene->addActor(ball11);
	SphereObjClass* ball12;
	ball12 = new SphereObjClass(glm::vec3(0, 10, -10), glm::vec3(0, 0, 0), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball12);
	// Sphere Objects
	SphereObjClass* ball8;
	ball8 = new SphereObjClass(glm::vec3(0, 5, 6), glm::vec3(0, 0, -3), 3.0f, 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(ball8);
	//============================================//
	// Plane Object
	PlaneClass* plane;
	plane = new PlaneClass(glm::normalize(glm::vec3(0, 1, 0)), 1.5f);
	physicScene->addActor(plane);
	//============================================//
	// Map Walls - Static
	BoxClass* Wall1;
	Wall1 = new BoxClass(glm::vec3(12, 4, 0), glm::vec3(1, 10, 13), glm::vec3(0, 0, 0), 1, glm::vec4(1, 0, 0, 1));
	Wall1->isObjectStatic = true;
	physicScene->addActor(Wall1);
	BoxClass* Wall2;
	Wall2 = new BoxClass(glm::vec3(0, 4, 12), glm::vec3(13, 10, 1), glm::vec3(0, 0, 0), 1, glm::vec4(1, 0, 0, 1));
	Wall2->isObjectStatic = true;
	physicScene->addActor(Wall2);
	BoxClass* Wall3;
	Wall3 = new BoxClass(glm::vec3(-12, 4, 0), glm::vec3(1, 10, 13), glm::vec3(0, 0, 0), 1, glm::vec4(1, 0, 0, 1));
	Wall3->isObjectStatic = true;
	physicScene->addActor(Wall3);
	BoxClass* Wall4;
	Wall4 = new BoxClass(glm::vec3(0, 4, -12), glm::vec3(13, 10, 1), glm::vec3(0, 0, 0), 1, glm::vec4(1, 0, 0, 1));
	Wall4->isObjectStatic = true;
	physicScene->addActor(Wall4);
	// Box Objects
	BoxClass* box;
	box = new BoxClass(glm::vec3(4, 5.5, 0), glm::vec3(0.8f, 0.8f, 0.8f), glm::vec3(0, 0, 2), 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(box);
	BoxClass* box2;
	box2 = new BoxClass(glm::vec3(-4, 5.5, 0), glm::vec3(0.8f, 0.8f, 0.8f), glm::vec3(0, 0, -2), 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(box2);
	BoxClass* box3;
	box3 = new BoxClass(glm::vec3(4, 5.5, -6), glm::vec3(0.8f, 0.8f, 0.8f), glm::vec3(-2, 0, 0), 1, glm::vec4(1, 0, 0, 1));
	physicScene->addActor(box3);

	//============================================//
	// Spring Objects
	SpringClass* spring1;
	spring1 = new SpringClass(ball1, ball2, 1.8f , 0.8f);
	physicScene->addActor(spring1);
	SpringClass* spring2;
	spring2 = new SpringClass(ball4, ball5, 1.7f, 0.8f);
	physicScene->addActor(spring2);
	SpringClass* spring3;
	spring3 = new SpringClass(ball6, ball7, 1.7f, 0.8f);
	physicScene->addActor(spring3);
	SpringClass* spring4;
	spring4 = new SpringClass(ball9, ball10, 1.7f, 0.8f);
	physicScene->addActor(spring4);
	SpringClass* spring5;
	spring5 = new SpringClass(ball11, ball12, 1.7f, 0.8f);
	physicScene->addActor(spring5);
	//============================================//
	m_lastFrameTime = (float)glfwGetTime();
}

//============================================//
// Physx Functions
// Setup
void Physics::setUpPhysx()
{
	PxAllocatorCallback *myCallBack = new myAllocator();
	m_PhysicsFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, *myCallBack, gDefaultErrorCallback);
	m_Physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_PhysicsFoundation, PxTolerancesScale());
	PxInitExtensions(*m_Physics);
	m_PhysicsMaterial = m_Physics->createMaterial(0.5f, 0.5f, 0.5f);
	PxSceneDesc sceneDesc(m_Physics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0, -10.0f, 0);
	sceneDesc.filterShader = &physx::PxDefaultSimulationFilterShader; // sceneDesc.filterShader = myFliterShader;
	sceneDesc.cpuDispatcher = PxDefaultCpuDispatcherCreate(1);
	m_PhysicsScene = m_Physics->createScene(sceneDesc);
	// Player Controller
	myHitReport = new MyControllerHitReport();
	m_characterManager = PxCreateControllerManager(*m_PhysicsScene);
	PxCapsuleControllerDesc desc;
	desc.height = 1.6f;
	desc.radius = 0.4f;
	desc.position.set(0, 0, 0);
	desc.material = m_PhysicsMaterial;
	desc.reportCallback = myHitReport;
	desc.density = 10;
	m_playerController = m_characterManager->createController(desc);
	m_playerController->setPosition(PxExtendedVec3(0, 4, 25));
	m_CharacterYVelocity = 0;
	m_CharacterRotation = 0;
	m_PlayerGravity = -0.5f;
	myHitReport->clearPlayerContactNormal();
}
// Filter Shader
PxFilterFlags Physics::myFilterShader(PxFilterObjectAttributes attribute, PxFilterData filterData, PxFilterObjectAttributes attribute2, PxFilterData filterData2, PxPairFlags& pairFlags, const void* constBlock, PxU32 constBlockSize)
{
	if (PxFilterObjectIsTrigger(attribute) || PxFilterObjectIsTrigger(attribute2))
	{
		pairFlags = PxPairFlag::eTRIGGER_DEFAULT;
		return PxFilterFlag::eDEFAULT;
	}
	pairFlags = PxPairFlag::eCONTACT_DEFAULT;
	if ((filterData.word0 & filterData2.word0) && (filterData2.word0 & filterData.word0))
	{
		pairFlags |= PxPairFlag::eNOTIFY_TOUCH_FOUND | PxPairFlag::eNOTIFY_TOUCH_LOST;
		return PxFilterFlag::eDEFAULT;
	}
}
void Physics::setUpFiltering(PxRigidBody* actor, PxU32 filterGroup, PxU32 filterMask)
{
	PxFilterData filterData;
	filterData.word0 = filterGroup;
	filterData.word1 = filterMask;
	const PxU32 numShapes = actor->getNbShapes();
	PxShape** shapes = (PxShape**)_aligned_malloc(sizeof(PxShape*)*numShapes, 16);
	actor->getShapes(shapes, numShapes);
	for (PxU32 i = 0; i < numShapes; i++)
	{
		PxShape* shape = shapes[i];
		shape->setSimulationFilterData(filterData);
	}
	_aligned_free(shapes);
}
void Physics::setShapeAsTrigger(PxRigidActor* actorIn)
{
	PxRigidStatic* staticActor = actorIn->is<PxRigidStatic>();
	assert(staticActor);
	const PxU32 numShapes = staticActor->getNbShapes();
	PxShape** shapes = (PxShape**)_aligned_malloc(sizeof(PxShape*)*numShapes, 16);
	staticActor->getShapes(shapes, numShapes);
	for (PxU32 i = 0; i < numShapes; i++)
	{
		shapes[i]->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);
		shapes[i]->setFlag(PxShapeFlag::eTRIGGER_SHAPE, true);
	}
}
// Update 
void Physics::upDatePhysx(float DeltaTime)
{
	if (DeltaTime <= 0){
		return; }
	
	// Player Controler
	UpdatePlayerController(DeltaTime);

	m_PhysicsScene->simulate(DeltaTime);
	while (m_PhysicsScene->fetchResults() == false)
	{
		// Fetch Results
	}
}
// Debug
void Physics::setUpVisualDebug()
{
	if (m_Physics->getPvdConnectionManager() == NULL) {
		return; }

	// Connection Parameters
	const char* pvd_host_ip = "127.0.0.1";
	// Ip Of Computor Port
	int port = 5425;
	// TimeOut In millisecs
	unsigned int timeout = 100;
	// Connection To PxVisualDebuggerExt
	PxVisualDebuggerConnectionFlags connectionFlags = PxVisualDebuggerExt::getAllConnectionFlags();
	auto theConnection = PxVisualDebuggerExt::createConnection(m_Physics->getPvdConnectionManager(), pvd_host_ip, port, timeout, connectionFlags);
}
// Player Controller
void MyControllerHitReport::onShapeHit(const PxControllerShapeHit &hit)
{
	PxRigidActor* actor = hit.shape->getActor();
	m_playerContactNormal = hit.worldNormal;
	//std::cout << "Hit: " << hit.actor->getType() << std::endl;
	PxRigidDynamic* myActor = actor->is<PxRigidDynamic>();
	if (myActor) {
	}
}
void Physics::UpdatePlayerController(float delta)
{	
	bool onGround;
	float movementSpeed = 10.0f;
	float rotationSpeed = 1.0f;
	if (myHitReport->getPlayerContactNormal().y > 0.3f) {
		m_CharacterYVelocity = -0.1f;
		onGround = true;
	}
	else {
		m_CharacterYVelocity += m_PlayerGravity * delta;
		onGround = false;
	}
	myHitReport->clearPlayerContactNormal();
	const PxVec3 up(0, 1, 0);
	PxVec3 velocity(0, m_CharacterYVelocity, 0);
	if (glfwGetKey(m_window, GLFW_KEY_UP) == GLFW_PRESS)
	{
		velocity.x -= movementSpeed * delta;
	}
	if (glfwGetKey(m_window, GLFW_KEY_DOWN) == GLFW_PRESS)
	{
		velocity.x += movementSpeed *  delta;
	}
	if (glfwGetKey(m_window, GLFW_KEY_LEFT) == GLFW_PRESS)
	{
		velocity.z += movementSpeed *  delta;
	}
	if (glfwGetKey(m_window, GLFW_KEY_RIGHT) == GLFW_PRESS)
	{
		velocity.z -= movementSpeed *  delta;
	}
	if (glfwGetKey(m_window, GLFW_KEY_SPACE) && onGround == true)
	{
		if (isPressed == false)
		{
			isPressed = true;
			m_CharacterYVelocity += movementSpeed * 1.2f * delta;
			velocity.y += m_CharacterYVelocity;
		}
	}
	else
	{
		isPressed = false;
	}
	float minDistance = 0.001f;
	PxControllerFilters filter;
	PxQuat rotation(m_CharacterRotation, PxVec3(0, 1, 0));
	m_playerController->move(rotation.rotate(velocity), minDistance, delta, filter);
}
// Objects
void Physics::setupTransforms()
{
	// Plane
	PxTransform pose = PxTransform(PxVec3(0.0f, 0, 0.0f), PxQuat(PxHalfPi * 1.0f, PxVec3(0.0f, 0.0f, 1.0f)));
	PxRigidStatic* plane = PxCreateStatic(*m_Physics, pose, PxPlaneGeometry(), *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*plane);
	// Wall 1
	PxBoxGeometry box1(1, 10, 13);
	PxTransform transform1(PxVec3(-12, 4, 30));
	PxRigidStatic *StaticActor1 = PxCreateStatic(*m_Physics, transform1, box1, *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*StaticActor1);
	// Wall 2
	PxBoxGeometry box2(13, 10, 1);
	PxTransform transform2(PxVec3(0, 4, 18));
	PxRigidStatic *StaticActor2 = PxCreateStatic(*m_Physics, transform2, box2, *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*StaticActor2);
	// Wall 3
	PxBoxGeometry box3(1, 10, 13);
	PxTransform transform3(PxVec3(12, 4, 30));
	PxRigidStatic *StaticActor3 = PxCreateStatic(*m_Physics, transform3, box3, *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*StaticActor3);
	// Wall 4
	PxBoxGeometry box4(13, 10, 1);
	PxTransform transform4(PxVec3(0, 4, 42));
	PxRigidStatic *StaticActor4 = PxCreateStatic(*m_Physics, transform4, box4, *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*StaticActor4);
	// TriggerBox
	PxBoxGeometry TriggerBox1(1.5f, 1.5f, 1.5f);
	PxTransform TriggerTransform(PxVec3(-12, 7, 30));
	PxRigidStatic *TriggerActor1 = PxCreateStatic(*m_Physics, TriggerTransform, TriggerBox1, *m_PhysicsMaterial);
	m_PhysicsScene->addActor(*TriggerActor1);
	// Dyanamic Box 1
	float density = 2;
	PxBoxGeometry DyBox1(1.5f, 1.5f, 1.5f);
	PxTransform DyTransform(PxVec3(0, 4, 30));
	PxRigidDynamic *DynamicActor1 = PxCreateDynamic(*m_Physics, DyTransform, DyBox1, *m_PhysicsMaterial, density);
	m_PhysicsScene->addActor(*DynamicActor1);
	// RagDoll
	PxArticulation* ragDollArticulation;
	ragDollArticulation = RagDolls::makeRagDoll(m_Physics, RagDolls::getData(), PxTransform(PxVec3(6, 2, 30)), 0.1f, m_PhysicsMaterial);
	m_PhysicsScene->addArticulation(*ragDollArticulation);
	// Particles
	PxParticleFluid *pf;
	PxU32 maxParticles = 2000;
	bool perParticleRestOffset = false;
	pf = m_Physics->createParticleFluid(maxParticles, perParticleRestOffset);
	pf->setRestParticleDistance(0.3);
	pf->setDynamicFriction(0.1);
	pf->setStaticFriction(0.1);
	pf->setDamping(0.1);
	pf->setParticleMass(0.1);
	pf->setRestitution(0);
	pf->setParticleBaseFlag(PxParticleBaseFlag::eCOLLISION_TWOWAY, true);
	pf->setStiffness(100);
	if (pf) {
		m_PhysicsScene->addActor(*pf);
		m_particleEmitter = new ParticleFluidEmitter(maxParticles, PxVec3(0, 14, 30), pf, 0.08);
		m_particleEmitter->setStartVelocityRange(-2.0f, 0, -2.0f, 2.0f, 0.0f, 2.0f); }
	// Filters
	TriggerActor1->setName("Trigger1");
	setShapeAsTrigger(TriggerActor1);
}
// Physics Gun
void Physics::ObjectThrow()
{
	if (glfwGetKey(m_window, GLFW_KEY_1))
	{
		if (isReadyToShoot)
		{
			// Initializing
			isReadyToShoot = false;
			vec3 cam_pos = (glm::vec3)m_camera.world[3];
			vec3 box_vel = (glm::vec3) - m_camera.world[2] * 20.0f;
			PxTransform boxTransform(PxVec3(cam_pos.x, cam_pos.y, cam_pos.z));
			PxSphereGeometry sphere(0.5f);
			float density = 10;
			PxRigidDynamic *new_actor = PxCreateDynamic(*m_Physics, boxTransform, sphere, *m_PhysicsMaterial, density);
			// Setting Direction & Velocity For Sphere
			float muzzleSpeed = 32;
			glm::vec3 direction(-m_camera.world[2]);
			physx::PxVec3 velocity = physx::PxVec3(direction.x, direction.y, direction.z) * muzzleSpeed;
			new_actor->setLinearVelocity(velocity, true);
			m_PhysicsScene->addActor(*new_actor);
			// Filter Check - Player, Ground, Platform
			new_actor->setName("Ball");
			setUpFiltering(new_actor, FilterGroup::ePLAYER, FilterGroup::eGROUND); // Player
			setUpFiltering(new_actor, FilterGroup::eGROUND, FilterGroup::ePLAYER); // Ground Filter
			setUpFiltering(new_actor, FilterGroup::ePLAYER, FilterGroup::eGROUND | FilterGroup::ePLATFORM); // Collision G + P
		}
	}
	else
	{
		isReadyToShoot = true;
	}
}
//============================================//

void CollisionCallBack::onTrigger(PxTriggerPair* pairs, PxU32 nbpairs)
{
	for (PxU32 i = 0; i < nbpairs; i++) {
		PxTriggerPair* pair = pairs + i;
		PxActor* triggerActor = pair->triggerActor;
		PxActor* otherActor = pair->otherActor;
		const char* pName = otherActor->getName();
		const char* pName1 = triggerActor->getName();
		// Collision Check
		if (pName != nullptr) {
			std::cout << otherActor->getName();
			if (ChangeColour == true)
			{
				std::cout << " Entered Trigger " << std::endl;
				m_particleEmitter->Colour = glm::vec4(1, 1, 0, 1);
				ChangeColour = false;
			}
			else if (ChangeColour == false )
			{
				std::cout << " Exited Trigger " << std::endl;
				m_particleEmitter->Colour = glm::vec4(0, 1, 0, 1);
				ChangeColour = true;
			}
		}
		else if (pName1 != nullptr) {
			std::cout << triggerActor->getName();
			std::cout << " Triggered " << std::endl;
		}
		else {
			std::cout << " UnTriggered " << std::endl;
		}
	}
}

bool Physics::update()
{
	if (Application::update() == false)
	{
		return false;
	}
	Gizmos::clear();
	// DeltaTime
	float currTime = (float)glfwGetTime();
	float dt = currTime - m_lastFrameTime;
	m_lastFrameTime = currTime;
	// Colours
	vec4 white(1);
	vec4 black(0, 0, 0, 1);
	// Grid Lines
	for (int i = 0; i <= 20; ++i)
	{
		Gizmos::addLine(vec3(-10 + i, -0.01, -10), vec3(-10 + i, -0.01, 10),
			i == 10 ? white : black);
		Gizmos::addLine(vec3(-10, -0.01, -10 + i), vec3(10, -0.01, -10 + i),
			i == 10 ? white : black);
	}
	m_camera.update(1.0f / 60.0f);
	//---------------------------------------------------//
	// PhysicObj Functions
	physicScene->update(dt);
	physicScene->updateGizmos();
	physicScene->checkForCollision();
	//---------------------------------------------------//
	// Physx Functions
	if (m_particleEmitter)
	{
		m_particleEmitter->update(dt);
		m_particleEmitter->renderParticles();
	}
	ObjectThrow();
	upDatePhysx(dt);
	renderGizmos(m_PhysicsScene);
	//---------------------------------------------------//
	return true;
}

void Physics::draw()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnable(GL_CULL_FACE);
	// Loaded Sphere's
	physicScene->draw();
    Gizmos::draw(m_camera.proj, m_camera.view);
    m_renderer->RenderAndClear(m_camera.view_proj);
    glfwSwapBuffers(m_window);
    glfwPollEvents();
}

void AddWidget(PxShape* shape, PxRigidActor* actor, vec4 geo_color)
{
    PxTransform full_transform = PxShapeExt::getGlobalPose(*shape, *actor);
    vec3 actor_position(full_transform.p.x, full_transform.p.y, full_transform.p.z);
    glm::quat actor_rotation(full_transform.q.w,
        full_transform.q.x,
        full_transform.q.y,
        full_transform.q.z);
    glm::mat4 rot(actor_rotation);
	mat4 rotate_matrix = glm::rotate(10.f, glm::vec3(7, 7, 7));

    PxGeometryType::Enum geo_type = shape->getGeometryType();
    switch (geo_type)
    {
    case (PxGeometryType::eBOX) :
    {
        PxBoxGeometry geo;
        shape->getBoxGeometry(geo);
        vec3 extents(geo.halfExtents.x, geo.halfExtents.y, geo.halfExtents.z);
        Gizmos::addAABBFilled(actor_position, extents, geo_color, &rot);
    } break;
    case (PxGeometryType::eCAPSULE) :
    {
        PxCapsuleGeometry geo;
        shape->getCapsuleGeometry(geo);
        Gizmos::addCapsule(actor_position, geo.halfHeight * 2, geo.radius, 16, 16, geo_color, &rot);
    } break;
    case (PxGeometryType::eSPHERE) :
    {
        PxSphereGeometry geo;
        shape->getSphereGeometry(geo);
        Gizmos::addSphereFilled(actor_position, geo.radius, 16, 16, geo_color, &rot);
    } break;
    case (PxGeometryType::ePLANE) :
    {
    } break;
    }
}

void Physics::renderGizmos(PxScene* physics_scene)
{
    PxActorTypeFlags desiredTypes = PxActorTypeFlag::eRIGID_STATIC | PxActorTypeFlag::eRIGID_DYNAMIC;
    PxU32 actor_count = physics_scene->getNbActors(desiredTypes);
    PxActor** actor_list = new PxActor*[actor_count];
	physics_scene->getActors(desiredTypes, actor_list, actor_count);

	vec4 geo_color = glm::vec4(0, 1, 0, 1);

    for (int actor_index = 0;
        actor_index < (int)actor_count;
        ++actor_index)
    {
        PxActor* curr_actor = actor_list[actor_index];
        if (curr_actor->isRigidActor())
        {
            PxRigidActor* rigid_actor = (PxRigidActor*)curr_actor;
            PxU32 shape_count = rigid_actor->getNbShapes();
            PxShape** shapes = new PxShape*[shape_count];
            rigid_actor->getShapes(shapes, shape_count);

            for (int shape_index = 0;
                shape_index < (int)shape_count;
                ++shape_index)
            {
                PxShape* curr_shape = shapes[shape_index];
                AddWidget(curr_shape, rigid_actor, geo_color);
            }
            delete[]shapes;
        }
    }

    delete[] actor_list;

    int articulation_count = physics_scene->getNbArticulations();

    for (int a = 0; a < articulation_count; ++a)
    {
        PxArticulation* articulation;
		physics_scene->getArticulations(&articulation, 1, a);

        int link_count = articulation->getNbLinks();

        PxArticulationLink** links = new PxArticulationLink*[link_count];
        articulation->getLinks(links, link_count);

        for (int l = 0; l < link_count; ++l)
        {
            PxArticulationLink* link = links[l];
            int shape_count = link->getNbShapes();

            for (int s = 0; s < shape_count; ++s)
            {
                PxShape* shape;
                link->getShapes(&shape, 1, s);
                AddWidget(shape, link, geo_color);
            }
        }
        delete[] links;
    }
}

Physics Header

#ifndef SOUND_PROGRAMMING_H_
#define SOUND_PROGRAMMING_H_
//============================================//
#include "PhysicObjs.h"
#include "SphereObjClass.h"
#include "Application.h"
#include "Camera.h"
#include "Render.h"
#include <iostream>
//============================================//
#include <PxPhysicsAPI.h>
#include <PxScene.h>
#include "ParticleFluidEmitter.h"
//============================================//
using namespace physx;

struct FilterGroup
{
	enum EnumFunc {
		ePLAYER = (1 << 0),
		ePLATFORM = (1 << 1),
		eGROUND = (1 << 2), };
};

class myAllocator : public PxAllocatorCallback
{
public:
	virtual ~myAllocator() {}
	virtual void *allocate(size_t size, const char* typeName, const char* fileName, int line) {
		void *pointer = _aligned_malloc(size, 16);
		return pointer; }
	virtual void deallocate(void* ptr) {
		_aligned_free(ptr); }
};

class MyControllerHitReport;
class Physics : public Application
{
public:
	virtual bool startup();
	virtual void shutdown();
    virtual bool update();
    virtual void draw();

	void PhysicsSceneFirst();
	void renderGizmos(PxScene* physics_scene);
	PhysicScene* physicScene;
    Renderer* m_renderer;
    FlyCamera m_camera;
    
	float m_lastFrameTime;
	float SphereHeight;
	bool isReadyToShoot;
	bool isPressed;
	//============================================//
	// Physx Functions
	void setUpPhysx();
	void upDatePhysx(float DeltaTime);
	void setUpVisualDebug();
	void setupTransforms();
	void ObjectThrow();
	void UpdatePlayerController(float delta);
	//--------------------------------//
	// Filtering
	void setShapeAsTrigger(PxRigidActor* actorIn);
	void setUpFiltering(PxRigidBody* actor, PxU32 filterGroup, PxU32 filterMask);
	PxFilterFlags myFilterShader(PxFilterObjectAttributes attribute, PxFilterData filterData, PxFilterObjectAttributes attribute2, 
		PxFilterData filterData2, PxPairFlags& pairFlags, const void* constBlock, PxU32 constBlockSize);
	//--------------------------------//
	float m_CharacterYVelocity;
	float m_CharacterRotation;
	float m_PlayerGravity;
	//--------------------------------//
	MyControllerHitReport* myHitReport;
	PxControllerManager* m_characterManager;
	PxController* m_playerController;
	PxFoundation* m_PhysicsFoundation;
	PxPhysics* m_Physics;
	PxScene* m_PhysicsScene;
	PxDefaultErrorCallback gDefaultErrorCallback;
	PxDefaultAllocator gDefaultAllocatorCallback;
	PxSimulationFilterShader gDefaultFilterShader = PxDefaultSimulationFilterShader;
	PxMaterial* m_PhysicsMaterial;
	PxMaterial* m_boxMaterial;
	PxCooking* m_PhysicsCooker;
	//============================================//
};

class CollisionCallBack : public PxSimulationEventCallback
{
	virtual void onContact(const PxContactPairHeader& pairHeader, const PxContactPair* pairs, PxU32 nbPairs) override {
		for (PxU32 i = 0; i < nbPairs; i++) {
			const PxContactPair& cp = pairs[i];
			if (cp.events &PxPairFlag::eNOTIFY_TOUCH_FOUND) {
				std::cout << "Collision Detected Between: ";
				std::cout << pairHeader.actors[0]->getName();
				std::cout << pairHeader.actors[1]->getName() << std::endl; }
		}
	};
	virtual void onTrigger(PxTriggerPair* pairs, PxU32 nbpairs) override;
	virtual void onConstraintBreak(PxConstraintInfo*, PxU32) override {};
	virtual void onWake(PxActor**, PxU32) override {};
	virtual void onSleep(PxActor**, PxU32) override {};
};

class MyControllerHitReport : public PxUserControllerHitReport
{
public:
	virtual void onShapeHit(const PxControllerShapeHit &hit);
	virtual void onControllerHit(const PxControllersHit &hit) {};
	virtual void onObstacleHit(const PxControllerObstacleHit &hit) {};

	PxVec3 m_playerContactNormal;
	MyControllerHitReport() : PxUserControllerHitReport() {};
	PxVec3 getPlayerContactNormal() { return m_playerContactNormal; };
	void clearPlayerContactNormal() { m_playerContactNormal = PxVec3(0, 0, 0); };
};

#endif //CAM_PROJ_H_

Physics Object cpp

#include "PhysicObjs.h"
#include "SphereObjClass.h"
#include "PlaneClass.h"
#include "BoxClass.h"
#include <algorithm>

typedef bool(*fn) (PhysicsObject*, PhysicsObject*);

//---------------------------------------------------//
// Physic Object Functions
PhysicsObject::PhysicsObject(bool isStatic)
{
	isObjectStatic = isStatic;
}

void PhysicsObject::update(glm::vec3 gravity, float timeStep) {}

void PhysicsObject::debug() {}

void PhysicsObject::makeGizmo() {}

//---------------------------------------------------//
// Physic Functions
void PhysicScene::addActor(PhysicsObject* actorList) {
	actors.push_back(actorList);
}																

void PhysicScene::removeActor(PhysicsObject*){
	actors.pop_back();
}

void PhysicScene::update(float deltaTime) 
{
	for (int i = 0; i < actors.size(); i++)
	{
		actors[i]->update(gravity, deltaTime);
	}
}

void PhysicScene::draw()
{
	for (int i = 0; i < actors.size(); i++)
	{
		actors[i]->makeGizmo();
	}
}

void PhysicScene::debugScene() {}

void PhysicScene::addGizmos() {}

void PhysicScene::updateGizmos() {}

//---------------------------------------------------//
// Collision Switch
static fn collisionfunctionArray[] =
{
	PhysicScene::planeToPlane,   PhysicScene::planeToSphere,   PhysicScene::planeToBox,
	PhysicScene::sphereToPlane,  PhysicScene::sphereToSphere,  PhysicScene::sphereToBox,
	PhysicScene::boxToPlane,     PhysicScene::boxToSphere,     PhysicScene::boxToBox,
};
//---------------------------------------------------//
// Plane To Plane
bool PhysicScene::planeToPlane(PhysicsObject* obj1 , PhysicsObject* obj2)
{
	return false;
}
// Plane To Sphere Collision
bool PhysicScene::planeToSphere(PhysicsObject* obj1, PhysicsObject* obj2)
{
	return sphereToPlane(obj2, obj1);
}
// Plane To Box Collision
bool PhysicScene::planeToBox(PhysicsObject* obj1, PhysicsObject* obj2)
{
	PlaneClass* plane = dynamic_cast<PlaneClass*>(obj1);
	BoxClass* box = dynamic_cast<BoxClass*>(obj2);
	if (plane != NULL && box != NULL && box->isObjectStatic == false)
	{
		glm::vec3 boxPos = box->m_Position;
		glm::vec3 minPos = boxPos - box->GetExtents();
		glm::vec3 maxPos = boxPos + box->GetExtents();
		glm::vec3 planeNormal = plane->Normal;

		float minPointAverage = glm::dot(minPos, planeNormal);
		float maxPointAverage = glm::dot(maxPos, planeNormal);
		float overlap = std::min(minPointAverage, maxPointAverage) - plane->Distance;

		if (overlap < 0)
		{
			planeNormal *= -1;
			glm::vec3 forceVector = -1 * box->m_Mass * planeNormal * (glm::dot(planeNormal, box->m_Velocity));
			box->applyForce(2 * forceVector);
			box->m_Position += planeNormal * overlap;
			return true;
		}
	}
	return false;
}
// Sphere To Plane Collision
bool PhysicScene::sphereToPlane(PhysicsObject* obj1, PhysicsObject* obj2)
{
	// Collision Detection
	SphereObjClass* sphere = dynamic_cast<SphereObjClass*>(obj1);
	PlaneClass* plane = dynamic_cast<PlaneClass*>(obj2);
	if (sphere != NULL && plane != NULL && sphere->isObjectStatic == false)
	{
		glm::vec3 collisonNormal = plane->Normal;
		float sphere2Plane = glm::dot(sphere->m_Position, plane->Normal) - plane->Distance;
		if (sphere2Plane < 0)
		{
			collisonNormal *= -1;
			sphere2Plane *= -1;
		}
		float intersection = sphere->m_Radius - sphere2Plane;
		// Collision Collider - Response
		if (intersection > 0)
		{
			glm::vec3 planeNormal = plane->Normal;
			if (sphere2Plane < 0)
			{
				planeNormal *= -1;
			}
			glm::vec3 forceVector = -1 * sphere->m_Mass * planeNormal * (glm::dot(planeNormal, sphere->m_Velocity));
			sphere->applyForce(2 * forceVector);
			sphere->m_Position += collisonNormal * intersection * 0.5f;
			return true;
		}
	}
	return false;
}
// Sphere To Sphere Collision
bool PhysicScene::sphereToSphere(PhysicsObject* obj1, PhysicsObject* obj2)
{
	// Collision Detection
	SphereObjClass* sphere1 = dynamic_cast<SphereObjClass*>(obj1);
	SphereObjClass* sphere2 = dynamic_cast<SphereObjClass*>(obj2);
	if (sphere1->isObjectStatic == true && sphere2->isObjectStatic == true)
	{
		return false;
	}
	if (sphere1->isObjectStatic == false && sphere2->isObjectStatic == false)
	{
		// Collision Collider - Response
		glm::vec3 delta = sphere2->m_Position - sphere1->m_Position;
		float distance = glm::length(delta);
		float intersection = sphere1->m_Radius + sphere2->m_Radius - distance;
		if (intersection > 0)
		{
			glm::vec3 collisionNormal = glm::normalize(delta);
			glm::vec3 relativeVelocity = sphere2->m_Velocity - sphere1->m_Velocity;
			glm::vec3 collisionVector = collisionNormal *(glm::dot(relativeVelocity, collisionNormal));
			glm::vec3 forceVector = collisionVector * 1.0f / (1 / sphere1->m_Mass + 1 / sphere2->m_Mass);
	
			if (sphere1->isObjectStatic == true)
			{
				sphere2->applyForce(forceVector * 1.5f);
				glm::vec3 seperationVector = collisionNormal * intersection;
				sphere2->m_Position += seperationVector;
			}
			else if (sphere2->isObjectStatic == true)
			{
				sphere1->applyForce(-forceVector * 1.5f);
				glm::vec3 seperationVector = collisionNormal * intersection;
				sphere1->m_Position -= seperationVector;
			}
			else 
			{
				sphere1->applyForceToActor(sphere2, -forceVector);
				glm::vec3 seperationVector = collisionNormal * intersection * 0.5f;
				sphere2->m_Position += seperationVector;
				sphere1->m_Position -= seperationVector;
			}
			
			return true;
		}
	}	
	return false;
}
// Sphere To Box Collision
bool PhysicScene::sphereToBox(PhysicsObject* obj1, PhysicsObject* obj2)
{
	SphereObjClass* sphere = dynamic_cast<SphereObjClass*>(obj1);
	BoxClass* box = dynamic_cast<BoxClass*>(obj2);
	if (sphere->isObjectStatic == true && box->isObjectStatic == true)
	{
		return false;
	}
	glm::vec3 minPos = -box->GetExtents();
	glm::vec3 maxPos = box->GetExtents();
	// Calculating Distance 
	glm::vec3 distance = sphere->m_Position - box->m_Position;
	glm::vec3 clampedPoint = distance;
	// Getting Extents
	if (sphere != NULL && box != NULL )
	{
		// X
		if (distance.x < minPos.x)
		{
			clampedPoint.x = minPos.x;
		}
		else if (distance.x > maxPos.x)
		{
			clampedPoint.x = maxPos.x;
		}
		// Y
		if (distance.y < minPos.y)
		{
			clampedPoint.y = minPos.y;
		}
		else if (distance.y > maxPos.y)
		{
			clampedPoint.y = maxPos.y;
		}
		// Z
		if (distance.z < minPos.z)
		{
			clampedPoint.z = minPos.z;
		}
		else if (distance.z > maxPos.z)
		{
			clampedPoint.z = maxPos.z;
		}
		glm::vec3 clampedDistance = distance - clampedPoint;
		// Collision Collider - Response
		float overlap = glm::length(clampedDistance) - sphere->m_Radius;
		if (overlap < 0)
		{
			overlap *= -1;
			glm::vec3 collisionNormal = glm::normalize(-clampedDistance);
			glm::vec3 relativeVelocity = box->m_Velocity - sphere->m_Velocity;
			glm::vec3 collisionVector = collisionNormal *(glm::dot(relativeVelocity, collisionNormal));

			if (sphere->isObjectStatic == true)
			{
				box->m_Velocity = box->m_Velocity - 2 * collisionNormal *(glm::dot(box->m_Velocity, collisionNormal));
				glm::vec3 seperationVector = collisionNormal * overlap;
				box->m_Position += seperationVector;
			}
			else if (box->isObjectStatic == true)
			{
				sphere->m_Velocity = sphere->m_Velocity - 2 * collisionNormal *(glm::dot(sphere->m_Velocity, collisionNormal));
				glm::vec3 seperationVector = collisionNormal * overlap;
				sphere->m_Position -= seperationVector;
			}
			else
			{
				glm::vec3 forceVector = collisionVector * 1.5f / (1 / box->m_Mass + 1 / sphere->m_Mass);
				box->applyForceToActor(sphere, forceVector);
				glm::vec3 seperationVector = collisionNormal * overlap * 0.5f;
				box->m_Position += seperationVector;
				sphere->m_Position -= seperationVector;
			}
			return true;
		}
	}
	return false;
}
// Box To Plane Collision
bool PhysicScene::boxToPlane(PhysicsObject* obj1, PhysicsObject* obj2)
{
	return planeToBox(obj2, obj1);
}
// Box To Box Collision
bool PhysicScene::boxToBox(PhysicsObject* obj1, PhysicsObject* obj2)
{
	BoxClass* box1 = dynamic_cast<BoxClass*>(obj1);
	BoxClass* box2 = dynamic_cast<BoxClass*>(obj2);
	if (box1->isObjectStatic == true && box2->isObjectStatic == true)
	{
		return false;
	}
	// Position
	glm::vec3 boxPos1 = box1->m_Position;
	glm::vec3 boxPos2 = box2->m_Position;
	glm::vec3 boxDeltaPos = boxPos2 - boxPos1;
	// Extents
	glm::vec3 boxExtents1 = box1->GetExtents();
	glm::vec3 boxExtents2 = box2->GetExtents();
	glm::vec3 boxExtentsComb = boxExtents1 + boxExtents2;
	if (box1 != NULL && box2 != NULL)
	{
		// Overlaps
		float xOverlap = std::abs(boxDeltaPos.x) - boxExtentsComb.x;
		float yOverlap = std::abs(boxDeltaPos.y) - boxExtentsComb.y;
		float zOverlap = std::abs(boxDeltaPos.z) - boxExtentsComb.z;

		if (xOverlap <= 0 && yOverlap <= 0 && zOverlap <= 0)
		{
			float minOverlap = xOverlap;
			minOverlap = yOverlap < 0 ? std::max(minOverlap, yOverlap) : minOverlap;
			minOverlap = zOverlap < 0 ? std::max(minOverlap, zOverlap) : minOverlap;

			glm::vec3 NormalSep(0);

			if (xOverlap == minOverlap)
			{
				NormalSep.x = std::signbit(boxDeltaPos.x) ? -1.0f : 1.0f;
			}
			else if (yOverlap == minOverlap)
			{
				NormalSep.y = std::signbit(boxDeltaPos.y) ? -1.0f : 1.0f;
			}
			else if (zOverlap == minOverlap)
			{
				NormalSep.z = std::signbit(boxDeltaPos.z) ? -1.0f : 1.0f;
			}
			// Calculate Response Of Cubes
			glm::vec3 relativeVelocity = box2->m_Velocity - box1->m_Velocity;
			float impulseAmount = -2.0f * (glm::dot(relativeVelocity, NormalSep));
			glm::vec3 forceVector = impulseAmount * NormalSep;

			if (box1->isObjectStatic == true)
			{
				impulseAmount /= 1 / box2->m_Mass;
				box2->m_Velocity += forceVector;
				glm::vec3 seperationVector = NormalSep * -minOverlap;
				box2->m_Position += seperationVector;
			}
			else if (box2->isObjectStatic == true)
			{
				impulseAmount /= 1.0f / box1->m_Mass;
				box1->m_Velocity += -forceVector;
				glm::vec3 seperationVector = NormalSep * -minOverlap;
				box1->m_Position -= seperationVector;
			}
			else
			{
				impulseAmount /= 1.0f / box1->m_Mass + 1 / box2->m_Mass;
				box1->m_Velocity += -forceVector;
				box2->m_Velocity += forceVector;
				glm::vec3 seperationVector = NormalSep * -minOverlap * 0.5f;
				box1->m_Position -= seperationVector;
				box2->m_Position += seperationVector;
			}
			return true;
		}
	}
	return false;
}
// Box To Sphere Collision
bool PhysicScene::boxToSphere(PhysicsObject* obj1, PhysicsObject* obj2)
{
	return sphereToBox(obj2, obj1);
}
// Collision Checker
void PhysicScene::checkForCollision()
{
	int actorCount = actors.size();
	for (int outer = 0; outer < actorCount - 1; outer++)
	{
		PhysicsObject* object1 = actors[outer];
		int m_shapeID1 = object1->m_ShapeID;
		if (m_shapeID1 == JOINT)
		{
			continue;
		}
		for (int inner = outer + 1; inner < actorCount; inner++)
		{
			PhysicsObject* object2 = actors[inner];
			int m_shapeID2 = object2->m_ShapeID;
			if (m_shapeID2 == JOINT)
			{
				continue;
			}

			int functionIndex = (m_shapeID1 * NUMBERSHAPE) + m_shapeID2;
			fn collisionFunctionPtr = collisionfunctionArray[functionIndex];
			if (collisionFunctionPtr != NULL)
			{
				collisionFunctionPtr(object1, object2);
			}
		}
	}                  
}

Physics Object Header

#pragma once
#include "gl_core_4_4.h"
#include "GLFW/glfw3.h"
#include "glm\glm.hpp"
#include "glm\ext.hpp"
#include <vector>

enum ShapeType
{
	PLANE = 0,
	SPHERE = 1,
	BOX = 2,
	NUMBERSHAPE = 3,
	JOINT = 4,
};

class PhysicsObject
{
public:
	PhysicsObject(bool isStatic = false);
	bool isObjectStatic;
	ShapeType m_ShapeID;
	void virtual update(glm::vec3 gravity, float timeStep) = 0;
	void virtual debug() = 0;
	void virtual makeGizmo() = 0;
	void virtual resetPositions() {};	
};

class PhysicScene
{
public:
	glm::vec3 gravity;
	std::vector<PhysicsObject*> actors;
	void addActor(PhysicsObject*);
	void removeActor(PhysicsObject*);
	void update(float deltaTime);
	void debugScene();
	void addGizmos();
	void updateGizmos();
	void draw();
	void checkForCollision();

	static bool planeToPlane(PhysicsObject*, PhysicsObject*);
	static bool planeToSphere(PhysicsObject*, PhysicsObject*);
	static bool planeToBox(PhysicsObject*, PhysicsObject*);
	static bool sphereToPlane(PhysicsObject*, PhysicsObject*);
	static bool sphereToSphere(PhysicsObject*, PhysicsObject*);
	static bool sphereToBox(PhysicsObject*, PhysicsObject*);
	static bool boxToPlane(PhysicsObject*, PhysicsObject*);
	static bool boxToSphere(PhysicsObject*, PhysicsObject*);
	static bool boxToBox(PhysicsObject*, PhysicsObject*);
};

Ragdoll cpp

#include "RagDolls.h"

RagDolls::RagDolls() {}

RagDollNode** RagDolls::getData()
{
	RagDollNode** ragDollData = new RagDollNode*[17]
	{
		new RagDollNode(PxQuat(PxPi / 2.0f, Z_AXIS), NO_PARENT, 1, 3, 1, 1, "lower spine"),
		new RagDollNode(PxQuat(PxPi, Z_AXIS), LOWER_SPINE, 1, 1, -1, 1, "left pelvis"),
		new RagDollNode(PxQuat(0, Z_AXIS), LOWER_SPINE, 1, 1, -1, 1, "right pelvis"),
		new RagDollNode(PxQuat(PxPi / 2.0f + 0.2f, Z_AXIS),LEFT_PELVIS, 5, 2, -1, 1,"L upper leg"),
		new RagDollNode(PxQuat(PxPi / 2.0f - 0.2f, Z_AXIS),RIGHT_PELVIS, 5, 2, -1, 1,"R upper leg"),
		new RagDollNode(PxQuat(PxPi / 2.0f + 0.2f, Z_AXIS),LEFT_UPPER_LEG, 5, 1.75, -1, 1,"L lower leg"),
		new RagDollNode(PxQuat(PxPi / 2.0f - 0.2f, Z_AXIS),RIGHT_UPPER_LEG,5, 1.75, -1, 1,"R lowerleg"),
		new RagDollNode(PxQuat(PxPi / 2.0f, Z_AXIS), LOWER_SPINE, 1, 3, 1, -1, "upper spine"),
		new RagDollNode(PxQuat(PxPi, Z_AXIS), UPPER_SPINE, 1, 1.5, 1, 1, "left clavicle"),
		new RagDollNode(PxQuat(0, Z_AXIS), UPPER_SPINE, 1, 1.5, 1, 1, "right clavicle"),
		new RagDollNode(PxQuat(PxPi / 2.0f, Z_AXIS), UPPER_SPINE, 1, 1, 1, -1, "neck"),
		new RagDollNode(PxQuat(PxPi / 2.0f, Z_AXIS), NECK, 1, 3, 1, -1, "HEAD"),
		new RagDollNode(PxQuat(PxPi - 0.3f, Z_AXIS), LEFT_CLAVICLE, 3, 1.5, -1, 1, "left upper arm"),
		new RagDollNode(PxQuat(0.3, Z_AXIS), RIGHT_CLAVICLE, 3, 1.5, -1, 1, "right upper arm"),
		new RagDollNode(PxQuat(PxPi - 0.3f, Z_AXIS), LEFT_UPPER_ARM, 3, 1, -1, 1, "left lower arm"),
		new RagDollNode(PxQuat(0.3, Z_AXIS), RIGHT_UPPER_ARM, 3, 1, -1, 1, "right lower arm"),
		NULL
	};
	return ragDollData;
}

PxArticulation *RagDolls::makeRagDoll(PxPhysics* m_Physics, RagDollNode** nodeArray, PxTransform worldPos, float scaleFactor, PxMaterial* ragDollMaterial)
{
	PxArticulation *articulation = m_Physics->createArticulation();
	RagDollNode** currentNode = nodeArray;
	while (*currentNode != NULL)
	{
		RagDollNode* currentNodePtr = *currentNode;
		RagDollNode* parentNode = nullptr;
		float radius = currentNodePtr->radius * scaleFactor;
		float halfLength = currentNodePtr->halfLength * scaleFactor;
		float childHalfLength = radius + halfLength;
		float parentHalfLength = 0;
		PxArticulationLink* parentLinkPtr = NULL;
		currentNodePtr->scaledGlobalPos = worldPos.p;
		if (currentNodePtr->parentNodeIDx != NO_PARENT)
		{
			parentNode = *(nodeArray + currentNodePtr->parentNodeIDx);
			parentLinkPtr = parentNode->linkPtr;
			parentHalfLength = (parentNode->radius + parentNode->halfLength) * scaleFactor;
			// Local Pos Of Node
			PxVec3 currentRelative = currentNodePtr->childLinkPos * currentNodePtr->globalRotation.rotate(PxVec3(childHalfLength, 0, 0));
			PxVec3 parentRelative = -currentNodePtr->parentLinkPos * parentNode->globalRotation.rotate(PxVec3(parentHalfLength, 0, 0));
			currentNodePtr->scaledGlobalPos = parentNode->scaledGlobalPos - (parentRelative + currentRelative);
		}

		// Bones
		PxTransform linkTransform = PxTransform(currentNodePtr->scaledGlobalPos, currentNodePtr->globalRotation);
		PxArticulationLink *link = articulation->createLink(parentLinkPtr, linkTransform);
		currentNodePtr->linkPtr = link;
		float jointSpace = 0.01f;  // <- Gap Between Joints //
		float capsuleHalfLength = (halfLength > jointSpace ? halfLength - jointSpace : 0) + 0.01f;
		PxCapsuleGeometry capsule(radius, capsuleHalfLength);
		link->createShape(capsule, *ragDollMaterial);
		PxRigidBodyExt::updateMassAndInertia(*link, 50.0f);
			
		// Joints
		if (currentNodePtr->parentNodeIDx != NO_PARENT)
		{
			PxArticulationJoint *joint = link->getInboundJoint();
			PxQuat frameRotation = parentNode->globalRotation.getConjugate() * currentNodePtr->globalRotation;
			PxTransform parentConstraintFrame = PxTransform(PxVec3(currentNodePtr->parentLinkPos * parentHalfLength, 0, 0), frameRotation);
			PxTransform thisConstraintFrame = PxTransform(PxVec3(currentNodePtr->childLinkPos * childHalfLength, 0, 0));
			// Setting Pose For Joint
			joint->setParentPose(parentConstraintFrame);
			joint->setChildPose(thisConstraintFrame);
			// Setting Up Constraints
			joint->setStiffness(20);
			joint->setDamping(20);
			joint->setSwingLimit(0.4f, 0.4f);
			joint->setSwingLimitEnabled(true);
			joint->setTwistLimit(-0.1f, 0.1f);
			joint->setTwistLimitEnabled(true);
		}
		currentNode++;
	}
	return articulation;
}

RagDolls::~RagDolls() {}

Ragdoll Header

#pragma once
#include <PxPhysicsAPI.h>
#include <PxScene.h>

using namespace physx;
enum RagDollParts
{
	NO_PARENT = -1,
	LOWER_SPINE,
	LEFT_PELVIS,
	RIGHT_PELVIS,
	LEFT_UPPER_LEG,
	RIGHT_UPPER_LEG,
	LEFT_LOWER_LEG,
	RIGHT_LOWER_LEG,
	UPPER_SPINE,
	LEFT_CLAVICLE,
	RIGHT_CLAVICLE,
	NECK,
	HEAD,
	LEFT_UPPER_ARM,
	RIGHT_UPPER_ARM,
	LEFT_LOWER_ARM,
	RIGHT_LOWER_ARM,
};

struct RagDollNode
{
	PxQuat globalRotation;
	PxVec3 scaledGlobalPos;
	int parentNodeIDx;
	float halfLength;
	float radius;
	float parentLinkPos;
	float childLinkPos;
	char* name;
	PxArticulationLink* linkPtr;
	RagDollNode(PxQuat _globalRotation, int _parentNodeId, float _halfLength, float _radius, float _parentLinkPos, float _childLinkPos, char* _name)
	{
		globalRotation = _globalRotation;
		parentNodeIDx = _parentNodeId;
		halfLength = _halfLength; 
		radius = _radius; 
		parentLinkPos = _parentLinkPos;
		childLinkPos = _childLinkPos;
		name = _name;
	}
};

const PxVec3 X_AXIS = PxVec3(1, 0, 0);
const PxVec3 Y_AXIS = PxVec3(0, 1, 0);
const PxVec3 Z_AXIS = PxVec3(0, 0, 1);

class RagDolls
{
public:
	RagDolls();
	~RagDolls();

	static RagDollNode **getData();
	static PxArticulation *makeRagDoll(PxPhysics* m_Physics, RagDollNode** nodeArray, PxTransform worldPos, float scaleFactor, PxMaterial* ragDollMaterial);
};

RigidBody cpp

#include "RigidBodyClass.h"

//---------------------------------------------------//
// RigidBody Functions
RigidBodyClass::RigidBodyClass(glm::vec3 position, glm::vec3 velocity, glm::quat rotation, float mass)
{
	m_Position = position;
	m_Velocity = velocity;
	m_Mass = mass;
	m_Accel = glm::vec3(0, 0, 0);
}

RigidBodyClass::RigidBodyClass() {}

void RigidBodyClass::update(glm::vec3 gravity, float DeltaTime)
{
	if (isObjectStatic == false)
	{
		m_Accel += gravity * DeltaTime;
		m_Velocity += m_Accel;
		m_Position += m_Velocity * DeltaTime;
		m_Accel = glm::vec3(0, 0, 0);
	}
}

void RigidBodyClass::debug() {}

void RigidBodyClass::applyForce(glm::vec3 force)
{
	m_Accel += (force / m_Mass);
}

void RigidBodyClass::applyForceToActor(RigidBodyClass* actor2, glm::vec3 force)
{
	applyForce(-force);
	actor2->applyForce(force);
}

RigidBodyClass::~RigidBodyClass() {}
//---------------------------------------------------//

RigidBody Header

#pragma once
#include "PhysicObjs.h"

class RigidBodyClass : public PhysicsObject
{
public:
	RigidBodyClass(glm::vec3 position, glm::vec3 velocity, glm::quat rotation, float mass);
	RigidBodyClass();
	~RigidBodyClass();

	glm::vec3 m_Position;
	glm::vec3 m_Velocity;
	glm::vec3 m_Accel;

	float m_Mass;
	float rotations2D;

	virtual void update(glm::vec3 gravity, float DeltaTime);
	virtual void debug();
	void applyForce(glm::vec3 force);
	void applyForceToActor(RigidBodyClass* actor2, glm::vec3 force);
};

Sphere cpp

#include "SphereObjClass.h"

SphereObjClass::SphereObjClass(glm::vec3 position, glm::vec3 velocity, float mass, float radius, glm::vec4 colour)
{
	m_Mass = mass;
	m_Velocity = velocity;
	m_Position = position;
	m_ShapeID = SPHERE;
	m_Radius = radius;
}

//---------------------------------------------------//
// Sphere Functions
void SphereObjClass::makeGizmo()
{
	Gizmos::addSphereFilled(m_Position, 1.0f, 15, 15, glm::vec4(0, 1, 0, 1));
}
//---------------------------------------------------//

SphereObjClass::~SphereObjClass() {}

Sphere Header

#pragma once
#include "RigidBodyClass.h"
#include "PhysicObjs.h"
#include "Gizmos.h"

class SphereObjClass : public RigidBodyClass
{
public:
	float m_Radius;
	SphereObjClass(glm::vec3 position, glm::vec3 velocity, float mass, float radius, glm::vec4 colour);
	~SphereObjClass();

	virtual void makeGizmo();
};

Spring cpp

#include "SpringClass.h"

SpringClass::SpringClass(RigidBodyClass* connection1, RigidBodyClass* connection2, float springCoeff, float damping)
{
	m_connections[0] = connection1;
	m_connections[1] = connection2;
	m_springCoeff = springCoeff;
	m_damping = damping;
	m_restLength = glm::length(m_connections[0]->m_Position - m_connections[1]->m_Position);
	m_ShapeID = JOINT;
}

void SpringClass::update(glm::vec3 gravity, float Delta)
{
	glm::vec3 Direction = m_connections[0]->m_Position - m_connections[1]->m_Position;
	glm::vec3 Velocity = m_connections[0]->m_Velocity + m_connections[1]->m_Velocity;
	float Length = glm::length(Direction);
	Direction = glm::normalize(Direction);
	float LengthDif = Length - m_restLength;
	glm::vec3 force = Direction * LengthDif * m_springCoeff;
	
	glm::vec3 dampValue = m_damping * Velocity;
	glm::vec3 dampEffect = (force - dampValue) * Delta;

	m_connections[0]->applyForce(-dampEffect);
	m_connections[1]->applyForce(dampEffect);
}

void SpringClass::debug() {}

void SpringClass::makeGizmo()
{
	Gizmos::addLine(m_connections[0]->m_Position, m_connections[1]->m_Position, glm::vec4(1, 1, 1, 1));
}

SpringClass::~SpringClass() {}

Spring Header

#pragma once
#include "PhysicObjs.h"
#include "RigidBodyClass.h"
#include "Gizmos.h"

class SpringClass : public PhysicsObject
{
public:
	SpringClass(RigidBodyClass* connection1, RigidBodyClass* connection2, float springCoeff, float damping);
	~SpringClass();

	void virtual update(glm::vec3 gravity, float Delta);
	void virtual debug();
	void virtual makeGizmo();
private:

	RigidBodyClass* m_connections[2];
	float m_damping;
	float m_restLength;
	float m_springCoeff;
};