public bool CalTargetByMatrix(out Locate3D position) { double x, y = 0; Matrix m = new Matrix(); Input i1 = new Input() { x = x1, y = y1, z = z1, r = range1, }; Input i2 = new Input() { x = x2, y = y2, z = z2, r = range2, }; Input i3 = new Input() { x = x3, y = y3, z = z3, r = range3, }; double[] D = new double[3]; MatrixLocate.InitMatrix(ref m, i1, i2, i3, ref D); position = new Locate3D(DateTime.UtcNow); if (MatrixLocate.locate(m, D, out x, out y) != 1) { return(false); } var diff = range1 * range1 - (y - y1) * (y - y1) - (x - x1) * (x - x1); if (diff < 0) { return(false); } double z = Math.Sqrt(diff); position = new Locate3D(DateTime.UtcNow, x, y, z); return(true); }
public void CalTargetLocationTest() { double x, y = 0; if (MatrixLocate.locate(m, D, out x, out y) == 1) { Assert.Warn("x:{0:F06},y:{1:F06}", x, y); if (Math.Abs(x) > range1 || Math.Abs(y) > range1) { Assert.Fail("can not locate object"); } double z = Math.Sqrt(range1 * range1 - (y - y1) * (y - y1) - (x - x1) * (x - x1)); string log = "x:" + x.ToString("F06") + ",y:" + y.ToString("F06") + ",z:" + z.ToString("F06"); Assert.Warn(log); Assert.Pass("locate passed!"); } Assert.Fail(); }