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); } } }