diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/Agent.cpp | 256 | ||||
| -rw-r--r-- | src/App.cpp | 538 | ||||
| -rw-r--r-- | src/NR_Utils.cpp | 88 | ||||
| -rw-r--r-- | src/Stage.cpp | 192 |
4 files changed, 1074 insertions, 0 deletions
diff --git a/src/Agent.cpp b/src/Agent.cpp new file mode 100644 index 0000000..c082b96 --- /dev/null +++ b/src/Agent.cpp @@ -0,0 +1,256 @@ +#include <Agent.hpp> + +#include <memory> + +#include <HyperNeat/Cppn.hpp> +#include <HyperNeat/Population.hpp> + +using namespace hn; +using namespace sf; +using namespace std; + +size_t +Agent::_currentLifetime = 1500; + +void +Agent::create(Stage& stage) +{ + _eyeSight.resize(5); + + _currCheckPoint = &stage._checkPoints.back(); + _nextCheckPoint = &stage._checkPoints.front(); + + _shape.setRadius(AGENT_RAD); + _shape.setOrigin(AGENT_RAD, AGENT_RAD); + _shape.setFillColor(AGENT_COLOR); + + b2CircleShape agentShape; + agentShape.m_radius = AGENT_RAD; + + b2FixtureDef agentFixture; + agentFixture.shape = &agentShape; + agentFixture.density = 1.0f; + agentFixture.friction = 0.3f; + agentFixture.filter.groupIndex = -1; + agentFixture.userData = this; + + b2BodyDef agentDef; + agentDef.type = b2_dynamicBody; + + _body = stage._world.CreateBody(&agentDef); + _body->CreateFixture(&agentFixture); + + recreate(stage, false); +} + +void +Agent::recreate(const Stage& stage, bool createNNet) +{ + _currCheckPoint = &stage._checkPoints.back(); + _nextCheckPoint = &stage._checkPoints.front(); + + _completedCircuits = 0; + _lifetime = 0; + _distance = 0.0; + _isOnTrap = false; + _isOld = false; + + auto factoryPos = stage._factory.getPosition(); + double factoryRot = stage._factory.getRotation() / IN_RADIANS; + + _body->SetTransform({factoryPos.x, factoryPos.y}, factoryRot); + _body->SetLinearVelocity( { 0.0f, 0.0f }); + _body->SetAngularVelocity(0.0f); + + float pX = _body->GetPosition().x; + float pY = _body->GetPosition().y; + float angle = _body->GetAngle() / IN_DEGREES; + + Transform tr; + tr.translate(pX, pY); + tr.rotate(angle); + + float sight = AGENT_SIGHT; + float iSight = AGENT_SIGHT * 0.7071f; + + _eyeSight[0] = tr.transformPoint({ -sight, 0.0f}); + _eyeSight[1] = tr.transformPoint({-iSight, iSight}); + _eyeSight[2] = tr.transformPoint({ 0.0f, sight}); + _eyeSight[3] = tr.transformPoint({ iSight, iSight}); + _eyeSight[4] = tr.transformPoint({ sight, 0.0f}); + + if (createNNet) { + _organism->createNeuralNet(); + } +} + +void +Agent::setPoints(const Stage& stage, bool passToOrg) +{ + if (!_organism->isOld()) { + return; + } + + // Calculate fitness + double dist = distanceTravelled(stage); + + if (passToOrg) { + _organism->_fitness = (dist / static_cast<double>(max(_lifetime, (size_t)1))); + } +} + +void +Agent::setBehavior(const Stage& stage) +{ + _organism->getBehavior().at(0) = _body->GetPosition().x; + _organism->getBehavior().at(1) = _body->GetPosition().y; + _organism->getBehavior().at(2) = (distanceTravelled(stage) / static_cast<double>(max(_lifetime, (size_t)1))); +} + +void +Agent::update(const Stage& stage, bool withNS) +{ + if (withNS) { + if (_organism->getLifetime() >= (_currentLifetime - 1)) { + if (!_isOld) { + _isOld = true; + } + } else if (_organism->getLifetime() < (_currentLifetime - 1)) { + if (_isOld) { + _isOld = false; + } + } + } + + ++_lifetime; + + if (_isOnTrap) { + return; + } + + float pX = _body->GetPosition().x; + float pY = _body->GetPosition().y; + float angle = _body->GetAngle() / IN_DEGREES; + + Transform tr, rot; + tr.translate(pX, pY); + tr.rotate(angle); + rot.rotate(angle); + + float sight = AGENT_SIGHT; + float iSight = AGENT_SIGHT * 0.7071f; + + _eyeSight[0] = tr.transformPoint({ -sight, 0.0f}); + _eyeSight[1] = tr.transformPoint({-iSight, iSight}); + _eyeSight[2] = tr.transformPoint({ 0.0f, sight}); + _eyeSight[3] = tr.transformPoint({ iSight, iSight}); + _eyeSight[4] = tr.transformPoint({ sight, 0.0f}); + + for (size_t i = 0; i < 5; ++i) { + RayCastCallback callback; + b2Vec2 b2EyeSight = { _eyeSight[i].x, _eyeSight[i].y }; + stage._world.RayCast(&callback, _body->GetPosition(), b2EyeSight); + _organism->_neuralNet->inputAt(i) = callback._fraction; + } + + _organism->_neuralNet->cycle(); + + Vector2f velocity + = rot.transformPoint({ 0.0f, static_cast<float>(_organism->_neuralNet->outputAt(1)) * AGENT_SPEED }); + float rotation + = (((_organism->_neuralNet->outputAt(0) + _organism->_neuralNet->outputAt(2)) * 2.0f - 1.0f) / IN_RADIANS) * + AGENT_SPEED; + + _body->SetLinearVelocity({velocity.x, velocity.y}); + _body->SetAngularVelocity(rotation); +} + +void +Agent::drawOn(sf::RenderWindow& surface) +{ + float pX = _body->GetPosition().x; + float pY = _body->GetPosition().y; + + _shape.setPosition(pX, pY); + + VertexArray eyeSightLines(Lines); + + for (auto & i : _eyeSight) { + eyeSightLines.append({{pX, pY}, SIGHT_COLOR}); + eyeSightLines.append({ i, SIGHT_COLOR}); + } + + surface.draw(_shape); + surface.draw(eyeSightLines); +} + +void +Agent::incrementCheckPoint(Stage& stage) +{ + _currCheckPoint = _nextCheckPoint; + + if (_currCheckPoint == &stage._checkPoints.back()) { + _nextCheckPoint = &stage._checkPoints.front(); + } else { + ++_nextCheckPoint; + } + + if (_currCheckPoint == &stage._checkPoints.back()) { + ++_completedCircuits; + } +} + +double +Agent::distanceTravelled(const Stage& stage) +{ + double pCurr = _currCheckPoint->_value; + double pLoop = stage._checkPoints.back()._value; + + if (_currCheckPoint == &stage._checkPoints.back()) { + pCurr -= pLoop; + } + + double dNext = distanceToCheckpoint(*_nextCheckPoint); + double sNext = _nextCheckPoint->_segment; + + if (dNext > sNext) { + _distance = pCurr + (pLoop * _completedCircuits); + } else { + double pNext = _nextCheckPoint->_value; + double f1 = dNext / sNext; + double f2 = 1.0 - f1; + _distance = (pCurr * f1) + (pNext * f2) + (pLoop * _completedCircuits); + } + + if (_distance < 1.0) { + _distance = 1.0; + } + + return _distance; +} + +float +Agent::distanceToCheckpoint(const Stage::CheckPoint& cp) const +{ + b2Vec2 p = _body->GetPosition(); + b2Vec2 v = {cp._line[0].position.x, cp._line[0].position.y}; + b2Vec2 w = {cp._line[1].position.x, cp._line[1].position.y}; + + float l2 = (w - v).LengthSquared(); + + if (l2 == 0.0f) { + return dist(p, v); + } + + float t = dotP(p - v, w - v) / l2; + + if (t < 0.0f) { + return dist(p, v); + } else if (t > 1.0f) { + return dist(p, w); + } + + b2Vec2 projection = v + t * (w - v); + + return dist(p, projection); +} diff --git a/src/App.cpp b/src/App.cpp new file mode 100644 index 0000000..c97061e --- /dev/null +++ b/src/App.cpp @@ -0,0 +1,538 @@ +#include <App.hpp> +#include <HyperNeat/Cppn.hpp> +#include <tbb/parallel_for.h> +#include <HyperNeat/Utils/Thread.hpp> +#include <HyperNeat/Utils/LoadFile.hpp> +#include <HyperNeat/Utils/SaveFile.hpp> + +using namespace hn; +using namespace sf; +using namespace tbb; +using namespace std; + +int +App::startup() +{ + ifstream paramsFile("parameters"); + ifstream popFile("current.population"); + + if (!paramsFile) { + return EXIT_FAILURE; + } + + cout << "Type hours to run sim.: "; + cin >> _secondsToRun; + _secondsToRun *= (60 * 60); + + bool popLoaded = false; + + if (popFile) { + popLoaded = true; + + LoadFile lFile(popFile); + lFile.loadPopulation(_population); + + _popPrms = _population.getPopulationPrms(); + _doNoveltySearch = _population.isNoveltyMetricSet(); + } + + auto until = numeric_limits<streamsize>::max(); + + while (!paramsFile.eof()) { + paramsFile.ignore(until, '<'); + + string command; + getline(paramsFile, command, '>'); + + if (!popLoaded && command == "POPULATION_PARAMETERS") { + paramsFile.ignore(until, '<'); + + while (paramsFile.peek() != '/') { + string subCommand; + getline(paramsFile, subCommand, '>'); + + if (subCommand == "populationSize") { + paramsFile >> _popPrms._popSize; + } else if (subCommand == "seed") { + paramsFile >> _popPrms._seed; + } else if (subCommand == "weightRange") { + paramsFile >> _popPrms._weightRange; + } else if (subCommand == "c1Disjoint") { + paramsFile >> _popPrms._c1Disjoint; + } else if (subCommand == "c3WeightDifference") { + paramsFile >> _popPrms._c3WeightDifference; + } else if (subCommand == "populationSize") { + paramsFile >> _popPrms._popSize; + } else if (subCommand == "initialDistanceThreshold") { + paramsFile >> _popPrms._initialDistanceThreshold; + } else if (subCommand == "distanceThresholdShift") { + paramsFile >> _popPrms._distanceThresholdShift; + } else if (subCommand == "sexualReproductionRate") { + paramsFile >> _popPrms._sexualReproductionRate; + } else if (subCommand == "weightMutationRate") { + paramsFile >> _popPrms._weightMutationRate; + } else if (subCommand == "weightDeviation") { + paramsFile >> _popPrms._weightDeviation; + } else if (subCommand == "interspeciesMatingRate") { + paramsFile >> _popPrms._interspeciesMatingRate; + } else if (subCommand == "geneDisablingRatio") { + paramsFile >> _popPrms._geneDisablingRatio; + } else if (subCommand == "linkMutationRate") { + paramsFile >> _popPrms._linkMutationRate; + } else if (subCommand == "nodeMutationRate") { + paramsFile >> _popPrms._nodeMutationRate; + } else if (subCommand == "targetSpeciesCount") { + paramsFile >> _popPrms._targetSpeciesCount; + } else if (subCommand == "eligibilityRatio") { + paramsFile >> _popPrms._eligibilityRatio; + } else if (subCommand == "minimumLifetime") { + paramsFile >> _popPrms._minimumLifetime; + } else if (subCommand == "replBeforeReorganization") { + paramsFile >> _popPrms._replBeforeReorganization; + } + + paramsFile.ignore(until, '<'); + } + } else if (command == "NEURAL_NET_PARAMETERS") { + paramsFile.ignore(until, '<'); + + while (paramsFile.peek() != '/') { + string subCommand; + getline(paramsFile, subCommand, '>'); + + if (subCommand == "testGridLevel") { + paramsFile >> _nnPrms._testGridLevel; + } else if (subCommand == "maxQuadTreeLevel") { + paramsFile >> _nnPrms._maxQuadTreeLevel; + } else if (subCommand == "minQuadTreeLevel") { + paramsFile >> _nnPrms._minQuadTreeLevel; + } else if (subCommand == "bandPruningThreshold") { + paramsFile >> _nnPrms._bandPruningThreshold; + } else if (subCommand == "varianceThreshold") { + paramsFile >> _nnPrms._varianceThreshold; + } else if (subCommand == "divisionThreshold") { + paramsFile >> _nnPrms._divisionThreshold; + } else if (subCommand == "searchIterations") { + paramsFile >> _nnPrms._iterations; + } + + paramsFile.ignore(until, '<'); + } + } else if (command == "NOVELTY_SEARCH_PARAMETERS") { + paramsFile.ignore(until, '<'); + + while (paramsFile.peek() != '/') { + string subCommand; + getline(paramsFile, subCommand, '>'); + + if (subCommand == "doNoveltySearch") { + paramsFile >> _doNoveltySearch; + } else if (subCommand == "noveltyThreshold") { + paramsFile >> _noveltyPrms._noveltyThreshold; + } else if (subCommand == "referenceOrganisms") { + paramsFile >> _noveltyPrms._referenceOrganisms; + } + + _noveltyPrms._characterizationSize = 3; + + paramsFile.ignore(until, '<'); + } + } else if (command == "STAGE_PARAMETERS") { + paramsFile.ignore(until, '<'); + + while (paramsFile.peek() != '/') { + string subCommand; + getline(paramsFile, subCommand, '>'); + + if (subCommand == "file") { + string fileName; + paramsFile >> fileName; + + if (_stage.load(fileName) == EXIT_FAILURE) { + return EXIT_FAILURE; + } + } else if (subCommand == "draw") { + paramsFile >> _draw; + } + + paramsFile.ignore(until, '<'); + } + } + } + + if (!_draw) { + _isVSyncEnabled = false; + _isPaused = false; + } + + _nnPrms._inputMap = { + { -1.0, -0.5 }, + { -0.7, 0.2 }, + { 0.0, 0.5 }, + { 0.7, 0.2 }, + { 1.0, -0.5 } + }; + + _nnPrms._outputMap = { + { -0.5, -0.5 }, + { 0.0, 0.0 }, + { 0.5, -0.5 } + }; + + if (!popLoaded) { + _population.create(_popPrms, _nnPrms); + } + + if (_doNoveltySearch && !_population.isNoveltyMetricSet()) { + _population.setNoveltyMetric(_noveltyPrms); + } else if (!_doNoveltySearch && _population.isNoveltyMetricSet()) { + _population.clearNoveltyMetric(); + } + + _agents.resize(_popPrms._popSize, Agent()); + + for (size_t i = 0; i < _agents.size(); ++i) { + _agents[i]._organism = &_population.getOrganism(i); + _agents[i].create(_stage); + } + + // Agent::_currentLifetime = (_stage._checkPoints.back()._value / 1.75); + + _champ.setFillColor(Color::Transparent); + _champ.setOutlineColor(CHAMP_COLOR); + _champ.setOutlineThickness(2); + _champ.setRadius(AGENT_RAD + 2); + _champ.setOrigin(AGENT_RAD + 2, AGENT_RAD + 2); + + _selected = _champ; + _selected.setOutlineColor(SELECTED_COLOR); + + // Print NoveltyMarks + if (_population.isNoveltyMetricSet()) { + for (auto& i : _population.getNoveltyMetric().getArchive()) { + _noveltyMarks.emplace_back(); + _noveltyMarks.back().setFillColor({ 255, 255, 0, 10 }); + _noveltyMarks.back().setRadius(AGENT_RAD + 2); + _noveltyMarks.back().setOrigin(AGENT_RAD + 2, AGENT_RAD + 2); + _noveltyMarks.back().setPosition(i[0], i[1]); + } + } + + // Prepare text + if (!_font.loadFromFile("C:/Windows/Fonts/font_1.ttf")) { + return EXIT_FAILURE; + } + + _updates.setFont(_font); + _champFitness.setFont(_font); + _avergFitness.setFont(_font); + + _updates.setCharacterSize(11); + _champFitness.setCharacterSize(11); + _avergFitness.setCharacterSize(11); + + _updates.setPosition(5, 5); + _champFitness.setPosition(5, 18); + _avergFitness.setPosition(5, 31); + + _updates.setColor(TEXT_COLOR); + _champFitness.setColor(TEXT_COLOR); + _avergFitness.setColor(TEXT_COLOR); + + return EXIT_SUCCESS; +} + +int +App::execute() +{ + if (!_draw) { + hn::Thread runner([&]() { + while (_runningWOGfx) { + update(); + } + }); + + cin.get(); + cin.get(); + + _runningWOGfx = false; + runner.join(); + + return EXIT_SUCCESS; + } + + _window.create(VIDEO_MODE, WIN_TITLE, WIN_STYLE, WIN_SETTINGS); + + _clock.restart(); + _globalClock.restart(); + + while (_window.isOpen()) { + if (!_isPaused) { + update(); + } + + Event event; + + while (_window.pollEvent(event)) { + if (event.type == Event::Closed) { + _cppnEx.shutdown(); + _window.close(); + } else if (event.type == Event::KeyPressed) { + if (event.key.code == Keyboard::Space) { + _isPaused = !_isPaused; + } else if (event.key.code == Keyboard::V) { + _isVSyncEnabled = !_isVSyncEnabled; + + if (_isVSyncEnabled) { + _window.setVerticalSyncEnabled(true); + } else { + _window.setVerticalSyncEnabled(false); + } + } else if (event.key.code == Keyboard::L) { + if (_selectedIdx != -1) { + if (_population.isOrganismLocked(_selectedIdx)) { + _population.unlockOrganism(_selectedIdx); + } else { + _population.lockOrganism(_selectedIdx); + } + } + } else if (event.key.code == Keyboard::F) { + if (_selectedIdx != -1) { + if (_population.isOrganismFrozen(_selectedIdx)) { + _population.unfreezeOrganism(_selectedIdx); + } else { + _population.freezeOrganism(_selectedIdx); + } + } + } else if (event.key.code == Keyboard::C) { + if (_selectedIdx != -1) { + auto& genome = _population.getOrganism(_selectedIdx).getGenome(); + _cppnEx.run(genome, &_nnPrms); + } + } + } else if (event.type == Event::MouseButtonPressed && event.mouseButton.button == Mouse::Left) { + float cX = static_cast<float>(Mouse::getPosition(_window).x); + float cY = static_cast<float>(Mouse::getPosition(_window).y); + + b2AABB aabb; + aabb.lowerBound = { cX - 0.5f, cY - 0.5f }; + aabb.upperBound = { cX + 0.5f, cY + 0.5f }; + + QueryCallback qCallback; + + _stage._world.QueryAABB(&qCallback, aabb); + + if (qCallback._fixture) { + if (qCallback._fixture->GetFilterData().groupIndex == -1) { + Agent* agent = static_cast<Agent*>(qCallback._fixture->GetUserData()); + size_t agentIdx = 0; + + while (&_agents[agentIdx] != agent) { + ++agentIdx; + } + + _selectedIdx = agentIdx; + + size_t oldOrg = 0; + double averageDist = 0.0; + + for (auto& i : _agents) { + if (i._organism->isOld()) { + ++oldOrg; + averageDist += i._distance; + } + } + + averageDist /= static_cast<double>(oldOrg); + + cout.setf(ios::boolalpha); + cout << endl; + cout << "completedCircuits : " << _agents[agentIdx]._completedCircuits << endl; + cout << "lifetime : " << _agents[agentIdx]._lifetime << endl; + cout << "distance : " << _agents[agentIdx]._distance << endl; + cout << "isOnTrap : " << _agents[agentIdx]._isOnTrap << endl; + cout << "isOld : " << _agents[agentIdx]._organism->isOld() << endl; + // cout << "noveltyScore : " << _agents[agentIdx]._organism->getBehavior().getNoveltyScore() << endl; + // cout << "criteriaReached : " << _agents[agentIdx]._organism->getBehavior()._criteriaReached << endl; + cout << "averageDist : " << averageDist << endl; + cout << endl; + } else { + _selectedIdx = -1; + } + } else { + _selectedIdx = -1; + } + } + } + + draw(); + } + + return EXIT_SUCCESS; +} + +void +App::update() +{ + parallel_for(size_t(0), _agents.size(), size_t(1), [&](size_t i) { + if (!_agents[i]._organism->isBeingGenerated()) { + _agents[i].update(_stage, _doNoveltySearch); + } + }); + + _population.update([&]() { + if (!_doNoveltySearch) { + for (auto& i : _agents) { + i.setPoints(_stage, true); + } + } else { + for (auto& i : _agents) { + i.setBehavior(_stage); + } + } + }, [&]() { + _agents[_population.getLastReplacement()->getIndex()].recreate(_stage, true); + }); + + if (_doNoveltySearch) { + size_t maxCircuits = 0; + + for (auto& i : _agents) { + maxCircuits = max(maxCircuits, i._completedCircuits); + } + + if (maxCircuits != _completedCircuits) { + _completedCircuits = maxCircuits; + size_t newLifetime = (_stage._checkPoints.back()._value / 1.75) * (_completedCircuits + 1); + + _population.setMinimumLifetime(newLifetime); + Agent::_currentLifetime = newLifetime; + } + } + + for (auto& i : _stage._traps) { + TrapCallback trapCallback; + _stage._world.QueryAABB(&trapCallback, i); + } + + if (_doNoveltySearch) { + auto& archive = _population.getNoveltyMetric().getArchive(); + + if (archive.size() > _noveltyMarks.size()) { + for (size_t i = _noveltyMarks.size(); i < archive.size(); ++i) { + _noveltyMarks.emplace_back(); + _noveltyMarks.back().setFillColor({ 255, 255, 0, 10 }); + _noveltyMarks.back().setRadius(AGENT_RAD + 2); + _noveltyMarks.back().setOrigin(AGENT_RAD + 2, AGENT_RAD + 2); + _noveltyMarks.back().setPosition(archive[i][0], archive[i][1]); + } + } + } + + _stage.update(); + + if (_population.getUpdates() % 10000 == 0) { + ofstream popFile("current.population"); + SaveFile sFile(popFile); + sFile.savePopulation(_population, true); + + using chrono::system_clock; + system_clock::time_point today = system_clock::now(); + + time_t tt = system_clock::to_time_t(today); + cout << "> Saved on : " << ctime(&tt); + cout << "> Champ Fitness : " << _population.getChampion()._fitness << endl; + cout << "> Comp. Circuits : " << _completedCircuits << endl; + + if (_population.isNoveltyMetricSet()) { + cout << "> Archive Size : " << _population.getNoveltyMetric().getArchive().size() << endl << endl; + } else { + cout << endl; + } + } + + if (_globalClock.getElapsedTime().asSeconds() > _secondsToRun) { + _window.close(); + _runningWOGfx = false; + } +} + +void +App::draw() +{ + if (!_isVSyncEnabled) { + if (_clock.getElapsedTime().asSeconds() < 1.0f / 60.0f) { + return; + } + + _clock.restart(); + } + + _window.clear(BG_COLOR); + + _stage.drawOn(_window); + + for (auto& i : _noveltyMarks) { + _window.draw(i); + } + + for (size_t i = 0, end = _agents.size(); i < end; ++i) { + if (!_population.isOrganismBeingGenerated(i)) { + _agents[i].drawOn(_window); + } + } + + // Highlight champ + double champFtn = _population.getChampion()._fitness; + ssize_t champIdx = _population.getChampion().getIndex(); + double averageFitness = _population.getAverageFitness(); + + if (champIdx != -1) { + auto cPos = _agents[champIdx]._body->GetPosition(); + _champ.setPosition(cPos.x, cPos.y); + _window.draw(_champ); + } + + // Highlight selected + if (_selectedIdx != -1) { + auto sPos = _agents[_selectedIdx]._body->GetPosition(); + _selected.setPosition(sPos.x, sPos.y); + _window.draw(_selected); + } + + // Print text + _updates.setString ("Simulation updates : " + toString(_population.getUpdates())); + _champFitness.setString("Champ's fitness : " + toString(champFtn)); + _avergFitness.setString("Average fitness : " + toString(averageFitness)); + + _window.draw(_updates); + _window.draw(_champFitness); + _window.draw(_avergFitness); + + _window.display(); +} + +void +App::shutdown() +{ + _population.shutdown(true); + + ofstream popFile("current.population"); + SaveFile sFile(popFile); + sFile.savePopulation(_population); + + cout << "SHUTTING DOWN!" << endl; +} + +int +main() +{ + App app; + + if (app.startup() == EXIT_FAILURE || app.execute() == EXIT_FAILURE) { + return EXIT_FAILURE; + } else { + app.shutdown(); + return EXIT_SUCCESS; + } +} diff --git a/src/NR_Utils.cpp b/src/NR_Utils.cpp new file mode 100644 index 0000000..2768dad --- /dev/null +++ b/src/NR_Utils.cpp @@ -0,0 +1,88 @@ +#include <NR_Utils.hpp> +#include <Agent.hpp> +#include <Stage.hpp> + +using namespace std; + +ssize_t +pMod(ssize_t n, ssize_t d) +{ + return (n % d + d) % d; +} + +float +dist(const b2Vec2& a, const b2Vec2 b) +{ + auto x = a.x - b.x; + auto y = a.y - b.y; + + return sqrt(x * x + y * y); +} + +float +dotP(const b2Vec2& a, const b2Vec2 b) +{ + return a.x * b.x + a.y * b.y; +} + +float32 +RayCastCallback::ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction) +{ + if (fixture->GetFilterData().groupIndex == -1 || fixture->IsSensor()) { + return -1.0f; + } + + _fixture = fixture; + _fraction = 1.0f - fraction; + + return fraction; +} + +bool +QueryCallback::ReportFixture(b2Fixture* fixture) +{ + _fixture = fixture; + + return false; +} + +bool +TrapCallback::ReportFixture(b2Fixture* fixture) +{ + if (fixture->GetBody()->GetType() != b2_dynamicBody) { + return true; + } + + Agent& agent = *static_cast<Agent*>(fixture->GetUserData()); + agent._isOnTrap = true; + agent._body->SetLinearVelocity({ 0.0f, 0.0f }); + agent._body->SetAngularVelocity(0.0f); + + return true; +} + +void +ContactListener::BeginContact(b2Contact* contact) +{ + b2Fixture* fixA = contact->GetFixtureA(); + b2Fixture* fixB = contact->GetFixtureB(); + + if (!fixA->IsSensor() && !fixB->IsSensor()) { + return; + } + + Agent* agent = nullptr; + b2Fixture* check = nullptr; + + if (fixA->IsSensor()) { + check = fixA; + agent = static_cast<Agent*>(fixB->GetUserData()); + } else { + check = fixB; + agent = static_cast<Agent*>(fixA->GetUserData()); + } + + if (check->GetUserData() == agent->_nextCheckPoint) { + agent->incrementCheckPoint(*_stage); + } +} diff --git a/src/Stage.cpp b/src/Stage.cpp new file mode 100644 index 0000000..b9fa67a --- /dev/null +++ b/src/Stage.cpp @@ -0,0 +1,192 @@ +#include <Stage.hpp> + +using namespace sf; +using namespace std; + +int +Stage::load(const std::string& fileName) +{ + ifstream stageFile(fileName); + + if (!stageFile) { + return EXIT_FAILURE; + } + + auto until = numeric_limits<streamsize>::max(); + + while (!stageFile.eof()) { + stageFile.ignore(until, '<'); + + string command; + getline(stageFile, command, '>'); + + if (command == "WALL") { + _walls.emplace_back(LinesStrip); + hn::Vector<b2Vec2> vertices; + + stageFile.ignore(until, '<'); + + while (stageFile.peek() != '/') { + Vector2f vtxPos; + + stageFile >> vtxPos.x; + stageFile.ignore(until, ','); + stageFile >> vtxPos.y; + stageFile.ignore(until, '<'); + + _walls.back().append({ vtxPos, WALL_COLOR }); + vertices.emplace_back(vtxPos.x, vtxPos.y); + } + + b2ChainShape shape; + shape.CreateChain(vertices.data(), vertices.size()); + + b2BodyDef bodyDef; + _wallBodies.emplace_back(_world.CreateBody(&bodyDef)); + _wallBodies.back()->CreateFixture(&shape, 0.0f); + } else if (command == "FACTORY") { + Vector2f factPos; + double factRot = 0.0; + + stageFile.ignore(until, '<'); + stageFile >> factPos.x; + stageFile.ignore(until, ','); + stageFile >> factPos.y; + stageFile.ignore(until, ','); + stageFile >> factRot; + stageFile.ignore(until, '<'); + + _factory.setPosition(factPos); + _factory.setRotation(factRot); + _factory.setOrigin(AGENT_RAD + 2, AGENT_RAD + 2); + _factory.setFillColor(Color::Black); + _factory.setOutlineColor(FACTORY_COLOR); + _factory.setOutlineThickness(2.0f); + } else if (command == "CHECKPOINT") { + stageFile.ignore(until, '<'); + + while (stageFile.peek() != '/') { + _checkPoints.emplace_back(); + + Vector2f vtx1Pos; + Vector2f vtx2Pos; + b2Vec2 shVx[2]; + + stageFile >> vtx1Pos.x; + shVx[0].x = vtx1Pos.x; + stageFile.ignore(until, ','); + stageFile >> vtx1Pos.y; + shVx[0].y = vtx1Pos.y; + stageFile.ignore(until, ','); + stageFile >> vtx2Pos.x; + shVx[1].x = vtx2Pos.x; + stageFile.ignore(until, ','); + stageFile >> vtx2Pos.y; + shVx[1].y = vtx2Pos.y; + stageFile.ignore(until, ','); + stageFile >> _checkPoints.back()._value; + stageFile.ignore(until, '<'); + + _checkPoints.back()._line.append({ vtx1Pos, CHECKPNT_COLOR }); + _checkPoints.back()._line.append({ vtx2Pos, CHECKPNT_COLOR }); + + _checkPoints.front().calculateSegment(_checkPoints.back()); + + b2ChainShape cpShape; + cpShape.CreateChain(shVx, 2); + + b2FixtureDef cpFixture; + cpFixture.shape = &cpShape; + cpFixture.isSensor = true; + + b2BodyDef cpDef; + _checkPoints.back()._body = _world.CreateBody(&cpDef); + _checkPoints.back()._body->CreateFixture(&cpFixture); + + if (_checkPoints.size() > 1) { + auto& prev = _checkPoints[_checkPoints.size() - 2]; + + _checkPoints.back().calculateSegment(prev); + } + } + + for (auto & i : _checkPoints) { + i._body->GetFixtureList()->SetUserData(&i); + } + } else if (command == "TRAP") { + stageFile.ignore(until, '<'); + + while (stageFile.peek() != '/') { + sf::Vector2f v1; + sf::Vector2f v2; + + stageFile >> v1.x; + stageFile.ignore(until, ','); + stageFile >> v1.y; + stageFile.ignore(until, ','); + stageFile >> v2.x; + stageFile.ignore(until, ','); + stageFile >> v2.y; + stageFile.ignore(until, '<'); + + _trapRects.emplace_back(sf::Vector2f(v2.x - v1.x, v2.y - v1.y)); + _trapRects.back().setPosition(v1); + _trapRects.back().setFillColor({ 128, 32, 32, 128 }); + + _traps.emplace_back(); + _traps.back().lowerBound.Set(v1.x, v1.y); + _traps.back().upperBound.Set(v2.x, v2.y); + } + } + } + + _world.SetContactListener(&_listener); + _listener._stage = this; + + return EXIT_SUCCESS; +} + +void +Stage::update() +{ + _world.Step(_TIME_STEP, _VEL_ITERS, _POS_ITERS); +} + +void +Stage::drawOn(RenderWindow& surface) const +{ + for (auto& i : _trapRects) { + surface.draw(i); + } + + for (auto& i : _walls) { + surface.draw(i); + } + + surface.draw(_factory); + + for (auto& i : _checkPoints) { + surface.draw(i._line); + } +} + +void +Stage::CheckPoint::calculateSegment(const CheckPoint& prev) +{ + float32 a0X = prev._line[0].position.x; + float32 a0Y = prev._line[0].position.y; + float32 a1X = prev._line[1].position.x; + float32 a1Y = prev._line[1].position.y; + + float32 b0X = _line[0].position.x; + float32 b0Y = _line[0].position.y; + float32 b1X = _line[1].position.x; + float32 b1Y = _line[1].position.y; + + float32 dA0B0 = dist( { a0X, a0Y }, { b0X, b0Y }); + float32 dA0B1 = dist( { a0X, a0Y }, { b1X, b1Y }); + float32 dA1B0 = dist( { a1X, a1Y }, { b0X, b0Y }); + float32 dA1B1 = dist( { a1X, a1Y }, { b1X, b1Y }); + + _segment = min(min(dA0B0, dA0B1), min(dA1B0, dA1B1)); +} |