//
// Line, plane, ray intersection with plane
//
//Ray-plane intersection
//http://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-plane-and-ray-disk-intersection
public static bool RayPlane(MyVector2 planePos, MyVector2 planeNormal, MyVector2 rayStart, MyVector2 rayDir)
{
//To avoid floating point precision issues we can add a small value
float epsilon = MathUtility.EPSILON;
bool areIntersecting = false;
float denominator = MyVector2.Dot(planeNormal * -1f, rayDir);
//Debug.Log(denominator);
//The ray has to point at the surface of the plane
//The surface of the plane is determined by the normal
if (denominator > epsilon)
{
//Now we have to figur out of the ray starts "inside" of the plane
//meaning on the other side of the normal
//If so it can't hit the plane
MyVector2 vecBetween = planePos - rayStart;
float t = MyVector2.Dot(vecBetween, planeNormal * -1f) / denominator;
//Debug.Log(t);
if (t >= 0f)
{
areIntersecting = true;
}
}
return(areIntersecting);
}
//
// Find the closest point on a line segment from a point
//
//From https://www.youtube.com/watch?v=KHuI9bXZS74
//Maybe better version https://stackoverflow.com/questions/3120357/get-closest-point-to-a-line
public static MyVector2 GetClosestPointOnLineSegment(MyVector2 a, MyVector2 b, MyVector2 p)
{
MyVector2 a_p = p - a;
MyVector2 a_b = b - a;
//This is using vector projections???
//Square magnitude of AB vector
float sqrMagnitudeAB = MyVector2.SqrMagnitude(a_b);
//The DOT product of a_p and a_b
float ABAPproduct = MyVector2.Dot(a_p, a_b);
//The normalized "distance" from a to the closest point
float distance = ABAPproduct / sqrMagnitudeAB;
//This point may not be on the line segment, if so return one of the end points
//Check if P projection is over vectorAB
if (distance < 0)
{
return(a);
}
else if (distance > 1)
{
return(b);
}
else
{
return(a + a_b * distance);
}
}
//Line-plane intersection
public static bool LinePlane(MyVector2 planePos, MyVector2 planeNormal, MyVector2 line_p1, MyVector2 line_p2)
{
//To avoid floating point precision issues we can add a small value
float epsilon = MathUtility.EPSILON;
bool areIntersecting = false;
MyVector2 lineDir = MyVector2.Normalize(line_p1 - line_p2);
float denominator = MyVector2.Dot(-planeNormal, lineDir);
//Debug.Log(denominator);
//No intersection if the line and plane are perpendicular
if (denominator > epsilon || denominator < -epsilon)
{
MyVector2 vecBetween = planePos - line_p1;
float t = MyVector2.Dot(vecBetween, -planeNormal) / denominator;
MyVector2 intersectionPoint = line_p1 + lineDir * t;
//Gizmos.DrawWireSphere(intersectionPoint, 0.5f);
if (_Geometry.IsPointBetweenPoints(line_p1, line_p2, intersectionPoint))
{
areIntersecting = true;
}
}
return(areIntersecting);
}
//
// Is a point to the left, to the right, or on a plane
//
//https://gamedevelopment.tutsplus.com/tutorials/understanding-sutherland-hodgman-clipping-for-physics-engines--gamedev-11917
//Notice that the plane normal doesnt have to be normalized
//public static float DistanceFromPointToPlane(Vector3 planeNormal, Vector3 planePos, Vector3 pointPos)
//{
// //Positive distance denotes that the point p is on the front side of the plane
// //Negative means it's on the back side
// float distance = Vector3.Dot(planeNormal, pointPos - planePos);
// return distance;
//}
public static float DistanceFromPointToPlane(MyVector2 planeNormal, MyVector2 planePos, MyVector2 pointPos)
{
//Positive distance denotes that the point p is on the front side of the plane
//Negative means it's on the back side
float distance = MyVector2.Dot(planeNormal, pointPos - planePos);
return(distance);
}
//
// Line, plane, ray intersection with plane
//
//This is a useful method to find the intersection coordinate if we know we are intersecting
//Is used for ray-plane, line-plane, plane-plane
//2d
private static MyVector2 GetIntersectionCoordinate(Plane2 plane, Ray2 ray)
{
float denominator = MyVector2.Dot(-plane.normal, ray.dir);
MyVector2 vecBetween = plane.pos - ray.origin;
float t = MyVector2.Dot(vecBetween, -plane.normal) / denominator;
MyVector2 intersectionPoint = ray.origin + ray.dir * t;
return(intersectionPoint);
}
//This is a useful method to find the intersection coordinate if we know we are intersecting
//Is used for ray-plane, line-plane, plane-plane
private static MyVector2 GetIntersectionCoordinate(MyVector2 planePos, MyVector2 planeNormal, MyVector2 rayStart, MyVector2 rayDir)
{
float denominator = MyVector2.Dot(-planeNormal, rayDir);
MyVector2 vecBetween = planePos - rayStart;
float t = MyVector2.Dot(vecBetween, -planeNormal) / denominator;
MyVector2 intersectionPoint = rayStart + rayDir * t;
return(intersectionPoint);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector2.Angle() but we are using MyVector2
public static float AngleBetween(MyVector2 from, MyVector2 to)
{
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector2.Dot(MyVector2.Normalize(from), MyVector2.Normalize(to));
//This shouldn't happen but may happen because of floating point precision issues
dot = Mathf.Clamp(dot, -1f, 1f);
float angleRad = Mathf.Acos(dot);
return(angleRad);
}
//
// Line-point calculations
//
//From https://stackoverflow.com/questions/3120357/get-closest-point-to-a-line
//and https://www.youtube.com/watch?v=_ENEsV_kNx8
public static MyVector2 GetClosestPointOnLine(Edge2 e, MyVector2 p, bool withinSegment)
{
MyVector2 a = e.p1;
MyVector2 b = e.p2;
//Assume the line goes from a to b
MyVector2 ab = b - a;
//Vector from "start" of the line to the point outside of line
MyVector2 ap = p - a;
//Scalar projection https://en.wikipedia.org/wiki/Scalar_projection
//The scalar projection is a scalar, equal to the length of the orthogonal projection of ap on ab, with a negative sign if the projection has an opposite direction with respect to ab.
//scalarProjection = Dot(ap, ab) / Magnitude(ab) where the magnitude of ab is the distance between a and b
//If ab is normalized, we get scalarProjection = Dot(ap, ab)
//The distance from a to q (the closes point on the line):
//float aq_distance = MyVector2.Dot(ap, ab) / MyVector2.Magnitude(ab);
//To get the closest point on the line:
//MyVector2 q = a + MyVector2.Normalize(ab) * aq_distance;
//Can we do better?
//Magnitude is defined as: Mathf.Sqrt((ab * ab))
//Normalization is defined as (ab / magnitude(ab))
//We get: q = a + (ab / magnitude(ab)) * (1 / magnitude(ab)) * dot(ap, ab)
//Ignore the q and the dot and we get: (ab / Mathf.Sqrt((ab * ab))) * (1 / Mathf.Sqrt((ab * ab))) = ab / (ab * ab)
//So we can use the square magnitude of ab and then we don't need to normalize ab (to get q), so we save two square roots, which is good because square root is a slow operation
//The normalized "distance" from a to the closest point, so between 0 and 1 if we are within the line segment
float distance = MyVector2.Dot(ap, ab) / MyVector2.SqrMagnitude(ab);
//This point may not be on the line segment, if so return one of the end points
float epsilon = MathUtility.EPSILON;
if (withinSegment && distance < 0f - epsilon)
{
return(a);
}
else if (withinSegment && distance > 1f + epsilon)
{
return(b);
}
else
{
//This works because a_b is not normalized and distance is [0,1] if distance is within ab
return(a + ab * distance);
}
}
//Help method to calculate the intersection point between two planes offset in normal direction by a width
private static MyVector2 GetIntersectionPoint(MyVector2 a, MyVector2 b, MyVector2 c, float halfWidth, bool isTopPoint)
{
//Direction of the lines going to and from point b
MyVector2 beforeDir = MyVector2.Normalize(b - a);
MyVector2 afterDir = MyVector2.Normalize(c - b);
MyVector2 beforeNormal = GetNormal(a, b);
MyVector2 afterNormal = GetNormal(b, c);
//Compare the normals!
//normalDirFactor is used to determine if we want to top point (same direction as normal)
float normalDirFactor = isTopPoint ? 1f : -1f;
//If they are the same it means we have a straight line and thus we cant do plane-plane intersection
//if (beforeNormal.Equals(afterNormal))
//When comparing the normals, we cant use the regular small value because then
//the line width goes to infinity when doing plane-plane intersection
float dot = MyVector2.Dot(beforeNormal, afterNormal);
//Dot is 1 if the point in the same dir and -1 if the point in the opposite dir
float one = 1f - 0.01f;
if (dot > one || dot < -one)
{
MyVector2 averageNormal = MyVector2.Normalize((afterNormal + beforeNormal) * 0.5f);
MyVector2 intersectionPoint = b + averageNormal * halfWidth * normalDirFactor;
return(intersectionPoint);
}
else
{
//Now we can calculate where the plane starts
MyVector2 beforePlanePos = b + beforeNormal * halfWidth * normalDirFactor;
MyVector2 afterPlanePos = b + afterNormal * halfWidth * normalDirFactor;
//Calculate the intersection point
//We know they are intersecting, so we don't need to test that
MyVector2 intersectionPoint = Intersections.GetPlanePlaneIntersectionPoint(beforePlanePos, beforeNormal, afterPlanePos, afterNormal);
return(intersectionPoint);
}
}
//
// Is a point p between point a and b (we assume all 3 are on the same line)
//
public static bool IsPointBetweenPoints(MyVector2 a, MyVector2 b, MyVector2 p)
{
bool isBetween = false;
//Entire line segment
MyVector2 ab = b - a;
//The intersection and the first point
MyVector2 ap = p - a;
//Need to check 2 things:
//1. If the vectors are pointing in the same direction = if the dot product is positive
//2. If the length of the vector between the intersection and the first point is smaller than the entire line
if (MyVector2.Dot(ab, ap) > 0f && MyVector2.SqrMagnitude(ab) >= MyVector2.SqrMagnitude(ap))
{
isBetween = true;
}
return(isBetween);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector2.Angle() but we are using MyVector2
public static float AngleBetween(MyVector2 from, MyVector2 to, bool shouldNormalize = true)
{
//from and to should be normalized
//But sometimes they are already normalized and then we dont need to do it again
if (shouldNormalize)
{
from = MyVector2.Normalize(from);
to = MyVector2.Normalize(to);
}
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector2.Dot(from, to);
//This shouldn't happen but may happen because of floating point precision issues
dot = Mathf.Clamp(dot, -1f, 1f);
float angleRad = Mathf.Acos(dot);
return(angleRad);
}
//Plane-plane intersection
public static bool PlanePlane(MyVector2 planePos_1, MyVector2 planeNormal_1, MyVector2 planePos_2, MyVector2 planeNormal_2)
{
bool areIntersecting = false;
float dot = MyVector2.Dot(planeNormal_1, planeNormal_2);
//Debug.Log(dot);
//No intersection if the planes are parallell
//The are parallell if the dot product is 1 or -1
//To avoid floating point precision issues we can add a small value
float one = 1f - MathUtility.EPSILON;
if (dot < one && dot > -one)
{
areIntersecting = true;
}
return(areIntersecting);
}
//2d
public static float GetSignedDistanceFromPointToPlane(Plane2 plane, MyVector2 pointPos)
{
float distance = MyVector2.Dot(plane.normal, pointPos - plane.pos);
return(distance);
}
