/// <summary>
/// Multiply matrix with vector as column vector.
/// Vector must have save length as columns of matrix.
/// Returns a vector with size of matrix rows.
/// </summary>
public static Vector <__ft__> Multiply(this Matrix <__ft__> mat, Vector <__ft__> vec)
{
if (mat.Dim.X != vec.Dim)
{
throw new InvalidOperationException(String.Format("Cannot multiply matrix {0} with vector of size {1}", mat.Dim, vec.Dim));
}
var result = new __ft__[mat.Dim.Y];
var data0 = mat.Data; var data1 = vec.Data;
long my0 = mat.DY, d1 = vec.D;
long mf1 = vec.FirstIndex;
long ds0 = mat.DSX, d0 = mat.DX;
for (long ri = 0, ye = mat.FirstIndex + mat.DSY, f0 = mat.FirstIndex, e0 = f0 + ds0;
f0 != ye; f0 += my0, e0 += my0, ri++)
{
__ft__ dot = 0.0__tc__;
for (long i0 = f0, i1 = mf1; i0 != e0; i0 += d0, i1 += d1)
{
dot += data0[i0] * data1[i1];
}
result[ri] = dot;
}
return(Vector.Create(result));
}
// todo andi {
public static __rot3t__ Divide(__ft__ s, __rot3t__ q)
{
return(new __rot3t__(
s / q.W,
s / q.X, s / q.Y, s / q.Z
));
}
/// <summary>
/// Creates quaternion representing a rotation around
/// an axis by an angle.
/// </summary>
// todo ISSUE 20090420 andi : sm, rft: What about adding an AxisAngle struct?.
public __rot3t__(__v3t__ axis, __ft__ angleInRadians)
{
var halfAngle = angleInRadians / 2;
W = halfAngle.Cos();
V = axis.Normalized * halfAngle.Sin();
}
// [todo ISSUE 20090225 andi : andi] Wir sollten auch folgendes beruecksichtigen -q == q, weil es die selbe rotation definiert.
// [todo ISSUE 20090427 andi : andi] use an angle-tolerance
// [todo ISSUE 20090427 andi : andi] add __rot3t__.ApproxEqual(__rot3t__ other);
public static bool ApproxEqual(__rot3t__ r0, __rot3t__ r1, __ft__ tolerance)
{
return((r0.W - r1.W).Abs() <= tolerance &&
(r0.X - r1.X).Abs() <= tolerance &&
(r0.Y - r1.Y).Abs() <= tolerance &&
(r0.Z - r1.Z).Abs() <= tolerance);
}
public static __m33t__ Multiply(__rot2t__ rot, __m33t__ mat)
{
__ft__ a = (__ft__)System.Math.Cos(rot.Angle);
__ft__ b = (__ft__)System.Math.Sin(rot.Angle);
return(new __m33t__(a * mat.M00 +
b * mat.M10,
a * mat.M01 +
b * mat.M11,
a * mat.M02 +
b * mat.M12,
-b * mat.M00 +
a * mat.M10,
-b * mat.M01 +
a * mat.M11,
-b * mat.M02 +
a * mat.M12,
mat.M20,
mat.M21,
mat.M22));
}
public Similarity__x3t__(M4__x4t__ m, __ft__ epsilon = (__ft__)0.00001)
{
if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
{
throw new ArgumentException("Matrix contains perspective components.");
}
if (m.M33.IsTiny(epsilon))
{
throw new ArgumentException("Matrix is not homogeneous.");
}
m /= m.M33; //normalize it
var m33 = (M3__x3t__)m;
var s0 = m33.C0.Norm2;
var s1 = m33.C1.Norm2;
var s2 = m33.C2.Norm2;
var s = (s0 * s1 * s2).Pow((__ft__)1.0 / 3); //geometric mean of scale
if (!((s0 / s - 1).IsTiny(epsilon) && (s1 / s - 1).IsTiny(epsilon) && (s2 / s - 1).IsTiny(epsilon)))
{
throw new ArgumentException("Matrix features non-uniform scaling");
}
m33 /= s;
Scale = s;
EuclideanTransformation = new Euclidean__x3t__(m33, m.C3.XYZ);
}
public static Matrix <__ft__> Multiply(this Matrix <__ft__> m0, Matrix <__ft__> m1)
{
if (m0.SX != m1.SY)
{
throw new ArgumentException("m0.SX != m1.SY");
}
var result = new Matrix <__ft__>(m1.SX, m0.SY);
var data = result.Data; var data0 = m0.Data; var data1 = m1.Data;
long i = result.FirstIndex, yj = result.JY, my0 = m0.DY;
long xs = result.DSX, mf1 = m1.FirstIndex, xj = result.JX, mx1 = m1.DX;
long ds0 = m0.DSX, d0 = m0.DX, d1 = m1.DY;
for (long ye = i + result.DSY, f0 = m0.FirstIndex, e0 = f0 + ds0;
i != ye; i += yj, f0 += my0, e0 += my0)
{
for (long xe = i + xs, f1 = mf1; i != xe; i += xj, f1 += mx1)
{
__ft__ dot = 0.0__tc__;
for (long i0 = f0, i1 = f1; i0 != e0; i0 += d0, i1 += d1)
{
dot += data0[i0] * data1[i1];
}
data[i] = dot;
}
}
return(result);
}
public __ft__ this[int index]
{
get
{
switch (index)
{
case 0: return(V.X);
case 1: return(V.Y);
case 2: return(V.Z);
case 3: return(W);
default: throw new ArgumentException();
}
}
set
{
switch (index)
{
case 0: V.X = value; break;
case 1: V.Y = value; break;
case 2: V.Z = value; break;
case 3: W = value; break;
default: throw new ArgumentException();
}
}
}
/// <summary>
/// Creates a similarity transformation from a rigid transformation <paramref name="euclideanTransformation"/> and an (subsequent) uniform scale by factor <paramref name="scale"/>.
/// </summary>
public Similarity__x3t__(Euclidean__x3t__ euclideanTransformation, __ft__ scale)
{
Scale = scale;
EuclideanTransformation = new Euclidean__x3t__(
euclideanTransformation.Rot,
scale * euclideanTransformation.Trans
);
}
public static __v2t__ Multiply(__rot2t__ rot, __v2t__ vec)
{
__ft__ a = (__ft__)System.Math.Cos(rot.Angle);
__ft__ b = (__ft__)System.Math.Sin(rot.Angle);
return(new __v2t__(a * vec.X + b * vec.Y,
-b * vec.X + a * vec.Y));
}
/// <summary>
/// Creates quaternion from euler angles [yaw, pitch, roll].
/// </summary>
/// <param name="yawInRadians">Rotation around X</param>
/// <param name="pitchInRadians">Rotation around Y</param>
/// <param name="rollInRadians">Rotation around Z</param>
public __rot3t__(__ft__ yawInRadians, __ft__ pitchInRadians, __ft__ rollInRadians)
{
var qx = new __rot3t__(__v3t__.XAxis, yawInRadians);
var qy = new __rot3t__(__v3t__.YAxis, pitchInRadians);
var qz = new __rot3t__(__v3t__.ZAxis, rollInRadians);
this = qz * qy * qx;
}
/// <summary>
/// Multiplies a complex number with this
/// </summary>
/// <param name="number"></param>
public void Multiply(__ct__ number)
{
__ft__ real = Real;
__ft__ imag = Imag;
Real = real * number.Real - imag * number.Imag;
Imag = real * number.Imag + imag * number.Real;
}
public static __v3t__ Multiply(__rot2t__ rot, __v3t__ vec)
{
__ft__ ca = (__ft__)System.Math.Cos(rot.Angle);
__ft__ sa = (__ft__)System.Math.Sin(rot.Angle);
return(new __v3t__(ca * vec.X + sa * vec.Y,
-sa * vec.X + ca * vec.Y,
vec.Z));
}
public static __m34t__ Multiply(__rot2t__ rot, __shift3t__ shift)
{
__ft__ a = (__ft__)System.Math.Cos(rot.Angle);
__ft__ b = (__ft__)System.Math.Sin(rot.Angle);
return(new __m34t__(a, b, 0, a * shift.X + b * shift.Y,
-b, a, 0, -b * shift.X + a * shift.Y,
0, 0, 1, shift.Z));
}
/// <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>
/// Exponentiates a complex by another
/// </summary>
/// <returns></returns>
public static __ct__ Pow(__ct__ number, __ct__ exponent)
{
__ft__ r = number.Norm;
__ft__ phi = number.Argument;
__ft__ a = exponent.Real;
__ft__ b = exponent.Imag;
return(__ct__.CreateRadial((__ft__)System.Math.Exp(System.Math.Log(r) * a - b * phi), a * phi + b * (__ft__)System.Math.Log(r)));
}
/// <summary>
/// Divides this by a complex number
/// </summary>
/// <param name="number"></param>
public void Divide(__ct__ number)
{
__ft__ t = 1 / number.NormSquared;
__ft__ real = Real;
__ft__ imag = Imag;
Real = t * (real * number.Real + imag * number.Imag);
Imag = t * (imag * number.Real - real * number.Imag);
}
public static Vector <__ft__> Multiply(this Vector <__ft__> vec, __ft__ s)
{
var result = new __ft__[vec.Dim];
__ft__[] a0 = vec.Data;
for (long i0 = vec.Origin, ri = 0, e0 = i0 + vec.DSX, d0 = vec.D; i0 != e0; i0 += d0, ri++)
{
result[ri] = a0[i0] * s;
}
return(Vector.Create(result));
}
/// <summary>
/// Exponentiates this by a real number
/// </summary>
/// <param name="scalar"></param>
public void Pow(__ft__ scalar)
{
__ft__ r = NormSquared;
__ft__ phi = (__ft__)System.Math.Atan2(Imag, Real);
r = (__ft__)System.Math.Pow(r, scalar);
phi *= scalar;
Real = r * (__ft__)System.Math.Cos(phi);
Imag = r * (__ft__)System.Math.Sin(phi);
}
/// <summary>
/// Calculates the Square-Root of a real number and returns a Complex
/// </summary>
/// <param name="number"></param>
/// <returns></returns>
public static __ct__ Sqrt(__ft__ number)
{
if (number >= 0)
{
return(new __ct__((__ft__)System.Math.Sqrt(number), 0.0f));
}
else
{
return(new __ct__(0.0f, (__ft__)System.Math.Sqrt(-1.0f * number)));
}
}
/// <summary>
/// Calculates e^Complex
/// </summary>
/// <param name="number"></param>
/// <returns></returns>
public static __ct__ Exp(__ct__ number)
{
__ct__ c = __ct__.Zero;
__ft__ factor = (__ft__)System.Math.Pow(Constant.E, number.Real);
c.Real = factor * (__ft__)System.Math.Cos(number.Imag);
c.Imag = factor * (__ft__)System.Math.Sin(number.Imag);
return(c);
}
/// <summary>
/// Calculates the squared norm of a vector:
/// ||a|| = Sum(a0*a0 + a1*a1 + ... aN*aN)
/// </summary>
public static __ft__ NormSquared(this Vector <__ft__> v)
{
__ft__ result = 0.0__tc__;
__ft__[] a = v.Data;
for (long i = v.Origin, e = i + v.DSX, d = v.D; i != e; i += d)
{
result += a[i] * a[i];
}
return(result);
}
public static Vector <__ft__> Add(this Vector <__ft__> a, __ft__ b)
{
var result = new __ft__[a.Dim];
__ft__[] a0 = a.Data;
for (long i0 = a.Origin, ri = 0, e0 = i0 + a.DSX, d0 = a.D; i0 != e0; i0 += d0, ri++)
{
result[ri] = a0[i0] + b;
}
return(Vector.Create(result));
}
//# foreach (var isDouble in new[] { false, true }) {
//# var ft = isDouble ? "double" : "float";
//# var tc = isDouble ? "d" : "f";
#region Vector Extensions
/// <summary>
/// Calculates the dot product of two vectors:
/// a dot b = Sum(a0*b0 + a1*b1 + ... aN*bN)
/// </summary>
public static __ft__ DotProduct(this Vector <__ft__> v0, Vector <__ft__> v1)
{
__ft__ result = 0.0__tc__;
__ft__[] a0 = v0.Data, a1 = v1.Data;
for (long i0 = v0.Origin, i1 = v1.Origin, e0 = i0 + v0.DSX, d0 = v0.D, d1 = v1.D;
i0 != e0; i0 += d0, i1 += d1)
{
result += a0[i0] * a1[i1];
}
return(result);
}
/// <summary>
/// Creates a rigid transformation from a matrix <paramref name="m"/>.
/// </summary>
public __e3t__(M4__s4f__ m, __ft__ epsilon = __eps__)
: this(((M3__s3f__)m) / m.M33, m.C3.XYZ / m.M33, epsilon)
{
if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
{
throw new ArgumentException("Matrix contains perspective components.");
}
if (m.M33.IsTiny(epsilon))
{
throw new ArgumentException("Matrix is not homogeneous.");
}
}
public static __ct__ Power(this __ct__ number, __ft__ exponent)
{
if (number.IsZero)
{
return(__ct__.Zero);
}
else
{
__ft__ r = number.Norm;
__ft__ phi = number.Argument;
return(__ct__.CreateRadial(Pow(r, exponent), exponent * phi));
}
}
/// <summary>
/// Inverts this quaternion (multiplicative inverse).
/// Returns this.
/// </summary>
public void Invert()
{
var norm = NormSquared;
if (norm == 0)
{
throw new ArithmeticException("quaternion is not invertible");
}
var scale = 1 / norm;
W *= scale;
V *= -scale;
}
/// <summary>
/// Calculates the n-th Root of a Complex number and returns n Solutions
/// </summary>
/// <param name="number"></param>
/// <param name="order">n</param>
/// <returns></returns>
public __ct__[] Root(__ct__ number, int order)
{
__ct__[] values = new __ct__[order];
__ft__ phi = number.Argument / 2.0f;
__ft__ dphi = (__ft__)Constant.PiTimesTwo / (__ft__)order;
__ft__ r = (__ft__)System.Math.Pow(number.Norm, 1.0f / order);
for (int i = 1; i < order; i++)
{
values[i] = __ct__.CreateRadial(r, phi + dphi * i);
}
return(values);
}
/// <summary>
/// Normalizes this quaternion.
/// </summary>
/// <returns>This.</returns>
public void Normalize()
{
var norm = Norm;
if (norm == 0)
{
this = __rot3t__.Identity;
}
else
{
var scale = 1 / norm;
W *= scale;
V *= scale;
}
}
/// <summary>
/// Creates a quaternion from a rotation matrix
/// </summary>
/// <param name="m"></param>
/// <param name="epsilon"></param>
public static __rot3t__ From__m33t__(__m33t__ m, __ft__ epsilon = (__ft__)1e-6)
{
if (!m.IsOrthonormal(epsilon))
{
throw new ArgumentException("Matrix is not orthonormal.");
}
var t = 1 + m.M00 + m.M11 + m.M22;
if (t > epsilon)
{
__ft__ s = t.Sqrt() * 2;
__ft__ x = (m.M21 - m.M12) / s;
__ft__ y = (m.M02 - m.M20) / s;
__ft__ z = (m.M10 - m.M01) / s;
__ft__ w = s / 4;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
if (m.M00 > m.M11 && m.M00 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M00 - m.M11 - m.M22) * 2;
__ft__ x = s / 4;
__ft__ y = (m.M01 + m.M10) / s;
__ft__ z = (m.M02 + m.M20) / s;
__ft__ w = (m.M21 - m.M12) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else if (m.M11 > m.M22)
{
__ft__ s = Fun.Sqrt(1 + m.M11 - m.M00 - m.M22) * 2;
__ft__ x = (m.M01 + m.M10) / s;
__ft__ y = s / 4;
__ft__ z = (m.M12 + m.M21) / s;
__ft__ w = (m.M20 - m.M02) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
else
{
__ft__ s = Fun.Sqrt(1 + m.M22 - m.M00 - m.M11) * 2;
__ft__ x = (m.M20 + m.M02) / s;
__ft__ y = (m.M12 + m.M21) / s;
__ft__ z = s / 4;
__ft__ w = (m.M01 - m.M10) / s;
return(new __rot3t__(w, x, y, z).Normalized);
}
}
}
