User:Jdev/Code/Fast wall determining

From Robowiki
Jump to navigation Jump to search

While optimising Tomcat i find out, may be, a quickest way to determine forward wall. This code uses Tomcat's specific classes, but it can be easy adopted to any robot. QuickMath it is my modification of User:Rednaxela/FastTrig. May be this code will help someone.


    public Wall getWall(LXXPoint pos, double heading) {
        final double normalHeadingTg = QuickMath.tan(heading % LXXConstants.RADIANS_90);
        if (heading < LXXConstants.RADIANS_90) {
            final double rightTopTg = (rightTop.x - pos.x) / (rightTop.y - pos.y);
            if (normalHeadingTg < rightTopTg) {
                return top;
            } else {
                return right;
            }
        } else if (heading < LXXConstants.RADIANS_180) {
            final double rightBottomTg = pos.y / (rightBottom.x - pos.x);
            if (normalHeadingTg < rightBottomTg) {
                return right;
            } else {
                return bottom;
            }
        } else if (heading < LXXConstants.RADIANS_270) {
            final double leftBottomTg = pos.x / pos.y;
            if (normalHeadingTg < leftBottomTg) {
                return bottom;
            } else {
                return left;
            }
        } else if (heading < LXXConstants.RADIANS_360) {
            final double leftTopTg = (leftTop.y - pos.y) / pos.x;
            if (normalHeadingTg < leftTopTg) {
                return left;
            } else {
                return top;
            }
        }
        throw new IllegalArgumentException("Invalid heading: " + heading);
    }