void Update() { Target.TrajectoryCorrection = Correction; Target.TrajectoryControl = false; Trajectory.Point pivot = Trajectory.Points[Current]; //GetClosestTrajectoryPoint(Target.transform.position); Trajectory.Point[] future = GetFutureTrajectory(pivot); //Target.SetTargetDirection((future[future.Length-1].GetPosition() - Target.transform.position).normalized); //Target.SetTargetVelocity(future[future.Length-1].GetPosition() - pivot.GetPosition()); Target.SetTargetDirection(Vector3.zero); Target.SetTargetVelocity(Vector3.zero); Trajectory targetTrajectory = Target.GetTrajectory(); for (int i = 0; i < future.Length; i++) { float weight = (float)(i + 1) / (float)future.Length; Trajectory.Point point = targetTrajectory.Points[60 + i + 1]; point.SetPosition((1f - weight) * Target.transform.position + weight * future[i].GetPosition()); //point.SetDirection(future[i].GetDirection()); point.SetDirection((future[i].GetDirection() + (future[i].GetPosition() - point.GetPosition()).normalized).normalized); //point.SetDirection((future[i].GetDirection() + (future[future.Length-1].GetPosition() - point.GetPosition()).normalized).normalized); point.SetVelocity(Vector3.Distance(pivot.GetPosition(), future[future.Length - 1].GetPosition())); } for (int i = 60; i < targetTrajectory.Points.Length; i++) { targetTrajectory.Points[i].Styles[0] = 0f; targetTrajectory.Points[i].Styles[1] = 1f; } Current += 1; if (Current == Trajectory.Points.Length) { Current = 0; } /* * for(int i=0; i<Feet.Length; i++) { * float heightThreshold = i==0 || i==1 ? 0.025f : 0.05f; * float velocityThreshold = i==0 || i==1 ? 0.015f : 0.015f; * Vector3 oldPosition = LastFeetPositions[i]; * Vector3 newPosition = Target.Joints[Feet[i]].position; * float velocityWeight = Utility.Exponential01((newPosition-oldPosition).magnitude / velocityThreshold); * float heightWeight = Utility.Exponential01(newPosition.y / heightThreshold); * float weight = 1f - Mathf.Min(velocityWeight, heightWeight); * Vector3 slide = newPosition - oldPosition; * slide.y = 0f; * Slidings.Add(weight * slide.magnitude * 60f); * LastFeetPositions[i] = newPosition; * } * * Positions.Add(Target.transform.position); * Vector3 p = pivot.GetPosition() - Target.transform.position; * p.y = 0f; * OffsetErrors.Add(p.magnitude); * AngleErrors.Add(Mathf.Abs(Vector3.SignedAngle(pivot.GetDirection(), targetTrajectory.Points[60].GetDirection(), Vector3.up))); */ }