private void calculateRulaEngine() { // Hitung nilai keseluruhan finalResult = RulaCalculation.calculateRula(); finalMsg = RulaCalculation.getFinalScoreMsg(finalResult); // Set Ke TexBlocknya this.finalScore.Text = finalResult.ToString(); // Set pewarnaan logo nya if (finalResult <= 2) { this.finalScore.Foreground = Brushes.Green; } else if (finalResult > 2 && finalResult <= 4) { this.finalScore.Foreground = Brushes.GreenYellow; } else if (finalResult > 4 && finalResult <= 7) { this.finalScore.Foreground = Brushes.Yellow; } else { this.finalScore.Foreground = Brushes.Red; } this.finalScoreMsg.Text = finalMsg; }
private void calculateTrunk(Vector3D trunk) { Joint start, end, poros; double angle, trunkBending; bool isBending; start = kalmanFilterFull(getBodyTypeSeq(JointType.SpineShoulder)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.SpineBase)); end = poros; end.Position.Z = 2 * poros.Position.Z; angle = calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, end.Position.Y, end.Position.Z); if (angle <= 100) // overestimate -> +-10 degree, actualy must be 90 degree { angle = 100 - angle; } else { angle -= 100; angle *= -1; } RulaCalculation.calculateTrunk(angle); // Trunk twistedi -> not done : Next Development //double trunkTwist; //start = kalmanFilterFull(getBodyTypeSeq(JointType.SpineMd)); //end = kalmanFilterFull(getBodyTypeSeq(JointType.AnkleLeft)); //trunkTwist = calculateAngle3D(convertJointoVector(start), convertJointoVector(end)); // Trunk side bending // Create a Vector that perpendicular between hip Joint Vector3D hipLeft = convertJointoVector(kalmanFilterFull(getBodyTypeSeq(JointType.HipLeft))); Vector3D hipRight = convertJointoVector(kalmanFilterFull(getBodyTypeSeq(JointType.HipRight))); Vector3D horizontalSide = hipLeft - hipRight; Vector3D perpendicularHorizontal = new Vector3D(); perpendicularHorizontal.X = horizontalSide.Y; perpendicularHorizontal.Y = -1 * horizontalSide.X; perpendicularHorizontal.Z = horizontalSide.Z; // Calculate between perpendicular to the horizontal vector // and trunk vector to get the angle //trunkBending = calculateAngle3D(perpendicularHorizontal, trunk); // Default position angle -> 108-110 trunkBending = calculateAngle2D(perpendicularHorizontal, trunk); // Trunk Bending isBending = RulaCalculation.calcTrunkbending(trunkBending); setStatus(this.txtTrunkBending, isBending); // Set on GUI this.textTrunk.Text = angle.ToString("0"); // Save data to static variable GlobalVal.trunk = angle; GlobalVal.trunkBending = isBending; }
private void calculateLowerArm(int sisiBadan) { Joint start, end, poros; double angle; bool isDeviation = false; if (0 == sisiBadan) { // Lengan Bawah - oke start = kalmanFilterFull(getBodyTypeSeq(JointType.ShoulderLeft)); end = kalmanFilterFull(getBodyTypeSeq(JointType.WristLeft)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowLeft)); angle = calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, end.Position.Y, end.Position.Z); angle = 180 - angle; RulaCalculation.calculateLowerArm(angle); // Cek arah lengan bwah apakah keluar dari batas midlane // Analyze relative position of the wrist X coordinate towards the shoulder position // Check deviation base on coordinate isDeviation = RulaCalculation.calcLowerArmDeviation(end.Position.X, start.Position.X); setStatus(this.txtLowerArmMidline, isDeviation); this.textLowerArm.Text = angle.ToString("0"); } else { // Lengan Bawah - oke start = kalmanFilterFull(getBodyTypeSeq(JointType.ShoulderRight)); end = kalmanFilterFull(getBodyTypeSeq(JointType.WristRight)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowRight)); angle = calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, end.Position.Y, end.Position.Z); angle = 180 - angle; RulaCalculation.calculateLowerArm(angle); // Cek arah lengan bwah apakah keluar dari batas midlane // Analyze relative position of the wrist X coordinate towards the shoulder position // Check deviation base on coordinate isDeviation = RulaCalculation.calcLowerArmDeviation(end.Position.X, start.Position.X); setStatus(this.txtLowerArmMidline, isDeviation); this.textLowerArm.Text = angle.ToString("0"); } // Save data to static variable GlobalVal.lowerArm = angle; GlobalVal.lowerArmMidline = isDeviation; }
private void calculateNeck() { Joint start, end, poros; double angle, bendingAngle; bool isBending; // Neck Angle corespondent to sagittal plane start = kalmanFilterFull(getBodyTypeSeq(JointType.Head)); end = kalmanFilterFull(getBodyTypeSeq(JointType.SpineBase)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.SpineShoulder)); angle = calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, end.Position.Y, end.Position.Z); if (angle > 90) { angle = 180 - angle; } else { angle *= -1; } // Neck Bending bendingAngle = calculateAngle(poros.Position.X, poros.Position.Y, start.Position.X, start.Position.Y, end.Position.X, end.Position.Y); if (bendingAngle > 150) { bendingAngle = 180 - bendingAngle; } RulaCalculation.calculateNeck(angle); this.textNeck.Text = angle.ToString("0"); // Neck Bending isBending = RulaCalculation.calcNeckBending(bendingAngle); setStatus(this.txtNeckBending, isBending); // Save data to static variable GlobalVal.neck = angle; GlobalVal.neckBending = isBending; }
private void calculateWrist(int sisiBadan) { Joint start, end, poros; double angle; if (0 == sisiBadan) { // Wrist Angle coresponding Horizontall plane start = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowLeft)); end = kalmanFilterFull(getBodyTypeSeq(JointType.HandTipLeft)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.WristLeft)); //angle = calculateAngle(poros.Position.X, poros.Position.Y, start.Position.X, poros.Position.Y, // end.Position.X, end.Position.Y); angle = poros.Angle(start, end) - 180; RulaCalculation.calculateWrist(angle); this.textWristArm.Text = angle.ToString("0"); } else // sisi Kanan { // Pergelangan Tangan - oke start = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowRight)); end = kalmanFilterFull(getBodyTypeSeq(JointType.HandTipRight)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.WristRight)); //angle = calculateAngle(poros.Position.X, poros.Position.Y, start.Position.X, poros.Position.Y, // end.Position.X, end.Position.Y); angle = poros.Angle(start, end) - 180; RulaCalculation.calculateWrist(angle); this.textWristArm.Text = angle.ToString("0"); } // Save to static variable GlobalVal.wrist = angle; }
public void calculateAngle(Body body) { Joint spineShoulder = kalmanFilterFull(getBodyTypeSeq(JointType.SpineShoulder)); Joint spineMid = kalmanFilterFull(getBodyTypeSeq(JointType.SpineMid)); Joint spineBase = kalmanFilterFull(getBodyTypeSeq(JointType.SpineBase)); Vector3D trunk = convertJointoVector(spineBase) - convertJointoVector(spineMid) - convertJointoVector(spineShoulder); // Group A calculateUpperArm(GlobalVal.BODY_SIDE); calculateLowerArm(GlobalVal.BODY_SIDE); calculateWrist(GlobalVal.BODY_SIDE); // Group B calculateNeck(); calculateTrunk(trunk); // Check if data being recorded or not if (GlobalVal.RECORD_STATUS) { RulaCalculation.recordDataToCSV(); } }
/******************* Method Angle Calc Body Part ******************/ private void calculateUpperArm(int sisiBadan) { Joint start, end, poros; double angle, shoulderRaise; bool isAbducted, isRaised; if (0 == sisiBadan) // sisi Kiri { // Upper arm correspondent to sagittal Plane start = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowLeft)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.ShoulderLeft)); // Calculate Angle angle = this.calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, poros.Position.Y, 2 * poros.Position.Z); // 0 -> full to backward, 90 -> at stright to the trunk, 180 -> pararell to shoulder if (angle > 90) { angle -= 90; } else if (angle < 90 && angle > 60) { angle = -1 * (90 - angle); } else if (angle < 90) { angle += 90; } RulaCalculation.calculateUpperArm(angle); // Upper Arm Abduction isAbducted = RulaCalculation.calculateUpperArmAbduction(start.Position.X, poros.Position.X); setStatus(this.txtUpperArmAbduction, isAbducted); // Shoulder is Raise start = poros; end = kalmanFilterFull(getBodyTypeSeq(JointType.Neck)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.SpineShoulder)); shoulderRaise = calculateAngle(poros.Position.X, poros.Position.Y, start.Position.X, start.Position.Y, 2 * poros.Position.X, poros.Position.Y); isRaised = RulaCalculation.calcShoulderRaise(shoulderRaise); setStatus(this.txtShoulderRaise, isRaised); // Update on GUI this.textUpperArm.Text = angle.ToString("0"); } else { // Lengan atas Oke start = kalmanFilterFull(getBodyTypeSeq(JointType.ElbowRight)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.ShoulderRight)); // Calculate Angle angle = this.calculateAngle(poros.Position.Y, poros.Position.Z, start.Position.Y, start.Position.Z, poros.Position.Y, -2 * poros.Position.Z); // 0 -> full to backward, 90 -> at stright to the trunk, 180 -> pararell to shoulder if (angle > 90) { angle -= 90; } else if (angle < 90 && angle > 60) { angle = -1 * (90 - angle); } else if (angle < 90) { angle += 90; } RulaCalculation.calculateUpperArm(angle); // Upper Arm Abduction isAbducted = RulaCalculation.calculateUpperArmAbduction(start.Position.X, poros.Position.X); setStatus(this.txtUpperArmAbduction, isAbducted); // Shoulder is Raise start = poros; end = kalmanFilterFull(getBodyTypeSeq(JointType.Neck)); poros = kalmanFilterFull(getBodyTypeSeq(JointType.SpineShoulder)); shoulderRaise = calculateAngle(poros.Position.X, poros.Position.Y, start.Position.X, start.Position.Y, 2 * poros.Position.X, poros.Position.Y); isRaised = RulaCalculation.calcShoulderRaise(shoulderRaise); setStatus(this.txtShoulderRaise, isRaised); // Update on GUI this.textUpperArm.Text = angle.ToString("0"); } // Save data into static variable GlobalVal.upperArm = angle; GlobalVal.uperArmAbduction = isAbducted; GlobalVal.shoulderAngle = isRaised; }