diff options
Diffstat (limited to 'src/Agent.cpp')
| -rw-r--r-- | src/Agent.cpp | 256 |
1 files changed, 256 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); +} |