程序主界面
实验视频
该软件基于C#开发，用到了Tao.OpenGL库，OpenTk库。 可以加载STL格式的模型，在OpenGL场景中显示模型，平移和旋转模型。通过聚类算法对模型的三角网格进行分组和简化，通过选择网格组生成路径点，也可以通过选择边上的点创建路径点。生成的路径点可以通过调整旋转角度，来调节机加工刀具的方向，路径起始位置增加进刀路径和结束位置增加退刀路径。 生成的路径可转化并导出为设备指令用于机器人或者机床进行机加工或者焊接等操作。
部分源码：
一、文件导出
//导出LS文件
public void ExportLS () {
if (Source.Count != 0) {
SaveFileDialog dgSaveFile = new SaveFileDialog();
dgSaveFile.Filter = "ls files(*.ls)|*.ls";
if (dgSaveFile.ShowDialog() == DialogResult.OK) {
try {
if (dgSaveFile.FileName != "") {
StringBuilder sb = new StringBuilder();
string tmpTime = DateTime.Now.ToString("yy-MM-dd HH:mm:ss");//时间戳
string[] tmp = tmpTime.Split(' ');//空格分开时间
MessageBox.Show("Save: " + dgSaveFile.FileName + " at : " + DateTime.Now.ToString("yy-MM-dd HH:mm:ss"));
sb.Append("/PROG " + System.IO.Path.GetFileNameWithoutExtension(dgSaveFile.FileName) + "\r\n"); //程序名
sb.Append("/ATTR\r\n");//属性
sb.Append("OWNER = MNEDITOR;\r\n");//所有者
sb.Append("COMMENT = \"Build by EdgeDetector\";\r\n");//注释
sb.Append("PROG_SIZE = 200;\r\n");//程序大小
sb.Append("CREATE = DATE " + tmp[0] + " TIME " + tmp[1] + ";\r\n");//创建日期和时间
sb.Append("MODIFIED = DATE " + tmp[0] + " TIME " + tmp[1] + ";\r\n");//编辑日期和时间
sb.Append("FILE_NAME = ;\r\n");//文件名
sb.Append("VERSION = 0;\r\n");//版本0
sb.Append("LINE_COUNT = " + Source.Count() + ";\r\n");//行数
sb.Append("MEMORY_SIZE = 1197;\r\n");//内存大小
sb.Append("PROTECT = READ_WRITE;\r\n");//读 写
sb.Append("TCD: STACK_SIZE = 0,\r\n");//堆栈大小
sb.Append(" TASK_PRIORITY = 50,\r\n");//任务属性 50
sb.Append(" TIME_SLICE = 0,\r\n");
sb.Append(" BUSY_LAMP_OFF = 0,\r\n");
sb.Append(" ABORT_REQUEST = 0,\r\n");//放弃请求
sb.Append(" PAUSE_REQUEST = 0;\r\n");//暂停请求
sb.Append("DEFAULT_GROUP = 1,*,*,*,*;\r\n");//默认组
sb.Append("CONTROL_CODE = 00000000 00000000;\r\n");//控制代码
sb.Append("/MN\r\n");
sb.Append("1: UTOOL_NUM=1;\r\n");//用户工具数
sb.Append("2: UFRAME_NUM=1;\r\n");//用户坐标系数
sb.Append("3: ;\r\n");
sb.Append("4: J P[1] 60% CNT50;\r\n");//指令：第四行 J P[1] 60% CNT50 关节运动第一个点P[1]
for (int i = 0; i < Source.Count(); i++) {//遍历路径点
sb.Append(i + 5); //行数
sb.Append(":L P[");//直线运动
sb.Append(i + 2);//第i+2个点 P[2] ……P[Source.Count()+1]
sb.Append("] 30mm/sec CNT10;\r\n"); //速度30mm/sec CNT10--圆弧过渡？
}
//sb.Append("] " + txt_LS_spd.Text.ToString() + "mm/sec CNT" + txt_CNT.Text.ToString() + " ;\r\n");
sb.Append("/POS\r\n");//位置
sb.Append("P[1]{\r\n");//第一个关节点
sb.Append(" GP1:\r\n");//GP1
sb.Append(" UF : 1, UT : 1, CONFIG : 'N U T, 0, 0, 0',\r\n");//配置
sb.AppendFormat(" X = {0:0.000} mm, Y = {1:0.000} mm, Z = {2:0.000} mm,\r\n", Source[0].X, Source[0].Y, Source[0].Z);//位置
sb.AppendFormat(" W = {0:0.000} deg, P = {1:0.000} deg, R = {2:0.000} deg\r\n", Source[0].A, Source[0].B, Source[0].C);//姿态
sb.Append("};\r\n");
for (int i = 0; i < Source.Count(); i++) {//遍历所有路径点
sb.Append("P[");
sb.Append(i + 2);//第i+2个点
sb.Append("]{\r\n");
sb.Append(" GP1:\r\n");//GP1
sb.Append(" UF : 1, UT : 1, CONFIG : 'N U T, 0, 0, 0',\r\n");//配置
sb.AppendFormat(" X = {0:0.000} mm, Y = {1:0.000} mm, Z = {2:0.000} mm,\r\n", Source[i].X, Source[i].Y, Source[i].Z);//位置
sb.AppendFormat(" W = {0:0.000} deg, P = {1:0.000} deg, R = {2:0.000} deg\r\n", Source[i].A, Source[i].B, Source[i].C);//姿态
sb.Append("};\r\n");
}
sb.Append("/END\r\n");//结束符
StreamWriter sw = new StreamWriter(dgSaveFile.FileName);
sw.WriteLine(sb.ToString());//写入文件
sw.Close();
}
}
catch (Exception ex) {
MessageBox.Show("Save File Error :" + ex.Message);
}
}
}
else
MessageBox.Show("Please calculate path first.");
}
//导出CSV文件
public void ExportCSV () {
if (Source.Count != 0) {
SaveFileDialog dgSaveFile = new SaveFileDialog();//保存对话框
dgSaveFile.Filter = "csv files(*.csv)|*.csv";//过滤器
if (dgSaveFile.ShowDialog() == DialogResult.OK) {
try {
if (dgSaveFile.FileName != "") {
var Path = new XYZABC[this.Source.Count];//初始化数组Path
this.Source.CopyTo(Path, 0);//拷贝
StringBuilder sb = new StringBuilder();
string tmpTime = DateTime.Now.ToString("yy-MM-dd HH:mm:ss");//时间戳
string[] tmp = tmpTime.Split(' ');//空格划分时间戳
MessageBox.Show("Save: " + dgSaveFile.FileName + " at : " + DateTime.Now.ToString("yy-MM-dd HH:mm:ss"));//保存文件
for (int i = 0; i < Path.Count(); i++) {
sb.AppendFormat("{0:0.000},{1:0.000},{2:0.000},", Path[i].X, Path[i].Y, Path[i].Z);//路径点坐标
sb.AppendFormat("{0:0.000},{1:0.000},{2:0.000}\r\n", Path[i].A, Path[i].B, Path[i].C);路径点姿态 三位小数
}
StreamWriter sw = new StreamWriter(dgSaveFile.FileName);
sw.WriteLine(sb.ToString());//写入文件
sw.Close();
}
}
catch (Exception ex) {
MessageBox.Show("Save File Error :" + ex.Message);
}
}
}
else
MessageBox.Show("Please calculate path first.");
}二、四元数 与 欧拉角
//四元数 ：从向量u 转到 向量v
private Quaternion FromV1toV2 (Vector3 u, Vector3 v) {
if (u == -v) {//方向相反
float x = Math.Abs(u.X);
float y = Math.Abs(u.Y);
float z = Math.Abs(u.Z);
//x,y,z哪个最小，就选哪个单位向量
Vector3 other = x < y ? (x < z ? new Vector3(1, 0, 0) : new Vector3(0, 0, 1)) : (y < z ? new Vector3(0, 1, 0) : new Vector3(0, 0, 1));
var tmp = Vector3.Cross(v, other);
return new Quaternion(Vector3.Normalize(tmp), 0);
}
u.Normalize();//归一化
v.Normalize();
return Quaternion.FromAxisAngle(Vector3.Cross(u, v), (float)Math.Acos(Vector3.Dot(u, v)));
}
//四元数：从u->v ?
private Quaternion fromV1toV2 (Vector3 u, Vector3 v) {
if (u == -v) {
float x = Math.Abs(u.X);
float y = Math.Abs(u.Y);
float z = Math.Abs(u.Z);
Vector3 other = x < y ? (x < z ? new Vector3(1, 0, 0) : new Vector3(0, 0, 1)) : (y < z ? new Vector3(0, 1, 0) : new Vector3(0, 0, 1));
var tmp = Vector3.Cross(v, other);
return new Quaternion(Vector3.Normalize(tmp), 0);
}
Vector3 half = Vector3.Normalize(u + v);//两个向量的平分向量
return new Quaternion(Vector3.Cross(u, half), Vector3.Dot(u, half));
}
//四元数转欧拉角
private Vector3 ToEulerAngles (Quaternion q) {
double x, y, z;
float sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
float cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
x = Math.Atan2(sinr_cosp, cosr_cosp);
float sinp = 2 * (q.W * q.Y - q.Z * q.X);
if (Math.Abs(sinp) >= 1)
y = Math.PI / 2 * Math.Sign(sinp); // use 90 degrees if out of range
else
y = Math.Asin(sinp);
float siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
float cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
z = Math.Atan2(siny_cosp, cosy_cosp);
return new Vector3((float)x, (float)y, (float)z);
}
//旋转矩阵转欧拉角
Vector3 RotationMatrixToEulerAngles (Matrix3 R) {
var sy = Math.Sqrt(R.M11 * R.M11 + R.M21 * R.M21);
bool singular = sy < 1e-6; // If
double x, y, z;
if (!singular) {
x = Math.Atan2(R.M32, R.M33);
y = Math.Atan2(-R.M31, sy);
z = Math.Atan2(R.M21, R.M11);
}
else {
x = Math.Atan2(-R.M23, R.M22);
y = Math.Atan2(-R.M31, sy);
z = 0;
}
return new Vector3((float)x, (float)y, (float)z);
}三、自定义输入对话框
public static class Dialog {
//自定义输入对话框：标题，提示，值
public static DialogResult InputBox (string title, string promptText, ref string value) {
Form form = new Form();//窗口
Label label = new Label();//标签
TextBox textBox = new TextBox();//文本框
Button buttonOk = new Button();//ok按钮
Button buttonCancel = new Button();//取消按钮
form.Text = title;//标题
label.Text = promptText;//提示文本
textBox.Text = value;//值
buttonOk.Text = "OK";
buttonCancel.Text = "Cancel";
buttonOk.DialogResult = DialogResult.OK;//单击按钮ok返回 对话框结果ok
buttonCancel.DialogResult = DialogResult.Cancel;
label.SetBounds(9, 20, 372, 13);//设置边框
textBox.SetBounds(12, 36, 372, 20);
buttonOk.SetBounds(228, 72, 75, 23);
buttonCancel.SetBounds(309, 72, 75, 23);
//控件风格
label.AutoSize = true;
textBox.Anchor = textBox.Anchor | AnchorStyles.Right;
buttonOk.Anchor = AnchorStyles.Bottom | AnchorStyles.Right;
buttonCancel.Anchor = AnchorStyles.Bottom | AnchorStyles.Right;
form.ClientSize = new Size(396, 107); //窗体客户端尺寸
form.Controls.AddRange(new Control[] { label, textBox, buttonOk, buttonCancel });//窗体添加控件
form.ClientSize = new Size(Math.Max(300, label.Right + 10), form.ClientSize.Height);//客户端尺寸
form.FormBorderStyle = FormBorderStyle.FixedDialog;//窗体边框风格
form.StartPosition = FormStartPosition.CenterScreen;//开始位置
form.MinimizeBox = false;
form.MaximizeBox = false;
form.AcceptButton = buttonOk;
form.CancelButton = buttonCancel;
DialogResult dialogResult = form.ShowDialog();//将窗体显示为模式对话框
value = textBox.Text;//获取文本框的值
return dialogResult;//返回对话框结果
}
}四、自定义OpenGL面板控件
class OpenGLPanel : Panel {
public Action<int, int> SelectEvent;//单击选择动作事件
Vector3 cmrCenter = new Vector3(0, 0, 0);//相机看的中心
Vector3 cmrX = new Vector3(-1, 0, 0);//当前
Vector3 cmrY = new Vector3(0, 1, 0);
IntPtr hdc, hrc;
Point CurrentRotate = new Point();
Point CurrentTranslate = new Point();
double scale = 2;
int Mouse_sensitive = 1;//鼠标敏感度
public OpenGLPanel () {
MouseDown += OpenGLPanel_MouseDown; //注册控件的鼠标按下事件
MouseMove += OpenGLPanel_MouseMove;//鼠标移动事件
MouseWheel += OpenGLPanel_MouseWheel;//鼠标滚轮事件
SizeChanged += OpenGLPanel_SizeChanged;//控件尺寸改变
InitOpenGL();//初始化GL
}
//渲染
public void Render () {
#region Control
//初始化
Gl.glFlush();
Gl.glClear(Gl.GL_COLOR_BUFFER_BIT | Gl.GL_DEPTH_BUFFER_BIT);
Gl.glLoadIdentity();
Gl.glOrtho(-Width / 2, Width / 2, -Height / 2, Height / 2, -20000, 10000);
SetCameraPosition();
Gl.glScaled(scale, scale, scale);
#endregion
//显示坐标轴
#region Axis
Gl.glColor3f(1, 1, 1);
//Glu.gluSphere(Glu.gluNewQuadric(), 3, 36, 36);
Gl.glLineWidth(3);
Gl.glBegin(Gl.GL_LINES);
Gl.glColor3f(1, 0, 0);
Gl.glVertex3f(20, 0, 0);
Gl.glVertex3f(0, 0, 0);
Gl.glColor3f(0, 0, 1);
Gl.glVertex3f(0, 0, 20);
Gl.glVertex3f(0, 0, 0);
Gl.glColor3f(0, 1, 0);
Gl.glVertex3f(0, 20, 0);
Gl.glVertex3f(0, 0, 0);
Gl.glEnd();
//Gl.glBegin(Gl.GL_QUADS);
//Gl.glColor3f(1, 1, 1);
//Gl.glVertex3f(1000, 1000, -200);
//Gl.glVertex3f(-1000, 1000, -200);
//Gl.glVertex3f(-1000, -1000, -200);
//Gl.glVertex3f(1000, -1000, -200);
//Gl.glEnd();
#endregion
//显示列表
#region List
Gl.glColor3d(0.5, 0.6, 0.6);
Gl.glCallList((int)GlList.Workpiece);
Gl.glCallList((int)GlList.Path);
Gl.glCallList((int)GlList.GridBall);
Gl.glColor3ub(40, 40, 40);
Gl.glCallList((int)GlList.Machine);
#endregion
}
//交换缓存
public void SwapBuffer () {
SwapBuffers(hdc);
}
//设置相机位置
private void SetCameraPosition () {
var cmrZ = Vector3.Cross(cmrX, cmrY);//相机方向
var cmrPosition = cmrCenter - 500 * cmrZ;//相机位置
Glu.gluLookAt(cmrPosition.X, cmrPosition.Y, cmrPosition.Z, cmrCenter.X, cmrCenter.Y, cmrCenter.Z, cmrY.X, cmrY.Y, cmrY.Z);
}
//鼠标移动事件
private void OpenGLPanel_MouseMove (object sender, MouseEventArgs e) {
switch (e.Button) {
case MouseButtons.Left:
break;
case MouseButtons.None:
break;
case MouseButtons.Right:// 旋转相机
var x = Matrix4.CreateFromAxisAngle(cmrY, (CurrentRotate.X - e.X) / 200f / Mouse_sensitive);
var y = Matrix4.CreateFromAxisAngle(cmrX, -(CurrentRotate.Y - e.Y) / 200f / Mouse_sensitive);
cmrY = Vector3.Transform(cmrY, y);
cmrX = Vector3.Transform(cmrX, x);//
CurrentRotate = e.Location;//获取
break;
case MouseButtons.Middle://平移相机
cmrCenter -= cmrX * ((CurrentTranslate.X - e.X) / Mouse_sensitive);
cmrCenter -= cmrY * ((CurrentTranslate.Y - e.Y) / Mouse_sensitive);
CurrentTranslate = e.Location;
break;
case MouseButtons.XButton1:
break;
case MouseButtons.XButton2:
break;
default:
break;
}
}
//鼠标按下
private void OpenGLPanel_MouseDown (object sender, MouseEventArgs e) {
switch (e.Button) {
case MouseButtons.Left:
SelectEvent(e.X, e.Y);//选择事件
break;
case MouseButtons.None:
break;
case MouseButtons.Right:
CurrentRotate = e.Location;//更新位置
break;
case MouseButtons.Middle:
CurrentTranslate = e.Location;
break;
case MouseButtons.XButton1:
break;
case MouseButtons.XButton2:
break;
default:
break;
}
}
//滚轮
private void OpenGLPanel_MouseWheel (object sender, MouseEventArgs e) {
if (scale > 0.1) {
if (e.Delta > 0)
scale *= 1.05;//缩放
else
scale *= 0.95;
if (scale < 0.1)
scale = 0.11;
}
}
private void OpenGLPanel_SizeChanged (object sender, EventArgs e) {
Gl.glViewport(0, 0, Width, Height);
}
//初始化opengl
private void InitOpenGL () {
hdc = GetDC(Handle);//获取句柄
SetPixelFormatDescriptor();//设置像素格式描述子
hrc = wglCreateContext(hdc);//函数创建一个新的OpenGL渲染描述表,此描述表必须适用于绘制到由hdc返回的设备。这个渲染描述表将有和设备上下文(dc)一样的像素格式。
if (wglMakeCurrent(hdc, hrc) == false) {//指定绘制设备
MessageBox.Show("Initial Fail!!");
return;
}
Gl.glEnable(Gl.GL_DEPTH_TEST);
//Gl.glEnable(Gl.GL_CULL_FACE);
Gl.glEnable(Gl.GL_LIGHTING);
Gl.glEnable(Gl.GL_COLOR_MATERIAL);
Gl.glColorMaterial(Gl.GL_FRONT_AND_BACK, Gl.GL_AMBIENT_AND_DIFFUSE);
Gl.glMatrixMode(Gl.GL_PROJECTION);
Gl.glClearColor(0.9f, 0.9f, 0.9f, 1.0f);//清空画布
//设置灯光0 1 2 位置
SetLight(0, 0, 1000, 1000);
SetLight(1, 500, -500, 1000);
SetLight(2, -500, -500, 1000);
}
//设置灯光
private void SetLight (int Light, int x, int y, int z) {
Gl.glLightfv(Gl.GL_LIGHT0 + Light, Gl.GL_AMBIENT, new float[4] { 0.1f, 0.1f, 0.1f, 1.0f });
Gl.glLightfv(Gl.GL_LIGHT0 + Light, Gl.GL_DIFFUSE, new float[4] { 0.2f, 0.2f, 0.2f, 1.0f });
Gl.glLightfv(Gl.GL_LIGHT0 + Light, Gl.GL_SPECULAR, new float[4] { 0.2f, 0.2f, 0.2f, 1.0f });
Gl.glLightfv(Gl.GL_LIGHT0 + Light, Gl.GL_POSITION, new float[4] { x, y, z, 0f });//位置
Gl.glLightModeli(Gl.GL_LIGHT_MODEL_TWO_SIDE, Gl.GL_TRUE);
Gl.glEnable(Gl.GL_LIGHT0 + Light);
}
private void SetPixelFormatDescriptor () {
const uint PFD_DOUBLEBUFFER = 0x00000001;
const uint PFD_STEREO = 0x00000002;
const uint PFD_DRAW_TO_WINDOW = 0x00000004;
const uint PFD_DRAW_TO_BITMAP = 0x00000008;
const uint PFD_SUPPORT_GDI = 0x00000010;
const uint PFD_SUPPORT_OPENGL = 0x00000020;
const uint PFD_GENERIC_FORMAT = 0x00000040;
const uint PFD_NEED_PALETTE = 0x00000080;
const uint PFD_NEED_SYSTEM_PALETTE = 0x00000100;
const uint PFD_SWAP_EXCHANGE = 0x00000200;
const uint PFD_SWAP_COPY = 0x00000400;
const uint PFD_SWAP_LAYER_BUFFERS = 0x00000800;
const uint PFD_GENERIC_ACCELERATED = 0x00001000;
const uint PFD_SUPPORT_DIRECTDRAW = 0x00002000;
const byte PFD_TYPE_RGBA = 0;
const byte PFD_MAIN_PLANE = 0;
PIXELFORMATDESCRIPTOR pfd = new PIXELFORMATDESCRIPTOR();
pfd.nSize = (ushort)Marshal.SizeOf(pfd);
pfd.nVersion = 1;
pfd.dwFlags = PFD_DRAW_TO_WINDOW | PFD_SUPPORT_OPENGL | PFD_DOUBLEBUFFER;
pfd.iPixelType = PFD_TYPE_RGBA;
pfd.cColorBits = 24;
pfd.cRedBits = 0;
pfd.cRedShift = 0;
pfd.cGreenBits = 0;
pfd.cGreenShift = 0;
pfd.cBlueBits = 0;
pfd.cBlueShift = 0;
pfd.cAlphaBits = 0;
pfd.cAlphaShift = 0;
pfd.cAccumBits = 0;
pfd.cAccumRedBits = 0;
pfd.cAccumGreenBits = 0;
pfd.cAccumBlueBits = 0;
pfd.cAccumAlphaBits = 0;
pfd.cDepthBits = 32;
pfd.cStencilBits = 0;
pfd.cAuxBuffers = 0;
pfd.iLayerType = PFD_MAIN_PLANE;
pfd.bReserved = 0;
pfd.dwLayerMask = 0;
pfd.dwVisibleMask = 0;
pfd.dwDamageMask = 0;
int PixelFormat = ChoosePixelFormat((int)hdc, ref pfd);
SetPixelFormat((int)hdc, PixelFormat, ref pfd);
}
[StructLayout(LayoutKind.Explicit)]
private struct PIXELFORMATDESCRIPTOR {
[FieldOffset(0)]
public ushort nSize;
[FieldOffset(2)]
public ushort nVersion;
[FieldOffset(4)]
public uint dwFlags;
[FieldOffset(8)]
public byte iPixelType;
[FieldOffset(9)]
public byte cColorBits;
[FieldOffset(10)]
public byte cRedBits;
[FieldOffset(11)]
public byte cRedShift;
[FieldOffset(12)]
public byte cGreenBits;
[FieldOffset(13)]
public byte cGreenShift;
[FieldOffset(14)]
public byte cBlueBits;
[FieldOffset(15)]
public byte cBlueShift;
[FieldOffset(16)]
public byte cAlphaBits;
[FieldOffset(17)]
public byte cAlphaShift;
[FieldOffset(18)]
public byte cAccumBits;
[FieldOffset(19)]
public byte cAccumRedBits;
[FieldOffset(20)]
public byte cAccumGreenBits;
[FieldOffset(21)]
public byte cAccumBlueBits;
[FieldOffset(22)]
public byte cAccumAlphaBits;
[FieldOffset(23)]
public byte cDepthBits;
[FieldOffset(24)]
public byte cStencilBits;
[FieldOffset(25)]
public byte cAuxBuffers;
[FieldOffset(26)]
public byte iLayerType;
[FieldOffset(27)]
public byte bReserved;
[FieldOffset(28)]
public uint dwLayerMask;
[FieldOffset(32)]
public uint dwVisibleMask;
[FieldOffset(36)]
public uint dwDamageMask;
}
[DllImport("gdi32")]
private static extern IntPtr SwapBuffers (IntPtr hdc);
[DllImport("gdi32")]
private static extern int SetPixelFormat (int hDC, int n, ref PIXELFORMATDESCRIPTOR pcPixelFormatDescriptor);
[DllImport("gdi32")]
private static extern int ChoosePixelFormat (int hDC, ref PIXELFORMATDESCRIPTOR pPixelFormatDescriptor);
[DllImport("opengl32.dll")]
private static extern IntPtr wglCreateContext (IntPtr HDC);//创建一个新的OpenGL渲染描述表,此描述表必须适用于绘制到由hdc返回的设备。这个渲染描述表将有和设备上下文(dc)一样的像素格式。
[DllImport("opengl32.dll")]
private static extern bool wglMakeCurrent (IntPtr HDC, IntPtr HGLRC);//一个设备上下文句柄。其后的OpenGL调用由调用线程所确定的设备HDC上绘制。
[DllImport("user32")]
private static extern IntPtr GetDC (IntPtr hwnd);//指定窗口的客户区域或整个屏幕的显示 设备上下文环境的句柄
}五、自定义路径面板控件
public partial class XyzwprPanel : UserControl {
List<List<XYZABC>> Sources = new List<List<XYZABC>>();//路径 列表。
BindingSource tmpSource;//面板绑定源
int cnt = 0;//路径数
int index = 0;//当前选择路径索引
public XyzwprPanel () {
InitializeComponent();
}
public void RefreshSource (BindingSource source) {
tmpSource = source;//刷新绑定的数据源路径
GridView.DataSource = source;//设置网格视图数据源
}
//保存
private void BtnSave_Click (object sender, EventArgs e) {
if (GridView.DataSource != null) {
BindingSource source = (BindingSource)GridView.DataSource;//绑定数据源
XYZABC[] Path = new XYZABC[source.Count];//创建路径
source.CopyTo(Path, 0);//拷贝数据源到路径
Sources.Add(Path.ToList());//添加到路径列表
CboPath.Items.Add("Path" + cnt);//增加路径下拉框选项
CboPath.SelectedIndex = cnt;//设置当前路径选择索引
cnt++;
}
}
//删除选择的路径
private void BtnDelete_Click (object sender, EventArgs e) {
try {
Sources.RemoveAt(index);//移除数据源
CboPath.Items.Remove("Path" + index);//移除路径
cnt -= 1;//数量-1
CboPath.SelectedIndex = -1;//不选择路径
}
catch (Exception) {
}
}
//导出路径
private void BtnExport_Click (object sender, EventArgs e) {
List<XYZABC> tmp = new List<XYZABC>();
foreach (var p in Sources)//遍历数据源
tmp.AddRange(p);
//ExportTool ET = new ExportTool(tmp);
//new TMListen(tmp).ShowDialog();
new TMPathView(tmp).Show();
}
//路径切换
private void CboPath_SelectedIndexChanged (object sender, EventArgs e) {
index = CboPath.SelectedIndex;//路径索引
GridView.DataSource = Sources[index];//路径对应数据源
Gl.glNewList((int)GlList.Path, Gl.GL_COMPILE);
Gl.glColor3ub(200, 200, 200);
foreach (var p in Sources[index]) {//遍历路径
Gl.glPushMatrix();
Gl.glTranslated(p.X, p.Y, p.Z);//移动到该点
Glu.gluSphere(Glu.gluNewQuadric(), 2, 10, 10);//画球
Gl.glPopMatrix();
}
Gl.glEndList();
}
//面板选择改变
private void GridView_SelectionChanged (object sender, EventArgs e) {
var current = GridView.CurrentRow.Cells;//当前
Gl.glNewList((int)GlList.GridBall, Gl.GL_COMPILE);
Gl.glPushMatrix();
Gl.glColor3d(0.8, 0.8, 1);
Gl.glTranslated(
float.Parse(current[0].Value.ToString()),
float.Parse(current[1].Value.ToString()),
float.Parse(current[2].Value.ToString()));//移动到当前点
Glu.gluSphere(Glu.gluNewQuadric(), 3, 20, 20);//画个球
Gl.glPopMatrix();
Gl.glEndList();
}
}六、三角网格聚类
//聚类算法：减小网格密度
public class DbscanAlgorithm<T> where T : TriMesh
{
private readonly Func<T, T, double> _metricFunc;//度量函数
public DbscanAlgorithm (Func<T, T, double> metricFunc)
{
_metricFunc = metricFunc;//度量函数:计算两个相邻三角网格的夹角。如果不相邻返回10，相邻返回夹角。
}
public DbscanAlgorithm () {
}
//计算聚类
public void ComputeClusterDbscan (T[] allPoints, double epsilon, int minPts, out HashSet<T[]> clusters) {
DbscanPoint<T>[] allPointsDbscan = allPoints.Select(x => new DbscanPoint<T>(x)).ToArray();//所有三角网格
int clusterId = 0;//所属类索引
for (int i = 0; i < allPointsDbscan.Length; i++) {
DbscanPoint<T> p = allPointsDbscan[i];//一个三角网格
if (p.IsVisited)
continue;
p.IsVisited = true;//可访问
//查找三角网格相邻元素
RegionQuery(allPointsDbscan, p.ClusterPoint, epsilon, out DbscanPoint<T>[] neighborPts);
if (neighborPts.Length < minPts)//邻居数minPts=1，如果没有邻居
p.ClusterId = (int)ClusterIds.Noise;//噪点
else {
clusterId++;//类索引+1
ExpandCluster(allPointsDbscan, p, neighborPts, clusterId, epsilon, minPts);
}
}
clusters = new HashSet<T[]>(
allPointsDbscan
.Where(x => x.ClusterId > 0)
.GroupBy(x => x.ClusterId)
.Select(x => x.Select(y => y.ClusterPoint).ToArray())
);//分组所有三角网格
}
//扩大聚类
private void ExpandCluster (DbscanPoint<T>[] allPoints, DbscanPoint<T> point, DbscanPoint<T>[] neighborPts, int clusterId, double epsilon, int minPts) {
point.ClusterId = clusterId;//类索引
for (int i = 0; i < neighborPts.Length; i++) {//遍历邻居
DbscanPoint<T> pn = neighborPts[i];//取一个邻居
if (!pn.IsVisited) {//邻居不可访问
pn.IsVisited = true;//设置邻居可访问
DbscanPoint<T>[] neighborPts2;//邻居集合
RegionQuery(allPoints, pn.ClusterPoint, epsilon, out neighborPts2);//查找邻居的邻居
if (neighborPts2.Length >= minPts)
{// 邻居pn有邻居
neighborPts = neighborPts.Union(neighborPts2).ToArray();//合并邻居
}
}
if (pn.ClusterId == (int)ClusterIds.Unclassified)//邻居未分类
pn.ClusterId = clusterId;//设置邻居的类ID为 当前聚类ID
}
}
//区域查询：查找point的邻居
private void RegionQuery (DbscanPoint<T>[] allPoints, T point, double epsilon, out DbscanPoint<T>[] neighborPts) {
neighborPts = (from p in allPoints.AsParallel()
where Math.Abs(p.ClusterPoint.norm.X - point.norm.X) < 0.1
where Math.Abs(p.ClusterPoint.norm.Y - point.norm.Y) < 0.1
where Math.Abs(p.ClusterPoint.norm.Z - point.norm.Z) < 0.1
where _metricFunc(point, p.ClusterPoint) <= epsilon
select p).ToArray();//点point的法线接近的，且与point 法线夹角小于epsilon的那些点 p.ClusterPoint
}
public enum ClusterIds {
Unclassified = 0,
Noise = -1
}
public class DbscanPoint<T> {
public bool IsVisited;
public T ClusterPoint;
public int ClusterId;//三角网格所属类ID
public DbscanPoint (T x) {
ClusterPoint = x;
IsVisited = false;
ClusterId = (int)ClusterIds.Unclassified;
}
}
}七、创建GL显示列表
//创建未分组的路径列表：根据颜色搜索网格面
public void MakeUnsortPathList (byte[] color)
{
int ID = 0;
bool hit = false;
//-----------通过颜色搜索对象 search object by color
foreach (var face in Meshgroup)//遍历网格组
if (color[0] == face.color[0] && color[1] == face.color[1] && color[2] == face.color[2]) {
hit = true;//找到目标颜色的网格面
ID = face.ID;//获取面ID
SelectedGroup.Add(Meshgroup[ID]);//将网格面添加到 选择的组中
break;
}
if (hit == false)
return;
//-----------设置组合颜色 set group and color
if (SelectedGroup.Where(p => p.ID == ID).Count() == 2) { // 选择了两个网格面
// 双选时更改组和颜色 change group and color when double select
SelectedGroup.RemoveAt(SelectedGroup.Count() - 1); //移除重复的项 remove repeat item
//切换分组：组A 组B 切换
if (Meshgroup[ID].groupB == true) {//网格组 属于组B：选中组
Meshgroup[ID].groupB = false;
Meshgroup[ID].groupA = true;
}
else {
Meshgroup[ID].groupB = true;
Meshgroup[ID].groupA = false;
}
MakeSortedList();//更新 创建已分组列表。
}
else {//第一次选择网格，设置分组A
// set group and color on selected item
Meshgroup[ID].groupB = false;//
Meshgroup[ID].groupA = true;//设置网格组A选中
MakeSortedList();
}
//-----------groupS 和 group B 之间的相交边 intersect edge betewwn groupA and groupB
myLines.Clear();//网格相交线条集合清空
var points = new HashSet<Vector3>(); //交点集合
foreach (var gA in SelectedGroup.Where(p => p.groupA == true))//遍历组A中网格gA
foreach (var gB in SelectedGroup.Where(p => p.groupB == true))//遍历组B中网格gB
foreach (var a in gA.meshes)//遍历网格gA中的三角网格a
foreach (var b in gB.meshes) {//遍历网格gB中的三角网格b
var intersect = a.vectors.Intersect(b.vectors).ToArray();//a b网格顶点的交集。
if (intersect.Count() == 2) {//a b 两个三角网格有两个交点
myLines.Add(new MyLine(intersect[0], intersect[1]) { Norm = b.norm });//交线添加到线条集合
foreach (var c in intersect)//遍历交点
points.Add(c);//将两个三角网格的交点添加到点集合
}
}
//-----------绘制边 draw edge
Gl.glNewList(PathNum, Gl.GL_COMPILE);
Gl.glColor3f(0, 1, 1);
foreach (var p in points) {//遍历所有交点
Gl.glPushMatrix();
Gl.glTranslated(p.X, p.Y, p.Z);//在交点处绘制球体
Glu.gluSphere(Glu.gluNewQuadric(), 0.8, 36, 36);
Gl.glPopMatrix();
}
Gl.glBegin(Gl.GL_LINES);//绘制线条
Gl.glLineWidth(2);//线宽2
Gl.glColor3ub(255, 0, 0);//红色
foreach (var p in myLines) {//遍历所有相交线集合
//线条：相交线的起点 ----->起点沿法线方向移动15
Gl.glVertex3f(p.Start.X, p.Start.Y, p.Start.Z);
Gl.glVertex3f(p.Start.X + p.Norm.X * 15, p.Start.Y + p.Norm.Y * 15, p.Start.Z + p.Norm.Z * 15);
//Gl.glVertex3f(p.v2.X, p.v2.Y, p.v2.Z);
//Gl.glVertex3f(p.v2.X + p.Norm.X * 15, p.v2.Y + p.Norm.Y * 15, p.v2.Z + p.Norm.Z * 15);
}
Gl.glEnd();
Gl.glEndList();
}八、保存机器人路径序列化文件并压缩
public class TMpath {
private List<XYZABC> robotPath;//机器人路径 位姿列表
private string RootPath;//根路径
//构造函数
public TMpath (List<XYZABC> RobotPath) {
robotPath = RobotPath;
}
//选择和创建： 目录名，速度
public void SelectAndCreate (string FileName, int speed) {
FolderBrowserDialog fbd = new FolderBrowserDialog();//选择对话框
fbd.ShowDialog();
var path = fbd.SelectedPath;//获取选择的目录
if (path != null) {
path = Path.Combine(path, FileName);//文件夹+目录名FileName
Directory.CreateDirectory(path);//创建目录
RootPath = path;//根目录
path += $@"\path1.PATH";//机器人路径 path1.PATH
CreatePathFile(path, speed);//创建路径文件
RunZip(RootPath);//压缩根目录
}
}
//创建配置文件
private void CreateConfig (string path) {
using (StreamWriter sw = new StreamWriter(path + @"/ConfigData.XML")) {
sw.WriteLine(@"<Configuration></Configuration>");
}
}
//创建路径文件
private void CreatePathFile (string path, int speed) {
var serilizer = new XmlSerializer(typeof(Root));//序列化
var root = new Root();
root.PointCount = robotPath.Count();//路径点数
for (int i = 0; i < robotPath.Count; i++) {
root.Point.Add(new Point(robotPath[i], speed, i));//添加到root
}
using (var stream = new FileStream(path, FileMode.Create))
serilizer.Serialize(stream, root, new XmlSerializerNamespaces(new[] { XmlQualifiedName.Empty }));//保存序列化root
}
//运行压缩
private void RunZip (string path) {
var proc = Process.Start("TMExportZip.exe", $@"/V:RoboHenry /P:1qaz2wsx {path}");//压缩
}
}
//机器人路径文件 的根节点
[XmlRoot(ElementName = "Root", Namespace = "")]
public class Root {
[XmlAttribute]
public int PointCount { get; set; } = 10; //点数
[XmlAttribute]
public int PvtPointCount { get; set; } = 0;
[XmlAttribute]
public int TaskCount { get; set; } = 0;//任务数
//三次多项式实际是速度规划里面常说的PVT算法。PVT 模 式 是 指 位 置 — 速 度 — 时 间（Position-Velocity-Time）模式。
//PVT模式是一种简单又有效的运动控制模式，用户只需要给定离散点的位置、速度和时间，运动控制卡的插补算法将会生成一条
//连续、平滑的运动路径。
[XmlAttribute]
public int PvtTaskCount { get; set; } = 0;
[XmlAttribute]
public string ID { get; set; } = "4YErH6fuLFvAgY95YCex/78hy9XGDhoM6xXSgaRkKPnuxr2XJJ9nQqDiWJzZMtfms6AmH6GxTj6XBE+gHA8zoQ==";
public string Version { get; set; } = "1.4";//版本
public Base Base { get; set; } = new Base();//基坐标系
public TCP TCP { get; set; } = new TCP();//工具
[XmlElement("Point")]
public List<Point> Point { get; set; } = new List<Point>();//路径点列表，包含速度
}
//基座标系
public class Base {
public string Name { get; set; } = "Base1";
public string Data { get; set; } = "500,0,0,0,0,0";
public string Type { get; set; } = "C";
}
//工具坐标
public class TCP {
public string Name { get; set; } = "NOTOOL";//默认名
public string Description { get; set; } = string.Empty;//描述
public GPTFF GPTFF { get; set; } = new GPTFF();//X Y Z W V U
public float Mass { get; set; } = 0;//质量
public GPTFF MassCenter { get; set; } = new GPTFF();//质心 //X Y Z W V U
public Inertia Inertia { get; set; } = new Inertia();//惯量
public GPTFF GPTCF { get; set; } = new GPTFF();//
public string Studio_tcp { get; set; } = string.Empty;//
public string Studio_stp { get; set; } = string.Empty;
}
//位姿
public class GPTFF {
public float X { get; set; } = 0;
public float Y { get; set; } = 0;
public float Z { get; set; } = 0;
public float W { get; set; } = 0;
public float V { get; set; } = 0;
public float U { get; set; } = 0;
}
//惯量
public class Inertia {
public float Ixx { get; set; } = 0;
public float Iyy { get; set; } = 0;
public float Izz { get; set; } = 0;
}
//机器人路径点
public class Point {
[XmlAttribute]
public int Index { get; set; } = 0;//索引
public string Motion { get; set; } = "PLine";//运动模式 直线
public string coordinate { get; set; } = "0,0,0,0,0,0";// 坐标
public string joint_angle { get; set; } = "0,0,0,0,0,0";//关节角度
public string tool_mode { get; set; } = "0,0,0,0,0,0";//工具坐标
public string Blend { get; set; } = "YES";//混合
public int BlendValue { get; set; } = 50;//混合值，过渡
public string LineABS { get; set; } = "ON"; //
public int LSAVelocity { get; set; } = 100; //速度
public int LSTTTS { get; set; } = 150;
public int PLSAVelocity { get; set; } = 100;
public int PLSTTTS { get; set; } = 150;
public int LSPercentage { get; set; } = 100;
public string PSTTTSOF { get; set; } = "ON";
public int PSTTTS { get; set; } = 250;
public int PSPercentage { get; set; } = 100;
public string Config { get; set; } = "0,2,4"; //配置
public Point () {
}
//构造函数 位姿，速度，索引
public Point (XYZABC p, int speed, int index) {
coordinate =
p.X.ToString("f4") + "," +
p.Y.ToString("f4") + "," +
p.Z.ToString("f4") + "," +
p.A.ToString("f4") + "," +
p.B.ToString("f4") + "," +
p.C.ToString("f4");//坐标
tool_mode = coordinate;//工具坐标 初始化为 路径点坐标
LSAVelocity = speed; //速度
this.Index = index;//索引
}
}附录-操作说明：
The End
1316
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
