public static double GetD2LengthByDAngle2(double a, double b, double c) { HDebug.ToDo("check !!!"); /// return d^2_a / d_A^2 /// /// A = acos(b^2 + c^2 - a^2 / 2 b c) /// cosA = (b^2 + c^2 - a^2 / 2 b c) /// a^2 = b^2 + c^2 - 2 b c cos(A) /// la = sqrt(b^2 + c^2 - 2 b c cos(A)) /// == a /// da_dA = (b c Sin[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] /// = (b c Sin[A])/( la) /// d2a_dA2 = d_dA (da_dA) /// = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(b^2 + c^2 - 2 b c Cos[A])^(3/2) /// = (b c Cos[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] - (b^2 c^2 Sin[A]^2)/(Sqrt[b^2 + c^2 - 2 b c Cos[A]]^3) /// = (b c Cos[A])/( la) - (b^2 c^2 Sin[A]^2)/( la^3) /// = (b c cosA )/( la) - (b2 c2 sinA2 )/( la^3) /// double a2 = a * a; double b2 = b * b; double c2 = c * c; double cosA = (b2 + c2 - a2) / (2 * b * c); double la = Math.Sqrt(b2 + c2 - 2 * b * c * cosA); double la3 = la * la * la; HDebug.AssertTolerance(0.00000001, la - a); double A = Math.Acos(cosA); double sinA = Math.Sin(A); double sinA2 = sinA * sinA; double da_dA = (b * c * sinA) / la; double d2a_dA2 = (b * c * cosA) / (la) - (b2 * c2 * sinA2) / (la3); return(d2a_dA2); }
public static Matrix DMD(Vector diagmat1, Matrix mat, Vector diagmat2) { if (DMD_selftest) #region selftest { HDebug.ToDo("check"); DMD_selftest = false; Vector td1 = new double[] { 1, 2, 3 }; Vector td2 = new double[] { 4, 5, 6 }; Matrix tm = new double[, ] { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, 9 } }; Matrix dmd0 = LinAlg.Diag(td1) * tm * LinAlg.Diag(td2); Matrix dmd1 = LinAlg.DMD(td1, tm, td2); double err = (dmd0 - dmd1).HAbsMax(); HDebug.Assert(err == 0); } #endregion Matrix DMD = mat.Clone(); for (int c = 0; c < mat.ColSize; c++) { for (int r = 0; r < mat.RowSize; r++) { double v0 = mat[c, r]; double v1 = diagmat1[c] * v0 * diagmat2[r]; if (v0 == v1) { continue; } DMD[c, r] = v1; } } return(DMD); }
public static bool IsRotMatrix(MatrixByArr rot) { // http://en.wikipedia.org/wiki/Rotation_matrix#Properties_of_a_rotation_matrix if (rot.ColSize != 3) { return(false); } if (rot.RowSize != 3) { return(false); } // R^t = R^-1 // det R = 1 { // (R-I)u = 0 where u is the null space of R-I MatrixByArr RI = rot - LinAlg.Eye(3); Vector[] eigvec; double[] eigval; NumericSolver.Eig(RI, out eigvec, out eigval); double min_eigval = eigval.HAbs().Min(); if (min_eigval > 0.0000001) { return(false); } } HDebug.ToDo("write selftest code"); return(true); }
public static Vector GetRotAxis(MatrixByArr rot) { HDebug.ToDo("write selftest code"); // http://en.wikipedia.org/wiki/Rotation_matrix#Determining_the_axis // // Determining the axis // Given a rotation matrix R, a vector u parallel to the rotation axis must satisfy // R u = u // since the rotation of u around the rotation axis must result in u. The equation // above may be solved for u which is unique up to a scalar factor. // Further, the equation may be rewritten // R u = I u => (R-I) u = 0 // which shows that u is the null space of R-I. Viewed another way, u is an eigenvector // of R corresponding to the eigenvalue λ=1(every rotation matrix must have this eigenvalue). HDebug.Assert(IsRotMatrix(rot)); MatrixByArr RI = rot - LinAlg.Eye(3); Vector[] eigvec; double[] eigval; NumericSolver.Eig(RI, out eigvec, out eigval); int idx = eigval.HAbs().HIdxMin(); Vector axis = eigvec[idx]; return(axis); }
public static double GetDLengthByDAngle(Vector pa, Vector pb, Vector pc) { HDebug.ToDo("check !!!"); double a = (pb - pc).Dist; double b = (pc - pa).Dist; double c = (pa - pb).Dist; return(GetDLengthByDAngle(a, b, c)); }
public static string HSubEndStringFromCount(this string str, int from, int length) { HDebug.ToDo(); throw new Exception(); //if(str.Length <= length) // return str; //str = str.Substring(str.Length-length); //HDebug.Assert(str.Length == length); //return str; }
public static Vector OptimalTrans(Vector up, Vector ux, MatrixByArr qR) { //HTLib.DoubleVector3 _up = new HTLib.DoubleVector3(up.ToArray()); //HTLib.DoubleVector3 _ux = new HTLib.DoubleVector3(ux.ToArray()); //HTLib.DoubleMatrix3 _qR = new HTLib.DoubleMatrix3(qR.ToArray()); //return HTLib.ICP3.OptimalTrans(_up, _ux, _qR).ToArray(); HDebug.ToDo("check"); double[] w = null; return(OptimalTransWeighted(up, ux, qR, w)); }
public static double GetRotAngle(MatrixByArr rot) { HDebug.ToDo("write selftest code"); // Determining the angle // To find the angle of a rotation, once the axis of the rotation is known, select // a vector v perpendicular to the axis. Then the angle of the rotation is the angle // between v and Rv. // A much easier method, however, is to calculate the trace (i.e. the sum of the // diagonal elements of the rotation matrix) which is 1+2cosθ. HDebug.Assert(IsRotMatrix(rot)); double tr = rot.Trace(); double angle = Math.Acos((tr - 1) / 2); return(angle); }
public static Quaternion OptimalRotation(IList <Vector> p, IList <Vector> x) { //HTLib.Vector[] pp = new HTLib.Vector[p.Count]; //HTLib.Vector[] xx = new HTLib.Vector[x.Count]; //for(int i=0; i<pp.Length; i++) pp[i] = p[i].ToArray(); //for(int i=0; i<xx.Length; i++) xx[i] = x[i].ToArray(); //HTLib.Quaternion quat = HTLib.ICP3.OptimalRotation(pp, xx); //return new Quaternion(quat.ToArray()); HDebug.ToDo("check"); double[] w = new double[p.Count]; for (int i = 0; i < w.Length; i++) { w[i] = 1; } return(OptimalRotationWeighted(p.ToArray(), x.ToArray(), w)); }
public static List <U> HSelectByTypeDeprec <T, U>(this IList <T> list) where U : T { HDebug.ToDo("depreciated"); List <U> select = new List <U>(); foreach (T item in list) { if (item is U) { select.Add((U)item); } } return(select); }
public static List <TSource> HTake <TSource>(this IEnumerable <TSource> source, int from, int count) { HDebug.ToDo("Depreciate. Call HSelectCount"); List <TSource> taken = new List <TSource>(count); int idx = 0; int to = from + count - 1; foreach (TSource elem in source) { if (idx >= from && idx <= to) { taken.Add(elem); } idx++; } return(taken); }
public static double GetDLengthByDLength(double a, double b, double c) { HDebug.ToDo("check !!!"); /// reurn da_dc /// when angle C is 90' /// /// * /// | \ /// | \ /// b c /// | \ /// |C \ /// *--a---* /// /// c = sqrt(a^2 + b^2) /// dc_da = a / sqrt(a^2 + b^2) double dc_da = a / Math.Sqrt(a * a + b * b); return(dc_da); }
public static double GetDLengthByDAngle(double a, double b, double c) { HDebug.ToDo("check !!!"); /// return d_a / d_A /// /// A = acos(b^2 + c^2 - a^2 / 2 b c) /// cosA = (b^2 + c^2 - a^2 / 2 b c) /// a^2 = b^2 + c^2 - 2 b c cos(A) /// a = sqrt(b^2 + c^2 - 2 b c cos(A)) /// da_dA = (b c Sin[A])/Sqrt[b^2 + c^2 - 2 b c Cos[A]] /// double a2 = a * a; double b2 = b * b; double c2 = c * c; double cosA = (b2 + c2 - a2) / (2 * b * c); double A = Math.Acos(cosA); double sinA = Math.Sin(A); double da_dA = (b * c * sinA) / Math.Sqrt(b2 + c2 - 2 * b * c * cosA); return(da_dA); }
public static MatrixByArr AlterDotProd(Vector v1, Vector v2) { HDebug.ToDo("depreciated. Call VVt(v1,v2)"); if (HDebug.IsDebuggerAttached && HDebug.Selftest()) { MatrixByArr M0 = AlterDotProd(v1, v2); MatrixByArr M1 = VVt(v1, v2); HDebug.AssertTolerance(0, M0 - M1); } MatrixByArr mat = new MatrixByArr(v1.Size, v2.Size); for (int c = 0; c < mat.ColSize; c++) { for (int r = 0; r < mat.RowSize; r++) { mat[c, r] = v1[c] * v2[r]; } } return(mat); }
public static Vector Derivative(Func <Vector, double> func, Vector x, double dx) { HDebug.ToDo("implement"); return(null); }
public VectorSparse <T> GetColVector(int row) { HDebug.ToDo(); return(null); }