/// <summary>
/// Gets the intersection point coordinate between the current plane and another specified plane.
/// </summary>
/// <param name="p">Plane that should cross the plane on the seeked intersection line.</param>
/// <returns>The parametric line at the intersection between the 2 planes.</returns>
/// <remarks>
/// Formula :
///
/// A(x1 + at) + B(y1 + bt) + C(z1 + ct) + D = 0
///
/// Then giving t :
///
/// -(Ax1 + By1 + Cz1 + D)
/// t = ------------------------
/// Aa + Bb + Cc
///
/// Coordinates are then :
///
/// a(Ax1 + By1 + Cz1 + D) b(Ax1 + By1 + Cz1 + D) c(Ax1 + By1 + Cz1 + D)
/// x = x1 - ------------------------ y = y1 - ------------------------ z = z1 - ------------------------
/// Aa + Bb + Cc Aa + Bb + Cc Aa + Bb + Cc
///
/// </remarks>
public ParametricLine GetIntersection(Plane p)
{
// Orientation of the intersection between the plane is a vector perpendicular to the normals of both planes.
Cartesian3dCoordinate orientation = this.Normal.CrossProduct(p.Normal);
// Base on the plane equation, we need to adapt to use the correct resolution to avoid usses coming from potential divide by 0.0.
Cartesian3dCoordinate point;
if (TryGetPointAtIntersectionOnZ(p, out Cartesian3dCoordinate rZ))
{
point = rZ;
}
else if (TryGetPointAtIntersectionOnY(p, out Cartesian3dCoordinate rY))
{
point = rY;
}
else if (TryGetPointAtIntersectionOnX(p, out Cartesian3dCoordinate rX))
{
point = rX;
}
else
{
throw new NotImplementedException("The method was not able to find a way to get the intersection between the 2 Plane with the currently implemented formula.");
}
return(new ParametricLine(point, orientation));
}
/// <summary>
/// Gets the Cross Product between this vector and another vector.
/// </summary>
/// <param name="v">Vector with which we will calculate the cross product.</param>
/// <returns>New coordinates containing the cross product from the 2 vectors.</returns>
public Cartesian3dCoordinate CrossProduct(Cartesian3dCoordinate v)
{
return(new Cartesian3dCoordinate(
(this.Y * v.Z) - (this.Z * v.Y),
(this.Z * v.X) - (this.X * v.Z),
(this.X * v.Y) - (this.Y * v.X)
));
}
/// <summary>
/// Create a new plane based on his normal and a random point that is part of the plane.
/// </summary>
/// <param name="normal">Vector of the normal to the plane. The normal is a perpendicular vector to the plane.</param>
/// <param name="point">Point included in the plane and not included in the normal.</param>
public Plane(Cartesian3dCoordinate normal, Cartesian3dCoordinate point)
{
this.Normal = normal;
// As : 0 = ax + by + cz + d
// -d = ax + by + cz
// d = -(ax + by + cz)
this.D = -((normal.X * point.X) + (normal.Y * point.Y) + (normal.Z * point.Z));
}
/// <summary>
/// Convert a Cartesian Point to his Homogeneous equivalent.
/// </summary>
/// <param name="cc">Cartesian coordinate to convert.</param>
/// <returns>The Vector converted to a Homogeneous representation.</returns>
public static HomogeneousCoordinate ConvertToHomogeneous(Cartesian3dCoordinate cc)
{
return(new HomogeneousCoordinate(
cc.X,
cc.Y,
cc.Z,
1.0
));
}
/// <summary>
/// Try to calculate a point at the intersection of 2 plane using linear combination.
/// As we only have 2 plane to find a point, the Z value is predefined to 0.0.
/// </summary>
/// <param name="p">Second Plane with which we search the intersection point.</param>
/// <param name="cc">Calculated coordinate of one of the intersection point between the 2 Planes in case of success.</param>
/// <returns>A boolean indicating whther this function was able to evaluate the expected point or not.</returns>
private bool TryGetPointAtIntersectionOnZ(Plane p, out Cartesian3dCoordinate cc)
{
// Avoid to divide by 0.0 at the end. (see last formula)
if (this.B.IsZero())
{
cc = Cartesian3dCoordinate.Zero;
return(false);
}
// We have 2 formula with 3 variables :
// A1 X + B1 Y + C1 Z + D1 = 0
// A2 X + B2 Y + C2 Z + D2 = 0
//
// We will limit equations to 2 variables by predefining Z to 0.0 :
// A1 X + B1 Y = -D1
// A2 X + B2 Y = -D2
//
// We will then use linear combination between the 2 formulas :
// A1 X B2 + B1 Y B2 = -D1 B2
// A2 X B1 + B2 Y B1 = -D2 B1
//
// B1 Y B2 = -D1 B2 - A1 X B2
// B2 Y B1 = -D2 B1 - A2 X B1
//
// This give us :
//
// A2 X B1 - A1 X B2 = D1 B2 - D2 B1
//
// (A2 B1 - A1 B2) X = D1 B2 - D2 B1
//
// D1 B2 - D2 B1
// X = ---------------
// A2 B1 - A1 B2
double divisor = (p.A * this.B) - (this.A * p.B);
if (divisor.IsZero())
{
cc = Cartesian3dCoordinate.Zero;
return(false);
}
double x = ((this.D * p.B) - (p.D * this.B)) / divisor;
// With this X calculated and Z to 0.0, we can then resolve one of the equations for Y :
//
// A1 X + D1
// Y = - -----------
// B1
cc = new Cartesian3dCoordinate(
x,
-((this.A * x) + this.D) / this.B,
0.0
);
return(true);
}
/// <summary>
/// Get the vector corresponding to this vector in the current referential as if this vector was related to another referential vector.
/// </summary>
/// <param name="referential">Vector correspoding to the X axis vector in the referential coordinates.</param>
/// <returns>A new coordinates sets in the global coordinates system corresponding to the translated coordinates.</returns>
/// <remarks>
/// Assuming a transposition of A on B.
///
/// Let V = a X b
/// Let s = ||V|| (sin of angle)
/// Let c = A . B (cos of angle)
///
/// Rotation matrix can be calculated by:
///
/// R = I + [V]x + [V]ײ (1 − c) / s²
///
/// As a reminder:
///
/// ( 0 -Vz Vy ) ( 1 0 0 ) ( 1 -Vz Vy )
/// [V]x = ( Vz 0 -Vx ) I = ( 0 1 0 ) [V]x + I = ( Vz 1 -Vx )
/// ( -Vy Vx 0 ) ( 0 0 1 ) ( -Vy Vx 1 )
/// </remarks>
public Cartesian3dCoordinate TransposeFromReferential(Cartesian3dCoordinate referential)
{
if (referential.IsOnX)
{
if (referential.X > 0.0)
{
// No need to change referential when we are on the correct one.
return(this);
}
else
{
// Formula is not working for reverse referential vector, we just reverse the current coordinates.
return(new Cartesian3dCoordinate(-this.X, -this.Y, -this.Z));
}
}
Cartesian3dCoordinate origin = new Cartesian3dCoordinate(1, 0, 0);
// Calculate formula variables.
Cartesian3dCoordinate v = origin.CrossProduct(referential);
double c = origin.DotProduct(referential);
// Calculate last part.
double cPart = (1.0 - c) / ((v.X * v.X) + (v.Y * v.Y) + (v.Z * v.Z));
// Calculate poer values.
double x2 = v.X * v.X;
double y2 = v.Y * v.Y;
double z2 = v.Z * v.Z;
// Calculate the rotation matrix
double[,] matrix = new double[3, 3]
{
{
1.0 - ((z2 + y2) * cPart),
(v.Y * -v.X * cPart) - v.Z,
(v.Z * v.X * cPart) + v.Y,
},
{
(v.X * v.Y * cPart) + v.Z,
1.0 - ((z2 + x2) * cPart),
(v.Z * v.Y * cPart) - v.X,
},
{
(v.X * v.Z * cPart) - v.Y,
(v.Y * v.Z * cPart) + v.X,
1.0 - ((y2 + x2) * cPart),
},
};
// Apply rotation matrix to vector.
return(new Cartesian3dCoordinate(
(this.X * matrix[0, 0]) + (this.Y * matrix[0, 1]) + (this.Z * matrix[0, 2]),
(this.X * matrix[1, 0]) + (this.Y * matrix[1, 1]) + (this.Z * matrix[1, 2]),
(this.X * matrix[2, 0]) + (this.Y * matrix[2, 1]) + (this.Z * matrix[2, 2])));
}
/// <summary>
/// Try to calculate a point at the intersection of 2 plane using linear combination.
/// As we only have 2 plane to find a point, the X value is predefined to 0.0.
/// </summary>
/// <param name="p">Second Plane with which we search the intersection point.</param>
/// <returns>Coordinate of one of the intersection point between the 2 Planes.</returns>
private bool TryGetPointAtIntersectionOnX(Plane p, out Cartesian3dCoordinate cc)
{
// Avoid to divide by 0.0 at the end. (see last formula)
if (this.C.IsZero())
{
cc = Cartesian3dCoordinate.Zero;
return(false);
}
// We have 2 formula with 3 variables :
// A1 X + B1 Y + C1 Z + D1 = 0
// A2 X + B2 Y + C2 Z + D2 = 0
//
// We will limit equations to 2 variables by predefining X to 0.0 :
// B1 Y + C1 Z = -D1
// B2 Y + C2 Z = -D2
//
// We will then use linear combination between the 2 formulas :
// B1 Y C2 + C1 Z C2 = -D1 C2
// B2 Y C1 + C2 Z C1 = -D2 C1
//
// C1 Z C2 = -D1 C2 - B1 Y C2
// C2 Z C1 = -D2 C1 - B2 Y C1
//
// This give us :
//
// B2 Y C1 - B1 Y C2 = D1 C2 - D2 C1
//
// (B2 C1 - B1 C2) Y = D1 C2 - D2 C1
//
// D1 C2 - D2 C1
// Y = ---------------
// B2 C1 - B1 C2
double divisor = (p.B * this.C) - (this.B * p.C);
if (divisor.IsZero())
{
cc = Cartesian3dCoordinate.Zero;
return(false);
}
double y = ((this.D * p.C) - (p.D * this.C)) / divisor;
// With this Y calculated and X to 0.0, we can then resolve one of the equations for Z :
//
// B1 Y + D1
// Z = - -----------
// C1
cc = new Cartesian3dCoordinate(
0.0,
y,
-((this.B * y) + this.D) / this.C
);
return(true);
}
/// <summary>
/// Gets the 2D coordinates as seen from a calculated plane.
/// </summary>
/// <param name="cc">3D coordinates to convert.</param>
/// <returns>The converted 2D coordinates.</returns>
private Cartesian2dCoordinate GetFromProjection(Cartesian3dCoordinate cc)
{
// Project the coordinates on the reference
ParametricLine projectionLine = this.Plane.GetPerpendicular(cc);
Cartesian3dCoordinate planeCoordinate = this.Plane.GetIntersection(projectionLine);
Cartesian3dCoordinate ccOnX = planeCoordinate.TransposeFromReferential(this.Plane.Normal);
return(GetFromFront(ccOnX));
}
/// <summary>
/// Create a new vector resulting from a rotation around a specific axis using the rodrigues' formula.
/// </summary>
/// <param name="k">Vector used as the rotation axis.</param>
/// <param name="theta">Angle fo rotation to execute in radians.</param>
/// <returns>The coordinates of the rotated vector.</returns>
/// <remarks>
/// Rotation formula :
///
/// V : This vector.
/// K : Vector used a rotation axis.
///
/// R = V cos theta + (K × V) sin theta + K (K . V) (1 − cos theta)
/// </remarks>
public Cartesian3dCoordinate RotateAroundVector(Cartesian3dCoordinate k, double theta)
{
// Precalculate intermediates values used more than once.
double cosTheta = Trigonometry.Cos(theta);
double sinTheta = Trigonometry.Sin(theta);
k = k.Normalize();
return((this * cosTheta) + ((-sinTheta) * CrossProduct(k)) + ((1.0 - cosTheta) * this.DotProduct(k) * k));
}
/// <summary>
/// Compares two objects and returns a value indicating whether one is less than, equal to, or greater than the other.
/// </summary>
/// <param name="x">The first object to compare.</param>
/// <param name="y">The second object to compare.</param>
/// <returns>
/// A signed integer that indicates the relative values of x and y:
/// - If less than 0, x is less than y.
/// - If 0, x equals y.
/// - If greater than 0, x is greater than y.
/// </returns>
public int Compare(Cartesian3dCoordinate x, Cartesian3dCoordinate y)
{
// Check perfect equality first to avoid further calculations.
if (x.IsSamePoint(y))
{
return(0);
}
Cartesian2dCoordinate x2 = m_Converter.ConvertTo2d(x);
Cartesian2dCoordinate y2 = m_Converter.ConvertTo2d(y);
return(Compare(x2, y2));
}
/// <summary>
/// Convert a Cartesian Vector to his Spherical equivalent.
/// </summary>
/// <param name="cc">Cartesian coordinate to convert.</param>
/// <returns>The Vector converted to a Spherical representation.</returns>
public static SphericalVector ConvertToSpherical(Cartesian3dCoordinate cc)
{
// Pre validate value to avoid double.NaN in calculation result.
if (cc.IsZero)
{
return(SphericalVector.Origin);
}
return(new SphericalVector(
Math.Atan2(cc.Y, cc.X),
Trigonometry.Acos(cc.Z / cc.Magnitude)
));
}
/// <summary>
/// Gets the intersection point coordinate between the current plane and the specified Vector.
/// </summary>
/// <param name="v">Vector that should cross the plane at the seeked intersection point.</param>
/// <returns>The cartesian coordinates of the intersection point between the Vector and the Plane.</returns>
/// <remarks>
/// This is a simplified version of the formula from the ParametricLine formula.
/// All part of the formula known as been equals to 0 are not calculated.
/// </remarks>
public Cartesian3dCoordinate GetIntersection(Cartesian3dCoordinate v)
{
double divisor = GetSumOfAbcProduct(v);
if (divisor.IsZero())
{
throw new InvalidOperationException("Cannot Get Intersection between a Plane and a parallel vector.");
}
double planePart = -this.D / divisor;
return(new Cartesian3dCoordinate(
v.X * planePart,
v.Y * planePart,
v.Z * planePart
));
}
/// <summary>
/// Gets the dot product between this vector in a 1 X 3 format and a second one in a 3 X 1 format.
/// </summary>
/// <param name="c">Vector that will be used as a 3 X 1 matrix.</param>
/// <returns>Calculated vector from the dot product of the 2 vectors.</returns>
public double DotProduct(Cartesian3dCoordinate c)
{
return((this.X * c.X) + (this.Y * c.Y) + (this.Z * c.Z));
}
/// <summary>
/// Calculate the theta angle relative to a provided vector.
/// </summary>
/// <param name="x">Vector that will be used to calculate the theta</param>
/// <returns>Calculated theta value between the two vectors.</returns>
/// <remarks>
/// Formula :
///
/// V : This vector.
/// X : Vector to which the angle theta is calculated.
///
/// V . X
/// cos theta = -------------
/// ||X|| ||Y||
/// </remarks>
public double GetThetaTo(Cartesian3dCoordinate x)
{
return(Trigonometry.Acos(this.DotProduct(x) / (this.Magnitude * x.Magnitude)));
}
/// <summary> /// Gets a ParametricLine perpendicular to the plane and passing through a provided point. /// </summary> /// <param name="point">Point through which the perpendicular line to the plane should go.</param> /// <returns>A new line going through the provided point and the current plane.</returns> public ParametricLine GetPerpendicular(Cartesian3dCoordinate point) => new ParametricLine(point, this.Normal);
/// <summary> /// Gets the sum of products of the A, B and C factor of the plane and vector formula. /// </summary> /// <param name="cc">Parametric line with which we calculate the product.</param> /// <returns>Result of the sum of the product of the A, B and C factor of the plane and parametric line formula.</returns> private double GetSumOfAbcProduct(Cartesian3dCoordinate cc) => (this.A * cc.X) + (this.B * cc.Y) + (this.C * cc.Z);
/// <summary>
/// Create a ParametricLine between 2 lines.
/// </summary>
/// <param name="p1">Starting point of the line.</param>
/// <param name="p2">Ending point of the line.</param>
/// <returns>The Parametric line linking the provided points.</returns>
public static ParametricLine FromTwoPoints(Cartesian3dCoordinate p1, Cartesian3dCoordinate p2)
{
return(new ParametricLine(p1, new Cartesian3dCoordinate(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z)));
}
/// <summary> /// Gets the 2D coordinates as seen from the top. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromTop(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(cc.Y, -cc.X);
/// <summary>
/// Indicate if a specific point is above the plane or not.
/// </summary>
/// <param name="point">Point for which position relative to the plane will be checked.</param>
/// <returns>A boolean indicating whether the point is above the plane or not.</returns>
/// <remarks>Formula : ax + by + cz + d = 0</remarks>
public bool IsAbovePlane(Cartesian3dCoordinate point)
{
double v = GetSumOfAbcProduct(point) + this.D;
return(!v.IsZero() && v > 0.0);
}
/// <summary> /// Gets the 2D coordinates as seen from the left. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromLeft(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(cc.X, cc.Z);
/// <summary> /// Gets the 2D coordinates as seen from the right. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromRight(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(-cc.X, cc.Z);
/// <summary> /// Gets the 2D coordinates as seen from the back. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromBack(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(-cc.Y, cc.Z);
/// <summary> /// Evaluate if the 2 Cartesian3dCoordinate are concidered as equal points in this context. /// </summary> /// <param name="cc">Coordinates to compare to the current object.</param> /// <returns>A boolean indicating whether the 2 coordinates are equals or not.</returns> public bool IsSamePoint(Cartesian3dCoordinate cc) => this.X.IsEqualTo(cc.X) && this.Y.IsEqualTo(cc.Y) && this.Z.IsEqualTo(cc.Z);
/// <summary> /// Gets the 2D coordinates as seen from the bottom. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromBottom(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(cc.Y, cc.X);
/// <summary> /// Evaluate if the 2 Cartesian3dCoordinate are concidered as equal vector in this context. /// </summary> /// <param name="cc">Coordinates to compare to the current object.</param> /// <returns>A boolean indicating whether the 2 coordinates are equals or not.</returns> public bool IsSameVector(Cartesian3dCoordinate cc) => this.Normalize().IsSamePoint(cc.Normalize());
/// <summary> /// Gets the 2D coordinates as seen from the front. /// </summary> /// <param name="cc">3D coordinates to convert.</param> /// <returns>The converted 2D coordinates.</returns> private Cartesian2dCoordinate GetFromFront(Cartesian3dCoordinate cc) => new Cartesian2dCoordinate(cc.Y, cc.Z);
/// <summary>
/// Create a ParametricLine starting from a specific point and following a provided Vector.
/// </summary>
/// <param name="p">Initial point of the line.</param>
/// <param name="v">Vector providing the direction of the line.</param
public ParametricLine(Cartesian3dCoordinate p, Cartesian3dCoordinate v)
{
this.Point = p;
this.Vector = v;
}
/// <summary>
/// Create a new Plane.
/// </summary>
/// <param name="n">Normal used to define a plane.</param>
/// <param name="d">D factor of the formula 'ax + by + cz + d = 0' used to define a plane.</param>
public Plane(Cartesian3dCoordinate n, double d)
{
this.Normal = n;
this.D = d;
}
/// <summary> /// COnvert the provided 3D coordiantes to their projected 2D counterpart on the Plane used to create this Cartesian3dCoordinatesConverter. /// </summary> /// <param name="c3d">2D coordinates to project on the Pland and converts in 2D.</param> /// <returns>2D coordinates projectes on the Plane.</returns> public Cartesian2dCoordinate ConvertTo2d(Cartesian3dCoordinate c3d) => m_From3dTo2d(c3d);
/// <summary> /// Indicate if a specific point is on the plane or not. /// </summary> /// <param name="point">Point for which appartenance to the plane will be checked.</param> /// <returns>A boolean indicating whether the point is ont the plane or not.</returns> /// <remarks>Formula : ax + by + cz + d = 0</remarks> public bool IsOnPlane(Cartesian3dCoordinate point) => (GetSumOfAbcProduct(point) + this.D).IsZero();
