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;
}
