Beispiel #1
0
    void Update()
    {
        if (TrajectoryControl)
        {
            //Update Target Direction / Velocity
            TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
            TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);

            //Update Bias
            Bias = Utility.Interpolate(Bias, PoolBias(), TargetBlending);

            //Update Trajectory Correction
            TrajectoryCorrection = Utility.Interpolate(TrajectoryCorrection, Mathf.Max(Controller.QueryMove().normalized.magnitude, Mathf.Abs(Controller.QueryTurn())), TargetBlending);

            //Update Style
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                if (i == 0)
                {
                    if (!Controller.QueryAny())
                    {
                        Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], 1f, StyleTransition);
                    }
                    else
                    {
                        Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, StyleTransition);
                    }
                }
                else
                {
                    Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, StyleTransition);
                }
            }

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetTransformation().GetPosition();
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));

                float scale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    scale * Bias * TargetVelocity,
                    scale_pos);

                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                Trajectory.Points[i].SetVelocity(Bias * TargetVelocity.magnitude);                 //Set Desired Smoothed Root Velocities

                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition(((1f - factor) * prev.GetPosition() + factor * next.GetPosition()));
                Trajectory.Points[i].SetDirection(((1f - factor) * prev.GetDirection() + factor * next.GetDirection()));
                Trajectory.Points[i].SetVelocity((1f - factor) * prev.GetVelocity() + factor * next.GetVelocity());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }
        }

        if (MLP.Parameters != null)
        {
            //Calculate Root
            Matrix4x4 currentRoot = Trajectory.Points[RootPointIndex].GetTransformation();
            //Fix for flat terrain
            Transformations.SetPosition(
                ref currentRoot,
                new Vector3(currentRoot.GetPosition().x, 0f, currentRoot.GetPosition().z)
                );
            //

            int start = 0;
            //Input Trajectory Positions / Directions
            for (int i = 0; i < PointSamples; i++)
            {
                Vector3 pos = GetSample(i).GetPosition().GetRelativePositionTo(currentRoot);
                Vector3 dir = GetSample(i).GetDirection().GetRelativeDirectionTo(currentRoot);
                MLP.SetInput(start + i * 6 + 0, pos.x);
                //MLP.SetInput(start + i*6 + 1, pos.y); //Fix for flat terrain
                MLP.SetInput(start + i * 6 + 1, 0f);
                MLP.SetInput(start + i * 6 + 2, pos.z);
                MLP.SetInput(start + i * 6 + 3, dir.x);
                //MLP.SetInput(start + i*6 + 4, dir.y); //Fix for flat terrain
                MLP.SetInput(start + i * 6 + 4, 0f);
                MLP.SetInput(start + i * 6 + 5, dir.z);
            }
            start += 6 * PointSamples;

            //Input Trajectory Heights
            for (int i = 0; i < PointSamples; i++)
            {
                //MLP.SetInput(start + i*2 + 0, GetSample(i).GetLeftSample().y - currentRoot.GetPosition().y); //Fix for flat terrain
                //MLP.SetInput(start + i*2 + 1, GetSample(i).GetRightSample().y - currentRoot.GetPosition().y); //Fix for flat terrain
                MLP.SetInput(start + i * 2 + 0, 0f);
                MLP.SetInput(start + i * 2 + 1, 0f);
            }
            start += 2 * PointSamples;

            //Input Trajectory Styles
            for (int i = 0; i < PointSamples; i++)
            {
                for (int j = 0; j < GetSample(i).Styles.Length; j++)
                {
                    MLP.SetInput(start + i * GetSample(i).Styles.Length + j, GetSample(i).Styles[j]);
                }
            }
            start += Controller.Styles.Length * PointSamples;

            //Input Previous Bone Positions / Velocities
            Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();
            //Fix for flat terrain
            Transformations.SetPosition(
                ref previousRoot,
                new Vector3(previousRoot.GetPosition().x, 0f, previousRoot.GetPosition().z)
                );
            //
            for (int i = 0; i < Joints.Length; i++)
            {
                Vector3 pos     = Positions[i].GetRelativePositionTo(previousRoot);
                Vector3 forward = Forwards[i].GetRelativeDirectionTo(previousRoot);
                Vector3 up      = Ups[i].GetRelativeDirectionTo(previousRoot);
                Vector3 vel     = Velocities[i].GetRelativeDirectionTo(previousRoot);
                MLP.SetInput(start + i * JointDimIn + 0, pos.x);
                MLP.SetInput(start + i * JointDimIn + 1, pos.y);
                MLP.SetInput(start + i * JointDimIn + 2, pos.z);
                MLP.SetInput(start + i * JointDimIn + 3, forward.x);
                MLP.SetInput(start + i * JointDimIn + 4, forward.y);
                MLP.SetInput(start + i * JointDimIn + 5, forward.z);
                MLP.SetInput(start + i * JointDimIn + 6, up.x);
                MLP.SetInput(start + i * JointDimIn + 7, up.y);
                MLP.SetInput(start + i * JointDimIn + 8, up.z);
                MLP.SetInput(start + i * JointDimIn + 9, vel.x);
                MLP.SetInput(start + i * JointDimIn + 10, vel.y);
                MLP.SetInput(start + i * JointDimIn + 11, vel.z);
            }
            start += JointDimIn * Joints.Length;

            if (name == "Wolf_MLP_P")
            {
                MLP.SetInput(start, Phase); start += 1;
            }

            //Predict
            MLP.Predict();

            //Update Past Trajectory
            for (int i = 0; i < RootPointIndex; i++)
            {
                Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                Trajectory.Points[i].SetVelocity(Trajectory.Points[i + 1].GetVelocity());
                Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                }
            }

            //Update Current Trajectory
            int     end = 6 * 4 + JointDimOut * Joints.Length;
            Vector3 translationalOffset = new Vector3(MLP.GetOutput(end + 0), 0f, MLP.GetOutput(end + 1));
            float   angularOffset       = MLP.GetOutput(end + 2);

            Trajectory.Points[RootPointIndex].SetPosition(translationalOffset.GetRelativePositionFrom(currentRoot));
            Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(angularOffset, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
            Trajectory.Points[RootPointIndex].Postprocess();
            Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();
            //Fix for flat terrain
            Transformations.SetPosition(
                ref nextRoot,
                new Vector3(nextRoot.GetPosition().x, 0f, nextRoot.GetPosition().z)
                );
            //

            //Update Future Trajectory
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + translationalOffset.GetRelativeDirectionFrom(nextRoot));
            }
            start = 0;
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                int   index           = i;
                int   prevSampleIndex = GetPreviousSample(index).GetIndex() / PointDensity;
                int   nextSampleIndex = GetNextSample(index).GetIndex() / PointDensity;
                float factor          = (float)(i % PointDensity) / PointDensity;

                float prevPosX = MLP.GetOutput(start + (prevSampleIndex - 6) * 4 + 0);
                float prevPosZ = MLP.GetOutput(start + (prevSampleIndex - 6) * 4 + 1);
                float prevDirX = MLP.GetOutput(start + (prevSampleIndex - 6) * 4 + 2);
                float prevDirZ = MLP.GetOutput(start + (prevSampleIndex - 6) * 4 + 3);

                float nextPosX = MLP.GetOutput(start + (nextSampleIndex - 6) * 4 + 0);
                float nextPosZ = MLP.GetOutput(start + (nextSampleIndex - 6) * 4 + 1);
                float nextDirX = MLP.GetOutput(start + (nextSampleIndex - 6) * 4 + 2);
                float nextDirZ = MLP.GetOutput(start + (nextSampleIndex - 6) * 4 + 3);

                float posX = (1f - factor) * prevPosX + factor * nextPosX;
                float posZ = (1f - factor) * prevPosZ + factor * nextPosZ;
                float dirX = (1f - factor) * prevDirX + factor * nextDirX;
                float dirZ = (1f - factor) * prevDirZ + factor * nextDirZ;

                Trajectory.Points[i].SetPosition(
                    Utility.Interpolate(
                        Trajectory.Points[i].GetPosition(),
                        new Vector3(posX, 0f, posZ).GetRelativePositionFrom(nextRoot),
                        TrajectoryCorrection
                        )
                    );
                Trajectory.Points[i].SetDirection(
                    Utility.Interpolate(
                        Trajectory.Points[i].GetDirection(),
                        new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                        TrajectoryCorrection
                        )
                    );
            }
            start += 6 * 4;
            for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition(((1f - factor) * prev.GetPosition() + factor * next.GetPosition()));
                Trajectory.Points[i].SetDirection(((1f - factor) * prev.GetDirection() + factor * next.GetDirection()));
                Trajectory.Points[i].SetVelocity((1f - factor) * prev.GetVelocity() + factor * next.GetVelocity());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            Trajectory.Points[RootPointIndex].SetVelocity((Trajectory.GetLast().GetPosition() - transform.position).magnitude);             //Correct Current Smoothed Root Velocity

            //Compute Posture
            for (int i = 0; i < Joints.Length; i++)
            {
                Vector3 position = new Vector3(MLP.GetOutput(start + i * JointDimOut + 0), MLP.GetOutput(start + i * JointDimOut + 1), MLP.GetOutput(start + i * JointDimOut + 2));
                Vector3 forward  = new Vector3(MLP.GetOutput(start + i * JointDimOut + 3), MLP.GetOutput(start + i * JointDimOut + 4), MLP.GetOutput(start + i * JointDimOut + 5)).normalized;
                Vector3 up       = new Vector3(MLP.GetOutput(start + i * JointDimOut + 6), MLP.GetOutput(start + i * JointDimOut + 7), MLP.GetOutput(start + i * JointDimOut + 8)).normalized;
                Vector3 velocity = new Vector3(MLP.GetOutput(start + i * JointDimOut + 9), MLP.GetOutput(start + i * JointDimOut + 10), MLP.GetOutput(start + i * JointDimOut + 11));

                Positions[i]  = Vector3.Lerp(Positions[i].GetRelativePositionTo(currentRoot) + velocity, position, 0.5f).GetRelativePositionFrom(currentRoot);
                Forwards[i]   = forward.GetRelativeDirectionFrom(currentRoot);
                Ups[i]        = up.GetRelativeDirectionFrom(currentRoot);
                Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
            }
            start += JointDimOut * Joints.Length;

            //Update Posture
            Root.position = nextRoot.GetPosition();
            Root.rotation = nextRoot.GetRotation();
            for (int i = 0; i < Joints.Length; i++)
            {
                Joints[i].position = Positions[i];
                Joints[i].rotation = Quaternion.LookRotation(Forwards[i], Ups[i]);
            }

            transform.position = new Vector3(Root.position.x, 0f, Root.position.z);             //Fix for flat ground

            /*
             * if(SolveIK) {
             *      //Step #1
             *      for(int i=0; i<IKSolvers.Length; i++) {
             *              if(IKSolvers[i].name != "Tail") {
             *                      float heightThreshold = i==0 || i==1 ? 0.025f : 0.05f;
             *                      float velocityThreshold = i==0 || i== 1 ? 0.015f : 0.015f;
             *                      Vector3 goal = IKSolvers[i].GetTipPosition();
             *                      IKSolvers[i].Goal.y = goal.y;
             *                      float velocityDelta = (goal - IKSolvers[i].Goal).magnitude;
             *                      float velocityWeight = Utility.Exponential01(velocityDelta / velocityThreshold);
             *                      float heightDelta = goal.y;
             *                      float heightWeight = Utility.Exponential01(heightDelta / heightThreshold);
             *                      float weight = Mathf.Min(velocityWeight, heightWeight);
             *                      IKSolvers[i].Goal = Vector3.Lerp(IKSolvers[i].Goal, goal, weight);
             *              }
             *      }
             *      for(int i=0; i<IKSolvers.Length; i++) {
             *              if(IKSolvers[i].name != "Tail") {
             *                      IKSolvers[i].ProcessIK();
             *              }
             *      }
             *      for(int i=0; i<Joints.Length; i++) {
             *              Positions[i] = Joints[i].position;
             *              //Forwards[i] = Joints[i].forward;
             *              //Ups[i] = Joints[i].up;
             *      }
             * }
             */

            transform.position = Trajectory.Points[RootPointIndex].GetPosition();             //Fix for flat ground

            /*
             * if(SolveIK) {
             *      //Step #2
             *      for(int i=0; i<IKSolvers.Length; i++) {
             *              IKSolvers[i].Goal = IKSolvers[i].GetTipPosition();
             *              float height = Utility.GetHeight(IKSolvers[i].Goal, LayerMask.GetMask("Ground"));
             *              if(IKSolvers[i].name == "Tail") {
             *                      IKSolvers[i].Goal.y = Mathf.Max(height, height + (IKSolvers[i].Goal.y - transform.position.y));
             *              } else {
             *                      IKSolvers[i].Goal.y = height + (IKSolvers[i].Goal.y - transform.position.y);
             *              }
             *      }
             *      Transform spine = Array.Find(Joints, x => x.name == "Spine1");
             *      Transform neck = Array.Find(Joints, x => x.name == "Neck");
             *      Transform leftShoulder = Array.Find(Joints, x => x.name == "LeftShoulder");
             *      Transform rightShoulder = Array.Find(Joints, x => x.name == "RightShoulder");
             *      Vector3 spinePosition = spine.position;
             *      Vector3 neckPosition = neck.position;
             *      Vector3 leftShoulderPosition = leftShoulder.position;
             *      Vector3 rightShoulderPosition = rightShoulder.position;
             *      float spineHeight = Utility.GetHeight(spine.position, LayerMask.GetMask("Ground"));
             *      float neckHeight = Utility.GetHeight(neck.position, LayerMask.GetMask("Ground"));
             *      float leftShoulderHeight = Utility.GetHeight(leftShoulder.position, LayerMask.GetMask("Ground"));
             *      float rightShoulderHeight = Utility.GetHeight(rightShoulder.position, LayerMask.GetMask("Ground"));
             *      spine.rotation = Quaternion.Slerp(spine.rotation, Quaternion.FromToRotation(neckPosition - spinePosition, new Vector3(neckPosition.x, neckHeight + (neckPosition.y - Root.position.y), neckPosition.z) - spinePosition) * spine.rotation, 0.5f);
             *      spine.position = new Vector3(spinePosition.x, spineHeight + (spinePosition.y - Root.position.y), spinePosition.z);
             *      neck.position = new Vector3(neckPosition.x, neckHeight + (neckPosition.y - Root.position.y), neckPosition.z);
             *      leftShoulder.position = new Vector3(leftShoulderPosition.x, leftShoulderHeight + (leftShoulderPosition.y - Root.position.y), leftShoulderPosition.z);
             *      rightShoulder.position = new Vector3(rightShoulderPosition.x, rightShoulderHeight + (rightShoulderPosition.y - Root.position.y), rightShoulderPosition.z);
             *      for(int i=0; i<IKSolvers.Length; i++) {
             *              IKSolvers[i].ProcessIK();
             *      }
             * }
             */

            //Update Skeleton
            Character.FetchTransformations(Root);

            if (name == "Wolf_MLP_P")
            {
                //Update Phase
                Phase = Mathf.Repeat(Phase + MLP.GetOutput(end + 3), 1f);
            }
        }
    }
Beispiel #2
0
        void Update()
        {
            if (NN.Model.Parameters == null)
            {
                return;
            }

            //Update Target Direction / Velocity
            TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
            TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);

            //Update Gait
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, GaitTransition);
            }
            //For Human Only
            // Trajectory.Points[RootPointIndex].Styles[0] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[0], 1.0f - Mathf.Clamp(Vector3.Magnitude(TargetVelocity) / 0.1f, 0.0f, 1.0f), GaitTransition);
            // Trajectory.Points[RootPointIndex].Styles[1] = Mathf.Max(Trajectory.Points[RootPointIndex].Styles[1] - Trajectory.Points[RootPointIndex].Styles[2], 0f);
            //

            /*
             * //Blend Trajectory Offset
             * Vector3 positionOffset = transform.position - Trajectory.Points[RootPointIndex].GetPosition();
             * Quaternion rotationOffset = Quaternion.Inverse(Trajectory.Points[RootPointIndex].GetRotation()) * transform.rotation;
             * Trajectory.Points[RootPointIndex].SetPosition(Trajectory.Points[RootPointIndex].GetPosition() + positionOffset);
             * Trajectory.Points[RootPointIndex].SetDirection(rotationOffset * Trajectory.Points[RootPointIndex].GetDirection());
             *
             * for(int i=RootPointIndex; i<Trajectory.Points.Length; i++) {
             *      float factor = 1f - (i - RootPointIndex)/(RootPointIndex - 1f);
             *      Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + factor*positionOffset);
             * }
             */

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetPosition();

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));
                float vel_boost = PoolBias();

                float rescale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    vel_boost * rescale * TargetVelocity,
                    scale_pos);

                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            //Avoid Collisions
            CollisionChecks(RootPointIndex + 1);

            if (NN.Model.Parameters != null)
            {
                //Calculate Root
                Matrix4x4 currentRoot  = Trajectory.Points[RootPointIndex].GetTransformation();
                Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();

                //Input Trajectory Positions / Directions
                for (int i = 0; i < PointSamples; i++)
                {
                    Vector3 pos = Trajectory.Points[i * PointDensity].GetPosition().GetRelativePositionTo(currentRoot);
                    Vector3 dir = Trajectory.Points[i * PointDensity].GetDirection().GetRelativeDirectionTo(currentRoot);
                    NN.Model.SetInput(PointSamples * 0 + i, UnitScale * pos.x);
                    NN.Model.SetInput(PointSamples * 1 + i, UnitScale * pos.z);
                    NN.Model.SetInput(PointSamples * 2 + i, dir.x);
                    NN.Model.SetInput(PointSamples * 3 + i, dir.z);
                }

                //Input Trajectory Gaits
                for (int i = 0; i < PointSamples; i++)
                {
                    for (int j = 0; j < Trajectory.Points[i * PointDensity].Styles.Length; j++)
                    {
                        NN.Model.SetInput(PointSamples * (4 + j) + i, Trajectory.Points[i * PointDensity].Styles[j]);
                    }
                    //FOR HUMAN ONLY - No terrain for us.
                    // NN.Model.SetInput(PointSamples*8 + i, Trajectory.Points[i*PointDensity].GetSlope());
                    //
                }

                //Input Previous Bone Positions / Velocities
                for (int i = 0; i < Joints.Length; i++)
                {
                    int     o   = 6 * PointSamples;
                    Vector3 pos = Joints[i].position.GetRelativePositionTo(previousRoot);
                    Vector3 vel = Velocities[i].GetRelativeDirectionTo(previousRoot);
                    NN.Model.SetInput(o + Joints.Length * 3 * 0 + i * 3 + 0, UnitScale * pos.x);
                    NN.Model.SetInput(o + Joints.Length * 3 * 0 + i * 3 + 1, UnitScale * pos.y);
                    NN.Model.SetInput(o + Joints.Length * 3 * 0 + i * 3 + 2, UnitScale * pos.z);
                    NN.Model.SetInput(o + Joints.Length * 3 * 1 + i * 3 + 0, UnitScale * vel.x);
                    NN.Model.SetInput(o + Joints.Length * 3 * 1 + i * 3 + 1, UnitScale * vel.y);
                    NN.Model.SetInput(o + Joints.Length * 3 * 1 + i * 3 + 2, UnitScale * vel.z);
                }

                // //Input Trajectory Heights - No hieght in our model
                // for(int i=0; i<PointSamples; i++) {
                //  int o = 10*PointSamples + Joints.Length*3*2;
                //  NN.Model.SetInput(o + PointSamples*0 + i, UnitScale * (Trajectory.Points[i*PointDensity].GetRightSample().y - currentRoot.GetPosition().y));
                //  NN.Model.SetInput(o + PointSamples*1 + i, UnitScale * (Trajectory.Points[i*PointDensity].GetPosition().y - currentRoot.GetPosition().y));
                //  NN.Model.SetInput(o + PointSamples*2 + i, UnitScale * (Trajectory.Points[i*PointDensity].GetLeftSample().y - currentRoot.GetPosition().y));
                // }

                //Predict
                start = Time.realtimeSinceStartup;
                NN.Model.Predict();
                end = Time.realtimeSinceStartup;
                time[predcount % 1000] = end - start;
                predcount++;
                if (predcount >= 1000)
                {
                    sum = 0f;
                    for (int i = 0; i < time.Length; i++)
                    {
                        sum += time[i];
                    }
                    Debug.Log(sum / time.Length);
                }

                //Update Past Trajectory
                for (int i = 0; i < RootPointIndex; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                    Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                    Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                    Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                    Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                    for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                    {
                        Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                    }
                }

                //Update Current Trajectory
                float rest = 1.0f;                 //Mathf.Pow(1.0f-Trajectory.Points[RootPointIndex].Styles[0], 0.25f);
                Trajectory.Points[RootPointIndex].SetPosition((rest * new Vector3(NN.Model.GetOutput(0) / UnitScale, 0f, NN.Model.GetOutput(1) / UnitScale)).GetRelativePositionFrom(currentRoot));
                // Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(rest * Mathf.Rad2Deg * (-NN.Model.GetOutput(2)), Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
                Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(rest * (NN.Model.GetOutput(2)), Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
                Trajectory.Points[RootPointIndex].Postprocess();
                Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();

                //Update Future Trajectory
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + (rest * new Vector3(NN.Model.GetOutput(0) / UnitScale, 0f, NN.Model.GetOutput(1) / UnitScale)).GetRelativeDirectionFrom(nextRoot));
                }
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    int   w    = RootSampleIndex;
                    float m    = Mathf.Repeat(((float)i - (float)RootPointIndex) / (float)PointDensity, 1.0f);
                    float posX = (1 - m) * NN.Model.GetOutput(4 + (w * 0) + (i / PointDensity) - w) + m * NN.Model.GetOutput(4 + (w * 0) + (i / PointDensity) - w + 1);
                    float posZ = (1 - m) * NN.Model.GetOutput(4 + (w * 1) + (i / PointDensity) - w) + m * NN.Model.GetOutput(4 + (w * 1) + (i / PointDensity) - w + 1);
                    float dirX = (1 - m) * NN.Model.GetOutput(4 + (w * 2) + (i / PointDensity) - w) + m * NN.Model.GetOutput(4 + (w * 2) + (i / PointDensity) - w + 1);
                    float dirZ = (1 - m) * NN.Model.GetOutput(4 + (w * 3) + (i / PointDensity) - w) + m * NN.Model.GetOutput(4 + (w * 3) + (i / PointDensity) - w + 1);
                    Trajectory.Points[i].SetPosition(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetPosition(),
                            new Vector3(posX / UnitScale, 0f, posZ / UnitScale).GetRelativePositionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                    Trajectory.Points[i].SetDirection(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetDirection(),
                            new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                }

                for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
                {
                    Trajectory.Points[i].Postprocess();
                }

                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    //ROOT	1		2		3		4		5
                    //.x....x.......x.......x.......x.......x
                    Trajectory.Point prev   = GetPreviousSample(i);
                    Trajectory.Point next   = GetNextSample(i);
                    float            factor = (float)(i % PointDensity) / PointDensity;

                    Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                    Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                    Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                    Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                    Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
                }

                //Avoid Collisions
                CollisionChecks(RootPointIndex);

                //Compute Posture
                Vector3[] positions = new Vector3[Joints.Length];
                // Quaternion[] rotations = new Quaternion[Joints.Length];
                int opos  = 4 + 4 * RootSampleIndex + Joints.Length * 3 * 0;
                int ovel  = 4 + 4 * RootSampleIndex + Joints.Length * 3 * 1;
                int orot  = 4 + 4 * RootSampleIndex + Joints.Length * 3 * 2;
                int orot2 = 4 + 4 * RootSampleIndex + Joints.Length * 3 * 3;
                for (int i = 0; i < Joints.Length; i++)
                {
                    Vector3 position = new Vector3(NN.Model.GetOutput(opos + i * 3 + 0), NN.Model.GetOutput(opos + i * 3 + 1), NN.Model.GetOutput(opos + i * 3 + 2)) / UnitScale;
                    Vector3 velocity = new Vector3(NN.Model.GetOutput(ovel + i * 3 + 0), NN.Model.GetOutput(ovel + i * 3 + 1), NN.Model.GetOutput(ovel + i * 3 + 2)) / UnitScale;
                    Vector3 forward  = new Vector3(NN.Model.GetOutput(orot + i * 3 + 0), NN.Model.GetOutput(orot + i * 3 + 1), NN.Model.GetOutput(orot + i * 3 + 2)) / UnitScale;
                    Vector3 up       = new Vector3(NN.Model.GetOutput(orot2 + i * 3 + 0), NN.Model.GetOutput(orot2 + i * 3 + 1), NN.Model.GetOutput(orot2 + i * 3 + 2)) / UnitScale;
                    //Quaternion rotation = new Quaternion(PFNN.GetOutput(orot+i*3+0), PFNN.GetOutput(orot+i*3+1), PFNN.GetOutput(orot+i*3+2), 0f).Exp();
                    positions[i]  = Vector3.Lerp(Joints[i].position.GetRelativePositionTo(currentRoot) + velocity, position, 0.5f).GetRelativePositionFrom(currentRoot);
                    Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
                    Forwards[i]   = forward.normalized.GetRelativeDirectionFrom(currentRoot);
                    Ups[i]        = up.normalized.GetRelativeDirectionFrom(currentRoot);
                    //rotations[i] = rotation.GetRelativeRotationFrom(currentRoot);
                }

                //Update Posture
                Root.position = nextRoot.GetPosition();
                Root.rotation = nextRoot.GetRotation();
                for (int i = 0; i < Joints.Length; i++)
                {
                    Joints[i].position = positions[i];
                    Joints[i].rotation = Quaternion.LookRotation(Forwards[i], Ups[i]);
                }

                //Map to Character
                Character.FetchTransformations(Root);

                //Update Phase
                ((PFNN)NN.Model).SetPhase(Mathf.Repeat(((PFNN)NN.Model).GetPhase() + (rest * 0.9f + 0.1f) * NN.Model.GetOutput(3) * 2f * Mathf.PI, 2f * Mathf.PI));
            }
        }
        void Update()
        {
            if (NN.Parameters == null)
            {
                return;
            }

            //Update Target Direction / Velocity
            TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
            TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);

            //Update Gait
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, GaitTransition);
            }

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetPosition();
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));
                float vel_boost = PoolBias();

                float rescale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    vel_boost * rescale * TargetVelocity,
                    scale_pos);

                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            //Avoid Collisions
            CollisionChecks(RootPointIndex + 1);

            if (NN.Parameters != null)
            {
                //Calculate Root
                Matrix4x4 currentRoot  = Trajectory.Points[RootPointIndex].GetTransformation();
                Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();

                //Input Trajectory Positions / Directions
                for (int i = 0; i < PointSamples; i++)
                {
                    Vector3 pos = Trajectory.Points[i * PointDensity].GetPosition().GetRelativePositionTo(currentRoot);
                    Vector3 dir = Trajectory.Points[i * PointDensity].GetDirection().GetRelativeDirectionTo(currentRoot);
                    NN.SetInput(PointSamples * 0 + i, UnitScale * pos.x);
                    NN.SetInput(PointSamples * 1 + i, UnitScale * pos.z);
                    NN.SetInput(PointSamples * 2 + i, dir.x);
                    NN.SetInput(PointSamples * 3 + i, dir.z);
                }

                //Input Trajectory Gaits
                for (int i = 0; i < PointSamples; i++)
                {
                    for (int j = 0; j < Trajectory.Points[i * PointDensity].Styles.Length; j++)
                    {
                        NN.SetInput(PointSamples * (4 + j) + i, Trajectory.Points[i * PointDensity].Styles[j]);
                    }
                    //FOR HUMAN ONLY
                    NN.SetInput(PointSamples * 8 + i, Trajectory.Points[i * PointDensity].GetSlope());
                    //
                }

                //Input Previous Bone Positions / Velocities
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    int     o   = 10 * PointSamples;
                    Vector3 pos = Positions[i].GetRelativePositionTo(previousRoot);
                    Vector3 vel = Velocities[i].GetRelativeDirectionTo(previousRoot);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 0, UnitScale * pos.x);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 1, UnitScale * pos.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 2, UnitScale * pos.z);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 0, UnitScale * vel.x);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 1, UnitScale * vel.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 2, UnitScale * vel.z);
                }

                //Input Trajectory Heights
                for (int i = 0; i < PointSamples; i++)
                {
                    int o = 10 * PointSamples + Actor.Bones.Length * 3 * 2;
                    NN.SetInput(o + PointSamples * 0 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetRightSample().y - currentRoot.GetPosition().y));
                    NN.SetInput(o + PointSamples * 1 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetPosition().y - currentRoot.GetPosition().y));
                    NN.SetInput(o + PointSamples * 2 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetLeftSample().y - currentRoot.GetPosition().y));
                }

                //Predict
                float tmp  = Trajectory.Points[RootPointIndex].Styles.Length == 0 ? 1 : Trajectory.Points[RootPointIndex].Styles[0];
                float rest = Mathf.Pow(1.0f - tmp, 0.25f);
                NN.SetDamping(1f - (rest * 0.9f + 0.1f));
                NN.Predict();

                //Update Past Trajectory
                for (int i = 0; i < RootPointIndex; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                    Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                    Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                    Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                    Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                    for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                    {
                        Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                    }
                }

                //Update Current Trajectory
                Trajectory.Points[RootPointIndex].SetPosition((rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativePositionFrom(currentRoot));
                Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(rest * Mathf.Rad2Deg * (-NN.GetOutput(2)), Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
                Trajectory.Points[RootPointIndex].Postprocess();
                Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();

                //Update Future Trajectory
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + (rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativeDirectionFrom(nextRoot));
                }
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    int   w    = RootSampleIndex;
                    float m    = Mathf.Repeat(((float)i - (float)RootPointIndex) / (float)PointDensity, 1.0f);
                    float posX = (1 - m) * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w + 1);
                    float posZ = (1 - m) * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w + 1);
                    float dirX = (1 - m) * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w + 1);
                    float dirZ = (1 - m) * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w + 1);
                    Trajectory.Points[i].SetPosition(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetPosition(),
                            new Vector3(posX / UnitScale, 0f, posZ / UnitScale).GetRelativePositionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                    Trajectory.Points[i].SetDirection(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetDirection(),
                            new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                }

                for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
                {
                    Trajectory.Points[i].Postprocess();
                }

                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    //ROOT	1		2		3		4		5
                    //.x....x.......x.......x.......x.......x
                    Trajectory.Point prev   = GetPreviousSample(i);
                    Trajectory.Point next   = GetNextSample(i);
                    float            factor = (float)(i % PointDensity) / PointDensity;

                    Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                    Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                    Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                    Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                    Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
                }

                //Avoid Collisions
                CollisionChecks(RootPointIndex);

                //Compute Posture
                int opos = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 0;
                int ovel = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 1;
                int orot = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 2;
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    Vector3 position = new Vector3(NN.GetOutput(opos + i * 3 + 0), NN.GetOutput(opos + i * 3 + 1), NN.GetOutput(opos + i * 3 + 2)) / UnitScale;
                    Vector3 velocity = new Vector3(NN.GetOutput(ovel + i * 3 + 0), NN.GetOutput(ovel + i * 3 + 1), NN.GetOutput(ovel + i * 3 + 2)) / UnitScale;

                    //Adjust for VRoid
                    if (i >= 13 && i <= 16)
                    {
                        position.z -= 0.06f;
                    }
                    if (i == 0 || i == 11 || i == 12)
                    {
                        position.z -= 0.02f;
                    }
                    if (i == 17 || i == 24)
                    {
                        position.z -= 0.05f;
                    }
                    if (i == 16)
                    {
                        position.y -= 0.01f;
                    }

                    Positions[i]  = Vector3.Lerp(Positions[i].GetRelativePositionTo(currentRoot) + velocity, position, 0.5f);
                    Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
                }

                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    if (i + 1 < Actor.Bones.Length)
                    {
                        Rotations[i] = Quaternion.FromToRotation(RotationsFrom[i], Positions[i + 1] - Positions[i]);
                    }
                    Rotations[i] = Rotations[i].GetRelativeRotationFrom(currentRoot);

                    Positions[i] = Positions[i].GetRelativePositionFrom(currentRoot);
                }

                //Update Posture
                transform.position = nextRoot.GetPosition();
                transform.rotation = nextRoot.GetRotation();

                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    //Adjust for VRoid
                    if (i == 20 || i == 27)
                    {
                        continue;
                    }
                    Actor.Bones[i].Transform.position = Positions[i];

                    //Adjust for VRoid
                    if (i == 15 || i == 16)
                    {
                        continue;
                    }

                    Actor.Bones[i].Transform.rotation = Rotations[i];
                }
            }
        }
        void Update()
        {
            if (NN.Parameters == null)
            {
                return;
            }

            //Update Target Direction / Velocity, 从控制器可以获得面朝向,移动方向
            TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
            TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);

            //Update Gait, , 从控制器获得姿势,这个是float,因为值得改变是每帧插值过去的
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, GaitTransition);
            }
            //For Human Only
            //Trajectory.Points[RootPointIndex].Styles[0] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[0], 1.0f - Mathf.Clamp(Vector3.Magnitude(TargetVelocity) / 0.1f, 0.0f, 1.0f), GaitTransition);
            //Trajectory.Points[RootPointIndex].Styles[1] = Mathf.Max(Trajectory.Points[RootPointIndex].Styles[1] - Trajectory.Points[RootPointIndex].Styles[2], 0f);
            //

            /*
             * //Blend Trajectory Offset
             * Vector3 positionOffset = transform.position - Trajectory.Points[RootPointIndex].GetPosition();
             * Quaternion rotationOffset = Quaternion.Inverse(Trajectory.Points[RootPointIndex].GetRotation()) * transform.rotation;
             * Trajectory.Points[RootPointIndex].SetPosition(Trajectory.Points[RootPointIndex].GetPosition() + positionOffset);
             * Trajectory.Points[RootPointIndex].SetDirection(rotationOffset * Trajectory.Points[RootPointIndex].GetDirection());
             *
             * for(int i=RootPointIndex; i<Trajectory.Points.Length; i++) {
             *      float factor = 1f - (i - RootPointIndex)/(RootPointIndex - 1f);
             *      Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + factor*positionOffset);
             * }
             */

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetPosition();

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));
                float vel_boost = PoolBias();                 //基本就是1啊

                // 因为总共预测得是1秒之后位置,所以每个点相对偏移要乘以这个
                float rescale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                // 未来位置相对偏移,是预测位置相对偏移,和当前移动速度 的一个插值
                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    vel_boost * rescale * TargetVelocity,
                    scale_pos);


                // 未来方向 是预测方向 和 当前方向 的插值
                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                // 未来姿势gait是跟当前一致
                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }

            // 设置上位置
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            // 只有整点 来做地形处理,获得高度啥的
            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }

            // 其他中间点,都通过整点来插值得到
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                Trajectory.Point prev   = GetPreviousSample(i);               // 得到前一个整点
                Trajectory.Point next   = GetNextSample(i);                   // 下一个整点
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            //Avoid Collisions,跟障碍物Obstacles碰撞后,后面预测的点都维持在碰撞的当前点上
            CollisionChecks(RootPointIndex + 1);

            if (NN.Parameters != null)
            {
                //Calculate Root
                Matrix4x4 currentRoot  = Trajectory.Points[RootPointIndex].GetTransformation();
                Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();

                //Input Trajectory Positions / Directions
                for (int i = 0; i < PointSamples; i++)
                {
                    Vector3 pos = Trajectory.Points[i * PointDensity].GetPosition().GetRelativePositionTo(currentRoot);
                    Vector3 dir = Trajectory.Points[i * PointDensity].GetDirection().GetRelativeDirectionTo(currentRoot);
                    NN.SetInput(PointSamples * 0 + i, UnitScale * pos.x);                   // 神经网络输入:先是12个x位置(相对于当前坐标)
                    NN.SetInput(PointSamples * 1 + i, UnitScale * pos.z);                   // 12个z位置
                    NN.SetInput(PointSamples * 2 + i, dir.x);                               // 12个x方向
                    NN.SetInput(PointSamples * 3 + i, dir.z);                               // 12个z方向
                }

                //Input Trajectory Gaits
                for (int i = 0; i < PointSamples; i++)                                  // 12个stand 姿势值, 12个 walk姿势值,12个 jog姿势值,crouch,jump,bump,共12*6
                {
                    for (int j = 0; j < Trajectory.Points[i * PointDensity].Styles.Length; j++)
                    {
                        NN.SetInput(PointSamples * (4 + j) + i, Trajectory.Points[i * PointDensity].Styles[j]);
                    }
                    //FOR HUMAN ONLY, 这是把jump的gait信息给覆盖了。破面太陡,就自动jump
                    NN.SetInput(PointSamples * 8 + i, Trajectory.Points[i * PointDensity].GetSlope());
                    //
                }

                //Input Previous Bone Positions / Velocities
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    int     o   = 10 * PointSamples;                                                                                                    // input的index从120开始了
                    Vector3 pos = Positions[i].GetRelativePositionTo(previousRoot);
                    Vector3 vel = Velocities[i].GetRelativeDirectionTo(previousRoot);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 0, UnitScale * pos.x);             // 31个骨骼节点位置信息,x,y,z
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 1, UnitScale * pos.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 2, UnitScale * pos.z);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 0, UnitScale * vel.x);             // 31个骨骼节点速度信息,x,y,z
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 1, UnitScale * vel.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 2, UnitScale * vel.z);
                }

                //Input Trajectory Heights
                for (int i = 0; i < PointSamples; i++)
                {
                    int o = 10 * PointSamples + Actor.Bones.Length * 3 * 2;                                                                                    // input的index从120+186=306开始了
                    NN.SetInput(o + PointSamples * 0 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetRightSample().y - currentRoot.GetPosition().y)); // 12个右边点高度
                    NN.SetInput(o + PointSamples * 1 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetPosition().y - currentRoot.GetPosition().y));    // 12个中间点高度
                    NN.SetInput(o + PointSamples * 2 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetLeftSample().y - currentRoot.GetPosition().y));  // 12个左边点高度
                }

                //Predict
                float rest = Mathf.Pow(1.0f - Trajectory.Points[RootPointIndex].Styles[0], 0.25f); // stand为1时,rest为0,阻尼damping为0.9
                NN.SetDamping(1f - (rest * 0.9f + 0.1f));                                          // stand为0时,rest为1,damping为0,从而下帧phase就取 当前phase + 此帧predict后的phase偏移,实际的PhaseIndex=3
                NN.Predict();

                //Update Past Trajectory,之前的[1,60]帧记录下来到[0,59]
                for (int i = 0; i < RootPointIndex; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                    Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                    Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                    Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                    Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                    for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                    {
                        Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                    }
                }

                //Update Current Trajectory,Y的 0,1是预测的下一帧的dx,dz,2是预测的下一帧的转向角度
                Trajectory.Points[RootPointIndex].SetPosition((rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativePositionFrom(currentRoot));
                Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(rest * Mathf.Rad2Deg * (-NN.GetOutput(2)), Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
                Trajectory.Points[RootPointIndex].Postprocess();
                Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();

                //Update Future Trajectory,剩下的路径点,用当前帧的路径点 + 本位点预测相对于 实际设置的位置和方向的偏移
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + (rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativeDirectionFrom(nextRoot));
                }
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    int   w    = RootSampleIndex;
                    float m    = Mathf.Repeat(((float)i - (float)RootPointIndex) / (float)PointDensity, 1.0f);                                              // index从8开始
                    float posX = (1 - m) * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w + 1); // 先有6个posX
                    float posZ = (1 - m) * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w + 1); // 6个posZ
                    float dirX = (1 - m) * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w + 1); // 6个dirX
                    float dirZ = (1 - m) * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w + 1); // 6个dirZ
                    Trajectory.Points[i].SetPosition(                                                                                                       //这50个中间的45个点的pos,dir好像白计算了,后面会从12个位点中做插值
                        Utility.Interpolate(
                            Trajectory.Points[i].GetPosition(),
                            new Vector3(posX / UnitScale, 0f, posZ / UnitScale).GetRelativePositionFrom(nextRoot),
                            TrajectoryCorrection                             //实际设置位0.75
                            )
                        );
                    Trajectory.Points[i].SetDirection(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetDirection(),
                            new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                }

                for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
                {
                    Trajectory.Points[i].Postprocess();
                }

                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    //ROOT	1		2		3		4		5
                    //.x....x.......x.......x.......x.......x
                    Trajectory.Point prev   = GetPreviousSample(i);
                    Trajectory.Point next   = GetNextSample(i);
                    float            factor = (float)(i % PointDensity) / PointDensity;

                    Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                    Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                    Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                    Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                    Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
                }

                //Avoid Collisions
                CollisionChecks(RootPointIndex);

                //Compute Posture
                int opos = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 0;              // index从 8+24=30开始,有31*3个骨骼点位置
                int ovel = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 1;              // 从30+31*3开始,有93个骨骼点速度
                //int orot = 8 + 4*RootSampleIndex + Actor.Bones.Length*3*2;
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    Vector3 position = new Vector3(NN.GetOutput(opos + i * 3 + 0), NN.GetOutput(opos + i * 3 + 1), NN.GetOutput(opos + i * 3 + 2)) / UnitScale;
                    Vector3 velocity = new Vector3(NN.GetOutput(ovel + i * 3 + 0), NN.GetOutput(ovel + i * 3 + 1), NN.GetOutput(ovel + i * 3 + 2)) / UnitScale;
                    //Quaternion rotation = new Quaternion(PFNN.GetOutput(orot+i*3+0), PFNN.GetOutput(orot+i*3+1), PFNN.GetOutput(orot+i*3+2), 0f).Exp();
                    Positions[i]  = Vector3.Lerp(Positions[i].GetRelativePositionTo(currentRoot) + velocity, position, 0.5f).GetRelativePositionFrom(currentRoot);
                    Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
                    //rotations[i] = rotation.GetRelativeRotationFrom(currentRoot);
                }

                //Update Posture
                transform.position = nextRoot.GetPosition();
                transform.rotation = nextRoot.GetRotation();
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    Actor.Bones[i].Transform.position = Positions[i];                     //也就是说这个允许各个拉伸,但没有任何旋转,这应该是它跟Adam的区别。
                    Actor.Bones[i].Transform.rotation = Quaternion.LookRotation(Forwards[i], Ups[i]);
                }
            }
        }
Beispiel #5
0
        void Update()
        {
            if (NN.Parameters == null)
            {
                return;
            }
            if (activeSpline)
            {
                if (SplineController.Style.type == BezierSolution.BezierPoint.StatusMode.Wait && !Controller.waiting)
                {
                    Controller.waiting = true;
                    Invoke("StopWaiting", Controller.getCurrentPoints()[0].timeout);
                }
                else
                {
                    Vector3 targetDir = Controller.getTransition(transform);
                    TargetDirection = Vector3.Lerp(TargetDirection, targetDir, TargetBlending);
                    TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);
                }
            }
            else
            {
                //Update Target Direction / Velocity
                TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
                TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);
            }
            //Update Gait
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, GaitTransition);
            }
            //For Human Only
            Trajectory.Points[RootPointIndex].Styles[0] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[0], 1.0f - Mathf.Clamp(Vector3.Magnitude(TargetVelocity) / 0.1f, 0.0f, 1.0f), GaitTransition);
            Trajectory.Points[RootPointIndex].Styles[1] = Mathf.Max(Trajectory.Points[RootPointIndex].Styles[1] - Trajectory.Points[RootPointIndex].Styles[2], 0f);
            //

            /*
             * //Blend Trajectory Offset
             * Vector3 positionOffset = transform.position - Trajectory.Points[RootPointIndex].GetPosition();
             * Quaternion rotationOffset = Quaternion.Inverse(Trajectory.Points[RootPointIndex].GetRotation()) * transform.rotation;
             * Trajectory.Points[RootPointIndex].SetPosition(Trajectory.Points[RootPointIndex].GetPosition() + positionOffset);
             * Trajectory.Points[RootPointIndex].SetDirection(rotationOffset * Trajectory.Points[RootPointIndex].GetDirection());
             *
             * for(int i=RootPointIndex; i<Trajectory.Points.Length; i++) {
             *      float factor = 1f - (i - RootPointIndex)/(RootPointIndex - 1f);
             *      Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + factor*positionOffset);
             * }
             */

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetPosition();

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));
                float vel_boost = PoolBias();

                float rescale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    vel_boost * rescale * TargetVelocity,
                    scale_pos);

                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }

            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            //Avoid Collisions
            CollisionChecks(RootPointIndex + 1);

            if (NN.Parameters != null)
            {
                //Calculate Root
                Matrix4x4 currentRoot  = Trajectory.Points[RootPointIndex].GetTransformation();
                Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();

                //Input Trajectory Positions / Directions
                for (int i = 0; i < PointSamples; i++)
                {
                    Vector3 pos = Trajectory.Points[i * PointDensity].GetPosition().GetRelativePositionTo(currentRoot);
                    Vector3 dir = Trajectory.Points[i * PointDensity].GetDirection().GetRelativeDirectionTo(currentRoot);
                    NN.SetInput(PointSamples * 0 + i, UnitScale * pos.x);
                    NN.SetInput(PointSamples * 1 + i, UnitScale * pos.z);
                    NN.SetInput(PointSamples * 2 + i, dir.x);
                    NN.SetInput(PointSamples * 3 + i, dir.z);
                }

                //Input Trajectory Gaits
                for (int i = 0; i < PointSamples; i++)
                {
                    for (int j = 0; j < Trajectory.Points[i * PointDensity].Styles.Length; j++)
                    {
                        NN.SetInput(PointSamples * (4 + j) + i, Trajectory.Points[i * PointDensity].Styles[j]);
                    }
                    //FOR HUMAN ONLY
                    NN.SetInput(PointSamples * 8 + i, Trajectory.Points[i * PointDensity].GetSlope());
                    //
                }

                //Input Previous Bone Positions / Velocities
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    int     o   = 10 * PointSamples;
                    Vector3 pos = Positions[i].GetRelativePositionTo(previousRoot);
                    Vector3 vel = Velocities[i].GetRelativeDirectionTo(previousRoot);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 0, UnitScale * pos.x);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 1, UnitScale * pos.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 0 + i * 3 + 2, UnitScale * pos.z);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 0, UnitScale * vel.x);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 1, UnitScale * vel.y);
                    NN.SetInput(o + Actor.Bones.Length * 3 * 1 + i * 3 + 2, UnitScale * vel.z);
                }

                //Input Trajectory Heights
                for (int i = 0; i < PointSamples; i++)
                {
                    int o = 10 * PointSamples + Actor.Bones.Length * 3 * 2;
                    NN.SetInput(o + PointSamples * 0 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetRightSample().y - currentRoot.GetPosition().y));
                    NN.SetInput(o + PointSamples * 1 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetPosition().y - currentRoot.GetPosition().y));
                    NN.SetInput(o + PointSamples * 2 + i, UnitScale * (Trajectory.Points[i * PointDensity].GetLeftSample().y - currentRoot.GetPosition().y));
                }

                //Predict
                float rest = Mathf.Pow(1.0f - Trajectory.Points[RootPointIndex].Styles[0], 0.25f);
                NN.SetDamping(1f - (rest * 0.9f + 0.1f));
                NN.Predict();

                //Update Past Trajectory
                for (int i = 0; i < RootPointIndex; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                    Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                    Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                    Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                    Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                    for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                    {
                        Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                    }
                }

                //Update Current Trajectory
                Trajectory.Points[RootPointIndex].SetPosition((rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativePositionFrom(currentRoot));
                Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(rest * Mathf.Rad2Deg * (-NN.GetOutput(2)), Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
                Trajectory.Points[RootPointIndex].Postprocess();
                Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();

                //Update Future Trajectory
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + (rest * new Vector3(NN.GetOutput(0) / UnitScale, 0f, NN.GetOutput(1) / UnitScale)).GetRelativeDirectionFrom(nextRoot));
                }
                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    int   w    = RootSampleIndex;
                    float m    = Mathf.Repeat(((float)i - (float)RootPointIndex) / (float)PointDensity, 1.0f);
                    float posX = (1 - m) * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 0) + (i / PointDensity) - w + 1);
                    float posZ = (1 - m) * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 1) + (i / PointDensity) - w + 1);
                    float dirX = (1 - m) * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 2) + (i / PointDensity) - w + 1);
                    float dirZ = (1 - m) * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w) + m * NN.GetOutput(8 + (w * 3) + (i / PointDensity) - w + 1);
                    Trajectory.Points[i].SetPosition(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetPosition(),
                            new Vector3(posX / UnitScale, 0f, posZ / UnitScale).GetRelativePositionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                    Trajectory.Points[i].SetDirection(
                        Utility.Interpolate(
                            Trajectory.Points[i].GetDirection(),
                            new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                            TrajectoryCorrection
                            )
                        );
                }

                for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
                {
                    Trajectory.Points[i].Postprocess();
                }

                for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
                {
                    //ROOT	1		2		3		4		5
                    //.x....x.......x.......x.......x.......x
                    Trajectory.Point prev   = GetPreviousSample(i);
                    Trajectory.Point next   = GetNextSample(i);
                    float            factor = (float)(i % PointDensity) / PointDensity;

                    Trajectory.Points[i].SetPosition((1f - factor) * prev.GetPosition() + factor * next.GetPosition());
                    Trajectory.Points[i].SetDirection((1f - factor) * prev.GetDirection() + factor * next.GetDirection());
                    Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                    Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                    Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
                }

                //Avoid Collisions
                CollisionChecks(RootPointIndex);

                //Compute Posture
                int opos = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 0;
                int ovel = 8 + 4 * RootSampleIndex + Actor.Bones.Length * 3 * 1;
                //int orot = 8 + 4*RootSampleIndex + Actor.Bones.Length*3*2;
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    Vector3 position = new Vector3(NN.GetOutput(opos + i * 3 + 0), NN.GetOutput(opos + i * 3 + 1), NN.GetOutput(opos + i * 3 + 2)) / UnitScale;
                    Vector3 velocity = new Vector3(NN.GetOutput(ovel + i * 3 + 0), NN.GetOutput(ovel + i * 3 + 1), NN.GetOutput(ovel + i * 3 + 2)) / UnitScale;
                    //Quaternion rotation = new Quaternion(PFNN.GetOutput(orot+i*3+0), PFNN.GetOutput(orot+i*3+1), PFNN.GetOutput(orot+i*3+2), 0f).Exp();
                    Positions[i]  = Vector3.Lerp(Positions[i].GetRelativePositionTo(currentRoot) + velocity, position, 0.5f).GetRelativePositionFrom(currentRoot);
                    Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
                    //rotations[i] = rotation.GetRelativeRotationFrom(currentRoot);
                }

                //Update Posture
                transform.position = nextRoot.GetPosition();
                transform.rotation = nextRoot.GetRotation();
                for (int i = 0; i < Actor.Bones.Length; i++)
                {
                    Actor.Bones[i].Transform.position = Positions[i];
                    Actor.Bones[i].Transform.rotation = Quaternion.LookRotation(Forwards[i], Ups[i]);
                }
            }
        }
Beispiel #6
0
    void Update()
    {
        if (TrajectoryControl)
        {
            //Update Target Direction / Velocity
            TargetDirection = Vector3.Lerp(TargetDirection, Quaternion.AngleAxis(Controller.QueryTurn() * 60f, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection(), TargetBlending);
            TargetVelocity  = Vector3.Lerp(TargetVelocity, (Quaternion.LookRotation(TargetDirection, Vector3.up) * Controller.QueryMove()).normalized, TargetBlending);

            //Update Bias
            Bias = Utility.Interpolate(Bias, PoolBias(), TargetBlending);

            //Update Trajectory Correction
            TrajectoryCorrection = Utility.Interpolate(TrajectoryCorrection, Mathf.Max(Controller.QueryMove().normalized.magnitude, Mathf.Abs(Controller.QueryTurn())), TargetBlending);

            //Update Style
            for (int i = 0; i < Controller.Styles.Length; i++)
            {
                if (i == 0)
                {
                    if (!Controller.QueryAny())
                    {
                        Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], 1f, StyleTransition);
                    }
                    else
                    {
                        Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, StyleTransition);
                    }
                }
                else
                {
                    Trajectory.Points[RootPointIndex].Styles[i] = Utility.Interpolate(Trajectory.Points[RootPointIndex].Styles[i], Controller.Styles[i].Query() ? 1f : 0f, StyleTransition);
                }
            }

            //Predict Future Trajectory
            Vector3[] trajectory_positions_blend = new Vector3[Trajectory.Points.Length];
            trajectory_positions_blend[RootPointIndex] = Trajectory.Points[RootPointIndex].GetTransformation().GetPosition();
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                float bias_pos  = 0.75f;
                float bias_dir  = 1.25f;
                float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_pos));
                float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - RootPointIndex) / (RootPointIndex)), bias_dir));

                float scale = 1f / (Trajectory.Points.Length - (RootPointIndex + 1f));

                trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
                    Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
                    scale * Bias * TargetVelocity,
                    scale_pos);

                Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), TargetDirection, scale_dir));

                Trajectory.Points[i].SetVelocity(Bias * TargetVelocity.magnitude);                 //Set Desired Smoothed Root Velocities

                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[RootPointIndex].Styles[j];
                }
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
            }

            for (int i = RootPointIndex; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition(((1f - factor) * prev.GetPosition() + factor * next.GetPosition()));
                Trajectory.Points[i].SetDirection(((1f - factor) * prev.GetDirection() + factor * next.GetDirection()));
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }
        }

        if (PFNN.Parameters != null)
        {
            //Calculate Root
            Matrix4x4 currentRoot  = Trajectory.Points[RootPointIndex].GetTransformation();
            Matrix4x4 previousRoot = Trajectory.Points[RootPointIndex - 1].GetTransformation();

            //Input Trajectory Positions / Directions
            int start = 0;
            for (int i = 0; i < PointSamples; i++)
            {
                Vector3 pos = GetSample(i).GetPosition().GetRelativePositionTo(currentRoot);
                Vector3 dir = GetSample(i).GetDirection().GetRelativeDirectionTo(currentRoot);
                PFNN.SetInput(start + i * 6 + 0, pos.x);
                //PFNN.SetInput(start + i*6 + 1, pos.y);
                PFNN.SetInput(start + i * 6 + 1, 0f);
                PFNN.SetInput(start + i * 6 + 2, pos.z);
                PFNN.SetInput(start + i * 6 + 3, dir.x);
                PFNN.SetInput(start + i * 6 + 4, dir.y);
                PFNN.SetInput(start + i * 6 + 5, dir.z);
            }
            start += 6 * PointSamples;

            //Input Trajectory Heights
            for (int i = 0; i < PointSamples; i++)
            {
                //PFNN.SetInput(start + i*2 + 0, GetSample(i).GetLeftSample().y - currentRoot.GetPosition().y);
                //PFNN.SetInput(start + i*2 + 1, GetSample(i).GetRightSample().y - currentRoot.GetPosition().y);
                PFNN.SetInput(start + i * 2 + 0, 0f);
                PFNN.SetInput(start + i * 2 + 1, 0f);
            }
            start += 2 * PointSamples;

            //Input Trajectory Styles
            for (int i = 0; i < PointSamples; i++)
            {
                for (int j = 0; j < GetSample(i).Styles.Length; j++)
                {
                    PFNN.SetInput(start + (i * GetSample(i).Styles.Length) + j, GetSample(i).Styles[j]);
                }
            }
            start += Controller.Styles.Length * PointSamples;

            //Input Previous Bone Positions / Velocities
            for (int i = 0; i < Joints.Length; i++)
            {
                Vector3 pos     = Positions[i].GetRelativePositionTo(previousRoot);
                Vector3 forward = Forwards[i].GetRelativeDirectionTo(previousRoot);
                Vector3 up      = Ups[i].GetRelativeDirectionTo(previousRoot);
                Vector3 vel     = Velocities[i].GetRelativeDirectionTo(previousRoot);
                PFNN.SetInput(start + i * JointDimIn + 0, pos.x);
                PFNN.SetInput(start + i * JointDimIn + 1, pos.y);
                PFNN.SetInput(start + i * JointDimIn + 2, pos.z);
                PFNN.SetInput(start + i * JointDimIn + 3, forward.x);
                PFNN.SetInput(start + i * JointDimIn + 4, forward.y);
                PFNN.SetInput(start + i * JointDimIn + 5, forward.z);
                PFNN.SetInput(start + i * JointDimIn + 6, up.x);
                PFNN.SetInput(start + i * JointDimIn + 7, up.y);
                PFNN.SetInput(start + i * JointDimIn + 8, up.z);
                PFNN.SetInput(start + i * JointDimIn + 9, vel.x);
                PFNN.SetInput(start + i * JointDimIn + 10, vel.y);
                PFNN.SetInput(start + i * JointDimIn + 11, vel.z);
            }
            start += JointDimIn * Joints.Length;

            //Predict
            PFNN.Predict(Phase);

            //Update Past Trajectory
            for (int i = 0; i < RootPointIndex; i++)
            {
                Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
                Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
                Trajectory.Points[i].SetLeftsample(Trajectory.Points[i + 1].GetLeftSample());
                Trajectory.Points[i].SetRightSample(Trajectory.Points[i + 1].GetRightSample());
                Trajectory.Points[i].SetSlope(Trajectory.Points[i + 1].GetSlope());
                for (int j = 0; j < Trajectory.Points[i].Styles.Length; j++)
                {
                    Trajectory.Points[i].Styles[j] = Trajectory.Points[i + 1].Styles[j];
                }
            }

            //Update Current Trajectory
            int     end = 6 * 4 + JointDimOut * Joints.Length;
            Vector3 translationalVelocity = new Vector3(PFNN.GetOutput(end + 0), 0f, PFNN.GetOutput(end + 1));
            float   angularVelocity       = PFNN.GetOutput(end + 2);
            Trajectory.Points[RootPointIndex].SetPosition((translationalVelocity).GetRelativePositionFrom(currentRoot));
            Trajectory.Points[RootPointIndex].SetDirection(Quaternion.AngleAxis(angularVelocity, Vector3.up) * Trajectory.Points[RootPointIndex].GetDirection());
            Trajectory.Points[RootPointIndex].Postprocess();
            Matrix4x4 nextRoot = Trajectory.Points[RootPointIndex].GetTransformation();

            //Update Future Trajectory
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + (translationalVelocity).GetRelativeDirectionFrom(nextRoot));
            }
            start = 0;
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                //ROOT	1		2		3		4		5
                //.x....x.......x.......x.......x.......x
                int   index           = i;
                int   prevSampleIndex = GetPreviousSample(index).GetIndex() / PointDensity;
                int   nextSampleIndex = GetNextSample(index).GetIndex() / PointDensity;
                float factor          = (float)(i % PointDensity) / PointDensity;

                float prevPosX = PFNN.GetOutput(start + (prevSampleIndex - 6) * 4 + 0);
                float prevPosZ = PFNN.GetOutput(start + (prevSampleIndex - 6) * 4 + 1);
                float prevDirX = PFNN.GetOutput(start + (prevSampleIndex - 6) * 4 + 2);
                float prevDirZ = PFNN.GetOutput(start + (prevSampleIndex - 6) * 4 + 3);

                float nextPosX = PFNN.GetOutput(start + (nextSampleIndex - 6) * 4 + 0);
                float nextPosZ = PFNN.GetOutput(start + (nextSampleIndex - 6) * 4 + 1);
                float nextDirX = PFNN.GetOutput(start + (nextSampleIndex - 6) * 4 + 2);
                float nextDirZ = PFNN.GetOutput(start + (nextSampleIndex - 6) * 4 + 3);

                float posX = (1f - factor) * prevPosX + factor * nextPosX;
                float posZ = (1f - factor) * prevPosZ + factor * nextPosZ;
                float dirX = (1f - factor) * prevDirX + factor * nextDirX;
                float dirZ = (1f - factor) * prevDirZ + factor * nextDirZ;

                Trajectory.Points[i].SetPosition(
                    Utility.Interpolate(
                        Trajectory.Points[i].GetPosition(),
                        new Vector3(posX, 0f, posZ).GetRelativePositionFrom(nextRoot),
                        TrajectoryCorrection
                        )
                    );
                Trajectory.Points[i].SetDirection(
                    Utility.Interpolate(
                        Trajectory.Points[i].GetDirection(),
                        new Vector3(dirX, 0f, dirZ).normalized.GetRelativeDirectionFrom(nextRoot),
                        TrajectoryCorrection
                        )
                    );
            }
            start += 6 * 4;
            for (int i = RootPointIndex + PointDensity; i < Trajectory.Points.Length; i += PointDensity)
            {
                Trajectory.Points[i].Postprocess();
            }
            for (int i = RootPointIndex + 1; i < Trajectory.Points.Length; i++)
            {
                Trajectory.Point prev   = GetPreviousSample(i);
                Trajectory.Point next   = GetNextSample(i);
                float            factor = (float)(i % PointDensity) / PointDensity;

                Trajectory.Points[i].SetPosition(((1f - factor) * prev.GetPosition() + factor * next.GetPosition()));
                Trajectory.Points[i].SetDirection(((1f - factor) * prev.GetDirection() + factor * next.GetDirection()));
                Trajectory.Points[i].SetLeftsample((1f - factor) * prev.GetLeftSample() + factor * next.GetLeftSample());
                Trajectory.Points[i].SetRightSample((1f - factor) * prev.GetRightSample() + factor * next.GetRightSample());
                Trajectory.Points[i].SetSlope((1f - factor) * prev.GetSlope() + factor * next.GetSlope());
            }

            //Compute Posture
            for (int i = 0; i < Joints.Length; i++)
            {
                Vector3 position = new Vector3(PFNN.GetOutput(start + i * JointDimOut + 0), PFNN.GetOutput(start + i * JointDimOut + 1), PFNN.GetOutput(start + i * JointDimOut + 2));
                Vector3 forward  = new Vector3(PFNN.GetOutput(start + i * JointDimOut + 3), PFNN.GetOutput(start + i * JointDimOut + 4), PFNN.GetOutput(start + i * JointDimOut + 5)).normalized;
                Vector3 up       = new Vector3(PFNN.GetOutput(start + i * JointDimOut + 6), PFNN.GetOutput(start + i * JointDimOut + 7), PFNN.GetOutput(start + i * JointDimOut + 8)).normalized;
                Vector3 velocity = new Vector3(PFNN.GetOutput(start + i * JointDimOut + 9), PFNN.GetOutput(start + i * JointDimOut + 10), PFNN.GetOutput(start + i * JointDimOut + 11));
                if (i == 0 || i == 1)
                {
                    position.x = 0f;
                    position.z = 0f;
                    velocity.x = 0f;
                    velocity.z = 0f;
                }
                if (i == 3)
                {
                    position.x = 0f;
                    velocity.x = 0f;
                }
                Positions[i]  = Vector3.Lerp(Positions[i].GetRelativePositionTo(currentRoot) + velocity, position, 0.5f).GetRelativePositionFrom(currentRoot);
                Forwards[i]   = forward.GetRelativeDirectionFrom(currentRoot);
                Ups[i]        = up.GetRelativeDirectionFrom(currentRoot);
                Velocities[i] = velocity.GetRelativeDirectionFrom(currentRoot);
            }
            start += JointDimOut * Joints.Length;

            //Update Posture
            Root.position = nextRoot.GetPosition();
            Root.rotation = nextRoot.GetRotation();
            for (int i = 0; i < Joints.Length; i++)
            {
                Joints[i].position = Positions[i];
                Joints[i].rotation = Quaternion.LookRotation(Forwards[i], Ups[i]);
            }

            /*
             * //Motion Editing
             * for(int i=0; i<IKSolvers.Length; i++) {
             *      IKSolvers[i].UpdateGoal();
             * }
             *
             * Transform spine = Array.Find(Joints, x => x.name == "Spine1");
             * Transform neck = Array.Find(Joints, x => x.name == "Neck");
             * Transform leftShoulder = Array.Find(Joints, x => x.name == "LeftShoulder");
             * Transform rightShoulder = Array.Find(Joints, x => x.name == "RightShoulder");
             * Vector3 spinePosition = spine.position;
             * Vector3 neckPosition = neck.position;
             * Vector3 leftShoulderPosition = leftShoulder.position;
             * Vector3 rightShoulderPosition = rightShoulder.position;
             * float spineHeight = Utility.GetHeight(spine.position, LayerMask.GetMask("Ground"));
             * float neckHeight = Utility.GetHeight(neck.position, LayerMask.GetMask("Ground"));
             * float leftShoulderHeight = Utility.GetHeight(leftShoulder.position, LayerMask.GetMask("Ground"));
             * float rightShoulderHeight = Utility.GetHeight(rightShoulder.position, LayerMask.GetMask("Ground"));
             *
             * spine.rotation = Quaternion.Slerp(spine.rotation, Quaternion.FromToRotation(neckPosition - spinePosition, new Vector3(neckPosition.x, neckHeight + (neckPosition.y - Root.position.y), neckPosition.z) - spinePosition) * spine.rotation, 0.5f);
             *
             * spine.position = new Vector3(spinePosition.x, spineHeight + (spinePosition.y - Root.position.y), spinePosition.z);
             * neck.position = new Vector3(neckPosition.x, neckHeight + (neckPosition.y - Root.position.y), neckPosition.z);
             * leftShoulder.position = new Vector3(leftShoulderPosition.x, leftShoulderHeight + (leftShoulderPosition.y - Root.position.y), leftShoulderPosition.z);
             * rightShoulder.position = new Vector3(rightShoulderPosition.x, rightShoulderHeight + (rightShoulderPosition.y - Root.position.y), rightShoulderPosition.z);
             *
             * for(int i=0; i<IKSolvers.Length; i++) {
             *      IKSolvers[i].ProcessIK();
             * }
             */

            //Update Skeleton
            Character.FetchTransformations(Root);

            //Update Phase
            Phase = Mathf.Repeat(Phase + PFNN.GetOutput(end + 3) * 2f * Mathf.PI, 2f * Mathf.PI);
        }
    }