//3d
//Same math as in 2d case
public static MyVector3 GetClosestPointOnLine(Edge3 e, MyVector3 p, bool withinSegment)
{
MyVector3 a = e.p1;
MyVector3 b = e.p2;
//Assume the line goes from a to b
MyVector3 ab = b - a;
//Vector from start of the line to the point outside of line
MyVector3 ap = p - a;
//The normalized "distance" from a to the closest point, so [0,1] if we are within the line segment
float distance = MyVector3.Dot(ap, ab) / MyVector3.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);
}
}
//Calculate the angle between two vectors
//This angle should be measured in 360 degrees (Vector3.Angle is measured in 180 degrees)
//Should maybe be moved to _Geometry??
//In 3d space [radians]
//https://stackoverflow.com/questions/5188561/signed-angle-between-two-3d-vectors-with-same-origin-within-the-same-plane
//https://math.stackexchange.com/questions/2906314/how-to-calculate-angle-between-two-vectors-in-3d-with-clockwise-or-counter-clock
public static float AngleFromToCCW(MyVector3 from, MyVector3 to, MyVector3 upRef)
{
//This is only working in 2d space
//float angleDegrees = Quaternion.FromToRotation(to.ToVector3(), from.ToVector3()).eulerAngles.y;
from = MyVector3.Normalize(from);
to = MyVector3.Normalize(to);
upRef = MyVector3.Normalize(upRef);
float angleRad = AngleBetween(from, to, shouldNormalize: false);
//To get 0-2pi (360 degrees) we can use the determinant [a, b, u] = (a x b) dot u
//Where u is a reference up vector
//Remember that the cross product is not alwayspointing up - it can change to down depending on how the vectors are aligned
//Which is why we need a fixed reference up
MyVector3 cross = MyVector3.Cross(from, to);
float determinant = MyVector3.Dot(MyVector3.Cross(from, to), upRef);
//Debug.Log(determinant);
if (determinant >= 0f)
{
return(angleRad);
}
else
{
return((Mathf.PI * 2f) - angleRad);
}
}
//3d
private static MyVector3 GetIntersectionCoordinate(Plane3 plane, Ray3 ray)
{
float denominator = MyVector3.Dot(-plane.normal, ray.dir);
MyVector3 vecBetween = plane.pos - ray.origin;
float t = MyVector3.Dot(vecBetween, -plane.normal) / denominator;
MyVector3 intersectionPoint = ray.origin + ray.dir * t;
return(intersectionPoint);
}
//
// Alternative 3. Rotation Minimising Frame (also known as "Parallel Transport Frame" or "Bishop Frame")
//
//Gets its stability by incrementally rotating a coordinate system (= frame) as it is translate along the curve
//Has to be computed for the entire curve because we need the previous frame (previousTransform) belonging to a point before this point
//Is initalized by using "Fixed Up" or "Frenet Normal"
public static MyQuaternion GetOrientation_RotationFrame(MyVector3 position, MyVector3 tangent, InterpolationTransform previousTransform)
{
/*
* //This version is from https://pomax.github.io/bezierinfo/#pointvectors3d
* //Reflect the known frame onto the next point, by treating the plane through the curve at the point exactly between the next and previous points as a "mirror"
* MyVector3 v1 = position - previousTransform.position;
*
* float c1 = MyVector3.Dot(v1, v1);
*
* MyVector3 riL = previousTransform.Right - v1 * (2f / c1) * MyVector3.Dot(v1, previousTransform.Right);
*
* MyVector3 tiL = previousTransform.Forward - v1 * (2f / c1) * MyVector3.Dot(v1, previousTransform.Forward);
*
* //This gives the next point a tangent vector that's essentially pointing in the opposite direction of what it should be, and a normal that's slightly off-kilter
* //reflect the vectors of our "mirrored frame" a second time, but this time using the plane through the "next point" itself as "mirror".
* MyVector3 v2 = tangent - tiL;
*
* float c2 = MyVector3.Dot(v2, v2);
*
* //Now we can calculate the normal and right vector belonging to this orientation
* MyVector3 right = riL - v2 * (2f / c2) * MyVector3.Dot(v2, riL);
*
* //The source has right x tangent, but then every second normal is flipped
* MyVector3 normal = MyVector3.Cross(tangent, right);
*
* MyQuaternion orientation = new MyQuaternion(tangent, normal);
*/
//This version is from Game Programming Gems 2: The Parallel Transport Frame
//They generate the same result and this one is easier to understand
//The two tangents
MyVector3 T1 = previousTransform.Forward;
MyVector3 T2 = tangent;
//You move T1 to the new position, so A is a vector going from the new position
MyVector3 A = MyVector3.Cross(T1, T2);
//This is the angle between T1 and T2
float alpha = Mathf.Acos(MyVector3.Dot(T1, T2) / (MyVector3.Magnitude(T1) * MyVector3.Magnitude(T2)));
//Now rotate the previous frame around axis A with angle alpha
MyQuaternion F1 = previousTransform.orientation;
MyQuaternion F2 = MyQuaternion.RotateQuaternion(F1, alpha * Mathf.Rad2Deg, A);
MyQuaternion orientation = F2;
return(orientation);
}
//The angle between two vectors 0 <= angle <= 180
//Same as Vector3.Angle() but we are using MyVector3
public static float AngleBetween(MyVector3 from, MyVector3 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 = MyVector3.Normalize(from);
to = MyVector3.Normalize(to);
}
//dot(a_normalized, b_normalized) = cos(alpha) -> acos(dot(a_normalized, b_normalized)) = alpha
float dot = MyVector3.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);
}
//
// Point-plane relations
//
//https://gamedevelopment.tutsplus.com/tutorials/understanding-sutherland-hodgman-clipping-for-physics-engines--gamedev-11917
//Notice that the plane normal doesnt have to be normalized
//The signed distance from a point to a plane
//- Positive distance denotes that the point p is on the front side of the plane (in the direction of the plane normal)
//- Negative means it's on the back side
//3d
public static float GetSignedDistanceFromPointToPlane(Plane3 plane, MyVector3 pointPos)
{
float distance = MyVector3.Dot(plane.normal, pointPos - plane.pos);
return(distance);
}
