public void FeedForwardConfiguration(float[] configuration, bool updateWorld = false)
{
HeuristicInputs = 0f;
HeuristicError = 0f;
bool updateLocal = false;
if (Motions[0] != null)
{
if (configuration[Motions[0].Index] != Values[0])
{
Values[0] = configuration[Motions[0].Index];
updateLocal = true;
updateWorld = true;
}
}
if (Motions[1] != null)
{
if (configuration[Motions[1].Index] != Values[1])
{
Values[1] = configuration[Motions[1].Index];
updateLocal = true;
updateWorld = true;
}
}
if (Motions[2] != null)
{
if (configuration[Motions[2].Index] != Values[2])
{
Values[2] = configuration[Motions[2].Index];
updateLocal = true;
updateWorld = true;
}
}
if (updateLocal)
{
Joint.ComputeTransformation(Values[0], Values[1], Values[2], out LPX, out LPY, out LPZ, out LRX, out LRY, out LRZ, out LRW);
}
if (updateWorld)
{
if (Parent == null)
{
TransformCoordinateSystem(
ref Model.OPX, ref Model.OPY, ref Model.OPZ, ref Model.ORX, ref Model.ORY, ref Model.ORZ, ref Model.ORW, ref Model.OSX, ref Model.OSY, ref Model.OSZ,
ref LPX, ref LPY, ref LPZ, ref LRX, ref LRY, ref LRZ, ref LRW, ref LSX, ref LSY, ref LSZ,
out WPX, out WPY, out WPZ, out WRX, out WRY, out WRZ, out WRW, out WSX, out WSY, out WSZ
);
}
else
{
TransformCoordinateSystem(
ref Parent.WPX, ref Parent.WPY, ref Parent.WPZ, ref Parent.WRX, ref Parent.WRY, ref Parent.WRZ, ref Parent.WRW, ref Parent.WSX, ref Parent.WSY, ref Parent.WSZ,
ref LPX, ref LPY, ref LPZ, ref LRX, ref LRY, ref LRZ, ref LRW, ref LSX, ref LSY, ref LSZ,
out WPX, out WPY, out WPZ, out WRX, out WRY, out WRZ, out WRW, out WSX, out WSY, out WSZ
);
}
}
for (int i = 0; i < Childs.Length; i++)
{
Childs[i].FeedForwardConfiguration(configuration, updateWorld);
}
}
