void Start()
 {
     r = this.GetComponent <Rigidbody>();
     previousRegion = spaceController.spacePolicy.GetPartition(this.transform.position);
     previousParams = spaceController.spacePolicy.GetTransformParams(this.transform.position);
     startMouseX    = previousMouseX = Input.mousePosition.x;
 }
    private void Update() // a little hack to assure smooth transitions between different euclidean partitions
    {
        int region = spaceController.spacePolicy.GetPartition(this.transform.position);

        if (region != previousRegion)
        {
            TransformParams currentParams = spaceController.spacePolicy.GetTransformParams(this.transform.position);
            float           currentZ      = this.transform.position.z;
            float           frontier      = currentParams.ZfrontierMax;
            float           d1            = Mathf.Abs(currentZ - currentParams.ZfrontierMax);
            float           d2            = Mathf.Abs(currentZ - currentParams.ZfrontierMin);
            if (d2 < d1)
            {
                frontier = currentParams.ZfrontierMin;
            }
            float currentRegionSegment = frontier - currentZ;
            float scaleFactor          = currentParams.speedScale.z / previousParams.speedScale.z;
            if (scaleFactor < 1.0f)
            {
                float   adjusterRegionSegment = currentRegionSegment * scaleFactor;
                Vector3 newPos = this.transform.position;
                newPos.z = frontier - adjusterRegionSegment;
                this.transform.position = newPos;
            }
            previousParams = currentParams;
            previousRegion = region;
            spaceController.TransformCamera();
        }
    }
    // Update is called once per frame
    void Update()
    {
        TransformParams parms           = spacePolicy.GetTransformParams(master.transform.position);
        TransformParams paramsWithSlack = spacePolicy.GetTransformParams(master.transform.position, true);

        slave.transform.position = parms.position;
        slave.transform.rotation = master.transform.rotation;
        invertStencil            = paramsWithSlack.invertStencil;
        partitionSpeed           = parms.speedScale;

        if (ForceStencil)
        {
            masterStencil.enabled = true;
            slaveStencil.enabled  = true;
        }
        else
        {
            masterStencil.enabled = invertStencil;
            slaveStencil.enabled  = invertStencil;
        }
        if (Input.GetKeyDown(KeyCode.S))
        {
            // switchCameras();
        }
        if (Debug_N != null)
        {
            int part = spacePolicy.GetPartition(master.transform.position);
            Debug_N.text = "" + part + "/" + master.transform.position.z + " / " + slave.transform.position.z;
        }
    }
 public static PosXYZ AffineInverseTransform(TransformParams trans, PosXYZ pos)
 {
     return(new PosXYZ(RigidAlign.AffineInverseTransform(pos.Data(), trans.ToDoubles()))
     {
         Z = pos.Z
     });
 }
Exemple #5
0
 public static void Draw(Image image, float x, float y, Graphics g, TransformParams t, BoundingArea.ReadOnly boundingArea)
 {
     //#if !NO_PARTIAL_RENDERING
     //            Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     //            Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     //            // TODO: throw other exceptions
     //            float width = t.Transform(image.Width);
     //            float height = t.Transform(image.Height);
     //            Vector2DF pos = t.Transform(new Vector2DF(x, y));
     //            BoundingArea inflatedArea = boundingArea.GetWritableCopy();
     //            inflatedArea.Inflate(5f, 5f);
     //            // *** the following code is using relatively slow GDI+ clipping :-/
     //            GraphicsPath gPath = new GraphicsPath();
     //            foreach (RectangleF rect in inflatedArea.Rectangles)
     //            {
     //                gPath.AddRectangle(rect);
     //            }
     //            g.SetClip(gPath, CombineMode.Union);
     //            PixelOffsetMode pixelOffsetMode = g.PixelOffsetMode;
     //            g.PixelOffsetMode = PixelOffsetMode.Half;
     //            g.DrawImage(image, pos.X, pos.Y, width, height);
     //            g.ResetClip();
     //            g.PixelOffsetMode = pixelOffsetMode;
     //#else
     Draw(image, x, y, g, t);
     //#endif
 }
    public void TransformCamera()
    {
        TransformParams parms = spacePolicy.GetTransformParams(master.transform.position);

        slave.transform.position = parms.position;
        slave.transform.rotation = master.transform.rotation;
    }
Exemple #7
0
        public override void DoCalib()
        {
            if (PlatformUp != null)
            {
                var isFirst = true;
                foreach (var pos in PlatformUpPos)
                {
                    if (isFirst)
                    {
                        isFirst = false;
                        PlatformUp.EnterAuto(this).Jump(pos, 0);
                    }
                    else
                    {
                        PlatformUp.EnterAuto(this).Jump(pos, PlatformUpJumpHeight);
                    }
                }
            }


            {
                var ret = XyzPlarformCalibration.CalcAffineTransform(ProductPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(), PlatformUpPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());

                Log($"计算产品坐标到上平台转换矩阵:\r\n"
                    + $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
                    + $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
                    + $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
                    + $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);

                Log($"计算误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);

                OutputTransform = new TransformParams(ret.Item1);
            }
        }
 public static PosXYZ AffineTransform(PosXYZ pos, TransformParams trans)
 {
     return(new PosXYZ(CoordAlignHelper.AffineTransform(pos.Data(), trans.ToDoubles()))
     {
         Z = pos.Z
     });
 }
Exemple #9
0
        public BitmapSource Transform(BitmapSource bitmapSource, TransformParams transformParams)
        {
            var firstBitmap  = new CroppedBitmap(bitmapSource, transformParams.FirstRectangle);
            var secondBitmap = new CroppedBitmap(bitmapSource, transformParams.SecondRectangle);


            var ss = _bitmapDiffService.Process(firstBitmap, secondBitmap);


            //using (FileStream mStream = new FileStream($@"E:\\tmp1.jpg", FileMode.Create))
            //{
            //    JpegBitmapEncoder jEncoder = new JpegBitmapEncoder();
            //    jEncoder.Frames.Add(BitmapFrame.Create(firstBitmap));
            //    jEncoder.Save(mStream);
            //}


            //using (FileStream mStream1 = new FileStream($@"E:\\tmp2.jpg", FileMode.Create))
            //{
            //    JpegBitmapEncoder jEncoder1 = new JpegBitmapEncoder();
            //    jEncoder1.Frames.Add(BitmapFrame.Create(secondBitmap));
            //    jEncoder1.Save(mStream1);
            //}

            return(null);
        }
Exemple #10
0
 public virtual BoundingArea GetBoundingArea(TransformParams tr)
 {
     Utils.ThrowException(tr == null ? new ArgumentNullException("tr") : null);
     if (mBoundingArea == null) { mBoundingArea = GetBoundingArea(); }
     BoundingArea boundingArea = mBoundingArea.Clone();
     boundingArea.Transform(tr);
     lock (mPen) { boundingArea.Inflate(mPen.Width / 2f + 5f, mPen.Width / 2f + 5f); }
     return boundingArea;
 }
    public override TransformParams Transform(Vector3 masterPos)
    {
        TransformParams res = new TransformParams();

        res.position      = masterPos + new Vector3(0, -33.32f, -20);
        res.speedScale    = Vector3.one;
        res.invertStencil = false;
        res.ZfrontierMin  = -1.0f;
        res.ZfrontierMax  = 0.0f;
        return(res);
    }
    public override TransformParams Transform(Vector3 masterPos)
    {
        TransformParams res          = new TransformParams();
        Vector3         intermediate = masterPos;

        intermediate.z   *= 21.0f;
        res.position      = intermediate + new Vector3(0, -33.32f, 0);
        res.speedScale    = new Vector3(1, 1, 1.0f / 14.0f); //0.142857f);
        res.invertStencil = true;
        res.ZfrontierMin  = -1.0f;
        res.ZfrontierMax  = 0.0f;
        return(res);
    }
Exemple #13
0
 public static void Draw(Image image, float x, float y, Graphics g, TransformParams t)
 {
     Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     // TODO: throw other exceptions
     float width = t.Transform(image.Width);
     float height = t.Transform(image.Height);
     Vector2DF pos = t.Transform(new Vector2DF(x, y));
     PixelOffsetMode pixelOffsetMode = g.PixelOffsetMode;
     g.PixelOffsetMode = PixelOffsetMode.Half;
     g.DrawImage(image, pos.X, pos.Y, width, height);
     g.PixelOffsetMode = pixelOffsetMode;
 }
Exemple #14
0
 // *** IDrawableObject interface implementation ***
 public virtual IDrawableObject[] GetObjectsAt(float x, float y, TransformParams tr, ref float[] distArray)
 {
     Utils.ThrowException(tr == null ? new ArgumentNullException("tr") : null);
     float dist = 0;
     IDrawableObject drawableObject = GetObjectAt(x, y, tr, ref dist);
     if (drawableObject != null)
     {
         distArray = new float[] { dist };
         return new IDrawableObject[] { drawableObject };
     }
     else
     {
         distArray = new float[] { };
         return new IDrawableObject[] { };
     }
 }
Exemple #15
0
 public override void Draw(Graphics g, TransformParams t, BoundingArea.ReadOnly boundingArea)
 {
     Draw(mImage, mX, mY, g, t, boundingArea); // throws ArgumentNullException
 }
Exemple #16
0
 // TODO: Image
 public override void Draw(Graphics g, TransformParams t)
 {
     Draw(mImage, mX, mY, g, t); // throws ArgumentNullException
 }
Exemple #17
0
 public static bool IsObjectAt(Image image, float x, float y, float ptX, float ptY, TransformParams t)
 {
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     // TODO: other exceptions
     float width = t.Transform(image.Width);
     float height = t.Transform(image.Height);
     Vector2DF pos = t.Transform(new Vector2DF(x, y));
     return VisualizationUtils.PointInsideRect(ptX, ptY, new RectangleF(pos.X, pos.Y, width, height));
 }
Exemple #18
0
 public virtual IDrawableObject[] GetObjectsIn(BoundingArea.ReadOnly area, TransformParams tr)
 {
     Utils.ThrowException(area == null ? new ArgumentNullException("area") : null);
     Utils.ThrowException(tr == null ? new ArgumentNullException("tr") : null);
     if (GetBoundingArea(tr).IntersectsWith(area))
     {
         return new IDrawableObject[] { this };
     }
     else
     {
         return new IDrawableObject[] { };
     }
 }
Exemple #19
0
 public abstract IDrawableObject GetObjectAt(float x, float y, TransformParams tr, ref float dist);
Exemple #20
0
 // *** The following functions need to be implemented in derived classes ***
 public abstract void Draw(Graphics gfx, TransformParams tr);
Exemple #21
0
 public static void Draw(float x1, float y1, float x2, float y2, Graphics g, Pen pen, TransformParams t, BoundingArea.ReadOnly boundingArea)
 {
     #if !NO_PARTIAL_RENDERING
     Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     Utils.ThrowException(pen == null ? new ArgumentNullException("pen") : null);
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     Utils.ThrowException(boundingArea == null ? new ArgumentNullException("boundingArea") : null);
     Vector2DF pt1 = t.Transform(new Vector2DF(x1, y1));
     Vector2DF pt2 = t.Transform(new Vector2DF(x2, y2));
     Vector2DF isectPt1 = new Vector2DF();
     Vector2DF isectPt2 = new Vector2DF();
     BoundingArea inflatedArea = boundingArea.GetWritableCopy();
     inflatedArea.Inflate(pen.Width / 2f + 5f, pen.Width / 2f + 5f);
     ArrayList<KeyDat<float, PointInfo>> points = new ArrayList<KeyDat<float, PointInfo>>();
     foreach (RectangleF rect in inflatedArea.Rectangles)
     {
         if (LineIntersectRectangle(pt1, pt2, rect, ref isectPt1, ref isectPt2))
         {
             float distPt1 = (pt1 - isectPt1).GetLength();
             float distPt2 = (pt1 - isectPt2).GetLength();
             bool startPt1 = distPt1 < distPt2;
             points.Add(new KeyDat<float, PointInfo>(distPt1, new PointInfo(isectPt1, startPt1)));
             points.Add(new KeyDat<float, PointInfo>(distPt2, new PointInfo(isectPt2, !startPt1)));
         }
     }
     points.Sort();
     int refCount = 0;
     int startIdx = 0;
     for (int i = 0; i < points.Count; i++)
     {
         PointInfo pointInfo = points[i].Dat;
         if (pointInfo.IsStartPoint)
         {
             refCount++;
         }
         else
         {
             refCount--;
             if (refCount == 0)
             {
                 g.DrawLine(pen, points[startIdx].Dat.Point, pointInfo.Point);
                 startIdx = i + 1;
             }
         }
     }
     #else
     Draw(x1, y1, x2, y2, g, pen, t);
     #endif
 }
Exemple #22
0
 public override void Draw(Graphics gfx, TransformParams tr)
 {
     Draw(mX1, mY1, mX2, mY2, gfx, mPen, tr); // throws ArgumentNullException, ArgumentValueException
 }
Exemple #23
0
 public virtual void Draw(Graphics gfx, TransformParams tr, BoundingArea.ReadOnly boundingArea)
 {
     Draw(gfx, tr);
 }
Exemple #24
0
 public override void Draw(Graphics g, TransformParams t)
 {
     Draw(mX, mY, mRX, mRY, g, mPen, mBrush, t); // throws ArgumentNullException
 }
Exemple #25
0
 public override IDrawableObject GetObjectAt(float x, float y, TransformParams t, ref float dist)
 {
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     float width = t.Transform(mWidth);
     float height = t.Transform(mHeight);
     Vector2DF pos = t.Transform(new Vector2DF(mX - mWidth / 2f, mY - mHeight / 2f));
     return VisualizationUtils.PointInsideRect(x, y, new RectangleF(pos.X, pos.Y, width, height)) ? this : null;
 }
Exemple #26
0
 public static void Draw(float x, float y, float rX, float rY, Graphics g, Pen pen, Brush brush, TransformParams t)
 {
     Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     Utils.ThrowException(pen == null ? new ArgumentNullException("pen") : null);
     Utils.ThrowException(brush == null ? new ArgumentNullException("brush") : null);
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     x -= rX;
     y -= rY;
     Vector2DF center = t.Transform(new Vector2DF(x, y));
     float dX = t.Transform(2f * rX);
     float dY = t.Transform(2f * rY);
     g.FillEllipse(brush, center.X, center.Y, dX, dY);
     g.DrawEllipse(pen, center.X, center.Y, dX, dY);
 }
Exemple #27
0
 public override IDrawableObject GetObjectAt(float x, float y, TransformParams t, ref float dist)
 {
     dist = 0;
     return IsObjectAt(mImage, mX, mY, x, y, t) ? this : null; // throws ArgumentNullException
 }
Exemple #28
0
 public static bool IsObjectAt(float ptX, float ptY, TransformParams t, float cX, float cY, float rX, float rY)
 {
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     Vector2DF center = t.Transform(new Vector2DF(cX, cY));
     Vector2DF pt = new Vector2DF(ptX, ptY);
     if (pt == center) { return true; }
     float angle = (pt - center).GetAngle();
     float x = (float)Math.Cos(angle) * t.Transform(rX);
     float y = (float)Math.Sin(angle) * t.Transform(rY);
     float r = new Vector2DF(x, y).GetLength();
     return (center - pt).GetLength() <= r;
 }
Exemple #29
0
 public static bool IsObjectAt(float ptX, float ptY, TransformParams t, float x1, float y1, float x2, float y2, ref float dist)
 {
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     Vector2DF pt1 = t.Transform(new Vector2DF(x1, y1));
     Vector2DF pt2 = t.Transform(new Vector2DF(x2, y2));
     return VisualizationUtils.TestLineHit(new Vector2DF(ptX, ptY), pt1, pt2, mHitDist, ref dist);
 }
Exemple #30
0
 public static void Draw(float x1, float y1, float x2, float y2, Graphics g, Pen pen, TransformParams t)
 {
     Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     Utils.ThrowException(pen == null ? new ArgumentNullException("pen") : null);
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     Vector2DF pt1 = t.Transform(new Vector2DF(x1, y1));
     Vector2DF pt2 = t.Transform(new Vector2DF(x2, y2));
     g.DrawLine(pen, pt1, pt2);
 }
Exemple #31
0
 public override void Draw(Graphics g, TransformParams t)
 {
     Utils.ThrowException(g == null ? new ArgumentNullException("g") : null);
     Utils.ThrowException(t == null ? new ArgumentNullException("t") : null);
     Vector2DF pos = t.Transform(new Vector2DF(mX - mWidth / 2f, mY - mHeight / 2f));
     using (Font font = new Font(mFont.FontFamily, t.Transform(mFont.Size), mFont.Style))
     {
         g.DrawString(mLabel, font, mBrush, pos.X, pos.Y, StringFormat.GenericTypographic);
     }
 }
Exemple #32
0
 public override void Draw(Graphics g, TransformParams t)
 {
     Draw(mX1, mY1, mX2, mY2, g, mPen, t); // throws ArgumentNullException
 }
Exemple #33
0
 public override void Draw(Graphics g, TransformParams t, BoundingArea.ReadOnly boundingArea)
 {
     Draw(mX1, mY1, mX2, mY2, g, mPen, t, boundingArea); // throws ArgumentNullException
 }
Exemple #34
0
 public override IDrawableObject GetObjectAt(float x, float y, TransformParams t, ref float dist)
 {
     return IsObjectAt(x, y, t, mX1, mY1, mX2, mY2, ref dist) ? this : null; // throws ArgumentNullException
 }
Exemple #35
0
        public override void DoCalib()
        {
            Platform1?.EnterAuto(this).MoveAbs("Wait");


            //上工站点位数据采集
            Log($"上平台Align点位标定\n------------------------------------------------------");
            List <PosXYZ> upAlignPos = new List <PosXYZ>();
            bool          isFirst    = true;

            foreach (var pos in AlignPosUp)
            {
                if (isFirst)
                {
                    isFirst = false;
                    Platform1?.EnterAuto(this).Jump(pos, 0);
                }
                else
                {
                    Platform1?.EnterAuto(this).Jump(pos, JumpHeight1);
                }

                upAlignPos.Add(pos);
                DataList.Add(pos.ToString());
                Log($"CurPos {pos.ToString()}", LogLevel.Info);
            }
            //复位平台1
            Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
            Log($"上平台Align点位标定 完成\n------------------------------------------------------");
            OnCalibProgress(50);



            Platform2?.EnterAuto(this).MoveAbs("Wait");
            //下工站点位 数据采集
            Log($"下平台Align点位标定\n------------------------------------------------------");
            List <PosXYZ> downAlingPos = new List <PosXYZ>();

            isFirst = true;
            foreach (var pos in AlignPosDown)
            {
                if (isFirst)
                {
                    isFirst = false;
                    Platform2?.EnterAuto(this).Jump(pos, 0);
                }
                else
                {
                    Platform2?.EnterAuto(this).Jump(pos, JumpHeight2);
                }

                downAlingPos.Add(pos);
                DataList.Add(pos.ToString());
                Log($"CurPos {pos.ToString()}", LogLevel.Info);
            }

            Log($"下平台Align点位标定 完成\n------------------------------------------------------");


            //计算下平台GT2偏移
            Log($"下平台GT1GT2偏移标定\n------------------------------------------------------");
            if (Platform2GtOffsetCalibGT1 != null && Platform2GtOffsetCalibGT2 != null)
            {
                Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT1, JumpHeight1);
                Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT2, JumpHeight1);

                var gt1 = Platform2GtOffsetCalibGT1 as PosXYZ;
                var gt2 = Platform2GtOffsetCalibGT2 as PosXYZ;
                OutputPlatform2GtOffset   = gt2 - gt1;
                OutputPlatform2GtOffset.Z = 0;
            }


            //复位平台2
            Platform2?.EnterAuto(this).Jump("Wait", JumpHeight2);
            Log($"下平台GT1GT2偏移标定 完成\n------------------------------------------------------");


            //复位治具
            PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");
            OnCalibProgress(100);


            //计算上下平台坐标转换
            {
                var ret = XyzPlarformCalibration.CalcAffineTransform(
                    upAlignPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(),
                    downAlingPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());

                Log($"计算上下平台转换矩阵:\r\n"
                    + $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
                    + $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
                    + $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
                    + $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);

                Log($"计算上下平台转换误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);

                OutputTransForm = new TransformParams(ret.Item1);
            }

            Log($"GT1GT2偏移:\r\n{OutputPlatform2GtOffset}\r\n", LogLevel.Info);
        }
Exemple #36
0
 public static void Draw(float x1, float y1, float x2, float y2, Graphics gfx, Pen pen, TransformParams tr)
 {
     Utils.ThrowException(gfx == null ? new ArgumentNullException("gfx") : null);
     Utils.ThrowException(pen == null ? new ArgumentNullException("pen") : null);
     Utils.ThrowException(tr.NotSet ? new ArgumentValueException("tr") : null);
     VectorF pt1 = tr.Transform(new VectorF(x1, y1));
     VectorF pt2 = tr.Transform(new VectorF(x2, y2));
     gfx.DrawLine(pen, pt1, pt2);
 }