public Command(Skeleton skel)
{
BodyPartData rightArm = new BodyPartData(skel, BodyPart.RightArm);
BodyPartData leftArm = new BodyPartData(skel, BodyPart.LeftArm);
this.valid = this.isValidCommand(rightArm, leftArm);
this.setUD(rightArm, leftArm);
this.setRL(rightArm, leftArm);
this.setFB(rightArm, leftArm);
this.setTRL(rightArm, leftArm);
//float max = Math.Max(Math.Max(this.UD, this.RL), Math.Max(this.FB, this.TRL));
//if (this.UD < max) this.UD = 0;
//if (this.RL < max) this.RL = 0;
//if (this.FB < max) this.FB = 0;
//if (this.TRL < max) this.TRL = 0;
}
private void setFB(BodyPartData rightArm, BodyPartData leftArm)
{
float tempFB = (rightArm.Z + leftArm.Z) / 2;
int sign = tempFB > 0 ? 1 : -1;
this.FB = sign * (float) Math.Round(Math.Max(tempFB * sign - WIGGLE_ROOM, 0) / (1 - WIGGLE_ROOM), 1);
}
/// <summary>
/// Returns true if both arms are more or less straight, and they dont point straight down.
/// </summary>
/// <param name="skel"></param>
/// <returns></returns>
private bool isValidCommand(BodyPartData rightArm, BodyPartData leftArm)
{
// Check for straightness.
if (!rightArm.straight || !leftArm.straight)
return false;
else if ((rightArm.Y < -0.8 && leftArm.Y < -0.8))
return false;
return true;
}
private void setUD(BodyPartData rightArm, BodyPartData leftArm)
{
float tempUD = (rightArm.Y + leftArm.Y) / 2;
int sign = tempUD > 0 ? 1 : -1;
this.UD = sign * (float) Math.Round(Math.Max(tempUD * sign - WIGGLE_ROOM, 0) / (1 - WIGGLE_ROOM), 1);
}
private void setTRL(BodyPartData rightArm, BodyPartData leftArm)
{
float tempTRL = (rightArm.Z - leftArm.Z) / 2;
int sign = tempTRL > 0 ? 1 : -1;
this.TRL = sign * (float) Math.Round(Math.Max(tempTRL * sign - WIGGLE_ROOM, 0) / (1 - WIGGLE_ROOM), 1);
}