private void Draw(float gModalGroup1, Point3D oldCoordinates, Point3D newCoordinates, float?radius, ArcCenters centers, FrameChangedEventArgs e)
{
ArcInterpolation interpolation;
switch (gModalGroup1)
{
case 0:
//Rapid
AddLine(oldCoordinates.ToVector3(), newCoordinates.ToVector3(), RapidGeometry);
break;
case 1:
//Linear
AddLine(oldCoordinates.ToVector3(), newCoordinates.ToVector3(), LinearGeometry);
break;
case 2:
//Arc clockwise
interpolation = e.Frame.RValue.HasValue ? new ArcInterpolation(oldCoordinates, radius: radius.Value, newCoordinates, true) : new ArcInterpolation(oldCoordinates, centers, newCoordinates, true);
for (int i = 0; i <= 100; i += 5)
{
var point = interpolation.GetArcCoordinatesEx(i);
AddLine(oldCoordinates.ToVector3(), point.ToVector3(), LinearGeometry);
oldCoordinates = point;
}
AddLine(oldCoordinates.ToVector3(), newCoordinates.ToVector3(), LinearGeometry);
break;
case 3:
//Arc Counter-clockwise
if (e.Frame.RValue.HasValue)
{
interpolation = new ArcInterpolation(oldCoordinates, radius: radius.Value, newCoordinates, false);
}
else
{
interpolation = new ArcInterpolation(oldCoordinates, centers, newCoordinates, false);
}
for (int i = 100; i >= 0; i -= 5)
{
var point = interpolation.GetArcCoordinatesEx(i);
AddLine(oldCoordinates.ToVector3(), point.ToVector3(), LinearGeometry);
oldCoordinates = point;
}
AddLine(oldCoordinates.ToVector3(), newCoordinates.ToVector3(), LinearGeometry);
break;
}
}
/// <summary>
/// コマンドから動きをシミュレートし、移動所要時間、移動距離、円弧補間情報などセットする
/// </summary>
/// <param name="state">状態</param>
/// <param name="cmd">コマンド</param>
public static void Simulate(State state, NcCodeCmd cmd)
{
switch (cmd.CmdType)
{
case NcCodeCmdType.G00:
// 位置決め
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
var move = cmd.EndPos - cmd.StartPos;
var time = move.Abs() / state.MachineSpeed;
cmd.XYTime = 60.0 * time.Max; // 全要素の中の最大値を取得
cmd.XYLength = move.Length; // 移動距離
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
break;
}
case NcCodeCmdType.G01:
// 直線補間
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
if ((cmd.Flags & NcCodeFlags.F) != 0)
{
state.Speed = cmd.F;
}
var move = cmd.EndPos - cmd.StartPos;
var time = 60.0 * new Vector2d(move.X, move.Y).Length / state.Speed;
cmd.XYTime = time; // 移動時間 ※コントローラが２軸の補間しかできないらしいので平面の距離から時間計算している
cmd.XYLength = move.Length; // 移動距離
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
break;
}
case NcCodeCmdType.G02:
case NcCodeCmdType.G03:
// 円弧補間
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
if ((cmd.Flags & NcCodeFlags.F) != 0)
{
state.Speed = cmd.F;
}
if (cmd.StartPos == cmd.EndPos)
{
// 始点と終点が完全に同じ、無視して良い命令
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
cmd.XYLength = 0.0; // 移動距離
cmd.XYTime = 0.0; // 移動時間
break;
}
var start2 = TransformPlane(state.Plane, cmd.StartPos);
var end2 = TransformPlane(state.Plane, cmd.EndPos);
if (start2 == end2)
{
var planeName = state.Plane.ToString();
throw new WarningException((cmd.LineIndex + 1) + "行目: 円弧補間の" + planeName + "始点と終点が同じで他軸値だけ異なっています。");
}
var arc = new ArcInterpolation(start2, end2, cmd.R, cmd.CmdType == NcCodeCmdType.G02);
if (arc.NoArcCenter)
{
throw new WarningException((cmd.LineIndex + 1) + "行目: 円弧計算解なし");
}
cmd.XYLength = Math.Abs(arc.Distance * arc.Gamma); // 移動距離
cmd.XYTime = 60.0 * cmd.XYLength / state.Speed; // 移動時間
cmd.Plane = state.Plane;
cmd.Arc = arc;
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength | NcCodeFlags.Arc;
break;
}
case NcCodeCmdType.G17:
// XY平面指定
state.Plane = PlaneEnum.XY;
break;
case NcCodeCmdType.G18:
// ZX平面指定
state.Plane = PlaneEnum.ZX;
break;
case NcCodeCmdType.G19:
// YZ平面指定
state.Plane = PlaneEnum.YZ;
break;
case NcCodeCmdType.G53:
// 機械座標系選択
// ※NCMATEにあわせるためワンショット扱い、次の移動命令が来たら解除される
state.G53Processing = true;
break;
case NcCodeCmdType.G90:
// アブソリュート指令
state.AbsoluteMode = true;
break;
case NcCodeCmdType.G91:
// インクリメンタル指令
state.AbsoluteMode = false;
break;
case NcCodeCmdType.G92:
// ワーク座標設定
state.WorkO = state.MachinePosition - new Vector3d(cmd.OrgX, cmd.OrgY, cmd.OrgZ);
break;
}
}