1111 lines
50 KiB
C#
1111 lines
50 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
namespace Unity.XR.CoreUtils
|
|
{
|
|
/// <summary>
|
|
/// Utility methods for common geometric operations.
|
|
/// </summary>
|
|
public static class GeometryUtils
|
|
{
|
|
// Used in approximate equality checks
|
|
const float k_TwoPi = Mathf.PI * 2f;
|
|
|
|
// constants/cached constructions for Vector/UV operations
|
|
static readonly Vector3 k_Up = Vector3.up;
|
|
static readonly Vector3 k_Forward = Vector3.forward;
|
|
static readonly Vector3 k_Zero = Vector3.zero;
|
|
static readonly Quaternion k_VerticalCorrection = Quaternion.AngleAxis(180.0f, k_Up);
|
|
const float k_MostlyVertical = 0.95f;
|
|
|
|
// Local method use only -- created here to reduce garbage collection. Collections must be cleared before use
|
|
static readonly List<Vector3> k_HullEdgeDirections = new List<Vector3>();
|
|
static readonly HashSet<int> k_HullIndices = new HashSet<int>();
|
|
|
|
/// <summary>
|
|
/// Finds the side of a polygon closest to a specified world space position.
|
|
/// </summary>
|
|
/// <param name="vertices">Vertices defining the outline of a polygon.</param>
|
|
/// <param name="point">The position in space to find the two closest outline vertices to.</param>
|
|
/// <param name="vertexA">The coordinates of the first vertex of the nearest side is assigned to this `out` parameter.</param>
|
|
/// <param name="vertexB">The coordinates of the second vertex of the nearest side is assigned to this `out` parameter.</param>
|
|
/// <returns>True if a nearest edge could be found.</returns>
|
|
public static bool FindClosestEdge(List<Vector3> vertices, Vector3 point,
|
|
out Vector3 vertexA, out Vector3 vertexB)
|
|
{
|
|
var vertexCount = vertices.Count;
|
|
if (vertexCount < 1)
|
|
{
|
|
vertexA = Vector3.zero;
|
|
vertexB = Vector3.zero;
|
|
return false;
|
|
}
|
|
|
|
var shortestSqrDistance = float.MaxValue;
|
|
var closestVertA = Vector3.zero;
|
|
var closestVertB = Vector3.zero;
|
|
for (var i = 0; i < vertexCount; i++)
|
|
{
|
|
var vert = vertices[i];
|
|
var nextVert = vertices[(i + 1) % vertices.Count];
|
|
|
|
var closestPointOnEdge = ClosestPointOnLineSegment(point, vert, nextVert);
|
|
var sqrDistanceToEdge = Vector3.SqrMagnitude(point - closestPointOnEdge);
|
|
if (sqrDistanceToEdge < shortestSqrDistance)
|
|
{
|
|
shortestSqrDistance = sqrDistanceToEdge;
|
|
closestVertA = vert;
|
|
closestVertB = nextVert;
|
|
}
|
|
}
|
|
|
|
vertexA = closestVertA;
|
|
vertexB = closestVertB;
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the point on a polygon perimeter farthest from a specified point in space.
|
|
/// </summary>
|
|
/// <param name="vertices">Vertices defining the outline of a polygon.</param>
|
|
/// <param name="point">The position in world space to find the furthest intersection point.</param>
|
|
/// <returns>A world space position of a point on the polygon that is as far from the input point as possible.
|
|
/// Returns <see cref="Vector3.zero"/> if <paramref name="vertices"/> contains less than tree points.</returns>
|
|
public static Vector3 PointOnOppositeSideOfPolygon(List<Vector3> vertices, Vector3 point)
|
|
{
|
|
const float oppositeSideBufferScale = 100.0f;
|
|
|
|
var vertexCount = vertices.Count;
|
|
if (vertexCount < 3)
|
|
return Vector3.zero;
|
|
|
|
var a = vertices[0];
|
|
var b = vertices[1];
|
|
var c = vertices[2];
|
|
var normal = Vector3.Cross(b - a, c - a).normalized;
|
|
var center = Vector3.zero;
|
|
foreach (var vertex in vertices)
|
|
{
|
|
center += vertex;
|
|
}
|
|
|
|
center *= 1f / vertexCount;
|
|
var toPoint = Vector3.ProjectOnPlane(point - center, normal);
|
|
|
|
var lengthMinusOne = vertexCount - 1;
|
|
for (var i = 0; i < vertexCount; i++)
|
|
{
|
|
var vertexA = vertices[i];
|
|
var aNeighbor = i == lengthMinusOne ? a : vertices[i + 1];
|
|
var aLineVector = aNeighbor - vertexA;
|
|
|
|
ClosestTimesOnTwoLines(vertexA, aLineVector, center, -toPoint * oppositeSideBufferScale, out var s, out var t);
|
|
if (t >= 0 && s >= 0 && s <= 1)
|
|
{
|
|
return vertexA + aLineVector * s;
|
|
}
|
|
}
|
|
|
|
return Vector3.zero;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generates a standard triangle buffer with a given number of indices and adds it to the specified list.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Set <paramref name="reverse"/> <see langword="true"/> to reverse the normal winding order.
|
|
///
|
|
/// Example winding orders:
|
|
///
|
|
/// | **Normal winding** | **Reverse winding** |
|
|
/// | :----------------- | :------------------ |
|
|
/// | 0, 1, 2, | 0, 2, 1,
|
|
/// | 0, 2, 3, | 0, 3, 2,
|
|
/// | 0, 3, 4, | 0, 4, 3,
|
|
/// | ... | ... |
|
|
/// </remarks>
|
|
/// <param name="indices">The list to which to add the triangle buffer. The list is not cleared.</param>
|
|
/// <param name="vertCount">The number of perimeter vertices.</param>
|
|
/// <param name="reverse">(Optional) Whether to reverse the winding order of the vertices.</param>
|
|
public static void TriangulatePolygon(List<int> indices, int vertCount, bool reverse = false)
|
|
{
|
|
vertCount -= 2;
|
|
indices.EnsureCapacity(vertCount * 3);
|
|
if (reverse)
|
|
{
|
|
for (var i = 0; i < vertCount; i++)
|
|
{
|
|
indices.Add(0);
|
|
indices.Add(i + 2);
|
|
indices.Add(i + 1);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (var i = 0; i < vertCount; i++)
|
|
{
|
|
indices.Add(0);
|
|
indices.Add(i + 1);
|
|
indices.Add(i + 2);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the times at which two linear trajectories are the closest to each other.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Two trajectories which may or may not intersect have a time along each path which minimizes the distance
|
|
/// between trajectories. This function finds those two times. The same logic applies to line segments, where
|
|
/// the one point is the starting position, and the second point is the position at t = 1.
|
|
/// </remarks>
|
|
/// <param name="positionA">Starting point of object a</param>
|
|
/// <param name="velocityA">Velocity (direction and magnitude) of object a</param>
|
|
/// <param name="positionB">Starting point of object b</param>
|
|
/// <param name="velocityB">Velocity (direction and magnitude) of object b</param>
|
|
/// <param name="s">The time along trajectory a</param>
|
|
/// <param name="t">The time along trajectory b</param>
|
|
/// <param name="parallelTest">(Optional) epsilon value for parallel lines test</param>
|
|
/// <returns>False if the lines are parallel, otherwise true</returns>
|
|
public static bool ClosestTimesOnTwoLines(Vector3 positionA, Vector3 velocityA, Vector3 positionB, Vector3 velocityB,
|
|
out float s, out float t, double parallelTest = double.Epsilon)
|
|
{
|
|
// Cast dot products to doubles because parallel test can fail on some hardware (iOS)
|
|
var a = (double)Vector3.Dot(velocityA, velocityA);
|
|
var b = (double)Vector3.Dot(velocityA, velocityB);
|
|
var e = (double)Vector3.Dot(velocityB, velocityB);
|
|
|
|
var d = a * e - b * b;
|
|
|
|
//lines are parallel
|
|
if (Math.Abs(d) < parallelTest)
|
|
{
|
|
s = 0;
|
|
t = 0;
|
|
return false;
|
|
}
|
|
|
|
var r = positionA - positionB;
|
|
var c = Vector3.Dot(velocityA, r);
|
|
var f = Vector3.Dot(velocityB, r);
|
|
|
|
s = (float)((b * f - c * e) / d);
|
|
t = (float)((a * f - c * b) / d);
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the times of closest approach between two non-parallel trajectories.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Two trajectories, which may or may not intersect, have a time along each path that minimizes the distance
|
|
/// between trajectories. This function finds those two times. The same logic applies to line segments, where
|
|
/// the one point is the starting position, and the second point is the position at t = 1.
|
|
/// This function ignores the y components.
|
|
/// </remarks>
|
|
/// <param name="positionA">Starting point of object A.</param>
|
|
/// <param name="velocityA">Velocity (direction and magnitude) of object A.</param>
|
|
/// <param name="positionB">Starting point of object B.</param>
|
|
/// <param name="velocityB">Velocity (direction and magnitude) of object B.</param>
|
|
/// <param name="s">Set to the calculated time of closest approach along trajectory A.</param>
|
|
/// <param name="t">Set to the calculated time of closest approach along trajectory B.</param>
|
|
/// <param name="parallelTest">(Optional) A custom epsilon value for teh parallel lines test.</param>
|
|
/// <returns>False if the lines are parallel, otherwise true.</returns>
|
|
public static bool ClosestTimesOnTwoLinesXZ(Vector3 positionA, Vector3 velocityA, Vector3 positionB, Vector3 velocityB,
|
|
out float s, out float t, double parallelTest = double.Epsilon)
|
|
{
|
|
// Cast dot products to doubles because parallel test can fail on some hardware (iOS)
|
|
var a = (double)(velocityA.x * velocityA.x + velocityA.z * velocityA.z);
|
|
var b = (double)(velocityA.x * velocityB.x + velocityA.z * velocityB.z);
|
|
var e = (double)(velocityB.x * velocityB.x + velocityB.z * velocityB.z);
|
|
|
|
var d = a * e - b * b;
|
|
|
|
//lines are parallel
|
|
if (Math.Abs(d) < parallelTest)
|
|
{
|
|
s = 0;
|
|
t = 0;
|
|
return false;
|
|
}
|
|
|
|
var r = positionA - positionB;
|
|
var c = velocityA.x * r.x + velocityA.z * r.z;
|
|
var f = velocityB.x * r.x + velocityB.z * r.z;
|
|
|
|
s = (float)((b * f - c * e) / d);
|
|
t = (float)((a * f - c * b) / d);
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the closest points between two line segments.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// If the two line segments are parallel, then <paramref name="resultA"/> and <paramref name="resultB"/>
|
|
/// are set to the midpoint of the respective line segments.
|
|
/// </remarks>
|
|
/// <param name="a">Starting point of segment A.</param>
|
|
/// <param name="aLineVector">Vector from point a to the end point of segment A.</param>
|
|
/// <param name="b">Starting point of segment B.</param>
|
|
/// <param name="bLineVector">Vector from point b to the end point of segment B.</param>
|
|
/// <param name="resultA">Set to the coordinates of the point along segment A that is closest to any point on segment B.</param>
|
|
/// <param name="resultB">Set to the coordinates of the point along segment B that is closest to any point on segment A.</param>
|
|
/// <param name="parallelTest">(Optional) epsilon value for parallel lines test</param>
|
|
/// <returns>True if the line segments are parallel, false otherwise</returns>
|
|
public static bool ClosestPointsOnTwoLineSegments(Vector3 a, Vector3 aLineVector, Vector3 b, Vector3 bLineVector,
|
|
out Vector3 resultA, out Vector3 resultB, double parallelTest = double.Epsilon)
|
|
{
|
|
var parallel = !ClosestTimesOnTwoLines(a, aLineVector, b, bLineVector,
|
|
out var s, out var t, parallelTest);
|
|
|
|
if (s > 0 && s <= 1 && t > 0 && t <= 1)
|
|
{
|
|
resultA = a + aLineVector * s;
|
|
resultB = b + bLineVector * t;
|
|
}
|
|
else
|
|
{
|
|
// Edge cases (literally--we are checking each of the four endpoints against the opposite segment)
|
|
var bNeighbor = b + bLineVector;
|
|
var aNeighbor = a + aLineVector;
|
|
var aOnB = ClosestPointOnLineSegment(a, b, bNeighbor);
|
|
var aNeighborOnB = ClosestPointOnLineSegment(aNeighbor, b, bNeighbor);
|
|
var minDist = Vector3.Distance(a, aOnB);
|
|
resultA = a;
|
|
resultB = aOnB;
|
|
|
|
var nextDist = Vector3.Distance(aNeighbor, aNeighborOnB);
|
|
if (nextDist < minDist)
|
|
{
|
|
resultA = aNeighbor;
|
|
resultB = aNeighborOnB;
|
|
minDist = nextDist;
|
|
}
|
|
|
|
var bOnA = ClosestPointOnLineSegment(b, a, aNeighbor);
|
|
nextDist = Vector3.Distance(b, bOnA);
|
|
if (nextDist < minDist)
|
|
{
|
|
resultA = bOnA;
|
|
resultB = b;
|
|
minDist = nextDist;
|
|
}
|
|
|
|
var bNeighborOnA = ClosestPointOnLineSegment(bNeighbor, a, aNeighbor);
|
|
nextDist = Vector3.Distance(bNeighbor, bNeighborOnA);
|
|
if (nextDist < minDist)
|
|
{
|
|
resultA = bNeighborOnA;
|
|
resultB = bNeighbor;
|
|
}
|
|
|
|
if (parallel)
|
|
{
|
|
if (Vector3.Dot(aLineVector, bLineVector) > 0)
|
|
{
|
|
t = Vector3.Dot(bNeighbor - a, aLineVector.normalized) * 0.5f;
|
|
var midA = a + aLineVector.normalized * t;
|
|
var midB = bNeighbor + bLineVector.normalized * -t;
|
|
if (t > 0 && t < aLineVector.magnitude)
|
|
{
|
|
resultA = midA;
|
|
resultB = midB;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
t = Vector3.Dot(aNeighbor - bNeighbor, aLineVector.normalized) * 0.5f;
|
|
var midA = aNeighbor + aLineVector.normalized * -t;
|
|
var midB = bNeighbor + bLineVector.normalized * -t;
|
|
if (t > 0 && t < aLineVector.magnitude)
|
|
{
|
|
resultA = midA;
|
|
resultB = midB;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return parallel;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the point along a line segment closest to a given point.
|
|
/// </summary>
|
|
/// <param name="point">The point to test against the line segment.</param>
|
|
/// <param name="a">The first point of the line segment.</param>
|
|
/// <param name="b">The second point of the line segment.</param>
|
|
/// <returns>The point along the line segment closest to <paramref name="point"/>.</returns>
|
|
public static Vector3 ClosestPointOnLineSegment(Vector3 point, Vector3 a, Vector3 b)
|
|
{
|
|
var segment = b - a;
|
|
var direction = segment.normalized;
|
|
var projection = Vector3.Dot(point - a, direction);
|
|
if (projection < 0)
|
|
return a;
|
|
|
|
if (projection * projection > segment.sqrMagnitude)
|
|
return b;
|
|
|
|
return a + projection * direction;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the closest points of the perimeters of two polygons.
|
|
/// </summary>
|
|
/// <param name="verticesA">Vertices defining the outline of polygon A.</param>
|
|
/// <param name="verticesB">Vertices defining the outline of polygon B.</param>
|
|
/// <param name="pointA">The point on polygon A closest to an edge of polygon B.</param>
|
|
/// <param name="pointB">The point on polygon B closest to an edge of polygon A.</param>
|
|
/// <param name="parallelTest">The minimum distance between closest approaches used to detect parallel line segments.</param>
|
|
public static void ClosestPolygonApproach(List<Vector3> verticesA, List<Vector3> verticesB,
|
|
out Vector3 pointA, out Vector3 pointB, float parallelTest = 0f)
|
|
{
|
|
pointA = default;
|
|
pointB = default;
|
|
var closest = float.MaxValue;
|
|
var aCount = verticesA.Count;
|
|
var bCount = verticesB.Count;
|
|
var aCountMinusOne = aCount - 1;
|
|
var bCountMinusOne = bCount - 1;
|
|
var firstVertexA = verticesA[0];
|
|
var firstVertexB = verticesB[0];
|
|
for (var i = 0; i < aCount; i++)
|
|
{
|
|
var vertexA = verticesA[i];
|
|
var aNeighbor = i == aCountMinusOne ? firstVertexA : verticesA[i + 1];
|
|
var aLineVector = aNeighbor - vertexA;
|
|
|
|
for (var j = 0; j < bCount; j++)
|
|
{
|
|
var vertexB = verticesB[j];
|
|
var bNeighbor = j == bCountMinusOne ? firstVertexB : verticesB[j + 1];
|
|
var bLineVector = bNeighbor - vertexB;
|
|
|
|
var parallel = ClosestPointsOnTwoLineSegments(vertexA, aLineVector, vertexB, bLineVector,
|
|
out var a, out var b, parallelTest);
|
|
|
|
var dist = Vector3.Distance(a, b);
|
|
|
|
if (parallel)
|
|
{
|
|
var delta = dist - closest;
|
|
if (delta < parallelTest)
|
|
{
|
|
closest = dist - parallelTest;
|
|
pointA = a;
|
|
pointB = b;
|
|
}
|
|
}
|
|
else if (dist < closest)
|
|
{
|
|
closest = dist;
|
|
pointA = a;
|
|
pointB = b;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if a point is inside of a polygon on the XZ plane. (The y value is not used.)
|
|
/// </summary>
|
|
/// <param name="testPoint">The point to test.</param>
|
|
/// <param name="vertices">Vertices defining the outline of a polygon.</param>
|
|
/// <returns>True if the point is inside the polygon, false otherwise.</returns>
|
|
public static bool PointInPolygon(Vector3 testPoint, List<Vector3> vertices)
|
|
{
|
|
// Sanity check - not enough bounds vertices = nothing to be inside of
|
|
if (vertices.Count < 3)
|
|
return false;
|
|
|
|
// Check how many lines this test point collides with going in one direction
|
|
// Odd = Inside, Even = Outside
|
|
var collisions = 0;
|
|
var vertexCounter = 0;
|
|
var startPoint = vertices[vertices.Count - 1];
|
|
|
|
// We recenter the test point around the origin to simplify the math a bit
|
|
startPoint.x -= testPoint.x;
|
|
startPoint.z -= testPoint.z;
|
|
|
|
var currentSide = false;
|
|
if (!MathUtility.ApproximatelyZero(startPoint.z))
|
|
{
|
|
currentSide = startPoint.z < 0f;
|
|
}
|
|
else
|
|
{
|
|
// We need a definitive side of the horizontal axis to start with (since we need to know when we
|
|
// cross it), so we go backwards through the vertices until we find one that does not lie on the horizontal
|
|
for (var i = vertices.Count - 2; i >= 0; --i)
|
|
{
|
|
var vertZ = vertices[i].z;
|
|
vertZ -= testPoint.z;
|
|
if (!MathUtility.ApproximatelyZero(vertZ))
|
|
{
|
|
currentSide = vertZ < 0f;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
while (vertexCounter < vertices.Count)
|
|
{
|
|
var endPoint = vertices[vertexCounter];
|
|
endPoint.x -= testPoint.x;
|
|
endPoint.z -= testPoint.z;
|
|
|
|
var startToEnd = endPoint - startPoint;
|
|
var edgeSqrMagnitude = startToEnd.sqrMagnitude;
|
|
if (MathUtility.ApproximatelyZero(startToEnd.x * endPoint.z - startToEnd.z * endPoint.x) &&
|
|
startPoint.sqrMagnitude <= edgeSqrMagnitude && endPoint.sqrMagnitude <= edgeSqrMagnitude)
|
|
{
|
|
// This line goes through the start point, which means the point is on an edge of the polygon
|
|
return true;
|
|
}
|
|
|
|
// Ignore lines that end at the horizontal axis
|
|
if (!MathUtility.ApproximatelyZero(endPoint.z))
|
|
{
|
|
var nextSide = endPoint.z < 0f;
|
|
if (nextSide != currentSide)
|
|
{
|
|
currentSide = nextSide;
|
|
|
|
// If we've crossed the horizontal, check if the origin is to the left of the line
|
|
if ((startPoint.x * endPoint.z - startPoint.z * endPoint.x) / -(startPoint.z - endPoint.z) > 0)
|
|
collisions++;
|
|
}
|
|
}
|
|
|
|
startPoint = endPoint;
|
|
vertexCounter++;
|
|
}
|
|
|
|
return collisions % 2 > 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if a point is inside of a convex polygon and lies on the surface.
|
|
/// </summary>
|
|
/// <param name="testPoint">The point to test.</param>
|
|
/// <param name="vertices">Vertices defining the outline of the polygon. The polygon must be convex.
|
|
/// The vertices must be coplanar, but can lie on any arbitrary plane.</param>
|
|
/// <returns>True if the point is inside the polygon and coplanar, false otherwise.</returns>
|
|
public static bool PointInPolygon3D(Vector3 testPoint, List<Vector3> vertices)
|
|
{
|
|
// Not enough bounds vertices = nothing to be inside of
|
|
if (vertices.Count < 3)
|
|
return false;
|
|
|
|
// Compute the sum of the angles between the test point and each pair of edge points
|
|
double angleSum = 0;
|
|
for (var vertIndex = 0; vertIndex < vertices.Count; vertIndex++)
|
|
{
|
|
var toA = vertices[vertIndex] - testPoint;
|
|
var toB = vertices[(vertIndex + 1) % vertices.Count] - testPoint;
|
|
var sqrDistances = toA.sqrMagnitude * toB.sqrMagnitude; // Use sqrMagnitude, take sqrt of result later
|
|
if (sqrDistances <= MathUtility.EpsilonScaled) // On a vertex
|
|
{
|
|
return true;
|
|
}
|
|
|
|
double cosTheta = Vector3.Dot(toA, toB) / Mathf.Sqrt(sqrDistances);
|
|
var angle = Math.Acos(cosTheta);
|
|
angleSum += angle;
|
|
}
|
|
// The sum will only be 2*PI if the point is on the plane of the polygon and on the interior
|
|
const float radiansCompareThreshold = 0.01f;
|
|
return Mathf.Abs((float)angleSum - k_TwoPi) < radiansCompareThreshold;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Returns the point on a plane closest to a specified point.
|
|
/// </summary>
|
|
/// <param name="planeNormal">The plane normal. (It does not need to be normalized.)</param>
|
|
/// <param name="planePoint">Any point on the plane.</param>
|
|
/// <param name="point">The point to test.</param>
|
|
/// <returns>The point on the plane closest to <paramref name="point"/>.</returns>
|
|
public static Vector3 ProjectPointOnPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 point)
|
|
{
|
|
var distance = -Vector3.Dot(planeNormal.normalized, point - planePoint);
|
|
return point + planeNormal.normalized * distance;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the smallest convex polygon in the XZ plane that contains <paramref name="points"/>.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Based on algorithm outlined in
|
|
/// <a href="https://www.bitshiftprogrammer.com/2018/01/gift-wrapping-convex-hull-algorithm.html">
|
|
/// Gift Wrapping Convex Hull Algorithm With Unity Implementation</a>.
|
|
/// </remarks>
|
|
/// <param name="points">Points used to find the convex hull. The y coordinates of these points are ignored.</param>
|
|
/// <param name="hull">The vertices that define the smallest convex polygon are assigned to this list. The list is not cleared.</param>
|
|
/// <returns>True if <paramref name="points"/> has at least 3 elements, false otherwise.</returns>
|
|
public static bool ConvexHull2D(List<Vector3> points, List<Vector3> hull)
|
|
{
|
|
if (points.Count < 3)
|
|
return false;
|
|
|
|
k_HullIndices.Clear();
|
|
var pointsCount = points.Count;
|
|
var leftmostPointIndex = 0;
|
|
for (var i = 1; i < pointsCount; ++i)
|
|
{
|
|
var point = points[i];
|
|
var pointX = point.x;
|
|
var pointZ = point.z;
|
|
var leftMost = points[leftmostPointIndex];
|
|
var leftmostX = leftMost.x;
|
|
var leftmostZ = leftMost.z;
|
|
|
|
// As we traverse the outermost points, if we find 3 or more collinear points then we skip points that
|
|
// fall in the middle. So if our starting point falls in the middle of a line, it will always be skipped
|
|
// and our loop's end condition will never be met. So if there are multiple leftmost points, we want to
|
|
// use the point that has the minimum Z.
|
|
if (pointX < leftmostX || MathUtility.Approximately(pointX, leftmostX) && pointZ < leftmostZ)
|
|
leftmostPointIndex = i;
|
|
}
|
|
|
|
// Starting from the leftmost point, move clockwise along outermost points until we are back at the starting point.
|
|
var currentIndex = leftmostPointIndex;
|
|
do
|
|
{
|
|
var currentPoint = points[currentIndex];
|
|
hull.Add(currentPoint);
|
|
k_HullIndices.Add(currentIndex);
|
|
|
|
// This loop is where we find the next outermost point (next point on the hull clockwise).
|
|
// To do this we start with a point "p" which is an arbitrary entry in "points".
|
|
// We iterate through each point "q" in "points". If "q" is to the left of the line from
|
|
// the current point to "p", then "p" takes on the value of "q" and we continue iterating.
|
|
// By the end of iteration, "p" will be the next point on the hull because no point is more to the left.
|
|
var pIndex = 0;
|
|
var p = points[pIndex];
|
|
for (var qIndex = 1; qIndex < pointsCount; ++qIndex)
|
|
{
|
|
if (qIndex == currentIndex)
|
|
continue;
|
|
|
|
// By explicitly ignoring points that are already on the hull, we prevent the possibility of an infinite loop.
|
|
// Without this check, a point could potentially be chosen again if a collinearity check results in a
|
|
// false negative due to floating point error.
|
|
if (k_HullIndices.Contains(qIndex) && qIndex != leftmostPointIndex)
|
|
continue;
|
|
|
|
var q = points[qIndex];
|
|
var currentToP = p - currentPoint;
|
|
var currentToQ = q - currentPoint;
|
|
|
|
// The y value of the cross product of (current -> p) and (current -> q) tells us where q is
|
|
// in relation to the line (current -> p).
|
|
// If y is zero, q is on the line.
|
|
var crossY = currentToP.z * currentToQ.x - currentToP.x * currentToQ.z;
|
|
|
|
// next few lines are an inlined ` MathUtility.ApproximatelyZero(crossY) `,
|
|
// because we sometimes call this equality check many thousands of times in a frame
|
|
var yIsNegative = crossY < 0f;
|
|
var absY = yIsNegative ? -crossY : crossY;
|
|
var approximatelyEqual = absY < MathUtility.EpsilonScaled;
|
|
|
|
if (approximatelyEqual)
|
|
{
|
|
// If current, p, and q are collinear, then we want p to be the point that is furthest from current.
|
|
if (Vector3.SqrMagnitude(currentPoint - p) < Vector3.SqrMagnitude(currentPoint - q))
|
|
{
|
|
pIndex = qIndex;
|
|
p = points[pIndex];
|
|
}
|
|
}
|
|
// If y is negative, q is to the left.
|
|
else if (yIsNegative)
|
|
{
|
|
pIndex = qIndex;
|
|
p = points[pIndex];
|
|
}
|
|
}
|
|
|
|
currentIndex = pIndex;
|
|
} while (currentIndex != leftmostPointIndex);
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Given a list of vertices of a 2d convex polygon, find the centroid of the polygon.
|
|
/// This implementation operates only on the X and Z axes.
|
|
/// </summary>
|
|
/// <param name="vertices">Vertices defining the outline of a 2D polygon.</param>
|
|
/// <returns>The centroid point for the polygon.</returns>
|
|
public static Vector3 PolygonCentroid2D(List<Vector3> vertices)
|
|
{
|
|
var vertexCount = vertices.Count;
|
|
double partialSignedArea, signedArea = 0;
|
|
double centroidX = 0, centroidZ = 0;
|
|
double currentX, currentZ;
|
|
double nextX, nextZ;
|
|
|
|
int i;
|
|
for (i = 0; i < vertexCount - 1; i++)
|
|
{
|
|
var vertex = vertices[i];
|
|
currentX = vertex.x;
|
|
currentZ = vertex.z;
|
|
var nextVertex = vertices[i + 1];
|
|
nextX = nextVertex.x;
|
|
nextZ = nextVertex.z;
|
|
|
|
partialSignedArea = currentX * nextZ - nextX * currentZ;
|
|
signedArea += partialSignedArea;
|
|
|
|
centroidX += (currentX + nextX) * partialSignedArea;
|
|
centroidZ += (currentZ + nextZ) * partialSignedArea;
|
|
}
|
|
|
|
// Do last vertex separately so we don't check indexes via modulo every iteration
|
|
var vertexI = vertices[i];
|
|
currentX = vertexI.x;
|
|
currentZ = vertexI.z;
|
|
var vertex0 = vertices[0];
|
|
nextX = vertex0.x;
|
|
nextZ = vertex0.z;
|
|
|
|
partialSignedArea = currentX * nextZ - nextX * currentZ;
|
|
signedArea += partialSignedArea;
|
|
|
|
centroidX += (currentX + nextX) * partialSignedArea;
|
|
centroidZ += (currentZ + nextZ) * partialSignedArea;
|
|
|
|
signedArea *= 0.5;
|
|
var signedAreaMultiple = 6.0 * signedArea;
|
|
centroidX /= signedAreaMultiple;
|
|
centroidZ /= signedAreaMultiple;
|
|
|
|
return new Vector3((float)centroidX, 0f, (float)centroidZ);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the oriented minimum bounding box for a 2D convex hull.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// This implements the 'rotating calipers' algorithm and operates in linear time.
|
|
/// Operates only on the X and Z axes of the input.
|
|
/// </remarks>
|
|
/// <param name="convexHull">The list of all points in a 2D convex hull on the X and Z axes, in a clockwise winding order.</param>
|
|
/// <param name="boundingBox">An array of length 4 to fill with the vertex positions of the bounding box,
|
|
/// in the order `{ top left, bottom left, bottom right, top right }`.</param>
|
|
/// <returns>The size of the bounding box on each axis. Y here maps to the Z axis.</returns>
|
|
public static Vector2 OrientedMinimumBoundingBox2D(List<Vector3> convexHull, Vector3[] boundingBox)
|
|
{
|
|
// Caliper lines start axis-aligned as shown before we orient
|
|
// top
|
|
// ^------>
|
|
// | | right
|
|
// left | |
|
|
// <------V
|
|
// bottom
|
|
|
|
var caliperLeft = new Vector3(0f, 0f, 1f);
|
|
var caliperRight = new Vector3(0f, 0f, -1f);
|
|
var caliperTop = new Vector3(1f, 0f, 0f);
|
|
var caliperBottom = new Vector3(-1f, 0f, 0f);
|
|
|
|
float xMin = float.MaxValue, yMin = float.MaxValue;
|
|
float xMax = float.MinValue, yMax = float.MinValue;
|
|
int leftIndex = 0, rightIndex = 0, topIndex = 0, bottomIndex = 0;
|
|
|
|
// find the indices of the 'extreme points' in the hull to use as starting edge indices
|
|
var vertexCount = convexHull.Count;
|
|
for (var i = 0; i < vertexCount; i++)
|
|
{
|
|
var vertex = convexHull[i];
|
|
var x = vertex.x;
|
|
if (x < xMin)
|
|
{
|
|
xMin = x;
|
|
leftIndex = i;
|
|
}
|
|
|
|
if (x > xMax)
|
|
{
|
|
xMax = x;
|
|
rightIndex = i;
|
|
}
|
|
|
|
var z = vertex.z;
|
|
if (z < yMin)
|
|
{
|
|
yMin = z;
|
|
bottomIndex = i;
|
|
}
|
|
|
|
if (z > yMax)
|
|
{
|
|
yMax = z;
|
|
topIndex = i;
|
|
}
|
|
}
|
|
|
|
// compute & store the direction of every edge in the hull
|
|
k_HullEdgeDirections.Clear();
|
|
var lastVertexIndex = vertexCount - 1;
|
|
for (var i = 0; i < lastVertexIndex; i++)
|
|
{
|
|
var edgeDirection = convexHull[i + 1] - convexHull[i];
|
|
edgeDirection.Normalize();
|
|
k_HullEdgeDirections.Add(edgeDirection);
|
|
}
|
|
|
|
// by doing the last vertex on its own, we can skip checking indices while iterating above
|
|
var lastEdgeDirection = convexHull[0] - convexHull[lastVertexIndex];
|
|
lastEdgeDirection.Normalize();
|
|
k_HullEdgeDirections.Add(lastEdgeDirection);
|
|
|
|
var bestOrientedBoundingBoxArea = double.MaxValue;
|
|
// for every vertex in the hull, try aligning a caliper edge with an edge the vertex lies on
|
|
for (var i = 0; i < vertexCount; i++)
|
|
{
|
|
var leftEdge = k_HullEdgeDirections[leftIndex];
|
|
var rightEdge = k_HullEdgeDirections[rightIndex];
|
|
var topEdge = k_HullEdgeDirections[topIndex];
|
|
var bottomEdge = k_HullEdgeDirections[bottomIndex];
|
|
|
|
// find the angles between our caliper lines and the polygon edges, by doing
|
|
// ` arccosine(caliperEdge · hullEdge) ` for each pair of caliper edge & polygon edge
|
|
var leftAngle = Math.Acos(caliperLeft.x * leftEdge.x + caliperLeft.z * leftEdge.z);
|
|
var rightAngle = Math.Acos(caliperRight.x * rightEdge.x + caliperRight.z * rightEdge.z);
|
|
var topAngle = Math.Acos(caliperTop.x * topEdge.x + caliperTop.z * topEdge.z);
|
|
var bottomAngle = Math.Acos(caliperBottom.x * bottomEdge.x + caliperBottom.z * bottomEdge.z);
|
|
|
|
// find smallest angle among the lines
|
|
var smallestAngleIndex = 0;
|
|
var smallestAngle = leftAngle;
|
|
if (rightAngle < smallestAngle)
|
|
{
|
|
smallestAngle = rightAngle;
|
|
smallestAngleIndex = 1;
|
|
}
|
|
|
|
if (topAngle < smallestAngle)
|
|
{
|
|
smallestAngle = topAngle;
|
|
smallestAngleIndex = 2;
|
|
}
|
|
|
|
if (bottomAngle < smallestAngle)
|
|
smallestAngleIndex = 3;
|
|
|
|
// based on which caliper edge had the smallest angle between it & the polygon, rotate our calipers
|
|
// and recalculate corners
|
|
Vector3 upperLeft, upperRight, bottomLeft, bottomRight;
|
|
switch (smallestAngleIndex)
|
|
{
|
|
// left
|
|
case 0:
|
|
RotateCalipers(leftEdge, convexHull, ref leftIndex, out topIndex, out rightIndex, out bottomIndex,
|
|
out caliperLeft, out caliperTop, out caliperRight, out caliperBottom,
|
|
out upperLeft, out upperRight, out bottomRight, out bottomLeft);
|
|
break;
|
|
// right
|
|
case 1:
|
|
RotateCalipers(rightEdge, convexHull, ref rightIndex, out bottomIndex, out leftIndex, out topIndex,
|
|
out caliperRight, out caliperBottom, out caliperLeft, out caliperTop,
|
|
out bottomRight, out bottomLeft, out upperLeft, out upperRight);
|
|
break;
|
|
// top
|
|
case 2:
|
|
RotateCalipers(topEdge, convexHull, ref topIndex, out rightIndex, out bottomIndex, out leftIndex,
|
|
out caliperTop, out caliperRight, out caliperBottom, out caliperLeft,
|
|
out upperRight, out bottomRight, out bottomLeft, out upperLeft);
|
|
break;
|
|
// bottom
|
|
default:
|
|
RotateCalipers(bottomEdge, convexHull, ref bottomIndex, out leftIndex, out topIndex, out rightIndex,
|
|
out caliperBottom, out caliperLeft, out caliperTop, out caliperRight,
|
|
out bottomLeft, out upperLeft, out upperRight, out bottomRight);
|
|
break;
|
|
}
|
|
|
|
// usually with rotating calipers, this comparison is talked about in terms of distance,
|
|
// but since we just want to know which is bigger it works to use square magnitudes
|
|
var sqrDistanceX = (upperLeft - upperRight).sqrMagnitude;
|
|
var sqrDistanceZ = (upperLeft - bottomLeft).sqrMagnitude;
|
|
var sqrDistanceProduct = sqrDistanceX * sqrDistanceZ;
|
|
|
|
// if this is a smaller box than any we've found before, it's our new candidate
|
|
if (sqrDistanceProduct < bestOrientedBoundingBoxArea)
|
|
{
|
|
bestOrientedBoundingBoxArea = sqrDistanceProduct;
|
|
boundingBox[0] = bottomLeft;
|
|
boundingBox[1] = bottomRight;
|
|
boundingBox[2] = upperRight;
|
|
boundingBox[3] = upperLeft;
|
|
}
|
|
}
|
|
|
|
// compute the size of the 2d bounds
|
|
var topLeft = boundingBox[0];
|
|
var leftRightDistance = Vector3.Distance(topLeft, boundingBox[3]);
|
|
var topBottomDistance = Vector3.Distance(topLeft, boundingBox[1]);
|
|
return new Vector2(leftRightDistance, topBottomDistance);
|
|
}
|
|
|
|
static void RotateCalipers(Vector3 alignEdge, List<Vector3> vertices,
|
|
ref int indexA, out int indexB, out int indexC, out int indexD,
|
|
out Vector3 caliperA, out Vector3 caliperB, out Vector3 caliperC, out Vector3 caliperD,
|
|
out Vector3 caliperAEndCorner, out Vector3 caliperBEndCorner, out Vector3 caliperCEndCorner, out Vector3 caliperDEndCorner)
|
|
{
|
|
var vertexCount = vertices.Count;
|
|
caliperA = alignEdge;
|
|
caliperB = new Vector3(caliperA.z, 0f, -caliperA.x); // orthogonal
|
|
caliperC = -caliperA; // opposite
|
|
caliperD = -caliperB; // opposite orthogonal
|
|
indexA = (indexA + 1) % vertexCount;
|
|
|
|
// For each caliper, determine the polygon edge for the next caliper by testing intersection between the current caliper
|
|
// and the opposite orthogonal from subsequent polygon vertices until we've found the maximum intersection point.
|
|
var startA = vertices[indexA];
|
|
indexB = indexA;
|
|
var maxS = 0f;
|
|
while (true)
|
|
{
|
|
var nextIndex = (indexB + 1) % vertexCount;
|
|
ClosestTimesOnTwoLinesXZ(startA, caliperA, vertices[nextIndex], caliperD, out var s, out _);
|
|
if (s <= maxS)
|
|
break;
|
|
|
|
maxS = s;
|
|
indexB = nextIndex;
|
|
}
|
|
|
|
caliperAEndCorner = startA + caliperA * maxS;
|
|
var startB = vertices[indexB];
|
|
indexC = indexB;
|
|
maxS = 0f;
|
|
while (true)
|
|
{
|
|
var nextIndex = (indexC + 1) % vertexCount;
|
|
ClosestTimesOnTwoLinesXZ(startB, caliperB, vertices[nextIndex], caliperA, out var s, out _);
|
|
if (s <= maxS)
|
|
break;
|
|
|
|
maxS = s;
|
|
indexC = nextIndex;
|
|
}
|
|
|
|
caliperBEndCorner = startB + caliperB * maxS;
|
|
var startC = vertices[indexC];
|
|
indexD = indexC;
|
|
maxS = 0f;
|
|
while (true)
|
|
{
|
|
var nextIndex = (indexD + 1) % vertexCount;
|
|
ClosestTimesOnTwoLinesXZ(startC, caliperC, vertices[nextIndex], caliperB, out var s, out _);
|
|
if (s <= maxS)
|
|
break;
|
|
|
|
maxS = s;
|
|
indexD = nextIndex;
|
|
}
|
|
|
|
caliperCEndCorner = startC + caliperC * maxS;
|
|
|
|
// No need for any intersection tests for the last corner since we have all the other corners
|
|
caliperDEndCorner = caliperCEndCorner + caliperAEndCorner - caliperBEndCorner;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Given a 2D bounding box's vertices, find the rotation of the box.
|
|
/// </summary>
|
|
/// <param name="vertices">The 4 vertices of the bounding box, in the order
|
|
/// `{ top left, bottom left, bottom right, top right }`.</param>
|
|
/// <returns>The rotation of the box, with the horizontal side aligned to the x axis and the
|
|
/// vertical side aligned to the z axis</returns>
|
|
public static Quaternion RotationForBox(Vector3[] vertices)
|
|
{
|
|
var topLeft = vertices[0];
|
|
var topRight = vertices[3];
|
|
var leftToRight = topRight - topLeft;
|
|
return Quaternion.FromToRotation(Vector3.right, leftToRight);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the area of a convex polygon.
|
|
/// </summary>
|
|
/// <param name="vertices">Vertices defining the outline of a polygon.
|
|
/// The polygon must be convex, but can be in either winding order.</param>
|
|
/// <returns>The area of the polygon.</returns>
|
|
public static float ConvexPolygonArea(List<Vector3> vertices)
|
|
{
|
|
var count = vertices.Count;
|
|
if (count < 3)
|
|
return 0f;
|
|
|
|
var firstVertex = vertices[0];
|
|
var lastIndex = count - 1;
|
|
var lastVertex = vertices[lastIndex];
|
|
var area = lastVertex.x * firstVertex.z - firstVertex.x * lastVertex.z;
|
|
for (var i = 0; i < lastIndex; i++)
|
|
{
|
|
var currentVertex = vertices[i];
|
|
var nextVertex = vertices[i + 1];
|
|
area += currentVertex.x * nextVertex.z - nextVertex.x * currentVertex.z;
|
|
}
|
|
|
|
// Take absolute value because area is negative if vertices are clockwise
|
|
return Math.Abs(area * 0.5f);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if one polygon lies completely inside another coplanar polygon.
|
|
/// </summary>
|
|
/// <param name="polygonA">The polygon to test for lying inside <paramref name="polygonB"/>.</param>
|
|
/// <param name="polygonB">The polygon to test for containing <paramref name="polygonA"/>.
|
|
/// Must be convex and coplanar with <paramref name="polygonA"/>.</param>
|
|
/// <returns>True if <paramref name="polygonA"/> lies completely inside <paramref name="polygonB"/>, false otherwise.</returns>
|
|
public static bool PolygonInPolygon(List<Vector3> polygonA, List<Vector3> polygonB)
|
|
{
|
|
if (polygonA.Count < 1)
|
|
return false;
|
|
|
|
foreach (var vertex in polygonA)
|
|
{
|
|
if (!PointInPolygon3D(vertex, polygonB))
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if two convex coplanar polygons are within a certain distance from each other.
|
|
/// This includes the polygon perimeters as well as their interiors.
|
|
/// </summary>
|
|
/// <param name="polygonA">The first polygon to test. Must be convex and coplanar with <paramref name="polygonB"/>.</param>
|
|
/// <param name="polygonB">The second polygon to test. Must be convex and coplanar with <paramref name="polygonA"/>.</param>
|
|
/// <param name="maxDistance">The maximum distance allowed between the two polygons.</param>
|
|
/// <returns>True if the polygons are within the specified distance from each other, false otherwise.</returns>
|
|
public static bool PolygonsWithinRange(List<Vector3> polygonA, List<Vector3> polygonB, float maxDistance)
|
|
{
|
|
return PolygonsWithinSqRange(polygonA, polygonB, maxDistance * maxDistance);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if two convex coplanar polygons are within a specified distance from each other.
|
|
/// </summary>
|
|
/// <param name="polygonA">The first polygon to test. Must be convex and coplanar with <paramref name="polygonB"/>.</param>
|
|
/// <param name="polygonB">The second polygon to test. Must be convex and coplanar with <paramref name="polygonA"/>.</param>
|
|
/// <param name="maxSqDistance">The square of the maximum distance allowed between the two polygons.</param>
|
|
/// <returns>True if the polygons are within the specified distance from each other, false otherwise.</returns>
|
|
public static bool PolygonsWithinSqRange(List<Vector3> polygonA, List<Vector3> polygonB, float maxSqDistance)
|
|
{
|
|
ClosestPolygonApproach(polygonA, polygonB, out var pointA, out var pointB);
|
|
return Vector3.SqrMagnitude(pointB - pointA) <= maxSqDistance ||
|
|
PolygonInPolygon(polygonA, polygonB) || PolygonInPolygon(polygonB, polygonA);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if a point lies on the bounds of a polygon, ignoring the y components.
|
|
/// </summary>
|
|
/// <param name="testPoint">The point to test.</param>
|
|
/// <param name="vertices">Vertices defining the outline of a polygon.</param>
|
|
/// <param name="epsilon">Custom epsilon value used when testing if the point lies on an edge.</param>
|
|
/// <returns>True if the point lies on any edge of the polygon, false otherwise.</returns>
|
|
public static bool PointOnPolygonBoundsXZ(Vector3 testPoint, List<Vector3> vertices, float epsilon = float.Epsilon)
|
|
{
|
|
var verticesCount = vertices.Count;
|
|
|
|
// No edge for the point to lie on
|
|
if (verticesCount < 2)
|
|
return false;
|
|
|
|
var lastVertex = vertices[verticesCount - 1];
|
|
foreach (var vertex in vertices)
|
|
{
|
|
if (PointOnLineSegmentXZ(testPoint, lastVertex, vertex, epsilon))
|
|
return true;
|
|
|
|
lastVertex = vertex;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines if a point lies on a line segment, ignoring the y components.
|
|
/// </summary>
|
|
/// <param name="testPoint">The point to test.</param>
|
|
/// <param name="lineStart">Starting point of the line segment.</param>
|
|
/// <param name="lineEnd">Ending point of the line segment.</param>
|
|
/// <param name="epsilon">Custom epsilon value used for comparison checks.</param>
|
|
/// <returns>True if the point lies on the line segment, false otherwise.</returns>
|
|
public static bool PointOnLineSegmentXZ(Vector3 testPoint, Vector3 lineStart, Vector3 lineEnd, float epsilon = float.Epsilon)
|
|
{
|
|
var startToEnd = lineEnd - lineStart;
|
|
var startToTestPoint = testPoint - lineStart;
|
|
var cross = startToEnd.z * startToTestPoint.x - startToEnd.x * startToTestPoint.z;
|
|
var absCross = cross >= 0f ? cross : -cross;
|
|
if (absCross >= epsilon)
|
|
return false;
|
|
|
|
var dot = startToEnd.x * startToTestPoint.x + startToEnd.z * startToTestPoint.z;
|
|
var lineSqrMagnitude = startToEnd.x * startToEnd.x + startToEnd.z * startToEnd.z;
|
|
return dot >= -epsilon && dot <= lineSqrMagnitude + epsilon;
|
|
}
|
|
|
|
static Quaternion NormalizeRotationKeepingUp(Quaternion rot)
|
|
{
|
|
var srcUp = (rot * k_Up).normalized;
|
|
var isMostlyVertical = Mathf.Abs(srcUp.y) > k_MostlyVertical;
|
|
|
|
Vector3 modFwd;
|
|
|
|
if (isMostlyVertical)
|
|
{
|
|
modFwd = Vector3.Cross(k_Forward, srcUp);
|
|
}
|
|
else
|
|
{
|
|
var side = Vector3.Cross(srcUp, k_Up);
|
|
modFwd = Vector3.Cross(srcUp, side);
|
|
}
|
|
|
|
return Quaternion.LookRotation(modFwd, srcUp);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets a corrected polygon uv pose from a given plane pose.
|
|
/// </summary>
|
|
/// <param name="pose">The source plane pose.</param>
|
|
/// <returns>The rotation-corrected pose for calculating UVs.</returns>
|
|
public static Pose PolygonUVPoseFromPlanePose(Pose pose)
|
|
{
|
|
return new Pose(k_Zero, NormalizeRotationKeepingUp(pose.rotation));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Takes a polygon UV coordinate, and produces a pose-corrected UV coordinate.
|
|
/// </summary>
|
|
/// <param name="vertexPos">Vertex to transform.</param>
|
|
/// <param name="planePose">Polygon pose.</param>
|
|
/// <param name="uvPose">UV-correction Pose.</param>
|
|
/// <returns>The corrected UV coordinate.</returns>
|
|
public static Vector2 PolygonVertexToUV(Vector3 vertexPos, Pose planePose, Pose uvPose)
|
|
{
|
|
var worldPos = planePose.position + planePose.rotation * vertexPos;
|
|
var localUv = Quaternion.Inverse(uvPose.rotation) * (worldPos - uvPose.position);
|
|
|
|
localUv = k_VerticalCorrection * localUv;
|
|
|
|
return new Vector2(localUv.x, localUv.z);
|
|
}
|
|
}
|
|
}
|