//compares all the bones from the passed body to the BodyAngles(a targetBodyAngles in all 3 default plane)
//returns a List of PoseError that tells which bones were off and the default plane they were judged
public static List <PoseError> CompareBodyAngles(BodyAngles currentBodyAngles, BodyAngles targetBodyAngles, float tolerance)
{
var totalErrors = new List <PoseError>();
List <PoseError> partialErrors;
partialErrors = ComparePlaneBasedBodyAngles(currentBodyAngles.FrontalAngles, targetBodyAngles.FrontalAngles, tolerance);
foreach (var error in partialErrors)
{
totalErrors.Add(error);
}
partialErrors = ComparePlaneBasedBodyAngles(currentBodyAngles.SagittalAngles, targetBodyAngles.SagittalAngles, tolerance);
foreach (var error in partialErrors)
{
totalErrors.Add(error);
}
partialErrors = ComparePlaneBasedBodyAngles(currentBodyAngles.HorizontalAngles, targetBodyAngles.HorizontalAngles, tolerance);
foreach (var error in partialErrors)
{
totalErrors.Add(error);
}
return(totalErrors);
}
public void ReceiveData(Voxar.BodyJoints[] bodys)
{
foreach (var body in bodys)
{
if (body.status == Status.Tracking)
{
Voxar.BodyAngles currentAngle = new Voxar.BodyAngles(body);
var errors = MovementAnalyzer.CompareBodyAngles(currentAngle, Target, 10.0f);
// MovementAnalyzer.CalculateAngleForBones(body, BoneType.RightClavicule, BoneType.LeftClavicule);
// Debug.Log(currentAngle.FrontalAngles.GetAngle(BoneType.LeftClavicule, BoneType.RightClavicule));
// pegar o angulo padrão entre eles!!!!
bool entrei = false;
foreach (var error in errors) // Vejo se a postura tá boa.
{
entrei = true;
var angle = currentAngle.FrontalAngles.angles[error.boneTypes[0]][error.boneTypes[1]];
Debug.Log(angle);
text.SetActive(true);
}
if (!entrei)
{
text.SetActive(false);
}
leftFoot = body.joints[JointType.LeftFoot].worldPosition / 1000.0f;
rightFoot = body.joints[JointType.RightFoot].worldPosition / 1000.0f;
var distanceFoots = Vector3.Distance(leftFoot, rightFoot);
var vectors = MovementAnalyzer.GenerateLowerBodyPlanes(body);
Vector3 myDirectionmyDirection = vectors[BasePlanes.Frontal];
Vector3 mustBeTheNext;
if (begin)
{
mustBeTheNext = rightFoot;
ball.transform.localPosition = mustBeTheNext;
begin = false;
}
if (turnFoot == false && distanceFoots >= 0.5 && !andando) // Pé direito
{
mustBeTheNext = rightFoot /*myDirection.normalized * 0.0045f */;
ball.transform.localPosition = mustBeTheNext;
var cubeRenderer = ball.GetComponent <Renderer>();
cubeRenderer.material.SetColor("_Color", Color.red);
turnFoot = !turnFoot;
andando = true;
}
else if (distanceFoots >= 0.5 && turnFoot == true && !andando) // Pé esquerdo
{
mustBeTheNext = leftFoot /*myDirection.normalized * 0.0045f */;
ball.transform.localPosition = mustBeTheNext;
var cubeRenderer = ball.GetComponent <Renderer>();
cubeRenderer.material.SetColor("_Color", Color.blue);
turnFoot = !turnFoot;
andando = true;
}
if (distanceFoots < 0.5 && andando)
{
andando = false;
}
float cg = CenterOfGravity(body, 71.45f); // value just for test
float p = body.joints[JointType.RightAnkle].worldPosition.z / 1000.0f;
float q = body.joints[JointType.LeftAnkle].worldPosition.z / 1000.0f;
float pt = (float)(Math.Truncate((double)p * 100.0) / 100.0);
float qt = (float)(Math.Truncate((double)q * 100.0) / 100.0);
float vp = (pt - qt) / (10 * (Time.deltaTime));
float vpp = 36 * (Math.Abs(vp / 10)) / 10;
float vpt = (float)(Math.Truncate((double)vpp * 100.0) / 100.0);
textRenderer.text = $"R {pt} Metros" + "\r\n" + $"L {qt} Metros" + "\r\n" + $"L {vpt} p/s";
Debug.Log(p + "Meters");
}
}
}
