public static IntersectPoints ClosestPointsOnSegmentToLine(Vector3 segment0, Vector3 segment1, Vector3 linePoint, Vector3 lineDirection)
{
IntersectPoints closests = ClosestPointsOnTwoLines(segment0, segment1 - segment0, linePoint, lineDirection);
closests.first = ClampToSegment(closests.first, segment0, segment1);
return(closests);
}
public ContactInfo ClosestPointOnSurface(Vector3 segment0, Vector3 segment1, float radius)
{
trianglesBufferSingleCaps.Clear();
CapsuleShape capsule = CapsuleShape.ToLocalOfUniformScale(new CapsuleShape(segment0, segment1, radius), transform);
Vector3 localSegment0 = capsule.top;
Vector3 localSegment1 = capsule.bottom;
radius = capsule.radius;
AABB aabb = AABB.CreateCapsuleAABB(localSegment0, localSegment1, radius + .001f);
tree.FindClosestTriangles(tree.rootNode, aabb, trianglesBufferSingleCaps, infoDebug);
Vector3 p1, p2, p3;
IntersectPoints near;
IntersectPoints nearest = new IntersectPoints();
float radiusSqrd = radius * radius;
float shortestDistance = float.MaxValue;
int shortestTriangleIndex = -1;
HashSet <int> .Enumerator enumerator = trianglesBufferSingleCaps.GetEnumerator();
while (enumerator.MoveNext())
{
tree.GetTriangleVertices(enumerator.Current, out p1, out p2, out p3);
near = Geometry.ClosestPointOnTriangleToLine(localSegment0, localSegment1, p1, p2, p3, true);
float distance = (near.second - near.first).sqrMagnitude;
if (PointIsBetter(distance, shortestDistance, nearest.first, nearest.second, near.first, near.second, shortestTriangleIndex, enumerator.Current, radiusSqrd))
{
shortestDistance = distance;
nearest = near;
shortestTriangleIndex = enumerator.Current;
}
}
if (nearest.second == Vector3.zero)
{
return(new ContactInfo());
}
return(new ContactInfo(transform.TransformPoint(nearest.second), transform.TransformDirection(tree.GetTriangleNormal(shortestTriangleIndex))));
}
//Returns 2 points since on line 1 there will be a closest point to line 2, and on line 2 there will be a closest point to line 1.
public static IntersectPoints ClosestPointsOnTwoLines(Vector3 point1, Vector3 point1Direction, Vector3 point2, Vector3 point2Direction)
{
IntersectPoints intersections = new IntersectPoints();
//I dont think we need to normalize
//point1Direction.Normalize();
//point2Direction.Normalize();
float a = Vector3.Dot(point1Direction, point1Direction);
float b = Vector3.Dot(point1Direction, point2Direction);
float e = Vector3.Dot(point2Direction, point2Direction);
float d = a * e - b * b;
//This is a check if parallel, howeverm since we are not normalizing the directions, it seems even if they are parallel they will not == 0
//so they will get past this point, but its seems to be alright since it seems to still give a correct point (although a point very fary away).
//Also, if they are parallel and we dont normalize, the deciding point seems randomly choses on the lines, which while is still correct,
//our ClosestPointsOnTwoLineSegments gets undesireable results when on corners. (for example when using it in our ClosestPointOnTriangleToLine).
if (d != 0f)
{
Vector3 r = point1 - point2;
float c = Vector3.Dot(point1Direction, r);
float f = Vector3.Dot(point2Direction, r);
float s = (b * f - c * e) / d;
float t = (a * f - c * b) / d;
intersections.first = point1 + point1Direction * s;
intersections.second = point2 + point2Direction * t;
}
else
{
//Lines are parallel, select any points next to eachother
intersections.first = point1;
intersections.second = point2 + Vector3.Project(point1 - point2, point2Direction);
}
return(intersections);
}
public static IntersectPoints ClosestPointsOnTwoLineSegments(Vector3 segment1Point1, Vector3 segment1Point2, Vector3 segment2Point1, Vector3 segment2Point2)
{
Vector3 line1Direction = segment1Point2 - segment1Point1;
Vector3 line2Direction = segment2Point2 - segment2Point1;
IntersectPoints closests = ClosestPointsOnTwoLines(segment1Point1, line1Direction, segment2Point1, line2Direction);
IntersectPoints clampedClosests = closests;
clampedClosests.first = ClampToSegment(clampedClosests.first, segment1Point1, segment1Point2);
clampedClosests.second = ClampToSegment(clampedClosests.second, segment2Point1, segment2Point2);
//Since this is a line segment, we need to decide which line we want to clamp both closest points to. So we choose the one that is farthest from its supposed closest point.
if ((closests.first - clampedClosests.first).sqrMagnitude > (closests.second - clampedClosests.second).sqrMagnitude)
{
clampedClosests.second = SegmentTargetAlignToPoint(clampedClosests.first, segment2Point1, segment2Point2);
}
else
{
clampedClosests.first = SegmentTargetAlignToPoint(clampedClosests.second, segment1Point1, segment1Point2);
}
return(clampedClosests);
}
//Returns 2 points since on line 1 there will be a closest point to line 2, and on line 2 there will be a closest point to line 1.
public static IntersectPoints ClosestPointsOnTwoLines(Vector3 point1, Vector3 point1Direction, Vector3 point2, Vector3 point2Direction)
{
IntersectPoints intersections = new IntersectPoints();
//While not normalizing can still give "correct" results, if the lines are parallel the closest points could be randomly chosen very far away,
//so far away that we can start to run into float point precision erros, so we normalize the directions.
point1Direction.Normalize();
point2Direction.Normalize();
float a = Vector3.Dot(point1Direction, point1Direction);
float b = Vector3.Dot(point1Direction, point2Direction);
float e = Vector3.Dot(point2Direction, point2Direction);
float d = a * e - b * b;
if (d != 0f)
{
Vector3 r = point1 - point2;
float c = Vector3.Dot(point1Direction, r);
float f = Vector3.Dot(point2Direction, r);
float s = (b * f - c * e) / d;
float t = (a * f - c * b) / d;
intersections.first = point1 + point1Direction * s;
intersections.second = point2 + point2Direction * t;
}
else
{
//Lines are parallel, select any points next to eachother
intersections.first = point1;
intersections.second = point2 + Vector3.Project(point1 - point2, point2Direction);
}
return(intersections);
}
//When isRectangle is true, the vertices must be on the same plane.
static IntersectPoints ClosestPointOnTriangleToLine(Vector3 segment0, Vector3 segment1, Vector3 vertex1, Vector3 vertex2, Vector3 vertex3, Vector3 vertex4, bool treatAsLineSegment = false, bool isRectangle = false)
{
Vector3 ab = vertex2 - vertex1;
Vector3 ac = vertex3 - vertex1;
Vector3 normal = Vector3.Cross(ab, ac);
float s0PlaneDistance = Vector3.Dot(segment0 - vertex1, normal);
float s1PlaneDistance = Vector3.Dot(segment1 - vertex1, normal);
IntersectPoints closestPoints = new IntersectPoints();
//If we want to treat it as a line segment then we will need to check its distance, but if the line is plane are parallel (s0PlaneDistance != s1PlaneDistance) then we will treat as line segment anyways since we need a point reference
if (s0PlaneDistance != s1PlaneDistance && (!treatAsLineSegment || (s0PlaneDistance * s1PlaneDistance) < 0f))
{
closestPoints.first = segment0 + (segment1 - segment0) * (-s0PlaneDistance / (s1PlaneDistance - s0PlaneDistance));
closestPoints.second = closestPoints.first;
}
else
{
//We get the closest segment and calculate its closest distance to the plane
closestPoints.first = (Mathf.Abs(s0PlaneDistance) < Mathf.Abs(s1PlaneDistance)) ? segment0 : segment1;
closestPoints.second = closestPoints.first + (normal * LinePlaneDistance(closestPoints.first, normal, vertex1, normal));
}
//Make sure plane intersection is within triangle bounds
float a = Vector3.Dot(Vector3.Cross(normal, vertex2 - vertex1), closestPoints.second - vertex1);
float b = Vector3.Dot(Vector3.Cross(normal, vertex3 - vertex2), closestPoints.second - vertex2);
float c = float.MaxValue;
float d = float.MaxValue;
if (!isRectangle)
{
c = Vector3.Dot(Vector3.Cross(normal, vertex1 - vertex3), closestPoints.second - vertex3);
}
else
{
c = Vector3.Dot(Vector3.Cross(normal, vertex4 - vertex3), closestPoints.second - vertex3);
d = Vector3.Dot(Vector3.Cross(normal, vertex1 - vertex4), closestPoints.second - vertex4);
}
if (a < 0f || b < 0f || c < 0f || d < 0f)
{
//We are not within the triangle, we are on an edge so find the closest
if (a < b && a < c && a < d)
{
return(ClosestPointsOnTwoLineSegments(segment0, segment1, vertex1, vertex2));
}
else if (b < a && b < c && b < d)
{
return(ClosestPointsOnTwoLineSegments(segment0, segment1, vertex2, vertex3));
}
else if ((c < a && c < b && c < d) || !isRectangle)
{
if (!isRectangle)
{
return(ClosestPointsOnTwoLineSegments(segment0, segment1, vertex3, vertex1));
}
else
{
return(ClosestPointsOnTwoLineSegments(segment0, segment1, vertex3, vertex4));
}
}
else if (isRectangle)
{
return(ClosestPointsOnTwoLineSegments(segment0, segment1, vertex4, vertex1));
}
}
return(closestPoints);
}
