protected void Update()
{
if (this.mode == ControlMode.Manual)
{
var types = new List <Spring.Type>()
{
Spring.Type.MuscleMiddle, Spring.Type.MuscleBack
};
foreach (var t in types)
{
this.activationData.SetActivationValue(t, Spring.Side.Left, this.activation);
this.activationData.SetActivationValue(t, Spring.Side.None, this.activation);
// a.Tuning.useFFT = false;
// a.DiscreteFunction.ResetValues(this.activation);
// a = this.activationData[t, Spring.Side.Right];
// a.Tuning.useFFT = false;
// var ra = (this.activation + 1) * 0.5f;
// ra = 1 - ra;
// ra = ra * 2 - 1;
// a.DiscreteFunction.ResetValues(ra);
}
}
this.sim.RunMode = this.stepMode;
if (Input.GetKey(KeyCode.Space) && this.stepMode == IterationAlgorithmMode.PerStep)
{
this.sim.TryToRun();
}
foreach (var a in this.activationData.ToActivationList())
{
a.Tuning.useFFT = this.fft;
}
if (this.loop && (this.sim.CurrentSolution as FishSimulatorOffline.Solution).IsDone)
{
this.sim.Restart();
}
if (Input.GetKeyDown(KeyCode.S))
{
FishActivationData.Save(this.activationData);
}
if (Input.GetKey(KeyCode.R))
{
this.fish.Init();
this.runtimeList = this.fish.modelData.FishGraph.Nodes.ToList();
this.problem.UpdateData(this.fish.modelData, this.activationData);
this.sim.Restart();
}
if (Input.GetKeyDown(KeyCode.U))
{
this.UpdateActivationDataFromCurves();
}
if (Input.GetKeyDown(KeyCode.G))
{
this.activationData.RandomActivation();
this.UpdateAnimationCurves();
}
}
protected void InitActivations()
{
if (this.mode == ControlMode.Random || this.mode == ControlMode.Manual)
{
if (this.useJellyFish)
{
this.activationData = new FishActivationDataJellyfish(this.timeInterval, this.sampleNum, true);
}
else
{
this.activationData = new FishActivationDataSwimming(this.timeInterval, this.sampleNum, true);
}
this.activationData.RandomActivation();
}
else
if (this.mode == ControlMode.Activations)
{
this.activationData = FishActivationData.Load(this.activationFileName);
}
else
if (this.mode == ControlMode.CosSwim)
{
this.activationData = new FishActivationDataSwimming(this.timeInterval, this.sampleNum, true);
var ml = this.activationData[Spring.Type.MuscleMiddle, Spring.Side.Left];
var bl = this.activationData[Spring.Type.MuscleBack, Spring.Side.Left];
for (var i = 0; i < this.sampleNum; ++i)
{
var x = i * 1f / (this.sampleNum - 1) * 2 * math.PI;
ml.DiscreteFunction[i] = math.sin(x);
bl.DiscreteFunction[i] = math.sin(x + math.PI);
}
ml.FFT.GenerateFFTData();
bl.FFT.GenerateFFTData();
}
else
if (this.mode == ControlMode.CosTrun)
{
var ar = FishActivationData.Load("TurnRight");
this.activationData = new FishActivationDataTurnLeft(this.timeInterval, this.sampleNum, true);
var fl = this.activationData[Spring.Type.MuscleFront, Spring.Side.Left];
var ml = this.activationData[Spring.Type.MuscleMiddle, Spring.Side.Left];
var afl = ar[Spring.Type.MuscleFront, Spring.Side.Left];
var aml = ar[Spring.Type.MuscleMiddle, Spring.Side.Left];
for (var i = 1; i < this.sampleNum; ++i)
{
var x = i * 1f / (this.sampleNum - 1) * 2 * math.PI;
if (i < this.sampleNum / 3)
{
// ml.DiscreteFunction[i] = -math.sin(x*3);
// fl.DiscreteFunction[i] = -math.sin(x*3);
}
fl.DiscreteFunction[i] = -afl.DiscreteFunction[i];
ml.DiscreteFunction[i] = -aml.DiscreteFunction[i];
}
ml.FFT.GenerateFFTData();
fl.FFT.GenerateFFTData();
}
}
// protected Dictionary<Spring.Type, X2FDiscreteFunction<float>> VectorToActivation(Vector<float> x, float2 interval, int sampleNum)
// {
// var activations = new Dictionary<Spring.Type, X2FDiscreteFunction<float>>();
// var types = this.GetSpringTypes();
// var count = 0;
// foreach (var t in types)
// {
// activations.Add(t, new X2FDiscreteFunction<float>(interval.x, interval.y, Vector<float>.Sub(sampleNum * count, sampleNum * count, x)));
// count++;
// }
// return activations;
// }
public static void Save(FishActivationData data)
{
var fileName = data.FileName + ".ad";
var path = System.IO.Path.Combine(Application.streamingAssetsPath, fileName);
FileTool.Write(path, data);
LogTool.Log("Saved " + path);
}
protected void Start()
{
var sampleNum = 15;
var inteval = new float2(0, 30);
var sa = this.LoadData();
this.activationData = (sa.Current as SAProblem.ActivationState.Data).ActivationData;
this.activationData = FishActivationData.Load();
// var act = this.activationData.ToDiscreteFunctions()[0];
/*for(var i = 0; i < act.SampleNum; ++i)
* {
* //act[i] = math.sin(2 * math.PI * i / (act.SampleNum-1) * 4);
* act[i] = 0;
* }
*
* act[3] = act[4] = 1;
* act[9] = act[10] = 1;
*
* this.curve = act.ToAnimationCurve();
*
* var vector = act.ToYVector();
*
* var array = vector.Select(s=>(double)s).ToArray();
* var dft = new DFT();
*
* dft.Initialize((uint)array.Length);
*
* // Call the DFT and get the scaled spectrum back
* Complex[] cSpectrum = dft.Execute(array);
*
* An = DSP.ConvertComplex.ToMagnitude(cSpectrum);
* Pn = DSP.ConvertComplex.ToPhaseRadians(cSpectrum);
*
* var maxAn = An.Max();
* var maxIndex = An.ToList().IndexOf(maxAn);
* var maxPn = Pn[maxIndex];
*
* var start = new Tuple<float, float>(inteval.x, 0);
* var end = new Tuple<float, float>(inteval.y, 0);
* var function = new X2FDiscreteFunction<float>(start, end, sampleNum);
*
* for (var i = 0; i < function.SampleNum; ++i)
* {
* var x = 2 * math.PI * i / (act.SampleNum - 1);
* //function[i] = (float)(An[0] / 2 + maxAn * math.cos(maxIndex * x + maxPn));
* //function[i] = FishActivationData.GetFx(An, Pn, x, 1);
* }
*
* this.curve2 = function.ToAnimationCurve();
*/
}
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);
}
protected void ApplyActivation(float t, FishModelData modelData, FishActivationData data)
{
var types = new List <Spring.Type>()
{
Spring.Type.MuscleFront, Spring.Type.MuscleMiddle, Spring.Type.MuscleBack
};
foreach (var type in types)
{
data.ApplyActivation(t, type, modelData);
}
}
public Data(Parameter para)
{
switch (para.type)
{
case OptType.Swimming: this.activationData = new FishActivationDataSwimming(para.interval, para.sampleNum, true); break;
case OptType.TurnLeft: this.activationData = new FishActivationDataTurnLeft(para.interval, para.sampleNum); break;
case OptType.TurnRight: this.activationData = new FishActivationDataTurnRight(para.interval, para.sampleNum); break;
}
this.Generate(null);
}
public MuscleMC()
{
this.muscleControlParamters = new Dictionary <Spring.Type, Parameter>();
var types = this.GetSpringTypes();
foreach (var t in types)
{
this.muscleControlParamters.Add(t, new Parameter());
}
this.activationData = FishActivationData.Load(this.FileName);
this.parameters = this.muscleControlParamters.Values.ToList();
}
protected void Update()
{
foreach (var a in this.Activation.ToActivationList())
{
a.Tuning.useFFT = this.fft;
}
if (this.loop && (this.sim.CurrentSolution as FishSimulatorOffline.Solution).IsDone)
{
this.sim.Restart();
}
if (Input.GetKeyDown(KeyCode.S))
{
FishActivationData.Save(this.Activation);
}
}
protected void Start()
{
this.body = this.GetComponent <FishBody>();
this.body.Init();
// this.activationData = new FishActivationDataSwimming(this.interval, this.sampleNum);
//this.activationData.RandomActivation();
this.activationData = FishActivationData.Load();
this.interval = this.activationData.Interval;
this.sampleNum = this.activationData.SampleNum;
this.curves = this.activationData.ToAnimationCurves();
this.activations = this.activationData.ToActivationList();
var problem = new FishSimulatorOffline.Problem(this.body.modelData, this.activationData);
var dt = new IterationDelta();
var sim = new FishSimulatorOffline(problem, dt);
sim.TryToRun();
}
public void UpdateData(FishModelData modelData, FishActivationData activationData)
{
this.bodyModel = modelData;
this.activationData = activationData;
}
public Problem(FishModelData model, FishActivationData activationData)
{
this.UpdateData(model, activationData);
}