AUVpathplanning/utils.py at master · michaelkapteyn/AUVpathplanning · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

"""
Return the distance from node1 to node2
"""
def dist(node1, node2):
    return (((node1.position[0] - node2.position[0]) ** 2) + ((node1.position[1] - node2.position[1]) ** 2)) **0.5

def InspectReefData3D():
    def StandardReefFunction(x, y):
        x = (x-0.5 *100)/15
        y = (y-0.5 *100)/15
        return (1.0 - x / 2.0 + x ** 5.0 + y ** 3.0) * np.exp(-x ** 2.0 - y ** 2.0) - 10.0

    fig = plt.figure()
    ax = fig.gca(projection='3d')

    # Make data.
    X = np.arange(0, 100, 0.25)
    Y = np.arange(0, 100, 0.25)
    X, Y = np.meshgrid(X, Y)
    Z1 = StandardReefFunction(X, Y)

    # Plot the surface.
    surf = ax.plot_surface(X, Y, Z1, cmap='gray',alpha=1)
    ax.set_aspect('equal', 'box')
    ax.grid(False)

    plt.show()

def InspectReefData2D():
    def StandardReefFunction(x, y):
        x = (x-0.5 *100)/15
        y = (y-0.5 *100)/15
        return (1.0 - x / 2.0 + x ** 5.0 + y ** 3.0) * np.exp(-x ** 2.0 - y ** 2.0) - 10.0

    fig, ax = plt.subplots()

    # Make data.
    X = np.arange(0, 100, 0.25)
    Y = np.arange(0, 100, 0.25)
    X, Y = np.meshgrid(X, Y)


    # reef levels
    plt.contourf(X, Y, StandardReefFunction(X, Y), 8, alpha=.75, cmap='gray')

    # contour lines
    ax.contourf(X, Y, StandardReefFunction(X, Y),levels = [-9.5, 10], colors = 'red', linestyles = 'solid',zorder=10)

    # some formatting
    ax.set_aspect('equal', 'box')
    ax.set_xlim([0, 100])
    ax.set_ylim([0, 100])
    plt.title('2-D Cut at Depth z = -9.5')

    plt.show()

def InspectReefData2DComplete():
    UnknownRegions = { \
                                   0.8: [(50, 15), (43, 25), (80, 25), (88, 19), (90,18)], \
                                   0.4: [(80, 84), (95, 80), (95, 92), (76, 95)], \
                                   0.1: [(11, 8), (40, 0), (40, 17), (11, 11)] \
                                   }

    def StandardReefFunction(x, y):
        x = (x-0.5 *100)/15
        y = (y-0.5 *100)/15
        return (1.0 - x / 2.0 + x ** 5.0 + y ** 3.0) * np.exp(-x ** 2.0 - y ** 2.0) - 10.0

    fig, ax = plt.subplots()

    # Make data.
    X = np.arange(0, 100, 0.25)
    Y = np.arange(0, 100, 0.25)
    X, Y = np.meshgrid(X, Y)


    # reef levels
    plt.contourf(X, Y, StandardReefFunction(X, Y), 8, alpha=.75, cmap='gray')

    # contour lines
    ax.contourf(X, Y, StandardReefFunction(X, Y),levels = [-9.5, 10], colors = 'red', linestyles = 'solid',zorder=10)

    # unknown regions
    from matplotlib import path as mpath
    import matplotlib.patches as patches

    for risk_key in UnknownRegions:
        unknown_region = patches.PathPatch(mpath.Path(UnknownRegions[risk_key]), facecolor='red', lw=0.0001, alpha = risk_key)
        ax.add_patch(unknown_region)

    # some formatting
    ax.set_aspect('equal', 'box')
    ax.set_xlim([0, 100])
    ax.set_ylim([0, 100])
    plt.title('2-D Cut at Depth z = -9.5')

    plt.show()