|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +# a 3D Bezier cylinder |
| 4 | +# - The 3D bezier curve |
| 5 | +# - Start and end radii |
| 6 | +# |
| 7 | +# Can: |
| 8 | +# - Evaluate points along the curve |
| 9 | +# - Make a cylinderical mesh |
| 10 | +# - Project self into an image |
| 11 | + |
| 12 | +import numpy as np |
| 13 | +from json import load, dump |
| 14 | + |
| 15 | + |
| 16 | +class BezierCyl3D: |
| 17 | + |
| 18 | + def __init__(self, p1=(0.0, 0.0, 0.0), p2=(0.5, 0.75, 0.5), p3=(1.0, 1.0, 1.0), start_radius=10.0, end_radius=20.0): |
| 19 | + """ Initialize a 3D curve, built from a quadratic Bezier with radii |
| 20 | + @param p1 - start pt, x,y,z |
| 21 | + @param p2 - mid pt, x,y,z |
| 22 | + @param p3 - end pt x,y,z |
| 23 | + @param start_radius - starting radius |
| 24 | + @param end_radius - ending radius""" |
| 25 | + |
| 26 | + # Information about the current branch/trunk |
| 27 | + self.pt1 = p1 |
| 28 | + self.pt2 = p2 |
| 29 | + self.pt3 = p3 |
| 30 | + self.start_radii = start_radius |
| 31 | + self.end_radii = end_radius |
| 32 | + |
| 33 | + # Drawing/mesh creation parameters |
| 34 | + self.n_along = 10 |
| 35 | + self.n_around = 64 |
| 36 | + |
| 37 | + self.vertex_locs = np.zeros((self.n_along, self.n_around, 3)) |
| 38 | + self.make_mesh() |
| 39 | + |
| 40 | + def n_vertices(self): |
| 41 | + return self.n_along * self.n_around |
| 42 | + |
| 43 | + def set_dims(self, n_along=10, n_radial=64): |
| 44 | + self.n_along = n_along |
| 45 | + self.n_around = n_radial |
| 46 | + self.vertex_locs = np.zeros((self.n_along, self.n_around, 3)) |
| 47 | + |
| 48 | + def set_pts(self, pt1, pt2, pt3): |
| 49 | + """ Turn into numpy array |
| 50 | + @param pt1 First point |
| 51 | + @param pt2 Mid point |
| 52 | + @param pt3 End point |
| 53 | + """ |
| 54 | + self.pt1 = pt1 |
| 55 | + self.pt2 = pt2 |
| 56 | + self.pt3 = pt3 |
| 57 | + |
| 58 | + def set_pts_from_pt_tangent(self, pt1, vec1, pt3): |
| 59 | + """Set the points from a starting point/tangent |
| 60 | + @param pt1 - starting point |
| 61 | + @param vec1 - starting tangent |
| 62 | + @param pt3 - ending point""" |
| 63 | + # v = - 2 * p0 + 2 * p1 |
| 64 | + # v/2 + p2 = p1 |
| 65 | + mid_pt = np.array(pt1) + np.array(vec1) * 0.5 |
| 66 | + self.set_pts(pt1, mid_pt, pt3) |
| 67 | + |
| 68 | + def set_radii(self, start_radius=1.0, end_radius=1.0): |
| 69 | + """ Set the radius of the branch |
| 70 | + @param start_radius - radius at pt1 |
| 71 | + @param end_radius - radius at pt3""" |
| 72 | + self.start_radii = start_radius |
| 73 | + self.end_radii = end_radius |
| 74 | + |
| 75 | + def pt_axis(self, t): |
| 76 | + """ Return a point along the bezier |
| 77 | + @param t in 0, 1 |
| 78 | + @return 2 or 3d point""" |
| 79 | + pts_axis = np.array([self.pt1[i] * (1-t) ** 2 + 2 * (1-t) * t * self.pt2[i] + t ** 2 * self.pt3[i] for i in range(0, 3)]) |
| 80 | + return pts_axis.transpose() |
| 81 | + # return self.p0 * (1-t) ** 2 + 2 * (1-t) * t * self.p1 + t ** 2 * self.p2 |
| 82 | + |
| 83 | + def tangent_axis(self, t): |
| 84 | + """ Return the tangent vec |
| 85 | + @param t in 0, 1 |
| 86 | + @return 3d vec""" |
| 87 | + vec_axis = [2 * t * (self.pt1[i] - 2.0 * self.pt2[i] + self.pt3[i]) - 2 * self.pt1[i] + 2 * self.pt2[i] for i in range(0, 3)] |
| 88 | + return np.array(vec_axis) |
| 89 | + |
| 90 | + def binormal_axis(self, t): |
| 91 | + """ Return the bi-normal vec, cross product of first and second derivative |
| 92 | + @param t in 0, 1 |
| 93 | + @return 3d vec""" |
| 94 | + vec_tang = self.tangent_axis(t) |
| 95 | + vec_tang = vec_tang / np.linalg.norm(vec_tang) |
| 96 | + vec_second_deriv = np.array([2 * (self.pt1[i] - 2.0 * self.pt2[i] + self.pt3[i]) for i in range(0, 3)]) |
| 97 | + |
| 98 | + vec_binormal = np.cross(vec_tang, vec_second_deriv) |
| 99 | + if np.isclose(np.linalg.norm(vec_second_deriv), 0.0): |
| 100 | + for i in range(0, 2): |
| 101 | + if not np.isclose(vec_tang[i], 0.0): |
| 102 | + vec_binormal[i] = -vec_tang[(i + 1) % 3] |
| 103 | + vec_binormal[(i + 1) % 3] = vec_tang[i] |
| 104 | + vec_binormal[(i + 2) % 3] = 0.0 |
| 105 | + break |
| 106 | + |
| 107 | + return vec_binormal / np.linalg.norm(vec_binormal) |
| 108 | + |
| 109 | + def frenet_frame(self, t): |
| 110 | + """ Return the matrix that will take the point 0,0,0 to crv(t) with x axis along tangent, y along binormal |
| 111 | + @param t - t value |
| 112 | + @return 4x4 transformation matrix""" |
| 113 | + pt_center = self.pt_axis(t) |
| 114 | + vec_tang = self.tangent_axis(t) |
| 115 | + vec_tang = vec_tang / np.linalg.norm(vec_tang) |
| 116 | + vec_binormal = self.binormal_axis(t) |
| 117 | + vec_x = np.cross(vec_tang, vec_binormal) |
| 118 | + |
| 119 | + mat = np.identity(4) |
| 120 | + mat[0:3, 3] = pt_center[0:3] |
| 121 | + mat[0:3, 0] = vec_x.transpose() |
| 122 | + mat[0:3, 1] = vec_binormal.transpose() |
| 123 | + mat[0:3, 2] = vec_tang.transpose() |
| 124 | + |
| 125 | + return mat |
| 126 | + |
| 127 | + def _calc_radii(self): |
| 128 | + """ Calculate the radii along the branch |
| 129 | + @return a numpy array of radii""" |
| 130 | + radii = np.linspace(self.start_radii, self.end_radii, self.n_along) |
| 131 | + return radii |
| 132 | + |
| 133 | + def _calc_cyl_vertices(self): |
| 134 | + """Calculate the cylinder vertices""" |
| 135 | + pt = np.ones(shape=(4)) |
| 136 | + radii = self._calc_radii() |
| 137 | + |
| 138 | + for it, t in enumerate(np.linspace(0, 1.0, self.n_along)): |
| 139 | + mat = self.frenet_frame(t) |
| 140 | + pt[0] = 0 |
| 141 | + pt[1] = 0 |
| 142 | + pt[2] = 0 |
| 143 | + for itheta, theta in enumerate(np.linspace(0, np.pi * 2.0, self.n_around, endpoint=False)): |
| 144 | + pt[0] = np.cos(theta) * radii[it] |
| 145 | + pt[1] = np.sin(theta) * radii[it] |
| 146 | + pt[2] = 0 |
| 147 | + pt_on_crv = mat @ pt |
| 148 | + |
| 149 | + self.vertex_locs[it, itheta, :] = pt_on_crv[0:3].transpose() |
| 150 | + |
| 151 | + def make_mesh(self): |
| 152 | + """ Make a 3D generalized cylinder """ |
| 153 | + self._calc_cyl_vertices() |
| 154 | + |
| 155 | + def write_mesh(self, fname): |
| 156 | + """Write out an obj file with the appropriate geometry |
| 157 | + @param fname - file name (should end in .obj""" |
| 158 | + with open(fname, "w") as fp: |
| 159 | + fp.write(f"# Branch\n") |
| 160 | + for it in range(0, self.n_along): |
| 161 | + for ir in range(0, self.n_around): |
| 162 | + fp.write(f"v ") |
| 163 | + fp.write(" ".join(["{:.6}"] * 3).format(*self.vertex_locs[it, ir, :])) |
| 164 | + fp.write(f"\n") |
| 165 | + for it in range(0, self.n_along - 1): |
| 166 | + i_curr = it * self.n_around + 1 |
| 167 | + i_next = (it+1) * self.n_around + 1 |
| 168 | + for ir in range(0, self.n_around): |
| 169 | + ir_next = (ir + 1) % self.n_around |
| 170 | + fp.write(f"f {i_curr + ir} {i_next + ir_next} {i_curr + ir_next} \n") |
| 171 | + fp.write(f"f {i_curr + ir} {i_next + ir} {i_next + ir_next} \n") |
| 172 | + |
| 173 | + def write_json(self, fname): |
| 174 | + """ Write out to json - does NOT write out the vertices for the mesh, use write_mesh for that |
| 175 | + @param fname - file name""" |
| 176 | + fix_nparray = [] |
| 177 | + for k, v in self.__dict__.items(): |
| 178 | + try: |
| 179 | + if v.size == 3: |
| 180 | + fix_nparray.append([k, v]) |
| 181 | + setattr(self, k, [float(x) for x in v]) |
| 182 | + except AttributeError: |
| 183 | + pass |
| 184 | + |
| 185 | + with open(fname, "w") as f: |
| 186 | + dump(self.__dict__, f, indent=2) |
| 187 | + |
| 188 | + for fix in fix_nparray: |
| 189 | + setattr(self, fix[0], fix[1]) |
| 190 | + |
| 191 | + @staticmethod |
| 192 | + def read_json(fname, bezier_crv=None, compute_mesh=True): |
| 193 | + """ Read back in from json file |
| 194 | + @param fname file name to read from |
| 195 | + @param bezier_crv - an existing bezier curve to put the data in |
| 196 | + @param compute_mesh - computer the mesh again, y/n |
| 197 | + @return the bezier curve""" |
| 198 | + with open(fname, 'r') as f: |
| 199 | + my_data = load(f) |
| 200 | + if not bezier_crv: |
| 201 | + bezier_crv = BezierCyl3D() |
| 202 | + for k, v in my_data.items(): |
| 203 | + try: |
| 204 | + if len(v) == 3: |
| 205 | + setattr(bezier_crv, k, np.array(v)) |
| 206 | + else: |
| 207 | + setattr(bezier_crv, k, v) |
| 208 | + except TypeError: |
| 209 | + setattr(bezier_crv, k, v) |
| 210 | + |
| 211 | + if compute_mesh: |
| 212 | + bezier_crv.make_mesh() |
| 213 | + return bezier_crv |
| 214 | + |
| 215 | + |
| 216 | +if __name__ == '__main__': |
| 217 | + from os.path import exists |
| 218 | + from os import mkdir |
| 219 | + |
| 220 | + if not exists("data/DebugImages"): |
| 221 | + mkdir("data/DebugImages") |
| 222 | + branch = BezierCyl3D([506.5, 156.0, 0.0], [457.49999996771703, 478.9999900052037, 0.0], [521.5, 318.0, 0.0], |
| 223 | + start_radius=10.5, end_radius=8.25) |
| 224 | + branch.make_mesh() |
| 225 | + branch.write_mesh("data/DebugImages/check_3d_bezier1.obj") |
| 226 | + |
| 227 | + branch = BezierCyl3D([-0.5, 0.0, 0.0], [0.0, 0.1, 0.05], [0.5, 0.0, 0.0], start_radius=0.5, end_radius=0.25) |
| 228 | + branch.make_mesh() |
| 229 | + branch.write_mesh("data/DebugImages/check_3d_bezier2.obj") |
| 230 | + |
| 231 | + branch.set_dims(n_along=30, n_radial=32) |
| 232 | + branch.make_mesh() |
| 233 | + branch.write_mesh("data/DebugImages/check_3d_bezier_more_vs.obj") |
0 commit comments