//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"); } } }