💿🐜 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.
222 lines
5.8 KiB
222 lines
5.8 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 "agent.hpp"
|
|
|
|
Agent::Agent():
|
|
navmeshTriangle(nullptr),
|
|
barycentricPosition(0.0f),
|
|
position(0.0f),
|
|
forward(0, 0, -1),
|
|
up(0, 1, 0),
|
|
right(1, 0, 0),
|
|
rotation(1, 0, 0, 0),
|
|
wanderDirection(0, 0, -1),
|
|
velocity(0.0f)
|
|
{}
|
|
|
|
void Agent::applyForce(const Vector3& force)
|
|
{
|
|
acceleration += force;
|
|
}
|
|
|
|
void Agent::updateVelocity()
|
|
{
|
|
// Limit acceleration
|
|
acceleration = limit(acceleration / mass, maxAcceleration);
|
|
|
|
// Add acceleration to velocity and limit
|
|
velocity = limit(velocity + acceleration, maxSpeed);
|
|
|
|
// Reset acceleration to zero
|
|
acceleration = Vector3(0.0f);
|
|
}
|
|
|
|
Vector3 Agent::wander(float dt)
|
|
{
|
|
// Calculate center of wander circle
|
|
Vector3 wanderCircleCenter = position + forward * wanderCircleDistance;
|
|
|
|
// Calculate wander force
|
|
Vector3 target = wanderCircleCenter + wanderDirection * wanderCircleRadius;
|
|
|
|
// Rotate wander direction by a random displacement angle
|
|
float displacement = frand(-wanderRate * 0.5f, wanderRate * 0.5f);
|
|
wanderDirection = glm::normalize(glm::angleAxis(displacement, up) * wanderDirection);
|
|
|
|
return seek(target);
|
|
}
|
|
|
|
Vector3 Agent::seek(const Vector3& target) const
|
|
{
|
|
Vector3 desiredVelocity = glm::normalize(target - position) * maxSpeed;
|
|
return desiredVelocity - velocity;
|
|
}
|
|
|
|
Vector3 Agent::flee(const Vector3& target) const
|
|
{
|
|
Vector3 desiredVelocity = glm::normalize(position - target) * maxSpeed;
|
|
return desiredVelocity - velocity;
|
|
}
|
|
|
|
Vector3 Agent::containment(const Vector3& probe) const
|
|
{
|
|
std::vector<Navmesh::Step> traversal;
|
|
Navmesh::traverse(navmeshTriangle, barycentricPosition, probe, &traversal);
|
|
|
|
if (traversal.empty())
|
|
{
|
|
return Vector3(0.0f);
|
|
}
|
|
|
|
const Navmesh::Step& step = traversal.back();
|
|
|
|
// If not on edge or on connected edge
|
|
if (step.edge == nullptr || step.edge->symmetric != nullptr)
|
|
{
|
|
return Vector3(0.0f);
|
|
}
|
|
|
|
/*
|
|
// Calculate difference between probe position and position on edge
|
|
Vector3 end = cartesian(step.end,
|
|
step.triangle->edge->vertex->position,
|
|
step.triangle->edge->next->vertex->position,
|
|
step.triangle->edge->previous->vertex->position);
|
|
|
|
Vector3 difference = probe - end;
|
|
|
|
float depth = 0.0f;
|
|
if (nonzero(difference))
|
|
{
|
|
depth = glm::length(difference);
|
|
}
|
|
*/
|
|
|
|
// Calculate edge normal
|
|
const Vector3& a = step.edge->vertex->position;
|
|
const Vector3& b = step.edge->next->vertex->position;
|
|
Vector3 ab = glm::normalize(b - a);
|
|
Vector3 edgeNormal = glm::cross(up, ab);
|
|
|
|
// Calculate reflection vector of forward vector and edge normal
|
|
//Vector3 reflection = glm::reflect(forward, edgeNormal);
|
|
|
|
/*
|
|
Vector3 target = cartesian(step.end,
|
|
step.triangle->edge->vertex->position,
|
|
step.triangle->edge->next->vertex->position,
|
|
step.triangle->edge->previous->vertex->position) + reflection * 0.1f;
|
|
*/
|
|
|
|
//std::cout << "reflection: " << reflection.x << ", " << reflection.y << ", " << reflection.z << std::endl;
|
|
|
|
return edgeNormal;
|
|
}
|
|
|
|
Vector3 Agent::separation(const std::list<Agent*>& neighbors) const
|
|
{
|
|
Vector3 force(0.0f);
|
|
|
|
for (Agent* neighbor: neighbors)
|
|
{
|
|
Vector3 difference = position - neighbor->position;
|
|
|
|
float distanceSquared = glm::dot(difference, difference);
|
|
if (distanceSquared > 0.0f && distanceSquared < separationRadiusSquared)
|
|
{
|
|
force += difference * (1.0f / distanceSquared);
|
|
}
|
|
}
|
|
|
|
if (nonzero(force))
|
|
{
|
|
force = glm::normalize(force);
|
|
}
|
|
|
|
return force;
|
|
}
|
|
|
|
void Agent::setPosition(Navmesh::Triangle* triangle, const Vector3& position)
|
|
{
|
|
// Update navmesh triangle and position
|
|
navmeshTriangle = triangle;
|
|
barycentricPosition = position;
|
|
|
|
// Convert navmesh-space barycentric position to world-space cartesian position
|
|
const Vector3& a = triangle->edge->vertex->position;
|
|
const Vector3& b = triangle->edge->next->vertex->position;
|
|
const Vector3& c = triangle->edge->previous->vertex->position;
|
|
this->position = cartesian(position, a, b, c);
|
|
}
|
|
|
|
void Agent::setOrientation(const Vector3& newForward, const Vector3& newUp)
|
|
{
|
|
// Calculate alignment quaternion
|
|
Quaternion alignment = glm::rotation(up, newUp);
|
|
|
|
// Rebuild vector basis
|
|
forward = newForward;
|
|
right = glm::normalize(glm::cross(newUp, forward));
|
|
up = glm::cross(forward, right);
|
|
|
|
// Calculate rotation quaternion from vector basis
|
|
rotation = glm::normalize(glm::quat_cast(Matrix3(right, up, forward)));
|
|
|
|
// Align wander direction
|
|
wanderDirection = glm::normalize(project_on_plane(alignment * wanderDirection, Vector3(0.0f), up));
|
|
}
|
|
|
|
void Agent::setMaxSpeed(float speed)
|
|
{
|
|
maxSpeed = speed;
|
|
}
|
|
|
|
void Agent::setVelocity(const Vector3& velocity)
|
|
{
|
|
this->velocity = velocity;
|
|
}
|
|
|
|
void Agent::setWanderCircleDistance(float distance)
|
|
{
|
|
wanderCircleDistance = distance;
|
|
}
|
|
|
|
void Agent::setWanderCircleRadius(float radius)
|
|
{
|
|
wanderCircleRadius = radius;
|
|
}
|
|
|
|
void Agent::setWanderRate(float angle)
|
|
{
|
|
wanderRate = angle;
|
|
}
|
|
|
|
void Agent::setSeparationRadius(float radius)
|
|
{
|
|
separationRadius = radius;
|
|
separationRadiusSquared = separationRadius * separationRadius;
|
|
}
|
|
|
|
/** EXAMPLE USAGE
|
|
Vector3 wanderForce = wander(dt) * wanderWeight;
|
|
Vector3 fleeForce = flee(mouse) * fleeWeight;
|
|
Vector3 steerForce = wanderForce + fleeForce;
|
|
steer(steerForce);
|
|
**/
|