us.ihmc.utilities.math.geometry
Class GeometryTools

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

public class GeometryTools
extends java.lang.Object


Constructor Summary
GeometryTools()
           
 
Method Summary
static boolean arePointsInOrderAndColinear(javax.vecmath.Point2d startPoint, javax.vecmath.Point2d middlePoint, javax.vecmath.Point2d endPoint, double epsilon)
          arePointsInOrderColinear: This returns true if: middle point is epsilon close to start or end Otherwise: if the start is epsilon close to the end, return false if |(start to middle unit vector) dot with (start to end unit vector) - 1| > epsilon return false else return true
static boolean arePointsInOrderAndColinear(javax.vecmath.Point3d startPoint, javax.vecmath.Point3d middlePoint, javax.vecmath.Point3d endPoint, double epsilon)
          arePointsInOrderColinear: This returns true if: middle point is epsilon close to start or end Otherwise: if the start is epsilon close to the end, return false if |(start to middle unit vector) dot with (start to end unit vector) - 1| > epsilon return false else return true
static javax.vecmath.Point2d averagePoint2ds(java.util.ArrayList<javax.vecmath.Point2d> points)
          Averages the 2D points in a given Array
static javax.vecmath.Point3d averagePoint3ds(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 java.util.ArrayList<FramePoint2d> changeFrameToZUpAndProjectToXYPlane(ReferenceFrame zUpFrame, java.util.List<FramePoint> points)
           
static javax.vecmath.Point2d[] computeMinimumDistancePoints(ConvexPolygon2d polygon1, ConvexPolygon2d polygon2)
           
static javax.vecmath.Point2d[] computeMinimumDistancePoints(ConvexPolygon2d polygon1, ConvexPolygon2d polygon2, double epsilon)
          Finds the minimum distance between two convex polygons Taken from http://cygnus-x1.cs.duke.edu/~edels/Papers/1985-J-02-ComputingExtremeDistances.pdf
static javax.media.j3d.Transform3D createTranslationTransform(javax.vecmath.Vector3d translation)
           
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 javax.vecmath.Point3d generateRandomPoint3d(java.util.Random random, double[] range1, double[] range2)
          Returns an evenly distributed random point3d.
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 javax.vecmath.Vector3d getProjectionOntoPlane(javax.vecmath.Vector3d p1, javax.vecmath.Vector3d p2, javax.vecmath.Vector3d p3, javax.vecmath.Vector3d p)
           
static javax.vecmath.AxisAngle4d getRotationBasedOnNormal(javax.vecmath.Vector3d normalVector3d)
           
static FrameOrientation 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 double getUnknownTriangleAngleByLawOfCosine(double lengthNeighbourSideA, double lengthNeighbourSideB, double lengthOppositeSideC)
          Calculate an unknown angle of a fully defined 2D Triangle by the law of Cosine
static double getUnknownTriangleSideLengthByLawOfCosine(double lengthSideA, double lengthSideB, double angleBetweenAAndB)
          Calculate an unknown side length of a fully defined 2D Triangle by the law of Cosine
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 double[] intersection(double firstPointX, double firstPointY, double firstVectorX, double firstVectorY, double secondPointX, double secondPointY, double secondVectorX, double secondVectorY)
          Finds the intersection parameters between two lines.
static boolean isFormingTriangle(double lengthNeighbourSideA, double lengthNeighbourSideB, double lengthOppositeSideC)
           
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 double minimumDistance(FramePoint testPoint, java.util.List<FramePoint> points)
           
static void movePointInsidePolygonAlongLine(FramePoint2d point, FrameConvexPolygon2d polygon, FrameLine2d line)
          Deprecated. 
static void movePointInsidePolygonAlongLine(FramePoint2d point, FrameConvexPolygon2d polygon, FrameLine2d line, double amountToBeInside)
          Deprecated. 
static void movePointInsidePolygonAlongVector(FramePoint2d pointToMove, FrameVector2d vector, FrameConvexPolygon2d polygon, double distanceToBeInside)
           
static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon)
           
static java.util.ArrayList<FramePoint2d> projectToXYPlane(java.util.List<FramePoint> points)
           
static double pythagorasGetCathetus(double hypothenuseC, double cathetusA)
          Get a unknown cathetus (90-deg triangle one of the two shorter triangle sides, neighbouring the 90-degree angle) by Pythagoras law, a^2+b^2=c^2
static double pythagorasGetHypothenuse(double cathetusA, double cathetusB)
          Get the hypothenuse c (90-degree triangle longest triangle length, opposite to the 90-degree angle) by Pythagoras law, a^2+b^2=c^2
 
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

averagePoint2ds

public static javax.vecmath.Point2d averagePoint2ds(java.util.ArrayList<javax.vecmath.Point2d> points)
Averages the 2D points in a given Array

Parameters:
points - ArrayList
Returns:
Point2d

averagePoint3ds

public static javax.vecmath.Point3d averagePoint3ds(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

getRotationBasedOnNormal

public static javax.vecmath.AxisAngle4d getRotationBasedOnNormal(javax.vecmath.Vector3d normalVector3d)

getTransform

public static FrameOrientation 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)

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

movePointInsidePolygonAlongLine

@Deprecated
public static void movePointInsidePolygonAlongLine(FramePoint2d point,
                                                              FrameConvexPolygon2d polygon,
                                                              FrameLine2d line)
Deprecated. 


movePointInsidePolygonAlongLine

@Deprecated
public static void movePointInsidePolygonAlongLine(FramePoint2d point,
                                                              FrameConvexPolygon2d polygon,
                                                              FrameLine2d line,
                                                              double amountToBeInside)
Deprecated. 


movePointInsidePolygonAlongVector

public static void movePointInsidePolygonAlongVector(FramePoint2d pointToMove,
                                                     FrameVector2d vector,
                                                     FrameConvexPolygon2d polygon,
                                                     double distanceToBeInside)

projectOntoPolygonAndCheckDistance

public static void projectOntoPolygonAndCheckDistance(FramePoint2d point,
                                                      FrameConvexPolygon2d polygon,
                                                      double epsilon)

arePointsInOrderAndColinear

public static boolean arePointsInOrderAndColinear(javax.vecmath.Point2d startPoint,
                                                  javax.vecmath.Point2d middlePoint,
                                                  javax.vecmath.Point2d endPoint,
                                                  double epsilon)
arePointsInOrderColinear: This returns true if: middle point is epsilon close to start or end Otherwise: if the start is epsilon close to the end, return false if |(start to middle unit vector) dot with (start to end unit vector) - 1| > epsilon return false else return true

Parameters:
startPoint - Point2d
middlePoint - Point2d
endPoint - Point2d
Returns:
boolean

arePointsInOrderAndColinear

public static boolean arePointsInOrderAndColinear(javax.vecmath.Point3d startPoint,
                                                  javax.vecmath.Point3d middlePoint,
                                                  javax.vecmath.Point3d endPoint,
                                                  double epsilon)
arePointsInOrderColinear: This returns true if: middle point is epsilon close to start or end Otherwise: if the start is epsilon close to the end, return false if |(start to middle unit vector) dot with (start to end unit vector) - 1| > epsilon return false else return true

Parameters:
startPoint - Point3d
middlePoint - Point3d
endPoint - Point3d
Returns:
boolean

getUnknownTriangleSideLengthByLawOfCosine

public static double getUnknownTriangleSideLengthByLawOfCosine(double lengthSideA,
                                                               double lengthSideB,
                                                               double angleBetweenAAndB)
Calculate an unknown side length of a fully defined 2D Triangle by the law of Cosine

Parameters:
lengthSideA -
lengthSideB -
angleBetweenAAndB -

getUnknownTriangleAngleByLawOfCosine

public static double getUnknownTriangleAngleByLawOfCosine(double lengthNeighbourSideA,
                                                          double lengthNeighbourSideB,
                                                          double lengthOppositeSideC)
Calculate an unknown angle of a fully defined 2D Triangle by the law of Cosine

Parameters:
lengthNeighbourSideA -
lengthNeighbourSideB -
lengthOppositeSideC -

pythagorasGetCathetus

public static double pythagorasGetCathetus(double hypothenuseC,
                                           double cathetusA)
Get a unknown cathetus (90-deg triangle one of the two shorter triangle sides, neighbouring the 90-degree angle) by Pythagoras law, a^2+b^2=c^2

Parameters:
hypothenuseC - the longest side
cathetusA - one short side
cathetusB - the other short side

isFormingTriangle

public static boolean isFormingTriangle(double lengthNeighbourSideA,
                                        double lengthNeighbourSideB,
                                        double lengthOppositeSideC)

pythagorasGetHypothenuse

public static double pythagorasGetHypothenuse(double cathetusA,
                                              double cathetusB)
Get the hypothenuse c (90-degree triangle longest triangle length, opposite to the 90-degree angle) by Pythagoras law, a^2+b^2=c^2

Parameters:
cathetusA - one short side
cathetusB - the other short side
hypothenuseC - the longest side

getProjectionOntoPlane

public static javax.vecmath.Vector3d getProjectionOntoPlane(javax.vecmath.Vector3d p1,
                                                            javax.vecmath.Vector3d p2,
                                                            javax.vecmath.Vector3d p3,
                                                            javax.vecmath.Vector3d p)

generateRandomPoint3d

public static javax.vecmath.Point3d generateRandomPoint3d(java.util.Random random,
                                                          double[] range1,
                                                          double[] range2)
Returns an evenly distributed random point3d. Each element is between the bounds specified by bounds1 and bounds 2. The bounds do not need to be in a min/max order.

Parameters:
random - Random number generator.
range1 - One side of the bounds for x, y, z.
bounds2 - Other side of the bounds for x, y, z.
Returns:
Point3d

intersection

public static double[] intersection(double firstPointX,
                                    double firstPointY,
                                    double firstVectorX,
                                    double firstVectorY,
                                    double secondPointX,
                                    double secondPointY,
                                    double secondVectorX,
                                    double secondVectorY)
Finds the intersection parameters between two lines. First line starts at (firstPointX, firstPointY) and has direction vector (firstVectorX, firstVectorY). The second line starts at (secondPointX, secondPointY) and has direction vector (secondVectorX, secondVectorY). Returns null if the lines are parallel. Returns {alpha, beta} such that the intersection between the lines occurs at P1 + alpha * V1 = P2 + beta * V2;

Parameters:
firstPointX - double First line starting x.
firstPointY - double First line starting y.
firstVectorX - double First line direction x.
firstVectorY - double First line direction y.
secondPointX - double Second line starting x.
secondPointY - double Second line starting y.
secondVectorX - double Second line direction x.
secondVectorY - double Second line direction y.
Returns:
double[] {alpha, beta} such that the intersection between the lines occurs at P1 + alpha * V1 = P2 + beta * V2;

minimumDistance

public static double minimumDistance(FramePoint testPoint,
                                     java.util.List<FramePoint> points)

changeFrameToZUpAndProjectToXYPlane

public static java.util.ArrayList<FramePoint2d> changeFrameToZUpAndProjectToXYPlane(ReferenceFrame zUpFrame,
                                                                                    java.util.List<FramePoint> points)

projectToXYPlane

public static java.util.ArrayList<FramePoint2d> projectToXYPlane(java.util.List<FramePoint> points)

createTranslationTransform

public static javax.media.j3d.Transform3D createTranslationTransform(javax.vecmath.Vector3d translation)

computeMinimumDistancePoints

public static javax.vecmath.Point2d[] computeMinimumDistancePoints(ConvexPolygon2d polygon1,
                                                                   ConvexPolygon2d polygon2,
                                                                   double epsilon)
Finds the minimum distance between two convex polygons Taken from http://cygnus-x1.cs.duke.edu/~edels/Papers/1985-J-02-ComputingExtremeDistances.pdf

Returns:
Two points, one from each polygon, between which is the minimum distance between the two polygons

computeMinimumDistancePoints

public static javax.vecmath.Point2d[] computeMinimumDistancePoints(ConvexPolygon2d polygon1,
                                                                   ConvexPolygon2d polygon2)