internal static Vector3 setOffsetVector(Vector3 desiredLocation, Plane plane, CvCoordinates coordinates) { Vector3 offsetVector = desiredLocation - getCoordinatesRelativeToPlane(coordinates, plane); offsetVector.y = 0; return(offsetVector); }
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); }
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)); }
internal static Vector3 CvCoordinatesToVec3(CvCoordinates coordinates) { return(new Vector3(coordinates.X, -coordinates.Y, coordinates.Z)); }