//逆行列
public MatrixNxM InverseMatrix()
{
MatrixNxM a = new MatrixNxM(this);
if (a.Mat.GetLength(0) != a.Mat.GetLength(1))
{
//非正方行列なら擬似逆行列を求める
return((a.TransMatrix() * a).InverseMatrix() * a.TransMatrix());
}
else
{
//掃き出し法(http://thira.plavox.info/blog/2008/06/_c.html を参考)
int n = a.Mat.GetLength(0);
MatrixNxM c = MatrixNxM.UnitMatrix(n);
int correctRows = 0;
double buf = 0.0f;
for (int i = 0; i < n; i++)
{
//a.Mat[i, l] != 0.0f(l>=i)となる行lを探す
int l = i;
while (l < n)
{
if (a.Mat [l, i] != 0.0f)
{
break;
}
l++;
}
//見つからなければ次の行を処理
if (l == n)
{
continue;
}
//l行目とi行目を入れ替え
for (int k = 0; k < n; k++)
{
buf = a.Mat [l, k];
a.Mat [l, k] = a.Mat [i, k];
a.Mat [i, k] = buf;
buf = c.Mat [l, k];
c.Mat [l, k] = c.Mat [i, k];
c.Mat [i, k] = buf;
}
//a.Mat[i, i]==1となるようにi行目を割り算
buf = a.Mat [i, i];
for (int j = 0; j < n; j++)
{
a.Mat [i, j] /= buf;
c.Mat [i, j] /= buf;
}
//a.Mat[j, i]==0となるように、(j行目)-(i行目)*a.Mat[j, i]をする
for (int j = 0; j < n; j++)
{
if (i == j)
{
continue;
}
buf = a.Mat [j, i];
for (int k = 0; k < n; k++)
{
a.Mat [j, k] -= a.Mat [i, k] * buf;
c.Mat [j, k] -= c.Mat [i, k] * buf;
}
}
correctRows++;
}
if (correctRows < n)
{
c = null;
}
return(c);
}
}
//面に貼る三角形のインデックス配列を作成(同一平面上に存在していることが前提)
// vertices :頂点の配列
public static int[] CreateIndexTriangles(Vector3[] vertices)
{
//インデックスを初期化
List <int> sortedIndex = new List <int> ();
for (int i = 0; i < vertices.Length; i++)
{
sortedIndex.Add(i);
}
//頂点数が規定に達したら終了
List <int> triangles = new List <int> ();
while (triangles.Count < (vertices.Length - 2) * 3)
{
int index0 = 0, index1 = 1, index2 = 2;
int count = sortedIndex.Count;
for (int i = 0; i < count; i++)
{
//2辺に接する三角形を選択
index0 = (i - 1 + count) % count;
index1 = (i + count) % count;
index2 = (i + 1 + count) % count;
Vector3 o = vertices [sortedIndex [index1]];
Vector3 oa = vertices [sortedIndex [index0]] - o;
Vector3 ob = vertices [sortedIndex [index2]] - o;
Vector3 oc = (oa + ob) / 2;
{
//この三角形が面内部に内包されているものか調べる
// 重心と注目した頂点でできる直線上の、交点の数によって判定
// op + s(oq-op) = toc + uN (0f<=s<1f, Nは法線)
// op = s(op-oq) + toc + uN
// で調べられる
int ocount = 0, ccount = 0;
for (int j = 0; j < count; j++)
{
int j0, j1, j2;
j0 = (j + count - 1) % count;
j1 = (j + count) % count;
j2 = (j + count + 1) % count;
Vector3 op = vertices [sortedIndex [j1]] - o;
Vector3 oq = vertices [sortedIndex [j2]] - o;
Vector3 qp = op - oq;
Vector3 n = Vector3.Cross(oc, qp);
MatrixNxM m = new MatrixNxM(3, 3);
m.Mat [0, 0] = qp.x; m.Mat [0, 1] = oc.x; m.Mat [0, 2] = n.x;
m.Mat [1, 0] = qp.y; m.Mat [1, 1] = oc.y; m.Mat [1, 2] = n.y;
m.Mat [2, 0] = qp.z; m.Mat [2, 1] = oc.z; m.Mat [2, 2] = n.z;
MatrixNxM v = new MatrixNxM(3, 1);
v.Mat [0, 0] = op.x;
v.Mat [1, 0] = op.y;
v.Mat [2, 0] = op.z;
MatrixNxM m_inv = m.InverseMatrix();
if (m_inv == null)
{
continue;
}
MatrixNxM x = m_inv * v;
int addCount = 0;
float s = (float)x.Mat [0, 0], t = (float)x.Mat [1, 0];
if (s == 0f)
{
Vector3 d, e;
d = vertices [sortedIndex [j0]] - vertices [sortedIndex [j1]];
e = vertices [sortedIndex [j2]] - vertices [sortedIndex [j1]];
if (Vector3.Dot(Vector3.Cross(oc, d), Vector3.Cross(oc, e)) < 0f)
{
addCount = 1;
}
else
{
addCount = 2;
}
}
else if (0f < s && s < 1f)
{
addCount = 1;
}
if (t <= 0f)
{
ocount += addCount;
}
else
{
ccount += addCount;
}
}
if ((ocount % 2) * (ccount % 2) == 0)
{
continue;
}
}
{
//三角形内部に他の頂点が存在しないか判定
// soa + tob + uN = op を解く (Nは法線)
//三角形の法線ベクトルを作成
Vector3 n = Vector3.Cross(oa, ob);
MatrixNxM m = new MatrixNxM(3, 3);
m.Mat [0, 0] = oa.x; m.Mat [0, 1] = ob.x; m.Mat [0, 2] = n.x;
m.Mat [1, 0] = oa.y; m.Mat [1, 1] = ob.y; m.Mat [1, 2] = n.y;
m.Mat [2, 0] = oa.z; m.Mat [2, 1] = ob.z; m.Mat [2, 2] = n.z;
MatrixNxM m_inv = m.InverseMatrix();
if (m_inv == null)
{
continue;
}
bool isValidTriangle = true;
for (int j = 0; j < count; j++)
{
Vector3 op = vertices [sortedIndex [j]] - o;
MatrixNxM v = new MatrixNxM(3, 1);
v.Mat [0, 0] = op.x;
v.Mat [1, 0] = op.y;
v.Mat [2, 0] = op.z;
MatrixNxM x = m_inv * v;
float s = (float)x.Mat [0, 0], t = (float)x.Mat [1, 0];
if (0f < s && s < 1f)
{
if (0f < t && t < 1f)
{
if (0f < s + t && s + t < 1f)
{
isValidTriangle = false;
break;
}
}
}
}
if (!isValidTriangle)
{
continue;
}
}
break;
}
triangles.Add(sortedIndex [index0]);
triangles.Add(sortedIndex [index1]);
triangles.Add(sortedIndex [index2]);
sortedIndex.RemoveAt(index1);
}
return(triangles.ToArray());
}
public static MatrixNxM operator *(MatrixNxM a, MatrixNxM b)
{
return(MatrixNxM.MaltiplyMatrix(a, b));
}
