/// <summary>
/// Returns the skew-symmetric "cross" matrix (A^T = -A) of the vector v.
/// </summary>
public static M3__x3t__ CrossMatrix(this V__x3t__ v)
{
return(new M3__x3t__(
0, -v.Z, +v.Y,
+v.Z, 0, -v.X,
-v.Y, +v.X, 0));
}
/// <summary>
/// Returns the outer product (tensor-product) of v1 * v2^T as a 3x3 Matrix.
/// </summary>
public static M3__x3t__ OuterProduct(this V__x3t__ v1, V__x3t__ v2)
{
return(new M3__x3t__(
v2.X * v1.X, v2.Y * v1.X, v2.Z * v1.X,
v2.X * v1.Y, v2.Y * v1.Y, v2.Z * v1.Y,
v2.X * v1.Z, v2.Y * v1.Z, v2.Z * v1.Z));
}
/// <summary>
/// Returns the matrix that transforms from the coordinate system
/// specified by the basis into the world cordinate system.
/// </summary>
public static M4__x4t__ FromBasis(V__x3t__ xAxis, V__x3t__ yAxis, V__x3t__ zAxis, V__x3t__ origin)
{
return(new M4__x4t__(
xAxis.X, yAxis.X, zAxis.X, origin.X,
xAxis.Y, yAxis.Y, zAxis.Y, origin.Y,
xAxis.Z, yAxis.Z, zAxis.Z, origin.Z,
0, 0, 0, 1));
}
/// <summary>
/// Creates a inverse view tranformation from the given vectors.
/// Transformation from view- into world-space.
/// The implementation is the same as FromBasis(right, up, normal, location)
/// </summary>
/// <param name="location">Origin of the view</param>
/// <param name="right">Right vector of the view-plane</param>
/// <param name="up">Up vector of the view-plane</param>
/// <param name="normal">Normal vector of the view-plane. This vector is suppsoed to point in view-direction for a left-handed view transformation and in opposit direction in the right-handed case.</param>
/// <returns>The inverse view transformation</returns>
public static M4__x4t__ InvViewTrafo(V__x3t__ location, V__x3t__ right, V__x3t__ up, V__x3t__ normal)
{
return(new M4__x4t__(
right.X, up.X, normal.X, location.X,
right.Y, up.Y, normal.Y, location.Y,
right.Z, up.Z, normal.Z, location.Z,
0, 0, 0, 1
));
}
/// <summary>
/// Creates a view tranformation from the given vectors.
/// Transformation from world- into view-space.
/// </summary>
/// <param name="location">Origin of the view</param>
/// <param name="right">Right vector of the view-plane</param>
/// <param name="up">Up vector of the view-plane</param>
/// <param name="normal">Normal vector of the view-plane. This vector is suppsoed to point in view-direction for a left-handed view transformation and in opposit direction in the right-handed case.</param>
/// <returns>The view transformation</returns>
public static M4__x4t__ ViewTrafo(V__x3t__ location, V__x3t__ right, V__x3t__ up, V__x3t__ normal)
{
return(new M4__x4t__(
right.X, right.Y, right.Z, -location.Dot(right),
up.X, up.Y, up.Z, -location.Dot(up),
normal.X, normal.Y, normal.Z, -location.Dot(normal),
0, 0, 0, 1
));
}
/// <summary>
/// Computes from a <see cref="V__x3t__"/> point (origin) and
/// a <see cref="V__x3t__"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M4__x4t__"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M4__x4t__"/>The trafofrom global to local system.</param>
public static void NormalFrame(V__x3t__ origin, V__x3t__ normal,
out M4__x4t__ local2global, out M4__x4t__ global2local
)
{
V__x3t__ min;
__ft__ x = Fun.Abs(normal.X);
__ft__ y = Fun.Abs(normal.Y);
__ft__ z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V__x3t__.XAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
else
{
if (y < z)
{
min = V__x3t__.YAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
V__x3t__ xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V__x3t__ yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V__x3t__ zVec = normal;
zVec.Normalize();
local2global = new M4__x4t__(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M4__x4t__ mat = new M4__x4t__(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M4__x4t__.Translation(-origin);
global2local = mat * shift;
}
/// <summary>
/// Creates an orthonormal basis from the given normal as z-axis.
/// The resulting matrix transforms from the local to the global coordinate system.
/// The normal is expected to be normalized.
///
/// The implementation is based on:
/// Building an Orthonormal Basis, Revisited, by Duff et al. 2017
/// </summary>
public static M3__x3t__ NormalFrame(V__x3t__ n)
{
var sg = n.Z >= 0 ? 1 : -1; // original uses copysign(1.0, n.Z) -> not the same as sign where 0 -> 0
var a = -1 / (sg + n.Z);
var b = n.X * n.Y * a;
// column 0: [1 + sg * n.X * n.X * a, sg * b, -sg * n.X]
// column 1: [b, sg + n.Y * n.Y * a, -n.Y]
// column 2: n
return(new M3__x3t__(1 + sg * n.X * n.X * a, b, n.X,
sg * b, sg + n.Y * n.Y * a, n.Y,
-sg * n.X, -n.Y, n.Z));
}
/// <summary>
/// Computes a Coordiante Frame Transformation (Basis) from current CS into
/// the (X, Y, Z)-System at a given Origin.
/// Note: you can use it, to transform from RH to LH and vice-versa, all depending
/// how you will specifie your new basis-vectors.
/// </summary>
/// <param name="xVec">New X Vector</param>
/// <param name="yVec">New Y Vector</param>
/// <param name="zVec">New Z vector</param>
/// <param name="oVec">New Origin.</param>
/// <param name="viewTrafo"></param>
/// <param name="viewTrafoInverse"></param>
public static void CoordinateFrameTransform(V__x3t__ xVec, V__x3t__ yVec, V__x3t__ zVec, V__x3t__ oVec,
out M4__x4t__ viewTrafo, out M4__x4t__ viewTrafoInverse)
{
oVec = -oVec;
viewTrafo = new M4__x4t__(
xVec.X, xVec.Y, xVec.Z, xVec.X * oVec.X + xVec.Y * oVec.Y + xVec.Z * oVec.Z,
yVec.X, yVec.Y, yVec.Z, yVec.X * oVec.X + yVec.Y * oVec.Y + yVec.Z * oVec.Z,
zVec.X, zVec.Y, zVec.Z, zVec.X * oVec.X + zVec.Y * oVec.Y + zVec.Z * oVec.Z,
0, 0, 0, 1
);
viewTrafoInverse = new M4__x4t__(
xVec.X, yVec.X, zVec.X, -oVec.X,
xVec.Y, yVec.Y, zVec.Y, -oVec.Y,
xVec.Z, yVec.Z, zVec.Z, -oVec.Z,
0, 0, 0, 1
);
}
/// <summary>
/// Computes from a <see cref="V__x3t__"/> normal the transformation matrix
/// from the local coordinate system where the normal is the z-axis to
/// the global coordinate system.
/// </summary>
/// <param name="normal">The normal vector of the new ground plane.</param>
public static M3__x3t__ NormalFrameLocal2Global(V__x3t__ normal)
{
V__x3t__ min;
double x = Fun.Abs(normal.X);
double y = Fun.Abs(normal.Y);
double z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V__x3t__.XAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
else
{
if (y < z)
{
min = V__x3t__.YAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
var xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
var yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
var zVec = normal;
zVec.Normalize();
return(new M3__x3t__(xVec.X, yVec.X, zVec.X,
xVec.Y, yVec.Y, zVec.Y,
xVec.Z, yVec.Z, zVec.Z));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by similarity transformation t.
/// </summary>
public static V__x3t__ TransformPos(Similarity__x3t__ t, V__x3t__ p)
{
return(t.EuclideanTransformation.TransformPos(t.Scale * p));
}
/// <summary>
/// Initializes a <see cref="Scale__x3t__"/> using a uniform __ft__ value.
/// </summary>
public Scale__x3t__(__ft__ uniform)
{
V = new V__x3t__(uniform, uniform, uniform);
}
/// <summary>
/// Initializes a <see cref="Scale__x3t__"/> class from __ft__-array.
/// </summary>
public Scale__x3t__(__ft__[] array)
{
V = new V__x3t__(array);
}
/// <summary>
/// Initializes a <see cref="Shift__x3t__"/> using three __ft__s.
/// </summary>
public Shift__x3t__(__ft__ x, __ft__ y, __ft__ z)
{
V = new V__x3t__(x, y, z);
}
public Shift__x3t__(V__x3t__ v)
{
V = v;
}
/// <summary>
/// Multiplies matrix with a V__x3t__.
/// </summary>
public static V__x3t__ Multiply(M2__x2t__ mat, V__x3t__ vec)
{
return(new V__x3t__(mat.M00 * vec.X + mat.M01 * vec.Y,
mat.M10 * vec.X + mat.M11 * vec.Y,
vec.Z));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of this similarity transformation.
/// </summary>
public V__x3t__ InvTransformPos(V__x3t__ p)
{
return(InvTransformPos(this, p));
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of this similarity transformation.
/// Actually, only the rotation is used.
/// </summary>
public V__x3t__ InvTransformDir(V__x3t__ v)
{
return(InvTransformDir(this, v));
}
/// <summary>
/// Transforms point p (p.w is presumed 1.0) by the inverse of the similarity transformation t.
/// </summary>
public static V__x3t__ InvTransformPos(Similarity__x3t__ t, V__x3t__ p)
{
return(t.EuclideanTransformation.InvTransformPos(p) / t.Scale);
}
public Scale__x3t__(V__x3t__ v)
{
V = v;
}
/// <summary>
/// Initializes a <see cref="Shift__x3t__"/> class from __ft__-array.
/// </summary>
public Shift__x3t__(__ft__[] array)
{
V = new V__x3t__(array);
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by similarity transformation t.
/// Actually, only the rotation and scale is used.
/// </summary>
public static V__x3t__ TransformDir(Similarity__x3t__ t, V__x3t__ v)
{
return(t.EuclideanTransformation.TransformDir(t.Scale * v));
}
/// <summary>
/// Initializes a <see cref="Shift__x3t__"/> class from __ft__-array starting from passed index
/// </summary>
public Shift__x3t__(__ft__[] array, int start)
{
V = new V__x3t__(array, start);
}
/// <summary>
/// Transforms direction vector v (v.w is presumed 0.0) by the inverse of the similarity transformation t.
/// Actually, only the rotation and scale is used.
/// </summary>
public static V__x3t__ InvTransformDir(Similarity__x3t__ t, V__x3t__ v)
{
return(t.EuclideanTransformation.InvTransformDir(v) / t.Scale);
}
/// <summary>
/// Multiplacation of a <see cref="Scale__x3t__"/> with a <see cref="V__x3t__"/>.
/// </summary>
public static V__x3t__ Multiply(Scale__x3t__ scale, V__x3t__ v)
{
return(new V__x3t__(scale.X * v.X, scale.Y * v.Y, scale.Z * v.Z));
}
