internal static float calculateElbowAngle(CvCoordinates arm, CvCoordinates elbow, CvCoordinates hands)
{
var firstVec = MonitoringUtils.CvCoordinatesToVec3(arm) - MonitoringUtils.CvCoordinatesToVec3(elbow);
var secondVec = MonitoringUtils.CvCoordinatesToVec3(elbow) - MonitoringUtils.CvCoordinatesToVec3(hands);
return(Math.Abs(Vector3.Angle(firstVec, secondVec)));
}
protected override void useMarkerCoordinates()
{
floorPlane = MonitoringUtils.getPlaneFromCvCoordinates(_balls[(int)markerIds.Floor3], _balls[(int)markerIds.Floor2], _balls[(int)markerIds.Floor1]);
calculateAngles();
if (firstMeasurement == true)
{
setInitialValues();
}
for (int i = 0; i < _balls.Length; i++)
{
markers[i].transform.position = MonitoringUtils.getCoordinatesRelativeToPlane(_balls[i], floorPlane) + offset;
}
dummy.updateDummyPose(
markers[(int)markerIds.Dummy1].transform.position,
markers[(int)markerIds.Dummy2].transform.position,
markers[(int)markerIds.Hands].transform.position, ref performTracking);
subject.updateSubjectPose(markers[(int)markerIds.rightArm].transform.position, markers[(int)markerIds.leftArm].transform.position);
checkCompressionDepth();
calculateCompressionRate();
updateMeasurementMessages();
updateGraphs();
if (saveCheckBox.isOn)
{
saveDataToTextFile();
}
}
internal static Plane getPlaneFromCvCoordinates(CvCoordinates first, CvCoordinates second, CvCoordinates third)
{
Plane plane = new Plane();
plane.Set3Points(
MonitoringUtils.CvCoordinatesToVec3(first),
MonitoringUtils.CvCoordinatesToVec3(second),
MonitoringUtils.CvCoordinatesToVec3(third));
return(plane);
}
void setInitialValues()
{
offset = MonitoringUtils.setOffsetVector(desiredLocation.transform.position, floorPlane, _balls[(int)markerIds.Hands]);
for (int i = 0; i < _balls.Length; i++)
{
markers[i].transform.position = MonitoringUtils.getCoordinatesRelativeToPlane(_balls[i], floorPlane) + offset;
}
dummy.setInitialPose(markers[(int)markerIds.Dummy1].transform.position, markers[(int)markerIds.Dummy2].transform.position, markers[(int)markerIds.Hands].transform.position);
setInitialAngleValues();
subject.setSubjectInitialPose(offset, dummy.transform);
}
void calculateAngles()
{
rightElbowAngle = Math.Abs(MonitoringUtils.calculateElbowAngle(_balls[0], _balls[2], _balls[4]) - initialRightArmAngle);
leftElbowAngle = Math.Abs(MonitoringUtils.calculateElbowAngle(_balls[1], _balls[3], _balls[4]) - initialLeftHandAngle);
var armsPlane = new Plane();
armsPlane.Set3Points(
MonitoringUtils.CvCoordinatesToVec3(_balls[(int)markerIds.rightArm]),
MonitoringUtils.CvCoordinatesToVec3(_balls[(int)markerIds.leftArm]),
MonitoringUtils.CvCoordinatesToVec3(_balls[(int)markerIds.Hands]));
armsFloorAngle = Vector3.Angle(floorPlane.normal, armsPlane.normal);
}
float getMarkerPositionChange(int index)
{
if (index >= previousMarkerPositions.Count)
{
previousMarkerPositions.Add(MonitoringUtils.CvCoordinatesToVec3(_balls[index]));
return(0);
}
Vector3 currentMarkerPosition = MonitoringUtils.CvCoordinatesToVec3(_balls[index]);
float distance = Vector3.Distance(previousMarkerPositions[index], currentMarkerPosition);
previousMarkerPositions[index] = currentMarkerPosition;
return(distance);
}
protected void Update()
{
MatToTexture2D();
checkFPS();
MonitoringUtils.setStartButtonText(performTracking, changeModeButton);
}
internal static Vector3 getCoordinatesRelativeToPlane(CvCoordinates coordinates, Plane plane)
{
return(new Vector3(Vector3.Project(MonitoringUtils.CvCoordinatesToVec3(coordinates), Vector3.ProjectOnPlane(Vector3.right, plane.normal)).magnitude,
Math.Abs(plane.GetDistanceToPoint(MonitoringUtils.CvCoordinatesToVec3(coordinates))),
Vector3.Project(MonitoringUtils.CvCoordinatesToVec3(coordinates), Vector3.Cross(plane.normal, Vector3.ProjectOnPlane(Vector3.right, plane.normal))).magnitude));
}
