001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.pipe.impl;
019
020import java.awt.*;
021import java.util.List;
022import org.apache.commons.lang3.tuple.Pair;
023import org.opencv.core.Mat;
024import org.opencv.core.Point;
025import org.opencv.imgproc.Imgproc;
026import org.photonvision.common.util.ColorHelper;
027import org.photonvision.vision.frame.FrameDivisor;
028import org.photonvision.vision.frame.FrameStaticProperties;
029import org.photonvision.vision.opencv.DualOffsetValues;
030import org.photonvision.vision.opencv.ImageRotationMode;
031import org.photonvision.vision.pipe.MutatingPipe;
032import org.photonvision.vision.target.RobotOffsetPointMode;
033import org.photonvision.vision.target.TargetCalculations;
034import org.photonvision.vision.target.TrackedTarget;
035
036public class Draw2dCrosshairPipe
037        extends MutatingPipe<
038                Pair<Mat, List<TrackedTarget>>, Draw2dCrosshairPipe.Draw2dCrosshairParams> {
039    @Override
040    protected Void process(Pair<Mat, List<TrackedTarget>> in) {
041        if (!params.shouldDraw) return null;
042
043        var image = in.getLeft();
044
045        if (params.showCrosshair) {
046            double x = params.frameStaticProperties.centerX;
047            double y = params.frameStaticProperties.centerY;
048            double scale = params.frameStaticProperties.imageWidth / (double) params.divisor.value / 32.0;
049
050            switch (params.robotOffsetPointMode) {
051                case None -> {}
052                case Single -> {
053                    if (params.singleOffsetPoint.x != 0 && params.singleOffsetPoint.y != 0) {
054                        x = params.singleOffsetPoint.x;
055                        y = params.singleOffsetPoint.y;
056                    }
057                }
058                case Dual -> {
059                    if (!in.getRight().isEmpty()) {
060                        var target = in.getRight().get(0);
061                        if (target != null) {
062                            var area = target.getArea();
063                            var offsetCrosshair =
064                                    TargetCalculations.calculateDualOffsetCrosshair(params.dualOffsetValues, area);
065                            x = offsetCrosshair.x;
066                            y = offsetCrosshair.y;
067                        }
068                    }
069                }
070            }
071
072            x /= (double) params.divisor.value;
073            y /= (double) params.divisor.value;
074
075            Point xMax = new Point(x + scale, y);
076            Point xMin = new Point(x - scale, y);
077            Point yMax = new Point(x, y + scale);
078            Point yMin = new Point(x, y - scale);
079
080            Imgproc.line(image, xMax, xMin, ColorHelper.colorToScalar(params.crosshairColor));
081            Imgproc.line(image, yMax, yMin, ColorHelper.colorToScalar(params.crosshairColor));
082        }
083        return null;
084    }
085
086    public static class Draw2dCrosshairParams {
087        public boolean showCrosshair = true;
088        public Color crosshairColor = Color.GREEN;
089
090        public final boolean shouldDraw;
091        public final FrameStaticProperties frameStaticProperties;
092        public final ImageRotationMode rotMode;
093        public final RobotOffsetPointMode robotOffsetPointMode;
094        public final Point singleOffsetPoint;
095        public final DualOffsetValues dualOffsetValues;
096        private final FrameDivisor divisor;
097
098        public Draw2dCrosshairParams(
099                FrameStaticProperties frameStaticProperties,
100                FrameDivisor divisor,
101                ImageRotationMode rotMode) {
102            shouldDraw = true;
103            this.frameStaticProperties = frameStaticProperties;
104            this.rotMode = rotMode;
105            robotOffsetPointMode = RobotOffsetPointMode.None;
106            singleOffsetPoint = new Point();
107            dualOffsetValues = new DualOffsetValues();
108            this.divisor = divisor;
109        }
110
111        public Draw2dCrosshairParams(
112                boolean shouldDraw,
113                RobotOffsetPointMode robotOffsetPointMode,
114                Point singleOffsetPoint,
115                DualOffsetValues dualOffsetValues,
116                FrameStaticProperties frameStaticProperties,
117                FrameDivisor divisor,
118                ImageRotationMode rotMode) {
119            this.shouldDraw = shouldDraw;
120            this.frameStaticProperties = frameStaticProperties;
121            this.robotOffsetPointMode = robotOffsetPointMode;
122            this.singleOffsetPoint = singleOffsetPoint;
123            this.dualOffsetValues = dualOffsetValues;
124            this.divisor = divisor;
125            this.rotMode = rotMode;
126        }
127    }
128}