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.common.util.math;
019
020import edu.wpi.first.apriltag.AprilTagPoseEstimate;
021import edu.wpi.first.math.VecBuilder;
022import edu.wpi.first.math.geometry.CoordinateSystem;
023import edu.wpi.first.math.geometry.Pose3d;
024import edu.wpi.first.math.geometry.Rotation3d;
025import edu.wpi.first.math.geometry.Transform3d;
026import edu.wpi.first.math.geometry.Translation3d;
027import edu.wpi.first.math.util.Units;
028import edu.wpi.first.networktables.NetworkTablesJNI;
029import java.util.Arrays;
030import java.util.List;
031import org.opencv.core.Core;
032import org.opencv.core.Mat;
033
034public class MathUtils {
035    MathUtils() {}
036
037    public static double toSlope(Number angle) {
038        return Math.atan(Math.toRadians(angle.doubleValue() - 90));
039    }
040
041    public static int safeDivide(int quotient, int divisor) {
042        if (divisor == 0) {
043            return 0;
044        } else {
045            return quotient / divisor;
046        }
047    }
048
049    public static double roundTo(double value, int to) {
050        double toMult = Math.pow(10, to);
051        return (double) Math.round(value * toMult) / toMult;
052    }
053
054    public static double nanosToMillis(long nanos) {
055        return nanos / 1000000.0;
056    }
057
058    public static double nanosToMillis(double nanos) {
059        return nanos / 1000000.0;
060    }
061
062    public static long millisToNanos(long millis) {
063        return millis * 1000000;
064    }
065
066    public static long microsToNanos(long micros) {
067        return micros * 1000;
068    }
069
070    public static long nanosToMicros(long nanos) {
071        return nanos / 1000;
072    }
073
074    /**
075     * Constrain a value to only take on certain values. Pick the next-highest allowed value in the
076     * array if in-between.
077     *
078     * @param value value to quantize
079     * @param allowableSteps sorted array of the allowed values
080     * @return quantized value
081     */
082    public static int quantize(int value, int[] allowableSteps) {
083        for (int step : allowableSteps) {
084            if (value <= step) {
085                return step;
086            }
087        }
088        return allowableSteps[allowableSteps.length - 1];
089    }
090
091    public static double map(
092            double value, double in_min, double in_max, double out_min, double out_max) {
093        return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
094    }
095
096    public static int map(int value, int inMin, int inMax, int outMin, int outMax) {
097        return (int) Math.floor(map((double) value, inMin, inMax, outMin, outMax) + 0.5);
098    }
099
100    public static long wpiNanoTime() {
101        return microsToNanos(NetworkTablesJNI.now());
102    }
103
104    /**
105     * Get the value of the nTh percentile of a list
106     *
107     * @param list The list to evaluate
108     * @param p The percentile, in [0,100]
109     * @return
110     */
111    public static double getPercentile(List<Double> list, double p) {
112        if ((p > 100) || (p <= 0)) {
113            throw new IllegalArgumentException("invalid quantile value: " + p);
114        }
115
116        if (list.isEmpty()) {
117            return Double.NaN;
118        }
119        if (list.size() == 1) {
120            return list.get(0); // always return single value for n = 1
121        }
122
123        // Sort array. We avoid a third copy here by just creating the
124        // list directly.
125        double[] sorted = new double[list.size()];
126        for (int i = 0; i < list.size(); i++) {
127            sorted[i] = list.get(i);
128        }
129        Arrays.sort(sorted);
130
131        return evaluateSorted(sorted, p);
132    }
133
134    private static double evaluateSorted(final double[] sorted, final double p) {
135        double n = sorted.length;
136        double pos = p * (n + 1) / 100;
137        double fpos = Math.floor(pos);
138        int intPos = (int) fpos;
139        double dif = pos - fpos;
140
141        if (pos < 1) {
142            return sorted[0];
143        }
144        if (pos >= n) {
145            return sorted[sorted.length - 1];
146        }
147        double lower = sorted[intPos - 1];
148        double upper = sorted[intPos];
149        return lower + dif * (upper - lower);
150    }
151
152    /**
153     * Linearly interpolates between two values.
154     *
155     * @param startValue The start value.
156     * @param endValue The end value.
157     * @param t The fraction for interpolation.
158     * @return The interpolated value.
159     */
160    @SuppressWarnings("ParameterName")
161    public static double lerp(double startValue, double endValue, double t) {
162        return startValue + (endValue - startValue) * t;
163    }
164
165    /**
166     * OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target
167     * transformation from EDN to NWU.
168     *
169     * <p>Note: The detected target's rvec and tvec perform a rotation-translation transformation
170     * which converts points in the target's coordinate system to the camera's. This means applying
171     * the transformation to the target point (0,0,0) for example would give the target's center
172     * relative to the camera. Conveniently, if we make a translation-rotation transformation out of
173     * these components instead, we get the transformation from the camera to the target.
174     *
175     * @param cameraToTarget3d A camera-to-target Transform3d in EDN.
176     * @return A camera-to-target Transform3d in NWU.
177     */
178    public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
179        // TODO: Refactor into new pipe?
180        return CoordinateSystem.convert(
181                cameraToTarget3d, CoordinateSystem.EDN(), CoordinateSystem.NWU());
182    }
183
184    /*
185     * From the AprilTag repo:
186     * "The coordinate system has the origin at the camera center. The z-axis points from the camera
187     * center out the camera lens. The x-axis is to the right in the image taken by the camera, and
188     * y is down. The tag's coordinate frame is centered at the center of the tag, with x-axis to the
189     * right, y-axis down, and z-axis into the tag."
190     *
191     * This means our detected transformation will be in EDN. Our subsequent uses of the transformation,
192     * however, assume the tag's z-axis point away from the tag instead of into it. This means we
193     * have to correct the transformation's rotation.
194     */
195    private static final Rotation3d APRILTAG_BASE_ROTATION =
196            new Rotation3d(VecBuilder.fill(0, 1, 0), Units.degreesToRadians(180));
197
198    /**
199     * AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag
200     * instead of away from it and towards the camera. This means we have to correct the
201     * transformation's rotation.
202     *
203     * @param pose The Transform3d with translation and rotation directly from the {@link
204     *     AprilTagPoseEstimate}.
205     */
206    public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
207        var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
208        return new Transform3d(pose.getTranslation(), ocvRotation);
209    }
210
211    public static Pose3d convertArucotoOpenCV(Transform3d pose) {
212        var ocvRotation =
213                APRILTAG_BASE_ROTATION.rotateBy(
214                        new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180))
215                                .rotateBy(pose.getRotation()));
216        return new Pose3d(pose.getTranslation(), ocvRotation);
217    }
218
219    public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
220        var angle = rotation.getAngle();
221        var axis = rotation.getAxis().times(angle);
222        rvecOutput.put(0, 0, axis.getData());
223    }
224
225    /**
226     * Convert an Opencv rvec+tvec pair to a Pose3d.
227     *
228     * @param rVec Axis-angle rotation vector, where norm(rVec) is the angle about a unit vector in
229     *     the direction of rVec
230     * @param tVec 3D translation
231     * @return Pose3d representing the same rigid transform
232     */
233    public static Pose3d opencvRTtoPose3d(Mat rVec, Mat tVec) {
234        Translation3d translation =
235                new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
236        Rotation3d rotation =
237                new Rotation3d(
238                        VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]),
239                        Core.norm(rVec));
240
241        return new Pose3d(translation, rotation);
242    }
243}