💿🐜 Antkeeper source code
https://antkeeper.com
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
362 lines
10 KiB
362 lines
10 KiB
/*
|
|
* 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);
|
|
}
|
|
|
|
// Locomotion
|
|
|
|
/*
|
|
As the ant moves forward, legs in the stance phase are kept grounded via IK. If IK constraints are violated, the swinging legs are grounded
|
|
and the grounded legs begin swinging.
|
|
|
|
Two poses are loaded from the model file: midswing and touchdown.
|
|
|
|
touchdown is the pose in which all legs are at the end of their swing phases and need to be grounded
|
|
midswing is the pose in which all legs are at the highest point in their swing phases
|
|
|
|
when a grounded leg enters the swing phases, its current pose is saved as the liftoff pose, then an animation is created using the liftoff pose, midswing pose, and touchdown pose.
|
|
*/
|
|
}
|
|
|
|
/*
|
|
Vector3 Ant::forage(const Vector3& leftReceptor, const Vector3& rightReceptor)
|
|
{
|
|
float leftSignal = 0.0f;
|
|
float rightSignal = 0.0f;
|
|
|
|
// Detect pheromones with left receptor
|
|
std::list<Pheromone*> leftPheromones;
|
|
colony->getPheromoneOctree()->query(AABB(leftReceptor, leftReceptor), &leftPheromones);
|
|
for (Pheromone* pheromone: leftPheromones)
|
|
{
|
|
Vector3 difference = pheromone->getPosition() - rightReceptor;
|
|
|
|
float distanceSquared = glm::dot(difference, difference);
|
|
if (distanceSquared <= pheromone->getRadiusSquared())
|
|
{
|
|
// Calculate attenuated pheromone strength using inverse-square law
|
|
float strength = pheromone->getStrength() / ((distanceSquared == 0.0f) ? 1.0f : distanceSquared);
|
|
leftSignal += strength;
|
|
}
|
|
}
|
|
|
|
// Detect pheromones with right receptor
|
|
std::list<Pheromone*> rightPheromones;
|
|
colony->getPheromoneOctree()->query(AABB(rightReceptor, rightReceptor), &rightPheromones);
|
|
for (Pheromone* pheromone: rightPheromones)
|
|
{
|
|
Vector3 difference = pheromone->getPosition() - rightReceptor;
|
|
|
|
float distanceSquared = glm::dot(difference, difference);
|
|
if (distanceSquared <= pheromone->getRadiusSquared())
|
|
{
|
|
// Calculate attenuated pheromone strength using inverse-square law
|
|
float strength = pheromone->getStrength() / ((distanceSquared == 0.0f) ? 1.0f : distanceSquared);
|
|
rightSignal += strength;
|
|
}
|
|
}
|
|
|
|
// Add noise
|
|
const float maxNoise = 0.1f;
|
|
leftSignal += frand(0.0f, maxNoise);
|
|
rightSignal += frand(0.0f, maxNoise);
|
|
|
|
if (leftSignal + rightSignal > 0.0f)
|
|
{
|
|
const float maxPheromoneTurningAngle = 0.1f;
|
|
|
|
// Use Weber's law (Perna et al.) to calculate turning angle based on pheromone signals
|
|
float turningAngle = maxPheromoneTurningAngle * ((leftSignal - rightSignal) / (leftSignal + rightSignal));
|
|
}
|
|
|
|
return Vector3(0.0f);
|
|
}
|
|
*/
|
|
|
|
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;
|
|
}
|