/// <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 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>
/// Gets the rotation from matrix
/// </summary>
/// <param name="matrix">Rotation from matrix</param>
/// <param name="axis">Around axis</param>
/// <returns>Angle of rotation (0 - 2PI)</returns>
public double GetRotation(IMatrix44 matrix, Axis axis)
{
double rot = 0.0;
double y = 0.0;
switch (axis)
{
case Axis.XAxis:
rot = GeomOperation.GetAngle(matrix.AxisZ, AxisZ);
y = AxisZ | (matrix.AxisX * matrix.AxisZ).Normalize;
break;
case Axis.YAxis:
rot = GeomOperation.GetAngle(matrix.AxisX, AxisX);
y = AxisX | (matrix.AxisY * matrix.AxisX).Normalize;
break;
case Axis.ZAxis:
rot = GeomOperation.GetAngle(matrix.AxisY, AxisY);
y = AxisY | (matrix.AxisZ * matrix.AxisY).Normalize;
break;
}
if (y.IsLesser(0))
{
rot = 2 * Math.PI - rot;
}
return(rot);
}
/// <summary>
/// Calculate Points On Region.
/// </summary>
/// <returns>returns the point on region</returns>
private IPoint3D GetPointOnRegion()
{
IPoint3D pointOnRegion = new Point3D(OffsetX, OffsetY, 0.0);
IMatrix44 matrix = GeomOperation.GetMatrixPlane(region);
return(matrix.TransformToGCS(pointOnRegion));
}
public static Polygon3D Convert(IPolyLine3D polyline)
{
var points = new List <IPoint3D>(polyline.Count);
GeomOperation.ConvertPolylineToPointList(polyline, points, false, 5);
var polygon = new Polygon3D(points.Select(p => p.ToMediaPoint()));
return(polygon);
}
internal ParabolaProperty(IArcSegment3D parabolicArc)
{
Matrix = new Matrix44();
IsValid = CalculateParabolaProperty(parabolicArc);
if (!IsValid)
{
var seg = new ArcSegment3D(parabolicArc.StartPoint, parabolicArc.IntermedPoint, parabolicArc.EndPoint);
ParabolaLength = GeomOperation.GetLength(seg);
}
}
/// <summary>
/// Concatenates a rotation matrix which rotates a vector around another vector to this matrix.
/// </summary>
/// <param name="rotationAngle">The angle of rotation in degrees</param>
/// <param name="normalVector">The orientation of the axis to rotate around</param>
public void Rotate(double rotationAngle, Vector3D normalVector)
{
if (rotationAngle.IsZero() == false)
{
double cosValue = System.Math.Cos(rotationAngle);
double sineValue = System.Math.Sin(rotationAngle);
AxisX = GeomOperation.Rotate(AxisX, normalVector, cosValue, sineValue);
AxisY = GeomOperation.Rotate(AxisY, normalVector, cosValue, sineValue);
AxisZ = GeomOperation.Rotate(AxisZ, normalVector, cosValue, sineValue);
}
}
/// <summary>
/// Remove segment[index], adaptes geometry so that shorter segment changes positions
/// </summary>
/// <param name="index">Segment index</param>
public void RemoveAtAdaptShorter(int index)
{
int segmentCount = segments.Count;
if (index == -1)
{
throw new ArgumentException("Segment is not included in polyline");
}
bool isClosed = IsClosed;
//if (isClosed || !(index == 0) || !(index == segmentCount - 1))
if (isClosed || ((index != 0) && (index != segmentCount - 1)))
{
if (segmentCount > 2)
{
ISegment3D prevSegment;
ISegment3D nextSegment;
if (index == 0)
{
prevSegment = segments[segmentCount - 1];
nextSegment = segments[1];
}
else if (index == segmentCount - 1)
{
prevSegment = segments[index - 1];
nextSegment = segments[0];
}
else
{
prevSegment = segments[index - 1];
nextSegment = segments[index + 1];
}
double prevLength = GeomOperation.GetLength(prevSegment);
double nextLength = GeomOperation.GetLength(nextSegment);
if (prevLength < nextLength)
{
prevSegment.EndPoint = nextSegment.StartPoint;
}
else
{
nextSegment.StartPoint = prevSegment.EndPoint;
}
}
}
UnsubscribeEventsFromSegment(segments[index]);
segments.RemoveAt(index);
}
/// <summary>
/// Calculate normal of line passing through start and end. Default plane is XY
/// </summary>
/// <param name="start">Start point</param>
/// <param name="end">End point</param>
/// <returns>Returns the normal</returns>
private static Vector3D GetNormal(IPoint3D start, IPoint3D end)
{
Vector3D localZ = new Vector3D(0, 0, 1);
Vector3D localX = Subtract(end, start).Normalize;
if (GeomOperation.IsCollinear(localX, localZ, MathConstants.ZeroGeneral))
{
localZ = new Vector3D(0, 1, 0);
}
Vector3D localY = localZ * localX;
return(localX * localY);
}
/// <summary>
/// Calculate relative position of point on polyline.
/// </summary>
/// <param name="region">region</param>
/// <param name="point">Point</param>
/// <param name="relativeX">Relative Position along local X axis</param>
/// <param name="relativeY">Relative Position along local Y axis</param>
/// <param name="toleranceLevel">Tolerance Level</param>
/// <returns>True if point exist in polyline</returns>
public static bool GetRelativePosition(IRegion3D region, IPoint3D point, ref double relativeX, ref double relativeY, double toleranceLevel = MathConstants.ZeroWeak)
{
IMatrix44 matrix = GeomOperation.GetMatrixPlane(region);
IPoint3D pointInLCS = matrix.TransformToLCS(point);
if (pointInLCS.Z.IsZero() == false)
{
return(false);
}
relativeX = pointInLCS.X;
relativeY = pointInLCS.Y;
return(true);
}
/// <summary>
/// Gets all angle change of parabola
/// </summary>
/// <returns>Angle change</returns>
internal double GetAngleChange()
{
double angle = 0;
if (Property.IsValid)
{
Vector3D vect1 = new Vector3D();
Vector3D vect2 = new Vector3D();
GeomOperation.GetTangentOnSegment(this, 0.0, ref vect1);
GeomOperation.GetTangentOnSegment(this, 1.0, ref vect2);
angle = GeomOperation.GetAngle(vect1, vect2);
}
else
{
var seg = new ArcSegment3D(StartPoint, IntermedPoint, EndPoint);
angle = GeomOperation.GetArcAngle(seg);
}
return(angle);
}
/// <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>
/// 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(WM.Point3D origin, WM.Point3D pointOnX)
{
Vector3D localX = new Vector3D(pointOnX.X - origin.X, pointOnX.Y - origin.Y, pointOnX.Z - origin.Z);
Vector3D localZ = new Vector3D(0, 0, 1);
if (GeomOperation.IsCollinear(localZ, localX, MathConstants.ZeroWeak)) //MathConstants.ZeroGeneral))
{
if (localX.DirectionZ > 0)
{
localZ = new Vector3D(-1, 0, 0);
}
else
{
localZ = new Vector3D(1, 0, 0);
}
}
Vector3D localY = (localZ * localX);
localZ = (localX * localY).Normalize;
Matrix44 matrix = new Matrix44(origin, localX.Normalize, localY.Normalize, localZ);
return(matrix);
}
/// <summary>
/// Calculate perpendicular vector of parabola.
/// </summary>
/// <param name="axisAngle">Axis Angle</param>
/// <returns>perpendicular vector of parabola</returns>
internal Vector3D GetY(double axisAngle)
{
return(GeomOperation.Rotate(localY, normalizedVector, axisAngle));
}
/// <summary>
/// SetToRotation - Resets this matrix to a rotation matrix. The resulting matrix
/// will rotate a arcPoint counter-clockwise around the specified axis
/// by the specified angle.
/// </summary>
/// <param name="angle">The angle of rotation in radians.</param>
/// <param name="axisVector">The orientation of the axis to rotate around.</param>
/// <param name="rotationPoint">rotationPoint on the axis of rotation.</param>
public void SetToRotation(double angle, Vector3D axisVector, Point3D rotationPoint)
{
// translate the rotation center back to the origin
SetToTranslation(-GeomOperation.ToVector(rotationPoint));
// rotate about the axis at the origin
Vector3D normalAxisVect = axisVector.Normalize;
double angle2 = -0.5 * angle;
double x = Math.Sin(angle2) * normalAxisVect.DirectionX;
double y = Math.Sin(angle2) * normalAxisVect.DirectionY;
double z = Math.Sin(angle2) * normalAxisVect.DirectionZ;
double w = Math.Cos(angle2);
double twoX = 2.0 * x;
double twoY = 2.0 * y;
double twoZ = 2.0 * z;
double threeX = x * twoX;
double threeY = y * twoY;
double threeZ = z * twoZ;
double wtwoX = w * twoX;
double wtwoY = w * twoY;
double wtwoZ = w * twoZ;
double xtwoY = x * twoY;
double xtwoZ = x * twoZ;
double ytwoZ = y * twoZ;
Matrix44 tempMat = new Matrix44();
tempMat.matrixElement[0, 0] = 1.0 - (threeY + threeZ);
tempMat.matrixElement[0, 1] = xtwoY - wtwoZ;
tempMat.matrixElement[0, 2] = xtwoZ + wtwoY;
tempMat.matrixElement[0, 3] = 0.0;
tempMat.matrixElement[1, 0] = xtwoY + wtwoZ;
tempMat.matrixElement[1, 1] = 1.0 - (threeX + threeZ);
tempMat.matrixElement[1, 2] = ytwoZ - wtwoX;
tempMat.matrixElement[1, 3] = 0.0;
tempMat.matrixElement[2, 0] = xtwoZ - wtwoY;
tempMat.matrixElement[2, 1] = ytwoZ + wtwoX;
tempMat.matrixElement[2, 2] = 1.0 - (threeX + threeY);
tempMat.matrixElement[2, 3] = 0.0;
tempMat.matrixElement[3, 0] = 0.0;
tempMat.matrixElement[3, 1] = 0.0;
tempMat.matrixElement[3, 2] = 0.0;
tempMat.matrixElement[3, 3] = 1.0;
Matrix44 m1 = tempMat * this;
this.Copy(m1);
// translate back to the center
tempMat.SetToTranslation(GeomOperation.ToVector(rotationPoint));
Matrix44 m2 = tempMat * this;
this.Copy(m2);
}
/// <summary>
/// Transforms a Point from LCS to GCS
/// </summary>
/// <param name="point">Point to be tranformed [In LCS].</param>
/// <returns>Point in GCS</returns>
public WM.Point3D TransformToGCS(WM.Point3D point)
{
return(GeomOperation.Add(Multiply(point, this), this.Origin));
}
/// <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))));
}
/// <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 WM.Point3D TransformToLCS(WM.Point3D point)
{
return((WM.Point3D)Multiply(this, GeomOperation.SubWM(point, this.Origin)));
}
/// <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>
/// Get LCS based on local Z axis up
/// In case local X is vertical then Y local axis follows Y global axis
/// </summary>
/// <param name="originPoint">Origin point of returned LCS</param>
/// <param name="pointAxisX">IPoint3D</param>
/// <returns>Matrix44 as LCS</returns>
public override IMatrix44 GetCoordinateSystemMatrix(IPoint3D originPoint, IPoint3D pointAxisX)
{
return(GeomOperation.GetLCSMatrix(originPoint, pointAxisX));
}
/// <summary>
/// Calculate Point on parabola.
/// </summary>
/// <param name="segmentParabola">Segment Parabola</param>
/// <param name="relativePosition">Relative Position</param>
/// <param name="point">Point On Parabola</param>
/// <returns>true if given input are valid</returns>
internal bool GetPointOnParabola(IArcSegment3D segmentParabola, double relativePosition, ref IPoint3D point)
{
if (segmentParabola == null)
{
throw new ArgumentNullException();
}
if (!IsValid)
{
IPoint3D a = segmentParabola.StartPoint;
IPoint3D b = segmentParabola.IntermedPoint;
point = GeomOperation.Add(GeomOperation.Multiply(b, relativePosition), GeomOperation.Multiply(a, 1 - relativePosition));
return(true);
}
double x = 0;
// CIH - nejak jsem ty Abs nepochopil
// if (Math.Abs(relativePosition).IsLesser(0.0))
// {
// x = parabolaProperty.ParabolicArcX1;
// }
// else if (Math.Abs(relativePosition - 1.0).IsLesser(0.0))
// {
// x = parabolaProperty.ParabolicArcX2;
// }
// else if (Math.Abs(parabolaProperty.ParabolaAxisAngle).IsLesser(0.0))
if (relativePosition.IsEqual(0.0))
{
x = ParabolicArcX1;
}
else if (relativePosition.IsEqual(1.0))
{
x = ParabolicArcX2;
}
//else if (ParabolaAxisAngle.IsLesser(0.0))
//{
// x = relativePosition * (ParabolicArcX2 - ParabolicArcX1) + ParabolicArcX1;
//}
else
{
x = 0.0;
int it = 100;
double pom = relativePosition * ParabolaLength + GetLengthToCurve(ParabolicArcX1);
double caa = 4 * ParabolaAxisAngle * ParabolaAxisAngle;
double len = pom;
do
{
x += len / Math.Sqrt(1 + caa * x * x);
len = pom - GetLengthToCurve(x);
}while ((!len.IsZero(MathConstants.ZeroGeneral)) && (--it > 0));
//while ((!Math.Abs(len).IsLesser(0.0)) && (--it > 0));
if (Math.Abs(len) > 1e-8)
{
return(false);
}
}
Vector3D normline = Matrix.AxisX;
Vector3D ortho = Matrix.AxisY;
point = GeomOperation.Add(segmentParabola.IntermedPoint, normline * x + ParabolaAxisAngle * ortho * x * x);
return(true);
}
