public float EstimateHeight(Vector3 position, Vector3 lastPosition)
{
if (!enableFootHeight)
{
transform.position = position;
return(0);
}
// TODO: Get rid of this hack!
position.y = lastPosition.y = 0;
// get speed and clamp it to max speed
speed = Mathf.Clamp((position - lastPosition).magnitude / Time.fixedDeltaTime, 0, maxSpeed);
// scale speed to 0 - 1
speed01 = speed / maxSpeed;
// apply filter
speed01 = (float)kalman.UseFilter(speed01);// CalcKalman(speed01);
// take height from curve
height = curve.Evaluate(speed01);
// apply height
transform.position = new Vector3(position.x, height, position.z);
return(height);
}
private void FootAndHipHeight()
{
// get heights
leftFootPosition.y = leftFoot.EstimateHeight(leftFootPosition, lastLeftFootPosition);
rightFootPosition.y = rightFoot.EstimateHeight(rightFootPosition, lastRightFootPosition);
distanceBetweenFeet = Vector3.Distance(leftFootPosition, rightFootPosition);
centerPosition.y = -(float)hipHeightFilter.UseFilter(Mathf.Clamp(((distanceBetweenFeet - 0.4f) * 0.5f), 0.0f, 1.0f) - 0.02f);
currenHipHeight = centerPosition.y;
}
private void UseSimpleKalman()
{
// get speed and clamp it to max speed
speed = Mathf.Clamp((transform.position - lastPosition).magnitude / Time.deltaTime, 0, maxSpeed);
// store last foot position
lastPosition = transform.position;
// scale speed to 0 - 1
speed01 = speed / maxSpeed;
// apply filter
speed01 = (float)kalman.UseFilter(speed01);// CalcKalman(speed01);
// take height from curve
height = curve.Evaluate(speed01);
// apply height
transform.position = new Vector3(transform.position.x, height, transform.position.z);
}
//private void FootFiltering() {
// if (!applyFilterOnFeet)
// return;
// foreach (SimpleKalman item in kalmanFilters) {
// item.Q = kalmanQ;
// item.R = kalmanR;
// }
// leftFootPosition = new Vector3((float)kalmanFilters[0].UseFilter(leftFootPosition.x), leftFootPosition.y, (float)kalmanFilters[1].UseFilter(leftFootPosition.z));
// rightFootPosition = new Vector3((float)kalmanFilters[2].UseFilter(rightFootPosition.x), rightFootPosition.y, (float)kalmanFilters[3].UseFilter(rightFootPosition.z));
//}
//private void OrientationFiltering() {
// Vector3 tempFast = (centerPosition - lastCenterPosition);
// fastOrientation = new Vector2(tempFast.x, tempFast.z).normalized;
// //fastOrientation = (Orientation + fastOrientation) / 2;
// fastOrientation = new Vector2((float)orientationFilter[0].UseFilter(fastOrientation.x), (float)orientationFilter[1].UseFilter(fastOrientation.y));
//}
private void FootAndHipHeight()
{
distanceBetweenFeet = Vector3.Distance(leftFootPosition, rightFootPosition);
centerPosition.y = -(float)hipHeightFilter.UseFilter(Mathf.Clamp(((distanceBetweenFeet - 0.5f) * 0.5f), 0.0f, 1.0f) - 0.0f);
}
