protected Position3D Localize2(int[] samplemaxes, ASIOLatencies lat)
{
Complex d1 = 0, d2 = 0, d3 = 0;
//Working vars
Position3D new_loc, loc = new Position3D();
int new_points = 0, points = 0;
double new_points_std = 0, points_std = 0;
for (int k = 0; k < 4; k++)
{
Complex C;
if (samplemaxes.Length >= 4)
{
if (k == 1)
{
d1 = samplemaxes[0] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[1] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[3] * c / ASIO.Fs; // X-direction
}
else if (k == 2)
{
d1 = samplemaxes[2] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[3] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[1] * c / ASIO.Fs; // X-direction
}
else if (k == 3)
{
d1 = samplemaxes[1] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[0] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[2] * c / ASIO.Fs; // X-direction
}
else if (k == 4)
{
d1 = samplemaxes[3] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[2] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[0] * c / ASIO.Fs; // X-direction
}
// !This is where the magic happens!: calculate the time difference between the transmission of the audio and the start time of the sampling (absolute time at sample index 0), this was found using the Matlab equation solver
C =
(
(fieldWidth * Complex.Pow(d1, 3) - Complex.Pow(fieldWidth, 3) * d1 + Complex.Pow(fieldWidth, 3) * d2 - fieldHeight * Complex.Pow(d1, 3) + 2 * d1 *
Complex.Sqrt(
((fieldWidth + d1 - d3) * (fieldWidth - d1 + d3) * (Complex.Pow(fieldWidth, 4) - 2 * Complex.Pow(fieldWidth, 3) * d1 + 2 * Complex.Pow(fieldWidth, 3) * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(fieldHeight, 2) + 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - 2 * Complex.Pow(fieldWidth, 2) * d1 * d3 - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) + 2 * Complex.Pow(fieldWidth, 2) * d2 * d3 - 2 * fieldWidth * fieldHeight * Complex.Pow(d1, 2) + 4 * fieldWidth * fieldHeight * d1 * d2 - 2 * fieldWidth * fieldHeight * Complex.Pow(d2, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d2 + 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + 4 * fieldWidth * d1 * Complex.Pow(d2, 2) - 4 * fieldWidth * d1 * d2 * d3 - 2 * fieldWidth * Complex.Pow(d2, 3) + 2 * fieldWidth * Complex.Pow(d2, 2) * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d1, 2) + 2 * Complex.Pow(fieldHeight, 2) * d1 * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d2 - 2 * fieldHeight * Complex.Pow(d1, 2) * d3 - 4 * fieldHeight * d1 * Complex.Pow(d2, 2) + 4 * fieldHeight * d1 * d2 * d3 + 2 * fieldHeight * Complex.Pow(d2, 3) - 2 * fieldHeight * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * d2 * d3 + Complex.Pow(d1, 2) * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 3) + 4 * d1 * Complex.Pow(d2, 2) * d3 - 2 * d1 * d2 * Complex.Pow(d3, 2) + Complex.Pow(d2, 4) - 2 * Complex.Pow(d2, 3) * d3 + Complex.Pow(d2, 2) * Complex.Pow(d3, 2))) / 4
)
-
2 * d2 *
Complex.Sqrt(
(
(fieldWidth + d1 - d3) * (fieldWidth - d1 + d3) * (Complex.Pow(fieldWidth, 4) - 2 * Complex.Pow(fieldWidth, 3) * d1 + 2 * Complex.Pow(fieldWidth, 3) * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(fieldHeight, 2) + 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - 2 * Complex.Pow(fieldWidth, 2) * d1 * d3 - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) + 2 * Complex.Pow(fieldWidth, 2) * d2 * d3 - 2 * fieldWidth * fieldHeight * Complex.Pow(d1, 2) + 4 * fieldWidth * fieldHeight * d1 * d2 - 2 * fieldWidth * fieldHeight * Complex.Pow(d2, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d2 + 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + 4 * fieldWidth * d1 * Complex.Pow(d2, 2) - 4 * fieldWidth * d1 * d2 * d3 - 2 * fieldWidth * Complex.Pow(d2, 3) + 2 * fieldWidth * Complex.Pow(d2, 2) * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d1, 2) + 2 * Complex.Pow(fieldHeight, 2) * d1 * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d2 - 2 * fieldHeight * Complex.Pow(d1, 2) * d3 - 4 * fieldHeight * d1 * Complex.Pow(d2, 2) + 4 * fieldHeight * d1 * d2 * d3 + 2 * fieldHeight * Complex.Pow(d2, 3) - 2 * fieldHeight * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * d2 * d3 + Complex.Pow(d1, 2) * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 3) + 4 * d1 * Complex.Pow(d2, 2) * d3 - 2 * d1 * d2 * Complex.Pow(d3, 2) + Complex.Pow(d2, 4) - 2 * Complex.Pow(d2, 3) * d3 + Complex.Pow(d2, 2) * Complex.Pow(d3, 2))
)
/
4
)
- Complex.Pow(d1, 3) * d2 + d1 * Complex.Pow(d3, 3) + Complex.Pow(d1, 3) * d3 - d2 * Complex.Pow(d3, 3) + Complex.Pow(fieldWidth, 4) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d3, 2) + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * Complex.Pow(d3, 2) + Complex.Pow(d2, 2) * Complex.Pow(d3, 2) + Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - Complex.Pow(fieldWidth, 2) * fieldHeight * d2 - fieldWidth * Complex.Pow(d1, 2) * d2 + Complex.Pow(fieldWidth, 2) * d1 * d2 + fieldWidth * d1 * Complex.Pow(d3, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + Complex.Pow(fieldWidth, 2) * d1 * d3 - fieldWidth * d2 * Complex.Pow(d3, 2) + Complex.Pow(fieldWidth, 2) * d2 * d3 + fieldHeight * Complex.Pow(d1, 2) * d2 - fieldHeight * d1 * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d3 + fieldHeight * d2 * Complex.Pow(d3, 2) + d1 * d2 * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * d2 * d3 + 2 * fieldWidth * d1 * d2 * d3 - 2 * fieldHeight * d1 * d2 * d3) / (Complex.Pow(fieldWidth, 2) - 2 * Complex.Pow(d1, 2) + 2 * d1 * d2 + 2 * d1 * d3 - Complex.Pow(d2, 2) - Complex.Pow(d3, 2)) - Complex.Pow(fieldWidth, 2) - Complex.Pow(d1, 2) + Complex.Pow(d2, 2)
)
/
(
2 * d1 - 2 * d2
);
}
else
{
C = 0;
}
//Make it a time.
C = C / c;
double[] t = new double[samplemaxes.Length];
for (int i = 0; i < t.Length; i++)
{
t[i] = lat.Input / ASIO.Fs + C.Real + samplemaxes[i] / ASIO.Fs;
}
Laterate(t, beaconHeight, c, out new_loc, out new_points, out new_points_std);
if (new_points > points || (new_points == points && new_points_std < points_std))
{
loc = new_loc;
points = new_points;
points_std = new_points_std;
}
}
return loc;
}
/// <summary>
///
/// </summary>
/// <param name="samplemaxes">Sample numbers of all microphones (one per microphone)</param>
/// <param name="lat">Latencies which are retrieved from the ASIO driver</param>
/// <returns></returns>
protected Position3D Localize(int[] samplemaxes, ASIOLatencies lat)
{
//Working vars
Position3D loc = new Position3D();
int nmics = Math.Min(Microphones.Count, asio.InChannels);
int row = 0;
if (nmics * (nmics - 1) < nmics + 1) {
Debug.WriteLine("Inconclusive.");
return null;
}
Matrix<double> a = DenseMatrix.Create(nmics * (nmics - 1), nmics + 1, 0);
Vector<double> b = DenseVector.Create(nmics * (nmics - 1), 0);
for (int i = 0; i < nmics; i++)
{
for (int j = 0; j < nmics; j++)
{
if (j <= i)
continue;
a[row, 0] = 2 * (Microphones[j].Position.X - Microphones[i].Position.X);
a[row, 1] = 2 * (Microphones[j].Position.Y - Microphones[i].Position.Y);
a[row, 1 + j] = -2 * (samplemaxes[i] - samplemaxes[j]) / ASIO.Fs * c;
b[row] = Math.Pow(((samplemaxes[i] - samplemaxes[j]) / ASIO.Fs * c), 2) - Math.Pow(Position3D.GetMagnitude(Microphones[i].Position), 2) + Math.Pow(Position3D.GetMagnitude(Microphones[j].Position), 2);
row++;
}
}
Vector<double> y = a.Solve(b);
loc = new Position3D() { X = y[0], Y = y[1], Z = 0 };
if (!Double.IsNaN(loc.X) && !Double.IsNaN(loc.Y))
{
return loc;
}
else
{
Debug.WriteLine("Inconclusive (NaN).");
return null;
}
}
