def test_booleans(): # ============================================================================== # Make a box and a sphere # ============================================================================== box = Box.from_width_height_depth(2, 2, 2) box = Mesh.from_shape(box) box.quads_to_triangles() A = box.to_vertices_and_faces() sphere = Sphere(Point(1, 1, 1), 1) sphere = Mesh.from_shape(sphere, u=30, v=30) sphere.quads_to_triangles() B = sphere.to_vertices_and_faces() # ============================================================================== # Remesh the sphere # ============================================================================== B = remesh(B, 0.3, 10) # ============================================================================== # Compute the boolean mesh # ============================================================================== V, F = boolean_union(A, B) mesh = Mesh.from_vertices_and_faces(V, F)
def test_sphere_scaling(): s = Sphere(Point(1, 1, 1), 10) s.transform(Scale.from_factors([2, 3, 4])) assert s.point.x == 2 assert s.point.y == 3 assert s.point.z == 4 assert s.radius == 40
def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=None): """Returns a position and orientation constraint on the group's end-effector link. Parameters ---------- frame_WCF : :class:`compas.geometry.Frame` The frame from which we create position and orientation constraints. tolerance_position : float The allowed tolerance to the frame's position. (Defined in the robot's units) group: str The planning group for which we specify the constraint. Defaults to the robot's main planning group. Examples -------- >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 >>> goal_constraints = robot.position_constraint_from_frame(frame, tolerance_position) Notes ----- There are many other possibilities of how to create a position and orientation constraints. Checkout :class:`compas_fab.robots.PositionConstraint` and :class:`compas_fab.robots.OrientationConstraint`. """ ee_link = self.get_end_effector_link_name(group) sphere = Sphere(frame_WCF.point, tolerance_position) return PositionConstraint.from_sphere(ee_link, sphere)
def sphere_to_compas(sphere): """Convert a Rhino sphere to a COMPAS sphere. Parameters ---------- sphere: :class:`Rhino.Geometry.Sphere` Returns ------- :class:`compas.geometry.Sphere` """ return Sphere(point_to_compas(sphere.Center), sphere.Radius)
return 'VolSphere({})'.format(str(self.sphere)) def get_distance(self, point): if not isinstance(point, Point): point = Point(*point) d = point.distance_to_point(self.sphere.center) return d - self.sphere.radius def get_distance_numpy(self, x, y, z): import numpy as np d = np.sqrt((x - self.sphere.center.x)**2 + (y - self.sphere.center.y)**2 + (z - self.sphere.center.z)**2) - self.sphere.radius return d if __name__ == "__main__": o = VolSphere(Sphere(Point(4, 2, 0), 8.5)) print(str(o)) for y in range(-15, 15): s = '' for x in range(-30, 30): d = o.get_distance(Point(x * 0.5, 0, -y)) if d < 0: s += 'x' else: s += '.' # print(s)
def from_data(cls, data): sphere = Sphere.from_data(data['geom']) vsphere = cls(sphere) return vsphere
from compas.geometry import Sphere, Translation, centroid_points from compas.utilities import geometric_key from compas_view2.app import App from compas_view2.objects import Collection from compas_gmsh.models import ShapeModel # ============================================================================== # Geometry # ============================================================================== sphere = Sphere([0, 0, 0], 2) # ============================================================================== # Solid Model # ============================================================================== model = ShapeModel(name="tets") model.add_sphere(sphere) model.options.mesh.lmax = 0.2 model.generate_mesh(3) # ============================================================================== # COMPAS mesh # ============================================================================== tets = model.mesh_to_tets() shell = model.mesh_to_compas()
import math from compas.geometry import Point from compas.geometry import Plane from compas.geometry import Frame from compas.geometry import Sphere from compas_fab.backends import AnalyticalInverseKinematics from compas_fab.backends import PyBulletClient from compas_fab.robots import ReachabilityMap from compas_fab.robots.reachability_map import DeviationVectorsGenerator from compas_fab import DATA # 1. Define generators sphere = Sphere((0.4, 0, 0), 0.15) def points_on_sphere_generator(sphere): for theta_deg in range(0, 360, 20): for phi_deg in range(0, 90, 10): theta = math.radians(theta_deg) phi = math.radians(phi_deg) x = sphere.point.x + sphere.radius * math.cos(theta) * math.sin(phi) y = sphere.point.y + sphere.radius * math.sin(theta) * math.sin(phi) z = sphere.point.z + sphere.radius * math.cos(phi) point = Point(x, y, z) axis = sphere.point - point plane = Plane((x, y, z), axis) f = Frame.from_plane(plane) # for UR5 is zaxis the xaxis
import rhinoscriptsyntax as rs import Rhino.Geometry as rg from compas_vol.primitives import VolSphere, VolBox from compas_vol.combinations import Union,Subtraction from compas.geometry import Sphere, Box, Frame from compas.rpc import Proxy s = VolSphere(Sphere((0, 0, 0), 6)) b = VolBox(Box(Frame.worldXY(), 10, 10, 10), 1.5) u = Union(s, b) t = Subtraction(b, s) p = Proxy('compas_vol.utilities') #p.stop_server() #p = Proxy('compas_vol.utilities') bounds = ((-25,25,100), (-25,25,100), (-25,25,100)) vs,fs = p.get_vfs_from_tree(str(t), bounds, 1.0) mesh = rg.Mesh() for v1, v2, v3 in vs: mesh.Vertices.Add(v1, v2, v3) for f in fs: if len(set(f))>2: mesh.Faces.AddFace(f[0], f[1], f[2]) mesh.Compact() mesh.Normals.ComputeNormals() a = mesh b = [rs.CreatePoint(v1,v2,v3) for v1, v2, v3 in vs]
# @me add restart option to init from compas.rpc import Proxy proxy = Proxy() # ============================================================================== # Make a box and a sphere # ============================================================================== box = Box.from_width_height_depth(2, 2, 2) box = Mesh.from_shape(box) box.quads_to_triangles() A = box.to_vertices_and_faces() sphere = Sphere(Point(1, 1, 1), 1) sphere = Mesh.from_shape(sphere, u=30, v=30) sphere.quads_to_triangles() B = sphere.to_vertices_and_faces() # ============================================================================== # Remesh the sphere # ============================================================================== proxy.package = 'compas_cgal.meshing' B = proxy.remesh(B, 0.3, 10) # ============================================================================== # Compute the intersections
import rhinoscriptsyntax as rs import Rhino.Geometry as rg import math frame = Frame([10, 0, 0], [1, 0, 0], [0, 1, 0]) # box = Box(Frame.worldXY(), 150, 90, 200) box = Box(frame, 150, 90, 200) volbox = VolBox(box) plane = Plane([0, 0, 0], [0, 0, 1]) circle = Circle(plane, 50) cylinder = Cylinder(circle, 90) volcylinder = VolCylinder(cylinder) sphere = Sphere((40, 20, 0), 50) volshpere = VolSphere(sphere) sphere_big = Sphere((40, 20, 0), 100) volshpere_big = VolSphere(sphere_big) union = Union(volbox, volshpere) substract = Subtraction(volshpere_big, volshpere) class FuckYouBernhardFunction: X_SCALE = 5.0 Y_SCALE = 5.0 Z_SCALE = 5.0
import numpy as np import matplotlib.pyplot as plt from compas_vol.primitives import VolSphere from compas_vol.analysis import Gradient from compas.geometry import Point, Sphere s = Sphere(Point(1, 2, 3), 4) vs = VolSphere(s) g = Gradient(vs) print(vs.get_distance(Point(4, 5, 6))) print(g.get_gradient(Point(5, 2, 3))) x, y, z = np.ogrid[-10:10:20j, -10:10:20j, -10:10:20j] d = g.get_gradient_numpy(x, y, z) plt.quiver(x, y, d[:, :, 0, 1], d[:, :, 0, 2]) plt.axis('equal') plt.show()
db = self.b.get_distance_numpy(x, y, z) h = np.minimum(np.maximum(0.5 + 0.5 * (db - da) / self.r, 0), 1) return (db * (1 - h) + h * da) - self.r * h * (1 - h) # ============================================================================== # Main # ============================================================================== if __name__ == "__main__": from compas_vol.primitives import VolSphere, VolBox from compas.geometry import Box, Frame, Point, Sphere import numpy as np import matplotlib.pyplot as plt s = Sphere(Point(4, 5, 0), 7) b = Box(Frame.worldXY(), 20, 15, 10) vs = VolSphere(s) vb = VolBox(b, 2.5) u = SmoothUnion(vs, vb, 6.5) # for y in range(-15, 15): # s = '' # for x in range(-30, 30): # d = u.get_distance(Point(x*0.5, y, 0)) # if d < 0: # s += 'x' # else: # s += '.' # print(s) x, y, z = np.ogrid[-15:15:100j, -15:15:100j, -15:15:100j]
foot_2 = Box(Frame([-2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5) leg_1 = Box(Frame([2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7) leg_2 = Box(Frame([-2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7) axis = Cylinder(Circle(Plane([0, 0, 7], [1, 0, 0]), .01), 4) legs_link = model.add_link('legs', visual_meshes=[foot_1, foot_2, leg_1, leg_2, axis]) torso = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .5), 8) torso_link = model.add_link('torso', visual_meshes=[torso]) legs_joint_origin = Frame([0, 0, 7], [1, 0, 0], [0, 1, 0]) joint_axis = Vector(1, 0, 0) model.add_joint('torso_base_joint', Joint.CONTINUOUS, legs_link, torso_link, legs_joint_origin, joint_axis) head = Sphere([0, 0, 0], 1) beak = Cylinder(Circle(Plane([0, 1, -.3], [0, 1, 0]), .3), 1.5) head_link = model.add_link('head', visual_meshes=[head, beak]) neck_joint_origin = Frame([0, 0, 4], [1, 0, 0], [0, 1, 0]) model.add_joint('neck_joint', Joint.FIXED, torso_link, head_link, origin=neck_joint_origin) tail = Sphere([0, 0, 0], 1) tail_link = model.add_link('tail', visual_meshes=[tail]) tail_joint_origin = Frame([0, 0, -4], [1, 0, 0], [0, 1, 0]) model.add_joint('tail_joint', Joint.FIXED, torso_link,
from compas.datastructures import mesh_quads_to_triangles from compas_viewers.multimeshviewer import MultiMeshViewer from compas_viewers.multimeshviewer import MeshObject import compas_libigl as igl # ============================================================================== # Input Geometry # ============================================================================== R = 1.4 point = [0.0, 0.0, 0.0] cube = Box.from_width_height_depth(2 * R, 2 * R, 2 * R) sphere = Sphere(point, R * 1.25) a = Mesh.from_shape(cube) b = Mesh.from_shape(sphere, u=30, v=30) mesh_quads_to_triangles(a) mesh_quads_to_triangles(b) A = a.to_vertices_and_faces() B = b.to_vertices_and_faces() # ============================================================================== # Booleans # ============================================================================== D = igl.mesh_intersection(A, B)
from compas.geometry import Box from compas.geometry import Circle from compas.geometry import Cylinder from compas.geometry import Frame from compas.geometry import Plane from compas.geometry import Sphere # Box b1 = Box(Frame.worldXY(), 5, 1, 3) # xsize, ysize, zsize b2 = Box.from_width_height_depth(5, 3, 1) # width=xsize, height=zsize, depth=ysize assert str(b1) == str(b2) print(b1) # Sphere s1 = Sphere([0, 0, 0], 5) print(s1) # Cylinder plane = Plane([0, 0, 0], [0, 0, 1]) circle = Circle(plane, 5) c1 = Cylinder(circle, height=4) print(c1)
def inverse_kinematics_spherical_wrist(p1, p2, p3, p4, target_frame): """Inverse kinematics function for spherical wrist robots. This is a modification of the *Lobster* tool for solving the inverse kinematics problem for a spherical wrist robots. https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster& This is the instruction on how to determine the 4 points. https://api.ning.com/files/KRgE1yt2kgG2IF9H8sre4CWfIDL9ytv5WvVn54zdOx6HE84gDordaHzo0jqwP-Qhry7MyRQ4IQxY1p3cIkqEDj1FAVVR2Xg0/Lobster_IK.pdf check p2, p3, p4 must be in one XY plane! """ end_frame = Frame(target_frame.point, target_frame.yaxis, target_frame.xaxis) wrist_offset = p4.x - p3.x lower_arm_length = p2.z - p1.z upper_arm_length = (p3 - p2).length pln_offset = math.fabs(p2.y - p1.y) axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x) wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset)) p1_proj = p1.copy() p1_proj.y = p2.y axis1_angles = [] axis2_angles = [] axis3_angles = [] axis4_angles = [] axis5_angles = [] axis6_angles = [] if pln_offset == 0: axis1_angle = -1 * math.atan2(wrist.y, wrist.x) if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 axis1_angle += math.pi if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 else: circle = (0, 0, 0), pln_offset point = (wrist.x, wrist.y, 0) t1, t2 = tangent_points_to_circle_xy(circle, point) a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1)) a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1)) axis1_angles += [a1] * 4 axis1_angles += [a2] * 4 for i in range(2): axis1_angle = axis1_angles[i * 4] Rot1 = Rotation.from_axis_and_angle([0, 0, 1], -1 * axis1_angle, point=[0, 0, 0]) p1A = p1_proj.transformed(Rot1) elbow_dir = Vector(1, 0, 0).transformed(Rot1) sphere1 = Sphere(p1A, lower_arm_length) sphere2 = Sphere(wrist, upper_arm_length) elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) result = intersection_sphere_sphere(sphere1, sphere2) if result is not None: case, (center, radius, normal) = result else: return None circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle( elbow_plane, circle) for j in range(2): if j == 0: elbow_pt = intersect_pt1 else: elbow_pt = intersect_pt2 elbow_pt = Point(*elbow_pt) elbowx, elbowy, elbowz = elbow_frame.to_local_coordinates(elbow_pt) wristx, wristy, wristz = elbow_frame.to_local_coordinates(wrist) axis2_angle = math.atan2(elbowy, elbowx) axis3_angle = math.pi - axis2_angle + math.atan2( wristy - elbowy, wristx - elbowx) - axis4_offset_angle for k in range(2): axis2_angles.append(-axis2_angle) axis3_angle_wrapped = -axis3_angle + math.pi while (axis3_angle_wrapped >= math.pi): axis3_angle_wrapped -= 2 * math.pi while (axis3_angle_wrapped < -math.pi): axis3_angle_wrapped += 2 * math.pi axis3_angles.append(axis3_angle_wrapped) for k in range(2): axis4 = wrist - elbow_pt axis4.transform( Rotation.from_axis_and_angle(elbow_frame.zaxis, -axis4_offset_angle)) temp_frame = elbow_frame.copy() # copy yes or no? temp_frame.transform( Rotation.from_axis_and_angle(temp_frame.zaxis, axis2_angle + axis3_angle)) # // B = TempPlane axis4_frame = Frame(wrist, temp_frame.zaxis, temp_frame.yaxis * -1.0) axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates( end_frame.point) axis4_angle = math.atan2(axis6y, axis6x) if k == 1: axis4_angle += math.pi if axis4_angle > math.pi: axis4_angle -= 2 * math.pi axis4_angle_wrapped = axis4_angle + math.pi / 2 while (axis4_angle_wrapped >= math.pi): axis4_angle_wrapped -= 2 * math.pi while (axis4_angle_wrapped < -math.pi): axis4_angle_wrapped += 2 * math.pi axis4_angles.append(axis4_angle_wrapped) axis5_frame = axis4_frame.copy() axis5_frame.transform( Rotation.from_axis_and_angle(axis4_frame.zaxis, axis4_angle)) axis5_frame = Frame(wrist, axis5_frame.zaxis * -1, axis5_frame.xaxis) axis6x, axis6y, axis6z = axis5_frame.to_local_coordinates( end_frame.point) axis5_angle = math.atan2(axis6y, axis6x) axis5_angles.append(axis5_angle) axis6_frame = axis5_frame.copy() axis6_frame.transform( Rotation.from_axis_and_angle(axis5_frame.zaxis, axis5_angle)) axis6_frame = Frame(wrist, axis6_frame.yaxis * -1, axis6_frame.zaxis) endx, endy, endz = axis6_frame.to_local_coordinates( end_frame.to_world_coordinates(Point(1, 0, 0))) axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle)
def sphere_generator(): sphere = Sphere((0.35, 0, 0), 0.15) for x in range(5): for z in range(7): center = sphere.point + Vector(x, 0, z) * 0.05 yield Sphere(center, sphere.radius)
# ============================================================================== # Geometry # ============================================================================== R = 1.4 P = Point(0, 0, 0) X = Vector(1, 0, 0) Y = Vector(0, 1, 0) Z = Vector(0, 0, 1) YZ = Plane(P, X) ZX = Plane(P, Y) XY = Plane(P, Z) box = Box.from_width_height_depth(2 * R, 2 * R, 2 * R) sphere = Sphere(P, 1.25 * R) cylx = Cylinder((YZ, 0.7 * R), 3 * R) cyly = Cylinder((ZX, 0.7 * R), 3 * R) cylz = Cylinder((XY, 0.7 * R), 3 * R) # ============================================================================== # CSG Model # ============================================================================== model = ShapeModel(name="booleans") model.mesh_lmin = 0.2 model.mesh_lmax = 0.2 model.boolean_difference(
from compas.geometry import centroid_points from compas.datastructures import Mesh from compas.datastructures import mesh_quads_to_triangles from compas.datastructures import mesh_subdivide_quad from compas_viewers.multimeshviewer import MultiMeshViewer from compas_viewers.multimeshviewer.model import MeshObject import compas_libigl as igl origin = Point(0.0, 0.0, 0.0) cube = Box.from_width_height_depth(1.0, 1.0, 1.0) sphere = Sphere(origin, 0.95 * sqrt(0.5 ** 2 + 0.5 ** 2)) xcyl = Cylinder(Circle(Plane(origin, Vector(1.0, 0.0, 0.0)), 0.35), 2.0) ycyl = Cylinder(Circle(Plane(origin, Vector(0.0, 1.0, 0.0)), 0.35), 2.0) zcyl = Cylinder(Circle(Plane(origin, Vector(0.0, 0.0, 1.0)), 0.35), 2.0) a = Mesh.from_vertices_and_faces(cube.vertices, cube.faces) a = mesh_subdivide_quad(a, k=3) b = Mesh.from_vertices_and_faces(* sphere.to_vertices_and_faces(u=30, v=30)) c = Mesh.from_vertices_and_faces(* xcyl.to_vertices_and_faces(u=30)) d = Mesh.from_vertices_and_faces(* ycyl.to_vertices_and_faces(u=30)) e = Mesh.from_vertices_and_faces(* zcyl.to_vertices_and_faces(u=30)) mesh_quads_to_triangles(a) mesh_quads_to_triangles(b) mesh_quads_to_triangles(c)
mesh = Mesh.from_off(igl.get('tubemesh.off')) mesh.quads_to_triangles() z = mesh.vertices_attribute('z') zmin = min(z) T = Translation.from_vector([0, 0, -zmin]) mesh.transform(T) # ============================================================================== # Rays # ============================================================================== base = Point(-7, 0, 0) sphere = Sphere(base, 1.0) theta = np.linspace(0, np.pi, 20, endpoint=False) phi = np.linspace(0, 2 * np.pi, 20, endpoint=False) theta, phi = np.meshgrid(theta, phi) theta = theta.ravel() phi = phi.ravel() r = 1.0 x = r * np.sin(theta) * np.cos(phi) + base.x y = r * np.sin(theta) * np.sin(phi) + base.y z = r * np.cos(theta) xyz = np.vstack((x, y, z)).T mask = xyz[:, 2] > 0 hemi = xyz[mask]
distances = ([o.get_distance_numpy(x, y, z) for o in self.objs]) return np.maximum.reduce(distances) # ============================================================================== # Main # ============================================================================== if __name__ == "__main__": import numpy as np import matplotlib.pyplot as plt from compas_vol.primitives import VolSphere, VolBox from compas.geometry import Box, Frame, Point, Sphere s = Sphere(Point(5, 6, 0), 9) b = Box(Frame.worldXY(), 20, 15, 10) vs = VolSphere(s) vb = VolBox(b, 2.5) u = Intersection(vs, vb) x, y, z = np.ogrid[-15:15:60j, -15:15:60j, -15:15:60j] d = u.get_distance_numpy(x, y, z) plt.imshow( d[:, :, 25].T, cmap='RdBu' ) # transpose because numpy indexing is 1)row 2) column instead of x y plt.show() for y in range(-15, 15): s = '' for x in range(-30, 30):
def inverse_kinematics_spherical_wrist(target_frame, points): """Inverse kinematics function for spherical wrist robots. Parameters ---------- frame : :class:`compas.geometry.Frame` The frame we search the inverse kinematics for. points : list of point A list of 4 points specifying the robot's joint positions. Returns ------- list of list of float The 8 analytical IK solutions. Notes ----- This is a modification of the *Lobster* tool for solving the inverse kinematics problem for a spherical wrist robots. https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster& This is the instruction on how to determine the 4 points. http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf Check that p2, p3, and p4 are all in the XZ plane (y coordinates are zero)! """ p1, p2, p3, p4 = points end_frame = Frame(target_frame.point, target_frame.yaxis, target_frame.xaxis) wrist_offset = p4.x - p3.x lower_arm_length = p2.z - p1.z upper_arm_length = (p3 - p2).length pln_offset = math.fabs(p2.y - p1.y) axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x) wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset)) p1_proj = p1.copy() p1_proj.y = p2.y axis1_angles = [] axis2_angles = [] axis3_angles = [] axis4_angles = [] axis5_angles = [] axis6_angles = [] if pln_offset == 0: axis1_angle = -1 * math.atan2(wrist.y, wrist.x) if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 axis1_angle += math.pi if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 else: circle = (0, 0, 0), pln_offset point = (wrist.x, wrist.y, 0) t1, t2 = tangent_points_to_circle_xy(circle, point) a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1)) a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1)) axis1_angles += [a1] * 4 axis1_angles += [a2] * 4 for i in range(2): axis1_angle = axis1_angles[i * 4] Rot1 = Rotation.from_axis_and_angle([0, 0, 1], -1 * axis1_angle, point=[0, 0, 0]) p1A = p1_proj.transformed(Rot1) elbow_dir = Vector(1, 0, 0).transformed(Rot1) sphere1 = Sphere(p1A, lower_arm_length) sphere2 = Sphere(wrist, upper_arm_length) elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) _, (center, radius, normal) = intersection_sphere_sphere(sphere1, sphere2) circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle( elbow_plane, circle) for j in range(2): if j == 0: elbow_pt = intersect_pt1 else: elbow_pt = intersect_pt2 elbow_pt = Point(*elbow_pt) elbowx, elbowy, _ = elbow_frame.to_local_coordinates(elbow_pt) wristx, wristy, _ = elbow_frame.to_local_coordinates(wrist) axis2_angle = math.atan2(elbowy, elbowx) axis3_angle = math.pi - axis2_angle + math.atan2( wristy - elbowy, wristx - elbowx) - axis4_offset_angle for k in range(2): axis2_angles.append(-axis2_angle) axis3_angle_wrapped = -axis3_angle + math.pi while (axis3_angle_wrapped >= math.pi): axis3_angle_wrapped -= 2 * math.pi while (axis3_angle_wrapped < -math.pi): axis3_angle_wrapped += 2 * math.pi axis3_angles.append(axis3_angle_wrapped) for k in range(2): axis4 = wrist - elbow_pt axis4.transform( Rotation.from_axis_and_angle(elbow_frame.zaxis, -axis4_offset_angle)) temp_frame = elbow_frame.copy() temp_frame.transform( Rotation.from_axis_and_angle(temp_frame.zaxis, axis2_angle + axis3_angle)) # // B = TempPlane axis4_frame = Frame(wrist, temp_frame.zaxis, temp_frame.yaxis * -1.0) axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates( end_frame.point) axis4_angle = math.atan2(axis6y, axis6x) if k == 1: axis4_angle += math.pi if axis4_angle > math.pi: axis4_angle -= 2 * math.pi axis4_angle_wrapped = axis4_angle + math.pi / 2 while (axis4_angle_wrapped >= math.pi): axis4_angle_wrapped -= 2 * math.pi while (axis4_angle_wrapped < -math.pi): axis4_angle_wrapped += 2 * math.pi axis4_angles.append(axis4_angle_wrapped) axis5_frame = axis4_frame.copy() axis5_frame.transform( Rotation.from_axis_and_angle(axis4_frame.zaxis, axis4_angle)) axis5_frame = Frame(wrist, axis5_frame.zaxis * -1, axis5_frame.xaxis) axis6x, axis6y, _ = axis5_frame.to_local_coordinates( end_frame.point) axis5_angle = math.atan2(axis6y, axis6x) axis5_angles.append(axis5_angle) axis6_frame = axis5_frame.copy() axis6_frame.transform( Rotation.from_axis_and_angle(axis5_frame.zaxis, axis5_angle)) axis6_frame = Frame(wrist, axis6_frame.yaxis * -1, axis6_frame.zaxis) endx, endy, _ = axis6_frame.to_local_coordinates( end_frame.to_world_coordinates(Point(1, 0, 0))) axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle) return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip( axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles)]
from compas_cgal.booleans import boolean_difference from compas_cgal.booleans import boolean_intersection from compas_cgal.meshing import remesh # ============================================================================== # Make a box and a sphere # ============================================================================== box = Box.from_width_height_depth(2, 2, 2) box = Mesh.from_shape(box) box.quads_to_triangles() A = box.to_vertices_and_faces() sphere = Sphere(Point(1, 1, 1), 1) sphere = Mesh.from_shape(sphere, u=30, v=30) sphere.quads_to_triangles() B = sphere.to_vertices_and_faces() bbox = bounding_box(box.vertices_attributes('xyz') + sphere.vertices_attributes('xyz')) bbox = Box.from_bounding_box(bbox) width = bbox.xsize # ============================================================================== # Remesh the sphere # ============================================================================== B = remesh(B, 0.3, 10)
def sphere(): sphere = Sphere([0, 0, 0], 1.0) mesh = Mesh.from_shape(sphere, u=16, v=16) return mesh
return da + self.f * db def get_distance_numpy(self, x, y, z): """ vectorized distance function """ da = self.a.get_distance_numpy(x, y, z) db = self.b.get_distance_numpy(x, y, z) return da + self.f * db if __name__ == "__main__": from compas_vol.primitives import VolSphere, VolBox from compas.geometry import Box, Frame, Point, Sphere s = Sphere(Point(2, 3, 0), 7) b = Box(Frame.worldXY(), 20, 15, 10) vs = VolSphere(s) vb = VolBox(b, 2.5) f = Overlay(vs, vb, 0.2) print(f) for y in range(-15, 15): s = '' for x in range(-30, 30): d = f.get_distance((x * 0.5, y, 0)) if d < 0: s += 'x' else: s += '.' print(s)
from compas.geometry import Box from compas.geometry import Circle from compas.geometry import Cylinder from compas.geometry import Frame from compas.geometry import Plane from compas.geometry import Sphere # Box b1 = Box(Frame.worldXY(), 10, 1, 4) # xsize, ysize, zsize b2 = Box.from_width_height_depth(10, 4, 1) # width=xsize, height=zsize, depth=ysize assert str(b1) == str(b2) print(b1) # Sphere s1 = Sphere([10, 0, 0], 4) print(s1) # Cylinder plane = Plane([20, 0, 0], [0, 0, 1]) circle = Circle(plane, 5) c1 = Cylinder(circle, height=4) print(c1) # Draw! artist = BoxArtist(b1, layer='shapes') artist.draw() artist = SphereArtist(s1, layer='shapes') artist.draw()