us.ihmc.utilities.math.geometry
Class GeometryTools

java.lang.Object
  us.ihmc.utilities.math.geometry.GeometryTools

public class GeometryTools
extends java.lang.Object

Constructor Summary

GeometryTools()

Method Summary

static javax.vecmath.Point2d	averagePoints(java.util.ArrayList<javax.vecmath.Point2d> points) Averages the 2D points in a given Array
static javax.vecmath.Point3d	averagePoints(java.util.ArrayList<javax.vecmath.Point3d> points) Averages the 3D points in an Array
static javax.vecmath.Point3d	averagePoints(javax.vecmath.Point3d a, javax.vecmath.Point3d b) Returns the average of two 3D points
static javax.media.j3d.BranchGroup	createCube(javax.vecmath.Vector3f thisColor, javax.vecmath.Point3d position, double size) Creates a Cube given size, color, and postion
static double	distanceBetweenPoints(double[] a, double[] b) Calculates distance between two Double points, a and b.
static double	distanceFromPointToLine(javax.vecmath.Point2d point, javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd) Returns the minimum distance between a 2D point and an infinitely long 2D line defined by a given line segment.
static double	distanceFromPointToLine(javax.vecmath.Point3d point, javax.vecmath.Point3d lineStart, javax.vecmath.Point3d lineEnd) Compute the distance from a point to a line (defined by two 3D points).
static double	distanceFromPointToLineSegment(javax.vecmath.Point2d point, javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd) Returns the minimum distance between a point and a given line segment.
static boolean	doLineSegmentsIntersect(javax.vecmath.Point2d lineStart1, javax.vecmath.Point2d lineEnd1, javax.vecmath.Point2d lineStart2, javax.vecmath.Point2d lineEnd2)
static double	getAngleFromFirstToSecondVector(javax.vecmath.Vector2d firstVector, javax.vecmath.Vector2d secondVector)
static void	getClosestPointsForTwoLines(FramePoint point1, FrameVector vector1, FramePoint point2, FrameVector vector2, FramePoint pointOnLine1, FramePoint pointOnLine2)
static javax.vecmath.Point2d	getClosestPointToLineSegment(javax.vecmath.Point2d testPoint, javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd) Let the test point be C (Cx,Cy) and the line be AB (Ax,Ay) to (Bx,By).
static FramePoint	getIntersectionBetweenLineAndPlane(FramePoint pointOnPlane, FrameVector planeNormal, FramePoint lineStart, FramePoint lineEnd) Locates and returns the intersection between the given line and plane
static javax.vecmath.Point2d	getIntersectionBetweenTwoLines(javax.vecmath.Point2d lineStart1, javax.vecmath.Point2d lineEnd1, javax.vecmath.Point2d lineStart2, javax.vecmath.Point2d lineEnd2)
static javax.vecmath.Point2d	getIntersectionBetweenTwoLines(javax.vecmath.Point2d point1, javax.vecmath.Vector2d vector1, javax.vecmath.Point2d point2, javax.vecmath.Vector2d vector2) Finds the intersection between two 2D lines.
static double[]	getLineSegmentPercentagesIfIntersecting(javax.vecmath.Point2d lineStart1, javax.vecmath.Point2d lineEnd1, javax.vecmath.Point2d lineStart2, javax.vecmath.Point2d lineEnd2) Returns the line segment percentages of the intersection point between two lines if the lines are intersecting and not colinear.
static javax.vecmath.Vector3d	getNormalToPlane(javax.vecmath.Point3d a, javax.vecmath.Point3d b, javax.vecmath.Point3d c) Returns the normal of a Plane specified by three points If 2 or more points are the same, returns NaN
static javax.vecmath.Point2d	getOrthogonalProjectionOnLine(javax.vecmath.Point2d testPoint, javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd)
static void	getPerpendicularBisector(javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd, javax.vecmath.Point2d bisectorStart, javax.vecmath.Vector2d bisectorDirection)
static javax.vecmath.Vector2d	getPerpendicularVector(javax.vecmath.Vector2d vector)
static FrameVector	getPerpendicularVectorFromLineToPoint(FramePoint point, FramePoint lineStart, FramePoint lineEnd, FramePoint intersectionPoint) Returns the Perpendicular Vector that is formed between a given Line (defined by two points) and a given point Returns zeros if point is located on the line
static FrameVector	getPlaneNormalGivenThreePoints(FramePoint point1, FramePoint point2, FramePoint point3) Returns the Normal of a plane that is defined by three points Returns a null if three points are linear, colinear, or equal Returns a null if two points are the same
static javax.vecmath.Vector3d	getPlaneNormalGivenThreePoints(javax.vecmath.Point3d point1, javax.vecmath.Point3d point2, javax.vecmath.Point3d point3) Converts three 3D points into 3 framePoints (defines a plane) Calls getPlaneNormalGivenThreePoints Returns normal to plane Returns null if three points are: linear colinear equal if 2 points are equal
static Orientation	getTransform(FramePoint point, FrameVector normal)
static javax.vecmath.Point2d	getTriangleBisector(javax.vecmath.Point2d A, javax.vecmath.Point2d B, javax.vecmath.Point2d C) This method returns the point representing where the bisector of an angle of a triangle intersects the opposite side.
static java.util.ArrayList<javax.vecmath.Point3d>	getVerticalSpansOfPoints(double xMin, double yMin, double zMin, double xMax, double yMax, double zMax, double xResolution, double yResolution, double zResolution)
static double	getXYDistance(FramePoint point1, FramePoint point2)
static double	getXYDistance(javax.vecmath.Point3d point1, javax.vecmath.Point3d point2)
static void	getZPlanePerpendicularBisector(FramePoint lineStart, FramePoint lineEnd, FramePoint bisectorStart, FrameVector bisectorDirection)
static boolean	isInsidePolygon(javax.vecmath.Point2d p, java.util.ArrayList<javax.vecmath.Point2d> polygon)
static boolean	isInsidePolygon(javax.vecmath.Point2d point, javax.vecmath.Point2d[] orderedPolygonPoints) Checks to make sure a point lies on the same side of the line for all lines going around a polygon; if true, then the point is inside the polygon.
static boolean	isInsidePolygon2(javax.vecmath.Point2d p, javax.vecmath.Point2d[] polygon)
static boolean	isPointOnLeftSideOfLine(FramePoint point, FramePoint lineStart, FramePoint lineEnd) Returns a Boolean value stating whether a point is on the left side of a given line
static boolean	isPointOnLeftSideOfLine(javax.vecmath.Point2d point, javax.vecmath.Point2d lineStart, javax.vecmath.Point2d lineEnd) Returns a boolean value, stating whether a 2D point is on the left side of a given line "Left side" is determined based on order of lineStart and lineEnd returns false if point is on line
static void	main(java.lang.String[] args)

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

GeometryTools

public GeometryTools()

Method Detail

distanceFromPointToLine

public static double distanceFromPointToLine(javax.vecmath.Point3d point,
                                             javax.vecmath.Point3d lineStart,
                                             javax.vecmath.Point3d lineEnd)

Compute the distance from a point to a line (defined by two 3D points). From http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html ^^Modified to return distance if line is defined by same point^^ returns distance between given 3D point and line(point)

Parameters:

point - Point3d

lineStart - Point3d

lineEnd - Point3d

Returns:

double

distanceFromPointToLine

public static double distanceFromPointToLine(javax.vecmath.Point2d point,
                                             javax.vecmath.Point2d lineStart,
                                             javax.vecmath.Point2d lineEnd)

Returns the minimum distance between a 2D point and an infinitely long 2D line defined by a given line segment. If line is defined by the same point, returns distance between that point and the given 2D point

Parameters:

point - Point2d

lineStart - Point2d

lineEnd - Point2d

Returns:

double

distanceFromPointToLineSegment

public static double distanceFromPointToLineSegment(javax.vecmath.Point2d point,
                                                    javax.vecmath.Point2d lineStart,
                                                    javax.vecmath.Point2d lineEnd)

Returns the minimum distance between a point and a given line segment. holds true if line segment shrinks to a single point

Parameters:

point - Point2d

lineStart - Point2d

lineEnd - Point2d

Returns:

double

isPointOnLeftSideOfLine

public static boolean isPointOnLeftSideOfLine(javax.vecmath.Point2d point,
                                              javax.vecmath.Point2d lineStart,
                                              javax.vecmath.Point2d lineEnd)

Returns a boolean value, stating whether a 2D point is on the left side of a given line "Left side" is determined based on order of lineStart and lineEnd returns false if point is on line

Parameters:

point - Point2d

lineStart - Point2d

lineEnd - Point2d

Returns:

boolean

isPointOnLeftSideOfLine

public static boolean isPointOnLeftSideOfLine(FramePoint point,
                                              FramePoint lineStart,
                                              FramePoint lineEnd)

Returns a Boolean value stating whether a point is on the left side of a given line

Parameters:

point - FramePoint

lineStart - FramePoint

lineEnd - FramePoint

Returns:

boolean

averagePoints

public static javax.vecmath.Point2d averagePoints(java.util.ArrayList<javax.vecmath.Point2d> points)

Averages the 2D points in a given Array

Parameters:

points - ArrayList

Returns:

Point2d

averagePoints

public static javax.vecmath.Point3d averagePoints(java.util.ArrayList<javax.vecmath.Point3d> points)

Averages the 3D points in an Array

Parameters:

points - ArrayList

Returns:

Point3d

averagePoints

public static javax.vecmath.Point3d averagePoints(javax.vecmath.Point3d a,
                                                  javax.vecmath.Point3d b)

Returns the average of two 3D points

Parameters:

a - Point3d

b - Point3d

Returns:

Point3d

isInsidePolygon

public static boolean isInsidePolygon(javax.vecmath.Point2d point,
                                      javax.vecmath.Point2d[] orderedPolygonPoints)

Checks to make sure a point lies on the same side of the line for all lines going around a polygon; if true, then the point is inside the polygon. The polygon vertices must be ordered so that each pair of consecutive vertices defines a side of the polygon.

Parameters:

point - Point2d

orderedPolygonPoints - Point2d[]

Returns:

boolean

isInsidePolygon2

public static boolean isInsidePolygon2(javax.vecmath.Point2d p,
                                       javax.vecmath.Point2d[] polygon)

Parameters:

p - Point2d

polygon - Point2d[]

Returns:

boolean

isInsidePolygon

public static boolean isInsidePolygon(javax.vecmath.Point2d p,
                                      java.util.ArrayList<javax.vecmath.Point2d> polygon)

getOrthogonalProjectionOnLine

public static javax.vecmath.Point2d getOrthogonalProjectionOnLine(javax.vecmath.Point2d testPoint,
                                                                  javax.vecmath.Point2d lineStart,
                                                                  javax.vecmath.Point2d lineEnd)

getClosestPointsForTwoLines

public static void getClosestPointsForTwoLines(FramePoint point1,
                                               FrameVector vector1,
                                               FramePoint point2,
                                               FrameVector vector2,
                                               FramePoint pointOnLine1,
                                               FramePoint pointOnLine2)

Parameters:

point1 - FramePoint

vector1 - FrameVector

point2 - FramePoint

vector2 - FrameVector

pointOnLine1 - FramePoint

pointOnLine2 - FramePoint

getIntersectionBetweenLineAndPlane

public static FramePoint getIntersectionBetweenLineAndPlane(FramePoint pointOnPlane,
                                                            FrameVector planeNormal,
                                                            FramePoint lineStart,
                                                            FramePoint lineEnd)

Locates and returns the intersection between the given line and plane

Parameters:

pointOnPlane - FramePoint

planeNormal - FrameVector

lineStart - FramePoint

lineEnd - FramePoint

Returns:

FramePoint

doLineSegmentsIntersect

public static boolean doLineSegmentsIntersect(javax.vecmath.Point2d lineStart1,
                                              javax.vecmath.Point2d lineEnd1,
                                              javax.vecmath.Point2d lineStart2,
                                              javax.vecmath.Point2d lineEnd2)

getIntersectionBetweenTwoLines

public static javax.vecmath.Point2d getIntersectionBetweenTwoLines(javax.vecmath.Point2d lineStart1,
                                                                   javax.vecmath.Point2d lineEnd1,
                                                                   javax.vecmath.Point2d lineStart2,
                                                                   javax.vecmath.Point2d lineEnd2)

getIntersectionBetweenTwoLines

public static javax.vecmath.Point2d getIntersectionBetweenTwoLines(javax.vecmath.Point2d point1,
                                                                   javax.vecmath.Vector2d vector1,
                                                                   javax.vecmath.Point2d point2,
                                                                   javax.vecmath.Vector2d vector2)

Finds the intersection between two 2D lines. Each line is represented as a Point2d and a Vector2d. This should work as long as the two lines are not parallel. If they are parallel, it tries to return something without crashing.

Parameters:

point1 - Start Point of first line.

vector1 - Direction Vector of first line.

point2 - Start Point of second line.

vector2 - Direction Vector of second line.

Returns:

Point of Intersection.

getLineSegmentPercentagesIfIntersecting

public static double[] getLineSegmentPercentagesIfIntersecting(javax.vecmath.Point2d lineStart1,
                                                               javax.vecmath.Point2d lineEnd1,
                                                               javax.vecmath.Point2d lineStart2,
                                                               javax.vecmath.Point2d lineEnd2)

Returns the line segment percentages of the intersection point between two lines if the lines are intersecting and not colinear. If colinear, or parallel, then returns null. This is epsilon conservative in determining parallelness or colinearity. If just slightly not parallel, will still return null.

Parameters:

lineStart1 - Point2d

lineEnd1 - Point2d

lineStart2 - Point2d

lineEnd2 - Point2d

Returns:

Intersecting percentages of the two line segments if intersecting and not colinear. Null if not intersecting, or colinear.

getPlaneNormalGivenThreePoints

public static FrameVector getPlaneNormalGivenThreePoints(FramePoint point1,
                                                         FramePoint point2,
                                                         FramePoint point3)

Returns the Normal of a plane that is defined by three points Returns a null if three points are linear, colinear, or equal Returns a null if two points are the same

Parameters:

point1 - FramePoint

point2 - FramePoint

point3 - FramePoint

Returns:

FrameVector

getPerpendicularVectorFromLineToPoint

public static FrameVector getPerpendicularVectorFromLineToPoint(FramePoint point,
                                                                FramePoint lineStart,
                                                                FramePoint lineEnd,
                                                                FramePoint intersectionPoint)

Returns the Perpendicular Vector that is formed between a given Line (defined by two points) and a given point Returns zeros if point is located on the line

Parameters:

point - FramePoint

lineStart - FramePoint

lineEnd - FramePoint

intersectionPoint - FramePoint

Returns:

FrameVector

getPerpendicularVector

public static javax.vecmath.Vector2d getPerpendicularVector(javax.vecmath.Vector2d vector)

getPlaneNormalGivenThreePoints

public static javax.vecmath.Vector3d getPlaneNormalGivenThreePoints(javax.vecmath.Point3d point1,
                                                                    javax.vecmath.Point3d point2,
                                                                    javax.vecmath.Point3d point3)

Converts three 3D points into 3 framePoints (defines a plane) Calls getPlaneNormalGivenThreePoints Returns normal to plane Returns null if three points are: linear colinear equal if 2 points are equal

Parameters:

point1 - Point3d

point2 - Point3d

point3 - Point3d

Returns:

Vector3d

getZPlanePerpendicularBisector

public static void getZPlanePerpendicularBisector(FramePoint lineStart,
                                                  FramePoint lineEnd,
                                                  FramePoint bisectorStart,
                                                  FrameVector bisectorDirection)

getPerpendicularBisector

public static void getPerpendicularBisector(javax.vecmath.Point2d lineStart,
                                            javax.vecmath.Point2d lineEnd,
                                            javax.vecmath.Point2d bisectorStart,
                                            javax.vecmath.Vector2d bisectorDirection)

Parameters:

lineStart - Point2d

lineEnd - Point2d

bisectorStart - Point2d

bisectorDirection - Vector2d

getTransform

public static Orientation getTransform(FramePoint point,
                                       FrameVector normal)

getTriangleBisector

public static javax.vecmath.Point2d getTriangleBisector(javax.vecmath.Point2d A,
                                                        javax.vecmath.Point2d B,
                                                        javax.vecmath.Point2d C)

This method returns the point representing where the bisector of an angle of a triangle intersects the opposite side. Given a triangle defined by three points (A,B,C), To find the Bisector that divides the angle at B in half and intersects AC at X. BA AX -- = -- BC CX

Parameters:

A - Point2d

B - Point2d

C - Point2d

Returns:

Point2d the intersection point of the bisector with the opposite side

getAngleFromFirstToSecondVector

public static double getAngleFromFirstToSecondVector(javax.vecmath.Vector2d firstVector,
                                                     javax.vecmath.Vector2d secondVector)

createCube

public static javax.media.j3d.BranchGroup createCube(javax.vecmath.Vector3f thisColor,
                                                     javax.vecmath.Point3d position,
                                                     double size)

Creates a Cube given size, color, and postion

Parameters:

thisColor - Vector3f

position - Point3d

size - double

Returns:

BranchGroup

distanceBetweenPoints

public static double distanceBetweenPoints(double[] a,
                                           double[] b)

Calculates distance between two Double points, a and b.

Parameters:

a - double[]

b - double[]

Returns:

double

getClosestPointToLineSegment

public static javax.vecmath.Point2d getClosestPointToLineSegment(javax.vecmath.Point2d testPoint,
                                                                 javax.vecmath.Point2d lineStart,
                                                                 javax.vecmath.Point2d lineEnd)

Let the test point be C (Cx,Cy) and the line be AB (Ax,Ay) to (Bx,By). Let P be the point of perpendicular projection of C on AB. The parameter r, which indicates P's position along AB, is computed by the dot product of AC and AB divided by the square of the length of AB: AC dot AB r = --------- ||AB||^2 Let the scalar r represent the proportional distance of the projected point along the line. If r < 0, then lineStart is the closest point. If r > 1, then lineEnd is the closest point. If 0 < r < 1, then the closest point is between lineStart and lineEnd.

getXYDistance

public static double getXYDistance(FramePoint point1,
                                   FramePoint point2)

getXYDistance

public static double getXYDistance(javax.vecmath.Point3d point1,
                                   javax.vecmath.Point3d point2)

getVerticalSpansOfPoints

public static java.util.ArrayList<javax.vecmath.Point3d> getVerticalSpansOfPoints(double xMin,
                                                                                  double yMin,
                                                                                  double zMin,
                                                                                  double xMax,
                                                                                  double yMax,
                                                                                  double zMax,
                                                                                  double xResolution,
                                                                                  double yResolution,
                                                                                  double zResolution)

Parameters:

xMin - double

yMin - double

zMin - double

xMax - double

yMax - double

zMax - double

xResolution - double

yResolution - double

zResolution - double

Returns:

ArrayList

getNormalToPlane

public static javax.vecmath.Vector3d getNormalToPlane(javax.vecmath.Point3d a,
                                                      javax.vecmath.Point3d b,
                                                      javax.vecmath.Point3d c)

Returns the normal of a Plane specified by three points If 2 or more points are the same, returns NaN

Parameters:

a - Point3d

b - Point3d

c - Point3d

Returns:

Vector3d

main

public static void main(java.lang.String[] args)