public static double Det4(MatrixByArr mat)
{
HDebug.Assert(mat.ColSize == 4, mat.RowSize == 4);
double a1 = mat[0, 0], a2 = mat[0, 1], a3 = mat[0, 2], a4 = mat[0, 3];
double b1 = mat[1, 0], b2 = mat[1, 1], b3 = mat[1, 2], b4 = mat[1, 3];
double c1 = mat[2, 0], c2 = mat[2, 1], c3 = mat[2, 2], c4 = mat[2, 3];
double d1 = mat[3, 0], d2 = mat[3, 1], d3 = mat[3, 2], d4 = mat[3, 3];
double det = a4 * b3 * c2 * d1 - a3 * b4 * c2 * d1 - a4 * b2 * c3 * d1 + a2 * b4 * c3 * d1
+ a3 * b2 * c4 * d1 - a2 * b3 * c4 * d1 - a4 * b3 * c1 * d2 + a3 * b4 * c1 * d2
+ a4 * b1 * c3 * d2 - a1 * b4 * c3 * d2 - a3 * b1 * c4 * d2 + a1 * b3 * c4 * d2
+ a4 * b2 * c1 * d3 - a2 * b4 * c1 * d3 - a4 * b1 * c2 * d3 + a1 * b4 * c2 * d3
+ a2 * b1 * c4 * d3 - a1 * b2 * c4 * d3 - a3 * b2 * c1 * d4 + a2 * b3 * c1 * d4
+ a3 * b1 * c2 * d4 - a1 * b3 * c2 * d4 - a2 * b1 * c3 * d4 + a1 * b2 * c3 * d4;
return(det);
}
public static string HToString(this IList <char> chars)
{
if (HToStringChars_selftest)
{
HToStringChars_selftest = false;
string str0 = "";
foreach (var ch in chars)
{
str0 += ch;
}
string str1 = HToString(chars);
HDebug.Exception(str0 == str1);
}
return(new String(chars.ToArray()));
}
public static string Execute(string script, bool ignoreAssert, params string[] replaces)
{
string nscript = CompleteScript(script, replaces);
string msg = matlab.Execute(nscript);
if (ignoreAssert == false)
{
HDebug.AssertIf(!ignoreAssert, msg == "");
if (msg != "")
{
System.Console.Error.WriteLine("Matlab error:\n" + msg);
throw new HException(string.Format("exception at Matlab.Execute(\"{0}\")", script));
}
}
return(msg);
}
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 double HAbsMaxDiffWith(this IMatrix <double> mat, IMatrix <double> diff)
{
HDebug.Assert(mat.ColSize == diff.ColSize);
HDebug.Assert(mat.RowSize == diff.RowSize);
double absmax = -1;
for (int c = 0; c < mat.ColSize; c++)
{
for (int r = 0; r < mat.RowSize; r++)
{
absmax = Math.Max(absmax, Math.Abs(mat[c, r] - diff[c, r]));
}
}
HDebug.Assert(absmax != -1);
return(absmax);
}
static double DistPointLine(Vector point, IList <Vector> line)
{
double dist2 = double.PositiveInfinity;
for (int i = 1; i < line.Count; i++)
{
Vector line0 = line[i - 1];
Vector line1 = line[i];
dist2 = Math.Min(dist2, Dist2PointLine(point, line0, line1));
HDebug.Assert(dist2 >= 0);
}
double dist = Math.Sqrt(dist2);
HDebug.Assert(double.IsNaN(dist) == false, double.IsInfinity(dist) == false);
return(dist);
}
public static Vector[] ExtendSegmentBegin(IList <Vector> segment, double extlength)
{
List <Vector> extsegment = segment.HCloneVectors().ToList();
Vector vec10 = (segment[0] - segment[1]).UnitVector();
extsegment.Insert(0, extsegment[0] + vec10 * extlength);
if (HDebug.IsDebuggerAttached)
{
Vector vec01 = (extsegment[0] - extsegment[1]).UnitVector();
Vector vec12 = (extsegment[1] - extsegment[2]).UnitVector();
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(vec01, vec12) - 1);
}
return(extsegment.ToArray());
}
public static int[] HCount <T>(this IList <T[]> valuess)
{
if (HDebug.Selftest())
{
double[][] tvaluess = new double[][] { new double[] { 1, 2, 3 }, new double[0], new double[] { 1, 2, 3, 4 } };
int[] tcnt = HCount(tvaluess);
HDebug.Assert(new TVector <int>(tcnt) == new int[] { 3, 0, 4 });
}
int[] counts = new int[valuess.Count];
for (int i = 0; i < counts.Length; i++)
{
counts[i] = valuess[i].Length;
}
return(counts);
}
public Node AddChild(Tree.Node parent, NODE childval)
{
if (nodeval_node.ContainsKey(childval))
{
HDebug.Assert(false);
return(null);
}
Func <Tree.Node, Tree.Node> nodebuilder = delegate(Tree.Node lparent)
{
return(Node.New(lparent, childval));
};
Node child = (Node)(AddChild(parent, nodebuilder));
nodeval_node.Add(childval, child);
return(child);
}
public static IList <T> HRemoveAllNull <T>(this IList <T> values, bool bAssert = true)
where T : class
{
//HDebug.Depreciated("use HRemoveAll(null), instead");
HDebug.Break(bAssert && HRemoveAllNull_break);
Predicate <T> match = delegate(T val)
{
return(val == null);
};
List <T> _values = new List <T>(values);
_values.RemoveAll(match);
return(_values);
}
public static string HSubEndStringFrom(this string str, int lastIdxFrom)
{
if (HSubEndStringFrom_selftest)
{
HSubEndStringFrom_selftest = false;
HDebug.Assert("abcdef".HSubEndStringFrom(1) == "abcde");
HDebug.Assert("abcdef".HSubEndStringFrom(0) == "abcdef");
}
int leng = str.Length;
int from = 0;
int count = leng - lastIdxFrom;
string substr = str.Substring(from, count);
return(substr);
}
public static TMatrix <T> operator+(TMatrix <T> val1, T[,] val2)
{
int colsize = val1.ColSize; HDebug.Assert(colsize == val2.GetLength(0));
int rowsize = val1.RowSize; HDebug.Assert(rowsize == val2.GetLength(1));
HDebug.Assert(val1.data.Length == val2.Length);
T[,] ret = new T[colsize, rowsize];
for (int c = 0; c < colsize; c++)
{
for (int r = 0; r < rowsize; r++)
{
ret[c, r] = (dynamic)ret[c, r] + val2[c, r];
}
}
return(ret);
}
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));
}
// update balance of (node, parent), and return (redetermined node, redetermined parent)
static (Node <AvlNodeInfo> nnode, Node <AvlNodeInfo> nparent) UpdateBalance(Node <AvlNodeInfo> node, Node <AvlNodeInfo> parent, ref Node <AvlNodeInfo> root)
{
HDebug.Assert(node != null);
HDebug.Assert(parent != null);
HDebug.Assert(parent == node.parent);
UpdateHeight(parent);
int node_bf = node.value.bf;
int parent_bf = parent.value.bf;
HDebug.Assert(Math.Abs(parent_bf) <= 2);
{
if (parent_bf == 2)
{
node = parent.right;
node_bf = node.value.bf;
}
else if (parent_bf == -2)
{
node = parent.left;
node_bf = node.value.bf;
}
}
Node <AvlNodeInfo> nnode = null;
Node <AvlNodeInfo> nparent = null;
if ((Math.Abs(node_bf) <= 1) && (Math.Abs(parent_bf) <= 1))
{
nnode = node;
nparent = parent;
}
else if (((node_bf == -1) && (parent_bf == -2)) || ((node_bf == 0) && (parent_bf == -2)))
{
//HDebug.Assert(parent_bf == -2);
ref Node <AvlNodeInfo> parent_ref = ref parent.GetThisRef(ref root);
BTree.RotateRight <AvlNodeInfo>(ref parent_ref);
UpdateHeight(parent);
UpdateHeight(node);
HDebug.Assert(parent.parent == node);
nnode = parent;
nparent = node;
}
public static MatrixByArr operator-(MatrixByArr lmat, MatrixByArr rmat)
{
HDebug.Assert(lmat.ColSize == rmat.ColSize);
HDebug.Assert(lmat.RowSize == rmat.RowSize);
int colsize = lmat.ColSize;
int rowsize = lmat.RowSize;
MatrixByArr result = new MatrixByArr(colsize, rowsize);
for (int c = 0; c < colsize; c++)
{
for (int r = 0; r < rowsize; r++)
{
result[c, r] = lmat[c, r] - rmat[c, r];
}
}
return(result);
}
public static void SelfTest()
{
if (_SelfTest == false)
{
return;
}
_SelfTest = false;
MatrixBySparseMatrix mat = new MatrixBySparseMatrix(4, 4, 2);
HDebug.AssertToleranceMatrix(0, mat - MatrixByArr.Zeros(4, 4));
mat[0, 0] = mat[1, 1] = mat[2, 2] = mat[3, 3] = 1;
HDebug.AssertToleranceMatrix(0, mat - LinAlg.Eye(4, 1));
mat[0, 0] = mat[1, 1] = mat[2, 2] = mat[3, 3] = 0;
HDebug.AssertToleranceMatrix(0, mat - MatrixByArr.Zeros(4, 4));
}
public void Compress()
{
foreach (var c_r_val in blkmatrix.EnumElements())
{
int c = c_r_val.Item1;
int r = c_r_val.Item2;
MatrixByArr lmat = c_r_val.Item3;
if (lmat.HNumZeros() < BlkSize * BlkSize)
{
continue;
}
HDebug.Assert(lmat.HNumZeros() == BlkSize * BlkSize);
blkmatrix[c, r] = null;
}
}
public static double?Det(double[,] mat)
{
if (mat.GetLength(0) == 2 && mat.GetLength(1) == 2)
{
return(Det2(mat));
}
if (mat.GetLength(0) == 3 && mat.GetLength(1) == 3)
{
return(Det3(mat));
}
if (mat.GetLength(0) == 4 && mat.GetLength(1) == 4)
{
return(Det4(mat));
}
HDebug.Assert(false);
return(null);
}
public static void MV <MATRIX>(MATRIX lmat, Vector rvec, Vector result, string options = "")
where MATRIX : IMatrix <double>
{
if (MV_SelfTest)
{
MV_SelfTest = false;
MatrixByArr tM = new double[4, 3] {
{ 1, 2, 3 }
, { 4, 5, 6 }
, { 7, 8, 9 }
, { 10, 11, 12 }
};
Vector tV = new double[3] {
1, 2, 3
};
Vector tMV0 = new double[4] {
14, 32, 50, 68
};
Vector tMV1 = MV(tM, tV);
double err = (tMV0 - tMV1).ToArray().HAbs().Max();
HDebug.Assert(err == 0);
}
HDebug.Assert(lmat.RowSize == rvec.Size);
HDebug.Assert(lmat.ColSize == result.Size);
if (options.Split(';').Contains("parallel") == false)
{
for (int c = 0; c < lmat.ColSize; c++)
{
for (int r = 0; r < lmat.RowSize; r++)
{
result[c] += lmat[c, r] * rvec[r];
}
}
}
else
{
System.Threading.Tasks.Parallel.For(0, lmat.ColSize, delegate(int c)
{
for (int r = 0; r < lmat.RowSize; r++)
{
result[c] += lmat[c, r] * rvec[r];
}
});
}
}
public VectorSparse <T> GetRowVector(int col)
{
HDebug.AssertAnd(0 <= col, col < RowSize);
VectorSparse <T> vec = new VectorSparse <T>(RowSize, GetDefault: GetDefault);
if (diagonal[col] != null)
{
vec[col] = diagonal[col];
}
foreach (var r_val in offdiagonal[col])
{
int r = r_val.Key;
T val = r_val.Value;
vec[r] = val;
}
return(vec);
}
public T this[long i]
{
get
{
HDebug.Assert(0 <= i && i < SizeLong);
long idx = i / MaxBlockCapacity;
long off = i % MaxBlockCapacity;
return(data[idx][off]);
}
set
{
HDebug.Assert(0 <= i && i < SizeLong);
long idx = i / MaxBlockCapacity;
long off = i % MaxBlockCapacity;
data[idx][off] = value;
}
}
public static string FindResourceName <T>(string resname)
{
List <string> names = new List <string>();
foreach (string name in GetResourceNames <T>())
{
if (name.Contains(resname))
{
names.Add(name);
}
}
if (names.Count() != 1)
{
HDebug.Assert(false);
}
return(names[0]);
}
public static Tuple <Vector, Vector> LineOnTwoPlanes(Vector norm1, double val1, Vector norm2, double val2)
{
/// http://intumath.org/Math/Geometry/Analytic%20Geometry/plane-planeinter.html
///
if (LineOnTwoPlanes_SelfTest)
{
LineOnTwoPlanes_SelfTest = false;
Vector tn1 = new double[] { 1, 2, 3 };
Vector tn2 = new double[] { 2, 5, 3 };
double td1 = 10;
double td2 = 2;
LineOnTwoPlanes(tn1, td1, tn2, td2);
}
Vector norm = LinAlg.CrossProd(norm1, norm2);
/// norm1' * pt = -val1 [n1x n1y n1z] [ptx] [-val1] [n1x n1y 0] [ptx] [-val1]
/// norm2' * pt = -val2 => [n2x n2y n2z] * [pty] = [-val2] => [n2x n2y 0] * [pty] = [-val2]
/// [ptz] [0 ]
/// => [n1x n1y] * [ptx] = [-val1] => [n1x n1y] * [ptx] = [-val1]
/// [n2x n2y] [pty] [-val2] [n2x n2y] [pty] [-val2]
/// => [ptx] = [n1x n1y]-1 [-val1] = [ n2y -n1y] * [-val1] / (n1x*n2y - n1y*n2x)
/// [pty] [n2x n2y] * [-val2] [-n2x n1x] [-val2]
/// = [ n2y*-val1 + -n1y*-val2 ] / (n1x*n2y - n1y*n2x)
/// [-n2x*-val1 + n1x*-val2 ]
double n1x = norm1[0], n1y = norm1[1];
double n2x = norm2[0], n2y = norm2[1];
double div = n1x * n2y - n1y * n2x;
Vector pt = new double[] { (n2y * -val1 + -n1y * -val2) / div,
(-n2x * -val1 + n1x * -val2) / div,
0 };
if (HDebug.IsDebuggerAttached)
{
Vector dbg_pt;
dbg_pt = pt + 1 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
dbg_pt = pt + 2 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
dbg_pt = pt - 2 * norm;
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm1, dbg_pt) + val1);
HDebug.AssertTolerance(0.00000001, LinAlg.VtV(norm2, dbg_pt) + val2);
}
return(new Tuple <Vector, Vector>(pt, norm));
}
public static Vector From(object data)
{
Type dataType = data.GetType();
if (dataType.IsSubclassOf(typeof(double[])))
{
return(new Vector((double[])data));
}
//if(dataType.IsSubclassOf(typeof(Double[]))) { return new Vector((double[])data); }
//if(dataType.IsInstanceOfType(typeof(Double[]))) { return new Vector((double[])data); }
if (data is double[])
{
return(new Vector((double[])data));
}
HDebug.Assert(false);
return(null);
}
public double Det(ILinAlgMat mat)
{
Func <ILinAlgMat, double> func = _Det;
if (HDebug.Selftest(func))
{
ILinAlgMat tA = ToILMat(new double[, ] {
{ 1, 2, 3 }, { 2, 4, 5 }, { 3, 5, 6 }
});
double tdet = Det(tA);
if (Math.Abs(tdet - (-1.0)) > 0.00000001)
{
Exit("Det(A) is wrong");
}
}
return(func(mat));
}
//public static double Corr(Vector vec1, Vector vec2)
//{
// return Corr(vec1.ToArray().ToList(), vec2.ToArray().ToList());
//}
public static double HCorr(double[] vec1, double[] vec2)
{
if (HDebug.Selftest())
{
// check with mathematica
double tcorr = HCorr(new double[] { 1, 2, 3 }, new double[] { 3, 7, 4 });
double terr = 0.2401922307076307 - tcorr;
HDebug.AssertTolerance(0.00000001, terr);
}
if (vec1.Length != vec2.Length)
{
throw new Exception();
}
double corr = HCov(vec1, vec2) / Math.Sqrt(vec1.HVar() * vec2.HVar());
return(corr);
}
public static double[,] DistPtPt(IList <Vector> points1, IList <Vector> points2)
{
HDebug.Assert(points1.Count == points2.Count);
double[,] dists = new double[points1.Count, points2.Count];
for (int c1 = 0; c1 < points1.Count; c1++)
{
for (int c2 = 0; c2 < points2.Count; c2++)
{
Vector pt1 = points1[c1];
Vector pt2 = points2[c2];
HDebug.Assert(pt1.Size == pt2.Size);
double dist2 = LinAlg.VtV(pt1, pt2);
dists[c1, c2] = Math.Sqrt(dist2);
}
}
return(dists);
}
public static T HGetItem <T>(this Tuple <T, T, T, T, T> values, int idx)
{
switch (idx)
{
case 0: return(values.Item1);
case 1: return(values.Item2);
case 2: return(values.Item3);
case 3: return(values.Item4);
case 4: return(values.Item5);
default: HDebug.Assert(false); throw new IndexOutOfRangeException();
}
}
public static double?Det(this MatrixByArr mat)
{
if (mat.ColSize == 2 && mat.RowSize == 2)
{
return(Det2(mat));
}
if (mat.ColSize == 3 && mat.RowSize == 3)
{
return(Det3(mat));
}
if (mat.ColSize == 4 && mat.RowSize == 4)
{
return(Det4(mat));
}
HDebug.Assert(false);
return(null);
}
public static T[] HSelectByIndex <T>
(this IList <T> list
, IDictionary <int, int> idx
, T defnull //=default(T)
)
{
if (HSelectByIndex_selftest)
#region selftest
{
HSelectByIndex_selftest = false;
string[] _list = new string[] { "3", "1", "0", "2", "x", };
Dictionary <int, int> _idx = new Dictionary <int, int>();
_idx.Add(0, 2);
_idx.Add(1, 1);
_idx.Add(3, 0);
string[] _out = _list.HSelectByIndex(_idx, "-");
HDebug.Assert(_out.Length == 4);
HDebug.Assert(_out[0] == "0");
HDebug.Assert(_out[1] == "1");
HDebug.Assert(_out[2] == "-");
HDebug.Assert(_out[3] == "3");
}
#endregion
int leng = idx.Keys.Max() + 1;
T[] select = new T [leng];
bool[] assign = new bool[leng];
for (int i = 0; i < leng; i++)
{
select[i] = defnull;
assign[i] = false;
}
foreach (var i_idxi in idx)
{
int i = i_idxi.Key;
int idxi = i_idxi.Value; HDebug.Assert(idxi == idx[i]);
if (assign[i] == true)
{
throw new HException("multi-index");
}
assign[i] = true;
select[i] = list[idxi];
}
return(select);
}