public void analyze()
{
if (elements.Count == 0)
{
return;
}
nodeList.Clear();
List <bool> internalList = new List <bool>();
Action <long, bool> pushNode = (id, isInternal) => {
if (!nodeList.Contains(id))
{
nodeList.Add(id);
internalList.Add(isInternal);
}
};
#region //// Look for Voltage or Ground element ////
// Search the circuit for a Ground, or Voltage sourcre
ICircuitElement voltageElm = null;
bool gotGround = false;
bool gotRail = false;
for (int i = 0; i != elements.Count; i++)
{
ICircuitElement elem = elements[i];
if (elem is Ground)
{
gotGround = true;
break;
}
if (elem is VoltageInput)
{
gotRail = true;
}
if (elem is Voltage && voltageElm == null)
{
voltageElm = elem;
}
}
// If no ground and no rails, then the voltage elm's first terminal is ground.
if (!gotGround && !gotRail && voltageElm != null)
{
int elm_ndx = elements.IndexOf(voltageElm);
long[] ndxs = nodeMesh[elm_ndx];
pushNode(ndxs[0], false);
}
else
{
// If the circuit contains a ground, rail, or voltage
// element, push a temporary node to the node list.
pushNode(snowflake.NextId(), false);
}
#endregion
// At this point, there is 1 node in the list, the special `global ground` node.
#region //// Nodes and Voltage Sources ////
int vscount = 0; // Number of voltage sources
for (int i = 0; i != elements.Count; i++)
{
ICircuitElement elm = elements[i];
int leads = elm.getLeadCount();
// If the leadCount reported by the element does
// not match the number of leads allocated,
// resize the array.
if (leads != nodeMesh[i].Length)
{
long[] leadMap = nodeMesh[i];
Array.Resize(ref leadMap, leads);
nodeMesh[i] = leadMap;
}
// For each lead in the element
for (int leadX = 0; leadX != leads; leadX++)
{
long leadNode = nodeMesh[i][leadX]; // Id of the node leadX is connected too
int node_ndx = nodeList.IndexOf(leadNode); // Index of the leadNode in the nodeList
if (node_ndx == -1)
{
// If the nodeList doesn't contain the node, push it
// onto the list and assign it's new index to the lead.
elm.setLeadNode(leadX, nodeList.Count);
pushNode(leadNode, false);
}
else
{
// Otherwise, assign the lead the index of
// the node in the nodeList.
elm.setLeadNode(leadX, node_ndx);
if (leadNode == 0)
{
// if it's the ground node, make sure the
// node voltage is 0, cause it may not get set later
elm.setLeadVoltage(leadX, 0); // TODO: ??
}
}
}
// Push an internal node onto the list for
// each internal lead on the element.
int internalLeads = elm.getInternalLeadCount();
for (int x = 0; x != internalLeads; x++)
{
elm.setLeadNode(leads + x, nodeList.Count);
pushNode(snowflake.NextId(), true);
}
vscount += elm.getVoltageSourceCount();
}
#endregion
// == Creeate the voltageSources array.
// Also determine if circuit is nonlinear.
// VoltageSourceId -> ICircuitElement map
voltageSources = new ICircuitElement[vscount];
vscount = 0;
circuitNonLinear = false;
//for(int i = 0; i != elements.Count; i++) {
// ICircuitElement elem = elements[i];
foreach (ICircuitElement elem in elements)
{
if (elem.nonLinear())
{
circuitNonLinear = true;
}
// Assign each votage source in the element a globally unique id,
// (the index of the next open slot in voltageSources)
for (int leadX = 0; leadX != elem.getVoltageSourceCount(); leadX++)
{
voltageSources[vscount] = elem;
elem.setVoltageSource(leadX, vscount++);
}
}
#region //// Matrix setup ////
int matrixSize = nodeList.Count - 1 + vscount;
// setup circuitMatrix
circuitMatrix = new double[matrixSize][];
for (int z = 0; z < matrixSize; z++)
{
circuitMatrix[z] = new double[matrixSize];
}
circuitRightSide = new double[matrixSize];
// setup origMatrix
origMatrix = new double[matrixSize][];
for (int z = 0; z < matrixSize; z++)
{
origMatrix[z] = new double[matrixSize];
}
origRightSide = new double[matrixSize];
// setup circuitRowInfo
circuitRowInfo = new RowInfo[matrixSize];
for (int i = 0; i != matrixSize; i++)
{
circuitRowInfo[i] = new RowInfo();
}
circuitPermute = new int[matrixSize];
circuitMatrixSize = circuitMatrixFullSize = matrixSize;
circuitNeedsMap = false;
#endregion
// Stamp linear circuit elements.
for (int i = 0; i != elements.Count; i++)
{
elements[i].stamp(this);
}
#region //// Determine nodes that are unconnected ////
bool[] closure = new bool[nodeList.Count];
bool changed = true;
closure[0] = true;
while (changed)
{
changed = false;
for (int i = 0; i != elements.Count; i++)
{
ICircuitElement ce = elements[i];
// loop through all ce's nodes to see if they are connected
// to other nodes not in closure
for (int leadX = 0; leadX < ce.getLeadCount(); leadX++)
{
if (!closure[ce.getLeadNode(leadX)])
{
if (ce.leadIsGround(leadX))
{
closure[ce.getLeadNode(leadX)] = changed = true;
}
continue;
}
for (int k = 0; k != ce.getLeadCount(); k++)
{
if (leadX == k)
{
continue;
}
int kn = ce.getLeadNode(k);
if (ce.leadsAreConnected(leadX, k) && !closure[kn])
{
closure[kn] = true;
changed = true;
}
}
}
}
if (changed)
{
continue;
}
// connect unconnected nodes
for (int i = 0; i != nodeList.Count; i++)
{
if (!closure[i] && !internalList[i])
{
//System.out.println("node " + i + " unconnected");
stampResistor(0, i, 1E8);
closure[i] = true;
changed = true;
break;
}
}
}
#endregion
#region //// Sanity checks ////
for (int i = 0; i != elements.Count; i++)
{
ICircuitElement ce = elements[i];
// look for inductors with no current path
if (ce is InductorElm)
{
FindPathInfo fpi = new FindPathInfo(this, FindPathInfo.PathType.INDUCT, ce, ce.getLeadNode(1));
// first try findPath with maximum depth of 5, to avoid slowdowns
if (!fpi.findPath(ce.getLeadNode(0), 5) && !fpi.findPath(ce.getLeadNode(0)))
{
//System.out.println(ce + " no path");
ce.reset();
}
}
// look for current sources with no current path
if (ce is CurrentSource)
{
FindPathInfo fpi = new FindPathInfo(this, FindPathInfo.PathType.INDUCT, ce, ce.getLeadNode(1));
if (!fpi.findPath(ce.getLeadNode(0)))
{
panic("No path for current source!", ce);
}
}
// look for voltage source loops
if ((ce is Voltage && ce.getLeadCount() == 2) || ce is Wire)
{
FindPathInfo fpi = new FindPathInfo(this, FindPathInfo.PathType.VOLTAGE, ce, ce.getLeadNode(1));
if (fpi.findPath(ce.getLeadNode(0)))
{
panic("Voltage source/wire loop with no resistance!", ce);
}
}
// look for shorted caps, or caps w/ voltage but no R
if (ce is CapacitorElm)
{
FindPathInfo fpi = new FindPathInfo(this, FindPathInfo.PathType.SHORT, ce, ce.getLeadNode(1));
if (fpi.findPath(ce.getLeadNode(0)))
{
//System.out.println(ce + " shorted");
ce.reset();
}
else
{
fpi = new FindPathInfo(this, FindPathInfo.PathType.CAP_V, ce, ce.getLeadNode(1));
if (fpi.findPath(ce.getLeadNode(0)))
{
panic("Capacitor loop with no resistance!", ce);
}
}
}
}
#endregion
#region //// Simplify the matrix //// =D
for (int i = 0; i != matrixSize; i++)
{
int qm = -1, qp = -1;
double qv = 0;
RowInfo re = circuitRowInfo[i];
if (re.lsChanges || re.dropRow || re.rsChanges)
{
continue;
}
double rsadd = 0;
// look for rows that can be removed
int leadX = 0;
for (; leadX != matrixSize; leadX++)
{
double q = circuitMatrix[i][leadX];
if (circuitRowInfo[leadX].type == RowInfo.ROW_CONST)
{
// keep a running total of const values that have been removed already
rsadd -= circuitRowInfo[leadX].value * q;
continue;
}
if (q == 0)
{
continue;
}
if (qp == -1)
{
qp = leadX;
qv = q;
continue;
}
if (qm == -1 && q == -qv)
{
qm = leadX;
continue;
}
break;
}
if (leadX == matrixSize)
{
if (qp == -1)
{
panic("Matrix error", null);
}
RowInfo elt = circuitRowInfo[qp];
if (qm == -1)
{
// we found a row with only one nonzero entry;
// that value is a constant
for (int k = 0; elt.type == RowInfo.ROW_EQUAL && k < 100; k++)
{
// follow the chain
// System.out.println("following equal chain from " + i + " " + qp + " to " + elt.nodeEq);
qp = elt.nodeEq;
elt = circuitRowInfo[qp];
}
if (elt.type == RowInfo.ROW_EQUAL)
{
// break equal chains
// System.out.println("Break equal chain");
elt.type = RowInfo.ROW_NORMAL;
continue;
}
if (elt.type != RowInfo.ROW_NORMAL)
{
//System.out.println("type already " + elt.type + " for " + qp + "!");
continue;
}
elt.type = RowInfo.ROW_CONST;
elt.value = (circuitRightSide[i] + rsadd) / qv;
circuitRowInfo[i].dropRow = true;
// System.out.println(qp + " * " + qv + " = const " + elt.value);
i = -1; // start over from scratch
}
else if (circuitRightSide[i] + rsadd == 0)
{
// we found a row with only two nonzero entries, and one
// is the negative of the other; the values are equal
if (elt.type != RowInfo.ROW_NORMAL)
{
// System.out.println("swapping");
int qq = qm;
qm = qp;
qp = qq;
elt = circuitRowInfo[qp];
if (elt.type != RowInfo.ROW_NORMAL)
{
// we should follow the chain here, but this hardly
// ever happens so it's not worth worrying about
//System.out.println("swap failed");
continue;
}
}
elt.type = RowInfo.ROW_EQUAL;
elt.nodeEq = qm;
circuitRowInfo[i].dropRow = true;
// System.out.println(qp + " = " + qm);
}
}
}
// == Find size of new matrix
int nn = 0;
for (int i = 0; i != matrixSize; i++)
{
RowInfo elt = circuitRowInfo[i];
if (elt.type == RowInfo.ROW_NORMAL)
{
elt.mapCol = nn++;
// System.out.println("col " + i + " maps to " + elt.mapCol);
continue;
}
if (elt.type == RowInfo.ROW_EQUAL)
{
RowInfo e2 = null;
// resolve chains of equality; 100 max steps to avoid loops
for (int leadX = 0; leadX != 100; leadX++)
{
e2 = circuitRowInfo[elt.nodeEq];
if (e2.type != RowInfo.ROW_EQUAL)
{
break;
}
if (i == e2.nodeEq)
{
break;
}
elt.nodeEq = e2.nodeEq;
}
}
if (elt.type == RowInfo.ROW_CONST)
{
elt.mapCol = -1;
}
}
for (int i = 0; i != matrixSize; i++)
{
RowInfo elt = circuitRowInfo[i];
if (elt.type == RowInfo.ROW_EQUAL)
{
RowInfo e2 = circuitRowInfo[elt.nodeEq];
if (e2.type == RowInfo.ROW_CONST)
{
// if something is equal to a const, it's a const
elt.type = e2.type;
elt.value = e2.value;
elt.mapCol = -1;
}
else
{
elt.mapCol = e2.mapCol;
}
}
}
// == Make the new, simplified matrix.
int newsize = nn;
double[][] newmatx = new double[newsize][];
for (int z = 0; z < newsize; z++)
{
newmatx[z] = new double[newsize];
}
double[] newrs = new double[newsize];
int ii = 0;
for (int i = 0; i != matrixSize; i++)
{
RowInfo rri = circuitRowInfo[i];
if (rri.dropRow)
{
rri.mapRow = -1;
continue;
}
newrs[ii] = circuitRightSide[i];
rri.mapRow = ii;
// System.out.println("Row " + i + " maps to " + ii);
for (int leadX = 0; leadX != matrixSize; leadX++)
{
RowInfo ri = circuitRowInfo[leadX];
if (ri.type == RowInfo.ROW_CONST)
{
newrs[ii] -= ri.value * circuitMatrix[i][leadX];
}
else
{
newmatx[ii][ri.mapCol] += circuitMatrix[i][leadX];
}
}
ii++;
}
#endregion
#region //// Copy matrix to orig ////
circuitMatrix = newmatx;
circuitRightSide = newrs;
matrixSize = circuitMatrixSize = newsize;
// copy `rightSide` to `origRightSide`
for (int i = 0; i != matrixSize; i++)
{
origRightSide[i] = circuitRightSide[i];
}
// copy `matrix` to `origMatrix`
for (int i = 0; i != matrixSize; i++)
{
for (int leadX = 0; leadX != matrixSize; leadX++)
{
origMatrix[i][leadX] = circuitMatrix[i][leadX];
}
}
#endregion
circuitNeedsMap = true;
_analyze = false;
// If the matrix is linear, we can do the lu_factor
// here instead of needing to do it every frame.
if (!circuitNonLinear)
{
if (!lu_factor(circuitMatrix, circuitMatrixSize, circuitPermute))
{
panic("Singular matrix!", null);
}
}
}
void tick()
{
// Execute beginStep() on all elements
for (int i = 0; i != elements.Count; i++)
{
elements[i].beginStep(this);
}
int subiter;
int subiterCount = 5000;
for (subiter = 0; subiter != subiterCount; subiter++)
{
converged = true;
subIterations = subiter;
// Copy `origRightSide` to `circuitRightSide`
for (int i = 0; i != circuitMatrixSize; i++)
{
circuitRightSide[i] = origRightSide[i];
}
// If the circuit is non linear, copy
// `origMatrix` to `circuitMatrix`
if (circuitNonLinear)
{
for (int i = 0; i != circuitMatrixSize; i++)
{
for (int j = 0; j != circuitMatrixSize; j++)
{
circuitMatrix[i][j] = origMatrix[i][j];
}
}
}
// Execute step() on all elements
for (int i = 0; i != elements.Count; i++)
{
elements[i].step(this);
}
// Can't have any values in the matrix be NaN or Inf
for (int j = 0; j != circuitMatrixSize; j++)
{
for (int i = 0; i != circuitMatrixSize; i++)
{
double x = circuitMatrix[i][j];
if (Double.IsNaN(x) || Double.IsInfinity(x))
{
panic("NaN/Infinite matrix!", null);
}
}
}
// If the circuit is non-Linear, factor it now,
// if it's linear, it was factored in analyze()
if (circuitNonLinear)
{
// Break if the circuit has converged.
if (converged && subiter > 0)
{
break;
}
if (!lu_factor(circuitMatrix, circuitMatrixSize, circuitPermute))
{
panic("Singular matrix!", null);
}
}
// Solve the factorized matrix
lu_solve(circuitMatrix, circuitMatrixSize, circuitPermute, circuitRightSide);
for (int j = 0; j != circuitMatrixFullSize; j++)
{
double res = 0;
RowInfo ri = circuitRowInfo[j];
res = (ri.type == RowInfo.ROW_CONST) ? ri.value : circuitRightSide[ri.mapCol];
// If any resuit is NaN, break
if (Double.IsNaN(res))
{
converged = false;
break;
}
if (j < nodeList.Count - 1)
{
// For each node in the mesh
for (int k = 0; k != nodeMesh.Count; k++)
{
List <long> leads = new List <long>(nodeMesh[k]); // Get the leads conected to the node
int ndx = leads.IndexOf(getNodeId(j + 1));
if (ndx != -1)
{
elements[k].setLeadVoltage(ndx, res);
}
}
}
else
{
int ji = j - (nodeList.Count - 1);
voltageSources[ji].setCurrent(ji, res);
}
}
// if the matrix is linear, we don't
// need to do any more iterations
if (!circuitNonLinear)
{
break;
}
}
if (subiter > 5)
{
Debug.LogF("Nonlinear curcuit converged after {0} iterations.", subiter);
}
if (subiter == subiterCount)
{
panic("Convergence failed!", null);
}
time = Math.Round(time + timeStep, 12); // Round to 12 digits
}