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