void drawStepCycles(float p_phi, float p_yOffset, LegFrame p_frame, int legFrameId)
{
for (int i = 0; i < LegFrame.c_legCount; i++)
{
StepCycle cycle = p_frame.m_tuneStepCycles[i];
if (cycle != null)
{
// DRAW!
float timelineLen = 300.0f;
float xpad = 10.0f;
float offset = cycle.m_tuneStepTrigger;
float len = cycle.m_tuneDutyFactor;
float lineStart = xpad;
float lineEnd = lineStart + timelineLen;
float dutyEnd = lineStart + timelineLen * (offset + len);
float w = 4.0f;
float y = p_yOffset + (float)i * w * 2.0f;
bool stance = cycle.isInStance(p_phi);
// Draw back
Color ucol = Color.white * 0.5f + new Color((float)(legFrameId % 2), (float)(i % 2), 1 - (float)(i % 2), 1.0f);
int h = (int)w / 2;
Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineEnd + 1, y - h - 1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart - 1, y + h), new Vector2(lineEnd + 1, y + h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineStart - 1, y + h + 1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineEnd + 1, y - h - 1), new Vector2(lineEnd + 1, y + h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineEnd, y), new Color(1.0f, 1.0f, 1.0f, 1.0f), w);
// Color depending on stance
Color currentCol = Color.black;
float phase = cycle.getStancePhase(p_phi);
if (stance)
{
currentCol = Color.Lerp(ucol, Color.black, phase * phase);
}
// draw df
Drawing.DrawLine(new Vector2(lineStart + timelineLen * offset, y), new Vector2(Mathf.Min(lineEnd, dutyEnd), y), currentCol, w);
// draw rest if out of bounds
if (offset + len > 1.0f)
{
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineStart + timelineLen * (offset + len - 1.0f), y), currentCol, w);
}
// Draw current time marker
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi - 1, y), new Vector2(lineStart + timelineLen * p_phi + 3, y),
Color.red, w);
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi, y), new Vector2(lineStart + timelineLen * p_phi + 2, y),
Color.green * 2, w);
}
}
}
// Quick and dirty temporary solution for applying
// foot torque. This should be done to the global torque array
// in the final solution
public void tempApplyFootTorque(float p_phi)
{
/*
* Toe-off is modeled with a linear target trajectory towards a fixed toe-off
* target angle θa that is triggered ∆ta seconds in advance of the start of the swing phase,
* as dictated by the gait graph.
*
* Foot- strike anticipation is done in an analogous fashion with
* respect to the anticipated foot-strike time and defined by θb and ∆tb.
*/
for (int i = 0; i < c_legCount; i++)
{
StepCycle stepcycle = m_tuneStepCycles[i];
Transform foot = m_feet[i].transform;
float toeOffStartOffset = m_tuneToeOffTime[i];
float footStrikeStartOffset = m_tuneFootStrikeTime[i];
// get absolutes
// Get point in time when toe lifts off from ground
float toeOffStart = stepcycle.m_tuneStepTrigger + stepcycle.m_tuneDutyFactor - toeOffStartOffset;
if (toeOffStart >= 1.0f)
{
toeOffStart -= 1.0f;
}
if (toeOffStart < 0.0f)
{
toeOffStart += 1.0f;
}
// Get point in time when foot touches the ground
float footStrikeStart = stepcycle.m_tuneStepTrigger - footStrikeStartOffset;
if (toeOffStart < 0.0f)
{
footStrikeStart += 1.0f;
}
//
Vector3 torque = Vector3.zero; Color rot = Color.red;
if ((p_phi > footStrikeStart && footStrikeStart > toeOffStart) || p_phi < toeOffStart) // catch first
{
torque = Vector3.right * (foot.localRotation.eulerAngles.x - m_tuneFootStrikeAngle);
}
else/* if (p_phi < toeOffStart)*/
{
torque = Vector3.right * (foot.localRotation.eulerAngles.x - m_tuneToeOffAngle);
rot = Color.green;
}
foot.rigidbody.AddRelativeTorque(torque /**Time.deltaTime*/);
foot.renderer.material.color = rot * Mathf.Abs(torque.x);
}
}
void debugColorLegs()
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
{
StepCycle cycle = lf.m_tuneStepCycles[n];
Rigidbody current = m_joints[lf.m_neighbourJointIds[n]];
if (lf.isInControlledStance(n, m_player.m_gaitPhase))
{
current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.yellow;
}
else
{
current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.white;
}
}
}
}
// Compute the torque of all PD-controllers in the joints
Vector3[] computePDTorques(float p_phi)
{
// This loop might have to be rewritten into something a little less cumbersome
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_usePDTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
newTorques[lf.m_id] = m_jointTorques[lf.m_id];
// All hip joints
for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
{
StepCycle cycle = lf.m_tuneStepCycles[n];
int jointID = lf.m_neighbourJointIds[n];
if (lf.isInControlledStance(i, m_player.m_gaitPhase))
{
newTorques[jointID] = Vector3.zero;
// m_jointTorques[jointID];
//Vector3.zero;
//
}
else if (m_desiredJointTorquesPD.Length > 0)
{
newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
}
}
// All other joints
for (int n = 0; n < lf.m_legJointIds.Length; n++)
{
int jointID = lf.m_legJointIds[n];
if (jointID > -1)
{
newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
}
}
}
}
return(newTorques);
}
