form1.vb

probabilistic road map que sirve para encontrar trayectiroasd robotr

Imports System.Math
Public Class Form1
    Dim cola As New Collections.Queue
    Dim colb As New Collections.Queue
    Dim colc As New Collections.Queue
    Dim cole As New Collections.Stack
    Dim vt1 As New Collections.ArrayList
    Dim vt2 As New Collections.ArrayList
    Dim aq(0 To 359, 0 To 359) As Integer
    Dim abq(0 To 359, 0 To 359) As Boolean
    Dim b, c, i, obs As Integer
    Dim x1, y1, x2, y2, xp, yp As Int16
    Dim orgx As Int16 = 200
    Dim orgy As Int16 = 200
    Dim lapiz As New System.Drawing.Pen(Color.Red, 3)
    Dim lapiz2 As New System.Drawing.Pen(Color.Black, 1)
    Dim lapiz3 As New System.Drawing.Pen(Color.Blue, 3)
    Dim lapiz4 As New System.Drawing.Pen(Color.Gold, 3)
    Dim lapiz5 As New System.Drawing.Pen(Color.White, 3)
    Dim result As String = "P" & vbTab & "X" & vbTab & "Y" & vbTab & "OBSTACULO" & vbCrLf
    Dim puntos(1000, 3) As Double
    Dim inf, sup As Int16
    Dim gi As Integer
    Dim posx1(1, 1000) As Double
    Dim posy1(1, 1000) As Double
    Dim posx2(1, 1000) As Double
    Dim posy2(1, 1000) As Double
    Dim r As Short = 0
    Dim numno As Integer = 10
    Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load
        b = 0
        c = 0
        i = 0
        obs = 1
        inf = 0
    End Sub

    Private Sub PictureBox1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles PictureBox1.Click

    End Sub

    Private Sub PictureBox1_MouseDown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles PictureBox1.MouseDown
        If e.Button = MouseButtons.Left Then
            If b = 0 Then
                x1 = e.X
                y1 = e.Y
                b = 1
                If c > 0 Then
                    PictureBox1.CreateGraphics.DrawLine(lapiz, x2, y2, x1, y1)
                End If
                c = 1
                puntos(i, 0) = (x1 - orgx) / 10
                puntos(i, 1) = (orgy - y1) / 10
                puntos(i, 2) = i
                puntos(i, 3) = obs
                result = result & puntos(i, 2) + 1 & vbTab & puntos(i, 0) & vbTab & puntos(i, 1) & vbTab & puntos(i, 3) & vbCrLf
            Else
                x2 = e.X
                y2 = e.Y
                b = 0
                puntos(i, 0) = (x2 - orgx) / 10
                puntos(i, 1) = (orgy - y2) / 10
                puntos(i, 2) = i
                puntos(i, 3) = obs
                result = result & puntos(i, 2) + 1 & vbTab & puntos(i, 0) & vbTab & puntos(i, 1) & vbTab & puntos(i, 3) & vbCrLf
                PictureBox1.CreateGraphics.DrawLine(lapiz, x1, y1, x2, y2)
            End If
            i = i + 1
        Else
            sup = i - 1
            PictureBox1.CreateGraphics.DrawLine(lapiz, CType((puntos(inf, 0) * 10) + orgx, Int16), CType(-(puntos(inf, 1) * 10) + orgy, Int16), CType((puntos(sup, 0) * 10) + orgx, Int16), CType(-(puntos(sup, 1) * 10) + orgy, Int16))
            inf = sup + 1
            b = 0
            c = 0
            obs = obs + 1
        End If
        t1.Text = result
    End Sub

    Private Sub PictureBox1_MouseMove(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles PictureBox1.MouseMove
        l1.Text = (e.X - orgx) / 10
        l2.Text = (orgy - e.Y) / 10
        If i = 0 Then
            PictureBox1.CreateGraphics.DrawLine(lapiz2, orgx, 0, orgx, 2 * orgy)
            PictureBox1.CreateGraphics.DrawLine(lapiz2, 0, orgy, 2 * orgx, orgy)
        End If
    End Sub

    Private Sub salida_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles salida.Click
        End
    End Sub

    Private Sub borrar_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles borrar.Click
        Dim j As Integer
        PictureBox1.Visible = False
        PictureBox1.Visible = True
        For j = 0 To 1000
            puntos(j, 0) = 0
            puntos(j, 1) = 0
            puntos(j, 2) = 0
            puntos(j, 3) = 0
        Next
        result = "P" & vbTab & "X" & vbTab & "Y" & vbTab & "OBSTACULO" & vbCrLf
        t1.Text = result
        't2.Text = ""
        b = 0
        c = 0
        i = 0
        obs = 1
        inf = 0
        vt1.Clear()
        vt2.Clear()
    End Sub
    Function choca(ByVal xinf As Double, ByVal yinf As Double, ByVal xsup As Double, ByVal ysup As Double, ByVal ob As Integer, ByVal vert As Integer) As Boolean
        Dim dx As Double = (xsup - xinf) / 100
        Dim dy As Double = (ysup - yinf) / 100
        Dim resp As Double
        Dim xm, ym As Double
        Dim l, m, k As Integer
        Dim a, bsa, bsb As Integer
        xm = xinf
        ym = yinf
        For l = 0 To 99
            'xm = xm + dx
            'ym = ym + dy
            For k = 1 To ob - 1
                a = 0
                bsa = 0
                For m = 0 To vert - 1
                    If puntos(m, 3) = k And bsa = 0 Then
                        bsb = puntos(m, 2)
                        bsa = 1
                    End If
                    If puntos(m, 3) = k And puntos(m + 1, 3) = k Then
                        resp = (ym - puntos(m, 1)) * (puntos(m + 1, 0) - puntos(m, 0)) - (xm - puntos(m, 0)) * (puntos(m + 1, 1) - puntos(m, 1))
                        If resp < -0.0001 Then
                            a = a + 1
                        End If
                    End If
                    If puntos(m, 3) = k And (puntos(m + 1, 3) > k Or puntos(m + 1, 3) < k) Then
                        resp = (ym - puntos(m, 1)) * (puntos(bsb, 0) - puntos(m, 0)) - (xm - puntos(m, 0)) * (puntos(bsb, 1) - puntos(m, 1))
                        If resp < -0.0001 Then
                            a = a + 1
                        End If
                        If a = (puntos(m, 2) - bsb + 1) Then
                            Return True
                        End If
                    End If
                Next
            Next
            xm = xm + dx
            ym = ym + dy
        Next
        Return False
    End Function
    Function chocados(ByVal xinf As Double, ByVal yinf As Double, ByVal ob As Integer, ByVal vert As Integer) As Boolean
        Dim xm, ym As Double
        Dim m, k As Integer
        Dim a, bsa, bsb As Integer
        Dim resp As Double
        xm = xinf
        ym = yinf
        For k = 1 To ob - 1
            a = 0
            bsa = 0
            For m = 0 To vert - 1
                If puntos(m, 3) = k And bsa = 0 Then
                    bsb = puntos(m, 2)
                    bsa = 1
                End If
                If puntos(m, 3) = k And puntos(m + 1, 3) = k Then
                    resp = (ym - puntos(m, 1)) * (puntos(m + 1, 0) - puntos(m, 0)) - (xm - puntos(m, 0)) * (puntos(m + 1, 1) - puntos(m, 1))
                    If resp < -0.0001 Then
                        a = a + 1
                    End If
                End If
                If puntos(m, 3) = k And (puntos(m + 1, 3) > k Or puntos(m + 1, 3) < k) Then
                    resp = (ym - puntos(m, 1)) * (puntos(bsb, 0) - puntos(m, 0)) - (xm - puntos(m, 0)) * (puntos(bsb, 1) - puntos(m, 1))
                    If resp < -0.0001 Then
                        a = a + 1
                    End If
                    If a = (puntos(m, 2) - bsb + 1) Then
                        Return True
                    End If
                End If
            Next
        Next
        Return False
    End Function
    Private Sub roadmap_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles roadmap.Click
        Dim rx1, ry1, bxinf, byinf, bxsup, bysup As Double
        Dim inci, incj, t, ini, final As Integer
        vt1.Clear()
        vt2.Clear()
        While vt1.Count < numno
            rx1 = 20 - (Rnd() * 40)
            ry1 = 20 - (Rnd() * 40)
            If chocados(rx1, ry1, obs, i) = False Then
                vt1.Add(rx1)
                vt2.Add(ry1)
            End If
        End While
        'For inci = 0 To numno - 1
        'For incj = 0 To numno - 1
        'If inci <> incj Then
        'If choca(vt1.Item(inci), vt2.Item(inci), vt1.Item(incj), vt2.Item(incj), obs, i) = False Then
        'PictureBox1.CreateGraphics.DrawLine(lapiz3, CType((vt1.Item(inci) * 10) + orgx, Int16), CType(-(vt2.Item(inci) * 10) + orgy, Int16), CType((vt1.Item(incj) * 10) + orgx, Int16), CType(-(vt2.Item(incj) * 10) + orgy, Int16))
        'End If
        'End If
        'Next
        'Next
    End Sub

    Private Sub CheckBox1_CheckedChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles CheckBox1.CheckedChanged
        Dim inci, incj, k, a, bsa, bsb, m As Integer
        If CheckBox1.Checked = True Then
            For inci = 0 To numno - 1
                For incj = 0 To numno - 1
                    If inci <> incj Then
                        If choca(vt1.Item(inci), vt2.Item(inci), vt1.Item(incj), vt2.Item(incj), obs, i) = False Then
                            PictureBox1.CreateGraphics.DrawLine(lapiz3, CType((vt1.Item(inci) * 10) + orgx, Int16), CType(-(vt2.Item(inci) * 10) + orgy, Int16), CType((vt1.Item(incj) * 10) + orgx, Int16), CType(-(vt2.Item(incj) * 10) + orgy, Int16))
                        End If
                    End If
                Next
            Next
            For k = 1 To obs - 1
                a = 0
                bsa = 0
                For m = 0 To i - 1
                    If puntos(m, 3) = k And bsa = 0 Then
                        bsb = puntos(m, 2)
                        bsa = 1
                    End If
                    If puntos(m, 3) = k And puntos(m + 1, 3) = k Then
                        PictureBox1.CreateGraphics.DrawLine(lapiz, CType((puntos(m, 0) * 10) + orgx, Int16), CType(-(puntos(m, 1) * 10) + orgy, Int16), CType((puntos(m + 1, 0) * 10) + orgx, Int16), CType(-(puntos(m + 1, 1) * 10) + orgy, Int16))
                    End If
                    If puntos(m, 3) = k And (puntos(m + 1, 3) > k Or puntos(m + 1, 3) < k) Then
                        PictureBox1.CreateGraphics.DrawLine(lapiz, CType((puntos(m, 0) * 10) + orgx, Int16), CType(-(puntos(m, 1) * 10) + orgy, Int16), CType((puntos(bsb, 0) * 10) + orgx, Int16), CType(-(puntos(bsb, 1) * 10) + orgy, Int16))
                    End If
                Next
            Next
        End If
        If CheckBox1.Checked = False Then
            PictureBox1.Visible = False
            PictureBox1.Visible = True
        End If
    End Sub

    Private Sub Button1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button1.Click
        Dim za As Boolean
        Dim ra, rb, rc
        Dim td, ts As Integer
        Dim nd, binf, bsup, jk As Integer
        Dim bxinf, byinf, bxsup, bysup As Double
        Dim t, ini, final As Integer
        bxinf = TextBox1.Text
        byinf = TextBox2.Text
        bxsup = TextBox3.Text
        bysup = TextBox4.Text
        PictureBox1.CreateGraphics.FillEllipse(Brushes.Black, CType((bxinf * 10) + orgx, Int16) - 4, CType(-(byinf * 10) + orgy, Int16) - 4, 8, 8)
        PictureBox1.CreateGraphics.FillEllipse(Brushes.Black, CType((bxsup * 10) + orgx, Int16) - 4, CType(-(bysup * 10) + orgy, Int16) - 4, 8, 8)
        ini = 1234567
        final = 1234567

        For t = 0 To numno - 1
            za = choca(bxinf, byinf, vt1.Item(t), vt2.Item(t), obs, i)
            If za = False Then
                ini = t
                t = i + 100000
            End If
        Next
        For t = 0 To numno - 1
            za = choca(bxsup, bysup, vt1.Item(t), vt2.Item(t), obs, i)
            If za = False Then 'And t <> ini Then
                final = t
                t = i + 100000
            End If
        Next
        If ini <> 1234567 And final <> 1234567 Then
            PictureBox1.CreateGraphics.DrawLine(lapiz4, CType((bxinf * 10) + orgx, Int16), CType(-(byinf * 10) + orgy, Int16), CType((vt1.Item(ini) * 10) + orgx, Int16), CType(-(vt2.Item(ini) * 10) + orgy, Int16))
            PictureBox1.CreateGraphics.DrawLine(lapiz4, CType((bxsup * 10) + orgx, Int16), CType(-(bysup * 10) + orgy, Int16), CType((vt1.Item(final) * 10) + orgx, Int16), CType(-(vt2.Item(final) * 10) + orgy, Int16))
        Else
            MsgBox("no hay solucion")
            cola.Clear()
            colb.Clear()
            colc.Clear()
            cole.Clear()
            Return
        End If
        colb.Enqueue(ini)
        cole.Push(ini)
        If ini = final Then
            cola.Enqueue(ini)
            colb.Enqueue(ini)
            cole.Push(ini)
        End If
        While colb.Contains(final) = False
            For t = 0 To numno - 1
                If t = ini Then
                    za = True
                Else
                    za = choca(vt1.Item(ini), vt2.Item(ini), vt1.Item(t), vt2.Item(t), obs, i)
                End If
                If za = False And colb.Contains(t) = False And colb.Contains(final) = False Then
                    cola.Enqueue(t)
                    colb.Enqueue(t)
                    cole.Push(t)
                    'PictureBox1.CreateGraphics.DrawLine(lapiz4, CType((puntos(ini, 0) * 10) + orgx, Int16), CType(-(puntos(ini, 1) * 10) + orgy, Int16), CType((puntos(t, 0) * 10) + orgx, Int16), CType(-(puntos(t, 1) * 10) + orgy, Int16))
                End If
            Next
            If cola.Count = 0 Then
                MsgBox("no hay solucion")
                cola.Clear()
                colb.Clear()
                colc.Clear()
                cole.Clear()
                Return
            Else
                ini = cola.Dequeue
            End If
        End While
        rc = cole.Count
        td = 0
        While cole.Count > 0
            ra = cole.Pop()
            colc.Enqueue(ra)
            ts = cole.Count
            td = td + 1
            rb = cole.ToArray()
            For t = 0 To ts - 1
                za = choca(vt1.Item(ra), vt2.Item(ra), vt1.Item(rb(t)), vt2.Item(rb(t)), obs, i)
                If za = False Then
                    rc = t
                End If
            Next
            If cole.Count > 0 Then
                For t = 1 To rc
                    cole.Pop()
                Next
            End If
        End While
        rb = colc.ToArray
        nd = colc.Count - 1
        binf = colc.Dequeue
        bsup = colc.Dequeue
        For jk = 1 To nd
            PictureBox1.CreateGraphics.DrawLine(lapiz4, CType((vt1.Item(binf) * 10) + orgx, Int16), CType(-(vt2.Item(binf) * 10) + orgy, Int16), CType((vt1.Item(bsup) * 10) + orgx, Int16), CType(-(vt2.Item(bsup) * 10) + orgy, Int16))
            If jk < nd Then
                binf = bsup
                bsup = colc.Dequeue
            End If
        Next
        'vt1.Clear()
        'vt2.Clear()
        cola.Clear()
        colb.Clear()
        colc.Clear()
        cole.Clear()
    End Sub
End Class