|
|
- /*
- * Copyright (C) 2017 Christopher J. Howard
- *
- * This file is part of Antkeeper Source Code.
- *
- * Antkeeper Source Code is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Antkeeper Source Code 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Antkeeper Source Code. If not, see <http://www.gnu.org/licenses/>.
- */
-
- #include "ant.hpp"
- #include "colony.hpp"
- #include "pheromone-matrix.hpp"
- #include <cmath>
-
- float FRAMES_PER_SECOND = 60;
- float TIMESTEP = 1.0f / FRAMES_PER_SECOND;
- float ANT_LENGTH = 0.5f; // 0.5 cm, head to abdomen (not including legs / antennae)
- float ANT_COLLISION_RADIUS = ANT_LENGTH * 1.25f;
- float RECEPTOR_RADIUS = 0.4f;
- float RECEPTOR_SEPARATION = 0.882f;
- float RECEPTOR_DISTANCE = 0.588f;
- float MOUTH_DISTANCE = 0.2646f;
- float BITE_RADIUS = 0.0294f;
- float FOOD_PARTICLE_RADIUS = 0.1176f;
- float MAX_RECEPTOR_NOISE = 0.05f; // essentially an epsilon
- float MAX_EXCITEMENT = 1.0f;
- float MAX_PHEROMONE_TURNING_ANGLE = glm::radians(8.5f);
- float MIN_WALK_TIME = 0.5f; // seconds
- float MAX_WALK_TIME = 8.0f; // seconds
- float MIN_REST_TIME = 0.15f;
- float MAX_REST_TIME = 0.7f;
- float MIN_CHEW_TIME = 0.25f;
- float MAX_CHEW_TIME = 0.5f;
- float DEEXCITEMENT_FACTOR = 0.999f; // This should probably always be less than the evaporation factor
- float CALM_FACTOR = 0.995f;
- float MAX_WALK_FORCE = 1.5;
- float MAX_PANIC_FORCE = 0.1029f;
- float MAX_WALK_SPEED = 3.0f; // cm/s
- float MAX_PANIC_SPEED = 8.82f; // cm/s
- float PANIC_RADIUS = 7.35f;
- float WANDER_CIRCLE_DISTANCE = 0.441f;
- float WANDER_CIRCLE_RADIUS = 0.0294f;
- float MAX_WANDER_ANGLE = 0.15f;
-
- inline float fwrap(float angle, float limit)
- {
- return angle - std::floor(angle / limit) * limit;
- }
-
- Ant::Ant(Colony* colony):
- colony(colony),
- state(Ant::State::IDLE),
- transform(Transform::getIdentity()),
- pose(nullptr)
- {
- pose = new Pose(colony->getAntModel()->getSkeleton());
- pose->reset();
- pose->concatenate();
-
- modelInstance.setModel(colony->getAntModel());
- modelInstance.setPose(pose);
-
- animationTime = frand(0.0f, 60.0f);
-
- velocity = Vector3(0);
- acceleration = Vector3(0);
- wanderDirection = getForward();
- excitement = MAX_EXCITEMENT;
- }
-
- Ant::~Ant()
- {
- delete pose;
- }
-
- void Ant::animate()
- {
- colony->getTripodGaitAnimation()->animate(pose, animationTime);
- pose->concatenate();
- animationTime = fwrap(animationTime + 4.0f, colony->getTripodGaitAnimation()->getEndTime());
- }
-
- void Ant::suspend(const Vector3& suspensionPoint, const Quaternion& suspensionRotation)
- {
- transform.translation = suspensionPoint;
- transform.rotation = suspensionRotation;
- modelInstance.setTransform(transform);
- }
-
- void Ant::move(const Vector3& velocity)
- {
- std::vector<Navmesh::Step> traversal;
- Navmesh::traverse(getNavmeshTriangle(), getBarycentricPosition(), velocity, &traversal);
-
- if (!traversal.empty())
- {
- const Navmesh::Step& step = traversal.back();
-
- if (step.start != step.end)
- {
- if (step.triangle != getNavmeshTriangle())
- {
- Quaternion alignment = glm::rotation(getNavmeshTriangle()->normal, step.triangle->normal);
- Vector3 newForward = glm::normalize(project_on_plane(alignment * getForward(), Vector3(0.0f), step.triangle->normal));
-
- setOrientation(newForward, step.triangle->normal);
- }
- }
-
- setPosition(step.triangle, step.end);
- }
- }
-
- void Ant::turn(float angle)
- {
- // Rotate forward vector
- Vector3 newForward = glm::normalize(glm::angleAxis(angle, getUp()) * getForward());
- setOrientation(newForward, getUp());
- }
-
- void Ant::update(float dt)
- {
- float probeLateralOffset = 0.1f;
- float probeForwardOffset = 0.3f;
-
- animate();
-
- // Calculate positions of receptors
- receptorL = getPosition() + getForward() * RECEPTOR_DISTANCE;
- receptorR = receptorL;
- receptorL -= getRight() * RECEPTOR_SEPARATION * 0.5f;
- receptorR += getRight() * RECEPTOR_SEPARATION * 0.5f;
-
- // Steering
- if (state == Ant::State::WANDER)
- {
- //setWanderCircleDistance(4.0f);
- //setWanderCircleRadius(0.3f);
- //setWanderRate(glm::radians(90.0f));
- //setSeparationRadius(0.5f);
- //setMaxSpeed(0.025f);
-
- // Calculate wander force
- Vector3 wanderForce = wander() * 1.5f;
- Vector3 followForce = follow() * 3.0f;
-
- // Setup containment probes
- //Vector3 leftProbe = getForward() * probeForwardOffset - getRight() * probeLateralOffset;
- //Vector3 rightProbe = getForward() * probeForwardOffset + getRight() * probeLateralOffset;
-
- // Calculate containment force
- //Vector3 containmentForce = containment(leftProbe) + containment(rightProbe);
-
- // Determine neighbors
- //float neighborhoodSize = 2.0f;
- //AABB neighborhoodAABB(getPosition() - Vector3(neighborhoodSize * 0.5f), getPosition() + Vector3(neighborhoodSize * 0.5f));
- //std::list<Agent*> neighbors;
- //colony->queryAnts(neighborhoodAABB, &neighbors);
-
- // Calculate separation force
- //Vector3 separationForce = separation(neighbors);
-
- applyForce(wanderForce);
- applyForce(followForce);
-
- float maxSpeed = MAX_WALK_SPEED * TIMESTEP;
-
- // Limit acceleration
- float accelerationMagnitudeSquared = glm::dot(acceleration, acceleration);
- if (accelerationMagnitudeSquared > MAX_WALK_FORCE * MAX_WALK_FORCE)
- {
- acceleration = glm::normalize(acceleration) * MAX_WALK_FORCE;
- }
-
- // Accelerate
- velocity += acceleration;
-
- // Limit speed
- float speedSquared = glm::dot(velocity, velocity);
- if (speedSquared > maxSpeed * maxSpeed)
- {
- velocity = glm::normalize(velocity) * maxSpeed;
- }
-
- Vector3 direction = glm::normalize(velocity);
- setOrientation(direction, getUp());
-
- // Deposit pheromones
- Vector2 position2D = Vector2(getPosition().x, getPosition().z);
- colony->getHomingMatrix()->deposit(position2D, excitement);
- excitement *= DEEXCITEMENT_FACTOR;
-
- // Move ant
- move(velocity);
- }
- else if (state == Ant::State::IDLE)
- {
- velocity = Vector3(0.0f);
-
- // Move ant
- move(velocity);
- }
-
- // Update transform
- if (state == Ant::State::WANDER || state == Ant::State::IDLE)
- {
- transform.translation = getPosition();
- transform.rotation = getRotation();
-
- // Update model instance
- modelInstance.setTransform(transform);
- }
- }
-
- void Ant::setState(Ant::State state)
- {
- this->state = state;
- }
-
- Vector3 Ant::seek(const Vector3& target)
- {
- Vector3 steer(0.0f);
- Vector3 difference = target - getPosition();
-
- float distanceSquared = glm::dot(difference, difference);
- if (distanceSquared > 0.0f)
- {
- float maxForce = MAX_WALK_FORCE;
-
- steer = glm::normalize(difference) * maxForce - velocity;
- }
-
- return steer;
- }
-
- Vector3 Ant::flee(const Vector3& target)
- {
- return -seek(target);
- }
-
- Vector3 Ant::wander()
- {
- // Determine center of wander circle
- Vector3 center = getPosition() + getForward() * WANDER_CIRCLE_DISTANCE;
-
- // Calculate wander target
- Vector3 target = center + wanderDirection * WANDER_CIRCLE_RADIUS;
-
- // Rotate wander direction by a random displacement angle
- float displacement = frand(-MAX_WANDER_ANGLE, MAX_WANDER_ANGLE);
- wanderDirection = glm::normalize(glm::angleAxis(displacement, getUp()) * wanderDirection);
-
- return seek(target);
- }
-
- Vector3 Ant::follow()
- {
- const PheromoneMatrix* pheromoneMatrix = colony->getRecruitmentMatrix();
-
- Vector2 receptorL2D = Vector2(receptorL.x, receptorL.z);
- Vector2 receptorR2D = Vector2(receptorR.x, receptorR.z);
-
- float signalL = pheromoneMatrix->query(receptorL2D, RECEPTOR_RADIUS);
- signalL += frand(0.0f, MAX_RECEPTOR_NOISE);
-
- float signalR = pheromoneMatrix->query(receptorR2D, RECEPTOR_RADIUS);
- signalR += frand(0.0f, MAX_RECEPTOR_NOISE);
-
- if (signalL + signalR > 0.0f)
- {
- float angle = -MAX_PHEROMONE_TURNING_ANGLE * ((signalL - signalR) / (signalL + signalR));
-
- Vector3 steer = glm::normalize(glm::angleAxis(angle, getUp()) * getForward());
- return steer;
- }
-
- return Vector3(0.0f);
- }
-
- void Ant::applyForce(const Vector3& force)
- {
- acceleration += force;
- }
|