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