PhysColliderSys.cpp

#include "PhysColliderSys.h"

#define BROADEXTENT 5.0f //how far away must objects be from each other before they test collisions.

// CORE

PhysColliderSys::PhysColliderSys(vector<GameObject*>& gameObjects) {
	//initialize the physics system by gathering a vector of collider references, instead of game objects, saving time finding the colliders.
	for (GameObject* gameObject : gameObjects) {
		PhysicsModel* modelTemp = gameObject->GetPhysicsModel();
		if (modelTemp != nullptr) {
			_physicsWorld.push_back(modelTemp);
			_colliderWorld.push_back(modelTemp->GetCollider()); //if it's null, let it be.  we want the same amount of indexes in this vector
		}
	}
}

PhysColliderSys::~PhysColliderSys() {
	//don't destroy the data in this vector, that data does not belong to this
	//instead, forget all the pointers immediately
	_physicsWorld.clear();
	_physicsWorld.shrink_to_fit();

	_colliderWorld.clear();
	_colliderWorld.shrink_to_fit();
}

void PhysColliderSys::CollisionTick() {
	BroadPhase();
}

// PHASES

inline void PhysColliderSys::BroadPhase() {
	//compare the distance of all objects and only test a collision if they are within 5 units of each other.
	for (int indexA = 0; indexA < _physicsWorld.size(); indexA++) {
		for (int indexB = 0; indexB < _physicsWorld.size(); indexB++) {
			//don't process against self or anything already fully cycled.
			//this allows us to permute combinations without repeats.
			if (indexA >= indexB) continue;

			Vector3 displacement = _colliderWorld[indexA]->GetPosition() - _colliderWorld[indexB]->GetPosition();
			//since planes are infinitely sized, include that.
			if ((CheckDistanceToBoundary(displacement, BROADEXTENT)) 
				|| (_colliderWorld[indexA]->GetColliderTag() == PLANE) 
				|| (_colliderWorld[indexB]->GetColliderTag() == PLANE)) {
				NarrowPhase(indexA, indexB);
			}
		}
	}

}

inline void PhysColliderSys::NarrowPhase(int index1, int index2) {
	CollisionManifold manifold = CollisionManifold();
	//this is a very silly narrow phase, but it works, and is scalable.  not very scalable, but scalable enough.
	//technically this is just a reorganization way around double reflection as was taught in class.
	//this made it easier to create headers without error.
	ColliderTag objectA = _colliderWorld[index1]->GetColliderTag();
	ColliderTag objectB = _colliderWorld[index2]->GetColliderTag();

	//same
	if ((objectA == SPHERE) && (objectB == SPHERE)) {
		if (CollisionCheck(*(SphereCollider*)_colliderWorld[index1], *(SphereCollider*)_colliderWorld[index2], manifold)) Solve(index1, index2, manifold);
		return;
	}
	if ((objectA == AABB) && (objectB == AABB)) {
		if (CollisionCheck(*(AABBCollider*)_colliderWorld[index1], *(AABBCollider*)_colliderWorld[index2], manifold)) Solve(index1, index2, manifold);
		return;
	}

	//alternate
	if ((objectA == SPHERE) && (objectB == PLANE)) {
		if (CollisionCheck(*(SphereCollider*)_colliderWorld[index1], *(PlaneCollider*)_colliderWorld[index2], manifold)) Solve(index1, index2, manifold);
		return;
	}
	if ((objectA == AABB) && (objectB == PLANE)) {
		if (CollisionCheck(*(AABBCollider*)_colliderWorld[index1], *(PlaneCollider*)_colliderWorld[index2], manifold)) Solve(index1, index2, manifold);
		return;
	}
	if ((objectA == SPHERE) && (objectB == AABB)) {
		if (CollisionCheck(*(SphereCollider*)_colliderWorld[index1], *(AABBCollider*)_colliderWorld[index2], manifold)) Solve(index1, index2, manifold);
		return;
	}

	//reverse alternate
	if ((objectA == PLANE) && (objectB == SPHERE)) {
		if (CollisionCheck(*(SphereCollider*)_colliderWorld[index2], *(PlaneCollider*)_colliderWorld[index1], manifold)) Solve(index2, index1, manifold);
		return;
	}
	if ((objectA == PLANE) && (objectB == AABB)) {
		if (CollisionCheck(*(AABBCollider*)_colliderWorld[index2], *(PlaneCollider*)_colliderWorld[index1], manifold)) Solve(index2, index1, manifold);
		return;
	}
	if ((objectA == AABB) && (objectB == SPHERE)) {
		if (CollisionCheck(*(SphereCollider*)_colliderWorld[index2], *(AABBCollider*)_colliderWorld[index1], manifold)) Solve(index2, index1, manifold);
		return;
	}
}

inline void PhysColliderSys::Solve(int index1, int index2, CollisionManifold& manifold) {
	SolveInterpenetration(_physicsWorld[index1], _physicsWorld[index2], manifold);
	ResolveManifold(manifold, _physicsWorld[index1], _physicsWorld[index2]);
}

// CHECKS

//note that both of these plane collision checks don't seem to work.
bool PhysColliderSys::CollisionCheck(SphereCollider& sphere, PlaneCollider& plane, CollisionManifold& manifold) {
	//if the closest point on the plane exists inside the sphere, it is collided.
	Vector3 closestPoint = ClosestPointOnPlane(sphere.GetPosition(), plane);
	Vector3 distanceToPoint = sphere.GetPosition() - closestPoint;

	if (abs(distanceToPoint.Magnitude()) <= sphere.GetRadius()) {
		manifold.collisionNormal = plane.GetNormal();
		manifold.contactPointCount = 1;
		manifold.points[0].position = closestPoint;
		manifold.points[0].penetrationDepth = fabs(sphere.GetRadius() - distanceToPoint.Magnitude());

		return true;
	}
	return false;
}

bool PhysColliderSys::CollisionCheck(AABBCollider& aabb, PlaneCollider& plane, CollisionManifold& manifold) {
	//get the closest point on the plane
	Vector3 closestPoint = ClosestPointOnPlane(aabb.GetPosition(), plane);

	//find the distance of the point to the cube
	float distanceToPoint = DistanceTo(aabb.GetPosition(), closestPoint);

	//TODO: fix this collision depth test
	if (CheckDistanceToExtents(distanceToPoint * plane.GetNormal(), aabb.GetExtents())) {
		manifold.collisionNormal = plane.GetNormal();
		manifold.contactPointCount = 1;
		manifold.points[0].position = closestPoint;

		//TODO: fix this penetration depth test.
		manifold.points[0].penetrationDepth = fabs(AABBRadiusOnNormal(aabb.GetExtents(), plane.GetNormal()) - distanceToPoint);

		return true;
	}
	return false;
}

bool PhysColliderSys::CollisionCheck(SphereCollider& sphere, AABBCollider& aabb, CollisionManifold& manifold) {
	//get inverse normal which represents direction from cube center to sphere center.
	Vector3 directionBetween = aabb.GetPosition() - sphere.GetPosition();
	directionBetween.Normalize();
	directionBetween.Reverse();

	//similar to sphere to plane check, this allows us to get the closest point on the sphere to the object.
	Vector3 closestPointToCube = sphere.GetPosition() + (directionBetween * sphere.GetRadius());

	//get the distance of this closest point to the center of the cube.
	Vector3 distanceToPoint = aabb.GetPosition() - closestPointToCube;

	if (CheckDistanceToExtents(distanceToPoint, aabb.GetExtents())) {
		//this value is already normalized distance between two objects, just unreverse it.
		manifold.collisionNormal = directionBetween;
		manifold.collisionNormal.Reverse();
		manifold.contactPointCount = 1;
		manifold.points[0].position = closestPointToCube;

		//TODO: fix this depth test
		manifold.points[0].penetrationDepth = fabs((AABBRadiusOnNormal(aabb.GetExtents(), directionBetween) + sphere.GetRadius()) - DistanceTo(distanceToPoint, Vector3(0, 0, 0)));

		return true;
	}
	return false;
}

bool PhysColliderSys::CollisionCheck(SphereCollider& sphere1, SphereCollider& sphere2, CollisionManifold& manifold) {
	//get the distance via position difference magnitude, and tell if that distance is as long or shorter than radius total
	Vector3 difference = sphere1.GetPosition() - sphere2.GetPosition();
	float radiiSum = sphere1.GetRadius() + sphere2.GetRadius();

	if (abs(difference.Magnitude() <= radiiSum)) {
		manifold.collisionNormal = difference;
		manifold.collisionNormal.Normalize();
		manifold.contactPointCount = 1;
		manifold.points[0].position = sphere1.GetPosition() + (manifold.collisionNormal * sphere1.GetRadius());
		manifold.points[0].penetrationDepth = fabs(difference.Magnitude() - radiiSum);

		return true;
	}
	return false;
}

//note that this collision results in inaccurate interpenetration on corners.
bool PhysColliderSys::CollisionCheck(AABBCollider& aabb1, AABBCollider& aabb2, CollisionManifold& manifold) {
	//get distance upon all three axes and then the total extents
	Vector3 displacementBetween = aabb1.GetPosition() - aabb2.GetPosition();
	Vector3 extentsTotal = aabb1.GetExtents() + aabb2.GetExtents();

	if (CheckDistanceToExtents(displacementBetween, extentsTotal)) {
		manifold.collisionNormal = displacementBetween;
		manifold.collisionNormal.Normalize();
		manifold.contactPointCount = 1;
		manifold.points[0].position = aabb1.GetPosition();

		//get distance between along the radius denoted by the collision normal.
		float distanceBetween = DistanceTo(aabb1.GetPosition(), aabb2.GetPosition());
		float radiiTotal = AABBRadiusOnNormal(aabb1.GetExtents(), manifold.collisionNormal) + AABBRadiusOnNormal(aabb2.GetExtents(), manifold.collisionNormal);

		manifold.points[0].penetrationDepth = fabs(radiiTotal - distanceBetween);
		return true;
	}
	return false;
}

// INTERPENETRATION SOLVERS

inline void PhysColliderSys::SolveInterpenetration(PhysicsModel* model1, PhysicsModel* model2, CollisionManifold& manifold) {
	Vector3 interpenetration = (manifold.collisionNormal * manifold.points[0].penetrationDepth);

	//in the event one of these objects are kinematic, only apply the bounce to the non-kinematic body.
	if (model1->IsKinematic()) {
		model2->AddDisplacement(interpenetration);
		return;
	}
	if (model2->IsKinematic()) {
		model1->AddDisplacement(interpenetration);
		return;
	}

	//otherwise apply the bounce to both 
	model1->AddDisplacement(interpenetration);
	model2->AddDisplacement(-interpenetration);
}

// HELPERS

inline float PhysColliderSys::DistanceTo(Vector3& vector1, Vector3& vector2)
{
	//calculate each dimension so they can be multiplied to square rather than using CPU power.
	float x = vector2.x - vector1.x;
	float y = vector2.y - vector1.y;
	float z = vector2.z - vector1.z;

	return sqrtf((x*x) + (y*y) + (z*z));
}

inline float PhysColliderSys::AABBRadiusOnNormal(Vector3& extents, Vector3& normal)
{
	//arbitrary value based on the extents of default aabb's.  
	return 1.0f;
}

inline Vector3 PhysColliderSys::ClosestPointOnPlane(Vector3& pointToCompare, PlaneCollider& plane) {
	//get the closest point on the plane assuming an infinite plane with any normal
	float spot = (plane.GetNormal() * pointToCompare) - plane.GetDistance();
	return pointToCompare - (spot * plane.GetNormal());
}

inline bool PhysColliderSys::CheckDistanceToExtents(Vector3& distance, Vector3& extents) {
	//if the absolute value of any axis of that distance is larger than the box's extent radii, the axis did not overlap.
	//an if statement procedure like this is optimal as we do not RET if we have not needed the return statement.  RET is slower than JMP
	if (abs(distance.x) > extents.x) return false;
	if (abs(distance.y) > extents.y) return false;
	if (abs(distance.z) > extents.z) return false;

	return true;
}

bool PhysColliderSys::CheckDistanceToBoundary(Vector3& distance, float boundary)
{
	if (abs(distance.x) > boundary) return false;
	if (abs(distance.y) > boundary) return false;
	if (abs(distance.z) > boundary) return false;

	return true;
}

inline void PhysColliderSys::ResolveManifold(CollisionManifold manifold, PhysicsModel* modelA, PhysicsModel* modelB) {
	Vector3 relativeVelocity = modelA->GetVelocity() - modelB->GetVelocity();

	//apply the forces necessary
	if (manifold.collisionNormal * relativeVelocity < 0.0f) {
		float elasticity = fminf(modelA->GetRestitution(), modelB->GetRestitution());

		float inverseMassA = modelA->GetInverseMass();
		float inverseMassB = modelB->GetInverseMass();

		float inverseMassSum = inverseMassA + inverseMassB;

		float velocityPower = -(1 + elasticity) * manifold.collisionNormal * relativeVelocity;
		float energy = velocityPower * inverseMassSum;

		modelA->AddImpulse(inverseMassA * energy * manifold.collisionNormal);
		modelB->AddImpulse(-(inverseMassB * energy * manifold.collisionNormal));
	}
}