/*
* Add star to system
*/
public void AddStar(Star a)
{
int index = -1;
bool flag = false;
for (index = 0; index < maxStarNum; index++)
{
if (validMap[index] == false)
{
flag = true;
break;
}
alpha = GetAlphaOfGearsMethod(gearsMethodOrderNum);
}
if (flag == false)
{
Debug.Log("Star system full!");
}
else
{
s[index] = a;
validMap[index] = true;
validCount++;
updateNumBeforeGearsMethod = 0;
old_velAll = new VEC[gearsMethodOrderNum - 1, validCount];
old_posAll = new VEC[gearsMethodOrderNum - 1, validCount];
}
}
public static VEC LU_Solve(MAT A1, VEC b1)
{
MAT A = new MAT(A1);
VEC b = new VEC(b1);
int n = A.dim();
A = luFact(A);
MAT L = new MAT(n);
MAT U = new MAT(n);
for (int i = 1; i < n; i++)
{
for (int j = 0; j < i; j++)
{
L[i][j] = A[i][j];
}
}
for (int i = 0; i < n; i++)
{
L[i][i] = 1;
}
for (int i = 0; i < n; i++)
{
for (int j = i; j < n; j++)
{
U[i][j] = A[i][j];
}
}
VEC y = fwdSubs(L, b);
VEC x = bckSubs(U, y);
return(x);
}
public MAT(int dim)
{
n = dim;
va = new VEC[n];
for (int i = 0; i < n; i++)
{
va[i] = new VEC(n);
}
}
public VECV3(int n, VEC[] a)
{
dim = n;
val = new VEC[dim];
for (int i = 0; i < dim; i++)
{
val[i] = new VEC(a[i]);
}
}
public VECV3(int n)
{
dim = n;
val = new VEC[dim];
for (int i = 0; i < dim; i++)
{
val[i] = new VEC(3);
}
}
public VECV3(VECV3 v1)
{
dim = v1.len();
val = new VEC[dim];
for (int i = 0; i < dim; i++)
{
val[i] = new VEC(v1[i]);
}
}
public MAT(MAT m1)
{
n = m1.dim();
va = new VEC[n];
for (int i = 0; i < n; i++)
{
va[i] = new VEC(m1[i]);
}
}
public VEC(VEC v1)
{
dim = v1.len();
val = new double[dim];
for (int i = 0; i < dim; i++)
{
val[i] = v1[i];
}
}
public starSystem()
{
validMap = new bool[maxStarNum];
s = new Star[maxStarNum];
for (int i = 0; i < maxStarNum; i++)
{
s[i] = new Star();
}
alpha = new VEC(gearsMethodOrderNum + 1);
}
/*
* Return distance between two input vectors
*/
private double Distance(VEC a, VEC b)
{
double dist = 0;
for (int i = 0; i < 3; i++)
{
dist += Math.Pow(a[i] - b[i], 2);
}
return(Math.Sqrt(dist));
}
public static VECV3 operator -(VECV3 v1)
{
int n = v1.len();
for (int i = 0; i < n; i++)
{
v1[i] = new VEC(-v1[i]);
}
return(v1);
}
public VEC abs()
{
VEC s = new VEC(dim);
for (int i = 0; i < dim; i++)
{
s[i] = val[i].abs();
}
return(s);
}
public static VEC operator *(double a, VEC v1)
{
int n = v1.len();
VEC v = new VEC(n);
for (int i = 0; i < n; i++)
{
v[i] = v1[i] * a;
}
return(v);
}
public static VEC operator /(VEC v1, double a)
{
int n = v1.len();
VEC v = new VEC(n);
for (int i = 0; i < n; i++)
{
v[i] = v1[i] / a;
}
return(v);
}
public static VEC operator -(VEC v1, VEC v2)
{
int n = v1.len();
VEC v = new VEC(n);
for (int i = 0; i < n; i++)
{
v[i] = v1[i] - v2[i];
}
return(v);
}
public static VEC fwdSubs(MAT m1, VEC b)
{
int len = b.len();
VEC v = new VEC(len);
for (int i = 0; i < len; i++)
{
double s = 0;
for (int j = 0; j < i; j++)
{
s += m1[i][j] * v[j];
}
v[i] = (b[i] - s) / m1[i][i];
}
return(v);
}
public static VEC Normalize(VEC v)
{
int n = v.len();
VEC X = new VEC(n);
double abs = v.abs();
if (abs < 1E-12)
{
return(X);
}
else
{
X = v / abs;
return(X);
}
}
public MAT(int dim, double[] a)
{
n = dim;
va = new VEC[n];
for (int i = 0; i < n; i++)
{
va[i] = new VEC(n);
}
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
this[i][j] = a[i * n + j];
}
}
}
public static VECV3 fwdSubs(MATM3 m1, VECV3 b)
{
int len = b.len();
VECV3 v = new VECV3(len);
for (int i = 0; i < len; i++)
{
VEC s = new VEC(3);
for (int j = 0; j < i; j++)
{
s += m1[i][j] * v[j];
}
v[i] = (b[i] - s) / m1[i][i];
}
return(v);
}
public static VEC operator *(VEC v1, MAT m1)
{
int n = m1.dim();
VEC v = new VEC(n);
double s;
for (int i = 0; i < n; i++)
{
s = 0;
for (int j = 0; j < n; j++)
{
s += v1[j] * m1[j][i];
}
v[i] = s;
}
return(v);
}
public static VEC bckSubs(MAT m1, VEC b)
{
int len = b.len();
VEC v = new VEC(len);
for (int i = 0; i < len; i++)
{
v[i] = b[i];
}
for (int i = len - 1; i >= 0; i--)
{
v[i] = v[i] / m1[i][i];
for (int j = i - 1; j >= 0; j--)
{
v[j] = v[j] - (m1[j][i] * v[i]);
}
}
return(v);
}
/*
* Return vector of coefficients of Gear's Method with certain order number
*/
private VEC GetAlphaOfGearsMethod(int order)
{
MAT A = new MAT(order + 1);
VEC b = new VEC(order + 1);
for (int i = 0; i < order + 1; i++)
{
b[i] = 1;
A[i][order] = i;
}
for (int k = 0; k <= order; k++)
{
for (int j = 0; j < order; j++)
{
A[k][j] = Math.Pow(-j, k);
}
}
VEC v = numericalFunctions.LU_Solve(A, b);
return(v);
}
public static double Lp_norm(VEC v, double p)
{
double s = 0;
if (p == -1)
{
for (int i = 0; i < v.len(); i++)
{
if (s < Math.Abs(v[i]))
{
s = Math.Abs(v[i]);
}
}
}
else
{
for (int i = 0; i < v.len(); i++)
{
s += Math.Pow(v[i], p);
}
s = Math.Pow(s, 1.0 / p);
}
return(s);
}
/*
* void UpdateTrapezoidalMethod(double h)
*
* Trapezoidal method on velocity
* Trapezoidal method on position
*
* Is the most accurate method
*/
public void UpdateTrapezoidalMethod(double h)
{
if (validCount <= 0)
{
return;
}
// Map valid star index into validIndex
int[] validIndex = new int[validCount];
int indexTmp = 0;
for (int i = 0; i < maxStarNum; i++)
{
if (validMap[i] == true)
{
validIndex[indexTmp] = i;
indexTmp++;
}
}
// copy velocity and position of all stars into arrays
VEC[] velAll = new VEC[validCount];
VEC[] new_velAll = new VEC[validCount];
VEC[] posAll = new VEC[validCount];
VEC[] new_posAll = new VEC[validCount];
VEC massAll = new VEC(validCount);
for (int i = 0; i < validCount; i++)
{
velAll[i] = new VEC(s[validIndex[i]].vel);
new_velAll[i] = new VEC(s[validIndex[i]].vel);
posAll[i] = new VEC(s[validIndex[i]].pos);
new_posAll[i] = new VEC(s[validIndex[i]].pos);
massAll[i] = s[validIndex[i]].mass;
}
double e_threshold = 1E-9; // the error threshold of performing Newton’s Method
double err = 1.0 + e_threshold;
int maxIterNum = 100; // Newton’s Method might not converge, we need a max iteration number to terminate.
while (err > e_threshold & maxIterNum > 0)
{
VECV3 F = new VECV3(2 * validCount);
for (int i = 0; i < validCount; i++)
{
VEC a = new VEC(3);
for (int j = 0; j < validCount; j++)
{
if (i != j)
{
a += 0.5 * VEC.Normalize(new_posAll[j] - new_posAll[i]) * massAll[j] / DistancePow2(new_posAll[i], new_posAll[j])
+ 0.5 * VEC.Normalize(posAll[j] - posAll[i]) * massAll[j] / DistancePow2(posAll[i], posAll[j]);
}
}
F[i] = G * a - (new_velAll[i] - velAll[i]) / h;
}
for (int i = validCount; i < 2 * validCount; i++)
{
F[i] = (new_posAll[i - validCount] - posAll[i - validCount]) / h - 0.5 * (new_velAll[i - validCount] + velAll[i - validCount]);
}
MATM3 JF = new MATM3(2 * validCount);
MAT tmp;
for (int i = 0; i < validCount; i++)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = -1.0 / h;
}
JF[i][i] = tmp;
}
for (int i = 0; i < validCount; i++)
{
for (int j_inJF = validCount; j_inJF < 2 * validCount; j_inJF++)
{
int j = j_inJF - validCount;
tmp = new MAT(3);
if (i != j)
{
for (int a = 0; a < 3; a++)
{
for (int b = 0; b < 3; b++)
{
if (a == b)
{
tmp[a][b] = massAll[j] * G *
(-3 * Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -2.5) * (new_posAll[j][a] - new_posAll[i][a]) * new_posAll[j][a]
+ Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -1.5));
}
else
{
tmp[a][b] = massAll[j] * G * -3 * Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -2.5) * (new_posAll[j][a] - new_posAll[i][a]) * (new_posAll[j][b] - new_posAll[i][b]);
}
}
}
}
else
{
for (int a = 0; a < 3; a++)
{
for (int b = 0; b < 3; b++)
{
if (a == b)
{
double sInTmp = 0;
for (int k = 0; k < validCount; k++)
{
if (k != j)
{
sInTmp += massAll[k] *
(3 * Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -2.5) * (new_posAll[k][a] - new_posAll[j][a]) * (new_posAll[k][a] - new_posAll[i][a])
- Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -1.5));
}
}
tmp[a][a] = G * sInTmp;
}
else
{
double sInTmp = 0;
for (int k = 0; k < validCount; k++)
{
if (k != j)
{
sInTmp += massAll[k] *
(3 * Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -2.5) * (new_posAll[k][a] - new_posAll[j][a]) * (new_posAll[k][b] * new_posAll[i][b]));
}
}
tmp[a][a] = G * sInTmp;
}
}
}
}
JF[i][j_inJF] = 0.5 * tmp;
}
}
for (int i = validCount; i < 2 * validCount; i++)
{
for (int j = 0; j < validCount; j++)
{
if (i - validCount == j)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = -0.5;
}
JF[i][j] = tmp;
}
}
}
for (int i = validCount; i < 2 * validCount; i++)
{
for (int j = validCount; j < 2 * validCount; j++)
{
if (i == j)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = 1.0 / h;
}
JF[i][j] = tmp;
}
}
}
VECV3 delta = numericalFunctions.LU_Solve(JF, -F);
for (int i = 0; i < validCount; i++)
{
new_velAll[i] += delta[i];
new_posAll[i] += delta[i + validCount];
}
err = VECV3.L2_norm_special(F);
maxIterNum--;
if (maxIterNum == 0)
{
Debug.Log("Max iter number reached. Err: " + err + " Using Forward method instead in this period.");
for (int i = 0; i < numOfForwardMethodInstead; i++)
{
UpdateForwardMethod(h / numOfForwardMethodInstead);
}
return;
}
}
// Apply new position and velocity to Star objects
for (int i = 0; i < validCount; i++)
{
if (maxIterNum != 0)
{
s[validIndex[i]].vel = new_velAll[i];
s[validIndex[i]].pos = new_posAll[i];
}
}
// Check if any pair of stars have distance smaller than minDistanceToDistroy.
// If so, merge two stars.
for (int i = 0; i < validCount; i++)
{
for (int j = i + 1; j < validCount; j++)
{
if (DistanceBetweenStarsPow2(s[validIndex[i]], s[validIndex[j]]) < minDistanceToDistroy)
{
s[validIndex[j]].pos += (s[validIndex[i]].pos - s[validIndex[j]].pos) * s[validIndex[i]].mass / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].vel = (s[validIndex[j]].vel * s[validIndex[j]].mass + s[validIndex[i]].vel * s[validIndex[i]].mass) / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].mass += s[validIndex[i]].mass;
DestroyStar(s[validIndex[i]], 0);
continue;
}
}
}
}
/*
* void UpdateBackwardMethod(double h)
*
* Backward method on velocity
* Trapezoidal method on position
*/
public void UpdateBackwardMethod(double h)
{
if (validCount <= 0)
{
return;
}
int[] validIndex = new int[validCount];
int indexTmp = 0;
for (int i = 0; i < maxStarNum; i++)
{
if (validMap[i] == true)
{
validIndex[indexTmp] = i;
indexTmp++;
}
}
VEC[] velAll = new VEC[validCount];
VEC[] new_velAll = new VEC[validCount];
VEC[] posAll = new VEC[validCount];
VEC[] new_posAll = new VEC[validCount];
VEC massAll = new VEC(validCount);
for (int i = 0; i < validCount; i++)
{
velAll[i] = new VEC(s[validIndex[i]].vel);
new_velAll[i] = new VEC(s[validIndex[i]].vel);
posAll[i] = new VEC(s[validIndex[i]].pos);
new_posAll[i] = new VEC(s[validIndex[i]].pos);
massAll[i] = s[validIndex[i]].mass;
}
double e_threshold = 1E-7;
double err = 1.0 + e_threshold;
int maxIterNum = 100;
while (err > e_threshold & maxIterNum > 0)
{
VECV3 F = new VECV3(2 * validCount);
for (int i = 0; i < validCount; i++)
{
VEC a = new VEC(3);
for (int j = 0; j < validCount; j++)
{
if (i != j)
{
a += VEC.Normalize(new_posAll[j] - new_posAll[i]) * massAll[j] / DistancePow2(new_posAll[i], new_posAll[j]);
}
}
F[i] = G * a - (new_velAll[i] - velAll[i]) / h;
}
for (int i = validCount; i < 2 * validCount; i++)
{
F[i] = (new_posAll[i - validCount] - posAll[i - validCount]) / h - 0.5 * (new_velAll[i - validCount] + velAll[i - validCount]);
}
MATM3 JF = new MATM3(2 * validCount);
MAT tmp;
for (int i = 0; i < validCount; i++)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = -1.0 / h;
}
JF[i][i] = tmp;
}
for (int i = 0; i < validCount; i++)
{
for (int j_inJF = validCount; j_inJF < 2 * validCount; j_inJF++)
{
int j = j_inJF - validCount;
tmp = new MAT(3);
if (i != j)
{
for (int a = 0; a < 3; a++)
{
for (int b = 0; b < 3; b++)
{
if (a == b)
{
tmp[a][b] = massAll[j] * G *
(-3 * Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -2.5) * (new_posAll[j][a] - new_posAll[i][a]) * new_posAll[j][a]
+ Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -1.5));
}
else
{
tmp[a][b] = massAll[j] * G * -3 * Math.Pow(DistancePow2(new_posAll[j], new_posAll[i]), -2.5) * (new_posAll[j][a] - new_posAll[i][a]) * (new_posAll[j][b] - new_posAll[i][b]);
}
}
}
}
else
{
for (int a = 0; a < 3; a++)
{
for (int b = 0; b < 3; b++)
{
if (a == b)
{
double sInTmp = 0;
for (int k = 0; k < validCount; k++)
{
if (k != j)
{
sInTmp += massAll[k] *
(3 * Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -2.5) * (new_posAll[k][a] - new_posAll[j][a]) * (new_posAll[k][a] - new_posAll[i][a])
- Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -1.5));
}
}
tmp[a][a] = G * sInTmp;
}
else
{
double sInTmp = 0;
for (int k = 0; k < validCount; k++)
{
if (k != j)
{
sInTmp += massAll[k] *
(3 * Math.Pow(DistancePow2(new_posAll[k], new_posAll[j]), -2.5) * (new_posAll[k][a] - new_posAll[j][a]) * (new_posAll[k][b] * new_posAll[i][b]));
}
}
tmp[a][a] = G * sInTmp;
}
}
}
}
JF[i][j_inJF] = tmp;
}
}
for (int i = validCount; i < 2 * validCount; i++)
{
for (int j = 0; j < validCount; j++)
{
if (i - validCount == j)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = -0.5;
}
JF[i][j] = tmp;
}
}
}
for (int i = validCount; i < 2 * validCount; i++)
{
for (int j = validCount; j < 2 * validCount; j++)
{
if (i == j)
{
tmp = new MAT(3);
for (int k = 0; k < 3; k++)
{
tmp[k][k] = 1.0 / h;
}
JF[i][j] = tmp;
}
}
}
VECV3 delta = numericalFunctions.LU_Solve(JF, -F);
for (int i = 0; i < validCount; i++)
{
new_velAll[i] += delta[i];
new_posAll[i] += delta[i + validCount];
}
err = VECV3.L2_norm_special(F);
maxIterNum--;
if (maxIterNum == 0)
{
Debug.Log("Max iter number reached. Err: " + err + " Using Forward method instead in this period.");
UpdateForwardMethod(h / 2.0);
UpdateForwardMethod(h / 2.0);
return;
}
}
for (int i = 0; i < validCount; i++)
{
if (maxIterNum != 0)
{
s[validIndex[i]].vel = new_velAll[i];
s[validIndex[i]].pos = new_posAll[i];
}
}
for (int i = 0; i < validCount; i++)
{
for (int j = i + 1; j < validCount; j++)
{
if (DistanceBetweenStarsPow2(s[validIndex[i]], s[validIndex[j]]) < minDistanceToDistroy)
{
s[validIndex[j]].pos += (s[validIndex[i]].pos - s[validIndex[j]].pos) * s[validIndex[i]].mass / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].vel = (s[validIndex[j]].vel * s[validIndex[j]].mass + s[validIndex[i]].vel * s[validIndex[i]].mass) / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].mass += s[validIndex[i]].mass;
DestroyStar(s[validIndex[i]], 0);
continue;
}
}
}
}
/*
* Simplest way to update the system but have the most error
*/
public void UpdateForwardMethod(double h)
{
int[] validIndex = new int[validCount];
int indexTmp = 0;
VEC[] newvels = new VEC[validCount];
for (int i = 0; i < validCount; i++)
{
newvels[i] = new VEC(3);
}
for (int i = 0; i < maxStarNum; i++)
{
if (validMap[i] == true)
{
validIndex[indexTmp] = i;
indexTmp++;
}
}
double[] arr = new double[2] {
-1, -1
};
VEC toDestroy = new VEC(2, arr);
for (int i = 0; i < validCount; i++)
{
VEC updateV;
updateV = new VEC(3);
for (int j = 0; j < validCount; j++)
{
if (j != i)
{
updateV += VEC.Normalize(s[validIndex[j]].pos - s[validIndex[i]].pos) * s[validIndex[j]].mass / DistanceBetweenStarsPow2(s[validIndex[i]], s[validIndex[j]]);
}
}
newvels[i] = s[validIndex[i]].vel + h * G * updateV;
}
for (int i = 0; i < validCount; i++)
{
s[validIndex[i]].pos += h * s[validIndex[i]].vel;
}
for (int i = 0; i < validCount; i++)
{
s[validIndex[i]].vel = newvels[i];
}
for (int i = 0; i < validCount; i++)
{
for (int j = i + 1; j < validCount; j++)
{
if (DistanceBetweenStarsPow2(s[validIndex[i]], s[validIndex[j]]) < minDistanceToDistroy)
{
s[validIndex[j]].pos += (s[validIndex[i]].pos - s[validIndex[j]].pos) * s[validIndex[i]].mass / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].vel = (s[validIndex[j]].vel * s[validIndex[j]].mass + s[validIndex[i]].vel * s[validIndex[i]].mass) / (s[validIndex[j]].mass + s[validIndex[i]].mass);
s[validIndex[j]].mass += s[validIndex[i]].mass;
DestroyStar(s[validIndex[i]], 0);
continue;
}
}
}
}