/// <summary>
/// Transforms a position by this matrix (fast).
///
/// Returns a position transformed by the current transformation matrix. This function is a faster version of
/// MultiplyPoint; but it can only handle regular 3D transformations. MultiplyPoint is slower, but can handle
/// projective transformations as well.
///
/// </summary>
/// <returns>The transformed position.</returns>
/// <param name="v">The position to transform.</param>
public DVector3 MultiplyPoint3x4(DVector3 v)
{
DVector3 transformed = new DVector3(
(this.m00 * v.x) + (this.m01 * v.y) + (this.m02 * v.z) + this.m03,
(this.m10 * v.x) + (this.m11 * v.y) + (this.m12 * v.z) + this.m13,
(this.m20 * v.x) + (this.m21 * v.y) + (this.m22 * v.z) + this.m23);
return(transformed);
}
/// <summary>
/// Transforms a position by this matrix (generic).
///
/// Returns a position v transformed by the current fully arbitrary matrix. If the matrix is a regular 3D
/// transformation matrix, it is much faster to use MultiplyPoint3x4 instead. MultiplyPoint is slower, but can
/// handle projective transformations as well.
/// </summary>
/// <returns>The transformed point.</returns>
/// <param name="v">The point to be transformed.</param>
public DVector3 MultiplyPoint(DVector3 v)
{
DVector3 transformed = new DVector3(
(this.m00 * v.x) + (this.m01 * v.y) + (this.m02 * v.z) + this.m03,
(this.m10 * v.x) + (this.m11 * v.y) + (this.m12 * v.z) + this.m13,
(this.m20 * v.x) + (this.m21 * v.y) + (this.m22 * v.z) + this.m23);
double proj = (this.m30 * v.x) + (this.m31 * v.y) + (this.m32 * v.z) + this.m33;
transformed.x /= proj;
transformed.y /= proj;
transformed.z /= proj;
return(transformed);
}
/// <summary>
/// Create a translation and rotation matrix.
/// </summary>
/// <param name="translation">Translation as 3 doubles in a DVector3 struct.</param>
/// <param name="orientation">Orientation as 4 doubles in a DVector4 struct.</param>
/// <returns>Double matrix.</returns>
public static DMatrix4x4 TR(DVector3 translation, DVector4 orientation)
{
double[] dTranslation = new double[3];
double[] dOrientation = new double[4];
dTranslation[0] = translation.x;
dTranslation[1] = translation.y;
dTranslation[2] = translation.z;
dOrientation[0] = orientation.x;
dOrientation[1] = orientation.y;
dOrientation[2] = orientation.z;
dOrientation[3] = orientation.w;
return(DMatrix4x4.TR(dTranslation, dOrientation));
}
/// <summary>
/// Dot Product of two vectors.
/// </summary>
/// <returns>Dot product as a double.</returns>
/// <param name="a">First vector.</param>
/// <param name="b">Second vector.</param>
public static double Dot(DVector3 a, DVector3 b)
{
return((a.x * b.x) + (a.y * b.y) + (a.z * b.z));
}
/// <summary>
/// Returns the distance between a and b.
/// </summary>
/// <returns>Euclidean distance as a double.</returns>
/// <param name="a">First vector.</param>
/// <param name="b">Second vector.</param>
public static double Distance(DVector3 a, DVector3 b)
{
return((a - b).Magnitude);
}