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
});
}
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;
}
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
});
}
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);
}
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);
}
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;
}
// *** 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[] { };
}
}
public override void Draw(Graphics g, TransformParams t, BoundingArea.ReadOnly boundingArea)
{
Draw(mImage, mX, mY, g, t, boundingArea); // throws ArgumentNullException
}
// TODO: Image
public override void Draw(Graphics g, TransformParams t)
{
Draw(mImage, mX, mY, g, t); // throws ArgumentNullException
}
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));
}
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[] { };
}
}
public abstract IDrawableObject GetObjectAt(float x, float y, TransformParams tr, ref float dist);
// *** The following functions need to be implemented in derived classes *** public abstract void Draw(Graphics gfx, TransformParams tr);
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
}
public override void Draw(Graphics gfx, TransformParams tr)
{
Draw(mX1, mY1, mX2, mY2, gfx, mPen, tr); // throws ArgumentNullException, ArgumentValueException
}
public virtual void Draw(Graphics gfx, TransformParams tr, BoundingArea.ReadOnly boundingArea)
{
Draw(gfx, tr);
}
public override void Draw(Graphics g, TransformParams t)
{
Draw(mX, mY, mRX, mRY, g, mPen, mBrush, t); // throws ArgumentNullException
}
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;
}
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);
}
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
}
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;
}
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);
}
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);
}
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);
}
}
public override void Draw(Graphics g, TransformParams t)
{
Draw(mX1, mY1, mX2, mY2, g, mPen, t); // throws ArgumentNullException
}
public override void Draw(Graphics g, TransformParams t, BoundingArea.ReadOnly boundingArea)
{
Draw(mX1, mY1, mX2, mY2, g, mPen, t, boundingArea); // throws ArgumentNullException
}
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
}
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);
}
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);
}