public static void Test_GetWestNorthVectors_02()
{
const double maxDev = 1E-6;
for (int i = -1; i <= 1; i += 2)
{
var v = new VectorD3D(0, 0, i);
var westNorth = PolylineMath3D.GetWestNorthVectors(new LineD3D(PointD3D.Empty, (PointD3D)v));
var west = westNorth.Item1;
var north = westNorth.Item2;
Assert.AreEqual(west.Length, 1, maxDev); // is west normalized
Assert.AreEqual(north.Length, 1, maxDev); // is north normalized
Assert.AreEqual(VectorD3D.DotProduct(west, v), 0, maxDev); // is west perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(north, v), 0, maxDev); // is north perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(west, north), 0, maxDev); // is west perpendicular to north
var matrix = Altaxo.Geometry.Matrix4x3.NewFromBasisVectorsAndLocation(west, north, v, PointD3D.Empty);
Assert.AreEqual(matrix.Determinant, 1, maxDev);
var westExpected = new VectorD3D(-1, 0, 0);
var northExpected = new VectorD3D(0, -i, 0);
Assert.AreEqual(westExpected.X, west.X, maxDev);
Assert.AreEqual(westExpected.Y, west.Y, maxDev);
Assert.AreEqual(westExpected.Z, west.Z, maxDev);
Assert.AreEqual(northExpected.X, north.X, maxDev);
Assert.AreEqual(northExpected.Y, north.Y, maxDev);
Assert.AreEqual(northExpected.Z, north.Z, maxDev);
}
}
/// <summary>
/// Makes a given vector n orthogonal to another vector v. This is done by adding a fraction of v to n, so that the new vector is orthogonal to v.
/// After this, the vector is normalized.
/// </summary>
/// <param name="n">Given vector.</param>
/// <param name="v">A vector, to which the returned vector should be perpendicular.</param>
/// <returns>A new vector n+t*v, so that this vector is orthogonal to v and normalized.</returns>
public static VectorD3D GetNormalizedVectorOrthogonalToVector(VectorD3D n, VectorD3D v)
{
double nv_vv = VectorD3D.DotProduct(n, v) / VectorD3D.DotProduct(v, v);
var result = VectorD3D.CreateNormalized(n.X - v.X * nv_vv, n.Y - v.Y * nv_vv, n.Z - v.Z * nv_vv);
return(result);
}
public static void Test_GetPolylinePointsWithWestAndNorth_02()
{
const double maxDev = 1E-6;
var rnd = new System.Random();
var testPoints = new PointD3D[1024];
testPoints[0] = PointD3D.Empty;
testPoints[1] = new PointD3D(0, 0.1, 0); // first line segment always in y direction, so that north is in z direction and west in -x direction
for (int i = 2; i < testPoints.Length; ++i)
{
testPoints[i] = new PointD3D(rnd.NextDouble(), rnd.NextDouble(), rnd.NextDouble());
}
var result = PolylineMath3D.GetPolylinePointsWithWestAndNorth(testPoints).ToArray();
for (int i = 1; i < result.Length; ++i)
{
var forwardRaw = result[i].Position - result[i - 1].Position;
var west = result[i].WestVector;
var north = result[i].NorthVector;
Assert.AreNotEqual(forwardRaw.Length, 0); // GetPolylinePointsWithWestAndNorth should only deliver non-empty segments
var forward = forwardRaw.Normalized;
Assert.AreEqual(west.Length, 1, maxDev); // is west normalized
Assert.AreEqual(north.Length, 1, maxDev); // is north normalized
Assert.AreEqual(VectorD3D.DotProduct(west, forward), 0, maxDev); // is west perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(north, forward), 0, maxDev); // is north perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(west, north), 0, maxDev); // is west perpendicular to north
var matrix = Altaxo.Geometry.Matrix4x3.NewFromBasisVectorsAndLocation(west, north, forward, PointD3D.Empty);
Assert.AreEqual(matrix.Determinant, 1, maxDev); // west-north-forward are a right handed coordinate system
}
}
/// <summary>
/// Gets a projection matrix that projects a point in the direction given by <paramref name="v"/> onto a plane with is given by an arbitrary point on the plane <paramref name="p"/> and the plane's normal <paramref name="q"/>.
/// </summary>
/// <param name="v">The projection direction. Not required to be normalized.</param>
/// <param name="p">An arbitrary point onto the projection plane.</param>
/// <param name="q">The projection plane's normal. Not required to be normalized.</param>
/// <returns>The projection matrix that projects a point in the direction given by <paramref name="v"/> onto a plane with is given by an arbitrary point on the plane <paramref name="p"/> and the plane's normal <paramref name="q"/>.</returns>
public static Matrix4x3 GetProjectionToPlane(VectorD3D v, PointD3D p, VectorD3D q)
{
double OneByQV = 1 / VectorD3D.DotProduct(q, v);
double DotPQ = p.X * q.X + p.Y * q.Y + p.Z * q.Z;
return(new Matrix4x3(
1 - q.X * v.X * OneByQV, -q.X * v.Y * OneByQV, -q.X * v.Z * OneByQV,
-q.Y * v.X * OneByQV, 1 - q.Y * v.Y * OneByQV, -q.Y * v.Z * OneByQV,
-q.Z * v.X * OneByQV, -q.Z * v.Y * OneByQV, 1 - q.Z * v.Z * OneByQV,
DotPQ * v.X * OneByQV, DotPQ * v.Y * OneByQV, DotPQ * v.Z * OneByQV
));
}
/// <summary>
/// Gets the fractional index of the point on a line that has a certain distance to another point <paramref name="ps"/>.
/// </summary>
/// <param name="p0">The start point of the line.</param>
/// <param name="p1">The end point of the line.</param>
/// <param name="ps">The other point.</param>
/// <param name="distance">The given distance.</param>
/// <returns>A relative index on the line [0..1] for the point on the line that has the provided distance to the point <paramref name="ps"/>. If the point <paramref name="ps"/> is too far away, the result will be double.NaN.
/// If the point <paramref name="ps"/> is too close, the result can be outside the interval [0,1].</returns>
public static double GetFractionalIndexOfPointOnLineInGivenDistanceToAnotherPoint(PointD3D p0, PointD3D p1, PointD3D ps, double distance)
{
VectorD3D p0s = p0 - ps;
VectorD3D seg = p1 - ps;
double dotps = VectorD3D.DotProduct(p0s, seg);
double slen_p0s = p0s.SquareOfLength;
double slen_seg = seg.SquareOfLength;
double sqrt = Math.Sqrt(dotps * dotps + (distance * distance - slen_p0s) * slen_seg);
double t1 = (-dotps - sqrt) / slen_seg;
double t2 = (-dotps + sqrt) / slen_seg;
return(t1 >= 0 ? t1 : t2);
}
/// <summary>
/// Creates a transformation matrix that does the following: First, it converts a 2D point into a 3D coordinate system with the origin given by <paramref name="p"/>, and the unit vectors <paramref name="e"/> and <paramref name="n"/>.
/// Then the thus created 3D point is projected in the direction of <paramref name="v"/> onto a plane that is defined by the same point <paramref name="p"/> on the plane and the plane's normal <paramref name="q"/>.
/// </summary>
/// <param name="e">East vector: Spans one dimension of the projection of the 2D points to a 3D plane.</param>
/// <param name="n">North vector: Spans the other dimension of the projection of the 2D input points to a 3D plane.</param>
/// <param name="v">Direction of the projection of the 3D points to a plane.</param>
/// <param name="p">Origin of the coordinate system, and point on the projection plane, too.</param>
/// <param name="q">Normal of the projection plane.</param>
/// <returns>Matrix that transforms 2D points to a plane. (The 2D points are in fact 3D points with a z-coordinate that is ignored.</returns>
public static Matrix4x3 Get2DProjectionToPlaneToPlane(VectorD3D e, VectorD3D n, VectorD3D v, PointD3D p, VectorD3D q)
{
double qn = VectorD3D.DotProduct(q, e);
double qw = VectorD3D.DotProduct(q, n);
double qv = VectorD3D.DotProduct(q, v);
double qn_qv = qn / qv;
double qw_qv = qw / qv;
return(new Matrix4x3(
e.X - v.X * qn_qv, e.Y - v.Y * qn_qv, e.Z - v.Z * qn_qv,
n.X - v.X * qw_qv, n.Y - v.Y * qw_qv, n.Z - v.Z * qw_qv,
0, 0, 0,
p.X, p.Y, p.Z));
}
public static void Test_GetWestNorthVectors_01()
{
const double maxDev = 1E-6;
for (int iTheta = -89; iTheta < 90; ++iTheta)
{
double theta = Math.PI * iTheta / 180.0;
for (int iPhi = 0; iPhi < 360; ++iPhi)
{
double phi = Math.PI * iPhi / 180.0;
var v = new VectorD3D(Math.Cos(phi) * Math.Cos(theta), Math.Sin(phi) * Math.Cos(theta), Math.Sin(theta));
Assert.AreEqual(v.Length, 1, maxDev); // is forward normalized
var rawNorth = PolylineMath3D.GetRawNorthVectorAtStart(v);
Assert.AreEqual(rawNorth.X, 0);
Assert.AreEqual(rawNorth.Y, 0);
Assert.AreEqual(rawNorth.Z, 1);
var westNorth = PolylineMath3D.GetWestNorthVectors(new LineD3D(PointD3D.Empty, (PointD3D)v));
var west = westNorth.Item1;
var north = westNorth.Item2;
Assert.AreEqual(west.Length, 1, maxDev); // is west normalized
Assert.AreEqual(north.Length, 1, maxDev); // is north normalized
Assert.AreEqual(VectorD3D.DotProduct(west, v), 0, maxDev); // is west perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(north, v), 0, maxDev); // is north perpendicular to forward
Assert.AreEqual(VectorD3D.DotProduct(west, north), 0, maxDev); // is west perpendicular to north
var matrix = Altaxo.Geometry.Matrix4x3.NewFromBasisVectorsAndLocation(west, north, v, PointD3D.Empty);
Assert.AreEqual(matrix.Determinant, 1, maxDev);
var westExpected = new VectorD3D(-Math.Sin(phi), Math.Cos(phi), 0);
var northExpected = new VectorD3D(-Math.Cos(phi) * Math.Sin(theta), -Math.Sin(phi) * Math.Sin(theta), Math.Cos(theta));
Assert.AreEqual(westExpected.X, west.X, maxDev);
Assert.AreEqual(westExpected.Y, west.Y, maxDev);
Assert.AreEqual(westExpected.Z, west.Z, maxDev);
Assert.AreEqual(northExpected.X, north.X, maxDev);
Assert.AreEqual(northExpected.Y, north.Y, maxDev);
Assert.AreEqual(northExpected.Z, north.Z, maxDev);
}
}
}
/// <summary>
/// Makes a given vector n orthogonal to another vector v. This is done by adding a fraction of v to n, so that the new vector is orthogonal to v.
/// </summary>
/// <param name="n">Given vector.</param>
/// <param name="v">A vector, to which the returned vector should be perpendicular.</param>
/// <returns>A new vector n+t*v, so that this vector is orthogonal to v (but not neccessarily normalized).</returns>
public static VectorD3D GetVectorOrthogonalToVector(VectorD3D n, VectorD3D v)
{
double nv_vv = VectorD3D.DotProduct(n, v) / VectorD3D.DotProduct(v, v);
return(new VectorD3D(n.X - v.X * nv_vv, n.Y - v.Y * nv_vv, n.Z - v.Z * nv_vv));
}
/// <summary>
/// Calculates a vector which is symmectrical to the provided vector <paramref name="n"/> with respected to the symmetry plane given by the normal normal <paramref name="q"/>.
/// The result is the same as if a ray is reflected on a miiror described by the plane, thus
/// an incident vector is resulting in an outcoming vector, and an outcoming vector is resulting in an incident vector.
/// </summary>
/// <param name="n">The vector for which to find the symmectrical counterpart. Not required to be normalized.</param>
/// <param name="q">Normal of a plane where the vector n is mirrored. Not required to be normalized.</param>
/// <returns>A vector which is symmectrical to the provided vector <paramref name="n"/> with respected to the symmetry plane given by the normal normal <paramref name="q"/>.</returns>
public static VectorD3D GetVectorSymmetricalToPlane(VectorD3D n, VectorD3D q)
{
double two_nq_qq = 2 * VectorD3D.DotProduct(n, q) / VectorD3D.DotProduct(q, q);
return(new VectorD3D(n.X - q.X * two_nq_qq, n.Y - q.Y * two_nq_qq, n.Z - q.Z * two_nq_qq));
}
/// <summary>
/// Calculates the counterpart of the provided vector <paramref name="n"/>, so that this vector <paramref name="n"/> and it's conterpart are symmetrical,
/// with the symmetry line provided by vector <paramref name="q"/>.
/// </summary>
/// <param name="n">The vector for which to find the symmectrical counterpart. Not required to be normalized.</param>
/// <param name="q">Symmetry line.</param>
/// <returns>The counterpart of the provided vector <paramref name="n"/>, so that this vector <paramref name="n"/> and it's conterpart are symmetrical,
/// with the symmetry line given by vector <paramref name="q"/>.</returns>
public static VectorD3D GetVectorSymmetricalToVector(VectorD3D n, VectorD3D q)
{
double two_nq_qq = 2 * VectorD3D.DotProduct(n, q) / VectorD3D.DotProduct(q, q);
return(new VectorD3D(q.X * two_nq_qq - n.X, q.Y * two_nq_qq - n.Y, q.Z * two_nq_qq - n.Z));
}