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; * }*/ }