/// <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); } } }