simulation.cpp

/*
 * This file is part of FGGL.
 *
 * FGGL is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any
 * later version.
 *
 * FGGL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with FGGL.
 * If not, see <https://www.gnu.org/licenses/>.
 */

/*
 * This file is part of FGGL.
 *
 * FGGL is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any
 * later version.
 *
 * FGGL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with FGGL.
 * If not, see <https://www.gnu.org/licenses/>.
 */

#include "../include/fggl/phys/bullet/types.hpp"
#include "../include/fggl/phys/bullet/motion.hpp"

namespace fggl::phys::bullet {

	inline btVector3 to_bullet(const math::vec3 fgglVec) {
		return {fgglVec.x, fgglVec.y, fgglVec.z};
	}

	BulletPhysicsEngine::BulletPhysicsEngine(entity::EntityManager* world) : m_ecs(world), m_config(), m_world(nullptr), m_debug(nullptr) {
		m_config.broadphase = new btDbvtBroadphase();
		m_config.collisionConfiguration = new btDefaultCollisionConfiguration();
		m_config.dispatcher = new btCollisionDispatcher(m_config.collisionConfiguration);
		m_config.solver = new btSequentialImpulseConstraintSolver();

		m_world = new btDiscreteDynamicsWorld(
			m_config.dispatcher,
			m_config.broadphase,
			m_config.solver,
			m_config.collisionConfiguration);


		// callbacks (for handling bullet -> ecs)

		// ensure we deal with ecs -> bullet changes
		//m_ecs->addDeathListener( [this](auto entity) { this->onEntityDeath(entity);} );
	}

	void BulletPhysicsEngine::step() {
		// this is now handled on creation
		//checkForPhys();

		auto dynamicEnts = m_ecs->find<phys::Dynamics, phys::bullet::BulletBody>();
		for (auto& ent : dynamicEnts) {
			auto& dynamicComp = m_ecs->get<phys::Dynamics>(ent);
			auto& bulletProxy = m_ecs->get<phys::bullet::BulletBody>(ent);

			const auto forceVec = dynamicComp.force;
			if ( glm::length(forceVec) != 0.F ) {
				bulletProxy.body->applyCentralForce({forceVec.x, forceVec.y, forceVec.z});
				bulletProxy.body->activate();
				dynamicComp.force = math::VEC3_ZERO;
			}
		}

		m_world->stepSimulation(60.0F);
		//syncToECS();
		pollCollisions();

		// if debug draw is enabled, trigger a draw call
		if ( m_debug != nullptr ) {
			m_world->debugDrawWorld();
		}
	}

	void BulletPhysicsEngine::addBody(entity::EntityID /*entity*/, BulletBody &body) {
		// ensure static objects are placed correctly by setting their transforms to the correct value
		btTransform transform;
		body.body->getMotionState()->getWorldTransform(transform);
		body.body->setWorldTransform(transform);

		m_world->addRigidBody(body.body);
	}

	void BulletPhysicsEngine::setDebugDraw(bool active)  {
		if ( active ) {
			m_debug = std::make_unique<debug::BulletDebugDrawList>();
			m_world->setDebugDrawer(m_debug.get());
			m_debug->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
		} else {
			m_debug = nullptr;
		}
	}

	void BulletPhysicsEngine::syncTransformToECS() {
		const auto physEnts = m_ecs->find<math::Transform, BulletBody>();
		for (const auto& ent : physEnts) {
			auto& physBody = m_ecs->get<BulletBody>(ent);
			// don't bother syncing things with motion states, it's not necessary
			if ( physBody.motion != nullptr ) {
				continue;
			}

			auto& transform = m_ecs->get<math::Transform>(ent);
			if ( physBody.body->isKinematicObject() ) {
				continue;
			}

			btTransform bTransform;
			physBody.motion->getWorldTransform(bTransform);

			// set calculate and set ecs position
			auto& btPos = bTransform.getOrigin();
			math::vec3 pos{btPos.x(), btPos.y(), btPos.z()};
			transform.origin( pos );

			// set the rotation
			const auto& btRot = bTransform.getRotation();

			math::vec3 rot;
			btRot.getEulerZYX(rot.x, rot.y, rot.z);
			transform.euler(rot);
		}
	}

	BulletPhysicsEngine::~BulletPhysicsEngine() {
		// clean up the rigid bodies
		auto entRB = m_ecs->find<BulletBody>();
		for (const auto& ent : entRB) {
			auto& bulletBody = m_ecs->get<BulletBody>(ent);
			if ( bulletBody.body != nullptr ) {
				m_world->removeCollisionObject(bulletBody.body);
			}

			// release resources and delete
			bulletBody.release();
		}

		// delete the world
		delete m_world;

		// cleanup the configuration object
		delete m_config.solver;
		delete m_config.broadphase;
		delete m_config.dispatcher;
		delete m_config.collisionConfiguration;
	}

	static void handleCollisionCallbacks(entity::EntityManager* world, entity::EntityID owner, entity::EntityID other) {
		if ( !world->has<CollisionCallbacks>(owner) ) {
			return;
		}

		auto* callbacks = world->tryGet<CollisionCallbacks>(owner);
		auto* cache = world->tryGet<CollisionCache>(owner);
		if ( cache != nullptr ) {
			auto itr = cache->collisions.find(other);
			if ( itr == cache->collisions.end() ) {
				if ( callbacks->onEnter != nullptr ) {
					callbacks->onEnter(owner, other);
				}
				cache->collisions.insert(  other );
			} else {
				if ( callbacks->onStay != nullptr ) {
					callbacks->onStay(owner, other);
				}
			}
		}
	}

	void BulletPhysicsEngine::pollCollisions() {
		// flush collision caches
		auto caches = m_ecs->find<CollisionCache>();
		for( const auto& ent : caches) {
			auto& cache = m_ecs->get<CollisionCache>(ent);
			std::swap(cache.collisions, cache.lastFrame);
			cache.collisions.clear();
		}

		// deal with the number of manifolds
		const auto numManifolds = m_config.dispatcher->getNumManifolds();
		for ( int i=0; i< numManifolds; ++i) {
			auto* contactManifold = m_config.dispatcher->getManifoldByIndexInternal(i);

			const auto* body0 = contactManifold->getBody0();
			const auto* body1 = contactManifold->getBody1();

			int numContacts = contactManifold->getNumContacts();
			for ( auto contactIdx = 0; contactIdx < numContacts; ++contactIdx ) {
				auto& point = contactManifold->getContactPoint(contactIdx);
				if ( point.getDistance() < 0.0F ) {
					auto worldPosA = point.getPositionWorldOnA();
					auto worldPosB = point.getPositionWorldOnB();
					auto normal = point.m_normalWorldOnB;
				}
			}

			//handleCollisionCallbacks(m_ecs, body0->getUserIndex(), body1->getUserIndex());
			//handleCollisionCallbacks(m_ecs, body1->getUserIndex(), body0->getUserIndex());
		}

		// note contacts that have ended
		caches = m_ecs->find<CollisionCache>(); // re-fetch, entities can (and probably do) die in handlers.
		for( const auto& ent : caches) {
			auto& cache = m_ecs->get<CollisionCache>(ent);

			// if we don't have an exit callback, it's pointless checking the cache
			auto* callbacks = m_ecs->tryGet<CollisionCallbacks>(ent);
			if ( callbacks == nullptr || callbacks->onExit == nullptr){
				continue;
			}

			// check if a collision has ended
			for (const auto& other : cache.lastFrame) {
				auto itr = cache.collisions.find(other);
				if (itr == cache.collisions.end()) {
					callbacks->onExit( ent,other);
				}
			}
		}
	}

	void BulletPhysicsEngine::onEntityDeath(entity::EntityID entity)  {
			auto* btPhysics = m_ecs->tryGet<BulletBody>(entity);
			if ( btPhysics != nullptr) {
				// decouple physics from entity
				//btPhysics->body->setUserIndex( entity::INVALID );
				m_world->removeRigidBody( btPhysics->body );
				btPhysics->release();
			}
	}

	void BulletPhysicsEngine::onCollisionCreate(ContactPoint point) {
		m_contactCache.created.push_back(point);
	}

	void BulletPhysicsEngine::onCollisionProcess(ContactPoint point) {
		m_contactCache.modified.push_back( point );
	}

	void BulletPhysicsEngine::onCollisionDestroyed(ContactPoint point) {
		m_contactCache.removed.push_back(point);
	}

	entity::EntityID BulletPhysicsEngine::raycast(math::vec3 from, math::vec3 to) {
		const auto btFrom = to_bullet(from);
		const auto btTo = to_bullet(to);

		btCollisionWorld::ClosestRayResultCallback rayTestResult(btFrom, btTo);
		m_world->rayTest(btFrom, btTo, rayTestResult);

		if ( !rayTestResult.hasHit() ) {
			return entity::INVALID;
		}

		// tell the user what it hit
		return entity::INVALID;
//		return rayTestResult.m_collisionObject->getUserIndex();
	}

	std::vector<ContactPoint> BulletPhysicsEngine::scanCollisions(entity::EntityID entity) {
		return std::vector<ContactPoint>();
	}

	std::vector<entity::EntityID> BulletPhysicsEngine::raycastAll(math::vec3 fromPos, math::vec3 toPos) {
		const auto btFrom = to_bullet(fromPos);
		const auto btTo = to_bullet(toPos);

		btCollisionWorld::AllHitsRayResultCallback rayTestResult(btFrom, btTo);
		m_world->rayTest(btFrom, btTo, rayTestResult);

		// we didn't hit anything
		if ( !rayTestResult.hasHit() ) {
			return {};
		}

		// hit processing
		const auto hits = rayTestResult.m_collisionObjects.size();

		std::vector<entity::EntityID> hitVec;
		hitVec.reserve(hits);

		for ( auto i = 0; i < hits; ++i) {
			auto entity = rayTestResult.m_collisionObjects[i]->getUserIndex();
			//hitVec.push_back(entity);
		}

		return hitVec;
	}

	std::vector<entity::EntityID> BulletPhysicsEngine::sweep(PhyShape &shape,
														   math::Transform &from,
														   math::Transform &to) {
		return std::vector<entity::EntityID>();
	}

} // namespace fggl::phys::bullet