protected override void Read()
{
//Update Past State
for (int i = 0; i < TimeSeries.Pivot; i++)
{
TimeSeries.Sample sample = TimeSeries.Samples[i];
PhaseSeries.Values[i] = PhaseSeries.Values[i + 1];
RootSeries.SetPosition(i, RootSeries.GetPosition(i + 1));
RootSeries.SetDirection(i, RootSeries.GetDirection(i + 1));
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[i][j] = StyleSeries.Values[i + 1][j];
}
for (int j = 0; j < ContactSeries.Bones.Length; j++)
{
ContactSeries.Values[i][j] = ContactSeries.Values[i + 1][j];
}
GoalSeries.Transformations[i] = GoalSeries.Transformations[i + 1];
for (int j = 0; j < GoalSeries.Actions.Length; j++)
{
GoalSeries.Values[i][j] = GoalSeries.Values[i + 1][j];
}
}
//Get Root
Matrix4x4 root = RootSeries.Transformations[TimeSeries.Pivot];
//Read Posture
Vector3[] positions = new Vector3[Actor.Bones.Length];
Vector3[] forwards = new Vector3[Actor.Bones.Length];
Vector3[] upwards = new Vector3[Actor.Bones.Length];
Vector3[] velocities = new Vector3[Actor.Bones.Length];
for (int i = 0; i < Actor.Bones.Length; i++)
{
Vector3 position = NeuralNetwork.ReadVector3().GetRelativePositionFrom(root);
Vector3 forward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 upward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 velocity = NeuralNetwork.ReadVector3().GetRelativeDirectionFrom(root);
positions[i] = Vector3.Lerp(Actor.Bones[i].Transform.position + velocity / GetFramerate(), position, 0.5f);
forwards[i] = forward;
upwards[i] = upward;
velocities[i] = velocity;
}
//Read Inverse Pose
for (int i = 0; i < Actor.Bones.Length; i++)
{
PosePrediction[i] = NeuralNetwork.ReadVector3().GetRelativePositionFrom(RootSeries.Transformations.Last());
velocities[i] = Vector3.Lerp(velocities[i], GetFramerate() * (PosePrediction[i] - Actor.Bones[i].Transform.position), 1f / GetFramerate());
}
//Read Future Trajectory
for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
{
TimeSeries.Sample sample = TimeSeries.GetKey(i);
Vector3 pos = NeuralNetwork.ReadXZ().GetRelativePositionFrom(root);
Vector3 dir = NeuralNetwork.ReadXZ().normalized.GetRelativeDirectionFrom(root);
RootSeries.SetPosition(sample.Index, pos);
RootSeries.SetDirection(sample.Index, dir);
float[] styles = NeuralNetwork.Read(StyleSeries.Styles.Length);
for (int j = 0; j < styles.Length; j++)
{
styles[j] = Mathf.Clamp(styles[j], 0f, 1f);
}
StyleSeries.Values[sample.Index] = styles;
RootPrediction[i - 6] = Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one);
}
//Read Inverse Trajectory
for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
{
TimeSeries.Sample sample = TimeSeries.GetKey(i);
Matrix4x4 goal = GoalSeries.Transformations[TimeSeries.Pivot];
goal[1, 3] = 0f;
Vector3 pos = NeuralNetwork.ReadXZ().GetRelativePositionFrom(goal);
Vector3 dir = NeuralNetwork.ReadXZ().normalized.GetRelativeDirectionFrom(goal);
if (i > TimeSeries.PivotKey)
{
Matrix4x4 pivot = RootSeries.Transformations[sample.Index];
pivot[1, 3] = 0f;
Matrix4x4 reference = GoalSeries.Transformations[sample.Index];
reference[1, 3] = 0f;
float distance = Vector3.Distance(pivot.GetPosition(), reference.GetPosition());
float weight = Mathf.Pow((float)(i - 6) / 7f, distance * distance);
RootSeries.SetPosition(sample.Index, Vector3.Lerp(RootSeries.GetPosition(sample.Index), pos, weight));
RootSeries.SetDirection(sample.Index, Vector3.Slerp(RootSeries.GetDirection(sample.Index), dir, weight));
}
GoalPrediction[i - 6] = Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one);
}
//Read and Correct Goals
for (int i = 0; i < TimeSeries.KeyCount; i++)
{
float weight = TimeSeries.GetWeight1byN1(TimeSeries.GetKey(i).Index, 2f);
TimeSeries.Sample sample = TimeSeries.GetKey(i);
Vector3 pos = NeuralNetwork.ReadVector3().GetRelativePositionFrom(root);
Vector3 dir = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
float[] actions = NeuralNetwork.Read(GoalSeries.Actions.Length);
for (int j = 0; j < actions.Length; j++)
{
actions[j] = Mathf.Clamp(actions[j], 0f, 1f);
}
GoalSeries.Transformations[sample.Index] = Utility.Interpolate(GoalSeries.Transformations[sample.Index], Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one), weight * NetworkControl);
GoalSeries.Values[sample.Index] = Utility.Interpolate(GoalSeries.Values[sample.Index], actions, weight * NetworkControl);
}
//Read Future Contacts
float[] contacts = NeuralNetwork.Read(ContactSeries.Bones.Length);
for (int i = 0; i < contacts.Length; i++)
{
contacts[i] = Mathf.Clamp(contacts[i], 0f, 1f);
}
ContactSeries.Values[TimeSeries.Pivot] = contacts;
//Read Phase Update
float phase = PhaseSeries.Values[TimeSeries.Pivot];
for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
{
PhaseSeries.Values[TimeSeries.GetKey(i).Index] = Mathf.Repeat(phase + NeuralNetwork.Read(), 1f);
}
//Interpolate Current to Future Trajectory
for (int i = 0; i < TimeSeries.Samples.Length; i++)
{
float weight = (float)(i % TimeSeries.Resolution) / TimeSeries.Resolution;
TimeSeries.Sample sample = TimeSeries.Samples[i];
TimeSeries.Sample prevSample = TimeSeries.GetPreviousKey(i);
TimeSeries.Sample nextSample = TimeSeries.GetNextKey(i);
//PhaseSeries.Values[sample.Index] = Mathf.Lerp(PhaseSeries.Values[prevSample.Index], PhaseSeries.Values[nextSample.Index], weight);
RootSeries.SetPosition(sample.Index, Vector3.Lerp(RootSeries.GetPosition(prevSample.Index), RootSeries.GetPosition(nextSample.Index), weight));
RootSeries.SetDirection(sample.Index, Vector3.Slerp(RootSeries.GetDirection(prevSample.Index), RootSeries.GetDirection(nextSample.Index), weight));
GoalSeries.Transformations[sample.Index] = Utility.Interpolate(GoalSeries.Transformations[prevSample.Index], GoalSeries.Transformations[nextSample.Index], weight);
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[i][j] = Mathf.Lerp(StyleSeries.Values[prevSample.Index][j], StyleSeries.Values[nextSample.Index][j], weight);
}
for (int j = 0; j < GoalSeries.Actions.Length; j++)
{
GoalSeries.Values[i][j] = Mathf.Lerp(GoalSeries.Values[prevSample.Index][j], GoalSeries.Values[nextSample.Index][j], weight);
}
}
//Assign Posture
transform.position = RootSeries.GetPosition(TimeSeries.Pivot);
transform.rotation = RootSeries.GetRotation(TimeSeries.Pivot);
for (int i = 0; i < Actor.Bones.Length; i++)
{
Actor.Bones[i].Velocity = velocities[i];
Actor.Bones[i].Transform.position = positions[i];
Actor.Bones[i].Transform.rotation = Quaternion.LookRotation(forwards[i], upwards[i]);
Actor.Bones[i].ApplyLength();
}
}
protected override void Feed()
{
Controller.Update();
//Get Root
Matrix4x4 root = RootSeries.Transformations[TimeSeries.Pivot];
//Control Cycle
Signals = Controller.PoolSignals();
UserControl = Controller.PoolUserControl(Signals);
NetworkControl = Controller.PoolNetworkControl(Signals);
if (IsInteracting)
{
//Do nothing because coroutines have control.
}
else if (Controller.QuerySignal("Sit"))
{
StartCoroutine(Sit());
}
else if (Controller.QuerySignal("Carry"))
{
StartCoroutine(Carry());
}
else if (Controller.QuerySignal("Open"))
{
StartCoroutine(Open());
}
else
{
Default();
}
//Input Bone Positions / Velocities
for (int i = 0; i < Actor.Bones.Length; i++)
{
NeuralNetwork.Feed(Actor.Bones[i].Transform.position.GetRelativePositionTo(root));
NeuralNetwork.Feed(Actor.Bones[i].Transform.forward.GetRelativeDirectionTo(root));
NeuralNetwork.Feed(Actor.Bones[i].Transform.up.GetRelativeDirectionTo(root));
NeuralNetwork.Feed(Actor.Bones[i].Velocity.GetRelativeDirectionTo(root));
}
//Input Trajectory Positions / Directions / Velocities / Styles
for (int i = 0; i < TimeSeries.KeyCount; i++)
{
TimeSeries.Sample sample = TimeSeries.GetKey(i);
NeuralNetwork.FeedXZ(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
NeuralNetwork.FeedXZ(RootSeries.GetDirection(sample.Index).GetRelativeDirectionTo(root));
NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
}
//Input Goals
for (int i = 0; i < TimeSeries.KeyCount; i++)
{
TimeSeries.Sample sample = TimeSeries.GetKey(i);
NeuralNetwork.Feed(GoalSeries.Transformations[sample.Index].GetPosition().GetRelativePositionTo(root));
NeuralNetwork.Feed(GoalSeries.Transformations[sample.Index].GetForward().GetRelativeDirectionTo(root));
NeuralNetwork.Feed(GoalSeries.Values[sample.Index]);
}
//Input Environment
Environment.Sense(root, LayerMask.GetMask("Default", "Interaction"));
NeuralNetwork.Feed(Environment.Occupancies);
//Input Geometry
for (int i = 0; i < Geometry.Points.Length; i++)
{
NeuralNetwork.Feed(Geometry.References[i].GetRelativePositionTo(root));
NeuralNetwork.Feed(Geometry.Occupancies[i]);
}
//Setup Gating Features
NeuralNetwork.Feed(GenerateGating());
}
