void LimbIKMoveToTargrtPoint(MonsterPhysicalLeg leg, Vector3 now_target_point, Vector3 now_root_point, Vector3 now_pole_point)
{
ConfigurableJoint joint_u = leg.joints[2];
ConfigurableJoint joint_l = leg.joints[1];
float upper_leg_length = leg.link_length[1];
float lower_leg_length = leg.link_length[0];
// Get base height of upper joint for character to maintain
Vector3 joint_pos_u = joint_u.connectedBody.transform.TransformPoint(joint_u.connectedAnchor);
joint_pos_u = Vector3.MoveTowards(joint_pos_u, now_root_point, keep_root_up_speed * Time.fixedDeltaTime);
Vector3 ik_plane_normal = Vector3.Cross(now_target_point - joint_pos_u, now_pole_point - joint_pos_u);
Vector3 target_c = now_target_point - joint_pos_u;
float max_leg_extend_length = upper_leg_length + lower_leg_length;
if (target_c.magnitude > max_leg_extend_length)
{
target_c = target_c.normalized * max_leg_extend_length;
}
float angle_u = LawOfCosines(upper_leg_length, target_c.magnitude, lower_leg_length);
Vector3 target_a = Quaternion.AngleAxis(angle_u, ik_plane_normal) * target_c.normalized * upper_leg_length;
Vector3 joint_pos_l = joint_pos_u + target_a;
Vector3 target_b = now_target_point - joint_pos_l;
SetJointPDTarget(joint_u, target_a.normalized, ik_plane_normal, joint_pos_u, leg.joints_axis[1]);
SetJointPDTarget(joint_l, target_b.normalized, ik_plane_normal, joint_pos_l, leg.joints_axis[0]);
// Debug
Vector3 ik_plane_center = (joint_pos_l + joint_pos_u + now_target_point) / 3f;
draw_lines.Add(new Tuple <Vector3, Vector3>(ik_plane_center, ik_plane_center + ik_plane_normal));
lines_colors.Add(Color.white);
draw_triangles.Add(joint_pos_u);
draw_triangles.Add(joint_pos_u + target_a);
draw_triangles.Add(now_target_point);
}
// Start is called before the first frame update
void Start()
{
root_rg = GetComponent <Rigidbody>();
if (transform.parent != null)
{
all_rgs = transform.parent.GetComponentsInChildren <Rigidbody>().ToList();
}
else
{
all_rgs = transform.GetComponentsInChildren <Rigidbody>().ToList();
}
center_of_all_mass = new GameObject("CoM").transform;
CalculateCenterOfMass();
// Get all legs joints
legs = new MonsterPhysicalLeg[feets.Length];
for (int i = 0; i < feets.Length; i++)
{
MonsterPhysicalLeg new_leg = new MonsterPhysicalLeg();
ConfigurableJoint leg_joint = feets[i].GetComponent <ConfigurableJoint>();
while (leg_joint != null)
{
new_leg.joints.Add(leg_joint);
leg_joint = leg_joint.connectedBody.GetComponent <ConfigurableJoint>();
}
// Init all legs parameters
new_leg.InitParameters(center_of_all_mass, poles_world_position[i]);
legs[i] = new_leg;
}
body_rg = GameObject.Find("Body").GetComponent <Rigidbody>();
start_feet_bias = new Vector3[feets.Length];
now_gait_duration = new float[feets.Length];
now_gait_duration_start = new float[feets.Length];
now_gait_duration_scale = new float[feets.Length];
}
// Start is called before the first frame update
void Start()
{
root_rg = GetComponent <Rigidbody>();
if (transform.parent != null)
{
all_rgs = transform.parent.GetComponentsInChildren <Rigidbody>().ToList();
}
else
{
all_rgs = transform.GetComponentsInChildren <Rigidbody>().ToList();
}
center_of_all_mass = new GameObject("CoM").transform;
CalculateCenterOfMass();
// Get all legs joints
legs = new MonsterPhysicalLeg[feets.Length];
foreach (Transform feet in feets)
{
for (int i = 0; i < feets.Length; i++)
{
MonsterPhysicalLeg new_leg = new MonsterPhysicalLeg();
ConfigurableJoint leg_joint = feets[i].GetComponent <ConfigurableJoint>();
while (leg_joint != null)
{
new_leg.joints.Add(leg_joint);
leg_joint = leg_joint.connectedBody.GetComponent <ConfigurableJoint>();
}
// Init all legs parameters
new_leg.InitParameters(center_of_all_mass, poles_world_position[i]);
legs[i] = new_leg;
}
}
constant_garound_height = legs[0].GetNowJointPosition(0).y;
body_rg = GameObject.Find("Body").GetComponent <Rigidbody>();
}
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);
}
