/// <summary>
/// Returns the yaw angle to the given point
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public Radians YawTo(G3DPoint pt)
{
double diffX = pt.X - X;
double diffY = pt.Y - Y;
return(Math.Atan2(diffY, diffX));
}
/// <summary>
/// Returns the XY distance to the given point
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public Millimeters XYDistanceTo(G3DPoint pt)
{
double diffX = pt.X - X;
double diffY = pt.Y - Y;
return(Math.Sqrt(diffX * diffX + diffY * diffY));
}
/// <summary>
/// Return the final robot commaded position
/// </summary>
public override G3DPoint GetRobotLoc(bool applyToolOffset, bool applyTrajLead)
{
G3DPoint pt = base.GetRobotLoc(applyToolOffset, applyTrajLead);
pt.Z.Val += ZOffset;
return(pt);
}
/// <summary>
/// Addition
/// </summary>
/// <param name="val1"></param>
/// <param name="val2"></param>
/// <returns></returns>
public static G3DPoint operator +(G3DPoint val1, G3DPoint val2)
{
G3DPoint newVal = new G3DPoint();
newVal.X = val1.X + val2.X;
newVal.Y = val1.Y + val2.Y;
newVal.Z = val1.Z + val2.Z;
return(newVal);
}
/// <summary>
/// Copy Constructor 1
/// </summary>
/// <param name="val"></param>
public G3DPoint(G3DPoint val)
{
if (val != null)
{
X.Val = val.X.Val;
Y.Val = val.Y.Val;
Z.Val = val.Z.Val;
Yaw.Val = val.Yaw.Val;
XYYawMode = val.XYYawMode;
ZMode = val.ZMode;
}
}
/// <summary>
/// Snap the line begin and endpoints to the grid
/// </summary>
/// <param name="gridSize"></param>
public void Snap(Millimeters gridSize)
{
G3DPoint ptEnd = ExtendPoint(EndPoint.Distance);
Loc.X = U.Snap(Loc.X, gridSize);
Loc.Y = U.Snap(Loc.Y, gridSize);
ptEnd.X = U.Snap(ptEnd.X, gridSize);
ptEnd.Y = U.Snap(ptEnd.Y, gridSize);
PointF ptLoc = Loc.ToPointF;
PointF ptfEnd = ptEnd.ToPointF;
PointF ptDiff = U.Subtract(ptfEnd, ptLoc);
EndPoint.Distance = U.GetDistance(ptDiff);
Loc.Yaw = U.GetAngle(ptDiff);
}
/// <summary>
/// Get extended point in XY
/// </summary>
/// <param name="distance"></param>
/// <returns></returns>
public G3DPoint ExtendPoint(Millimeters distance)
{
G3DPoint pt = new G3DPoint(Loc);
//double cos = Math.Cos(Loc.Yaw);
//double sin = Math.Sin(Loc.Yaw);
//double cosx = distance * cos;
//double siny = distance * sin;
//double x = pt.X + cosx;
//double y = pt.Y + siny;
//pt.X = x;
//pt.Y = y;
pt.X = pt.X + distance * Math.Cos(Loc.Yaw);
pt.Y = pt.Y + distance * Math.Sin(Loc.Yaw);
return(pt);
}
/// <summary>
/// Get the absolute robot coordinates (Used for Line elements: Trigger)
/// </summary>
/// <param name="distance">Distance along the Loc vector (angle of Yaw) </param>
/// <param name="applyToolOffset"> Ad the too offset to adjust for tool moounting position</param>
/// <returns></returns>
public G3DPoint GetRobotLoc(Millimeters distance, bool applyToolOffset)
{
if (Loc.XYYawMode == G3DPoint.eMode.Absolute)
{
double x = distance * Math.Cos(Loc.Yaw);
double y = distance * Math.Sin(Loc.Yaw);
if (Loc.ZMode == G3DPoint.eMode.Absolute)
{
return(new G3DPoint(Loc.X + x, Loc.Y + y, Loc.Z));
}
else
{
return(new G3DPoint(Loc.X + x, Loc.Y + y, 0));
}
}
G3DPoint pt = TransformXY(distance, 0);
if (Loc.ZMode == G3DPoint.eMode.Absolute)
{
pt.Z.Val = Loc.Z;
}
else
{
pt.Z.Val = TransformZ(0);
}
if (applyToolOffset && ToolComp != null)
{
if (_offsetComp == null && _offsetCompCheck == false)
{
_offsetComp = ToolComp.FilterByTypeSingle <G3DCompBase>();
_offsetCompCheck = true;
}
if (_offsetComp != null)
{
pt.X += _offsetComp.Loc.X;
pt.Y += _offsetComp.Loc.Y;
if (Loc.ZMode == G3DPoint.eMode.Relative)
{
pt.Z += _offsetComp.Loc.Z;
}
}
}
return(pt);
}
/// <summary>
/// Transform a point (x,y) to the
/// </summary>
/// <param name="X"></param>
/// <param name="Y"></param>
/// <returns></returns>
public G3DPoint TransformXY(Millimeters X, Millimeters Y)
{
G3DCompBase reference = Container;
if (Loc.XYYawMode == G3DPoint.eMode.Absolute)
{
return(new G3DPoint(Loc.X + X, Loc.Y + Y, 0.0));
}
G3DPoint outerPt = outerPt = new G3DPoint(Loc);
double dist = Math.Sqrt(X * X + Y * Y);
double theta = Math.Atan2(Y, X);
outerPt.X += dist * Math.Cos(theta + Loc.Yaw);
outerPt.Y += dist * Math.Sin(theta + Loc.Yaw);
if (reference == null)
{
return(outerPt);
}
return(reference.TransformXY(outerPt.X, outerPt.Y));
}
/// <summary>
/// Set the absolute end point
/// </summary>
/// <param name="ptEnd"></param>
public void SetAbsEndPoint(G3DPoint ptEnd)
{
Loc.Yaw = Loc.YawTo(ptEnd);
EndPoint.Distance = Loc.XYDistanceTo(ptEnd);
EndPoint.ZOffset = ptEnd.Z - Loc.Z;
}
/// <summary>
/// Set the absolute start point
/// </summary>
/// <param name="ptStart"></param>
public void SetAbsStartPoint(G3DPoint ptStart)
{
Loc = new G3DPoint(ptStart);
Loc.XYYawMode = G3DPoint.eMode.Absolute;
Loc.ZMode = G3DPoint.eMode.Absolute;
}
/// <summary>
/// Create a GLine given the absolute coordinate positions
/// </summary>
/// <param name="ptStart"></param>
/// <param name="ptEnd"></param>
public GLine(G3DPoint ptStart, G3DPoint ptEnd)
: base()
{
SetAbsStartPoint(ptStart);
SetAbsEndPoint(ptEnd);
}
/// <summary>
/// Convert the specific point to pixel space
/// </summary>
/// <param name="absPt"></param>
/// <returns></returns>
public PointF AbsPointToPixel(G3DPoint absPt)
{
return(AbsPointToPixel(absPt.X, absPt.Y));
}
/// <summary>
/// Manual creation constructor
/// </summary>
/// <param name="name"></param>
public G3DCompBase(string name)
: base(name)
{
Loc = new G3DPoint();
}
