private void OnNewStep(Footstep step)
{
lock (_footLock)
{
_footsteps.Enqueue(step);
}
}
private void AddFootstep(List <FootData> collection, LolaComms.Footstep step)
{
var stepobj = Instantiate(Footprint, transform);
stepobj.transform.localPosition = new Vector3(step.start_x, step.start_y, step.start_z);
var foot = (LolaComms.Foot)step.stance;
switch (foot)
{
case LolaComms.Foot.Left:
{
stepobj.transform.localRotation = Quaternion.AngleAxis(Mathf.Rad2Deg * left_foot_angle, new Vector3(0, 0, 1)) * stepobj.transform.localRotation;
stepobj.GetComponentInChildren <MeshRenderer>().material = foot_left;
}
break;
case LolaComms.Foot.Right:
{
stepobj.transform.localRotation = Quaternion.AngleAxis(Mathf.Rad2Deg * right_foot_angle, new Vector3(0, 0, 1)) * stepobj.transform.localRotation;
stepobj.GetComponentInChildren <MeshRenderer>().material = foot_right;
}
break;
default:
{
Debug.LogError("Unknown foot stance: " + step.stance);
}
break;
}
if (drawTrajectories)
{
// generate path between current and previous step
if (collection.Count > 0)
{
Debug.Log("Plotting path between current step and prev (" + collection.Count + ")");
var line = collection[collection.Count - 1].go.GetComponent <LineRenderer>();
if (line == null)
{
Debug.LogWarning("LineRenderer was null!");
return;
}
var start_pos = Vector3.zero;
var delta = stepobj.transform.localPosition - collection[collection.Count - 1].go.transform.localPosition;
var end_pos = start_pos + delta;
Debug.Log("Start: " + start_pos + ", End: " + end_pos);
var mid_pos = start_pos + new Vector3(delta.x / 2, step.dy, step.dz_clear);
line.positionCount = 8;
for (int i = 0; i < line.positionCount / 2; i++)
{
line.SetPosition(i, Hermite(start_pos, mid_pos, (float)i / (float)(line.positionCount / 2)));
}
for (int i = line.positionCount / 2; i < line.positionCount; i++)
{
line.SetPosition(i, Hermite(mid_pos, end_pos, (float)(i - line.positionCount / 2) / (float)(line.positionCount / 2)));
}
line.positionCount = line.positionCount + 1;
line.SetPosition(line.positionCount - 1, end_pos);
}
else if (foot == LolaComms.Foot.Left && footsteps_L_real.Count > 0)
{
Debug.Log("Plotting path between current step and prev (" + footsteps_L_real.Count + ")");
var line = footsteps_L_real[footsteps_L_real.Count - 1].go.GetComponent <LineRenderer>();
if (line == null)
{
Debug.LogWarning("LineRenderer was null!");
return;
}
var start_pos = Vector3.zero;
var delta = stepobj.transform.localPosition - footsteps_L_real[footsteps_L_real.Count - 1].go.transform.localPosition;
var end_pos = start_pos + delta;
Debug.Log("Start: " + start_pos + ", End: " + end_pos);
var mid_pos = start_pos + new Vector3(delta.x / 2, step.dy, step.dz_clear);
line.positionCount = 8;
for (int i = 0; i < line.positionCount / 2; i++)
{
line.SetPosition(i, Hermite(start_pos, mid_pos, (float)i / (float)(line.positionCount / 2)));
}
for (int i = line.positionCount / 2; i < line.positionCount; i++)
{
line.SetPosition(i, Hermite(mid_pos, end_pos, (float)(i - line.positionCount / 2) / (float)(line.positionCount / 2)));
}
line.positionCount = line.positionCount + 1;
line.SetPosition(line.positionCount - 1, end_pos);
}
else if (foot == LolaComms.Foot.Right && footsteps_R_real.Count > 0)
{
Debug.Log("Plotting path between current step and prev (" + footsteps_R_real.Count + ")");
var line = footsteps_R_real[footsteps_R_real.Count - 1].go.GetComponent <LineRenderer>();
if (line == null)
{
Debug.LogWarning("LineRenderer was null!");
return;
}
var start_pos = Vector3.zero;
var delta = stepobj.transform.localPosition - footsteps_R_real[footsteps_R_real.Count - 1].go.transform.localPosition;
var end_pos = start_pos + delta;
Debug.Log("Start: " + start_pos + ", End: " + end_pos);
var mid_pos = start_pos + new Vector3(delta.x / 2, step.dy, step.dz_clear);
line.positionCount = 8;
for (int i = 0; i < line.positionCount / 2; i++)
{
line.SetPosition(i, Hermite(start_pos, mid_pos, (float)i / (float)(line.positionCount / 2)));
}
for (int i = line.positionCount / 2; i < line.positionCount; i++)
{
line.SetPosition(i, Hermite(mid_pos, end_pos, (float)(i - line.positionCount / 2) / (float)(line.positionCount / 2)));
}
line.positionCount = line.positionCount + 1;
line.SetPosition(line.positionCount - 1, end_pos);
}
else
{
Debug.Log("No previous footsteps to plot a line from...");
}
}
collection.Add(new FootData(stepobj, step));
}
public FootData(GameObject g, LolaComms.Footstep s)
{
go = g;
step = s;
}
