public void Inverse2()
{
var expected = new double[, ] {
{ 227560.34109131934, 225945.06485176072, 90981.3565338888, 1783.94909384747, -0.4730581622363843, -4458.690089213085, -720000, 0, 0, 0, 0, 0, -9938.65105968789, 0, 0, 0, 0, -24846.627649219725, -227560.34109131934, -225945.06485176072, -90981.3565338888, 1232.703872075042, 214.37257135695188, -3617.6911085799843 },
{ 225945.06485176072, 227957.93288043974, 89987.37706108784, -3408.805715052969, 1155.0843313380512, 5634.303459287295, 0, -6834.2300987018625, -1991.098516469141, 0, -5702.283133971421, 18639.349520300333, 0, -720000, 0, 0, 0, 0, -225945.06485176072, -227957.93288043974, -89987.37706108784, -2570.17474038732, 798.1600667442415, 4430.0366841076975 },
{ 90981.35653388879, 89987.37706108783, 37476.11456919402, 4062.141553013745, -2886.528182939537, -2939.033425185521, 0, -1991.0985164691415, -3492.770037088091, 0, -9069.782171132645, 5702.283133971421, 0, 0, -2564.305550717474, 6410.763876793685, 0, 0, -90981.35653388879, -89987.37706108783, -37476.11456919402, 3343.6771707806965, -2531.3315952529833, -2030.863938819283 },
{ 1783.9490938474705, -3408.8057150529685, 4062.1415530137456, 19903.70428775242, -12022.805603202049, -19702.246711375938, 0, 0, 0, -6030.719999999999, 0, 0, 0, 0, -6410.763876793685, 10626.909691984212, 0, 0, -1783.9490938474705, 3408.8057150529685, -4062.1415530137456, 7224.614907422239, -4720.003974171739, -6261.527333126251 },
{ -0.4730581622363843, 1155.084331338051, -2886.528182939537, -12022.805603202049, 10089.211088587075, 4833.986286537434, 0, 4253.209448374284, 8394.06801430781, 0, 13830.102768176941, -6869.2697196236095, 0, 0, 0, 0, -6030.719999999999, 0, 0.4730581622363843, -1155.084331338051, 2886.528182939537, -4720.003974171739, 4342.48370978614, 943.8006609639997 },
{ -4458.690089213086, 5634.303459287295, -2939.033425185521, -19702.246711375938, 4833.986286537434, 37170.651062096265, 0, -15531.800973208978, -4253.209448374284, 0, -6869.2697196236095, 25358.106144563797, 24846.627649219725, 0, 0, 0, 0, 40516.56912304932, 4458.690089213086, -5634.303459287295, 2939.033425185521, -6261.5273331262515, 943.8006609639997, 13294.31668040563 },
{ -720000, 0, 0, 0, 0, 0, 1774.0574303131118, -218.1165710996554, 829.6409745079811, 2082.5543064828144, 2082.5543064828144, -3869.149378639904, -1774.0574303131098, 218.11657109965347, -829.6409745079811, 2065.6505660570933, 2065.6505660570933, -3910.5549174273774, -2055.5275225379605, 0, 0, 0, 4111.05504507592, -10277.637612689801 },
{ 0, -6834.2300987018625, -1991.098516469141, 0, 4253.209448374284, -15531.800973208978, -218.1165710996554, 1774.0574303131118, 829.6409745079811, 2082.5543064828144, 2082.5543064828144, -3869.149378639904, 218.11657109965347, -1774.0574303131098, -829.6409745079811, 2065.6505660570933, 2065.6505660570933, -3910.5549174273774, 0, -576566.19095693, -229842.56620114218, -1927.5609368718526, 0, 0 },
{ 0, -1991.0985164691415, -3492.770037088091, 0, 8394.06801430781, -4253.209448374284, 829.6409745079811, 829.6409745079811, 2910.7388973800817, 7269.946320318962, 7269.946320318962, -4165.108612965627, -829.6409745079811, -829.6409745079811, -2910.7388973800817, 7283.748166581453, 7283.748166581453, -4131.301132114185, 0, -229842.56620114218, -93896.80193453135, 4818.902342179632, 0, 0 },
{ 0, 0, 0, -6030.719999999999, 0, 0, 2082.5543064828144, 2082.5543064828144, 7269.946320318962, 26488.875178226604, 22224.51217078927, -13976.596285852873, -2082.5543064828144, -2082.5543064828144, -7269.946320318962, 9860.85642336822, 14125.219430805551, -6848.946778975265, 0, 1991.9899712771255, -4979.974928192814, 9273.174486477054, 0, 0 },
{ 0, -5702.283133971421, -9069.782171132645, 0, 13830.102768176941, -6869.2697196236095, 2082.5543064828144, 2082.5543064828144, 7269.946320318962, 22224.51217078927, 26488.875178226604, -13976.596285852873, -2082.5543064828144, -2082.5543064828144, -7269.946320318962, 14125.219430805551, 9860.85642336822, -6848.946778975265, 0, 0, 0, 0, -4827.0542665996145, -1930.8217066398454 },
{ 0, 18639.349520300333, 5702.283133971421, 0, -6869.2697196236095, 25358.106144563797, -3869.149378639904, -3869.149378639904, -4165.108612965627, -13976.596285852873, -13976.596285852873, 25889.701188203806, 3869.149378639904, 3869.149378639904, 4165.108612965627, -6848.946778975265, -6848.946778975265, 12801.792598195232, 0, 0, 0, 0, -1930.8217066398454, -772.328682655938 },
{ -9938.65105968789, 0, 0, 0, 0, 24846.627649219725, -1774.0574303131098, 218.11657109965347, -829.6409745079811, -2082.5543064828144, -2082.5543064828144, 3869.149378639904, 1774.0574303131098, -218.11657109965347, 829.6409745079811, -2065.6505660570933, -2065.6505660570933, 3910.5549174273774, -576725.8595645671, 845.7065865417906, -229443.39468204963, 845.7065865417906, 3117.372859442855, -2114.266466354477 },
{ 0, -720000, 0, 0, 0, 0, 218.11657109965347, -1774.0574303131098, -829.6409745079811, -2082.5543064828144, -2082.5543064828144, 3869.149378639904, -218.11657109965347, 1774.0574303131098, 829.6409745079811, -2065.6505660570933, -2065.6505660570933, 3910.5549174273774, 845.7065865417908, -6938.85858879817, -2114.266466354477, -6938.858588798169, -6131.372752427983, 17347.146471995424 },
{ 0, 0, -2564.305550717474, -6410.763876793685, 0, 0, -829.6409745079811, -829.6409745079811, -2910.7388973800817, -7269.946320318962, -7269.946320318962, 4165.108612965627, 829.6409745079811, 829.6409745079811, 2910.7388973800817, -7283.748166581453, -7283.748166581453, 4131.301132114185, -229443.39468204963, -2114.266466354477, -94894.7307322627, -2114.2664663544765, -7793.432148607137, 5285.666165886192 },
{ 0, 0, 6410.763876793685, 10626.909691984212, 0, 0, 2065.6505660570933, 2065.6505660570933, 7283.748166581453, 9860.85642336822, 14125.219430805551, -6848.946778975265, -2065.6505660570933, -2065.6505660570933, -7283.748166581453, 26557.88440953906, 22293.521402101724, -13807.558881595662, -845.7065865417906, 6938.858588798169, 2114.2664663544765, 4543.18642872923, 3991.731376819802, -11357.966071823077 },
{ 0, 0, 0, 0, -6030.719999999999, 0, 2065.6505660570933, 2065.6505660570933, 7283.748166581453, 14125.219430805551, 9860.85642336822, -6848.946778975265, -2065.6505660570933, -2065.6505660570933, -7283.748166581453, 22293.521402101724, 26557.88440953906, -13807.558881595662, -3117.372859442855, 6131.372752427981, 7793.432148607137, 3991.731376819802, 14900.705737558506, -9979.328442049506 },
{ -24846.627649219725, 0, 0, 0, 0, 40516.56912304932, -3910.5549174273774, -3910.5549174273774, -4131.301132114185, -6848.946778975265, -6848.946778975265, 12801.792598195232, 3910.5549174273774, 3910.5549174273774, 4131.301132114185, -13807.558881595662, -13807.558881595662, 26303.756576078544, 2114.266466354477, -17347.146471995424, -5285.666165886192, -11357.966071823075, -9979.328442049506, 28394.91517955769 },
{ -227560.34109131934, -225945.06485176072, -90981.3565338888, -1783.94909384747, 0.4730581622363843, 4458.690089213085, -2055.5275225379605, 0, 0, 0, 0, 0, -576725.8595645671, 845.7065865417906, -229443.39468204963, -845.7065865417906, -3117.372859442855, 2114.266466354477, 227560.34109131934, 225945.06485176072, 90981.3565338888, -1232.703872075042, -214.37257135695188, 3617.6911085799843 },
{ -225945.06485176072, -227957.93288043974, -89987.37706108784, 3408.805715052969, -1155.0843313380512, -5634.303459287295, 0, -576566.19095693, -229842.56620114218, 1991.9899712771255, 0, 0, 845.7065865417908, -6938.85858879817, -2114.266466354477, 6938.858588798169, 6131.372752427983, -17347.146471995424, 225945.06485176072, 227957.93288043974, 89987.37706108784, 2570.17474038732, -798.1600667442415, -4430.0366841076975 },
{ -90981.35653388879, -89987.37706108783, -37476.11456919402, -4062.141553013745, 2886.528182939537, 2939.033425185521, 0, -229842.56620114218, -93896.80193453135, -4979.974928192814, 0, 0, -229443.39468204963, -2114.266466354477, -94894.7307322627, 2114.2664663544765, 7793.432148607137, -5285.666165886192, 90981.35653388879, 89987.37706108783, 37476.11456919402, -3343.6771707806965, 2531.3315952529833, 2030.863938819283 },
{ 1232.7038720750415, -2570.17474038732, 3343.6771707806965, 7224.614907422239, -4720.003974171739, -6261.527333126251, 0, -1927.5609368718526, 4818.902342179632, 9273.174486477054, 0, 0, 845.7065865417906, -6938.858588798169, -2114.2664663544765, 4543.18642872923, 3991.731376819802, -11357.966071823077, -1232.7038720750415, 2570.17474038732, -3343.6771707806965, 14634.120427255879, -9532.97413558166, -12752.865729185549 },
{ 214.37257135695188, 798.1600667442415, -2531.3315952529833, -4720.003974171739, 4342.48370978614, 943.8006609639997, 4111.05504507592, 0, 0, 0, -4827.0542665996145, -1930.8217066398454, 3117.372859442855, -6131.372752427981, -7793.432148607137, 3991.731376819802, 14900.705737558506, -9979.328442049506, -214.37257135695188, -798.1600667442415, 2531.3315952529833, -9532.974135581662, 8742.919409192684, 1975.1368159724425 },
{ -3617.6911085799843, 4430.0366841076975, -2030.863938819283, -6261.5273331262515, 943.8006609639997, 13294.31668040563, -10277.637612689801, 0, 0, 0, -1930.8217066398454, -772.328682655938, -2114.266466354477, 17347.146471995424, 5285.666165886192, -11357.966071823075, -9979.328442049506, 28394.91517955769, 3617.6911085799843, -4430.0366841076975, 2030.863938819283, -12752.86572918555, 1975.1368159724434, 26944.322283032772 },
};
var newm = new LWMatrix(expected);
var _ = newm.Inverse();
}
/// <summary>Converts from polar to world coordinates.</summary>
/// <returns>Point in world space.</returns>
/// <param name="polarPoint">Point in polar space.</param>
///
/// Converts a point from polar space to world space.
public Vector3 PolarToWorld(Vector3 polarPoint)
{
var vec =
polarPoint[idx[0]] * Mathf.Cos(Float2Rad(polarPoint[idx[1]])) * Units[idx[0]]
+ polarPoint[idx[0]] * Mathf.Sin(Float2Rad(polarPoint[idx[1]])) * Units[idx[1]]
+ polarPoint[idx[2]] * Units[idx[2]];
return(LWMatrix.MultiplyPoint3x4(vec));
}
public void InverseTest()
{
var matrix = new LWMatrix(new[, ] {
{ 1.0 / 2, -2, -30, 20 },
{ 15, 2.1, 4, 3 },
{ 3, 2, 1, 3 },
{ 4, -4, 3, 2 }
});
var determActual = matrix.Inverse();
var determinantExpected = new LWMatrix(new[, ] {
{ 0.0068357113280509, 0.097412395516784, -0.13109967926899, -0.017826187652207 }, { -0.0092640030318555, 0.01172036747212, 0.15713714233579, -0.16064623439331 }, { -0.027034045211142, -0.05670692764954, 0.18582747899808, 0.076659625088605 }, { 0.0083516390969001, -0.086323664615018, 0.29773242471243, 0.099370468884881 }
});
Assert.AreEqual(determinantExpected, determActual);
}
/// <summary>Converts from polar to world coordinates.</summary>
/// <returns>Point in world space.</returns>
/// <param name="polarPoint">Point in polar space.</param>
///
/// Converts a point from polar space to world space.
public Vector3 PolarToWorld(Vector3 polarPoint)
{
var vec =
polarPoint[idx[0]] * Mathf.Cos(Float2Rad(polarPoint[idx[1]])) * Units[idx[0]]
+ polarPoint[idx[0]] * Mathf.Sin(Float2Rad(polarPoint[idx[1]])) * Units[idx[1]]
+ polarPoint[idx[2]] * Units[idx[2]];
return(LWMatrix.MultiplyPoint3x4(vec));
//return _Transform.GFTransformPointFixed(
// polarPoint[idx[0]] * Mathf.Cos(Float2Rad(polarPoint[idx[1]])) * Units[idx[0]]
// + polarPoint[idx[0]] * Mathf.Sin(Float2Rad(polarPoint[idx[1]])) * Units[idx[1]]
// + polarPoint[idx[2]] * Units[idx[2]])
// + _Transform.TransformDirection(originOffset);
}
protected override void DrawPointsCalculate(Vector3[][][] points, ref int[] amount, Vector3 from, Vector3 to)
{
float r, phi, z, delta_phi = angleDeg / smoothness;
Vector3 origin = LWMatrix.MultiplyPoint3x4(Vector3.zero);
/* How to fill the arc segment line array: Each segment consists of two
* points, the first point of a segment is the second point of the
* previous segment and the second point of a segment is the first
* point of the next segment. The very first point will be the first
* point of the very first segment (segment 0). For every other segment
* (segment i) add only its first point as its first point and as the
* second point of the previous segment (segment i-1). The very last
* point will be added as the second point of the very last segment
* (segment n-1).
*/
// segment lines
z = Mathf.Ceil(from[idx[2]]) * depth;
for (int i = 0; i < layer_count; ++i) // for every layer
{
r = Mathf.Ceil(from[idx[0]]) * radius;
for (int j = 0; j < arc_count; ++j) // for every circle/arc
// the first point is an exception
{
points[idx[0]][i * arc_count * segment_count + j * segment_count + 0][0] = ContributePoint(r, from[idx[1]] * angleDeg, z, origin);
phi = Mathf.Ceil(from[idx[1]] * smoothness) / smoothness * angleDeg;
for (int k = 1; k < segment_count; ++k) // for every segment
{
Vector3 point = ContributePoint(r, phi, z, origin);
points[idx[0]][i * arc_count * segment_count + j * segment_count + k - 1][1] = point;
points[idx[0]][i * arc_count * segment_count + j * segment_count + k][0] = point;
phi += delta_phi;
}
// the last point is an exception as well
points[idx[0]][i * arc_count * segment_count + j * segment_count + segment_count - 1][1] = ContributePoint(r, to[idx[1]] * angleDeg, z, origin);
r += radius;
}
z += depth;
}
// radial lines
z = Mathf.Ceil(from[idx[2]]) * depth;
for (int i = 0; i < layer_count; ++i)
{
phi = Mathf.Ceil(from[idx[1]]) * angleDeg;
for (int j = 0; j < sector_count; ++j)
{
points[idx[1]][i * sector_count + j][0] = ContributePoint(from[idx[0]] * radius, phi, z, origin);
points[idx[1]][i * sector_count + j][1] = ContributePoint(to[idx[0]] * radius, phi, z, origin);
phi += angleDeg;
}
z += depth;
}
// cylindric lines
r = Mathf.Ceil(from[idx[0]]) * radius;
from[idx[2]] *= depth; // the start of the cylindric lines is affected by depth for relative-sized grids
to [idx[2]] *= depth; // we could do this in the loop when contributing the point, but that would be linear complexity
for (int i = 0; i < arc_count; ++i)
{
phi = Mathf.Ceil(from[idx[1]]) * angleDeg;
for (int j = 0; j < sector_count; ++j)
{
points[idx[2]][i * sector_count + j][0] = ContributePoint(r, phi, from[idx[2]], origin);
points[idx[2]][i * sector_count + j][1] = ContributePoint(r, phi, to[idx[2]], origin);
phi += angleDeg;
}
r += radius;
}
from[idx[2]] /= depth; // undo the change from above
to [idx[2]] /= depth;
/* TO DO: performance can be improved by reducing the amount of
* rotations computed. To this end invert the loop, i.e. iterate over
* the angles first, inside those iterate over the arcs and inside
* those over the layers. Compute the rotation once per angle and use
* that for drawing radial lines, segments and layer lines. PROBLEM:
* handling over- and underflow in a loop without repeating; in the
* current implementation we can make the two exceptions without
* repeating code, because they are in the inner-most loop.
*/
/*Quaternion rotation;
* int i = 0, j = 0, k = 0;
* phi = from[idx[2]] * angleDeg;
* r = Mathf.CeilToInt(from[idx[0]]);
* z = Mathf.CeilToInt(from[idx[2]]);
* while (r < to[idx[0]]) {
* // draw layer line here
* while (z < to[idx[2]]) {
* // draw segment here
++k;
* }
++j;
* }*/
}
