public float Evaluate(Vector <float> input)
{
if (this.isDirty)
{
var logger = new FishLogger();
var useSim = true;
if (useSim)
{
//start new simulation to get trajactory
var body = GeometryFunctions.Load();
var problem = new FishSimulatorOffline.Problem(body, this.activationData);
var dt = new IterationDelta();
var sim = new FishSimulatorOffline(problem, dt);
sim.TryToRun();
// wait to finish
var sol = sim.CurrentSolution as FishSimulatorOffline.Solution;
while (sol.IsDone == false)
{
}
logger = sol.logger.DeepCopy();
sim.Dispose();
}
var e = GetCurrentE(logger, this.activationData);
this.LatestE = e;
this.isDirty = false;
// Debug.Log("end with e = " + this.LatestE);
}
//cal new E from RandomX2FDiscreteFunction and trajactory
return(this.LatestE);
}
protected static float GetCurrentE(FishLogger logger, FishActivationData activationData)
{
var mu1 = 0.5f;
var mu2 = 0.5f;
//As the value of v1 increases,
//both the amplitude and the frequency decrease
//v1 = 0; 0.1; 0.2
var v1 = 0.001f;
//As the value of v2 increases,
//the amplitude remains constant and only the frequency decreases.
//v2 = 0; 0.002; 0.006
var v2 = 0.002f;
var E = 0f;
var goalPos = new float3(0, 0, 100);
var orgPos = new float3(0, 0, 0);
var goalVel = 10f;
var Ev = 0f;
foreach (var pos in logger.LogData.trajectory.ToYVector())
{
Ev += math.distance(pos, goalPos) / math.distance(orgPos, goalPos);
}
foreach (var vel in logger.LogData.velocity.ToYVector())
{
Ev += -vel.x / goalVel;
}
var Eu = 0f;
{
foreach (var activation in activationData.ToActivationList())
{
var du = 0f;
var du2 = 0f;
var func = activation.DiscreteFunction;
for (var i = 0; i < func.SampleNum; ++i)
{
var dev = func.Derivate(i);
var dev2 = func.Derivate2(i);
du += dev * dev;
du2 += dev2 * dev2;
}
Eu += 0.5f * (v1 * du + v2 * du2);
}
}
E = mu1 * Eu + mu2 * Ev;
return(E);
}
// public FishBrain Brain => this.brain;
// public bool IsDone { get; set; }
public void Init()
{
this.body = this.GetComponentInChildren <FishBody>();
this.body.Init();
this.brain = this.GetComponentInChildren <FishBrain>();
this.brain.Init();
this.perception = this.GetComponentInChildren <Perception>();
this.perception.Init();
this.logger = this.logger ?? new FishLogger();
this.localDelta = this.localDelta ?? new FishSimulator.Delta();
this.Reset();
FishSimulatorRunner.Instance.Controller.AddController(this);
}