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
}
}
public PolylinePointD3DAsClass(VectorD3D forwardVector, VectorD3D westVector, VectorD3D northVector, PointD3D position)
{
Position = position;
WestVector = westVector;
NorthVector = northVector;
ForwardVector = forwardVector;
}
/// <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_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>
/// Creates the rotation matrix from axis and angle radian.
/// </summary>
/// <param name="u">The axis about which the rotation takes place.</param>
/// <param name="angleRadian">The rotation angle in radian.</param>
/// <param name="center">The center of rotation.</param>
/// <returns>Matrix that describes the drotation.</returns>
public static Matrix4x3 NewRotationFromAxisAndAngleRadian(VectorD3D u, double angleRadian, PointD3D center)
{
double cosTheta = Math.Cos(angleRadian);
double oMCosTheta = 1 - cosTheta;
double sinTheta = Math.Sin(angleRadian);
double m11 = cosTheta + u.X * u.X * oMCosTheta;
double m12 = u.X * u.Y * oMCosTheta + u.Z * sinTheta;
double m13 = u.Z * u.X * oMCosTheta - u.Y * sinTheta;
double m21 = u.X * u.Y * oMCosTheta - u.Z * sinTheta;
double m22 = cosTheta + u.Y * u.Y * oMCosTheta;
double m23 = u.Z * u.Y * oMCosTheta + u.X * sinTheta;
double m31 = u.X * u.Z * oMCosTheta + u.Y * sinTheta;
double m32 = u.Y * u.Z * oMCosTheta - u.X * sinTheta;
double m33 = cosTheta + u.Z * u.Z * oMCosTheta;
double offsetX = 0, offsetY = 0, offsetZ = 0;
if (center.X != 0.0 || center.Y != 0.0 || center.Z != 0.0)
{
offsetX = -center.X * m11 - center.Y * m21 - center.Z * m31 + center.X;
offsetY = -center.X * m12 - center.Y * m22 - center.Z * m32 + center.Y;
offsetZ = -center.X * m13 - center.Y * m23 - center.Z * m33 + center.Z;
}
return(new Matrix4x3(m11, m12, m13, m21, m22, m23, m31, m32, m33, offsetX, offsetY, offsetZ));
}
/// <summary>
/// Creates a transformation matrix that uses three basis vectors to construct the matrix that transform points expressed in the three basis vectors to points in
/// the coordinate system.
/// </summary>
/// <param name="xBasis">Basis vector for the x-direction.</param>
/// <param name="yBasis">Basis vector for the y-direction.</param>
/// <param name="zBasis">Basis vector for the z-direction.</param>
/// <returns>A transformation matrix that uses the three basis vectors, and a location</returns>
public static Matrix3x3 NewFromBasisVectors(VectorD3D xBasis, VectorD3D yBasis, VectorD3D zBasis)
{
return(new Matrix3x3(
xBasis.X, xBasis.Y, xBasis.Z,
yBasis.X, yBasis.Y, yBasis.Z,
zBasis.X, zBasis.Y, zBasis.Z));
}
/// <summary>
/// Creates a transformation matrix that uses three basis vectors, and a location to construct the matrix that transform points expressed in the three basis vectors to points in
/// the coordinate system.
/// </summary>
/// <param name="xBasis">Basis vector for the x-direction.</param>
/// <param name="yBasis">Basis vector for the y-direction.</param>
/// <param name="zBasis">Basis vector for the z-direction.</param>
/// <param name="origin">The origin of the coordinate system.</param>
/// <returns>A transformation matrix that uses the three basis vectors, and a location</returns>
public static Matrix4x3 NewFromBasisVectorsAndLocation(VectorD3D xBasis, VectorD3D yBasis, VectorD3D zBasis, PointD3D origin)
{
return(new Matrix4x3(
xBasis.X, xBasis.Y, xBasis.Z,
yBasis.X, yBasis.Y, yBasis.Z,
zBasis.X, zBasis.Y, zBasis.Z,
origin.X, origin.Y, origin.Z));
}
/// <summary>
/// Creates a transformation matrix that projects 2D points (in fact: 3D-points with ignored z-coordinate) to a plane that is defined by 2 vectors (<paramref name="e"/> and <paramref name="n"/>) and a point
/// on that plane <paramref name="p"/>. The x-coordinates of the original point is projected in the <paramref name="e"/> direction, the y-coordinate in the <paramref name="n"/> direction.
/// </summary>
/// <param name="e">East vector: direction, in which the x-coordinate of the original points is projected.</param>
/// <param name="n">North vector: direction, in which the y-coordinate of the original points is projected.</param>
/// <param name="p">The 3D point, which is the origin of the spanned plane (the original point with the coordinates (0,0) is projected to this point.</param>
/// <returns>A transformation matrix that projects 2D points (in fact: 3D-points with ignored z-coordinate) to a plane in 3D space.</returns>
public static Matrix4x3 Get2DProjectionToPlane(VectorD3D e, VectorD3D n, PointD3D p)
{
return(new Matrix4x3(
e.X, e.Y, e.Z,
n.X, n.Y, n.Z,
0, 0, 0,
p.X, p.Y, p.Z));
}
public RectangleD3D(PointD3D position, VectorD3D size)
{
_x = position.X;
_y = position.Y;
_z = position.Z;
_sizeX = size.X;
_sizeY = size.Y;
_sizeZ = size.Z;
}
public static Matrix4x3 NewTranslation(VectorD3D d)
{
return(new Matrix4x3(
1, 0, 0,
0, 1, 0,
0, 0, 1,
d.X, d.Y, d.Z,
1
));
}
/// <summary>
/// Inverse transform a vector p in such a way that the result will fullfill the relation p = result * matrix ( the * operator being the prepend transformation for vectors).
/// </summary>
/// <param name="p">The point p to inverse transform.</param>
/// <returns>The inverse transformation of point <paramref name="p"/>.</returns>
public VectorD3D InverseTransform(VectorD3D p)
{
return(new VectorD3D(
(-(M23 * M32 * p.X) + M22 * M33 * p.X + M23 * M31 * p.Y - M21 * M33 * p.Y - M22 * M31 * p.Z + M21 * M32 * p.Z) / Determinant,
(M13 * M32 * p.X - M12 * M33 * p.X - M13 * M31 * p.Y + M11 * M33 * p.Y + M12 * M31 * p.Z - M11 * M32 * p.Z) / Determinant,
(-(M13 * M22 * p.X) + M12 * M23 * p.X + M13 * M21 * p.Y - M11 * M23 * p.Y - M12 * M21 * p.Z + M11 * M22 * p.Z) / Determinant
));
}
/// <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>
/// Transforms the specified vector <paramref name="v"/>. For a vector transform, the offset elements M41..M43 are ignored.
/// The transformation is carried out as a prepend transformation, i.e. result = v * matrix (v considered as horizontal vector).
/// </summary>
/// <param name="v">The vector to transform.</param>
/// <returns>The transformed vector.</returns>
public VectorD3D Transform(VectorD3D v)
{
double x = v.X;
double y = v.Y;
double z = v.Z;
return(new VectorD3D(
x * M11 + y * M21 + z * M31,
x * M12 + y * M22 + z * M32,
x * M13 + y * M23 + z * M33
));
}
/// <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>
/// Creates the rotation matrix from axis and angle radian.
/// </summary>
/// <param name="u">The axis about which the rotation takes place.</param>
/// <param name="angleRadian">The rotation angle in radian.</param>
/// <returns>Matrix that describes the drotation.</returns>
public static Matrix3x3 CreateRotationMatrixFromAxisAndAngleRadian(VectorD3D u, double angleRadian)
{
double cosTheta = Math.Cos(angleRadian);
double oMCosTheta = 1 - cosTheta;
double sinTheta = Math.Sin(angleRadian);
double m11 = cosTheta + u.X * u.X * oMCosTheta;
double m12 = u.X * u.Y * oMCosTheta + u.Z * sinTheta;
double m13 = u.Z * u.X * oMCosTheta - u.Y * sinTheta;
double m21 = u.X * u.Y * oMCosTheta - u.Z * sinTheta;
double m22 = cosTheta + u.Y * u.Y * oMCosTheta;
double m23 = u.Z * u.Y * oMCosTheta + u.X * sinTheta;
double m31 = u.X * u.Z * oMCosTheta + u.Y * sinTheta;
double m32 = u.Y * u.Z * oMCosTheta - u.X * sinTheta;
double m33 = cosTheta + u.Z * u.Z * oMCosTheta;
return(new Matrix3x3(m11, m12, m13, m21, m22, m23, m31, m32, m33));
}
/// <summary>
/// Inverse transform a vector p in such a way that the result will fullfill the relation p = result * matrix ( the * operator being the prepend transformation for vectors).
/// </summary>
/// <param name="p">The vector p to inverse transform.</param>
/// <returns>The inverse transformation of point <paramref name="p"/>.</returns>
public VectorD2D InverseTransform(VectorD3D p)
{
return(new VectorD2D((M22 * p.X - M21 * p.Y) / Determinant, (M11 * p.Y - M12 * p.X) / Determinant));
}
/// <summary>
/// Initializes a new instance of the <see cref="Ray3D"/> class.
/// </summary>
/// <param name="origin">The origin of the line (one arbitrary point at the line).</param>
/// <param name="direction">The direction of the line.</param>
public Ray3D(PointD3D origin, VectorD3D direction)
{
_origin = origin;
_direction = direction;
}
public RectangleD3D WithSize(VectorD3D newSize)
{
return(new RectangleD3D(X, Y, Z, newSize.X, newSize.Y, newSize.Z));
}
/// <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));
}
public RectangleD3D WithSizePlus(VectorD3D sizeOffset)
{
return(new RectangleD3D(X, Y, Z, SizeX + sizeOffset.X, SizeY + sizeOffset.Y, SizeZ + sizeOffset.Z));
}
/// <summary>
/// Gets the distance of a point <paramref name="a"/> to a plane defined by a point <paramref name="p"/> and a normal vector <paramref name="q"/>. The distance is considered to be positive
/// if the point <paramref name="a"/> is located in the half space into which the vector <paramref name="q"/> is pointing.
/// </summary>
/// <param name="a">The point a.</param>
/// <param name="p">A point on a plane.</param>
/// <param name="q">The normal vector of that plane (can be not-normalized).</param>
/// <returns></returns>
public static double GetDistancePointToPlane(PointD3D a, PointD3D p, VectorD3D q)
{
return(((a.X - p.X) * q.X + (a.Y - p.Y) * q.Y + (a.Z - p.Z) * q.Z) / q.Length);
}
