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]);
}
}
}
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);
}
//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]);
}
}
}
void Update()
{
//Update Target Direction / Velocity
Trajectory.UpdateTarget(Controller.QueryMove(), Controller.QueryTurn());
//TODO: Update strafe etc.
//Update Gait
Trajectory.GetRoot().Stand = Utility.Interpolate(Trajectory.GetRoot().Stand, 1f - Mathf.Clamp(Trajectory.TargetVelocity.magnitude, 0f, 1f), Trajectory.GaitSmoothing);
Trajectory.GetRoot().Walk = Utility.Interpolate(Trajectory.GetRoot().Walk, 1f - Controller.QueryJog(), Trajectory.GaitSmoothing);
Trajectory.GetRoot().Jog = Utility.Interpolate(Trajectory.GetRoot().Jog, Controller.QueryJog(), Trajectory.GaitSmoothing);
Trajectory.GetRoot().Crouch = Utility.Interpolate(Trajectory.GetRoot().Crouch, Controller.QueryCrouch(), Trajectory.GaitSmoothing);
//Trajectory.GetRoot().Jump = Utility.Interpolate(Trajectory.GetRoot().Jump, Controller.QueryJump(), Trajectory.GaitSmoothing);
Trajectory.GetRoot().Bump = Utility.Interpolate(Trajectory.GetRoot().Bump, 0f, Trajectory.GaitSmoothing);
/*
* if(Vector3.Magnitude(Trajectory.TargetVelocity) < 0.25f) {
* //float standAmount = 1.0f - Mathf.Clamp(Vector3.Magnitude(Trajectory.TargetVelocity) / 0.1f, 0.0f, 1.0f);
* Trajectory.GetRoot().Stand = Utility.Interpolate(Trajectory.GetRoot().Stand, standAmount, Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Walk = Utility.Interpolate(Trajectory.GetRoot().Walk, 0f, Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Jog = Utility.Interpolate(Trajectory.GetRoot().Jog, 0f, Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Crouch = Utility.Interpolate(Trajectory.GetRoot().Crouch, Controller.QueryCrouch(), Trajectory.GaitSmoothing);
* //Trajectory.GetRoot().Jump = Utility.Interpolate(Trajectory.GetRoot().Jump, Controller.QueryJump(), Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Bump = Utility.Interpolate(Trajectory.GetRoot().Bump, 0f, Trajectory.GaitSmoothing);
* } else {
* float standAmount = 1.0f - Mathf.Clamp(Vector3.Magnitude(Trajectory.TargetVelocity) / 0.1f, 0.0f, 1.0f);
* Trajectory.GetRoot().Stand = Utility.Interpolate(Trajectory.GetRoot().Stand, standAmount, Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Walk = Utility.Interpolate(Trajectory.GetRoot().Walk, 1f-Controller.QueryJog(), Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Jog = Utility.Interpolate(Trajectory.GetRoot().Jog, Controller.QueryJog(), Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Crouch = Utility.Interpolate(Trajectory.GetRoot().Crouch, Controller.QueryCrouch(), Trajectory.GaitSmoothing);
* //Trajectory.GetRoot().Jump = Utility.Interpolate(Trajectory.GetRoot().Jump, Controller.QueryJump(), Trajectory.GaitSmoothing);
* Trajectory.GetRoot().Bump = Utility.Interpolate(Trajectory.GetRoot().Bump, 0f, Trajectory.GaitSmoothing);
* }
*/
//TODO: Update gait for jog, crouch, ...
//Blend Trajectory Offset
Vector3 positionOffset = transform.position - Trajectory.GetRoot().GetPosition();
Quaternion rotationOffset = Quaternion.Inverse(Trajectory.GetRoot().GetRotation()) * transform.rotation;
Trajectory.GetRoot().SetPosition(Trajectory.GetRoot().GetPosition() + positionOffset, false);
Trajectory.GetRoot().SetDirection(rotationOffset * Trajectory.GetRoot().GetDirection());
/*
* for(int i=Trajectory.GetRootPointIndex(); i<Trajectory.GetPointCount(); i++) {
* float factor = 1f - (i - Trajectory.GetRootPointIndex())/(Trajectory.GetRootPointIndex() - 1f);
* Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + factor*positionOffset, false);
* }
*/
//Predict Future Trajectory
Vector3[] trajectory_positions_blend = new Vector3[Trajectory.GetPointCount()];
trajectory_positions_blend[Trajectory.GetRootPointIndex()] = Trajectory.GetRoot().GetPosition();
for (int i = Trajectory.GetRootPointIndex() + 1; i < Trajectory.GetPointCount(); i++)
{
float bias_pos = 0.75f;
float bias_dir = 1.25f;
float scale_pos = (1.0f - Mathf.Pow(1.0f - ((float)(i - Trajectory.GetRootPointIndex()) / (Trajectory.GetRootPointIndex())), bias_pos));
float scale_dir = (1.0f - Mathf.Pow(1.0f - ((float)(i - Trajectory.GetRootPointIndex()) / (Trajectory.GetRootPointIndex())), bias_dir));
float vel_boost = 1f;
float rescale = 1f / (Trajectory.GetPointCount() - (Trajectory.GetRootPointIndex() + 1f));
trajectory_positions_blend[i] = trajectory_positions_blend[i - 1] + Vector3.Lerp(
Trajectory.Points[i].GetPosition() - Trajectory.Points[i - 1].GetPosition(),
vel_boost * rescale * Trajectory.TargetVelocity,
scale_pos);
Trajectory.Points[i].SetDirection(Vector3.Lerp(Trajectory.Points[i].GetDirection(), Trajectory.TargetDirection, scale_dir));
Trajectory.Points[i].Stand = Trajectory.GetRoot().Stand;
Trajectory.Points[i].Walk = Trajectory.GetRoot().Walk;
Trajectory.Points[i].Jog = Trajectory.GetRoot().Jog;
Trajectory.Points[i].Crouch = Trajectory.GetRoot().Crouch;
//Trajectory.Points[i].Jump = Trajectory.GetRoot().Jump;
Trajectory.Points[i].Bump = Trajectory.GetRoot().Bump;
}
for (int i = Trajectory.GetRootPointIndex() + 1; i < Trajectory.GetPointCount(); i++)
{
Trajectory.Points[i].SetPosition(trajectory_positions_blend[i]);
}
//Post-Correct Trajectory
CollisionChecks(Trajectory.GetRootPointIndex() + 1);
//Calculate Current and Previous Root
Transformation prevRoot = new Transformation(Trajectory.GetPrevious().GetPosition(), Trajectory.GetPrevious().GetRotation());
Transformation currRoot = new Transformation(Trajectory.GetRoot().GetPosition(), Trajectory.GetRoot().GetRotation());
if (PFNN.Parameters != null)
{
//Input Trajectory Positions / Directions
for (int i = 0; i < Trajectory.GetSampleCount(); i++)
{
Vector3 pos = Trajectory.GetSample(i).GetPosition(currRoot);
Vector3 dir = Trajectory.GetSample(i).GetDirection(currRoot);
PFNN.SetInput(Trajectory.GetSampleCount() * 0 + i, UnitScale * pos.x);
PFNN.SetInput(Trajectory.GetSampleCount() * 1 + i, UnitScale * pos.z);
PFNN.SetInput(Trajectory.GetSampleCount() * 2 + i, dir.x);
PFNN.SetInput(Trajectory.GetSampleCount() * 3 + i, dir.z);
}
//Input Trajectory Gaits
for (int i = 0; i < Trajectory.GetSampleCount(); i++)
{
PFNN.SetInput(Trajectory.GetSampleCount() * 4 + i, Trajectory.GetSample(i).Stand);
PFNN.SetInput(Trajectory.GetSampleCount() * 5 + i, Trajectory.GetSample(i).Walk);
PFNN.SetInput(Trajectory.GetSampleCount() * 6 + i, Trajectory.GetSample(i).Jog);
PFNN.SetInput(Trajectory.GetSampleCount() * 7 + i, Trajectory.GetSample(i).Crouch);
PFNN.SetInput(Trajectory.GetSampleCount() * 8 + i, Trajectory.GetSample(i).Jump);
PFNN.SetInput(Trajectory.GetSampleCount() * 9 + i, Trajectory.GetSample(i).Bump);
}
//Input Joint Previous Positions / Velocities / Rotations
for (int i = 0; i < Character.Joints.Length; i++)
{
int o = 10 * Trajectory.GetSampleCount();
Vector3 pos = Character.Joints[i].GetPosition(prevRoot);
Vector3 vel = Character.Joints[i].GetVelocity(prevRoot);
PFNN.SetInput(o + Character.Joints.Length * 3 * 0 + i * 3 + 0, UnitScale * pos.x);
PFNN.SetInput(o + Character.Joints.Length * 3 * 0 + i * 3 + 1, UnitScale * pos.y);
PFNN.SetInput(o + Character.Joints.Length * 3 * 0 + i * 3 + 2, UnitScale * pos.z);
PFNN.SetInput(o + Character.Joints.Length * 3 * 1 + i * 3 + 0, UnitScale * vel.x);
PFNN.SetInput(o + Character.Joints.Length * 3 * 1 + i * 3 + 1, UnitScale * vel.y);
PFNN.SetInput(o + Character.Joints.Length * 3 * 1 + i * 3 + 2, UnitScale * vel.z);
}
//Input Trajectory Heights
for (int i = 0; i < Trajectory.GetSampleCount(); i++)
{
int o = 10 * Trajectory.GetSampleCount() + Character.Joints.Length * 3 * 2;
PFNN.SetInput(o + Trajectory.GetSampleCount() * 0 + i, UnitScale * (Trajectory.GetSample(i).Project(Trajectory.Width / 2f).y - currRoot.Position.y));
PFNN.SetInput(o + Trajectory.GetSampleCount() * 1 + i, UnitScale * (Trajectory.GetSample(i).GetHeight() - currRoot.Position.y));
PFNN.SetInput(o + Trajectory.GetSampleCount() * 2 + i, UnitScale * (Trajectory.GetSample(i).Project(-Trajectory.Width / 2f).y - currRoot.Position.y));
}
//Predict
PFNN.Predict(Character.Phase);
}
//Update Past Trajectory
for (int i = 0; i < Trajectory.GetRootPointIndex(); i++)
{
Trajectory.Points[i].SetPosition(Trajectory.Points[i + 1].GetPosition());
Trajectory.Points[i].SetDirection(Trajectory.Points[i + 1].GetDirection());
Trajectory.Points[i].Stand = Trajectory.Points[i + 1].Stand;
Trajectory.Points[i].Walk = Trajectory.Points[i + 1].Walk;
Trajectory.Points[i].Jog = Trajectory.Points[i + 1].Jog;
Trajectory.Points[i].Crouch = Trajectory.Points[i + 1].Crouch;
//Trajectory.Points[i].Jump = Trajectory.Points[i+1].Jump;
Trajectory.Points[i].Bump = Trajectory.Points[i + 1].Bump;
}
if (PFNN.Parameters != null)
{
//Update Current Trajectory
float stand_amount = Mathf.Pow(1.0f - Trajectory.GetRoot().Stand, 0.25f);
Trajectory.GetRoot().SetPosition(Trajectory.GetRoot().GetPosition() + stand_amount * (Trajectory.GetRoot().GetRotation() * new Vector3(PFNN.GetOutput(0) / UnitScale, 0f, PFNN.GetOutput(1) / UnitScale)));
Trajectory.GetRoot().SetDirection(Quaternion.AngleAxis(stand_amount * Mathf.Rad2Deg * (-PFNN.GetOutput(2)), Vector3.up) * Trajectory.GetRoot().GetDirection());
for (int i = Trajectory.GetRootPointIndex() + 1; i < Trajectory.GetPointCount(); i++)
{
Trajectory.Points[i].SetPosition(Trajectory.Points[i].GetPosition() + stand_amount * (Trajectory.GetRoot().GetRotation() * new Vector3(PFNN.GetOutput(0) / UnitScale, 0f, PFNN.GetOutput(1) / UnitScale)));
}
//Update Future Trajectory
Transformation reference = new Transformation(Trajectory.GetRoot().GetPosition(), Trajectory.GetRoot().GetRotation());
for (int i = Trajectory.GetRootPointIndex() + 1; i < Trajectory.GetPointCount(); i++)
{
int w = Trajectory.GetRootSampleIndex();
float m = Mathf.Repeat(((float)i - (float)Trajectory.GetRootPointIndex()) / (float)Trajectory.GetDensity(), 1.0f);
float posX = (1 - m) * PFNN.GetOutput(8 + (w * 0) + (i / Trajectory.GetDensity()) - w) + m * PFNN.GetOutput(8 + (w * 0) + (i / Trajectory.GetDensity()) - w + 1);
float posZ = (1 - m) * PFNN.GetOutput(8 + (w * 1) + (i / Trajectory.GetDensity()) - w) + m * PFNN.GetOutput(8 + (w * 1) + (i / Trajectory.GetDensity()) - w + 1);
float dirX = (1 - m) * PFNN.GetOutput(8 + (w * 2) + (i / Trajectory.GetDensity()) - w) + m * PFNN.GetOutput(8 + (w * 2) + (i / Trajectory.GetDensity()) - w + 1);
float dirZ = (1 - m) * PFNN.GetOutput(8 + (w * 3) + (i / Trajectory.GetDensity()) - w) + m * PFNN.GetOutput(8 + (w * 3) + (i / Trajectory.GetDensity()) - w + 1);
Trajectory.Points[i].SetPosition(
Utility.Interpolate(
Trajectory.Points[i].GetPosition(),
reference.Position + reference.Rotation * new Vector3(posX / UnitScale, 0f, posZ / UnitScale),
1f - Trajectory.CorrectionSmoothing
)
);
Trajectory.Points[i].SetDirection(
Utility.Interpolate(
Trajectory.Points[i].GetDirection(),
reference.Rotation * new Vector3(dirX, 0f, dirZ).normalized,
1f - Trajectory.CorrectionSmoothing
)
);
}
//Post-Correct Trajectory
CollisionChecks(Trajectory.GetRootPointIndex());
//Update Root Position
transform.position = Trajectory.GetRoot().GetPosition();
transform.rotation = Trajectory.GetRoot().GetRotation();
//Transformation root = new Transformation(transform.position, transform.rotation);
//Update Joint Positions and Velocities
int opos = 8 + 4 * Trajectory.GetRootSampleIndex() + Character.Joints.Length * 3 * 0;
int ovel = 8 + 4 * Trajectory.GetRootSampleIndex() + Character.Joints.Length * 3 * 1;
for (int i = 0; i < Character.Joints.Length; i++)
{
Vector3 position = new Vector3(PFNN.GetOutput(opos + i * 3 + 0), PFNN.GetOutput(opos + i * 3 + 1), PFNN.GetOutput(opos + i * 3 + 2)) / UnitScale;
Vector3 velocity = new Vector3(PFNN.GetOutput(ovel + i * 3 + 0), PFNN.GetOutput(ovel + i * 3 + 1), PFNN.GetOutput(ovel + i * 3 + 2)) / UnitScale;
//Optional: Smooth motion out a bit.
position = Vector3.Lerp(Character.Joints[i].GetPosition(currRoot) + velocity, position, 1f);
Character.Joints[i].SetPosition(position, currRoot);
Character.Joints[i].SetVelocity(velocity, currRoot);
}
/* Update Phase */
Character.Phase = Mathf.Repeat(Character.Phase + (stand_amount * 0.9f + 0.1f) * PFNN.GetOutput(3) * 2f * Mathf.PI, 2f * Mathf.PI);
}
//PFNN.Finish();
}