private void CalculateSpeedErrors(ArticolationError articolationError,
ArticolationPoint currentArticolationPoint, ArticolationPoint previousArticolationPoint,
ArticolationPoint currentIdealArticolationPoint, ArticolationPoint previousIdealArticolationPoint)
{
if (previousIdealArticolationPoint != null && previousArticolationPoint != null)
{
articolationError.Position.Speed = CalculateSpeedError(
currentIdealArticolationPoint.Position, previousIdealArticolationPoint.Position,
currentArticolationPoint.Position, previousArticolationPoint.Position,
_trainingSet.timing);
articolationError.Angle.Speed = CalculateSpeedError(
currentIdealArticolationPoint.Angle, previousIdealArticolationPoint.Angle,
currentArticolationPoint.Angle, previousArticolationPoint.Angle,
_trainingSet.timing);
}
else
{
articolationError.Position.Speed = Vector3.zero;
articolationError.Angle.Speed = Vector3.zero;
}
}
/// <summary>
///
/// </summary>
/// <param name="tollerances"></param>
/// <param name="currentStep"></param>
/// <param name="idealExerciseStep"></param>
/// <returns></returns>
public ArticolationError[] EvaluateExerciseStep(ArticolationTollerance[] tollerances, ExerciseStep currentStep, ExerciseStep idealExerciseStep = null)
{
// check abiity to evaluate the step
if (_isRealTimeSampling && idealExerciseStep == null)
{
throw new Exception("Unable to evaluate exercise step without ideal sample or a training set!");
}
PerformedMovementSteps.Add(currentStep);
// eventually keep training the ai (doing this all the following code is the same in both cases)
if (idealExerciseStep != null)
{
_trainingSet.idealMovementSteps.Add(idealExerciseStep);
}
ExerciseStep previousStep = GetPerformedStep(-1),
currentIdealStep = GetIdealStep(0),
previousIdealStep = GetIdealStep(-1);
if (currentIdealStep == null)
{
int lastStepIndex = _trainingSet.idealMovementSteps.Count - 1;
currentIdealStep = _trainingSet.idealMovementSteps[lastStepIndex];
previousIdealStep = _trainingSet.idealMovementSteps[lastStepIndex - 1];
}
List <ArticolationError> articolationErrors = new List <ArticolationError>();
ArticolationPoint currentStepArticolationPoint = currentStep.Root,
previousStepArticolationPoint = previousStep == null ? null : previousStep.Root,
currentStepIdealArticolationPoint = currentIdealStep.Root,
previousStepIdealArticolationPoint = previousIdealStep == null ? null : previousIdealStep.Root;
int i = 0;
while (currentStepArticolationPoint != null)
{
ArticolationError articolationError = new ArticolationError();
ArticolationTollerance tollerance = tollerances[i++];
articolationError.Position.Magnitude = currentStepIdealArticolationPoint.Position - currentStepArticolationPoint.Position;
Debug.Log("Articolation [" + i + "]");
Debug.Log("ideal position (" +
currentStepIdealArticolationPoint.Position.x + ", " + currentStepIdealArticolationPoint.Position.y + ", " + currentStepIdealArticolationPoint.Position.z + ")"
);
Debug.Log("real position (" +
currentStepArticolationPoint.Position.x + ", " + currentStepArticolationPoint.Position.y + ", " + currentStepArticolationPoint.Position.z + ")"
);
articolationError.Position.IsMagnitudeCorrect = articolationError.Position.Magnitude.magnitude < tollerance.positionTolleranceRadius;
articolationError.Angle.Magnitude = currentStepIdealArticolationPoint.Angle - currentStepArticolationPoint.Angle;
articolationError.Angle.IsMagnitudeCorrect = articolationError.Angle.Magnitude.magnitude < tollerance.rotationTolleranceRadius;
CalculateSpeedErrors(articolationError, currentStepArticolationPoint, previousStepArticolationPoint, currentStepIdealArticolationPoint, previousStepIdealArticolationPoint);
articolationError.Position.IsSpeedCorrect = articolationError.Position.Speed.magnitude < tollerance.positionSpeedTolleranceRadius;
articolationError.Angle.IsSpeedCorrect = articolationError.Angle.Speed.magnitude < tollerance.positionSpeedTolleranceRadius;
articolationErrors.Add(articolationError);
currentStepArticolationPoint = currentStepArticolationPoint.Substaining;
previousStepArticolationPoint = previousStepArticolationPoint == null ? null : previousStepArticolationPoint.Substaining;
currentStepIdealArticolationPoint = currentStepIdealArticolationPoint.Substaining;
previousStepIdealArticolationPoint = previousStepIdealArticolationPoint == null ? null : previousStepIdealArticolationPoint.Substaining;
}
return(articolationErrors.ToArray());
}
