ClientController.cpp

#include <numeric>
#include <iostream>

#define GLM_FORCE_ALIGNED_GENTYPES
#include "glm/gtx/matrix_decompose.hpp"
#include "glm/gtx/string_cast.hpp"
#include "glm/gtx/projection.hpp"

#include "controllers/ClientController.h"

class IgnoreBodyAndGhostCast :
	public btCollisionWorld::ClosestRayResultCallback
{
private:
	btRigidBody* m_pBody;
	btPairCachingGhostObject* m_pGhostObject;

public:
	IgnoreBodyAndGhostCast(btRigidBody* pBody, btPairCachingGhostObject* pGhostObject)
		: btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)),
		m_pBody(pBody), m_pGhostObject(pGhostObject)
	{
	}

	btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
	{
		if (rayResult.m_collisionObject == m_pBody || rayResult.m_collisionObject == m_pGhostObject)
			return 1.0f;

		return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
	}
};


ClientController::ClientController(vel::scene::stage::CollisionWorld* collisionWorld, vel::scene::stage::Actor* clientActor) :

	collisionWorld(collisionWorld),
	clientActor(clientActor),

	radius(0.625f),
	height(0.55f),
	mass(1.0f),

	bottomYOffset(height / 2.0f + this->radius),
	bottomRoundedRegionYOffset((height + radius) / 2.0f),
	onGround(false),
	hittingWall(false),
	stepHeight(0.1f),


	moveDirection(glm::vec3(0.0f, 0.0f, -1.0f)),
	lookDirection(glm::vec3(0.0f, 0.0f, -1.0f)),
	moveSpeed(4.0f),
	mouseDeltaBufferSize(3),
	yaw(270.0f),
	pitch(0.0f),
	jumping(false),
	falling(false),
	firstMouseInput(true),
	mouseDeltaBufferCursor(0),
	up(glm::vec3(0.0f, 1.0f, 0.0f)),
	velocity(glm::vec3(0.0f, 0.0f, 0.0f)),
	//jumpHeight(0.0f),
	//jumpDistance(1.5f),
	//jumpSpeed(4.0f),
	vel::scene::stage::Controller()
{
	// update pitch and yaw with values from clientActor transform
	//glm::vec3 eulers = glm::eulerAngles(clientActor->getTransform().getRotation()) * 180.0f / 3.14159f;
	//this->pitch = eulers.x;
	//this->yaw = eulers.y;
	//std::cout << eulers.x << "\n";

	// fill mouse delta buffers
	for (size_t i = 0; i < this->mouseDeltaBufferSize; i++)
	{
		this->mouseDeltaBufferX.push_back(0.0f);
		this->mouseDeltaBufferY.push_back(0.0f);
	}

	////////////////////

	btCollisionShape* capsuleShape = new btCapsuleShape(this->radius, this->height); // total height is height+2*radius
	this->collisionWorld->collisionShapes.push_back(capsuleShape);

	btTransform capsuleTransform;
	capsuleTransform.setIdentity();
	auto actorTranslation = this->clientActor->getTransform().getTranslation();
	capsuleTransform.setOrigin(btVector3(actorTranslation.x, actorTranslation.y, actorTranslation.z));

	btVector3 capsuleInertia(0.0f, 0.0f, 0.0f);
	capsuleShape->calculateLocalInertia(this->mass, capsuleInertia);

	this->motionState = new btDefaultMotionState(capsuleTransform);
	btRigidBody::btRigidBodyConstructionInfo clientBodyInfo(this->mass, this->motionState, capsuleShape, capsuleInertia);
	clientBodyInfo.m_friction = 0.0f;
	clientBodyInfo.m_restitution = 0.0f;
	clientBodyInfo.m_linearDamping = 0.0f;

	this->clientBody = new btRigidBody(clientBodyInfo);
	this->clientBody->setAngularFactor(btVector3(0.0f, 0.0f, 0.0f));
	this->clientBody->setActivationState(DISABLE_DEACTIVATION);

	this->collisionWorld->dynamicsWorld->addRigidBody(this->clientBody);


	// Ghost object that is synchronized with rigid body
	this->ghostObject = new btPairCachingGhostObject();

	this->ghostObject->setCollisionShape(capsuleShape);
	this->ghostObject->setUserPointer(this);
	this->ghostObject->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);

	// Specify filters manually, otherwise ghost doesn't collide with statics for some reason
	this->collisionWorld->dynamicsWorld->addCollisionObject(this->ghostObject, 
		btBroadphaseProxy::KinematicFilter, btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);

};

void ClientController::logic()
{
	// Synch ghost with actually object
	this->ghostObject->setWorldTransform(this->clientBody->getWorldTransform());

	// Update transform
	this->motionState->getWorldTransform(this->motionTransform);

	this->onGround = false;

	this->parseGhostContacts();
	this->updatePosition();

	this->updateRotation();
	this->updateVelocity();



	// update our position based on physics object (which handles collision response)
	auto newPosition = glm::vec3(this->clientBody->getWorldTransform().getOrigin().getX(),
		this->clientBody->getWorldTransform().getOrigin().getY(),
		this->clientBody->getWorldTransform().getOrigin().getZ());

	this->clientActor->getTransform().setTranslation(newPosition);

	//auto newPosition = this->clientActor->getTransform().getTranslation() + this->velocity;
	//this->clientActor->getTransform().setTranslation(newPosition);

	//// update our falling switch
	//this->falling = this->ellipsoidCollider.getFalling();

	// update client actor rotation (definitely a better way to do this, but this should work for now)
	glm::mat4 rotMat = glm::lookAt(newPosition, newPosition + this->moveDirection, glm::vec3(0.0f, 1.0f, 0.0f));
	glm::vec3 scale;
	glm::quat rotation;
	glm::vec3 translation;
	glm::vec3 skew;
	glm::vec4 perspective;
	glm::decompose(rotMat, scale, rotation, translation, skew, perspective);
	rotation = glm::conjugate(rotation);

	this->clientActor->getTransform().setRotation(rotation);

}


void ClientController::parseGhostContacts()
{
	btManifoldArray manifoldArray;
	btBroadphasePairArray &pairArray = this->ghostObject->getOverlappingPairCache()->getOverlappingPairArray();
	int numPairs = pairArray.size();

	// Set false now, may be set true in test
	this->hittingWall = false;

	this->surfaceHitNormals.clear();

	for (int i = 0; i < numPairs; i++)
	{
		std::cout << "here002\n";
		manifoldArray.clear();

		const btBroadphasePair &pair = pairArray[i];

		btBroadphasePair* collisionPair = this->collisionWorld->dynamicsWorld->getPairCache()->findPair(pair.m_pProxy0, pair.m_pProxy1);

		if (collisionPair == NULL)
			continue;

		if (collisionPair->m_algorithm != NULL)
			collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);

		for (int j = 0; j < manifoldArray.size(); j++)
		{
			btPersistentManifold* pManifold = manifoldArray[j];

			// Skip the rigid body the ghost monitors
			if (pManifold->getBody0() == this->clientBody)
				continue;

			for (int p = 0; p < pManifold->getNumContacts(); p++)
			{
				const btManifoldPoint &point = pManifold->getContactPoint(p);

				if (point.getDistance() < 0.0f)
				{
					//const btVector3 &ptA = point.getPositionWorldOnA();
					const btVector3 &ptB = point.getPositionWorldOnB();

					//const btVector3 &normalOnB = point.m_normalWorldOnB;

					// If point is in rounded bottom region of capsule shape, it is on the ground
					if (ptB.getY() < this->motionTransform.getOrigin().getY() - this->bottomRoundedRegionYOffset)
						this->onGround = true;
					else
					{
						this->hittingWall = true;

						this->surfaceHitNormals.push_back(glm::vec3(point.m_normalWorldOnB.getX(), point.m_normalWorldOnB.getY(), point.m_normalWorldOnB.getZ()));
					}
				}
			}
		}
	}
}

void ClientController::updatePosition()
{
	// Ray cast, ignore rigid body
	IgnoreBodyAndGhostCast rayCallBack_bottom(this->clientBody, this->ghostObject);

	this->collisionWorld->dynamicsWorld->rayTest(
		this->clientBody->getWorldTransform().getOrigin(), 
		this->clientBody->getWorldTransform().getOrigin() - btVector3(0.0f, this->bottomYOffset + this->stepHeight, 0.0f), 
		rayCallBack_bottom
	);

	// Bump up if hit
	if (rayCallBack_bottom.hasHit())
	{
		//std::cout << "here003\n";
		float previousY = this->clientBody->getWorldTransform().getOrigin().getY();

		this->clientBody->getWorldTransform().getOrigin().setY(previousY + (this->bottomYOffset + this->stepHeight) * (1.0f - rayCallBack_bottom.m_closestHitFraction));

		btVector3 vel(this->clientBody->getLinearVelocity());

		vel.setY(0.0f);

		this->clientBody->setLinearVelocity(vel);

		this->onGround = true;
	}

	float testOffset = 0.07f;

	// Ray cast, ignore rigid body
	IgnoreBodyAndGhostCast rayCallBack_top(this->clientBody, this->ghostObject);

	this->collisionWorld->dynamicsWorld->rayTest(
		this->clientBody->getWorldTransform().getOrigin(), 
		this->clientBody->getWorldTransform().getOrigin() + btVector3(0.0f, this->bottomYOffset + testOffset, 0.0f), 
		rayCallBack_top
	);

	// Bump up if hit
	if (rayCallBack_top.hasHit())
	{
		this->clientBody->getWorldTransform().setOrigin(this->previousPosition);

		btVector3 vel(this->clientBody->getLinearVelocity());

		vel.setY(0.0f);

		this->clientBody->setLinearVelocity(vel);
	}

	this->previousPosition = this->clientBody->getWorldTransform().getOrigin();
}


void ClientController::updateVelocity()
{
	//float currentSpeed = this->moveSpeed * this->deltaTime;
	float currentSpeed = this->moveSpeed;

	//this->velocity = glm::vec3(0.0f, this->clientBody->getLinearVelocity().getY(), 0.0f);

	this->velocity.y = this->clientBody->getLinearVelocity().getY();

	if (this->input.keyW)
		this->velocity += currentSpeed * this->moveDirection;
	if (this->input.keyS)
		this->velocity -= currentSpeed * this->moveDirection;
	if (this->input.keyA)
		this->velocity -= glm::normalize(glm::cross(this->moveDirection, this->up)) * currentSpeed;
	if (this->input.keyD)
		this->velocity += glm::normalize(glm::cross(this->moveDirection, this->up)) * currentSpeed;

	this->clientBody->setLinearVelocity(btVector3(this->velocity.x, this->velocity.y, this->velocity.z));

	this->velocity -= this->velocity;

	if (this->hittingWall)
	{
		std::cout << "hittingWall -- " << this->surfaceHitNormals.size() << "\n";
		for (unsigned int i = 0, size = this->surfaceHitNormals.size(); i < size; i++)
		{
			// Cancel velocity across normal
			//Vec3f velInNormalDir(m_manualVelocity.Project(m_surfaceHitNormals[i]));
			glm::vec3 velInNormalDir = glm::proj(this->velocity, this->surfaceHitNormals[i]);

			// Apply correction
			this->velocity -= velInNormalDir * 1.05f;
		}

		// Do not adjust rigid body velocity manually (so bodies can still be pushed by character)
		return;
	}

	//if (this->input.keySpace)
	//{
	//	if (!this->jumping && !this->falling)
	//	{
	//		this->jumping = true;
	//		this->jumpHeight = this->clientActor->getTransform().getTranslation().y + this->jumpDistance;
	//	}
	//}

	//if (this->jumping)
	//{
	//	if (this->clientActor->getTransform().getTranslation().y < this->jumpHeight) 
	//	{
	//		this->velocity += (this->jumpSpeed * this->deltaTime) * this->up;
	//	}
	//	else
	//	{
	//		this->falling = true;
	//		this->jumping = false;
	//	}
	//}

}

void ClientController::updateRotation()
{
	this->updatePitchYaw();

	// rotate movement direction
	this->moveDirection.x = cos(glm::radians(this->yaw));
	this->moveDirection.y = 0.0f;
	this->moveDirection.z = sin(glm::radians(this->yaw));
	this->moveDirection = glm::normalize(this->moveDirection);

	// rotate view direction
	this->lookDirection.x = cos(glm::radians(this->yaw)) * cos(glm::radians(this->pitch));
	this->lookDirection.y = sin(glm::radians(this->pitch));
	this->lookDirection.z = sin(glm::radians(this->yaw)) * cos(glm::radians(this->pitch));
	this->lookDirection = glm::normalize(this->lookDirection);
}

void ClientController::updatePitchYawRaw()
{
	//std::cout << this->pitch << "\n";
	float sensitivity = 0.09f; // change accordingly

	this->yaw += ((this->input.mouseXPos - this->lastX)) * sensitivity;
	this->pitch += ((this->lastY - this->input.mouseYPos)) * sensitivity;

	this->lastX = this->input.mouseXPos;
	this->lastY = this->input.mouseYPos;
}

void ClientController::updatePitchYawSmoothed()
{
	float sensitivity = 0.1f; // change accordingly

	float xOffset = (this->input.mouseXPos - this->lastX) * sensitivity;
	float yOffset = (this->lastY - this->input.mouseYPos) * sensitivity;

	this->lastX = this->input.mouseXPos;
	this->lastY = this->input.mouseYPos;


	this->mouseDeltaBufferX[this->mouseDeltaBufferCursor] = xOffset;
	this->mouseDeltaBufferY[this->mouseDeltaBufferCursor] = yOffset;

	this->mouseDeltaBufferCursor = this->mouseDeltaBufferCursor == (this->mouseDeltaBufferSize - 1) ? 0 : this->mouseDeltaBufferCursor + 1;


	this->yaw += (std::accumulate(this->mouseDeltaBufferX.begin(), this->mouseDeltaBufferX.end(), 0.0f) / (float)this->mouseDeltaBufferSize);
	this->pitch += (std::accumulate(this->mouseDeltaBufferY.begin(), this->mouseDeltaBufferY.end(), 0.0f) / (float)this->mouseDeltaBufferSize);
}

void ClientController::updatePitchYawKeyboard()
{
	float currentSpeed = (this->moveSpeed * 16) * this->deltaTime;

	if (this->input.keyUp)
		this->pitch += currentSpeed;
	if (this->input.keyDown)
		this->pitch -= currentSpeed;
	if (this->input.keyLeft)
		this->yaw -= currentSpeed;
	if (this->input.keyRight)
		this->yaw += currentSpeed;
}

void ClientController::updatePitchYaw()
{
	if (this->firstMouseInput)
	{
		this->lastX = this->input.mouseXPos;
		this->lastY = this->input.mouseYPos;
		this->firstMouseInput = false;
	}


	this->updatePitchYawRaw();

	//this->updatePitchYawSmoothed();

	//this->updatePitchYawKeyboard();


	// make sure that when pitch is out of bounds, screen doesn't get flipped
	if (this->pitch > 89.0f)
		this->pitch = 89.0f;
	if (this->pitch < -89.0f)
		this->pitch = -89.0f;
}