001/** 002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868 003 * Open Source Software; you may modify and/or share it under the terms of 004 * the 3-Clause BSD License found in the root directory of this project. 005 */ 006 007package tagalong.math; 008 009import edu.wpi.first.math.geometry.Pose2d; 010import edu.wpi.first.math.geometry.Rotation2d; 011 012/** 013 * Utility functions for geometric math like finding if a location is within a 014 * given 2d-shapes. 015 */ 016public class GeometricUtils { 017 /** 018 * Checks if current position is in range of the target 019 * 020 * @param curPose current position of the robot 021 * @param yaw current yaw (position of robot with z-axis) 022 * @param targetPose target position 023 * @param xyToleranceM tolerance for x and y in meters 024 * @param thetaToleranceDeg Tolerance in degrees 025 * @return True if the current pose is in the specified range 026 */ 027 public static boolean inRange( 028 Pose2d curPose, 029 Rotation2d yaw, 030 Pose2d targetPose, 031 double xyToleranceM, 032 double thetaToleranceDeg 033 ) { 034 double thetaDiff = targetPose.getRotation().getDegrees() - yaw.getDegrees(); 035 return Math.abs(targetPose.getX() - curPose.getX()) <= xyToleranceM 036 && Math.abs(targetPose.getY() - curPose.getY()) <= xyToleranceM 037 && Math.abs(thetaDiff > 180 ? thetaDiff - 360 : thetaDiff) <= thetaToleranceDeg; 038 } 039 040 /** 041 * @param X X-coordinate of the point to check 042 * @param Y Y-coordinate of the point to check 043 * @param T1X X-coordinate of the first vertex 044 * @param T1Y Y-coordinate of the first vertex 045 * @param T2X X-coordinate of the second vertex 046 * @param T2Y Y-coordinate of the second vertex 047 * @param T3X X-coordinate of the third vertex 048 * @param T3Y Y-coordinate of the third vertex 049 * @return Whether or not (X, Y) falls within the triangle specified 050 */ 051 public static boolean inTriangle( 052 double X, double Y, double T1X, double T1Y, double T2X, double T2Y, double T3X, double T3Y 053 ) { 054 double t1yMt2y = T1Y - T2Y; 055 double t1xMt2x = T1X - T2X; 056 double t2yMt3y = T2Y - T3Y; 057 double t2xMt3x = T2X - T3X; 058 double t1yMt3y = T1Y - T3Y; 059 double t1xMt3x = T1X - T3X; 060 061 double Slope12 = t1yMt2y / t1xMt2x; 062 double Slope23 = t2yMt3y / t2xMt3x; 063 double Slope13 = t1yMt3y / t1xMt3x; 064 065 // check whether to shade up or down. True is up and False is down 066 boolean isInequality12Up = t1yMt3y > Slope12 * t1xMt3x; 067 boolean isInequality23Up = t1yMt2y > Slope23 * t1xMt2x; 068 boolean isInequality13Up = t2yMt3y > Slope13 * t2xMt3x; 069 070 // check whether coordinate is in determined shaded region 071 double comparisonValue12 = (Slope12 * (X - T1X)) - (Y - T1Y); 072 double comparisonValue23 = (Slope23 * (X - T2X)) - (Y - T2Y); 073 double comparisonValue13 = (Slope13 * (X - T3X)) - (Y - T3Y); 074 075 boolean checkLine12 = isInequality12Up ? 0 > comparisonValue12 : 0 < comparisonValue12; 076 boolean checkLine23 = isInequality23Up ? 0 > comparisonValue23 : 0 < comparisonValue23; 077 boolean checkLine13 = isInequality13Up ? 0 > comparisonValue13 : 0 < comparisonValue13; 078 079 return checkLine13 && checkLine23 && checkLine12; 080 } 081 082 /** 083 * 084 * @param X X-coordinate of the point to check 085 * @param Y Y-coordinate of the point to check 086 * @param R1X X-coordinate of the first vertex 087 * @param R1Y Y-coordinate of the first vertex 088 * @param R2X X-coordinate of the second vertex 089 * @param R2Y Y-coordinate of the second vertex 090 * @param R3X X-coordinate of the third vertex 091 * @param R3Y Y-coordinate of the third vertex 092 * @param R4X X-coordinate of the fourth vertex 093 * @param R4Y Y-coordinate of the fourth vertex 094 * @return Whether or not (X, Y) falls within the rectangle specified 095 */ 096 public static boolean inRectangle( 097 double X, 098 double Y, 099 double R1X, 100 double R1Y, 101 double R2X, 102 double R2Y, 103 double R3X, 104 double R3Y, 105 double R4X, 106 double R4Y 107 ) { 108 double r1yMr2y = R1Y - R2Y; // 1 and 2 109 double r1xMr2x = R1X - R2X; 110 double r2yMr3y = R2Y - R3Y; // 2 and 3 111 double r2xMr3x = R2X - R3X; 112 double r3yMr4y = R3Y - R4Y; // 3 and 4 113 double r3xMr4x = R3X - R4X; 114 double r1yMr4y = R1Y - R4Y; // 1 and 4 115 double r1xMr4x = R1X - R4X; 116 117 double Slope12 = r1yMr2y / r1xMr2x; 118 double Slope23 = r2yMr3y / r2xMr3x; 119 double Slope34 = r3yMr4y / r3xMr4x; 120 double Slope14 = r1yMr4y / r1xMr4x; 121 122 // check whether to shade up or down 123 boolean isInequality12Up = r2yMr3y > Slope12 * r2xMr3x; 124 boolean isInequality23Up = r3yMr4y > Slope23 * r3xMr4x; 125 boolean isInequality34Up = r1yMr4y > Slope34 * r1xMr4x; 126 boolean isInequality14Up = r1yMr2y > Slope14 * r1xMr2x; 127 128 // check whether coordinate is in shaded range 129 double comparisonValue12 = Slope12 * (X - R1X) - (Y - R1Y); 130 double comparisonValue23 = Slope23 * (X - R2X) - (Y - R2Y); 131 double comparisonValue34 = Slope34 * (X - R3X) - (Y - R3Y); 132 double comparisonValue14 = Slope14 * (X - R4X) - (Y - R4Y); 133 134 boolean checkLine12 = isInequality12Up ? 0 > comparisonValue12 : 0 < comparisonValue12; 135 boolean checkLine23 = isInequality23Up ? 0 > comparisonValue23 : 0 < comparisonValue23; 136 boolean checkLine34 = isInequality34Up ? 0 > comparisonValue34 : 0 < comparisonValue34; 137 boolean checkLine14 = isInequality14Up ? 0 > comparisonValue14 : 0 < comparisonValue14; 138 139 return checkLine12 && checkLine23 && checkLine34 && checkLine14; 140 } 141}