////////////////////////////////////////////////////////////////////// // LibFile: vectors.scad // This file provides some mathematical operations that apply to each // entry in a vector. It provides normalization and angle computation, and // it provides functions for searching lists of vectors for matches to // a given vector. // Includes: // include // FileGroup: Math // FileSummary: Vector arithmetic, angle, and searching. // FileFootnotes: STD=Included in std.scad ////////////////////////////////////////////////////////////////////// // Section: Vector Testing // Function: is_vector() // Usage: // bool = is_vector(v, [length], [zero=], [all_nonzero=], [eps=]); // Description: // Returns true if v is a list of finite numbers. // Arguments: // v = The value to test to see if it is a vector. // length = If given, make sure the vector is `length` items long. // --- // zero = If false, require that the `norm()` of the vector is not approximately zero. If true, require the `norm()` of the vector to be approximately zero. Default: `undef` (don't check vector `norm()`.) // all_nonzero = If true, requires all elements of the vector to be more than `eps` different from zero. Default: `false` // eps = The minimum vector length that is considered non-zero. Default: `EPSILON` (`1e-9`) // Example: // is_vector(4); // Returns false // is_vector([4,true,false]); // Returns false // is_vector([3,4,INF,5]); // Returns false // is_vector([3,4,5,6]); // Returns true // is_vector([3,4,undef,5]); // Returns false // is_vector([3,4,5],3); // Returns true // is_vector([3,4,5],4); // Returns true // is_vector([]); // Returns false // is_vector([0,4,0],3,zero=false); // Returns true // is_vector([0,0,0],zero=false); // Returns false // is_vector([0,0,1e-12],zero=false); // Returns false // is_vector([0,1,0],all_nonzero=false); // Returns false // is_vector([1,1,1],all_nonzero=false); // Returns true // is_vector([],zero=false); // Returns false function is_vector(v, length, zero, all_nonzero=false, eps=EPSILON) = is_list(v) && len(v)>0 && []==[for(vi=v) if(!is_finite(vi)) 0] && (is_undef(length) || len(v)==length) && (is_undef(zero) || ((norm(v) >= eps) == !zero)) && (!all_nonzero || all_nonzero(v)) ; // Section: Scalar operations on vectors // Function: add_scalar() // Usage: // v_new = add_scalar(v, s); // Topics: List Handling // Description: // Given a vector and a scalar, returns the vector with the scalar added to each item in it. // Arguments: // v = The initial array. // s = A scalar value to add to every item in the array. // Example: // a = add_scalar([1,2,3],3); // Returns: [4,5,6] function add_scalar(v,s) = assert(is_vector(v), "Input v must be a vector") assert(is_finite(s), "Input s must be a finite scalar") [for(entry=v) entry+s]; // Function: v_mul() // Usage: // v3 = v_mul(v1, v2); // Description: // Element-wise multiplication. Multiplies each element of `v1` by the corresponding element of `v2`. // Both `v1` and `v2` must be the same length. Returns a vector of the products. Note that // the items in `v1` and `v2` can be anything that OpenSCAD will multiply. // Arguments: // v1 = The first vector. // v2 = The second vector. // Example: // v_mul([3,4,5], [8,7,6]); // Returns [24, 28, 30] function v_mul(v1, v2) = assert( is_list(v1) && is_list(v2) && len(v1)==len(v2), "Incompatible input") [for (i = [0:1:len(v1)-1]) v1[i]*v2[i]]; // Function: v_div() // Usage: // v3 = v_div(v1, v2); // Description: // Element-wise vector division. Divides each element of vector `v1` by // the corresponding element of vector `v2`. Returns a vector of the quotients. // Arguments: // v1 = The first vector. // v2 = The second vector. // Example: // v_div([24,28,30], [8,7,6]); // Returns [3, 4, 5] function v_div(v1, v2) = assert( is_vector(v1) && is_vector(v2,len(v1)), "Incompatible vectors") [for (i = [0:1:len(v1)-1]) v1[i]/v2[i]]; // Function: v_abs() // Usage: // v2 = v_abs(v); // Description: Returns a vector of the absolute value of each element of vector `v`. // Arguments: // v = The vector to get the absolute values of. // Example: // v_abs([-1,3,-9]); // Returns: [1,3,9] function v_abs(v) = assert( is_vector(v), "Invalid vector" ) [for (x=v) abs(x)]; // Function: v_floor() // Usage: // v2 = v_floor(v); // Description: // Returns the given vector after performing a `floor()` on all items. function v_floor(v) = assert( is_vector(v), "Invalid vector" ) [for (x=v) floor(x)]; // Function: v_ceil() // Usage: // v2 = v_ceil(v); // Description: // Returns the given vector after performing a `ceil()` on all items. function v_ceil(v) = assert( is_vector(v), "Invalid vector" ) [for (x=v) ceil(x)]; // Function: v_lookup() // Usage: // v2 = v_lookup(x, v); // Description: // Works just like the built-in function [`lookup()`](https://en.wikibooks.org/wiki/OpenSCAD_User_Manual/Mathematical_Functions#lookup), except that it can also interpolate between vector result values of the same length. // Arguments: // x = The scalar value to look up. // v = A list of [KEY,VAL] pairs. KEYs are scalars. VALs should either all be scalar, or all be vectors of the same length. // Example: // x = v_lookup(4.5, [[4, [3,4,5]], [5, [5,6,7]]]); // Returns: [4,5,6] function v_lookup(x, v) = is_num(v[0][1])? lookup(x,v) : let( i = lookup(x, [for (i=idx(v)) [v[i].x,i]]), vlo = v[floor(i)], vhi = v[ceil(i)], lo = vlo[1], hi = vhi[1] ) assert(is_vector(lo) && is_vector(hi), "Result values must all be numbers, or all be vectors.") assert(len(lo) == len(hi), "Vector result values must be the same length") vlo.x == vhi.x? vlo[1] : let( u = (x - vlo.x) / (vhi.x - vlo.x) ) lerp(lo,hi,u); // Section: Vector Properties // Function: unit() // Usage: // v = unit(v, [error]); // Description: // Returns the unit length normalized version of vector v. If passed a zero-length vector, // asserts an error unless `error` is given, in which case the value of `error` is returned. // Arguments: // v = The vector to normalize. // error = If given, and input is a zero-length vector, this value is returned. Default: Assert error on zero-length vector. // Example: // v1 = unit([10,0,0]); // Returns: [1,0,0] // v2 = unit([0,10,0]); // Returns: [0,1,0] // v3 = unit([0,0,10]); // Returns: [0,0,1] // v4 = unit([0,-10,0]); // Returns: [0,-1,0] // v5 = unit([0,0,0],[1,2,3]); // Returns: [1,2,3] // v6 = unit([0,0,0]); // Asserts an error. function unit(v, error=[[["ASSERT"]]]) = assert(is_vector(v), "Invalid vector") norm(v)=EPSILON,"Cannot normalize a zero vector") : error) : v/norm(v); // Function: v_theta() // Usage: // theta = v_theta([X,Y]); // Description: // Given a vector, returns the angle in degrees counter-clockwise from X+ on the XY plane. function v_theta(v) = assert( is_vector(v,2) || is_vector(v,3) , "Invalid vector") atan2(v.y,v.x); // Function: vector_angle() // Usage: // ang = vector_angle(v1,v2); // ang = vector_angle([v1,v2]); // ang = vector_angle(PT1,PT2,PT3); // ang = vector_angle([PT1,PT2,PT3]); // Description: // If given a single list of two vectors, like `vector_angle([V1,V2])`, returns the angle between the two vectors V1 and V2. // If given a single list of three points, like `vector_angle([A,B,C])`, returns the angle between the line segments AB and BC. // If given two vectors, like `vector_angle(V1,V2)`, returns the angle between the two vectors V1 and V2. // If given three points, like `vector_angle(A,B,C)`, returns the angle between the line segments AB and BC. // Arguments: // v1 = First vector or point. // v2 = Second vector or point. // v3 = Third point in three point mode. // Example: // ang1 = vector_angle(UP,LEFT); // Returns: 90 // ang2 = vector_angle(RIGHT,LEFT); // Returns: 180 // ang3 = vector_angle(UP+RIGHT,RIGHT); // Returns: 45 // ang4 = vector_angle([10,10], [0,0], [10,-10]); // Returns: 90 // ang5 = vector_angle([10,0,10], [0,0,0], [-10,10,0]); // Returns: 120 // ang6 = vector_angle([[10,0,10], [0,0,0], [-10,10,0]]); // Returns: 120 function vector_angle(v1,v2,v3) = assert( ( is_undef(v3) && ( is_undef(v2) || same_shape(v1,v2) ) ) || is_consistent([v1,v2,v3]) , "Bad arguments.") assert( is_vector(v1) || is_consistent(v1), "Bad arguments.") let( vecs = ! is_undef(v3) ? [v1-v2,v3-v2] : ! is_undef(v2) ? [v1,v2] : len(v1) == 3 ? [v1[0]-v1[1], v1[2]-v1[1]] : v1 ) assert(is_vector(vecs[0],2) || is_vector(vecs[0],3), "Bad arguments.") let( norm0 = norm(vecs[0]), norm1 = norm(vecs[1]) ) assert(norm0>0 && norm1>0, "Zero length vector.") // NOTE: constrain() corrects crazy FP rounding errors that exceed acos()'s domain. acos(constrain((vecs[0]*vecs[1])/(norm0*norm1), -1, 1)); // Function: vector_axis() // Usage: // axis = vector_axis(v1,v2); // axis = vector_axis([v1,v2]); // axis = vector_axis(PT1,PT2,PT3); // axis = vector_axis([PT1,PT2,PT3]); // Description: // If given a single list of two vectors, like `vector_axis([V1,V2])`, returns the vector perpendicular the two vectors V1 and V2. // If given a single list of three points, like `vector_axis([A,B,C])`, returns the vector perpendicular to the plane through a, B and C. // If given two vectors, like `vector_axis(V1,V2)`, returns the vector perpendicular to the two vectors V1 and V2. // If given three points, like `vector_axis(A,B,C)`, returns the vector perpendicular to the plane through a, B and C. // Arguments: // v1 = First vector or point. // v2 = Second vector or point. // v3 = Third point in three point mode. // Example: // axis1 = vector_axis(UP,LEFT); // Returns: [0,-1,0] (FWD) // axis2 = vector_axis(RIGHT,LEFT); // Returns: [0,-1,0] (FWD) // axis3 = vector_axis(UP+RIGHT,RIGHT); // Returns: [0,1,0] (BACK) // axis4 = vector_axis([10,10], [0,0], [10,-10]); // Returns: [0,0,-1] (DOWN) // axis5 = vector_axis([10,0,10], [0,0,0], [-10,10,0]); // Returns: [-0.57735, -0.57735, 0.57735] // axis6 = vector_axis([[10,0,10], [0,0,0], [-10,10,0]]); // Returns: [-0.57735, -0.57735, 0.57735] function vector_axis(v1,v2=undef,v3=undef) = is_vector(v3) ? assert(is_consistent([v3,v2,v1]), "Bad arguments.") vector_axis(v1-v2, v3-v2) : assert( is_undef(v3), "Bad arguments.") is_undef(v2) ? assert( is_list(v1), "Bad arguments.") len(v1) == 2 ? vector_axis(v1[0],v1[1]) : vector_axis(v1[0],v1[1],v1[2]) : assert( is_vector(v1,zero=false) && is_vector(v2,zero=false) && is_consistent([v1,v2]) , "Bad arguments.") let( eps = 1e-6, w1 = point3d(v1/norm(v1)), w2 = point3d(v2/norm(v2)), w3 = (norm(w1-w2) > eps && norm(w1+w2) > eps) ? w2 : (norm(v_abs(w2)-UP) > eps)? UP : RIGHT ) unit(cross(w1,w3)); // Section: Vector Searching // Function: pointlist_bounds() // Usage: // pt_pair = pointlist_bounds(pts); // Topics: Geometry, Bounding Boxes, Bounds // Description: // Finds the bounds containing all the points in `pts` which can be a list of points in any dimension. // Returns a list of two items: a list of the minimums and a list of the maximums. For example, with // 3d points `[[MINX, MINY, MINZ], [MAXX, MAXY, MAXZ]]` // Arguments: // pts = List of points. function pointlist_bounds(pts) = assert(is_path(pts,dim=undef,fast=true) , "Invalid pointlist." ) let( select = ident(len(pts[0])), spread = [ for(i=[0:len(pts[0])-1]) let( spreadi = pts*select[i] ) [ min(spreadi), max(spreadi) ] ] ) transpose(spread); // Function: closest_point() // Usage: // index = closest_point(pt, points); // Topics: Geometry, Points, Distance // Description: // Given a list of `points`, finds the index of the closest point to `pt`. // Arguments: // pt = The point to find the closest point to. // points = The list of points to search. function closest_point(pt, points) = assert( is_vector(pt), "Invalid point." ) assert(is_path(points,dim=len(pt)), "Invalid pointlist or incompatible dimensions." ) min_index([for (p=points) norm(p-pt)]); // Function: furthest_point() // Usage: // index = furthest_point(pt, points); // Topics: Geometry, Points, Distance // Description: // Given a list of `points`, finds the index of the furthest point from `pt`. // Arguments: // pt = The point to find the farthest point from. // points = The list of points to search. function furthest_point(pt, points) = assert( is_vector(pt), "Invalid point." ) assert(is_path(points,dim=len(pt)), "Invalid pointlist or incompatible dimensions." ) max_index([for (p=points) norm(p-pt)]); // Function: vector_search() // Usage: // indices = vector_search(query, r, target); // See Also: vector_search_tree(), vector_nearest() // Topics: Search, Points, Closest // Description: // Given a list of query points `query` and a `target` to search, // finds the points in `target` that match each query point. A match holds when the // distance between a point in `target` and a query point is less than or equal to `r`. // The returned list will have a list for each query point containing, in arbitrary // order, the indices of all points that match that query point. // The `target` may be a simple list of points or a search tree. // When `target` is a large list of points, a search tree is constructed to // speed up the search with an order around O(log n) per query point. // For small point lists, a direct search is done dispensing a tree construction. // Alternatively, `target` may be a search tree built with `vector_search_tree()`. // In that case, that tree is parsed looking for matches. // An empty list of query points will return a empty output list. // An empty list of target points will return a output list with an empty list for each query point. // Arguments: // query = list of points to find matches for. // r = the search radius. // target = list of the points to search for matches or a search tree. // Example: A set of four queries to find points within 1 unit of the query. The circles show the search region and all have radius 1. // $fn=32; // k = 2000; // points = list_to_matrix(rands(0,10,k*2,seed=13333),2); // queries = [for(i=[3,7],j=[3,7]) [i,j]]; // search_ind = vector_search(queries, points, 1); // move_copies(points) circle(r=.08); // for(i=idx(queries)){ // color("blue")stroke(move(queries[i],circle(r=1)), closed=true, width=.08); // color("red") move_copies(select(points, search_ind[i])) circle(r=.08); // } // Example: when a series of searches with different radius are needed, its is faster to pre-compute the tree // $fn=32; // k = 2000; // points = list_to_matrix(rands(0,10,k*2),2,seed=13333); // queries1 = [for(i=[3,7]) [i,i]]; // queries2 = [for(i=[3,7]) [10-i,i]]; // r1 = 1; // r2 = .7; // search_tree = vector_search_tree(points); // search_1 = vector_search(queries1, r1, search_tree); // search_2 = vector_search(queries2, r2, search_tree); // move_copies(points) circle(r=.08); // for(i=idx(queries1)){ // color("blue")stroke(move(queries1[i],circle(r=r1)), closed=true, width=.08); // color("red") move_copies(select(points, search_1[i])) circle(r=.08); // } // for(i=idx(queries2)){ // color("green")stroke(move(queries2[i],circle(r=r2)), closed=true, width=.08); // color("red") move_copies(select(points, search_2[i])) circle(r=.08); // } function vector_search(query, r, target) = query==[] ? [] : is_list(query) && target==[] ? is_vector(query) ? [] : [for(q=query) [] ] : assert( is_finite(r) && r>=0, "The query radius should be a positive number." ) let( tgpts = is_matrix(target), // target is a point list tgtree = is_list(target) // target is a tree && (len(target)==2) && is_matrix(target[0]) && is_list(target[1]) && (len(target[1])==4 || (len(target[1])==1 && is_list(target[1][0])) ) ) assert( tgpts || tgtree, "The target should be a list of points or a search tree compatible with the query." ) let( dim = tgpts ? len(target[0]) : len(target[0][0]), simple = is_vector(query, dim) ) assert( simple || is_matrix(query,undef,dim), "The query points should be a list of points compatible with the target point list.") tgpts ? len(target)<=400 ? simple ? [for(i=idx(target)) if(norm(target[i]-query)<=r) i ] : [for(q=query) [for(i=idx(target)) if(norm(target[i]-q)<=r) i ] ] : let( tree = _bt_tree(target, count(len(target)), leafsize=25) ) simple ? _bt_search(query, r, target, tree) : [for(q=query) _bt_search(q, r, target, tree)] : simple ? _bt_search(query, r, target[0], target[1]) : [for(q=query) _bt_search(q, r, target[0], target[1])]; //Ball tree search function _bt_search(query, r, points, tree) = assert( is_list(tree) && ( ( len(tree)==1 && is_list(tree[0]) ) || ( len(tree)==4 && is_num(tree[0]) && is_num(tree[1]) ) ), "The tree is invalid.") len(tree)==1 ? assert( tree[0]==[] || is_vector(tree[0]), "The tree is invalid." ) [for(i=tree[0]) if(norm(points[i]-query)<=r) i ] : norm(query-points[tree[0]]) > r+tree[1] ? [] : concat( [ if(norm(query-points[tree[0]])<=r) tree[0] ], _bt_search(query, r, points, tree[2]), _bt_search(query, r, points, tree[3]) ) ; // Function: vector_search_tree() // Usage: // tree = vector_search_tree(points,leafsize); // See Also: vector_nearest(), vector_search() // Topics: Search, Points, Closest // Description: // Construct a search tree for the given list of points to be used as input // to the function `vector_search()`. The use of a tree speeds up the // search process. The tree construction stops branching when // a tree node represents a number of points less or equal to `leafsize`. // Search trees are ball trees. Constructing the // tree should be O(n log n) and searches should be O(log n), though real life // performance depends on how the data is distributed, and it will deteriorate // for high data dimensions. This data structure is useful when you will be // performing many searches of the same data, so that the cost of constructing // the tree is justified. (See https://en.wikipedia.org/wiki/Ball_tree) // For a small lists of points, the search with a tree may be more expensive // than direct comparisons. The argument `treemin` sets the minimum length of // point set for which a tree search will be done by `vector_search`. // For an empty list of points it returns an empty list. // Arguments: // points = list of points to store in the search tree. // leafsize = the size of the tree leaves. Default: 25 // treemin = the minimum size of the point list for which a tree search is done. Default: 400 // Example: A set of four queries to find points within 1 unit of the query. The circles show the search region and all have radius 1. // $fn=32; // k = 2000; // points = random_points(k, scale=10, dim=2,seed=13333); // queries = [for(i=[3,7],j=[3,7]) [i,j]]; // search_tree = vector_search_tree(points); // search_ind = vector_search(queries,1,search_tree); // move_copies(points) circle(r=.08); // for(i=idx(queries)){ // color("blue") stroke(move(queries[i],circle(r=1)), closed=true, width=.08); // color("red") move_copies(select(points, search_ind[i])) circle(r=.08); // } function vector_search_tree(points, leafsize=25, treemin=400) = points==[] ? [] : assert( is_matrix(points), "The input list entries should be points." ) assert( is_int(leafsize) && leafsize>=1, "The tree leaf size should be an integer greater than zero.") len(points)meanpr && i!=pivot) ind[i] ] ) [ ind[pivot], radius, _bt_tree(points, Lind, leafsize), _bt_tree(points, Rind, leafsize) ]; // Function: vector_nearest() // Usage: // indices = vector_nearest(query, k, target); // See Also: vector_search(), vector_search_tree() // Description: // Search `target` for the `k` points closest to point `query`. // The input `target` is either a list of points to search or a search tree // pre-computed by `vector_search_tree(). A list is returned containing the indices // of the points found in sorted order, closest point first. // Arguments: // query = point to search for // k = number of neighbors to return // target = a list of points or a search tree to search in // Example: Four queries to find the 15 nearest points. The circles show the radius defined by the most distant query result. Note they are different for each query. // $fn=32; // k = 1000; // points = list_to_matrix(rands(0,10,k*2,seed=13333),2); // tree = vector_search_tree(points); // queries = [for(i=[3,7],j=[3,7]) [i,j]]; // search_ind = [for(q=queries) vector_nearest(q, 15, tree)]; // move_copies(points) circle(r=.08); // for(i=idx(queries)){ // circle = circle(r=norm(points[last(search_ind[i])]-queries[i])); // color("red") move_copies(select(points, search_ind[i])) circle(r=.08); // color("blue") stroke(move(queries[i], circle), closed=true, width=.08); // } function vector_nearest(query, k, target) = assert(is_int(k) && k>0) assert(is_vector(query), "Query must be a vector.") let( tgpts = is_matrix(target,undef,len(query)), // target is a point list tgtree = is_list(target) // target is a tree && (len(target)==2) && is_matrix(target[0],undef,len(query)) && (len(target[1])==4 || (len(target[1])==1 && is_list(target[1][0])) ) ) assert( tgpts || tgtree, "The target should be a list of points or a search tree compatible with the query." ) assert((tgpts && (k<=len(target))) || (tgtree && (k<=len(target[0]))), "More results are requested than the number of points.") tgpts ? let( tree = _bt_tree(target, count(len(target))) ) column(_bt_nearest( query, k, target, tree),0) : column(_bt_nearest( query, k, target[0], target[1]),0); //Ball tree nearest function _bt_nearest(p, k, points, tree, answers=[]) = assert( is_list(tree) && ( ( len(tree)==1 && is_list(tree[0]) ) || ( len(tree)==4 && is_num(tree[0]) && is_num(tree[1]) ) ), "The tree is invalid.") len(tree)==1 ? _insert_many(answers, k, [for(entry=tree[0]) [entry, norm(points[entry]-p)]]) : let( d = norm(p-points[tree[0]]) ) len(answers)==k && ( d > last(answers)[1]+tree[1] ) ? answers : let( answers1 = _insert_sorted(answers, k, [tree[0],d]), answers2 = _bt_nearest(p, k, points, tree[2], answers1), answers3 = _bt_nearest(p, k, points, tree[3], answers2) ) answers3; function _insert_sorted(list, k, new) = (len(list)==k && new[1]>= last(list)[1]) ? list : [ for(entry=list) if (entry[1]<=new[1]) entry, new, for(i=[0:1:min(k-1,len(list))-1]) if (list[i][1]>new[1]) list[i] ]; function _insert_many(list, k, newlist,i=0) = i==len(newlist) ? list : assert(is_vector(newlist[i],2), "The tree is invalid.") _insert_many(_insert_sorted(list,k,newlist[i]),k,newlist,i+1); // vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap