/// <summary>
/// Create and return a matrix based on given two points
/// The matrix represents a coordinate system.
/// </summary>
/// <param name="origin">Origin of the new coordinate system</param>
/// <param name="pointOnX">Point on which new X axis lies</param>
/// <returns>The matrix which represents the co-ordinate system</returns>
public static Matrix44 GetLCSMatrix(IPoint3D origin, IPoint3D pointOnX)
{
//Vector3D localX = GeomOperation.Subtract(pointOnX, origin).Normalize;
Vector3D localX = GeomOperation.Subtract(pointOnX, origin);
Vector3D localZ = new Vector3D(0, 0, 1);
if (GeomOperation.IsCollinear(localZ, localX, MathConstants.ZeroWeak)) //MathConstants.ZeroGeneral))
{
// When X is vertical then Z goes in X dir or -X
//localZ = new Vector3D(0, 1, 0);
if (localX.DirectionZ > 0)
{
localZ = new Vector3D(-1, 0, 0);
}
else
{
localZ = new Vector3D(1, 0, 0);
}
}
//Vector3D localY = (localZ * localX).Normalize;
//Matrix44 matrix = new Matrix44(origin, localX, localY);
Vector3D localY = (localZ * localX);
localZ = (localX * localY).Normalize;
Matrix44 matrix = new Matrix44(origin, localX.Normalize, localY.Normalize, localZ);
return(matrix);
}
/// <summary>
/// Check Arc segment is collinear.
/// </summary>
/// <param name="arcSegment">Arc Segment</param>
/// <param name="toleranceLevel">Tolerance Level</param>
/// <returns>return true if two vectors are collinear</returns>
public static bool IsCollinear(IArcSegment3D arcSegment, double toleranceLevel = MathConstants.ZeroGeneral)
{
Vector3D vector1 = GeomOperation.Subtract(arcSegment.StartPoint, arcSegment.IntermedPoint);
Vector3D vector2 = GeomOperation.Subtract(arcSegment.IntermedPoint, arcSegment.EndPoint);
return(IsCollinear(vector1, vector2, toleranceLevel));
}
/// <summary>
/// Create and return a matrix based on given points
/// The matrix represents a coordinate system.
/// </summary>
/// <param name="origin">Origin of the new coordinate system</param>
/// <param name="pointOnX">Point on which new X axis lies</param>
/// <param name="pointInPlane">Third point on the plane</param>
/// <param name="plane">Specifies pointInPlane is in XY plane or in XZ plane</param>
/// <returns>Return the matrix which represents the co-ordinate system</returns>
public static IMatrix44 GetLCSMatrix(IPoint3D origin, IPoint3D pointOnX, IPoint3D pointInPlane, Plane plane)
{
// Vector3D localX = GeomOperation.Subtract(pointOnX, origin).Normalize;
Vector3D localX = GeomOperation.Subtract(pointOnX, origin);
Vector3D localY = GeomOperation.Subtract(pointInPlane, origin);
if (GeomOperation.IsCollinear(localY, localX, MathConstants.ZeroGeneral))
{
//throw new NotSupportedException("Given points are in a line. Unable to create coordinate system");
return(null);
}
if (plane == Plane.XY)
{
//Vector3D localZ = localX * localY;
//localY = (localZ * localX).Normalize;
////TODO localY = localY.Normalize; only
//Matrix44 matrix = new Matrix44(origin, localX, localY);
Vector3D localZ = localX * localY;
localY = (localZ * localX);
localZ = (localX * localY).Normalize;
IMatrix44 matrix = new Matrix44(origin, localX.Normalize, localY.Normalize, localZ);
return(matrix);
}
else if (plane == Plane.ZX)
{
//Vector3D localZ = (localY * localX).Normalize;
//Matrix44 matrix = new Matrix44(origin, localX, localZ);
Vector3D localZ = (localY * localX);
localY = (localX * localZ).Normalize;
IMatrix44 matrix = new Matrix44(origin, localX.Normalize, localZ.Normalize, localY);
return(matrix);
}
throw new NotSupportedException("Third point should be either in XY plane or in ZX plane");
}
/// <summary>
/// Calculate the property of parabola.
/// </summary>
/// <param name="parabolicArc">Segment Parabola</param>
/// <returns>true if given parabola segment is valid to calculate Length</returns>
private bool CalculateParabolaProperty(IArcSegment3D parabolicArc)
{
Vector3D cb = GeomOperation.Subtract(parabolicArc.EndPoint, parabolicArc.IntermedPoint);
Vector3D ncb = cb.Normalize;
Vector3D ca = GeomOperation.Subtract(parabolicArc.StartPoint, parabolicArc.IntermedPoint);
Vector3D nca = ca.Normalize;
double d = ~(ncb * nca);
//if (Math.Abs(d).IsLesser(0.0, MathConstants.ZeroGeneral))
if (Math.Abs(d).IsZero(1e-5))
{
Vector3D vv = GeomOperation.Subtract(parabolicArc.EndPoint, parabolicArc.StartPoint);
Vector3D normalizedLine = vv.Normalize;
double parabolaX1 = normalizedLine | ca;
double parabolaX2 = normalizedLine | cb;
if (parabolaX2 < parabolaX1)
{
normalizedLine = -normalizedLine;
parabolaX1 = -parabolaX1;
parabolaX2 = -parabolaX2;
}
ParabolaAxisAngle = 0.0;
ParabolicArcX1 = parabolaX1;
ParabolicArcX2 = parabolaX2;
ParabolaLength = vv.Magnitude;
//SwapParabolicSegment(ref normalizedLine, ref parabolaX1, ref parabolaX2);
//ParabolaLength = ~vv;
//ParabolicArcX1 = normalizedLine | ca;
//ParabolicArcX2 = normalizedLine | cb;
//double parX1 = ParabolicArcX1;
//double parX2 = ParabolicArcX2;
//SwapParabolicSegment(ref normalizedLine, ref parX1, ref parX2);
//ParabolicArcX1 = parX1;
//ParabolicArcX2 = parX2;
//Vector3D localY = new Vector3D(0, 0, 0);
//if ((normalizedLine.DirectionZ - 1.0).IsZero())
//{
// localY.DirectionY = 1.0;
//}
//else if (normalizedLine.DirectionZ.IsZero())
//{
// localY.DirectionZ = 1.0;
//}
//else
//{
// localY.DirectionX = -normalizedLine.DirectionX;
// localY.DirectionY = -normalizedLine.DirectionY;
// localY.DirectionZ = ((normalizedLine.DirectionX * normalizedLine.DirectionX) + (normalizedLine.DirectionY * normalizedLine.DirectionY)) / normalizedLine.DirectionZ;
// localY = localY.Normalize;
//}
//Vector3D vectOrtho = GeomOperation.Rotate(localY, normalizedLine, 0.0);
//ParabolaAxisAngle = (vectOrtho | ca) / (ParabolicArcX1 * ParabolicArcX1);
//Matrix.AxisX = normalizedLine;
//Matrix.AxisY = vectOrtho;
//Matrix.AxisZ = (normalizedLine * vectOrtho).Normalize;
//ParabolaLength = GetLengthToCurve(ParabolicArcX2) - GetLengthToCurve(ParabolicArcX1);
//ParabolaLength = ~vv;
return(false);
}
double parabolaAngle = 0.0;
Vector3D vectorOrtho = new Vector3D();
Vector3D normalizedVector = new Vector3D();
bool repeat = false;
do
{
if (!CalculateParabolaPlane(ca, cb, ref parabolaAngle, ref vectorOrtho, ref normalizedVector, repeat))
{
return(false);
}
Vector3D normalLine = vectorOrtho * normalizedVector;
double parabolaX1 = normalLine | ca;
double parabolaX2 = normalLine | cb;
if (parabolaX2 < parabolaX1)
{
normalLine = -normalLine;
parabolaX1 = -parabolaX1;
parabolaX2 = -parabolaX2;
}
ParabolicArcX1 = parabolaX1;
ParabolicArcX2 = parabolaX2;
//ParabolicArcX1 = normalLine | ca;
//ParabolicArcX2 = normalLine | cb;
//double parX1 = ParabolicArcX1;
//double parX2 = ParabolicArcX2;
//SwapParabolicSegment(ref normalLine, ref parX1, ref parX2);
//ParabolicArcX1 = parX1;
//ParabolicArcX2 = parX2;
ParabolaAxisAngle = (vectorOrtho | ca) / (ParabolicArcX1 * ParabolicArcX1);
Matrix.AxisX = normalLine;
Matrix.AxisY = vectorOrtho;
Matrix.AxisZ = normalizedVector;
ParabolaLength = GetLengthToCurve(ParabolicArcX2) - GetLengthToCurve(ParabolicArcX1);
double d1 = Math.Sqrt(((parabolicArc.StartPoint.X - parabolicArc.IntermedPoint.X) * (parabolicArc.StartPoint.X - parabolicArc.IntermedPoint.X)) + ((parabolicArc.StartPoint.Y - parabolicArc.IntermedPoint.Y) * (parabolicArc.StartPoint.Y - parabolicArc.IntermedPoint.Y)) + ((parabolicArc.StartPoint.Z - parabolicArc.IntermedPoint.Z) * (parabolicArc.StartPoint.Z - parabolicArc.IntermedPoint.Z)));
double d2 = Math.Sqrt(((parabolicArc.EndPoint.X - parabolicArc.IntermedPoint.X) * (parabolicArc.EndPoint.X - parabolicArc.IntermedPoint.X)) + ((parabolicArc.EndPoint.Y - parabolicArc.IntermedPoint.Y) * (parabolicArc.EndPoint.Y - parabolicArc.IntermedPoint.Y)) + ((parabolicArc.EndPoint.Z - parabolicArc.IntermedPoint.Z) * (parabolicArc.EndPoint.Z - parabolicArc.IntermedPoint.Z)));
if (!repeat)
{
if (ParabolaLength.IsLesser(d1 + d2))
{
repeat = true;
}
}
else
{
repeat = false;
}
}while (repeat);
return(true);
}
/// <summary>
/// Transforms a Point from GCS to LCS
/// </summary>
/// <param name="point">Point to be tranformed [In GCS].</param>
/// <returns>Point in LCS</returns>
public IPoint3D TransformToLCS(IPoint3D point)
{
return(GeomOperation.ToPoint(Multiply(this, GeomOperation.Subtract(point, this.Origin))));
}