Vector3 PlanNowTargetPoint(MonsterPhysicalLeg leg, Vector3 target_feet_point)
{
//
Vector3 start_feet_point = center_of_all_mass.TransformPoint(start_feet_bias);
Vector3 feet_move_direction = target_feet_point - start_feet_point;
feet_move_direction = Vector3.ProjectOnPlane(feet_move_direction, Vector3.up);
float angle_sign = Vector3.Dot(feet_move_direction, Vector3.ProjectOnPlane(root_rg.transform.forward, Vector3.up)) > 0f ? 1f : -1f;
float target_add_length = Mathf.Lerp(0f, max_feet_over_length, Mathf.InverseLerp(0f, angle_sign * 90f, GetRootAngle()));
feet_move_direction += feet_move_direction.normalized * target_add_length;
Vector3 now_feet_move_length = feet_move_direction.normalized * feet_move_speed * Time.fixedDeltaTime;
Vector3 now_feet_position = leg.GetNowJointPosition(0);
Vector3 feet_already_move_length = Vector3.Project(now_feet_position - start_feet_point, feet_move_direction);
if (Vector3.Dot(feet_already_move_length, feet_move_direction) < 0f)
{
feet_already_move_length = Vector3.zero;
}
if (now_gait_duration == 0f)
{
while (now_gait_duration < 0.5f && Mathf.Lerp(constant_garound_height, constant_garound_height + gait_max_height, gait_curve.Evaluate(now_gait_duration)) < start_feet_point.y)
{
now_gait_duration += 0.01f;
}
now_gait_duration_start = now_gait_duration;
now_gait_duration_scale = 1f - now_gait_duration_start;
}
float new_gait_duration = Mathf.Max(now_gait_duration_start + Mathf.InverseLerp(0f, feet_move_direction.magnitude, feet_already_move_length.magnitude) * now_gait_duration_scale,
now_gait_duration + Mathf.InverseLerp(0f, feet_move_direction.magnitude, now_feet_move_length.magnitude));
if (new_gait_duration >= 1f)
{
now_feet_move_length *= (1f - now_gait_duration) / (new_gait_duration - now_gait_duration);
now_gait_duration = 0f;
is_feet_moveing = false;
backing_leg_index = moveing_leg_index;
}
else
{
now_gait_duration = new_gait_duration;
}
Vector3 now_target_point = start_feet_point + feet_move_direction * now_gait_duration + now_feet_move_length;
now_target_point.y = Mathf.Lerp(constant_garound_height, Mathf.Max(constant_garound_height + gait_max_height, now_feet_position.y), gait_curve.Evaluate(now_gait_duration));
// Debug
if (now_gait_duration > 0f)
{
for (float t = now_gait_duration; t <= 1f; t += 0.1f)
{
Vector3 v = start_feet_point + feet_move_direction * t;
v.y = Mathf.Lerp(constant_garound_height, Mathf.Max(constant_garound_height + gait_max_height, now_feet_position.y), gait_curve.Evaluate(t));
draw_trajectory.Add(v);
}
}
return(now_target_point);
}
Vector3 PlanNowTargetPoint(MonsterPhysicalLeg leg, Vector3 target_feet_point, int feet_index)
{
//
target_feet_point += now_root_velocity * over_feet_scale;
Vector3 start_feet_point = center_of_all_mass.TransformPoint(start_feet_bias[feet_index]);
Vector3 feet_move_direction = target_feet_point - start_feet_point;
feet_move_direction = Vector3.ProjectOnPlane(feet_move_direction, Vector3.up);
Vector3 now_feet_move_length = feet_move_direction.normalized * feet_move_speed * Time.fixedDeltaTime;
Vector3 now_feet_position = leg.GetNowJointPosition(0);
Vector3 feet_already_move_length = Vector3.Project(now_feet_position - start_feet_point, feet_move_direction);
if (Vector3.Dot(feet_already_move_length, feet_move_direction) < 0f)
{
feet_already_move_length = Vector3.zero;
}
if (now_gait_duration[feet_index] == 0f)
{
while (now_gait_duration[feet_index] < 0.5f && Mathf.Lerp(feets[feet_index].garound_height, feets[feet_index].garound_height + gait_max_height, gait_curve.Evaluate(now_gait_duration[feet_index])) < start_feet_point.y)
{
now_gait_duration[feet_index] += 0.01f;
}
now_gait_duration_start[feet_index] = now_gait_duration[feet_index];
now_gait_duration_scale[feet_index] = 1f - now_gait_duration_start[feet_index];
}
float new_gait_duration = Mathf.Max(now_gait_duration_start[feet_index] + Mathf.InverseLerp(0f, feet_move_direction.magnitude, feet_already_move_length.magnitude) * now_gait_duration_scale[feet_index],
now_gait_duration[feet_index] + Mathf.InverseLerp(0f, feet_move_direction.magnitude, now_feet_move_length.magnitude));
if (new_gait_duration >= 1f)
{
now_feet_move_length *= (1f - now_gait_duration[feet_index]) / (new_gait_duration - now_gait_duration[feet_index]);
now_gait_duration[feet_index] = 0f;
moveing_leg_indexes.Remove(feet_index);
if (!backing_leg_indexes.Contains(feet_index))
{
backing_leg_indexes.Add(feet_index);
if (backing_leg_indexes.Count > max_feet_moveing_num)
{
backing_leg_indexes.RemoveAt(0);
}
}
}
else
{
now_gait_duration[feet_index] = new_gait_duration;
}
Vector3 now_target_point = start_feet_point + feet_move_direction * now_gait_duration[feet_index] + now_feet_move_length;
now_target_point.y = Mathf.Lerp(feets[feet_index].garound_height, Mathf.Max(feets[feet_index].garound_height + gait_max_height, now_feet_position.y), gait_curve.Evaluate(now_gait_duration[feet_index]));
// Debug
if (now_gait_duration[feet_index] > 0f)
{
var trajectory = new List <Vector3>();
for (float t = now_gait_duration[feet_index]; t <= 1f; t += 0.1f)
{
Vector3 v = start_feet_point + feet_move_direction * t;
v.y = Mathf.Lerp(feets[feet_index].garound_height, Mathf.Max(feets[feet_index].garound_height + gait_max_height, now_feet_position.y), gait_curve.Evaluate(t));
trajectory.Add(v);
}
draw_trajectorys.Add(trajectory);
}
return(now_target_point);
}