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 Vector2 GetCurrentSample(Agent agent)
{
if (this.currentSampleIdx >= this.layerCount * this.outerLayerSampleCount)
return Vector2.Zero;
var layerIdx = this.currentSampleIdx % layerCount;
var directionIdx = (this.currentSampleIdx / this.layerCount) % this.outerLayerSampleCount;
float angle = ((float)directionIdx / this.outerLayerSampleCount) * MathF.RadAngle360;
float speedFactor = (float)(layerIdx + 1) / this.layerCount;
return new Vector2(MathF.Cos(angle) * speedFactor, MathF.Sin(angle) * speedFactor);
}
public float CalculateVelocityCost(Agent agent, Vector2 sampleVelocity, float toiPenality)
{
if (this.updateBaseImplParams)
{
// [aggressiveness]
// interpretation: aggressive agents are likely to ignore other agents more => avoid them only if absolutly necessary
// implementation:
// we use a function which is
// - zero if aggressiveness == 0
// - one if aggressiveness == 0.5
// - infinity if aggressiveness == 1 [to prevent numerical problem we don't actually use infinity but just a high value]
// ... and use it as toi exponent. This will lead to a toi-cost-function which is very steep close to one
// => the agent will avoid other agents in the last moment
// => if aggressiveness is 0 the opposit happens - the agent avoids other agents very early
this.baseImpl.ToiExponent = AdvancedAgentCharacteristics.DEFAULT_TOI_EXPONENT * (1f / (1f - MathF.Lerp(0.01f, 0.99f, this.aggressiveness)) - 1f);
this.updateBaseImplParams = false;
}
return this.baseImpl.CalculateVelocityCost(agent, sampleVelocity, toiPenality);
}
public float CalculateCost(Agent agent, Vector2 sampleDirection)
{
var agentPos = agent.GameObj.Transform.Pos.Xy;
var posDelta = this.location - agentPos;
return 0.5f * (1f - Vector2.Dot(posDelta.Normalized, sampleDirection));
}
public float CalculateCost(Agent agent, Vector2 sampleDirection)
{
float dirLen = this.direction.Length;
return 0.5f * (1.0f - Vector2.Dot(this.direction / (dirLen > 0.0f ? dirLen : 1.0f), sampleDirection));
}
public IEnumerable<Agent> FindNeighborAgents(Agent referenceAgent)
{
// ToDo: Performance Optimization when necessary.
return Scene.Current.FindComponents<Agent>().Where(a => a != referenceAgent);
}
public float CalculateVelocityCost(Agent agent, Vector2 sampleVelocity, float toiPenality)
{
float score = 0.0f;
float deltaVelocityPenality = (sampleVelocity - agent.GameObj.Transform.Vel.Xy).Length / (2.0f * this.MaxSpeed);
score += this.velocityPreservationFactor * deltaVelocityPenality;
if (agent.Target != null)
{
if (sampleVelocity.LengthSquared > float.Epsilon)
score += this.directionFactor * agent.Target.CalculateCost(agent, sampleVelocity.Normalized);
score += this.speedFactor * (this.prefSpeed - sampleVelocity.Length) / this.MaxSpeed;
}
score += this.toiFactor * MathF.Pow(toiPenality, this.toiExponent);
score /= this.velocityPreservationFactor + this.speedFactor + this.directionFactor + this.toiFactor;
return score;
}
