From b3068baa114fc4ed29bd0901d8f5c9fc4b29a77f Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Mon, 25 Apr 2016 13:06:09 -0300 Subject: [PATCH] AP_Math: add geodesic_grid toolset That was used to aid development AP_GeodesicGrid and understanding its concepts. --- .../AP_Math/tools/geodesic_grid/README.md | 6 + .../tools/geodesic_grid/geodesic_grid.py | 291 ++++++++++++++++++ .../tools/geodesic_grid/icosahedron.py | 198 ++++++++++++ libraries/AP_Math/tools/geodesic_grid/plot.py | 104 +++++++ 4 files changed, 599 insertions(+) create mode 100644 libraries/AP_Math/tools/geodesic_grid/README.md create mode 100755 libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py create mode 100644 libraries/AP_Math/tools/geodesic_grid/icosahedron.py create mode 100644 libraries/AP_Math/tools/geodesic_grid/plot.py diff --git a/libraries/AP_Math/tools/geodesic_grid/README.md b/libraries/AP_Math/tools/geodesic_grid/README.md new file mode 100644 index 0000000000..311036892b --- /dev/null +++ b/libraries/AP_Math/tools/geodesic_grid/README.md @@ -0,0 +1,6 @@ +# Geodesic Grid Tool # + +This folder a toolset for helping understanding the concepts used by +[`AP_GeodesicGrid`](../../AP_GeodesicGrid.cpp) as well as for aiding its +development. The main script is named `geodesic_grid.py`. Use `geodesic_grid.py +--help` to know how to use it. diff --git a/libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py b/libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py new file mode 100755 index 0000000000..6fe55c12c4 --- /dev/null +++ b/libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py @@ -0,0 +1,291 @@ +#!/usr/bin/python + +# Copyright (C) 2016 Intel Corporation. All rights reserved. +# +# This file is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the +# Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This file is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . +from __future__ import print_function +import argparse +import numpy as np +import sys + +import icosahedron as ico + +def print_code_gen_notice(): + print("/* This was generated with") + print(" * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */") + +def header_neighbor_umbrella(index): + t = ico.triangles[0] + a, b, c = t + + triangle, edge = ( + ( t, ( a, b)), + ( t, ( b, c)), + ( t, ( c, a)), + (-t, (-a, -b)), + (-t, (-b, -c)), + (-t, (-c, -a)), + )[index] + + return ico.neighbor_umbrella(triangle, edge), edge + +parser = argparse.ArgumentParser( + description=""" +Utility script for helping to understand concepts used by AP_GeodesicGrid as +well as for aiding its development. + +When passing a vertex as argument to one of the options, the valid values for +the coordinates are 0, -1, 1, g and -g, where g is the golden ratio. +""", +) + +parser.add_argument( + '-p', '--plot', + action='store_true', + help=""" +Plot results when applicable. +""", +) + +parser.add_argument( + '-s', '--plot-subtriangles', + action='store_true', + help=""" +Plot subtriangles as well. This implies -p. +""", +) + +parser.add_argument( + '--icosahedron', + action='store_true', + help='Get the icosahedron triangles.', +) + +parser.add_argument( + '-t', '--triangle', + action='append', + type=int, + nargs='+', + metavar='INDEX', + help=""" +Get the icosahedron triangle at INDEX. +""", +) + +parser.add_argument( + '-u', '--umbrella', + action='append', + nargs=3, + metavar=('X', 'Y', 'Z'), + help=""" +Get the umbrella with pivot denoted by (X, Y, Z). The pivot must be one of the +icosahedron's vertices. +""", +) + +parser.add_argument( + '-n', '--neighbor-umbrella', + action='append', + nargs='+', + metavar='INDEX', + help=""" +Get the neighbor umbrella at INDEX as described by _neighbor_umbrellas in +AP_GeodesicGrid.h. The special value "all" for INDEX is also accepted, which +will make it ignore other indexes passed and get all neighbor umbrellas for +that member. +""", +) + +parser.add_argument( + '--neighbor-umbrella-gen', + action='store_true', + help=""" +Generate C++ code for the initialization of the member _neighbor_umbrellas +described in AP_GeodesicGrid.h. +""", +) + +parser.add_argument( + '--inverses-gen', + action='store_true', + help=""" +Generate C++ code for the initialization of members _inverses and _mid_inverses +declared in AP_GeodesicGrid.h. +""") + + +args = parser.parse_args() + +if args.plot_subtriangles: + args.plot = True + +if args.plot: + import plot + +polygons_to_plot = [] + +if args.triangle: + indexes = [] + for l in args.triangle: + indexes += l + + for i in indexes: + if 0 > i or i >= len(ico.triangles): + print( + 'Triangle index must be in the range [0,%d)' % len(ico.triangles), + file=sys.stderr, + ) + sys.exit(1) + + print(ico.triangles[i]) + if args.plot: + plot.polygon(ico.triangles[i]) + +if args.umbrella: + for pivot in args.umbrella: + for i, x in enumerate(pivot): + if x == 'g': + x = ico.g + elif x == '-g': + x = -ico.g + else: + try: + x = int(x) + if x not in (0, -1, 1): + raise ValueError() + except ValueError: + print( + 'umbrella: invalid pivot coordinate: %s' % str(x), + file=sys.stderr, + ) + sys.exit(1) + pivot[i] = x + + pivot = ico.Vertex(*pivot) + if pivot not in ico.vertices: + print( + 'umbrella: invalid pivot:', pivot, + file=sys.stderr, + ) + sys.exit(1) + u = ico.umbrella(pivot) + + print("Components of the umbrella of %s:" % str(pivot)) + for c in u.components: + print(" %s" % str(c)) + + if args.plot: + plot.polygons(u.components) + +if args.neighbor_umbrella: + indexes = [] + for l in args.neighbor_umbrella: + indexes += l + + if 'all' in indexes: + indexes = range(6) + else: + for i, arg in enumerate(indexes): + try: + arg = int(arg) + if arg not in range(6): + raise ValueError() + except ValueError: + print( + 'neighbor_umbrella: invalid index %s' % str(arg), + file=sys.stderr, + ) + sys.exit(1) + indexes[i] = arg + + for i in indexes: + u, order_edge = header_neighbor_umbrella(i) + print("Header umbrella %d:" % i) + print(" Pivot:", u.pivot) + for i in range(5): + print(" Vertex %d:" % i, u.vertex(i, order_edge)) + for i in range(5): + print(" Component %d:" % i, u.component(i, order_edge)) + + if args.plot: + plot.polygons(u.components) + +if args.neighbor_umbrella_gen: + print("Header neighbor umbrellas code generation:") + print_code_gen_notice() + print("const struct AP_GeodesicGrid::neighbor_umbrella") + print("AP_GeodesicGrid::_neighbor_umbrellas[3]{") + for i in range(6): + u, order_edge = header_neighbor_umbrella(i) + + components = tuple( + ico.triangles.index(u.component(i, order_edge)) for i in range(5) + ) + + def vi_cj(i, j): + v = u.vertex(i, order_edge) + t = u.component(j, order_edge) + return t.index(v) + + vi_cj_values = tuple( + vi_cj(a, b) for a, b in ((0, 0), (1, 1), (2, 1), (4, 4), (0, 4)) + ) + + print(" {{%s}, %s}," % ( + ", ".join("%2d" % i for i in components), + ", ".join(str(i) for i in vi_cj_values), + )) + print("};") + +if args.inverses_gen: + print("Header inverses code generation:") + print_code_gen_notice() + print("const Matrix3f AP_GeodesicGrid::_inverses[10]{") + for i in range(10): + a, b, c = ico.triangles[i] + m = np.matrix(( + (a.x, b.x, c.x), + (a.y, b.y, c.y), + (a.z, b.z, c.z), + )).getI() + print(" {{%9.6ff, %9.6ff, %9.6ff}," % (m[0,0], m[0,1], m[0,2])) + print(" {%9.6ff, %9.6ff, %9.6ff}," % (m[1,0], m[1,1], m[1,2])) + print(" {%9.6ff, %9.6ff, %9.6ff}}," % (m[2,0], m[2,1], m[2,2])) + print("};") + print() + print_code_gen_notice() + print("const Matrix3f AP_GeodesicGrid::_mid_inverses[10]{") + for i in range(10): + a, b, c = ico.triangles[i] + ma, mb, mc = .5 * (a + b), .5 * (b + c), .5 * (c + a) + m = np.matrix(( + (ma.x, mb.x, mc.x), + (ma.y, mb.y, mc.y), + (ma.z, mb.z, mc.z), + )).getI() + print(" {{%9.6ff, %9.6ff, %9.6ff}," % (m[0,0], m[0,1], m[0,2])) + print(" {%9.6ff, %9.6ff, %9.6ff}," % (m[1,0], m[1,1], m[1,2])) + print(" {%9.6ff, %9.6ff, %9.6ff}}," % (m[2,0], m[2,1], m[2,2])) + print("};") + + +if args.icosahedron: + print('Icosahedron:') + for i, t in enumerate(ico.triangles): + print(' %s' % str(t)) + if args.plot: + plot.polygons(ico.triangles) + +if args.plot: + plot.show(subtriangles=args.plot_subtriangles) diff --git a/libraries/AP_Math/tools/geodesic_grid/icosahedron.py b/libraries/AP_Math/tools/geodesic_grid/icosahedron.py new file mode 100644 index 0000000000..3418ed2836 --- /dev/null +++ b/libraries/AP_Math/tools/geodesic_grid/icosahedron.py @@ -0,0 +1,198 @@ +# Copyright (C) 2016 Intel Corporation. All rights reserved. +# +# This file is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the +# Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This file is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . +import math +from scipy.constants import golden as g + +class Vertex(tuple): + def __new__(cls, x, y, z): + instance = tuple.__new__(cls, (x, y, z)) + instance.x = x + instance.y = y + instance.z = z + return instance + + def __repr__(self): + return "(" + ",".join(Vertex._print_map.get(x, str(x)) for x in self) + ")" + + def __str__(self): + return self.__repr__() + + def __neg__(self): + return Vertex(-self.x, -self.y, -self.z) + + def __add__(self, other): + return Vertex(self.x + other.x, self.y + other.y, self.z + other.z) + + def __sub__(self, other): + return Vertex(self.x - other.x, self.y - other.y, self.z - other.z) + + def __mul__(self, s): + return Vertex(s * self.x, s * self.y, s * self.z) + __rmul__ = __mul__ + + def length(self): + return math.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2) + + def normalized(self): + return (1.0 / self.length()) * self + +class Triangle(tuple): + def __new__(cls, a, b, c): + instance = tuple.__new__(cls, (a, b, c)) + instance.a = a + instance.b = b + instance.c = c + return instance + + def __neg__(self): + return Triangle(-self.a, -self.b, -self.c) + + def __str__(self): + if self in triangles: + i = triangles.index(self) + return "Triangle %2d: %s" % (i, self.__repr__()) + else: + return self.__repr__() + +Vertex._print_map = { + g: ' g', -g: '-g', 1: ' 1', -1: '-1', 0: ' 0', +} + +vertices = tuple( + Vertex(x, y, z) for x, y, z in ( + ( g, 1, 0), + ( g,-1, 0), + (-g, 1, 0), + (-g,-1, 0), + ( 1, 0, g), + (-1, 0, g), + ( 1, 0,-g), + (-1, 0,-g), + ( 0, g, 1), + ( 0, g,-1), + ( 0,-g, 1), + ( 0,-g,-1), + ) +) + +_first_half = ( + Triangle(Vertex(-g, 1, 0), Vertex(-1, 0,-g), Vertex(-g,-1, 0)), + Triangle(Vertex(-1, 0,-g), Vertex(-g,-1, 0), Vertex( 0,-g,-1)), + Triangle(Vertex(-g,-1, 0), Vertex( 0,-g,-1), Vertex( 0,-g, 1)), + Triangle(Vertex(-1, 0,-g), Vertex( 0,-g,-1), Vertex( 1, 0,-g)), + Triangle(Vertex( 0,-g,-1), Vertex( 0,-g, 1), Vertex( g,-1, 0)), + Triangle(Vertex( 0,-g,-1), Vertex( 1, 0,-g), Vertex( g,-1, 0)), + Triangle(Vertex( g,-1, 0), Vertex( 1, 0,-g), Vertex( g, 1, 0)), + Triangle(Vertex( 1, 0,-g), Vertex( g, 1, 0), Vertex( 0, g,-1)), + Triangle(Vertex( 1, 0,-g), Vertex( 0, g,-1), Vertex(-1, 0,-g)), + Triangle(Vertex( 0, g,-1), Vertex(-g, 1, 0), Vertex(-1, 0,-g)), +) + +_second_half = tuple(-t for t in _first_half) + +triangles = _first_half + _second_half + +_neighbor_triangle_data = {} +def neighbor_triangle(t, edge): + """ Return the neighbor triangle of t with respect to edge = (a, b) """ + e = frozenset(edge) + if (t, e) in _neighbor_triangle_data: + return _neighbor_triangle_data[(t, e)] + + a, b = edge + if a not in t or b not in t: + return None + + for w in triangles: + if a in w and b in w and w != t: + _neighbor_triangle_data[(t, e)] = w + return w + + return None + +class _Umbrella: + def __init__(self, pivot): + self.pivot = pivot + self.components = frozenset(t for t in triangles if pivot in t) + + all_vertices = set() + for t in self.components: + for v in t: + if v != pivot: + all_vertices.add(v) + self.all_vertices = frozenset(all_vertices) + + self._vertex_data = {} + self._component_data = {} + + def vertex(self, i, ordered_edge): + """ Return the i-th vertex with respect to ordered_edge = (a, b) """ + a, b = ordered_edge + if a not in self.all_vertices: + return None + if b not in self.all_vertices: + return None + + if i == 0: + return a + if i == 1: + return b + + if (i, a, b) in self._vertex_data: + return self._vertex_data[(i, a, b)] + + previous = self.vertex(i - 1, ordered_edge) + comp = self.component(i - 2, ordered_edge) + neighbor = neighbor_triangle(comp, (self.pivot, previous)) + + for v in neighbor: + if v not in (self.pivot, previous): + self._vertex_data[(i, a, b)] = v + return v + return None + + def component(self, i, ordered_edge): + """ Return the i-th component with respect to ordered_edge = (a, b) """ + a, b = ordered_edge + if (i, a, b) in self._component_data: + return self._component_data[(i, a, b)] + + vi = self.vertex(i, ordered_edge) + vj = self.vertex(i + 1, ordered_edge) + + for t in self.components: + if vi in t and vj in t: + self._component_data[(i, a, b)] = t + return t + return None + +_umbrelas = {} +def umbrella(pivot): + if pivot not in vertices: + return None + + if pivot not in _umbrelas: + _umbrelas[pivot] = _Umbrella(pivot) + return _umbrelas[pivot] + +def neighbor_umbrella(t, edge): + neighbor = neighbor_triangle(t, edge) + if not neighbor: + return None + + for pivot in neighbor: + if pivot in edge: + continue + return umbrella(pivot) diff --git a/libraries/AP_Math/tools/geodesic_grid/plot.py b/libraries/AP_Math/tools/geodesic_grid/plot.py new file mode 100644 index 0000000000..ac9710aaa5 --- /dev/null +++ b/libraries/AP_Math/tools/geodesic_grid/plot.py @@ -0,0 +1,104 @@ +# Copyright (C) 2016 Intel Corporation. All rights reserved. +# +# This file is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the +# Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This file is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . +import matplotlib.pyplot as plt +import matplotlib.patches as mpatches + +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection + +import icosahedron as ico + +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim3d(-2, 2) +ax.set_ylim3d(-2, 2) +ax.set_zlim3d(-2, 2) + +ax.set_xlabel('x') +ax.set_ylabel('y') +ax.set_zlabel('z') + +ax.invert_zaxis() +ax.invert_xaxis() + +ax.set_aspect('equal') + +added_polygons = set() + +def polygons(polygons): + for p in polygons: + polygon(p) + + +def polygon(polygon): + added_polygons.add(polygon) + +def show(subtriangles=False): + polygons = [] + facecolors = [] + + for p in added_polygons: + try: + i = ico.triangles.index(p) + except ValueError: + polygons.append(p) + continue + + if subtriangles: + a, b, c = p + + # project the middle points to the sphere + alpha = a.length() / (2.0 * ico.g) + ma, mb, mc = alpha * (a + b), alpha * (b + c), alpha * (c + a) + + polygons.append(ico.Triangle(ma, mb, mc)) + facecolors.append('#CCCCCC') + + polygons.append(ico.Triangle( a, ma, mc)) + facecolors.append('#CCE5FF') + + polygons.append(ico.Triangle(ma, b, mb)) + facecolors.append('#E5FFCC') + + polygons.append(ico.Triangle(mc, mb, c)) + facecolors.append('#FFCCCC') + else: + polygons.append(p) + facecolors.append('#DDDDDD') + + mx = my = mz = 0 + for x, y, z in p: + mx += x + my += y + mz += z + ax.text(mx / 2.6, my /2.6, mz / 2.6, i, color='#444444') + + ax.add_collection3d(Poly3DCollection( + polygons, + facecolors=facecolors, + edgecolors="#777777", + )) + + if subtriangles: + ax.legend( + handles=( + mpatches.Patch(color='#CCCCCC', label='Sub-triangle #0'), + mpatches.Patch(color='#CCE5FF', label='Sub-triangle #1'), + mpatches.Patch(color='#E5FFCC', label='Sub-triangle #2'), + mpatches.Patch(color='#FFCCCC', label='Sub-triangle #3'), + ), + ) + + plt.show()