|
|
|
|
|
using Duality;
|
|
|
|
|
|
using System;
|
|
|
|
|
|
using System.Collections.Generic;
|
|
|
|
|
|
|
|
|
|
|
|
namespace Steering
|
|
|
|
|
|
{
|
|
|
|
|
|
/// <summary>
|
|
|
|
|
|
/// Samples velocities based on the velocity the agent chose. The sampling
|
|
|
|
|
|
/// density is higher velocities close to the last best velocity.
|
|
|
|
|
|
/// This reduces samples needed massively compared to <see cref="BruteForceVelocitySampler"/>
|
|
|
|
|
|
/// but can potentially lead to undesired behavior.
|
|
|
|
|
|
/// </summary>
|
|
|
|
|
|
public class AdaptiveVelocitySampler : IVelocitySampler
|
|
|
|
|
|
{
|
|
|
|
|
|
private int layerCount = 5;
|
|
|
|
|
|
private int outerLayerSampleCount = 11;
|
|
|
|
|
|
|
|
|
|
|
|
[DontSerialize] private int currentSampleIdx = 0;
|
|
|
|
|
|
|
|
|
|
|
|
public void Reset()
|
|
|
|
|
|
{
|
|
|
|
|
|
this.currentSampleIdx = 0;
|
|
|
|
|
|
}
|
|
|
|
|
|
public Vector2 GetCurrentSample(Agent agent)
|
|
|
|
|
|
{
|
|
|
|
|
|
int commonSampleCount = this.layerCount * this.outerLayerSampleCount;
|
|
|
|
|
|
Vector2 oldVelocity = agent.SuggestedVel / agent.Characteristics.MaxSpeed;
|
|
|
|
|
|
|
|
|
|
|
|
// this can happen if the max speed changed during the last update
|
|
|
|
|
|
if (oldVelocity.LengthSquared > 1f)
|
|
|
|
|
|
oldVelocity.Normalize();
|
|
|
|
|
|
|
|
|
|
|
|
if (this.currentSampleIdx >= commonSampleCount + 1)
|
|
|
|
|
|
return Vector2.Zero;
|
|
|
|
|
|
if (this.currentSampleIdx >= commonSampleCount)
|
|
|
|
|
|
return oldVelocity;
|
|
|
|
|
|
|
|
|
|
|
|
int layerIdx = this.currentSampleIdx % this.layerCount;
|
|
|
|
|
|
int directionIdx = (this.currentSampleIdx / this.layerCount) % this.outerLayerSampleCount;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float undistortedSpeedFactor = (float)(layerIdx + 1) / this.layerCount;
|
|
|
|
|
|
float undistortedAngle = MathF.Lerp(-1.0f, 1.0f, ((float)directionIdx / this.outerLayerSampleCount));
|
|
|
|
|
|
|
|
|
|
|
|
float speedFactor = undistortedSpeedFactor;
|
|
|
|
|
|
float angle = MathF.Atan2(oldVelocity.Y, oldVelocity.X) + MathF.Pow(Math.Abs(undistortedAngle), 0.8f) * undistortedAngle * MathF.RadAngle180;
|
|
|
|
|
|
|
|
|
|
|
|
return new Vector2(MathF.Cos(angle) * speedFactor, MathF.Sin(angle) * speedFactor);
|
|
|
|
|
|
}
|
|
|
|
|
|
public bool SetCurrentCost(float cost)
|
|
|
|
|
|
{
|
|
|
|
|
|
this.currentSampleIdx++;
|
|
|
|
|
|
if (this.currentSampleIdx <= this.layerCount * this.outerLayerSampleCount)
|
|
|
|
|
|
return true;
|
|
|
|
|
|
else
|
|
|
|
|
|
return false;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
public override string ToString()
|
|
|
|
|
|
{
|
|
|
|
|
|
return string.Format("Adaptive, {0} Layers, {1} Samples", this.layerCount, this.outerLayerSampleCount);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
