/* * 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 . */ #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) {} 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 = forward * wanderCircleDistance; // Calculate wander force Vector3 force = 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 force; } Vector3 Agent::seek(const Vector3& target) const { Vector3 desiredVelocity = glm::normalize(position - target) * maxSpeed; return desiredVelocity - velocity; } Vector3 Agent::flee(const Vector3& target) const { Vector3 desiredVelocity = glm::normalize(target - position) * maxSpeed; return desiredVelocity - velocity; } Vector3 Agent::containment(const Vector3& probe) const { std::vector 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 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 force = glm::reflect(forward, edgeNormal); 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::setWanderCircleDistance(float distance) { wanderCircleDistance = distance; } void Agent::setWanderCircleRadius(float radius) { wanderCircleRadius = radius; } void Agent::setWanderRate(float angle) { wanderRate = angle; } /** EXAMPLE USAGE Vector3 wanderForce = wander(dt) * wanderWeight; Vector3 fleeForce = flee(mouse) * fleeWeight; Vector3 steerForce = wanderForce + fleeForce; steer(steerForce); **/