User:Chase-san/GeomTools

My set of geometry based tools.

import java.awt.geom.*;
import java.util.ArrayList;

/**
 * I honestly wrote all these from scratch!
 * 
 * @author Chase
 */
public final class GeomTools {
	private GeomTools() {}
	public static final Line2D[] rectToLines(Rectangle2D rect) {
		Line2D[] lines = new Line2D[4];
		//Left, Right, Bottom, Top
		lines[0] = new Line2D.Double(rect.getMinX(),rect.getMinY(),rect.getMinX(),rect.getMaxY());
		lines[1] = new Line2D.Double(rect.getMaxX(),rect.getMinY(),rect.getMaxX(),rect.getMaxY());
		lines[2] = new Line2D.Double(rect.getMinX(),rect.getMinY(),rect.getMaxX(),rect.getMinY());
		lines[3] = new Line2D.Double(rect.getMinX(),rect.getMaxY(),rect.getMaxX(),rect.getMaxY());
		return lines;
	}
	/**
	 * Gets intersections between a rectangle and a line 
	 * @param rect Rectangle
	 * @param line Line
	 * @return The points that it intesects with, or a zero length array if
	 * 		no intersections occur.
	 */
	public static final Point2D[] intersectRectLine(Rectangle2D rect, Line2D line) {
		ArrayList<Point2D> points = new ArrayList<Point2D>();
		Line2D[] r = rectToLines(rect);
		for(Line2D edge : r) {
			Point2D sec = intersectSegLine(edge, line);
			if(sec != null)
				points.add(sec);
		}
		Point2D[] ps = new Point2D[points.size()];
		points.toArray(ps);
		return ps;
	}
	
	/**
	 * Gets intersections between a rectangle and a line 
	 * @param rect Rectangle
	 * @param line Line
	 * @return The points that it intesects with, or a zero length array if
	 * 		no intersections occur.
	 */
	public static final Point2D[] intersectRectCircle(Rectangle2D rect, Point2D c, double r) {
		ArrayList<Point2D> points = new ArrayList<Point2D>();
		Line2D[] rct = rectToLines(rect);
		for(Line2D edge : rct) {
			Point2D sec[] = intersectCircleSeg(c, r, edge);
			for(Point2D p : sec)
				points.add(p);
		}
		Point2D[] ps = new Point2D[points.size()];
		points.toArray(ps);
		return ps;
	}
	
	/**
	 * Gets intersections between a rectangle and a line segment
	 * @param rect Rectangle
	 * @param line Line Segment
	 * @return The points that it intesects with, or a zero length array if
	 * 		no intersections occur.
	 */
	public static final Point2D[] intersectRectSeg(Rectangle2D rect, Line2D line) {
		ArrayList<Point2D> points = new ArrayList<Point2D>();
		Line2D[] r = rectToLines(rect);
		for(Line2D edge : r) {
			Point2D sec = intersectSegSeg(edge, line);
			if(sec != null) {
				points.add(sec);
			}
		}
		Point2D[] ps = new Point2D[points.size()];
		points.toArray(ps);
		return ps;
	}
	public static final Point2D intersectLineLine(Line2D p1, Line2D p2) {
		double dom = (p2.getY2()-p2.getY1())*(p1.getX2()-p1.getX1())
					-(p2.getX2()-p2.getX1())*(p1.getY2()-p1.getY1());
		double ua = (p2.getX2()-p2.getX1())*(p1.getY1()-p2.getY1())
					-(p2.getY2()-p2.getY1())*(p1.getX1()-p2.getX1());
		//If dom is 0 then they are parallel
		if(Math.abs(dom) < 0.0001)
			return null;
		
		ua /= dom;

		Point2D P = new Point2D.Double(
				p1.getX1() + ua*(p1.getX2()-p1.getX1()),
				p1.getY1() + ua*(p1.getY2()-p1.getY1()));
		
		return P;
	}
	
	public static final Point2D intersectLineSeg(Line2D p1, Line2D p2) {
		double dom = (p2.getY2()-p2.getY1())*(p1.getX2()-p1.getX1())
					-(p2.getX2()-p2.getX1())*(p1.getY2()-p1.getY1());
		double ub = (p1.getX2()-p1.getX1())*(p1.getY1()-p2.getY1())
					-(p1.getY2()-p1.getY1())*(p1.getX1()-p2.getX1());
		//If dom is 0 then they are parallel
		//If ua and ub are also 0, then they are coincident
		if(Math.abs(dom) < 0.0001)
			return null;
		ub /= dom;
		if(ub >= 0.0 && ub <= 1.0) {
			Point2D P = new Point2D.Double(
					p1.getX1() + ub*(p1.getX2()-p1.getX1()),
					p1.getY1() + ub*(p1.getY2()-p1.getY1()));
			return P;
		}
		return null;
	}
	
	public static final Point2D intersectSegLine(Line2D p1, Line2D p2) {
		double dom = (p2.getY2()-p2.getY1())*(p1.getX2()-p1.getX1())
					-(p2.getX2()-p2.getX1())*(p1.getY2()-p1.getY1());
		double ua = (p2.getX2()-p2.getX1())*(p1.getY1()-p2.getY1())
					-(p2.getY2()-p2.getY1())*(p1.getX1()-p2.getX1());
		//If dom is 0 then they are parallel
		//If ua and ub are also 0, then they are coincident
		if(Math.abs(dom) < 0.0001)
			return null;
		ua /= dom;
		if(ua >= 0.0 && ua <= 1.0) {
			Point2D P = new Point2D.Double(
					p1.getX1() + ua*(p1.getX2()-p1.getX1()),
					p1.getY1() + ua*(p1.getY2()-p1.getY1()));
			return P;
		}
		return null;
	}
	
	public static final Point2D intersectSegSeg(Line2D p1, Line2D p2) {
		double dom = (p2.getY2()-p2.getY1())*(p1.getX2()-p1.getX1())
					-(p2.getX2()-p2.getX1())*(p1.getY2()-p1.getY1());
		double ua = (p2.getX2()-p2.getX1())*(p1.getY1()-p2.getY1())
					-(p2.getY2()-p2.getY1())*(p1.getX1()-p2.getX1());
		double ub = (p1.getX2()-p1.getX1())*(p1.getY1()-p2.getY1())
					-(p1.getY2()-p1.getY1())*(p1.getX1()-p2.getX1());
		//If dom is 0 then they are parallel
		//If ua and ub are also 0, then they are coincident
		if(Math.abs(dom) < 0.0001)
			return null;
		ua /= dom;
		ub /= dom;
		if(ua >= 0.0 && ua <= 1.0
		&& ub >= 0.0 && ub <= 1.0) {
			
			Point2D P = new Point2D.Double(
					p1.getX1() + ua*(p1.getX2()-p1.getX1()),
					p1.getY1() + ua*(p1.getY2()-p1.getY1()));
			return P;
		}
		return null;
	}
	
	public static final Point2D[] intersectCircleLine(Point2D c, double r, Line2D p1) {
		double dx = c.getX() - p1.getX1();
		double dy = c.getY() - p1.getY1();
		
		double dirx = p1.getX2() - p1.getX1();
		double diry = p1.getY2() - p1.getY1();
		double a0 = Math.sqrt(dirx*dirx+diry*diry);
		dirx /= a0;
		diry /= a0;

		a0 = dx*dx + dy*dy - r*r;
		double a1 = dx*dirx + dy*diry;
		double discr = a1*a1 - a0;
		
		double m1 = 0;
		double m2 = 0;
		
		if (discr > 0) {
			discr = Math.sqrt(discr);
			m1 = a1 - discr;
			m2 = a1 + discr;
			return new Point2D[]{
	        		new Point2D.Double( p1.getX1() + m1*dirx, p1.getY1() + m1*diry ),
	        		new Point2D.Double( p1.getX1() + m2*dirx, p1.getY1() + m2*diry )
	        	};
		} else if(discr == 0) {
			m1 = a1;
			return new Point2D[]{
	        		new Point2D.Double( p1.getX1() + m1*dirx, p1.getY1() + m1*diry )
	        	};
		}
		return new Point2D[0];
	}
	
	public static final Point2D[] intersectCircleSeg(Point2D c, double r, Line2D p1) {
		double dx = c.getX() - p1.getX1();
		double dy = c.getY() - p1.getY1();
		
		double dirx = p1.getX2() - p1.getX1();
		double diry = p1.getY2() - p1.getY1();
		double a0 = Math.sqrt(dirx*dirx+diry*diry);
		dirx /= a0;
		diry /= a0;
		double length = p1.getP1().distance(p1.getP2());

		a0 = dx*dx + dy*dy - r*r;
		double a1 = dx*dirx + dy*diry;
		double discr = a1*a1 - a0;
		
		double m1 = 0;
		double m2 = 0;
		
		if (discr > 0) {
			discr = Math.sqrt(discr);
			m1 = a1 - discr;
			m2 = a1 + discr;
			if(m1 > 0 && m1 < length && m2 > 0 && m2 < length)
		        	return new Point2D[]{
	        			new Point2D.Double( p1.getX1() + m1*dirx, p1.getY1() + m1*diry ),
	        			new Point2D.Double( p1.getX1() + m2*dirx, p1.getY1() + m2*diry )
		        	};
		        else if(m1 > 0 && m1 < length)
		        	return new Point2D[]{
	        			new Point2D.Double( p1.getX1() + m1*dirx, p1.getY1() + m1*diry ),
	        		};
		        else if(m2 > 0 && m2 < length)
		        	return new Point2D[]{
			        	new Point2D.Double( p1.getX1() + m2*dirx, p1.getY1() + m2*diry ),
			        };
		} else if(discr == 0) {
			//Its a tangent
			m1 = a1;
			if(m1 > 0 && m1 < length)
				return new Point2D[]{
	        			new Point2D.Double( p1.getX1() + m1*dirx, p1.getY1() + m1*diry )
	        		};
		}
		return new Point2D[0];
	}
	
	public static final boolean testCircle(Point2D s1, double r1, Point2D s2, double r2) {
		return s1.distanceSq(s2) < r1*r1+r2*r2; 
	}
	
	/**
	 * Tests two moving circles, with directional vectors <s1x,s1y> and <s2x,s2y>
	 */
	public static final boolean testCircle(Point2D s1, double r1, Point2D s2, double r2,
			double s1x, double s1y, double s2x, double s2y, int time) {
		double rVx = s1x - s2x;
		double rVy = s1y - s2y;
		double a = rVx*rVx + rVy*rVy;
		double dx = s1.getX() - s2.getX();
		double dy = s1.getY() - s2.getY();
		double c = dx*dx+dy*dy;
		double rSum = r1 + r2;
		double rSumSqr = rSum * rSum;
		if(a > 0.0) {
			double b = dx*rVx+dy*rVy;
			if(b <= 0.0) {
				if(-time * a <= b)
					return a * c - b * b <= a * rSumSqr;
				return time * (time * a + 2.0 * b) + c <= rSumSqr;
			}
		}
		return c <= rSumSqr;
	}
	
	public static final Point2D[] getCorners(Rectangle2D rect) {
		Point2D[] corner = new Point2D[4];
		corner[0] = new Point2D.Double(rect.getMinX(),rect.getMinY());
		corner[1] = new Point2D.Double(rect.getMinX(),rect.getMaxY());
		corner[2] = new Point2D.Double(rect.getMaxX(),rect.getMinY());
		corner[3] = new Point2D.Double(rect.getMaxX(),rect.getMaxY());
		return corner;
	}
	
	public static final Point2D between(Point2D p1, Point2D p2) {
		double x = p1.getX() + p2.getX();
		double y = p1.getY() + p2.getY();
		return new Point2D.Double(x/2.0,y/2.0);
	}
}

New Geometry Tools

	public static final double[] intersectRectCircle(
			double rx, double ry, double rw, double rh,
			double cx, double cy, double r) {
		double mx = rx+rw;
		double my = ry+rh;

		//every line can intersect twice, meaning 4 points at most per line
		double[] intersect = new double[16];
		int n = 0;

		double[] in = intersectSegCircle(cx,cy,r,rx,ry,mx,ry); //top
		for(int i=0;i!=in.length;++i)
			intersect[n++] = in[i];

		in = intersectSegCircle(cx,cy,r,rx,my,mx,my); //bottom
		for(int i=0;i!=in.length;++i)
			intersect[n++] = in[i];

		in = intersectSegCircle(cx,cy,r,rx,ry,rx,my); //left
		for(int i=0;i!=in.length;++i)
			intersect[n++] = in[i];

		in = intersectSegCircle(cx,cy,r,mx,ry,mx,my); //right
		for(int i=0;i!=in.length;++i)
			intersect[n++] = in[i];

		double[] output = new double[n];
		for(int i=0;i!=n;++i)
			output[i] = intersect[i];

		return output;
	}

	public static final double[] intersectSegCircle(double cx, double cy, double r,
			double lax, double lay, double lbx, double lby) {

		double diffx = cx - lax;
		double diffy = cy - lay;

		double dirx = lbx-lax;
		double diry = lby-lay;
		double l = Math.sqrt(dirx*dirx + diry*diry);

		dirx /= l;
		diry /= l;

		double a0 = diffx*diffx+diffy*diffy - r*r;
		double a1 = diffx*dirx+diffy*diry;

		double discr = a1 * a1 - a0;

		if (discr > 0) {
			/* The circle and line meet at two places */
			double lengthSq = (lbx-lax)*(lbx-lax)+(lby-lay)*(lby-lay);

			discr = Math.sqrt(discr);
			double m1 = a1 - discr;
			double m2 = a1 + discr;

			if(m1 > 0 && m1*m1 < lengthSq && m2 > 0 && m2*m2 < lengthSq) {
				return new double[] {
						lax + m1 * dirx, lay + m1 * diry,
						lax + m2 * dirx, lay + m2 * diry
				};
			} else if (m1 > 0 && m1*m1 < lengthSq) {
				return new double[] {
						lax + m1 * dirx, lay + m1 * diry
				};
			} else if (m2 > 0 && m2*m2 < lengthSq) {
				return new double[] {
						lax + m2 * dirx, lay + m2 * diry
				};
			}
		} else if (discr == 0) {
			double lengthSq = (lbx-lax)*(lbx-lax)+(lby-lay)*(lby-lay);
			/* We have ourselves a tangent */
			if (a1 > 0 && a1*a1 < lengthSq) {
				return new double[] {
						lax+a1*dirx, lay+a1*diry
				};
			}
		}

		return new double[0];
	}