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.opencv;
019
020import edu.wpi.first.math.geometry.Pose2d;
021import edu.wpi.first.math.geometry.Rotation2d;
022import edu.wpi.first.math.util.Units;
023import org.opencv.core.Core;
024import org.opencv.core.Point;
025
026/**
027 * An image rotation about the camera's +Z axis, which points out of the camera towards the world.
028 * This is mirrored relative to what you might traditionally think of as image rotation, which is
029 * about an axis coming out of the image towards the viewer or camera. TODO: pull this from
030 * image-rotation.md
031 */
032public enum ImageRotationMode {
033    DEG_0(-1, new Rotation2d()),
034    // rotating an image matrix clockwise is a ccw rotation about camera +Z, lmao
035    DEG_90_CCW(Core.ROTATE_90_COUNTERCLOCKWISE, new Rotation2d(Units.degreesToRadians(90))),
036    DEG_180_CCW(Core.ROTATE_180, new Rotation2d(Units.degreesToRadians(180))),
037    DEG_270_CCW(Core.ROTATE_90_CLOCKWISE, new Rotation2d(Units.degreesToRadians(-90)));
038
039    public final int value;
040    public final Rotation2d rotation2d;
041
042    private ImageRotationMode(int value, Rotation2d tr) {
043        this.value = value;
044        this.rotation2d = tr;
045    }
046
047    /**
048     * Rotate a point in an image
049     *
050     * @param point The point in the unrotated image
051     * @param width Image width, in pixels
052     * @param height Image height, in pixels
053     * @return The point in the rotated frame
054     */
055    public Point rotatePoint(Point point, double width, double height) {
056        Pose2d offset;
057        switch (this) {
058            case DEG_0:
059                return point;
060            case DEG_90_CCW:
061                offset = new Pose2d(width, 0, rotation2d);
062                break;
063            case DEG_180_CCW:
064                offset = new Pose2d(width, height, rotation2d);
065                break;
066            case DEG_270_CCW:
067                offset = new Pose2d(0, height, rotation2d);
068                break;
069            default:
070                throw new RuntimeException("Totally bjork");
071        }
072
073        var pointAsPose = new Pose2d(point.x, point.y, new Rotation2d());
074        var ret = pointAsPose.relativeTo(offset);
075
076        return new Point(ret.getX(), ret.getY());
077    }
078}