/// <summary>
/// Determine which "clock direction" (e.g. clockwise or counter-clockwise) the other
/// vector is from this vector, assuming both are "rooted" at (0,0)
/// </summary>
/// <remarks>
/// This is basically the calculation for the determinant of the two vectors.
/// </remarks>
/// <param name="_other">The other vector</param>
/// <returns>-1: Clockwise, 1: CounterClockwise, 0: Colinear</returns>
public int ClockDirection(SVector2d _other)
{
var left = X * _other.Y;
var right = Y * _other.X;
return((right > left) ? -1 : (left > right) ? 1 : 0);
}
/// <summary>
/// The square of the distance between this vector and another vector (assumed to be
/// point vectors)
/// </summary>
/// <param name="_other">The other vector</param>
/// <returns>The distance squared between this and another vector</returns>
public double DistanceSquared(SVector2d _other)
{
var xx = X - _other.X;
var yy = Y - _other.Y;
return(xx * xx + yy * yy);
}
/// <summary>
/// Return true if the vector AB to vector AC is counterclockwise. It is
/// counterclockwise when the determinant is positive. Clockwise: Negative.
/// Co-linear: 0
/// </summary>
/// <remarks>This is really just a determinant of vectors AB and AC</remarks>
/// <param name="_a"></param>
/// <param name="_b"></param>
/// <param name="_c"></param>
/// <returns>-1: Clockwise, 1: CounterClockwise, 0: Colinear</returns>
private static int ClockDirection(SVector2d _a, SVector2d _b, SVector2d _c)
{
var left = (_c.Y - _a.Y) * (_b.X - _a.X);
var right = (_b.Y - _a.Y) * (_c.X - _a.X);
return(left <right ? -1 : left> right ? 1 : 0);
}
/// <summary>
/// Determine if a point is inside the "box" formed by the endpoints of this segment
/// </summary>
/// <param name="_point">The point to test</param>
/// <returns>
/// TRUE if a point in space is inside the "box" formed by the two endpoints of this
/// segment
/// </returns>
public bool HasInBox(SVector2d _point)
{
if (_point.X < m_point1.X && _point.X < m_point2.X)
{
return(false);
}
if (_point.X > m_point1.X && _point.X > m_point2.X)
{
return(false);
}
if (_point.Y < m_point1.Y && _point.Y < m_point2.Y)
{
return(false);
}
if (_point.Y > m_point1.Y && _point.Y > m_point2.Y)
{
return(false);
}
return(true);
}
/// <summary> /// Determine if this and another vector form acute angles with each other /// </summary> /// <param name="_other">The other vector</param> /// <returns>TRUE if the vectors are acute</returns> public bool AreAcute(SVector2d _other) => NormalizedDot(_other) > 0;
/// <summary> /// Determine if this and another vector form obtuse angles with each other /// </summary> /// <param name="_other">The other vector</param> /// <returns>TRUE if the vectors are obtuse</returns> public bool AreObtuse(SVector2d _other) => NormalizedDot(_other) < 0;
/// <summary> /// Determine if this and another vector are orthogonal- perpendicular /// </summary> /// <param name="_other">The other vector</param> /// <returns>TRUE if the vectors are orthogonal</returns> public bool AreOrthogonal(SVector2d _other) => NormalizedDot(_other).IsClose(0);
/// <summary>
/// Determine if this and another vector are parallel and pointing in the same direction
/// </summary>
/// <param name="_other">The other vector</param>
/// <returns>TRUE if the vectors are parallel and in the same direction</returns>
public bool AreParallelSameDir(SVector2d _other)
{
var dot = NormalizedDot(_other);
return(dot.IsClose(1));
}
/// <summary>
/// Construct with axis coordinates of the endpoints
/// </summary>
/// <param name="_x1"></param>
/// <param name="_y1"></param>
/// <param name="_x2"></param>
/// <param name="_y2"></param>
public LineSegment(double _x1, double _y1, double _x2, double _y2)
{
m_point1 = new SVector2d(_x1, _y1);
m_point2 = new SVector2d(_x2, _y2);
}
/// <summary> /// Calculate the dot-product of this vector and another vector. This is also equal to /// the cosine of the angle between the two vectors. /// </summary> /// <param name="_other">The other vector</param> /// <returns>The normalized dot-product, a scalar value</returns> public double CosineOfAngleBetween(SVector2d _other) => Dot(_other) / (Length * _other.Length);
/// <summary> /// Calculate the dot-product of this vector and another vector. This is also equal to /// the cosine of the angle between the two vectors. /// </summary> /// <param name="_other">The other vector</param> /// <returns>The normalized dot-product, a scalar value</returns> public double NormalizedDot(SVector2d _other) => Dot(_other) / (Length * _other.Length);
/// <summary> /// Return a vector equal to another vector minus this vector /// </summary> /// <param name="_other">The vector to subtract this vector from</param> /// <returns> /// A vector pointing from "this" (a position vector) to another position vector. /// </returns> public SVector2d PointTo(SVector2d _other) => _other - this;
/// <summary> /// Return a vector equal to this vector plus another vector /// </summary> /// <param name="_other">The vector to add to this one</param> /// <returns>A vector representing the sum of two vectors</returns> public SVector2d AddTo(SVector2d _other) => this + _other;
/// <summary> /// Calculate the dot-product of this vector and another vector /// </summary> /// <param name="_other">The other vector</param> /// <returns>The dot-product, a scalar value</returns> public double Dot(SVector2d _other) => X * _other.X + Y * _other.Y;
/// <summary>
/// Determine if this (point) vector travelling at a specific speed can intersect a
/// second (point) vector travelling at a given linear velocity
/// </summary>
/// <param name="_myPosition">The position of "me"</param>
/// <param name="_mySpeed">The speed at which this point can travel</param>
/// <param name="_otherPosition">The position of the target point</param>
/// <param name="_otherVelocity">The velocity of the target point</param>
/// <param name="_interceptPosition">
/// If interception is possible, the point of interception
/// </param>
/// <param name="_interceptTime">
/// If interception is possible, the time of interception
/// </param>
/// <returns>
/// The velocity vector that this point should use in order to intercept, or
/// <see cref="SVector2d.NotAVector"/> if interception is not possible
/// </returns>
public static SVector2d Intercept(
SVector2d _myPosition,
double _mySpeed,
SVector2d _otherPosition,
SVector2d _otherVelocity,
out SVector2d _interceptPosition,
out double _interceptTime)
{
// First check- Are we already on top of the target? If so, its valid and we're done
if (_myPosition.AreSame(_otherPosition))
{
_interceptPosition = _myPosition;
_interceptTime = 0;
return(SVector2d.Zero); // (0,0)
}
// Set "out" parameters as if a failure occurred.
_interceptPosition = NotAVector;
_interceptTime = double.NaN;
// Check- Am I moving? Be gracious about exception throwing even though negative
// speed is undefined.
if (_mySpeed <= 0)
{
return(NotAVector); // No interception
}
var otherSpeed = _otherVelocity.Length;
var vectorFromOther = _myPosition - _otherPosition;
var distanceToOther = vectorFromOther.Length;
// Check- Is the other thing not moving? If it isn't, the calcs don't work because
// we can't use the Law of Cosines
if (otherSpeed.IsClose(0))
{
_interceptPosition = _otherPosition;
_interceptTime = distanceToOther / _mySpeed;
}
else // Everything looks OK for the Law of Cosines approach
{
var cosTheta = vectorFromOther.Dot(_otherVelocity) / (distanceToOther * otherSpeed);
var a = _mySpeed * _mySpeed - otherSpeed * otherSpeed;
var b = 2 * distanceToOther * otherSpeed * cosTheta;
var c = -distanceToOther * distanceToOther;
if (!CMath.QuadraticSolver(a, b, c, out var t1, out var t2))
{
return(SVector2d.NotAVector);
}
if (t1 < 0 && t2 < 0)
{
return(SVector2d.NotAVector);
}
else if (t1 > 0 && t2 > 0)
{
_interceptTime = Math.Min(t1, t2);
}
else
{
_interceptTime = Math.Max(t1, t2);
}
_interceptPosition = _otherPosition + _otherVelocity * _interceptTime;
}
// Calculate the resulting velocity based on the time and intercept position
var velocity = _interceptPosition - _myPosition;
return(velocity.WithNewLength(_mySpeed));
}
/// <summary> /// Return the angle (radians) between this and the provided vector /// </summary> /// <remarks>Another way of saying SVector2.ToRadians( _other - this )</remarks> /// <param name="_other"></param> /// <returns></returns> public double AngleBetween(SVector2d _other) => Math.Atan2(_other.Y - Y, _other.X - X);
/// <summary> /// Is another vector the "same" as this vector? "Same" implies "really close", as /// opposed to "double==double" /// </summary> /// <param name="_other">The vector to compare to this one</param> /// <returns>TRUE if the X,Y values are "close"</returns> public bool AreSame(SVector2d _other) => X.IsClose(_other.X) && Y.IsClose(_other.Y);
/// <summary> /// Return the angle (radians) as if using the law of cosines, where "this" is the /// mid-point (where the interesting angle is) of the triangle formed between this and /// the two points provided. /// </summary> /// <param name="_first">the first one of the two other points in the triangle</param> /// <param name="_second">the second one of the two other points in the triangle</param> /// <returns></returns> public double AngleBetween(SVector2d _first, SVector2d _second) => AngleBetween(_first) - AngleBetween(_second);
/// <summary> /// The distance between this vector and another vector /// </summary> /// <param name="_other">The other vector</param> /// <returns>The distance between this and another vector</returns> public double Distance(SVector2d _other) => Math.Sqrt(DistanceSquared(_other));
/// <summary>
/// Construct with two vectors
/// </summary>
/// <param name="_point1"></param>
/// <param name="_point2"></param>
public LineSegment(SVector2d _point1, SVector2d _point2)
{
m_point1 = _point1;
m_point2 = _point2;
}