Beispiel #1
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]);
                }
            }
        }