protected override void Read()
{
//Update Past States
DribbleSeries.IncrementBall(0, TimeSeries.Pivot);
ContactSeries.Increment(0, TimeSeries.Pivot);
PhaseSeries.Increment(0, TimeSeries.Pivot);
//Update Root State
Vector3 offset = NeuralNetwork.ReadVector3();
offset = Vector3.Lerp(offset, Vector3.zero, StyleSeries.Values[TimeSeries.Pivot].First());
Matrix4x4 root = Actor.GetRoot().GetWorldMatrix(true) * Matrix4x4.TRS(new Vector3(offset.x, 0f, offset.z), Quaternion.AngleAxis(offset.y, Vector3.up), Vector3.one);
RootSeries.Transformations[TimeSeries.Pivot] = root;
RootSeries.Velocities[TimeSeries.Pivot] = NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root);
DribbleSeries.Pivots[TimeSeries.Pivot] = DribbleSeries.InterpolatePivot(
DribbleSeries.Pivots[TimeSeries.Pivot],
NeuralNetwork.ReadVector3(),
Controller.QueryFunction("BallHorizontalCorrection", TimeSeries.Pivot),
Controller.QueryFunction("BallHeightCorrection", TimeSeries.Pivot)
);
DribbleSeries.Momentums[TimeSeries.Pivot] = DribbleSeries.InterpolateMomentum(
DribbleSeries.Momentums[TimeSeries.Pivot],
NeuralNetwork.ReadVector3(),
Controller.QueryFunction("BallHorizontalCorrection", TimeSeries.Pivot),
Controller.QueryFunction("BallSpeedCorrection", TimeSeries.Pivot)
);
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[TimeSeries.Pivot][j] = Mathf.Lerp(
StyleSeries.Values[TimeSeries.Pivot][j],
NeuralNetwork.Read(0f, 1f),
Controller.QueryFunction(StyleSeries.Styles[j] + "Correction", TimeSeries.Pivot)
);
}
//Read Future States
for (int i = TimeSeries.PivotKey + 1; i < TimeSeries.KeyCount; i++)
{
int index = TimeSeries.GetKey(i).Index;
Matrix4x4 m = Matrix4x4.TRS(NeuralNetwork.ReadXZ().GetRelativePositionFrom(root), Quaternion.LookRotation(NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root).normalized, Vector3.up), Vector3.one);
RootSeries.Transformations[index] = Utility.Interpolate(RootSeries.Transformations[index], m,
Controller.QueryFunction("RootPositionCorrection", index),
Controller.QueryFunction("RootRotationCorrection", index)
);
RootSeries.Velocities[index] = Vector3.Lerp(RootSeries.Velocities[index], NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root),
Controller.QueryFunction("RootVelocityCorrection", index)
);
DribbleSeries.Pivots[index] = DribbleSeries.InterpolatePivot(DribbleSeries.Pivots[index], NeuralNetwork.ReadVector3(),
Controller.QueryFunction("BallHorizontalCorrection", index),
Controller.QueryFunction("BallHeightCorrection", index)
);
DribbleSeries.Momentums[index] = DribbleSeries.InterpolateMomentum(DribbleSeries.Momentums[index], NeuralNetwork.ReadVector3(),
Controller.QueryFunction("BallHorizontalCorrection", index),
Controller.QueryFunction("BallSpeedCorrection", index)
);
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[index][j] = Mathf.Lerp(StyleSeries.Values[index][j], NeuralNetwork.Read(0f, 1f), Controller.QueryFunction(StyleSeries.Styles[j] + "Correction", index));
}
}
//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 = forward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 upward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 velocity = NeuralNetwork.ReadVector3().GetRelativeDirectionFrom(root);
velocities[i] = velocity;
positions[i] = Vector3.Lerp(Actor.Bones[i].Transform.position + velocity / Framerate, position, 0.5f);
forwards[i] = forward;
upwards[i] = upward;
}
//Compute Ball
float controlWeight = NeuralNetwork.Read(0f, 1f);
Vector3 ballPosition = NeuralNetwork.ReadVector3();
Vector3 ballVelocity = NeuralNetwork.ReadVector3();
Vector3 ballForward = NeuralNetwork.ReadVector3();
Vector3 ballUp = NeuralNetwork.ReadVector3();
if (Carrier && controlWeight > 0f)
{
ballPosition = (ballPosition / controlWeight).GetRelativePositionFrom(root);
ballVelocity = (ballVelocity / controlWeight).GetRelativeDirectionFrom(root);
Quaternion ballRotation = DribbleSeries.BallTransformations[TimeSeries.Pivot].GetRotation() * Quaternion.Slerp(
Quaternion.LookRotation((ballForward / controlWeight).normalized, (ballUp / controlWeight).normalized),
Quaternion.identity,
StyleSeries.GetStyle(TimeSeries.Pivot, "Hold")
);
//Assign Ball
Ball.SetPosition(
DribbleSeries.BallTransformations[TimeSeries.Pivot].GetPosition() + Vector3.ClampMagnitude(
Vector3.Lerp(
ballPosition,
DribbleSeries.BallTransformations[TimeSeries.Pivot].GetPosition() + ballVelocity / Framerate,
0.5f
) - DribbleSeries.BallTransformations[TimeSeries.Pivot].GetPosition(),
2f * ballVelocity.magnitude / Framerate
)
);
Ball.SetRotation(ballRotation);
Ball.SetVelocity(ballVelocity);
}
DribbleSeries.BallTransformations[TimeSeries.Pivot] =
Matrix4x4.TRS(
Ball.GetPosition(),
Ball.GetRotation(),
Vector3.one
);
DribbleSeries.BallVelocities[TimeSeries.Pivot] = Ball.GetVelocity();
//Update Contacts
float[] contacts = NeuralNetwork.Read(ContactSeries.Bones.Length, 0f, 1f);
for (int i = 0; i < ContactSeries.Bones.Length; i++)
{
if (i == 4)
{
ContactSeries.Values[TimeSeries.Pivot][i] = contacts[i].SmoothStep(ContactPower, BallContactThreshold);
}
else
{
ContactSeries.Values[TimeSeries.Pivot][i] = contacts[i].SmoothStep(ContactPower, BoneContactThreshold);
}
}
//Update Phases
for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
{
int index = TimeSeries.GetKey(i).Index;
float stability = Controller.QueryFunction("PhaseStability", index);
for (int b = 0; b < PhaseSeries.Bones.Length; b++)
{
Vector2 update = NeuralNetwork.ReadVector2();
Vector3 state = NeuralNetwork.ReadVector2();
float phase = Utility.PhaseValue(
Vector2.Lerp(
Utility.PhaseVector(Mathf.Repeat(PhaseSeries.Phases[index][b] + Utility.PhaseValue(update), 1f)),
Utility.PhaseVector(Mathf.Repeat(PhaseSeries.Phases[index][b] + Utility.SignedPhaseUpdate(PhaseSeries.Phases[index][b], Utility.PhaseValue(state)), 1f)),
stability).normalized
);
PhaseSeries.Amplitudes[index][b] = update.magnitude;
PhaseSeries.Phases[index][b] = phase;
}
}
//Interpolate Timeseries
RootSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
DribbleSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
StyleSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
PhaseSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
//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]);
}
//Correct Twist
for (int i = 0; i < Actor.Bones.Length; i++)
{
if (Actor.Bones[i].Childs.Length == 1)
{
Vector3 position = Actor.Bones[i].Transform.position;
Quaternion rotation = Actor.Bones[i].Transform.rotation;
Vector3 childPosition = Actor.Bones[i].GetChild(0).Transform.position;
Quaternion childRotation = Actor.Bones[i].GetChild(0).Transform.rotation;
Vector3 aligned = (position - childPosition).normalized;
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(rotation.GetRight(), aligned) * rotation;
Actor.Bones[i].GetChild(0).Transform.position = childPosition;
Actor.Bones[i].GetChild(0).Transform.rotation = childRotation;
}
}
//Resolve Trajectory Collisions
RootSeries.ResolveCollisions(Collider.radius, CollisionMask);
//Refine Ball
if (PhaseSeries.IsActive("Ball"))
{
if (Carrier)
{
Vector3 position = DribbleSeries.BallTransformations[TimeSeries.Pivot].GetPosition();
Vector3 velocity = DribbleSeries.BallVelocities[TimeSeries.Pivot];
//Process Hold Ball
if (Controller.QueryLogic("Hold") && Controller.QueryLogic("HorizontalControl"))
{
Vector3 target = DribbleSeries.Pivots.Last().GetRelativePositionFrom(RootSeries.Transformations[TimeSeries.Pivot]);
Vector3 modified = Vector3.Lerp(position, target, 0.15f);
velocity += (modified - position) * Framerate;
position = modified;
}
//Process Ground Ball
float weight = Physics.Raycast(
position,
velocity,
Ball.Radius + velocity.magnitude,
LayerMask.GetMask("Ground")
) ? ContactSeries.Values[TimeSeries.Pivot].Last() : 0f;
float prev = position.y;
float next = Mathf.Lerp(position.y, Ball.Radius, weight);
position.y = next;
velocity.y += Framerate * (next - prev);
Ball.SetPosition(position);
Ball.SetVelocity(velocity);
}
else
{
if (Controller.QueryLogic("Hold") && DribbleSeries.IsInsideControlRadius(Ball.GetPosition(), root.GetPosition()))
{
//Process Flying Ball
float leftWeight = GetBallControlWeight(LeftHandIK.Bones.Last().Transform.position);
float rightWeight = GetBallControlWeight(RightHandIK.Bones.Last().Transform.position);
float weight = 0.5f * (leftWeight + rightWeight);
Vector3 leftPos = LeftHandIK.Bones.Last().Transform.position;
Vector3 rightPos = RightHandIK.Bones.Last().Transform.position;
Vector3 target = 0.5f * (rightPos + leftPos);
Vector3 position = Ball.GetPosition();
Vector3 velocity = Ball.GetVelocity();
float angle = Vector3.Angle(target - position, velocity);
float correction = 1f - angle / 180f;
correction = weight * correction.ActivateCurve(0.25f, 0f, 1f);
Ball.SetVelocity(Vector3.Lerp(velocity, velocity.magnitude * (target - position).normalized, correction));
}
}
//Resolve Ball-Body Penetrations
BroadcastMessage("ResolveBallCollisions");
//Synchronize Ball
DribbleSeries.BallTransformations[TimeSeries.Pivot] =
Matrix4x4.TRS(
Ball.GetPosition(),
Ball.GetRotation(),
Vector3.one
);
DribbleSeries.BallVelocities[TimeSeries.Pivot] = Ball.GetVelocity();
}
//Process Contact States
ProcessBody();
ProcessFootIK(LeftFootIK, ContactSeries.Values[TimeSeries.Pivot][0]);
ProcessFootIK(RightFootIK, ContactSeries.Values[TimeSeries.Pivot][1]);
ProcessHandIK(LeftHandIK, ContactSeries.Values[TimeSeries.Pivot][2]);
ProcessHandIK(RightHandIK, ContactSeries.Values[TimeSeries.Pivot][3]);
ProcessHeadIK();
}
protected override void Read()
{
//Update Past States
ContactSeries.Increment(0, TimeSeries.Pivot);
PhaseSeries.Increment(0, TimeSeries.Pivot);
//Update Root State
Vector3 offset = NeuralNetwork.ReadVector3();
offset = Vector3.Lerp(offset, Vector3.zero, StyleSeries.Values[TimeSeries.Pivot].First());
Matrix4x4 root = Actor.GetRoot().GetWorldMatrix(true) * Matrix4x4.TRS(new Vector3(offset.x, 0f, offset.z), Quaternion.AngleAxis(offset.y, Vector3.up), Vector3.one);
RootSeries.Transformations[TimeSeries.Pivot] = root;
RootSeries.Velocities[TimeSeries.Pivot] = NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root);
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[TimeSeries.Pivot][j] = Mathf.Lerp(
StyleSeries.Values[TimeSeries.Pivot][j],
NeuralNetwork.Read(),
Controller.QueryFunction(StyleSeries.Styles[j] + "Correction", TimeSeries.Pivot)
);
}
//Read Future States
for (int i = TimeSeries.PivotKey + 1; i < TimeSeries.KeyCount; i++)
{
int index = TimeSeries.GetKey(i).Index;
Matrix4x4 m = Matrix4x4.TRS(NeuralNetwork.ReadXZ().GetRelativePositionFrom(root), Quaternion.LookRotation(NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root).normalized, Vector3.up), Vector3.one);
RootSeries.Transformations[index] = Utility.Interpolate(RootSeries.Transformations[index], m,
Controller.QueryFunction("RootPositionCorrection", index),
Controller.QueryFunction("RootRotationCorrection", index)
);
RootSeries.Velocities[index] = Vector3.Lerp(RootSeries.Velocities[index], NeuralNetwork.ReadXZ().GetRelativeDirectionFrom(root),
Controller.QueryFunction("RootVelocityCorrection", index)
);
for (int j = 0; j < StyleSeries.Styles.Length; j++)
{
StyleSeries.Values[index][j] = Mathf.Lerp(StyleSeries.Values[index][j], NeuralNetwork.Read(), Controller.QueryFunction(StyleSeries.Styles[j] + "Correction", index));
}
}
//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 = forward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 upward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
Vector3 velocity = NeuralNetwork.ReadVector3().GetRelativeDirectionFrom(root);
velocities[i] = velocity;
positions[i] = Vector3.Lerp(Actor.Bones[i].Transform.position + velocity / Framerate, position, 0.5f);
forwards[i] = forward;
upwards[i] = upward;
}
//Update Contacts
float[] contacts = NeuralNetwork.Read(ContactSeries.Bones.Length, 0f, 1f);
for (int i = 0; i < ContactSeries.Bones.Length; i++)
{
ContactSeries.Values[TimeSeries.Pivot][i] = contacts[i].SmoothStep(ContactPower, ContactThreshold);
}
//Update Phases
for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
{
int index = TimeSeries.GetKey(i).Index;
float stability = Controller.QueryFunction("PhaseStability", index);
for (int b = 0; b < PhaseSeries.Bones.Length; b++)
{
Vector2 update = NeuralNetwork.ReadVector2();
Vector3 state = NeuralNetwork.ReadVector2();
float phase = Utility.PhaseValue(
Vector2.Lerp(
Utility.PhaseVector(Mathf.Repeat(PhaseSeries.Phases[index][b] + Utility.PhaseValue(update), 1f)),
Utility.PhaseVector(Mathf.Repeat(PhaseSeries.Phases[index][b] + Utility.SignedPhaseUpdate(PhaseSeries.Phases[index][b], Utility.PhaseValue(state)), 1f)),
stability).normalized
);
PhaseSeries.Amplitudes[index][b] = update.magnitude;
PhaseSeries.Phases[index][b] = phase;
}
}
//Interpolate Timeseries
RootSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
StyleSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
PhaseSeries.Interpolate(TimeSeries.Pivot, TimeSeries.Samples.Length);
//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]);
}
// Correct Twist
for (int i = 0; i < Actor.Bones.Length; i++)
{
if (Actor.Bones[i].Childs.Length == 1)
{
Vector3 position = Actor.Bones[i].Transform.position;
Quaternion rotation = Actor.Bones[i].Transform.rotation;
Vector3 childPosition = Actor.Bones[i].GetChild(0).Transform.position;
Quaternion childRotation = Actor.Bones[i].GetChild(0).Transform.rotation;
Vector3 aligned = (position - childPosition).normalized;
float[] angles = new float[] {
Vector3.Angle(rotation.GetRight(), aligned),
Vector3.Angle(rotation.GetUp(), aligned),
Vector3.Angle(rotation.GetForward(), aligned),
Vector3.Angle(-rotation.GetRight(), aligned),
Vector3.Angle(-rotation.GetUp(), aligned),
Vector3.Angle(-rotation.GetForward(), aligned)
};
float min = angles.Min();
if (min == angles[0])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(rotation.GetRight(), aligned) * rotation;
}
if (min == angles[1])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(rotation.GetUp(), aligned) * rotation;
}
if (min == angles[2])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(rotation.GetForward(), aligned) * rotation;
}
if (min == angles[3])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(-rotation.GetRight(), aligned) * rotation;
}
if (min == angles[4])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(-rotation.GetRight(), aligned) * rotation;
}
if (min == angles[5])
{
Actor.Bones[i].Transform.rotation = Quaternion.FromToRotation(-rotation.GetForward(), aligned) * rotation;
}
Actor.Bones[i].GetChild(0).Transform.position = childPosition;
Actor.Bones[i].GetChild(0).Transform.rotation = childRotation;
}
}
//Resolve Trajectory Collisions
RootSeries.ResolveCollisions(Collider.radius, CollisionMask);
//Process Contact States
ProcessFootIK(LeftHandIK, ContactSeries.Values[TimeSeries.Pivot][0]);
ProcessFootIK(RightHandIK, ContactSeries.Values[TimeSeries.Pivot][1]);
ProcessFootIK(LeftFootIK, ContactSeries.Values[TimeSeries.Pivot][2]);
ProcessFootIK(RightFootIK, ContactSeries.Values[TimeSeries.Pivot][3]);
}
