From 68ffead0480094a59f2afb59681a8d06e44f135a Mon Sep 17 00:00:00 2001 From: JamesWigglesworth Date: Sat, 25 Mar 2017 18:43:17 -0400 Subject: [PATCH] Add files via upload Coor contains an object system and functions that allow the user to work in multiple coordinate systems. Kin is updated with functions that utilize Coor. DXF contains functions that parse and manipulate the universal CAD file format, DXF (Data Exchnge File). --- math/Coor | 284 ++++++ math/DXF | 2214 ++++++++++++++++++++++++++++++++++++++++ math/Kin | 2886 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 5384 insertions(+) create mode 100644 math/Coor create mode 100644 math/DXF create mode 100644 math/Kin diff --git a/math/Coor b/math/Coor new file mode 100644 index 00000000..f37a8b6a --- /dev/null +++ b/math/Coor @@ -0,0 +1,284 @@ +//Coordinate System +//James Wigglesworth +//Started: 1_19_17 +//Updated: 2_10_17 + + +var Coor = newObject({prototype: Root, name: "Coor", pose: Vector.make_pose()}) + +Coor.create_child = function(pose, name){ + if(pose === undefined){ + pose = Vector.make_pose() + } + if (!Vector.is_pose(pose)){ + dde_error("pose is not constructed properly") + } + var old_child = this[name] + if(Object.isNewObject(old_child)){ + old_child.set_pose(pose) + return old_child + }else if(name){ + return newObject({prototype: this, pose: pose, name: name}) + }else{ + return newObject({prototype: this, pose: pose}) + } + +} + + +var Table = Coor.create_child(Vector.make_pose(), "Table") +var L0 = Table.create_child(Vector.make_pose(), "L0") +var L1 = L0.create_child(Vector.make_pose(), "L1") +var L2 = L1.create_child(Vector.make_pose(), "L2") +var L3 = L2.create_child(Vector.make_pose(), "L3") +var L4 = L3.create_child(Vector.make_pose(), "L4") +var L5 = L4.create_child(Vector.make_pose(), "L5") + + + +Coor.get_pose = function(reference_coordinate_system){ + let result, obj_elt, base_idx, ref_idx + let base = this + let ref = reference_coordinate_system + if (ref === undefined || this === ref){ + return base.pose + }else{ + let base_path = base.ancestors() + base_path.push(base) + let ref_path = ref.ancestors() + ref_path.push(ref) + let common = Object.lowestCommonAncestor(base, ref) + + for(let i = 1; i < base_path.length; i++){ + if (common === base_path[i]){ + base_idx = i + break + } + } + let common_to_base = Vector.make_pose() + for(i = base_idx; i < base_path.length; i++){ + obj_elt = base_path[i] + common_to_base = Vector.matrix_multiply(common_to_base, obj_elt.pose) + } + //common_to_base = Vector.matrix_multiply(common_to_base, base.pose) + + for(i = 1; i < ref_path.length; i++){ + if (common === ref_path[i]){ + ref_idx = i + break + } + } + let common_to_ref = Vector.make_pose() + for(i = ref_idx; i < ref_path.length; i++){ + obj_elt = ref_path[i] + common_to_ref = Vector.matrix_multiply(common_to_ref, obj_elt.pose) + } + //common_to_ref = Vector.matrix_multiply(common_to_ref, ref.pose) + + return Vector.matrix_divide(common_to_base, common_to_ref) + } +} + +Coor.get_position = function(reference_coordinate_system){ + let result + if(reference_coordinate_system === undefined){ + result = Vector.pull(this.pose, [0, 2], 3) + }else{ + let pose = this.get_pose(reference_coordinate_system) + result = Vector.pull(pose, [0, 2], 3) + } + return Vector.transpose(result) +} + +Coor.get_orientation = function(reference_coordinate_system){ + let result + if(reference_coordinate_system === undefined){ + result = Vector.pull(this.pose, [0, 2], [0, 2]) + }else{ + let pose = this.get_pose(reference_coordinate_system) + result = Vector.pull(pose, [0, 2], [0, 2]) + } + return result +} + +Coor.set_pose = function(pose, reference_coordinate_system){ + let result + let ref = reference_coordinate_system + if(reference_coordinate_system === undefined){ + this.pose = pose + result = pose + }else{ + let base_ancestors = this.ancestors() + let parent = base_ancestors[base_ancestors.length-1] + result = Vector.matrix_multiply(pose, ref.get_pose(parent)) + this.pose = result + } + return result +} + + +Coor.set_position = function(position, reference_coordinate_system){ + let result, current_pose + let ref = reference_coordinate_system + if(reference_coordinate_system === undefined){ + current_pose = this.pose + current_pose[0][3] = position[0] + current_pose[1][3] = position[1] + current_pose[2][3] = position[2] + this.pose = current_pose + result = current_pose + }else{ + let base_ancestors = this.ancestors() + let parent = base_ancestors[base_ancestors.length-1] + let temp_position = Vector.transpose(Vector.matrix_multiply(ref.get_pose(parent), Vector.properly_define_point(position))) + temp_position.pop() + this.pose = Vector.make_pose(temp_position, this.get_orientation()) + result = this.pose + } + return result +} + + +Coor.set_orientation = function(orientation, reference_coordinate_system){ + let result, current_pose + let ref = reference_coordinate_system + if(reference_coordinate_system === undefined){ + this.pose = Vector.make_pose(this.get_position(), orientation) + result = this.pose + }else{ + let base_ancestors = this.ancestors() + let parent = base_ancestors[base_ancestors.length-1] + let temp_orientation = Vector.matrix_multiply(ref.get_orientation(parent), orientation) + this.pose = Vector.make_pose(this.get_position(), temp_orientation) + result = this.pose + } + return result +} + +Coor.is_Coor = function(){ + +} + +Coor.insert = function(){ + Object.set_prototypeof +} + +Coor.move_points_to_coor = function(points, destination_coordinate_system, reference_coordinate_system){ + let dest = destination_coordinate_system + let ref = reference_coordinate_system + + if(ref === undefined){ + ref = Table + } + let trans = dest.get_pose(Table) + let result = points + let dim = Vector.matrix_dimensions(points) + if(dim[1] == 3){ + for(let i = 0; i < dim[0]; i++){ + result[i] = Vector.transpose(Vector.matrix_multiply(trans, Vector.properly_define_point(points[i]))) + } + result = Vector.pull(result, [0, dim[0] - 1], [0, 2]) + } + + return result +} +/* +var board = Table.create_child(Vector.make_pose([0, 0, 1000], [0, 0, 324000]), "board") +var points = [[1, 2, 3], [4, 5, 6]] +debugger +var result = Coor.move_points_to_coor(points, board) +*/ + +Coor.rotate = function(axis_of_rotation, angle, point_of_rotation, reference_coordinate_system){ + let pose, result + let ref = reference_coordinate_system + if(ref === undefined){ + pose = this.get_pose() + if(point_of_rotation == undefined){ + point_of_rotation = this.get_position() + } + pose = Vector.rotate_pose(pose, axis_of_rotation, angle, point_of_rotation) + this.set_pose(pose) + result = this.pose + }else{ + pose = this.get_pose(ref) + if(point_of_rotation == undefined){ + point_of_rotation = [0, 0, 0] + } + pose = Vector.rotate_pose(pose, axis_of_rotation, angle, point_of_rotation) + this.set_pose(pose, ref) + result = this.pose + } + return result +} + +new TestSuite("Coordinate Object System", + ['table = Coor.create_child(Vector.make_pose(), "table")', "Coor.table"], + ['J0 = table.create_child(Vector.make_pose(), "J0")', "Coor.table.J0"], + ['J1 = J0.create_child(Vector.make_pose([10, 0, 0]), "J1")', "Coor.table.J0.J1"], + ['J2 = J1.create_child(Vector.make_pose([0, 0, 20]), "J2")', "TestSuite.error"], + ['J3 = J2.create_child(Vector.make_pose([0, 0, 20]), "J3")', "Coor.table.J0.J1.J2.J3"], + ['cube = table.create_child(Vector.make_pose([15, 10, 5], [0, 0, 0]), "cube")', "Coor.table.cube"], + ["J3.get_pose()", "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 20], [0, 0, 0, 1]]"], + ["J3.get_pose(table)", "[[1, 0, 0, 10], [0, 1, 0, 0], [0, 0, 1, 40], [0, 0, 0, 1]]"], + ["cube.get_pose()", "[[1, 0, 0, 15], [0, 1, 0, 10], [0, 0, 1, 5], [0, 0, 0, 1]]"], + ["J3.get_pose(cube)", "[[1, 0, 0, -5], [0, 1, 0, -10], [0, 0, 1, 35], [0, 0, 0, 1]]"], + ["cube.get_pose(J3)", "[[1, 0, 0, 5], [0, 1, 0, 10], [0, 0, 1, -35], [0, 0, 0, 1]]"], + ['cube = table.create_child(Vector.make_pose([15, 10, 5], [0, 0, Convert.degrees_to_arcseconds(45)]), "cube")', "Coor.table.cube"], + ["cube.get_pose()", "[ [0.7071067811865476, 0.7071067811865475, 0, 15], [-0.7071067811865475, 0.7071067811865476, 0, 10], [0, 0, 1, 5], [0, 0, 0, 1]]"], + ["J3.get_pose(cube)", "[ [0.7071067811865476, -0.7071067811865475, 0, 6.464466094067261], [0.7071067811865475, 0.7071067811865476, 0, -17.67766952966369], [0, 0, 1, 35], [0, 0, 0, 1]]"], + ["J3.get_position()", "[0, 0, 20]"], + ["J3.get_pose(cube)", "[ [0.7071067811865476, -0.7071067811865475, 0, 6.464466094067261], [0.7071067811865475, 0.7071067811865476, 0, -17.67766952966369], [0, 0, 1, 35], [0, 0, 0, 1]]"], + ["J3.get_orientation()", "[[1, 0, 0], [0, 1, 0], [0, 0, 1]]"], + ["J3.get_orientation(cube)", "[ [0.7071067811865476, -0.7071067811865475, 0], [0.7071067811865475, 0.7071067811865476, 0], [0, 0, 1]]"], + ["J3.set_pose([[1, 0, 0, 10], [0, 1, 0, 10],[0, 0, 1, 10],[0, 0, 0, 1]])", "[[1, 0, 0, 10], [0, 1, 0, 10], [0, 0, 1, 10], [0, 0, 0, 1]]"], + ["J3.set_pose([[1, 0, 0, 10], [0, 1, 0, 10],[0, 0, 1, 10],[0, 0, 0, 1]], table) ", "[[1, 0, 0, 0], [0, 1, 0, 10], [0, 0, 1, -10], [0, 0, 0, 1]]"], + ["J3.set_position([10, 10, 10])", "[[1, 0, 0, 10], [0, 1, 0, 10], [0, 0, 1, 10], [0, 0, 0, 1]]"], + ["J3.set_position([10, 10, 10], table)", "[[1, 0, 0, 0], [0, 1, 0, 10], [0, 0, 1, -10], [0, 0, 0, 1]]"], + ["J3.set_orientation([[-1, 0, 0], [0, 0, 1], [0, 0, 1]])", "[[-1, 0, 0, 0], [0, 0, 1, 10], [0, 0, 1, -10], [0, 0, 0, 1]]"], + ["J3.set_orientation([[-1, 0, 0], [0, 0, 1], [0, 0, 1]], cube)", "[[-1, 0, 0, 0], [0, 0, 1, 10], [0, 0, 1, -10], [0, 0, 0, 1]]"] +) + + + + +/* +Table = Coor.create_child(Vector.make_pose(), "Table") +J0 = table.create_child(Vector.make_pose(), "J0") +J1 = J0.create_child(Vector.make_pose([10, 0, 0]), "J1") +J2 = J1.create_child(Vector.make_pose([0, 0, 20]), "J2") +J3 = J2.create_child(Vector.make_pose([0, 0, 20]), "J3") +cube = table.create_child(Vector.make_pose([15, 10, 5], [0, 0, Convert.degrees_to_arcseconds(0)]), "cube") + +debugger + +J3.set_pose([[ 0.707, 0.707, 0, 15], [-0.707, 0.707, 0, 10],[0, 0, 1, 5],[0, 0, 0, 1]]) +J3.set_pose([[1, 0, 0, 10], [0, 1, 0, 10],[0, 0, 1, 10],[0, 0, 0, 1]]) +J3.set_pose([[1, 0, 0, 10], [0, 1, 0, 10],[0, 0, 1, 10],[0, 0, 0, 1]], table) +J3.set_position([10, 10, 10]) +J3.set_position([10, 10, 10], table) +debugger +J3.set_orientation(Convert.angles_to_DCM([0, 0, Convert.degrees_to_arcseconds(90)])) +J3.set_orientation([[-1, 0, 0], [0, 0, 1], [0, 0, 1]]) +J3.set_orientation([[-1, 0, 0], [0, 0, 1], [0, 0, 1]], cube) + + +J3.get_pose() +J3.get_pose(table) + +cube.get_pose() + +J3.get_pose(cube) +cube.get_pose(J3) + +cube = table.create_child(Vector.make_pose([15, 10, 5], [0, 0, Convert.degrees_to_arcseconds(45)]), "cube") +J3.get_pose(cube) +cube.get_pose(J3) + + +J3.get_position() +J3.get_position(cube) + +J3.get_orientation() +J3.get_orientation(cube) +*/ \ No newline at end of file diff --git a/math/DXF b/math/DXF new file mode 100644 index 00000000..8aeeddaf --- /dev/null +++ b/math/DXF @@ -0,0 +1,2214 @@ +//DXF class +//James Wigglesworth +//Started: 2_10_2017 +//Updated: 3_12_2017 + + +var DXF = new function(){ + + this.content_to_entities = function(content){ + let parser = new DxfParser() + return parser.parseSync(content).entities + } + + this.entities_to_points = function(dxf_entities){ + let ent, a, b + let points = [] + for(let i = 0; i < dxf_entities.length; i++){ + ent = dxf_entities[i] + if (ent.type == "LINE"){ + a = point_object_to_array(ent.vertices[0]) + b = point_object_to_array(ent.vertices[1]) + points.push(a) + points.push(b) + } + }//end for + return points + } + + this.edit = function(points, shift = [0, 0, 0], scale_factor = 1){ + let dim = Vector.matrix_dimensions(points) + let result = points + for(let i = 0; i < dim[0]; i++){ + result[i] = Vector.add(shift, points[i]) + } + return Vector.multiply(scale_factor, result) + } + + this.points_to_path = function(points, lift_height){ + let path = [] + let Ua, Ub, Ub_old + let dim = Vector.matrix_dimensions(points) + path.push(Vector.add(points[0], [0, 0, lift_height])) + path.push(points[0]) + path.push(points[1]) + let rapid = [1, 1, 0] + Ub_old = points[1] + for(let i = 3; i < dim[0]; i+=2){ + Ua = points[i-1] + Ub = points[i] + if(!Vector.is_equal(Ua, Ub_old, 10)){ + path.push(Vector.add(Ub_old, [0, 0, lift_height])) + path.push(Vector.add(Ua, [0, 0, lift_height])) + path.push(Ua) + rapid.push(1) + rapid.push(1) + rapid.push(1) + } + path.push(Ub) + rapid.push(0) + Ub_old = Ub + } + path.push(Vector.add(Ub, [0, 0, lift_height])) + rapid.push(1) + return [path, rapid] + } + + this.move_path_to_coor = function(path, destination_coordinate_system){ + return Coor.move_points_to_coor(path, destination_coordinate_system) + } + + this.auto_fit = function(dxf_points, J_angles_1, J_angles_2, J_angles_3){ + let points_A, points_B, points_C, UA5, UA4, UB5, UB4, UC5, UC4, U5_ave, U4_ave, U45 + let point, x_vector, y_vector, z_vector, pose, angleA, angleB, angleC, vector_1, vector_2, x_length, y_length, y_dist_1, y_dist_2 + + points_A = Kin.J_angles_to_xyz(J_angles_1) + points_B = Kin.J_angles_to_xyz(J_angles_2) + points_C = Kin.J_angles_to_xyz(J_angles_3) + + UA5 = points_A[5] + UA4 = points_A[4] + UB5 = points_B[5] + UB4 = points_B[4] + UC5 = points_C[5] + UC4 = points_C[4] + + U5_ave = Vector.average(UA5, UB5, UC5) + U4_ave = Vector.average(UA4, UB4, UC4) + U45 = Vector.subtract(U4_ave, U5_ave) + + angleA = Vector.angle(Vector.subtract(UB5, UA5), Vector.subtract(UC5, UA5)) + angleB = Vector.angle(Vector.subtract(UA5, UB5), Vector.subtract(UC5, UB5)) + angleC = Vector.angle(Vector.subtract(UB5, UC5), Vector.subtract(UA5, UC5)) + + switch(Math.max(angleA, angleB, angleC)){ + case angleA: + point = UA5 + vector_1 = Vector.subtract(UB5, UA5) + vector_2 = Vector.subtract(UC5, UA5) + y_dist_1 = Vector.distance(UC5, point, UB5) + y_dist_2 = Vector.distance(UB5, point, UC5) + break + + case angleB: + point = UB5 + vector_1 = Vector.subtract(UA5, UB5) + vector_2 = Vector.subtract(UC5, UB5) + y_dist_1 = Vector.distance(UC5, point, UA5) + y_dist_2 = Vector.distance(UA5, point, UC5) + break + + case angleC: + point = UC5 + vector_1 = Vector.subtract(UB5, UC5) + vector_2 = Vector.subtract(UA5, UC5) + y_dist_1 = Vector.distance(UA5, point, UB5) + y_dist_2 = Vector.distance(UB5, point, UA5) + break + } + + if(0 < Vector.dot(Vector.cross(vector_1, vector_2), U45)){ + x_length = Vector.magnitude(vector_1) + x_vector = Vector.normalize(vector_1) + y_length = y_dist_1 + }else{ + x_length = Vector.magnitude(vector_2) + x_vector = Vector.normalize(vector_2) + y_length = y_dist_2 + } + + let upper = Vector.max(dxf_points) + let lower = Vector.min(dxf_points) + let diff = Vector.subtract(upper, lower) + let shift = Vector.multiply(-1, lower) + let width = Math.abs(diff[0]) + let height = Math.abs(diff[1]) + + let scale_factor_1 = x_length / width + let scale_factor_2 = y_length / height + + return [shift, Math.min(scale_factor_1, scale_factor_2)] + } + + + this.draw_dxf_on_plane = function(dxf_content = Hello_World_DXF, J_angles){ + let lift_height = 25000 + let low_speed = 60000 + let high_speed = 130000 + let J_angles_1 = J_angles[0] + let J_angles_2 = J_angles[1] + let J_angles_3 = J_angles[2] + + + let my_pose = Kin.three_positions_to_pose(J_angles_1, J_angles_2, J_angles_3) + let J5_dir = Vector.multiply(-1, Vector.pull(my_pose, [0, 2], 2)) + var white_board = Table.create_child(my_pose, "white_board") + let my_entities = DXF.content_to_entities(dxf_content) + let my_points = DXF.entities_to_points(my_entities) + let my_edit = DXF.auto_fit(my_points, J_angles_1, J_angles_2, J_angles_3) + my_points = DXF.edit(my_points, my_edit[0], my_edit[1]) + let my_path = DXF.points_to_path(my_points, lift_height) + let path_points = my_path[0] + let rapid = my_path[1] + path_points = Coor.move_points_to_coor(path_points, white_board) + let dim = Vector.matrix_dimensions(path_points) + let movCMD = [] + + let temp_mov + movCMD.push(Dexter.move_to(path_points[0], J5_dir)) + for(let i = 1; i < dim[0]; i++){ + if(rapid[i] == 0){ + temp_mov = move_straight_to(path_points[i-1], path_points[i], 500, J5_dir) + for(let j = 0; j < temp_mov.length; j++){ + movCMD.push(temp_mov[j]) + } + }else{ + temp_mov = move_straight_to(path_points[i-1], path_points[i], 200000, J5_dir) + for(let j = 0; j < temp_mov.length; j++){ + movCMD.push(temp_mov[j]) + } + } + } + return movCMD + } + + + +} + + + + + + +/******************************************************************/ + + +/*downloaded from https://github.com/gdsestimating/dxf-parser on Aug 15 by Fry and hacked +to remove all the export junk. +This version of the file contains all 3 of the color table, scanner, and parser. +The last mod date on the file is May 27, 2016 + */ + +/** + * AutoCad files sometimes use an indexed color value between 1 and 255 inclusive. + * Each value corresponds to a color. index 1 is red, that is 16711680 or 0xFF0000. + * index 0 and 256, while included in this array, are actually reserved for inheritance + * values in AutoCad so they should not be used for index color lookups. + */ + +var AUTO_CAD_COLOR_INDEX = [ + 0, + 16711680, + 16776960, + 65280, + 65535, + 255, + 16711935, + 16777215, + 8421504, + 12632256, + 16711680, + 16744319, + 13369344, + 13395558, + 10027008, + 10046540, + 8323072, + 8339263, + 4980736, + 4990502, + 16727808, + 16752511, + 13382400, + 13401958, + 10036736, + 10051404, + 8331008, + 8343359, + 4985600, + 4992806, + 16744192, + 16760703, + 13395456, + 13408614, + 10046464, + 10056268, + 8339200, + 8347455, + 4990464, + 4995366, + 16760576, + 16768895, + 13408512, + 13415014, + 10056192, + 10061132, + 8347392, + 8351551, + 4995328, + 4997670, + 16776960, + 16777087, + 13421568, + 13421670, + 10000384, + 10000460, + 8355584, + 8355647, + 5000192, + 5000230, + 12582656, + 14679935, + 10079232, + 11717734, + 7510016, + 8755276, + 6258432, + 7307071, + 3755008, + 4344870, + 8388352, + 12582783, + 6736896, + 10079334, + 5019648, + 7510092, + 4161280, + 6258495, + 2509824, + 3755046, + 4194048, + 10485631, + 3394560, + 8375398, + 2529280, + 6264908, + 2064128, + 5209919, + 1264640, + 3099686, + 65280, + 8388479, + 52224, + 6736998, + 38912, + 5019724, + 32512, + 4161343, + 19456, + 2509862, + 65343, + 8388511, + 52275, + 6737023, + 38950, + 5019743, + 32543, + 4161359, + 19475, + 2509871, + 65407, + 8388543, + 52326, + 6737049, + 38988, + 5019762, + 32575, + 4161375, + 19494, + 2509881, + 65471, + 8388575, + 52377, + 6737074, + 39026, + 5019781, + 32607, + 4161391, + 19513, + 2509890, + 65535, + 8388607, + 52428, + 6737100, + 39064, + 5019800, + 32639, + 4161407, + 19532, + 2509900, + 49151, + 8380415, + 39372, + 6730444, + 29336, + 5014936, + 24447, + 4157311, + 14668, + 2507340, + 32767, + 8372223, + 26316, + 6724044, + 19608, + 5010072, + 16255, + 4153215, + 9804, + 2505036, + 16383, + 8364031, + 13260, + 6717388, + 9880, + 5005208, + 8063, + 4149119, + 4940, + 2502476, + 255, + 8355839, + 204, + 6710988, + 152, + 5000344, + 127, + 4145023, + 76, + 2500172, + 4129023, + 10452991, + 3342540, + 8349388, + 2490520, + 6245528, + 2031743, + 5193599, + 1245260, + 3089996, + 8323327, + 12550143, + 6684876, + 10053324, + 4980888, + 7490712, + 4128895, + 6242175, + 2490444, + 3745356, + 12517631, + 14647295, + 10027212, + 11691724, + 7471256, + 8735896, + 6226047, + 7290751, + 3735628, + 4335180, + 16711935, + 16744447, + 13369548, + 13395660, + 9961624, + 9981080, + 8323199, + 8339327, + 4980812, + 4990540, + 16711871, + 16744415, + 13369497, + 13395634, + 9961586, + 9981061, + 8323167, + 8339311, + 4980793, + 4990530, + 16711807, + 16744383, + 13369446, + 13395609, + 9961548, + 9981042, + 8323135, + 8339295, + 4980774, + 4990521, + 16711743, + 16744351, + 13369395, + 13395583, + 9961510, + 9981023, + 8323103, + 8339279, + 4980755, + 4990511, + 3355443, + 5987163, + 8684676, + 11382189, + 14079702, + 16777215 +] + +/** + * DxfArrayScanner + * + * Based off the AutoCad 2012 DXF Reference + * http://images.autodesk.com/adsk/files/autocad_2012_pdf_dxf-reference_enu.pdf + * + * Reads through an array representing lines of a dxf file. Takes an array and + * provides an easy interface to extract group code and value pairs. + * @param data - an array where each element represents a line in the dxf file + * @constructor + */ +function DxfArrayScanner(data) { + this._pointer = 0; + this._data = data; + this._eof = false; +} + +/** + * Gets the next group (code, value) from the array. A group is two consecutive elements + * in the array. The first is the code, the second is the value. + * @returns {{code: Number}|*} + */ +DxfArrayScanner.prototype.next = function() { + var group; + if(!this.hasNext()) { + if(!this._eof) + throw new Error('Unexpected end of input: EOF group not read before end of file. Ended on code ' + this._data[this._pointer]); + else + throw new Error('Cannot call \'next\' after EOF group has been read'); + } + + group = { + code: parseInt(this._data[this._pointer]) + }; + + this._pointer++; + + group.value = parseGroupValue(group.code, this._data[this._pointer].trim()); + + this._pointer++; + + if(group.code === 0 && group.value === 'EOF') this._eof = true; + + return group; +}; + +/** + * Returns true if there is another code/value pair (2 elements in the array). + * @returns {boolean} + */ +DxfArrayScanner.prototype.hasNext = function() { + // Check if we have read EOF group code + if(this._eof) { + return false; + } + + // We need to be sure there are two lines available + if(this._pointer > this._data.length - 2) { + return false; + } + return true; +}; + +/** + * Returns true if the scanner is at the end of the array + * @returns {boolean} + */ +DxfArrayScanner.prototype.isEOF = function() { + return this._eof; +}; + +/** + * Parse a value to its proper type. + * See pages 3 - 10 of the AutoCad DXF 2012 reference given at the top of this file + * + * @param code + * @param value + * @returns {*} + */ +function parseGroupValue(code, value) { + if(code <= 9) return value; + if(code >= 10 && code <= 59) return parseFloat(value); + if(code >= 60 && code <= 99) return parseInt(value); + if(code >= 100 && code <= 109) return value; + if(code >= 110 && code <= 149) return parseFloat(value); + if(code >= 160 && code <= 179) return parseInt(value); + if(code >= 210 && code <= 239) return parseFloat(value); + if(code >= 270 && code <= 289) return parseInt(value); + if(code >= 290 && code <= 299) return parseBoolean(value); + if(code >= 300 && code <= 369) return value; + if(code >= 370 && code <= 389) return parseInt(value); + if(code >= 390 && code <= 399) return value; + if(code >= 400 && code <= 409) return parseInt(value); + if(code >= 410 && code <= 419) return value; + if(code >= 420 && code <= 429) return parseInt(value); + if(code >= 430 && code <= 439) return value; + if(code >= 440 && code <= 459) return parseInt(value); + if(code >= 460 && code <= 469) return parseFloat(value); + if(code >= 470 && code <= 481) return value; + if(code === 999) return value; + if(code >= 1000 && code <= 1009) return value; + if(code >= 1010 && code <= 1059) return parseFloat(value); + if(code >= 1060 && code <= 1071) return parseInt(value); + + console.log('WARNING: Group code does not have a defined type: %j', { code: code, value: value }); + return value; +} + +/** + * Parse a boolean according to a 1 or 0 value + * @param str + * @returns {boolean} + */ +function parseBoolean(str) { + if(str === '0') return false; + if(str === '1') return true; + throw TypeError('String \'' + str + '\' cannot be cast to Boolean type'); +} + + +//fry comment out var log = require('loglevel'); + +//log.setLevel('trace'); +//log.setLevel('debug'); +//log.setLevel('info'); +//log.setLevel('warn'); +//fry comment out log.setLevel('error'); +//log.setLevel('silent'); + + +function DxfParser(stream) {} + +DxfParser.prototype.parse = function(source, done) { + throw new Error("read() not implemented. Use readSync()"); +}; + +DxfParser.prototype.parseSync = function(source) { + if(typeof(source) === 'string') { + return this._parse(source); + }else { + console.error('Cannot read dxf source of type `' + typeof(source)); + return null; + } +}; + +DxfParser.prototype.parseStream = function(stream, done) { + + var dxfString = ""; + var self = this; + + stream.on('data', onData); + stream.on('end', onEnd); + stream.on('error', onError); + + function onData(chunk) { + dxfString += chunk; + } + + function onEnd() { + try { + var dxf = self._parse(dxfString); + }catch(err) { + return done(err); + } + done(null, dxf); + } + + function onError(err) { + done(err); + } +}; + +DxfParser.prototype._parse = function(dxfString) { + var scanner, curr, dxf = {}, lastHandle = 0; + var dxfLinesArray = dxfString.split(/\r\n|\r|\n/g); + + scanner = new DxfArrayScanner(dxfLinesArray); + if(!scanner.hasNext()) throw Error('Empty file'); + + var parseAll = function() { + curr = scanner.next(); + while(!scanner.isEOF()) { + if(curr.code === 0 && curr.value === 'SECTION') { + curr = scanner.next(); + + // Be sure we are reading a section code + if (curr.code !== 2) { + console.error('Unexpected code %s after 0:SECTION', debugCode(curr)); + curr = scanner.next(); + continue; + } + + if (curr.value === 'HEADER') { + log.debug('> HEADER'); + dxf.header = parseHeader(); + log.debug('<'); + } else if (curr.value === 'BLOCKS') { + log.debug('> BLOCKS'); + dxf.blocks = parseBlocks(); + log.debug('<'); + } else if(curr.value === 'ENTITIES') { + log.debug('> ENTITIES'); + dxf.entities = parseEntities(false); + log.debug('<'); + } else if(curr.value === 'TABLES') { + log.debug('> TABLES'); + dxf.tables = parseTables(); + log.debug('<'); + } else if(curr.value === 'EOF') { + log.debug('EOF'); + } else { + log.warn('Skipping section \'%s\'', curr.value); + } + } else { + curr = scanner.next(); + } + // If is a new section + } + }; + + var groupIs = function(code, value) { + return curr.code === code && curr.value === value; + }; + + /** + * + * @return {object} header + */ + var parseHeader = function() { + // interesting variables: + // $ACADVER, $VIEWDIR, $VIEWSIZE, $VIEWCTR, $TDCREATE, $TDUPDATE + // http://www.autodesk.com/techpubs/autocad/acadr14/dxf/header_section_al_u05_c.htm + // Also see VPORT table entries + var currVarName = null, currVarValue = null; + var header = {}; + // loop through header variables + curr = scanner.next(); + + while(true) { + if(groupIs(0, 'ENDSEC')) { + if(currVarName) header[currVarName] = currVarValue; + break; + } else if(curr.code === 9) { + if(currVarName) header[currVarName] = currVarValue; + currVarName = curr.value; + // Filter here for particular variables we are interested in + } else { + if(curr.code === 10) { + currVarValue = { x: curr.value }; + } else if(curr.code === 20) { + currVarValue.y = curr.value; + } else if(curr.code === 30) { + currVarValue.z = curr.value; + } else { + currVarValue = curr.value; + } + } + curr = scanner.next(); + } + // console.log(util.inspect(header, { colors: true, depth: null })); + curr = scanner.next(); // swallow up ENDSEC + return header; + }; + + + /** + * + */ + var parseBlocks = function() { + var blocks = {}, block; + + curr = scanner.next(); + + while(curr.value !== 'EOF') { + if(groupIs(0, 'ENDSEC')) { + break; + } + + if(groupIs(0, 'BLOCK')) { + log.debug('block {'); + block = parseBlock(); + log.debug('}'); + ensureHandle(block); + if(!block.name) + log.error('block with handle "' + block.handle + '" is missing a name.'); + else + blocks[block.name] = block; + } else { + logUnhandledGroup(curr); + curr = scanner.next(); + } + } + return blocks; + }; + + var parseBlock = function() { + var block = {}; + curr = scanner.next(); + + while(curr.value !== 'EOF') { + switch(curr.code) { + case 1: + block.xrefPath = curr.value; + curr = scanner.next(); + break; + case 2: + block.name = curr.value; + curr = scanner.next(); + break; + case 3: + block.name2 = curr.value; + curr = scanner.next(); + break; + case 5: + block.handle = curr.value; + curr = scanner.next(); + break; + case 8: + block.layer = curr.value; + curr = scanner.next(); + break; + case 10: + block.position = parsePoint(); + break; + case 67: + block.paperSpace = (curr.value && curr.value == 1) ? true : false; + curr = scanner.next(); + break; + case 70: + if (curr.value != 0) { + //if(curr.value & BLOCK_ANONYMOUS_FLAG) console.log(' Anonymous block'); + //if(curr.value & BLOCK_NON_CONSTANT_FLAG) console.log(' Non-constant attributes'); + //if(curr.value & BLOCK_XREF_FLAG) console.log(' Is xref'); + //if(curr.value & BLOCK_XREF_OVERLAY_FLAG) console.log(' Is xref overlay'); + //if(curr.value & BLOCK_EXTERNALLY_DEPENDENT_FLAG) console.log(' Is externally dependent'); + //if(curr.value & BLOCK_RESOLVED_OR_DEPENDENT_FLAG) console.log(' Is resolved xref or dependent of an xref'); + //if(curr.value & BLOCK_REFERENCED_XREF) console.log(' This definition is a referenced xref'); + block.type = curr.value; + } + curr = scanner.next(); + break; + case 100: + // ignore class markers + curr = scanner.next(); + break; + case 330: + block.ownerHandle = curr.value; + curr = scanner.next(); + break; + case 0: + if(curr.value == 'ENDBLK') + break; + block.entities = parseEntities(true); + break; + default: + logUnhandledGroup(curr); + curr = scanner.next(); + } + + if(groupIs(0, 'ENDBLK')) { + curr = scanner.next(); + break; + } + } + return block; + }; + + /** + * parseTables + * @return {Object} Object representing tables + */ + var parseTables = function() { + var tables = {}; + curr = scanner.next(); + while(curr.value !== 'EOF') { + if(groupIs(0, 'ENDSEC')) + break; + + if(groupIs(0, 'TABLE')) { + curr = scanner.next(); + + var tableDefinition = tableDefinitions[curr.value]; + if(tableDefinition) { + log.debug(curr.value + ' Table {'); + tables[tableDefinitions[curr.value].tableName] = parseTable(); + log.debug('}'); + } else { + log.debug('Unhandled Table ' + curr.value); + } + } else { + // else ignored + curr = scanner.next(); + } + } + + curr = scanner.next(); + return tables; + }; + + const END_OF_TABLE_VALUE = 'ENDTAB'; + + var parseTable = function() { + var tableDefinition = tableDefinitions[curr.value], + table = {}, + expectedCount = 0, + actualCount; + + curr = scanner.next(); + while(!groupIs(0, END_OF_TABLE_VALUE)) { + + switch(curr.code) { + case 5: + table.handle = curr.value; + curr = scanner.next(); + break; + case 330: + table.ownerHandle = curr.value; + curr = scanner.next(); + break; + case 100: + if(curr.value === 'AcDbSymbolTable') { + // ignore + curr = scanner.next(); + }else{ + logUnhandledGroup(curr); + curr = scanner.next(); + } + break; + case 70: + expectedCount = curr.value; + curr = scanner.next(); + break; + case 0: + if(curr.value === tableDefinition.dxfSymbolName) { + table[tableDefinition.tableRecordsProperty] = tableDefinition.parseTableRecords(); + } else { + logUnhandledGroup(curr); + curr = scanner.next(); + } + break; + default: + logUnhandledGroup(curr); + curr = scanner.next(); + } + } + var tableRecords = table[tableDefinition.tableRecordsProperty]; + if(tableRecords) { + if(tableRecords.constructor === Array){ + actualCount = tableRecords.length; + } else if(typeof(tableRecords) === 'object') { + actualCount = Object.keys(tableRecords).length; + } + if(expectedCount !== actualCount) log.warn('Parsed ' + actualCount + ' ' + tableDefinition.dxfSymbolName + '\'s but expected ' + expectedCount); + } + curr = scanner.next(); + return table; + }; + + var parseViewPortRecords = function() { + var viewPorts = [], // Multiple table entries may have the same name indicating a multiple viewport configuration + viewPort = {}; + + log.debug('ViewPort {'); + curr = scanner.next(); + while(!groupIs(0, END_OF_TABLE_VALUE)) { + + switch(curr.code) { + case 2: // layer name + viewPort.name = curr.value; + curr = scanner.next(); + break; + case 10: + viewPort.lowerLeftCorner = parsePoint(); + break; + case 11: + viewPort.upperRightCorner = parsePoint(); + break; + case 12: + viewPort.center = parsePoint(); + break; + case 13: + viewPort.snapBasePoint = parsePoint(); + break; + case 14: + viewPort.snapSpacing = parsePoint(); + break; + case 15: + viewPort.gridSpacing = parsePoint(); + break; + case 16: + viewPort.viewDirectionFromTarget = parsePoint(); + break; + case 17: + viewPort.viewTarget = parsePoint(); + break; + case 42: + viewPort.lensLength = curr.value; + curr = scanner.next(); + break; + case 43: + viewPort.frontClippingPlane = curr.value; + curr = scanner.next(); + break; + case 44: + viewPort.backClippingPlane = curr.value; + curr = scanner.next(); + break; + case 45: + viewPort.viewHeight = curr.value; + curr = scanner.next(); + break; + case 50: + viewPort.snapRotationAngle = curr.value; + curr = scanner.next(); + break; + case 51: + viewPort.viewTwistAngle = curr.value; + curr = scanner.next(); + break; + case 79: + viewPort.orthographicType = curr.value; + curr = scanner.next(); + break; + case 110: + viewPort.ucsOrigin = parsePoint(); + break; + case 111: + viewPort.ucsXAxis = parsePoint(); + break; + case 112: + viewPort.ucsYAxis = parsePoint(); + break; + case 110: + viewPort.ucsOrigin = parsePoint(); + break; + case 281: + viewPort.renderMode = curr.value; + curr = scanner.next(); + break; + case 281: + // 0 is one distant light, 1 is two distant lights + viewPort.defaultLightingType = curr.value; + curr = scanner.next(); + break; + case 292: + viewPort.defaultLightingOn = curr.value; + curr = scanner.next(); + break; + case 330: + viewPort.ownerHandle = curr.value; + curr = scanner.next(); + break; + case 63: + case 421: + case 431: + viewPort.ambientColor = curr.value; + curr = scanner.next(); + break; + case 0: + // New ViewPort + if(curr.value === 'VPORT') { + log.debug('}'); + viewPorts.push(viewPort); + log.debug('ViewPort {'); + viewPort = {}; + curr = scanner.next(); + } + break; + default: + logUnhandledGroup(curr); + curr = scanner.next(); + break; + } + } + // Note: do not call scanner.next() here, + // parseTable() needs the current group + log.debug('}'); + viewPorts.push(viewPort); + + return viewPorts; + }; + + var parseLineTypes = function() { + var ltypes = {}, + ltypeName, + ltype = {}, + length; + + log.debug('LType {'); + curr = scanner.next(); + while(!groupIs(0, 'ENDTAB')) { + + switch(curr.code) { + case 2: + ltype.name = curr.value; + ltypeName = curr.value; + curr = scanner.next(); + break; + case 3: + ltype.description = curr.value; + curr = scanner.next(); + break; + case 73: // Number of elements for this line type (dots, dashes, spaces); + length = curr.value; + if(length > 0) ltype.pattern = []; + curr = scanner.next(); + break; + case 40: // total pattern length + ltype.patternLength = curr.value; + curr = scanner.next(); + break; + case 49: + ltype.pattern.push(curr.value); + curr = scanner.next(); + break; + case 0: + log.debug('}'); + if(length > 0 && length !== ltype.pattern.length) log.warn('lengths do not match on LTYPE pattern'); + ltypes[ltypeName] = ltype; + ltype = {}; + log.debug('LType {'); + curr = scanner.next(); + break; + default: + curr = scanner.next(); + } + } + + log.debug('}'); + ltypes[ltypeName] = ltype; + return ltypes; + }; + + var parseLayers = function() { + var layers = {}, + layerName, + layer = {}; + + log.debug('Layer {'); + curr = scanner.next(); + while(!groupIs(0, 'ENDTAB')) { + + switch(curr.code) { + case 2: // layer name + layer.name = curr.value; + layerName = curr.value; + curr = scanner.next(); + break; + case 62: // color, visibility + layer.visible = curr.value <= 0; + // TODO 0 and 256 are BYBLOCK and BYLAYER respectively. Need to handle these values for layers?. + layer.color = getAcadColor(Math.abs(curr.value)); + curr = scanner.next(); + break; + case 0: + // New Layer + if(curr.value === 'LAYER') { + log.debug('}'); + layers[layerName] = layer; + log.debug('Layer {'); + layer = {}; + layerName = undefined; + curr = scanner.next(); + } + break; + default: + logUnhandledGroup(curr); + curr = scanner.next(); + break; + } + } + // Note: do not call scanner.next() here, + // parseLayerTable() needs the current group + log.debug('}'); + layers[layerName] = layer; + + return layers; + }; + + var tableDefinitions = { + VPORT: { + tableRecordsProperty: 'viewPorts', + tableName: 'viewPort', + dxfSymbolName: 'VPORT', + parseTableRecords: parseViewPortRecords + }, + LTYPE: { + tableRecordsProperty: 'lineTypes', + tableName: 'lineType', + dxfSymbolName: 'LTYPE', + parseTableRecords: parseLineTypes + }, + LAYER: { + tableRecordsProperty: 'layers', + tableName: 'layer', + dxfSymbolName: 'LAYER', + parseTableRecords: parseLayers + } + }; + + /** + * Is called after the parser first reads the 0:ENTITIES group. The scanner + * should be on the start of the first entity already. + * @return {Array} the resulting entities + */ + var parseEntities = function(forBlock) { + var entities = []; + + var endingOnValue = forBlock ? 'ENDBLK' : 'ENDSEC'; + + if (!forBlock) { + curr = scanner.next(); + } + while(true) { + + if(curr.code === 0) { + if(curr.value === endingOnValue) { + break; + } + + var entity; + // Supported entities here + if(curr.value === 'LWPOLYLINE') { + log.debug('LWPOLYLINE {'); + entity = parseLWPOLYLINE(); + log.debug('}') + } else if(curr.value === 'POLYLINE') { + log.debug('POLYLINE {'); + entity = parsePOLYLINE(); + log.debug('}'); + } else if(curr.value === 'LINE') { + log.debug('LINE {'); + entity = parseLINE(); + log.debug('}'); + } else if(curr.value === 'CIRCLE') { + log.debug('CIRCLE {'); + entity = parseCIRCLE(); + log.debug('}'); + } else if(curr.value === 'ARC') { + log.debug('ARC {'); + // similar properties to circle? + entity = parseCIRCLE(); + log.debug('}') + } else if(curr.value === 'TEXT') { + log.debug('TEXT {'); + entity = parseTEXT(); + log.debug('}') + } else if(curr.value === 'DIMENSION') { + log.debug('DIMENSION {'); + entity = parseDIMENSION(); + log.debug('}') + } else if(curr.value === 'SOLID') { + log.debug('SOLID {'); + entity = parseSOLID(); + log.debug('}') + } else if(curr.value === 'POINT') { + log.debug('POINT {'); + entity = parsePOINT(); + log.debug('}') + } else if(curr.value === 'MTEXT') { + log.debug('MTEXT {'); + entity = parseMTEXT(); + log.debug('}') + } else if(curr.value === 'ATTDEF') { + log.debug('ATTDEF {'); + entity = parseATTDEF(); + log.debug('}') + } else { + log.warn('Unhandled entity ' + curr.value); + curr = scanner.next(); + continue; + } + ensureHandle(entity); + entities.push(entity); + } else { + // ignored lines from unsupported entity + curr = scanner.next(); + } + } + // console.log(util.inspect(entities, { colors: true, depth: null })); + if(endingOnValue == 'ENDSEC') curr = scanner.next(); // swallow up ENDSEC, but not ENDBLK + return entities; + }; + + /** + * + * @param entity + */ + var checkCommonEntityProperties = function(entity) { + switch(curr.code) { + case 0: + entity.type = curr.value; + curr = scanner.next(); + break; + case 5: + entity.handle = curr.value; + curr = scanner.next(); + break; + case 6: + entity.lineType = curr.value; + curr = scanner.next(); + break; + case 8: // Layer name + entity.layer = curr.value; + curr = scanner.next(); + break; + case 48: + entity.lineTypeScale = curr.value; + curr = scanner.next(); + break; + case 60: + entity.visible = curr.value === 0; + curr = scanner.next(); + break; + case 62: // Acad Index Color. 0 inherits ByBlock. 256 inherits ByLayer. Default is bylayer + entity.colorIndex = curr.value; + entity.color = getAcadColor(Math.abs(curr.value)); + curr = scanner.next(); + break; + case 67: + entity.inPaperSpace = curr.value !== 0; + curr = scanner.next(); + break; + case 330: + entity.ownerHandle = curr.value; + curr = scanner.next(); + break; + case 347: + entity.materialObjectHandle = curr.value; + curr = scanner.next(); + break; + case 370: + // This is technically an enum. Not sure where -2 comes from. + //From https://www.woutware.com/Forum/Topic/955/lineweight?returnUrl=%2FForum%2FUserPosts%3FuserId%3D478262319 + // An integer representing 100th of mm, must be one of the following values: + // 0, 5, 9, 13, 15, 18, 20, 25, 30, 35, 40, 50, 53, 60, 70, 80, 90, 100, 106, 120, 140, 158, 200, 211. + entity.lineweight = curr.value; + curr = scanner.next(); + break; + case 420: // TrueColor Color + entity.color = curr.value; + curr = scanner.next(); + break; + case 100: + //ignore + curr = scanner.next(); + break; + default: + logUnhandledGroup(curr); + curr = scanner.next(); + break; + } + }; + + + var parseVertex = function() { + var entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: // X + entity.x = curr.value; + curr = scanner.next(); + break; + case 20: // Y + entity.y = curr.value; + curr = scanner.next(); + break; + case 30: // Z + entity.z = curr.value; + curr = scanner.next(); + break; + case 40: // start width + case 41: // end width + case 42: // bulge + curr = scanner.next(); + break; + case 70: // flags + entity.curveFittingVertex = (curr.value & 1) !== 0; + entity.curveFitTangent = (curr.value & 2) !== 0; + entity.splineVertex = (curr.value & 8) !== 0; + entity.splineControlPoint = (curr.value & 16) !== 0; + entity.threeDPolylineVertex = (curr.value & 32) !== 0; + entity.threeDPolylineMesh = (curr.value & 64) !== 0; + entity.polyfaceMeshVertex = (curr.value & 128) !== 0; + curr = scanner.next(); + break; + case 50: // curve fit tangent direction + case 71: // polyface mesh vertex index + case 72: // polyface mesh vertex index + case 73: // polyface mesh vertex index + case 74: // polyface mesh vertex index + curr = scanner.next(); + break; + default: + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + var parseSeqEnd = function() { + var entity = { type: curr.value }; + curr = scanner.next(); + while(curr != 'EOF') { + if (curr.code == 0) break; + checkCommonEntityProperties(entity); + } + + return entity; + }; + + /** + * Parses a 2D or 3D point, returning it as an object with x, y, and + * (sometimes) z property if it is 3D. It is assumed the current group + * is x of the point being read in, and scanner.next() will return the + * y. The parser will determine if there is a z point automatically. + * @return {Object} The 2D or 3D point as an object with x, y[, z] + */ + var parsePoint = function() { + var point = {}, + code = curr.code; + + point.x = curr.value; + + code += 10; + curr = scanner.next(); + if(curr.code != code) + throw new Error('Expected code for point value to be ' + code + + ' but got ' + curr.code + '.'); + point.y = curr.value; + + code += 10; + curr = scanner.next(); + if(curr.code != code) + return point; + point.z = curr.value; + + curr = scanner.next(); // advance the scanner before returning + return point; + }; + + var parseLWPolylineVertices = function(n) { + if(!n || n <= 0) throw Error('n must be greater than 0 verticies'); + var vertices = [], i; + var vertexIsStarted = false; + var vertexIsFinished = false; + + for(i = 0; i < n; i++) { + var vertex = {}; + while(curr !== 'EOF') { + if(curr.code === 0 || vertexIsFinished) break; + + switch(curr.code) { + case 10: // X + if(vertexIsStarted) { + vertexIsFinished = true; + continue; + } + vertex.x = curr.value; + vertexIsStarted = true; + break; + case 20: // Y + vertex.y = curr.value; + break; + case 30: // Z + vertex.z = curr.value; + break; + case 40: // start width + vertex.startWidth = curr.value; + break; + case 41: // end width + vertex.endWidth = curr.value; + break; + case 42: // bulge + if(curr.value != 0) vertex.bulge = curr.value; + break; + default: + //todo: mark unhandled somehow? + curr = scanner.next(); + continue; + } + curr = scanner.next(); + } + // See https://groups.google.com/forum/#!topic/comp.cad.autocad/9gn8s5O_w6E + vertices.push(vertex); + vertexIsStarted = false; + vertexIsFinished = false; + } + return vertices; + }; + + var parsePolylineVertices = function() { + var vertices = []; + while (curr !== 'EOF') { + if (curr.code === 0) { + if (curr.value === 'VERTEX') { + vertices.push(parseVertex()); + } else if (curr.value === 'SEQEND') { + parseSeqEnd(); + break; + } + } + } + return vertices; + }; + + var parseMTEXT = function() { + var entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 1: + entity.text = curr.value; + curr = scanner.next(); + break; + case 3: + entity.text += curr.value; + curr = scanner.next(); + break; + case 10: + entity.position = parsePoint(); + break; + case 40: + entity.height = curr.value; + curr = scanner.next(); + break; + case 41: + entity.width = curr.value; + curr = scanner.next(); + break; + case 71: + entity.attachmentPoint = curr.value; + curr = scanner.next(); + break; + case 72: + entity.drawingDirection = curr.value; + curr = scanner.next(); + break; + default: + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + var parseATTDEF = function() { + var entity = { + type: curr.value, + scale: 1, + textStyle: 'STANDARD' + }; + curr = scanner.next(); + while (curr !== 'EOF') { + if (curr.code === 0) { + break; + } + switch(curr.code) { + case 1: + entity.text = curr.value; + curr = scanner.next(); + break; + case 2: + entity.tag = curr.value; + curr = scanner.next(); + break; + case 3: + entity.prompt = curr.value; + curr = scanner.next(); + break; + case 7: + entity.textStyle = curr.value; + curr = scanner.next(); + break; + case 10: + entity.x = curr.value; + curr = scanner.next(); + break; + case 20: + entity.y = curr.value; + curr = scanner.next(); + break; + case 30: + entity.z = curr.value; + curr = scanner.next(); + break; + case 39: + entity.thickness = curr.value; + curr = scanner.next(); + break; + case 40: + entity.textHeight = curr.value; + curr = scanner.next(); + break; + case 41: + entity.scale = curr.value; + curr = scanner.next(); + break; + case 50: + entity.rotation = curr.value; + curr = scanner.next(); + break; + case 51: + entity.obliqueAngle = curr.value; + curr = scanner.next(); + break; + case 70: + entity.invisible = !!(curr.value & 0x01); + entity.constant = !!(curr.value & 0x02); + entity.verificationRequired = !!(curr.value & 0x04); + entity.preset = !!(curr.value & 0x08); + curr = scanner.next(); + break; + case 71: + entity.backwards = !!(curr.value & 0x02); + entity.mirrored = !!(curr.value & 0x04); + curr = scanner.next(); + break; + case 72: + // TODO: enum values? + entity.horizontalJustification = curr.value; + curr = scanner.next(); + break; + case 73: + entity.fieldLength = curr.value; + curr = scanner.next(); + break; + case 74: + // TODO: enum values? + entity.verticalJustification = curr.value; + curr = scanner.next(); + break; + case 100: + // subclass + curr = scanner.next(); + break; + case 210: + entity.extrusionDirectionX = curr.value; + curr = scanner.next(); + break; + case 220: + entity.extrusionDirectionY = curr.value; + curr = scanner.next(); + break; + case 230: + entity.extrusionDirectionZ = curr.value; + curr = scanner.next(); + break; + default: + checkCommonEntityProperties(entity); + break; + } + } + + return entity; + }; + + /** + * Called when the parser reads the beginning of a new entity, + * 0:LWPOLYLINE. Scanner.next() will return the first attribute of the + * entity. + * @return {Object} the entity parsed + */ + var parseLWPOLYLINE = function() { + var entity = { type: curr.value, vertices: [] }, + numberOfVertices = 0; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 38: + entity.elevation = curr.value; + curr = scanner.next(); + break; + case 39: + entity.depth = curr.value; + curr = scanner.next(); + break; + case 70: // 1 = Closed shape, 128 = plinegen?, 0 = default + entity.shape = (curr.value === 1); + curr = scanner.next(); + break; + case 90: + numberOfVertices = curr.value; + curr = scanner.next(); + break; + case 10: // X coordinate of point + entity.vertices = parseLWPolylineVertices(numberOfVertices); + break; + case 43: + if(curr.value !== 0) entity.width = curr.value; + curr = scanner.next(); + break; + default: + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + /** + * Called when the parser reads the beginning of a new entity, + * 0:POLYLINE. Scanner.next() will return the first attribute of the + * entity. + * @return {Object} the entity parsed + */ + var parsePOLYLINE = function() { + var entity = { type: curr.value, vertices: [] }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: // always 0 + case 20: // always 0 + case 30: // elevation + case 39: // thickness + entity.thickness = curr.value; + curr = scanner.next(); + break; + case 40: // start width + case 41: // end width + curr = scanner.next(); + break; + case 70: + entity.shape = (curr.value & 1) !== 0; + entity.includesCurveFitVertices = (curr.value & 2) !== 0; + entity.includesSplineFitVertices = (curr.value & 4) !== 0; + entity.is3dPolyline = (curr.value & 8) !== 0; + entity.is3dPolygonMesh = (curr.value & 16) !== 0; + entity.is3dPolygonMeshClosed = (curr.value & 32) !== 0; // 32 = The polygon mesh is closed in the N direction + entity.isPolyfaceMesh = (curr.value & 64) !== 0; + entity.hasContinuousLinetypePattern = (curr.value & 128) !== 0; + curr = scanner.next(); + break; + case 71: // Polygon mesh M vertex count + case 72: // Polygon mesh N vertex count + case 73: // Smooth surface M density + case 74: // Smooth surface N density + case 75: // Curves and smooth surface type + curr = scanner.next(); + break; + case 210: + extrusionDirection = parsePoint(); + break; + default: + checkCommonEntityProperties(entity); + break; + } + } + + entity.vertices = parsePolylineVertices(); + + return entity; + }; + + + /** + * Called when the parser reads the beginning of a new entity, + * 0:LINE. Scanner.next() will return the first attribute of the + * entity. + * @return {Object} the entity parsed + */ + var parseLINE = function() { + var entity = { type: curr.value, vertices: [] }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: // X coordinate of point + entity.vertices.unshift(parsePoint()); + break; + case 11: + entity.vertices.push(parsePoint()); + break; + case 210: + entity.extrusionDirection = parsePoint(); + break; + case 100: + if(curr.value == 'AcDbLine') { + curr = scanner.next(); + break; + } + default: + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + /** + * Used to parse a circle or arc entity. + * @return {Object} the entity parsed + */ + var parseCIRCLE = function() { + var entity, endAngle; + entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: // X coordinate of point + entity.center = parsePoint(); + break; + case 40: // radius + entity.radius = curr.value; + curr = scanner.next(); + break; + case 50: // start angle + entity.startAngle = Math.PI / 180 * curr.value; + curr = scanner.next(); + break; + case 51: // end angle + endAngle = Math.PI / 180 * curr.value; + if(endAngle < entity.startAngle) + entity.angleLength = endAngle + 2 * Math.PI - entity.startAngle; + else + entity.angleLength = endAngle - entity.startAngle; + entity.endAngle = endAngle; + curr = scanner.next(); + break; + default: // ignored attribute + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + var parseTEXT = function() { + var entity; + entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + switch(curr.code) { + case 10: // X coordinate of 'first alignment point' + entity.startPoint = parsePoint(); + break; + case 11: // X coordinate of 'second alignment point' + entity.endPoint = parsePoint(); + break; + case 40: // Text height + entity.textHeight = curr.value; + curr = scanner.next(); + break; + case 41: + entity.xScale = curr.value; + curr = scanner.next(); + break; + case 1: // Text + entity.text = curr.value; + curr = scanner.next(); + break; + // NOTE: 72 and 73 are meaningless without 11 (second alignment point) + case 72: // Horizontal alignment + entity.halign = curr.value; + curr = scanner.next(); + break; + case 73: // Vertical alignment + entity.valign = curr.value; + curr = scanner.next(); + break; + default: // check common entity attributes + checkCommonEntityProperties(entity); + break; + } + } + return entity; + }; + + var parseDIMENSION = function() { + var entity; + entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 2: // Referenced block name + entity.block = curr.value; + curr = scanner.next(); + break; + case 10: // X coordinate of 'first alignment point' + entity.anchorPoint = parsePoint(); + break; + case 11: + entity.middleOfText = parsePoint(); + break; + case 71: // 5 = Middle center + entity.attachmentPoint = curr.value; + curr = scanner.next(); + break; + case 42: // Actual measurement + entity.actualMeasurement = curr.value; + curr = scanner.next(); + break; + case 1: // Text entered by user explicitly + entity.text = curr.value; + curr = scanner.next(); + break; + case 50: // Angle of rotated, horizontal, or vertical dimensions + entity.angle = curr.value; + curr = scanner.next(); + break; + default: // check common entity attributes + checkCommonEntityProperties(entity); + break; + } + } + + return entity; + }; + + var parseSOLID = function() { + var entity; + entity = { type: curr.value }; + entity.points = []; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: + entity.points[0] = parsePoint(); + break; + case 11: + entity.points[1] = parsePoint(); + break; + case 12: + entity.points[2] = parsePoint(); + break; + case 13: + entity.points[3] = parsePoint(); + break; + case 210: + entity.extrusionDirection = parsePoint(); + break; + default: // check common entity attributes + checkCommonEntityProperties(entity); + break; + } + } + + return entity; + }; + + var parsePOINT = function() { + var entity; + entity = { type: curr.value }; + curr = scanner.next(); + while(curr !== 'EOF') { + if(curr.code === 0) break; + + switch(curr.code) { + case 10: + entity.position = parsePoint(); + break; + case 39: + entity.thickness = curr.value; + curr = scanner.next(); + break; + case 210: + entity.extrusionDirection = parsePoint(); + break; + case 100: + if(curr.value == 'AcDbPoint') { + curr = scanner.next(); + break; + } + default: // check common entity attributes + checkCommonEntityProperties(entity); + break; + } + } + + return entity; + }; + + var ensureHandle = function(entity) { + if(!entity) throw new TypeError('entity cannot be undefined or null'); + + if(!entity.handle) entity.handle = lastHandle++; + }; + + parseAll(); + return dxf; +}; + +function logUnhandledGroup(curr) { + log.debug('unhandled group ' + debugCode(curr)); +} + + +function debugCode(curr) { + return curr.code + ':' + curr.value; +} + +/** + * Returns the truecolor value of the given AutoCad color index value + * @return {Number} truecolor value as a number + */ +function getAcadColor(index) { + return AUTO_CAD_COLOR_INDEX[index]; +} + +const BLOCK_ANONYMOUS_FLAG = 1; +const BLOCK_NON_CONSTANT_FLAG = 2; +const BLOCK_XREF_FLAG = 4; +const BLOCK_XREF_OVERLAY_FLAG = 8; +const BLOCK_EXTERNALLY_DEPENDENT_FLAG = 16; +const BLOCK_RESOLVED_OR_DEPENDENT_FLAG = 32; +const BLOCK_REFERENCED_XREF = 64; + +//*! loglevel - v1.4.1 - https://github.com/pimterry/loglevel - (c) 2016 Tim Perry - licensed MIT */ +(function (root, definition) { + "use strict"; + if (typeof define === 'function' && define.amd) { + define(definition); + } else if (typeof module === 'object' && module.exports) { + module.exports = definition(); + } else { + root.log = definition(); + } +}(this, function () { + "use strict"; + var noop = function() {}; + var undefinedType = "undefined"; + + function realMethod(methodName) { + if (typeof console === undefinedType) { + return false; // We can't build a real method without a console to log to + } else if (console[methodName] !== undefined) { + return bindMethod(console, methodName); + } else if (console.log !== undefined) { + return bindMethod(console, 'log'); + } else { + return noop; + } + } + + function bindMethod(obj, methodName) { + var method = obj[methodName]; + if (typeof method.bind === 'function') { + return method.bind(obj); + } else { + try { + return Function.prototype.bind.call(method, obj); + } catch (e) { + // Missing bind shim or IE8 + Modernizr, fallback to wrapping + return function() { + return Function.prototype.apply.apply(method, [obj, arguments]); + }; + } + } + } + + // these private functions always need `this` to be set properly + + function enableLoggingWhenConsoleArrives(methodName, level, loggerName) { + return function () { + if (typeof console !== undefinedType) { + replaceLoggingMethods.call(this, level, loggerName); + this[methodName].apply(this, arguments); + } + }; + } + + function replaceLoggingMethods(level, loggerName) { + /*jshint validthis:true */ + for (var i = 0; i < logMethods.length; i++) { + var methodName = logMethods[i]; + this[methodName] = (i < level) ? + noop : + this.methodFactory(methodName, level, loggerName); + } + } + + function defaultMethodFactory(methodName, level, loggerName) { + /*jshint validthis:true */ + return realMethod(methodName) || + enableLoggingWhenConsoleArrives.apply(this, arguments); + } + + var logMethods = [ + "trace", + "debug", + "info", + "warn", + "error" + ]; + + function Logger(name, defaultLevel, factory) { + var self = this; + var currentLevel; + var storageKey = "loglevel"; + if (name) { + storageKey += ":" + name; + } + + function persistLevelIfPossible(levelNum) { + var levelName = (logMethods[levelNum] || 'silent').toUpperCase(); + + // Use localStorage if available + /* fry commented out to get rid of console.log message + try { + window.localStorage[storageKey] = levelName; + return; + } catch (ignore) {} + */ + // Use session cookie as fallback + try { + window.document.cookie = + encodeURIComponent(storageKey) + "=" + levelName + ";"; + } catch (ignore) {} + } + + function getPersistedLevel() { //might return undefined + var storedLevel; + + /* fry commented out to get rid of console.log error message + try { + storedLevel = window.localStorage[storageKey]; + } catch (ignore) {} + */ + + if (typeof storedLevel === undefinedType) { + try { + var cookie = window.document.cookie; + var location = cookie.indexOf( + encodeURIComponent(storageKey) + "="); + if (location != -1) { + storedLevel = /^([^;]+)/.exec(cookie.slice(location))[1]; + } + } catch (ignore) {} + } + + // If the stored level is not valid, treat it as if nothing was stored. + if (self.levels[storedLevel] === undefined) { + storedLevel = undefined; + } + + return storedLevel; + } + + /* + * + * Public API + * + */ + + self.levels = { "TRACE": 0, "DEBUG": 1, "INFO": 2, "WARN": 3, + "ERROR": 4, "SILENT": 5}; + + self.methodFactory = factory || defaultMethodFactory; + + self.getLevel = function () { + return currentLevel; + }; + + self.setLevel = function (level, persist) { + if (typeof level === "string" && self.levels[level.toUpperCase()] !== undefined) { + level = self.levels[level.toUpperCase()]; + } + if (typeof level === "number" && level >= 0 && level <= self.levels.SILENT) { + currentLevel = level; + if (persist !== false) { // defaults to true + persistLevelIfPossible(level); + } + replaceLoggingMethods.call(self, level, name); + if (typeof console === undefinedType && level < self.levels.SILENT) { + return "No console available for logging"; + } + } else { + throw "log.setLevel() called with invalid level: " + level; + } + }; + + self.setDefaultLevel = function (level) { + if (!getPersistedLevel()) { + self.setLevel(level, false); + } + }; + + self.enableAll = function(persist) { + self.setLevel(self.levels.TRACE, persist); + }; + + self.disableAll = function(persist) { + self.setLevel(self.levels.SILENT, persist); + }; + + // Initialize with the right level + var initialLevel = getPersistedLevel(); + if (initialLevel == null) { + initialLevel = defaultLevel == null ? "WARN" : defaultLevel; + } + self.setLevel(initialLevel, false); + } + + /* + * + * Package-level API + * + */ + + var defaultLogger = new Logger(); + + var _loggersByName = {}; + defaultLogger.getLogger = function getLogger(name) { + if (typeof name !== "string" || name === "") { + throw new TypeError("You must supply a name when creating a logger."); + } + + var logger = _loggersByName[name]; + if (!logger) { + logger = _loggersByName[name] = new Logger( + name, defaultLogger.getLevel(), defaultLogger.methodFactory); + } + return logger; + }; + + // Grab the current global log variable in case of overwrite + var _log = (typeof window !== undefinedType) ? window.log : undefined; + defaultLogger.noConflict = function() { + if (typeof window !== undefinedType && + window.log === defaultLogger) { + window.log = _log; + } + + return defaultLogger; + }; + + return defaultLogger; +})); \ No newline at end of file diff --git a/math/Kin b/math/Kin new file mode 100644 index 00000000..01a37f63 --- /dev/null +++ b/math/Kin @@ -0,0 +1,2886 @@ +//Vector Library + Inverse Kinematics + Forward Kinematics +//Convert, Vector, and Kin classes +//James Wigglesworth +//Started: 6_18_16 +//Updated: 3_6_17 + + +//Public +//This class is used to convert units of numbers and arrays +var Convert = new function(){ + + + //Private + //This is used to prevent functions from altering outside arrays + this.deep_copy = function(arg){ + if (typeof(arg) == "number"){ + return arg + }else{ + let result = [] + for(var i = 0; i < arg.length; i++){ + let elt = arg[i] + if (typeof(elt) !== "number"){ + elt = elt.slice(0) + } + result.push(elt) + } + return result + } + } + + + + //******************************************* + //Angles: + //Note: general syntax is "[input units (plural)]_to_[output units (plural)]" + + //Public + this.radians_to_degrees = function(radians){ + var degrees + if (Vector.size(radians) === 1){ + degrees = radians * 180 / Math.PI + }else{ + degrees = new Array(Vector.size(radians)) + for(var i = 0; i < Vector.size(radians); i++){ + degrees[i] = radians[i] * 180 / Math.PI + } + } + return degrees + } + + + //Public + this.degrees_to_radians = function(degrees){ + var radians + if (Vector.size(degrees) === 1){ + radians = degrees * Math.PI / 180 + }else{ + radians = new Array(Vector.size(degrees)) + for(var i = 0; i < Vector.size(degrees); i++){ + radians[i] = degrees[i] * Math.PI / 180 + } + } + return radians + } + + //Public + this.degrees_to_arcseconds = function(degrees){ + var arcseconds + if (Vector.size(degrees) === 1){ + arcseconds = degrees * 3600 + //arcseconds = Math.round(degrees * 3600) + }else{ + arcseconds = new Array(Vector.size(degrees)) + for(var i = 0; i < Vector.size(degrees); i++){ + arcseconds[i] = degrees[i] * 3600 + //arcseconds[i] = Math.round(degrees[i] * 3600) + } + } + return arcseconds + } + + //Public + this.arcseconds_to_degrees = function(arcseconds){ + var degrees + if (Vector.size(arcseconds) === 1){ + degrees = arcseconds / 3600 + }else{ + degrees = new Array(Vector.size(arcseconds)) + for(var i = 0; i < Vector.size(arcseconds); i++){ + degrees[i] = arcseconds[i] / 3600 + } + } + return degrees + } + + //Public + this.radians_to_arcseconds = function(radians){ + var degrees = Convert.radians_to_degrees(radians) + return Convert.degrees_to_arcseconds(degrees) + } + + //Public + this.arcseconds_to_radians = function(arcseconds){ + var degrees = Convert.arcseconds_to_degrees(arcseconds) + return Convert.degrees_to_radians(degrees) + } + + //******************************************* + + //******************************************* + //distance + + //Public + this.mms_to_microns = function(mm){ + var microns + if (Vector.size(mm) === 1){ + microns = mm * 1000 + //microns = Math.round(mm * 1000) + }else{ + microns = new Array(Vector.size(mm)) + for(var i = 0; i < Vector.size(mm); i++){ + microns[i] = mm[i] * 1000 + //microns[i] = Math.round(mm[i] * 1000) + } + } + return microns + } + + //Public + this.microns_to_mms = function(microns){ + var mms + if (Vector.size(microns) === 1){ + mms = microns / 1000 + }else{ + mms = Vector.multiply(.001, microns) + } + return mms + } + + //Public + this.mms_to_meters = function(mms){ + var meters + if (Vector.size(mms) === 1){ + meters = mms / 1000 + }else{ + meters = new Array(Vector.size(mms)) + for(var i = 0; i < Vector.size(mms); i++){ + meters[i] = mms[i] / 1000 + } + } + return meters + } + + //Public + this.meters_to_mms = function(meters){ + var mms + if (Vector.size(meters) === 1){ + mms = meters * 1000 + }else{ + mms = new Array(Vector.size(meters)) + for(var i = 0; i < Vector.size(meters); i++){ + mms[i] = meters[i] * 1000 + } + } + return mms + } + + //Public + this.meters_to_microns = function(meters){ + var mms = Convert.meters_to_mms(meters) + return Convert.mms_to_microns(mms) + } + + //Public + this.microns_to_meters = function(microns){ + var mms = Convert.microns_to_mms(microns) + return Convert.mms_to_meters(mms) + } + + this.inches_to_microns = function(inches){ + let meters = inches*0.0254 + var mms = Convert.meters_to_mms(meters) + return Convert.mms_to_microns(mms) + } + + + //******************************************* + //Rotation representation conversions: + + this.angles_to_DCM = function(angles = [0, 0, 0], sequence = "XYZ"){ + //default could be ZX'Z' + + var result = [] + let elt = "" + for(let char of sequence){ + if(elt.length == 1){ + if(char == "'"){ + elt += char + result.push(elt) + elt = "" + }else{ + result.push(elt) + elt = char + } + }else{ + elt = char + } + } + if((elt != "'") && (elt.length == 1)){ + result.push(elt) + } + + + let DCM = Vector.identity_matrix(3) + if(result.length == 3){ + for(var i = 0; i < 3; i++){ + DCM = Vector.rotate_DCM(DCM, result[i], angles[i]) + } + } + return Vector.transpose(DCM) + } + //Convert.angles_to_DCM([Convert.degrees_to_arcseconds(45), Convert.degrees_to_arcseconds(45), 0]) + + this.DCM_to_angles = function(DCM, sequence = "XYZ"){ + + } + + this.quat_to_DCM = function(quaternion = [1, 0, 0, 0]){ + //Algorithm was found here: + //http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ + let w = quaternion[0] + let x = quaternion[1] + let y = quaternion[2] + let z = quaternion[3] + + let DCM = Vector.make_matrix(3,3) + DCM[0][0] = 1-2*y*y-2*z*z + DCM[1][0] = 2*x*y+2*z*w + DCM[2][0] = 2*x*z-2*y*w + DCM[0][1] = 2*x*y-2*z*w + DCM[1][1] = 1-2*x*x-2*z*z + DCM[2][1] = 2*y*z+2*x*w + DCM[0][2] = 2*x*z+2*y*w + DCM[1][2] = 2*y*z-2*x*w + DCM[2][2] = 1-2*x*x-2*y*y + return DCM + } + + + this.DCM_to_quat = function(DCM){ + //Algorithm was found here: + //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + let trace = DCM[0][0] + DCM[1][1] + DCM[2][2] + let S, w, x, y, z, quaternion + if(trace > 0){ + S = Math.sqrt(1.0 + trace) * 2 + w = .25 * S + x = (DCM[2][1] - DCM[1][2]) / S + y = (DCM[2][1] - DCM[1][2]) / S + z = (DCM[2][1] - DCM[1][2]) / S + }else if(DCM[0][0] > DCM[1][1] && DCM[0][0] > DCM[2][2]){ + S = 2 * Math.sqrt(1 + DCM[0][0] - DCM[1][1] - DCM[2][2]) + w = (DCM[2][1] - DCM[1][2]) / S + x = .25 * S + y = (DCM[0][1] + DCM[1][0]) / S + z = (DCM[0][2] + DCM[2][0]) / S + }else if(DCM[1][1] > DCM[2][2]){ + S = 2 * Math.sqrt(1 + DCM[1][1] - DCM[0][0] - DCM[2][2]) + w = (DCM[0][2] - DCM[2][0]) / S + x = (DCM[0][1] + DCM[1][0]) / S + y = .25 * S + z = (DCM[1][2] + DCM[2][1]) / S + }else if(DCM[1][1] > DCM[2][2]){ + S = 2 * Math.sqrt(1 + DCM[2][2] - DCM[0][0] - DCM[1][1]) + w = (DCM[1][0] - DCM[0][1]) / S + x = (DCM[0][2] + DCM[2][0]) / S + y = (DCM[1][2] + DCM[2][1]) / S + z = .25 * S + } + quaternion = [w, x, y, z] + return quaternion + } + +} + + + + + +//Public +var Vector = new function(){ +//The Vector Class contains functions for manipulating the following: +/* + + Name | Variable | Syntax | Example | Description + point U [x, y, z] [1, 2, 3] Defines a 3D position. Default units are microns. + vector Uab [x, y, z] [1, 2, 3] Defines a 3D direction and magnitude. Uab = Ua-Ub. + unit vector V [x, y, z] [0, .707, .707] Defines a 3D direction. Magnitude is scaled to be 1. + plane P [x, y, z, d] [0, .707, .707, 5] Unit vector perpendicular to plane and distance from the origin. + +*/ + + + + //Public + //a more robust version of ".length" + //will return 1 for both a number and array elemement (if it's length is 1) + this.size = function(a){ + + if (a === undefined){ + out("Error: input to function 'size()' is undefined", "red") + return + } + if (a === null){ + out("Error: input to function 'size()' is null:") + return + } + + if (typeof(a) == "number"){ + return 1 + } + //debugger + let temp_size = a.length + + + if (temp_size === undefined){ + return 1 + }else{ + /* + let a_temp = a + let result = [] + let i = 0 + while (a_temp !== undefined){ + i++ + result.push(a_temp.length) + a_temp = a[i] + }*/ + return temp_size + } + } + + //Vector.size([0, 0, 1]) + + //Public + //Returns the unit vector of the input + //Works for both 2D and 3D vectors + this.normalize = function(vector){ + let magnitude = Vector.magnitude(vector) + return Vector.divide(vector, magnitude) + } + + + //Public + //Returns the dot product of two arrays + //Will work for arrays of any equal length + this.dot = function(vector_A, vector_B){ + var A_size = Vector.size(vector_A) + var B_size = Vector.size(vector_B) + var point, plane + var product = 0 + if (A_size === B_size){ + for(var i = 0; i < Math.min(A_size, B_size); i++){ + product += vector_A[i] * vector_B[i] + } + + }else{ + if (A_size === 3 && B_size === 4){ + point = [vector_A[0], vector_A[1], vector_A[2], 1] + plane = vector_B + }else{ + if (A_size === 4 && B_size === 3){ + point = [vector_B[0], vector_B[1], vector_B[2], 1] + plane = vector_A + }else{ + if (A_size === 3 && B_size === 4){ + point = [vector_A[0], vector_A[1], vector_A[2], 1] + plane = vector_B + }else{ + if(vector_A[0].length == vector_B.length){ + for(var i = 0; i < vector_A[0].length; i++){ + product += vector_A[0][i] * vector_B[i] + } + return product + }else{ + if(vector_B[0].length == vector_A.length){ + for(var i = 0; i < vector_B[0].length; i++){ + product += vector_A[i] * vector_B[0][i] + } + }else{ + out("Error: Reconsider the input size in the function 'Vector.dot()'", "red") + return null + } + } + } + } + } + + for(var i = 0; i < 4; i++){ + product += point[i] * plane[i] + } + } + return product + } + + //Public + //Returns the cross product of two vectors + //Vectors must be equal lengths + this.cross = function(vector_A, vector_B){ + var A_size = vector_A.length + var B_size = vector_B.length + var mat_size = Math.min(A_size, B_size) + var vector_C = new Array(mat_size) + for(var i = 0; i < mat_size; i++){ + vector_C[i] = (vector_A[(i + 1) % mat_size] * vector_B[(i + 2) % mat_size]) - (vector_A[(i + 2) % mat_size] * vector_B[(i + 1) % mat_size]) + } + return vector_C + } + + + + //Private + //************************************************* + //trigonometric functions with angles in arcseconds + //These might belong in a different Class + + + //Trig + this.cos_arcsec = function(theta){ + return Math.cos(Convert.arcseconds_to_radians(theta)) + } + this.sin_arcsec = function(theta){ + return Math.sin(Convert.arcseconds_to_radians(theta)) + } + this.tan_arcsec = function(theta){ + return Math.tan(Convert.arcseconds_to_radians(theta)) + } + + //Inverse Trig + this.asin_arcsec = function(ratio){ + //return Math.round(Convert.radians_to_arcseconds(Math.asin(ratio))) + return Convert.radians_to_arcseconds(Math.asin(ratio)) + } + + this.acos_arcsec = function(ratio){ + return Convert.radians_to_arcseconds(Math.acos(ratio)) + //return Math.round(Convert.radians_to_arcseconds(Math.acos(ratio))) + } + + this.atan_arcsec = function(ratio){ + return Convert.radians_to_arcseconds(Math.atan(ratio)) + //return Math.round(Convert.radians_to_arcseconds(Math.atan(ratio))) + } + + this.atan2_arcsec = function(a, b){ + return Convert.radians_to_arcseconds(Math.atan2(a, b)) + //return Math.round(Convert.radians_to_arcseconds(Math.atan2(a, b))) + } + + + + //************************************************* + + + //Public + //This is used to add vectors or equal length + //Can also add scalars to each element in vector + //unlimited number of inputs args + this.add = function(...args){ + let temp_args = Convert.deep_copy(args) + var sum = temp_args[0] + + for(var i = 1; i < Vector.size(arguments); i++){ + if (Vector.size(arguments[i]) === Vector.size(sum)){ + if (Vector.size(sum) === 1){ + sum += arguments[i] + }else{ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += arguments[i][j] + } + } + + }else{ + if (Vector.size(arguments[i]) === 1){ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += arguments[i] + } + }else{ + if (Vector.size(sum) === 1){ + var temp = sum + sum = arguments[i] + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += temp + } + }else{ + out("Error: inputs to function 'add()' are not correct sizes", "red") + } + } + } + } + return sum + + } + + + + //Public + //This is used to add vectors or equal length + //Can also add scalars to each element in vector + //unlimited number of inputs args + this.subtract = function(){ + let temp_args = Convert.deep_copy(arguments) + var sum = temp_args[0] + + for(var i = 1; i < Vector.size(temp_args); i++){ + if (Vector.size(temp_args[i]) === Vector.size(sum)){ + if (Vector.size(sum) === 1){ + sum -= temp_args[i] + }else{ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] -= temp_args[i][j] + } + } + + }else{ + if (Vector.size(temp_args[i]) === 1){ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] -= temp_args[i] + } + }else{ + if (Vector.size(sum) === 1){ + var temp = sum + sum = temp_args[i] + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] -= temp + } + }else{ + out("Error: inputs to function 'add()' are not correct sizes", "red") + } + } + } + } + return sum + } + + //Public + //This should be re-written in a more clever way.... + this.multiply = function(...args){ + if (args === undefined){ + out("Error: the function 'Vector.multiply' has undefined inputs") + } + + let temp_args = Convert.deep_copy(args) + var product = temp_args[0] + + for(var i = 1; i < Vector.size(args); i++){ + let temp_arg = args[i] + var temp_arg_size = Vector.size(temp_arg) + var product_size = Vector.size(product) + if (product_size === 1 && temp_arg_size === 1){ + product *= temp_arg + }else{ + if (temp_arg_size === product_size){ + for(var j = 0; j < temp_arg_size; j++){ + var arg_element_length = Vector.size(temp_arg[j]) + var product_element_length = Vector.size(product[j]) + if (arg_element_length === 1 && arg_element_length === 1){ + product[j] *= temp_arg[j] + }else{ + if (arg_element_length === product_element_length){ + for(var k = 0; k < arg_element_length; k++){ + product[j][k] *= temp_arg[j][k] + } + }else{ + if (arg_element_length === 1){ + for(var k = 0; k < product_element_length; k++){ + product[j][k] *= temp_arg[j] + } + } + if (product_element_length === 1){ + var product_element_val = product[j] + for(var k = 0; k < arg_element_length; k++){ + product[j][k] = temp_arg[j][k] * product_element_val + } + } + } + } + } + }else{ + if (Vector.size(temp_arg) === 1){ + for(var j = 0; j < Vector.size(product); j++){ + if (Vector.size(product[j]) === 1){ + product[j] *= temp_arg + }else{ + for(var k = 0; k < product[j].length; k++){ + product[j][k] *= temp_arg + } + } + } + }else{ + if (Vector.size(product) === 1){ + var temp = product + product = temp_arg.slice(0) + for(var j = 0; j < Vector.size(product); j++){ + if (Vector.size(product[j]) === 1){ + product[j] *= temp + }else{ + for(var k = 0; k < product[j].length; k++){ + product[j][k] *= temp + } + } + } + }else{ + out("Error: inputs to function 'add()' are not correct sizes", "red") + } + } + } + } + } + return product + } + /* + var pose_1 = [[1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1]] + var pose_2 = [[1, 0, 0, 100], + [0, 1, 0, 200], + [0, 0, 1, 300], + [0, 0, 0, 1]] + var result_1 = Vector.matrix_multiply(pose_1, pose_2) + var result_2 = Vector.matrix_multiply(pose_2, pose_1) + */ + + //Public + this.divide = function(...args){ + if (args === undefined){ + out("Error: the function 'Vector.divide' has undefined inputs") + } + + let temp_args = Convert.deep_copy(args) + var quotient = temp_args[0] + + for(var i = 1; i < Vector.size(args); i++){ + let temp_arg = args[i] + var temp_arg_size = Vector.size(temp_arg) + var quotient_size = Vector.size(quotient) + if (quotient_size === 1 && temp_arg_size === 1){ + quotient /= temp_arg + }else{ + if (temp_arg_size === quotient_size){ + for(var j = 0; j < temp_arg_size; j++){ + var arg_element_length = Vector.size(temp_arg[j]) + var quotient_element_length = Vector.size(quotient[j]) + if (arg_element_length === 1 && arg_element_length === 1){ + quotient[j] /= temp_arg[j] + }else{ + if (arg_element_length === quotient_element_length){ + for(var k = 0; k < arg_element_length; k++){ + quotient[j][k] /= temp_arg[j][k] + } + }else{ + if (arg_element_length === 1){ + for(var k = 0; k < quotient_element_length; k++){ + quotient[j][k] /= temp_arg[j] + } + } + if (quotient_element_length === 1){ + var quotient_element_val = product[j] + for(var k = 0; k < arg_element_length; k++){ + quotient[j][k] = quotient_element_val / temp_arg[j][k] + } + } + } + } + } + }else{ + if (Vector.size(temp_arg) === 1){ + for(var j = 0; j < Vector.size(quotient); j++){ + if (Vector.size(quotient[j]) === 1){ + quotient[j] /= temp_arg + }else{ + for(var k = 0; k < quotient[j].length; k++){ + quotient[j][k] /= temp_arg + } + } + } + }else{ + if (Vector.size(quotient) === 1){ + var temp = quotient + quotient = temp_arg.slice(0) + for(var j = 0; j < Vector.size(quotient); j++){ + if (Vector.size(quotient[j]) === 1){ + quotient[j] /= temp + }else{ + for(var k = 0; k < quotient[j].length; k++){ + quotient[j][k] /= temp + } + } + } + }else{ + out("Error: inputs to function 'add()' are not correct sizes", "red") + } + } + } + } + } + return quotient + } + /* + var pose_1 = [[1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1]] + var pose_2 = [[1, 0, 0, 100], + [0, 1, 0, 200], + [0, 0, 1, 300], + [0, 0, 0, 1]] + var result_1 = Vector.matrix_divide(pose_1, pose_2) + var result_2 = Vector.matrix_divide(pose_2, pose_1) + */ + + this.average = function(...args){ + let temp_args = Convert.deep_copy(args) + var sum = temp_args[0] + + for(var i = 1; i < Vector.size(arguments); i++){ + if (Vector.size(arguments[i]) === Vector.size(sum)){ + if (Vector.size(sum) === 1){ + sum += arguments[i] + }else{ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += arguments[i][j] + } + } + + }else{ + if (Vector.size(arguments[i]) === 1){ + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += arguments[i] + } + }else{ + if (Vector.size(sum) === 1){ + var temp = sum + sum = arguments[i] + for(var j = 0; j < Vector.size(sum); j++){ + sum[j] += temp + } + }else{ + out("Error: inputs to function 'add()' are not correct sizes", "red") + } + } + } + } + return Vector.divide(sum, args.length) + } + //Vector.average([2, 2], [4, 4]) + + //Private + function dist_point_to_plane(point, plane){ + if (Vector.size(plane) !== 4){ + out("Error: Complete the plane by using the function 'Vector.complete_plane(vector, point)'") + return null + } + return -Vector.dot(point, plane) + } + + //Private + function dist_point_to_line(point, line_point_A, line_point_B){ + var term1 = Vector.subtract(point, line_point_A) + var term2 = Vector.subtract(point, line_point_B) + var term3 = Vector.subtract(line_point_B, line_point_A) + var d = Vector.distance(Vector.cross(term1, term2)) / Vector.distance(term3) + return d + } + + + + //Public + this.distance = function() { + /*SYNTAX: + Kin.distance(POINT) -> distance between point and origin / magnitude of VECTOR + Kin.distance(POINT, POINT) -> distance between points + Kin.distance(PLANE, POINT) -> distance between plane and point + Kin.distance(POINT, PLANE) -> distance between point and point + */ + var temp_args = Convert.deep_copy(arguments) + + + switch(Vector.size(arguments)){ + + case 1: + var a = temp_args[0] + switch(Vector.size(a)){ + case 2: + //magnitude of 2D vector + return Math.hypot(a[0], a[1]) + case 3: + //magnitude of 3D vector + return Math.hypot(a[0], a[1], a[2]) + case 4: + //distance between plane and origin + return a[4] + default: + out("Error: single vector input to function 'distance()' must have a size of 2, 3, or 4", "red") + return null + } + break + + + case 2: + var a = arguments[0].slice(0) + var b = arguments[1].slice(0) + var aL = Vector.size(a) + var bL = Vector.size(b) + var point + var plane + if (aL === 2 && bL === 2){ + return Math.hypot(a[0] - b[0], a[1] - b[1]) + } + if (aL === 3 && bL === 3){ + return Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]) + }else{ + if (aL === 3 && bL === 4){ + point = a + plane = b + }else{ + if (aL === 4 && bL === 3){ + plane = a + point = b + }else{ + out("Error: inputs for function 'distance()' must be a point, plane, or vector", "red") + return null + } + } + return dist_point_to_plane(point, plane) + } + break + + case 3: + point = arguments[0].slice(0) + var line_point_A = arguments[1].slice(0) + var line_point_B = arguments[2].slice(0) + if (Vector.size(point) === 3 && Vector.size(line_point_A) === 3 && Vector.size(line_point_B) === 3){ + return dist_point_to_line(point, line_point_A, line_point_B) + } + break + + default: + let sum = 0 + for(var i = 0; i < temp_args.length; i++){ + sum += temp_args[i] * temp_args[i] + } + return Math.sqrt(sum) + } + + } + + this.magnitude = function(vector){ + if(vector.length == undefined){ + return vector + } + let sum = 0 + for(var i = 0; i < vector.length; i++){ + sum += vector[i] * vector[i] + } + return Math.sqrt(sum) + } + + //Public + this.complete_plane = function(plane, point){ + if (Vector.size(plane) === 3){ + var vector = Vector.normalize(plane) + var d = Vector.dot(vector, point) + vector.push(d) + return vector + }else{ + return plane + } + } + + //Public + this.project_vector_onto_plane = function(vector, plane){ + if (plane.length === 4){ + var short_plane = [plane[0], plane[1], plane[2]] + } + var term1 = Vector.dot(vector, short_plane) + var term2 = Math.pow(Vector.distance(short_plane), 2) + return Vector.subtract(vector, Vector.multiply(term1 / term2, short_plane)) + } + + //Public + this.points_to_plane = function(Ua, Ub, Uc){ + var Uba = Vector.subtract(Ub, Ua) + var Uca = Vector.subtract(Uc, Ua) + var Uba_norm = Vector.round(Vector.normalize(Uba),10) + var Uca_norm = Vector.round(Vector.normalize(Uca),10) + /*if (Vector.is_equal(Uba_norm, Uca_norm, 10) || Vector.is_equal(Vector.multiply(-1, Uba_norm), Uca_norm, 10)){ + return Kin.base_rotation_to_plane(0, Vector.normalize(Uba)) + }*/ + var vector = Vector.normalize(Vector.cross(Uba, Uca)) + + + return Vector.complete_plane(vector, Ua) + } + + + //Public + this.round = function(number_or_array, digits = 1){ + let mulitplier = Math.pow(10, digits) + let temp_args = Convert.deep_copy(arguments) + if(typeof(number_or_array) == "number"){ + return Math.round(mulitplier * number_or_array) / mulitplier + }else{ + let temp_array = Convert.deep_copy(number_or_array) + let dim = Vector.matrix_dimensions(number_or_array) + if(dim[0] == 1){ + for(var i = 0; i < number_or_array.length; i++){ + temp_array[i] = Math.round(mulitplier * temp_array[i]) / mulitplier + } + }else{ + for(var i = 0; i < dim[0]; i++){ + for(var j = 0; j < dim[1]; j++){ + temp_array[i][j] = Math.round(mulitplier * temp_array[i][j]) / mulitplier + } + } + } + return temp_array + } + } + + + + //Public + this.is_equal = function(array1, array2, decimal_places = 14){ + if (array1.length !== array2.length){ + return false + }else{ + let array1_temp = Vector.round(Convert.deep_copy(array1), decimal_places) + let array2_temp = Vector.round(Convert.deep_copy(array2), decimal_places) + var result = true + for(var i = 0; i < array1_temp.length; i++){ + if (array1_temp[i] !== array2_temp[i]){ + result = false + break + } + } + } + return result + } + + + + //Public + this.shorten = function(matrix){ + return [matrix[0], matrix[1], matrix[2]] + } + + //Public + //Returns the smallest angle between two vectors with range 0-180 degrees + this.angle = function(vector_A, vector_B){ + //in case one of the vectors is a complete plane + var short_A = Vector.shorten(vector_A) + var short_B = Vector.shorten(vector_B) + var result + if (Vector.is_equal(short_A, short_B)){ + result = 0 + }else{ + if (Vector.distance(Vector.add(short_A, short_B)) === 0){ + result = 648000 //this is 180 degrees in arcseconds + }else{ + var result = Vector.atan2_arcsec(Vector.distance(Vector.cross(short_A, short_B)), Vector.dot(short_A, short_B)) + } + } + return result + } + + //Public + //Returns angle between two vectors with range -180 to 180 + this.signed_angle = function(vector_A, vector_B, plane){ + //checks if vectors lie in plane + var cross_product = Vector.normalize(Vector.cross(Vector.shorten(vector_A), Vector.shorten(vector_B))) + var short_plane = Vector.shorten(plane) + if (!(Vector.is_equal(cross_product, short_plane) || Vector.is_equal(Vector.multiply(-1, cross_product), short_plane)) && cross_product[0] === NaN){ + //debugger + out("Error: input vectors do not lie in plane") + } + + var guess_angle = Vector.angle(vector_A, vector_B) + var guess_vector = Vector.round(Vector.rotate(vector_A, plane, guess_angle), 3) + var test_vector = Vector.round([vector_B[0], vector_B[1], vector_B[2]], 3) + + if (Vector.is_equal(guess_vector, test_vector)){ + return guess_angle + }else{ + return -guess_angle + } + } + + + + + //Public + //returns intersection of two planes, a plane and a line, and two lines + this.intersection = function(){ + switch (Vector.size(arguments)){ + case 2: + //Assumes intersection between two planes + + + + return + case 3: + //Assumes intersection between plane and line + var line_vector, complete_point, alpha, intersection_point + + for(var i = 0; i < 3; i++){ + if (Vector.size(arguments[i]) === 4){ + var plane = arguments[i].slice(0) + var point_A = arguments[(i + 1) % 3].slice(0) + var point_B = arguments[(i + 2) % 3].slice(0) + } + + } + if (plane === undefined){ + out("Error: inputs to the function 'Vector.intersection' must be a plane and two points or two planes") + } + if (Vector.size(point_A) !== 3 || Vector.size(point_B) !== 3){ + out("Error: inputs to the function 'Vector.intersection' must be a plane and two points or two planes") + } + + //Assumes plane is passed in along with a line defined by a point and unit vector + if (Vector.distance(point_A) === 1 && Vector.distance(point_B) !== 1){ + line_vector = point_A + complete_point = [point_B[0], point_B[1], point_B[2], 1] + alpha = -Vector.dot(plane, complete_point) / (Math.pow(line_vector[0], 2), Math.pow(line_vector[1], 2), Math.pow(line_vector[2], 2)) + intersection_point = Vector.add(Vector.multiply(alpha, line_vector), point_B) + return intersection_point + } + + //Assumes plane is passed in along with a line defined by a point and unit vector + if (Vector.distance(point_A) !== 1 && Vector.distance(point_B) === 1){ + line_vector = point_B + complete_point = [point_A[0], point_A[1], point_A[2], 1] + alpha = -Vector.dot(plane, complete_point) / (Math.pow(line_vector[0], 2), Math.pow(line_vector[1], 2), Math.pow(line_vector[2], 2)) + intersection_point = Vector.add(Vector.multiply(alpha, line_vector), point_A) + return intersection_point + } + + //Assumes plane is passed in along with a line defined by two points + line_vector = Vector.subtract(point_B, point_A) + complete_point = [point_A[0], point_A[1], point_A[2], 1] + alpha = -Vector.dot(plane, complete_point) / (Math.pow(line_vector[0], 2), Math.pow(line_vector[1], 2), Math.pow(line_vector[2], 2)) + intersection_point = Vector.add(Vector.multiply(alpha, line_vector), point_A) + return [intersection_point, alpha] + } + } + + //Public + //rotates a vector in 3D space on a plane by angle theta + //will also rotate a point about a line by substituting the line's vector in plane and its point in point + this.rotate = function(vector, plane, theta, point = [0, 0, 0]){ + plane = Vector.normalize(Vector.shorten(plane)) + let result, short_vector, term_1, term_2, term_3 + if (vector[0].length == 3){ + result = [0, 0, 0] + for(var i = 0; i < vector.length; i++){ + short_vector = Vector.subtract(vector[i], point) + if(Vector.is_equal(short_vector, point)){ + result[i] = short_vector + }else{ + term_1 = Vector.multiply(Vector.cos_arcsec(theta), short_vector) + term_2 = Vector.multiply(Vector.sin_arcsec(theta), Vector.cross(Vector.shorten(plane), short_vector)) + result[i] = Vector.add(Vector.multiply(Vector.magnitude(vector), Vector.normalize(Vector.add(term_1, term_2))), point) + } + } + }else{ + short_vector = Vector.subtract(Vector.shorten(vector), point) + if(Vector.is_equal(short_vector, point)){ + return short_vector + } + term_1 = Vector.multiply(Vector.cos_arcsec(theta), short_vector) + term_2 = Vector.multiply(Vector.sin_arcsec(theta), Vector.cross(Vector.shorten(plane), short_vector)) + result = Vector.add(Vector.multiply(Vector.magnitude(short_vector), Vector.normalize(Vector.add(term_1, term_2))), point) + } + return result + } + + + this.three_points_to_transformation = function(point_list, pointA = [0, 0, 0], pointB = [1, 0, 0], pointC = [0, 1, 0], U4){ + + let points_plane = Vector.points_to_plane(pointA, pointB, pointC) + let dist = Vector.distance(U4, points_plane) + out("input plane and dist:") + out(points_plane) + out(dist) + if(dist < 0){ + points_plane = Vector.multiply(-1, points_plane) + } + if(!Vector.is_equal(points_plane, [0, 0, 1], 4)){ + let intersection_axis = Vector.cross(points_plane, [0, 0, 1]) + let phi = Vector.signed_angle([1, 0, 0], points_plane, intersection_axis) + for(var i = 0; i < point_list.length; i++){ + point_list[i] = Vector.rotate(point_list[i], [0, 0, 1], phi) + } + } + + let rot_plane = Vector.cross(points_plane, [0, 0, 1]) + let theta = Vector.signed_angle([0, 0, 1], points_plane, rot_plane) + + let new_point_list = [] + + let sum = [0, 0, 0] + if (point_list[0].length === 2){ + for(var i = 0; i < point_list.length; i++){ + new_point_list[i] = Vector.rotate([point_list[i][0], point_list[i][1], 0], rot_plane, theta) + drawing_centroid = Vector.add(sum, new_point_list[i]) + } + } + if (point_list[0].length === 3){ + for(var i = 0; i < point_list.length; i++){ + point_list[i] = [-point_list[i][0], point_list[i][1], point_list[i][2]] + } + for(var i = 0; i < point_list.length; i++){ + new_point_list.push(Vector.rotate([point_list[i][0], point_list[i][1], point_list[i][2]], rot_plane, theta)) + drawing_centroid = Vector.add(sum, new_point_list[i]) + } + } + drawing_centroid = Vector.multiply(drawing_centroid, 1 / point_list.length) + let plane_centroid = Vector.multiply(Vector.add(pointA, pointB, pointC), 1 / 3) + let distance_vector = Vector.subtract(plane_centroid, drawing_centroid) + for(var i = 0; i < point_list.length; i++){ + new_point_list[i] = Vector.add(new_point_list[i], distance_vector) + } + + return [new_point_list, Vector.shorten(points_plane)] + //[x*cos(theta)+dx, y*cos(theta)+dy, (Py*y + Pz*x)*sin(theta)+dz] + } + + this.max = function(vector){ + let dim = Vector.matrix_dimensions(vector) + let temp_max + if(dim[0] == 1){ + temp_max = -Infinity + for(let i = 0; i < dim[1]; i++){ + if(vector[i] > temp_max){ + temp_max = vector[i] + } + } + }else{ + temp_max = Vector.make_matrix(1, dim[1], -Infinity)[0] + for(let j = 0; j < dim[1]; j++){ + for(let i = 0; i < dim[0]; i++){ + if(vector[i][j] > temp_max[j]){ + temp_max[j] = vector[i][j] + } + } + } + } + return temp_max + } + /* + debugger + var result = Vector.max([[1, 2, 10], [4, 5, 6]]) + var result = Vector.max([1, 2, 10]) + */ + + this.min = function(vector){ + let dim = Vector.matrix_dimensions(vector) + let temp_min + if(dim[0] == 1){ + temp_min = Infinity + for(let i = 0; i < dim[1]; i++){ + if(vector[i] < temp_min){ + temp_min = vector[i] + } + } + }else{ + temp_min = Vector.make_matrix(1, dim[1], Infinity)[0] + for(let j = 0; j < dim[1]; j++){ + for(let i = 0; i < dim[0]; i++){ + if(vector[i][j] < temp_min[j]){ + temp_min[j] = vector[i][j] + } + } + } + } + return temp_min + } + /* + var result = Vector.min([[1, 2, 10], [4, 5, 6]]) + var result = Vector.min([1, 2, 10]) + */ + + + /********************************************************** + //Matrix Math + ***********************************************************/ + + this.make_matrix = function(nRows, nColumns, value = 0){ + let result = [] + if(nColumns === undefined){ + if(Vector.matrix_dimensions(nRows)[1] == 2){ + nColumns = nRows[1] + nRows = nRows[0] + }else{ + nColumns = nRows + } + } + if(nRows < 1 || nColumns < 1){ + dde_error("matrix dimensions must be greater than 1") + } + + for(var i = 0; i < nRows; i++){ + result.push([]) + for(var j = 0; j < nColumns; j++){ + result[i].push(value) + } + } + return result + } + //Vector.make_matrix(10, 7) + //Vector.make_matrix(3) + //Vector.make_matrix(3, 2, 1) + //Vector.make_matrix([2,3]) + + function multiply_two_matrices(matrix_A, matrix_B){ + let A_height = matrix_A.length + let B_height = matrix_B.length + let A_width = matrix_A[0].length + let B_width = matrix_B[0].length + if(A_width == undefined){ + A_width = A_height + A_height = 1 + } + if(B_width == undefined){ + B_width = B_height + B_height = 1 + } + if(A_width != B_height){ + dde_error("Inner matrix dimension must match") + } + let result = Vector.make_matrix(A_height, B_width) + for(var i = 0; i < A_height; i++){ + for(var j = 0; j < B_width; j++){ + let verticle = Vector.make_matrix(1, B_height)[0] + for(var k = 0; k < B_height; k++){ + verticle[k] = matrix_B[k][j] + } + if(A_height == 1){ + result[i][j] = Vector.dot(matrix_A, verticle) + }else{ + result[i][j] = Vector.dot(matrix_A[i], verticle) + } + } + } + return result + } + + this.transpose = function(matrix){ + let height = matrix.length + let width = matrix[0].length + if(width == undefined){ + width = height + height = 1 + matrix = [matrix] + } + let result = Vector.make_matrix(width, height) + for(var i = 0; i < width; i++){ + for(var j = 0; j < height; j++){ + result[i][j] = matrix[j][i] + } + } + if(result.length == 1){ + return result[0] + }else{ + return result + } + } + /* + var v = [1, 2, 3] + v = Vector.transpose(v) + var v2 = Vector.transpose(v) + */ + + this.matrix_multiply = function(...args){ + if (args === undefined){ + out("Error: the function 'Vector.matrix_multiply' has undefined inputs") + } + let temp_args = Convert.deep_copy(args) + let matrix_A = temp_args[0] + for(var i = 1; i < temp_args.length; i++){ + let matrix_B = temp_args[i] + matrix_A = multiply_two_matrices(matrix_A, matrix_B) + } + return matrix_A + } + + function divide_two_matrices(matrix_numerator, matrix_denominator){ + let dim_num = Vector.matrix_dimensions(matrix_numerator) + let dim_den = Vector.matrix_dimensions(matrix_denominator) + if (!((dim_num[0] == dim_den[0]) && (dim_num[1] == dim_den[1]))){ + dde_error("matrix dimensions must match in Vector.matrix_divide") + } + return Vector.matrix_multiply(matrix_numerator, Vector.inverse(matrix_denominator)) + } + + this.matrix_divide = function(...args){ + if (args === undefined){ + out("Error: the function 'Vector.matrix_multiply' has undefined inputs") + } + let temp_args = Convert.deep_copy(args) + let matrix_A = temp_args[0] + for(var i = 1; i < temp_args.length; i++){ + let matrix_B = temp_args[i] + matrix_A = divide_two_matrices(matrix_A, matrix_B) + } + return matrix_A + } + + + this.determinant = function(matrix){ + let result + let dim = Vector.matrix_dimensions(matrix) + if (dim[0] == 2 && dim[1] == 2){ + result = matrix[0][0]*matrix[1][1] - matrix[0][1]*matrix[1][0] + }else if(dim[0] == 3 && dim[1] == 3){ + //Source: https://en.wikipedia.org/wiki/Determinant#n_.C3.97_n_matrices + let a, b, c, d, e, f, g, h, i + a = matrix[0][0] + b = matrix[0][1] + c = matrix[0][2] + d = matrix[1][0] + e = matrix[1][1] + f = matrix[1][2] + g = matrix[2][0] + h = matrix[2][1] + i = matrix[2][2] + result = a*(e*i-f*h)-b*(d*i-f*g)+c*(d*h-e*g) + }else if(dim[0] == 4 && dim[1] == 4){ + // Source: http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche23.html + let a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34, a41, a42, a43, a44 + a11 = matrix[0][0] + a12 = matrix[0][1] + a13 = matrix[0][2] + a14 = matrix[0][3] + a21 = matrix[1][0] + a22 = matrix[1][1] + a23 = matrix[1][2] + a24 = matrix[1][3] + a31 = matrix[2][0] + a32 = matrix[2][1] + a33 = matrix[2][2] + a34 = matrix[2][3] + a41 = matrix[3][0] + a42 = matrix[3][1] + a43 = matrix[3][2] + a44 = matrix[3][3] + + result = a11*a22*a33*a44 + a11*a23*a34*a42 + a11*a24*a32*a43 + +a12*a21*a34*a43 + a12*a23*a31*a44 + a12*a24*a33*a41 + +a13*a21*a32*a44 + a13*a22*a34*a41 + a13*a24*a31*a42 + +a14*a21*a33*a42 + a14*a22*a31*a43 + a14*a23*a32*a41 + -a11*a22*a34*a43 - a11*a23*a32*a44 - a11*a24*a33*a42 + -a12*a21*a33*a44 - a12*a23*a34*a41 - a12*a24*a31*a43 + -a13*a21*a34*a42 - a13*a22*a31*a44 - a13*a24*a32*a41 + -a14*a21*a32*a43 - a14*a22*a33*a41 - a14*a23*a31*a42 + }else{ + dde_error("determinants of matricies with these dimensions are not supported yet") + } + return result + } + /* + var my_matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + Vector.determinant(my_matrix) + + var mat = [[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12], [13, 14, 15, 16]] + var det = Vector.determinant(mat) + + var mat = [[3, 2, 1.7, 1.5],[4.5, 5, 4.1, 1.9], [1.1, 8.5, 9, 8], [3, 9, 9, 10]] + var det = Vector.determinant(mat) + */ + + this.inverse = function(matrix){ + let result + let dim = Vector.matrix_dimensions(matrix) + if (dim[0] == 2 && dim[1] == 2){ + result = [[matrix[1][1], -matrix[1][0]], [-matrix[0][1], matrix[0][0]]] + result = Vector.multiply(1/Vector.determinant(matrix), result) + }else if(dim[0] == 3 && dim[1] == 3){ + //Source: University of Massachusetts Lowell - MECH 5960 Mechanics of Composite Materials + let a, b, c, d, e, f, g, h, i, A, B, C, D, E, F, G, H, I + a = matrix[0][0] + b = matrix[0][1] + c = matrix[0][2] + d = matrix[1][0] + e = matrix[1][1] + f = matrix[1][2] + g = matrix[2][0] + h = matrix[2][1] + i = matrix[2][2] + + A = Vector.determinant([[e, f], [h, i]]) + B = -Vector.determinant([[d, f], [g, i]]) + C = Vector.determinant([[d, e], [g, h]]) + D = -Vector.determinant([[b, c], [h, i]]) + E = Vector.determinant([[a, c], [g, i]]) + F = -Vector.determinant([[a, b], [g, h]]) + G = Vector.determinant([[b, c], [e, f]]) + H = -Vector.determinant([[a, c], [d, f]]) + I = Vector.determinant([[a, b], [d, e]]) + + result = [[A, B, C], [D, E, F], [G, H, I]] + result = Vector.multiply(1/Vector.determinant(matrix), result) + }else if(dim[0] == 4 && dim[1] == 4){ + // Source: http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche23.html + let a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34, a41, a42, a43, a44 + let b11, b12, b13, b14, b21, b22, b23, b24, b31, b32, b33, b34, b41, b42, b43, b44 + a11 = matrix[0][0] + a12 = matrix[0][1] + a13 = matrix[0][2] + a14 = matrix[0][3] + a21 = matrix[1][0] + a22 = matrix[1][1] + a23 = matrix[1][2] + a24 = matrix[1][3] + a31 = matrix[2][0] + a32 = matrix[2][1] + a33 = matrix[2][2] + a34 = matrix[2][3] + a41 = matrix[3][0] + a42 = matrix[3][1] + a43 = matrix[3][2] + a44 = matrix[3][3] + + b11 = a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42 + b12 = a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43 + b13 = a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42 + b14 = a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33 + + b21 = a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43 + b22 = a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41 + b23 = a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43 + b24 = a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31 + + b31 = a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41 + b32 = a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42 + b33 = a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41 + b34 = a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32 + + b41 = a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42 + b42 = a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41 + b43 = a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42 + b44 = a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31 + + result = [[b11, b12, b13, b14], [b21, b22, b23, b24], [b31, b32, b33, b34], [b41, b42, b43, b44]] + result = Vector.multiply(1/Vector.determinant(matrix),result) + }else{ + dde_error("the inverse of matricies with these dimensions is not supported yet") + } + return result + } + /* + var mat = Vector.identity_matrix(3) + var mat = [[3, 2, 1.7, 1.5],[4.5, 5, 4.1, 1.9], [1.1, 8.5, 9, 8], [3, 9, 9, 10]] + var det = Vector.determinant(mat) + var imat = Vector.inverse(mat) + + var my_DCM = + + var det = Vector.determinant(imat) + var mat2 = Vector.inverse(imat) + */ + + + this.matrix_dimensions = function(matrix){ + let width + let height = matrix.length + if(height == undefined || height == 0){ + width = 1 + height = 1 + return [height, width] + } + width = matrix[0].length + if(width == undefined){ + width = height + height = 1 + } + return [height, width] + } + //Vector.matrix_dimensions([10, 20, 30]) + //Vector.matrix_dimensions([[10], [20], [30]]) + + this.properly_define_point = function(points){ + //a proper point takes the following form: [[x], [y], [z], [1]] + //for points: [[x1, x2, ..., xn], [y1, y2, ..., yn], [z1, z2, ..., zm=n] [1, 1, ..., 1]] + let dim = Vector.matrix_dimensions(points) + let proper_points = Convert.deep_copy(points) + if(dim[0] == 1){ + proper_points = Vector.transpose(proper_points) + proper_points.push([1]) + return proper_points + }else{ + if(dim[1] == 3){ + for(var i = 0; i < dim[0]; i++){ + proper_points[i].push(1) + } + proper_points = Vector.transpose(proper_points) + return proper_points + }else{ + if(dim[0] == 3){ + //let ones = Vector.add(Vector.make_matrix(1, dim[0])[0], 1) + proper_points.push([1]) + return proper_points + } + } + } + } + /* + Vector.properly_define_point([10, 20, 30]) + Vector.properly_define_point([[10], [20], [30]]) + debugger + Vector.properly_define_point([[10, 20, 30], [10, 20, 30], [10, 20, 30]]) + */ + + this.make_dcm = function(x_vector, y_vector, z_vector){ + let dcm = Vector.identity_matrix(3) + + if(x_vector == undefined && y_vector == undefined && z_vector == undefined){ + return dcm + }else if(x_vector == undefined && y_vector != undefined && z_vector != undefined){ + x_vector = Vector.cross(y_vector, z_vector) + }else if(x_vector != undefined && y_vector == undefined && z_vector != undefined){ + y_vector = Vector.cross(z_vector, x_vector) + }else if(x_vector != undefined && y_vector != undefined && z_vector == undefined){ + z_vector = Vector.cross(x_vector, y_vector) + } + + x_vector = Vector.normalize(x_vector) + y_vector = Vector.normalize(y_vector) + z_vector = Vector.normalize(z_vector) + + dcm = Vector.insert(dcm, Vector.transpose(x_vector), [0, 0]) + dcm = Vector.insert(dcm, Vector.transpose(y_vector), [0, 1]) + dcm = Vector.insert(dcm, Vector.transpose(z_vector), [0, 2]) + + return dcm + } + /* + debugger + var result = Vector.make_dcm([1, 1, 0], undefined, [0, 0, 1]) + */ + + this.make_pose = function(position = [0, 0, 0], orientation = [0, 0, 0], sequence = "XYZ"){ + let dim = Vector.matrix_dimensions(orientation) + let DCM + if(dim[0] == 1 && dim[1] == 3){ + //Euler Angle + DCM = Convert.angles_to_DCM(orientation, sequence) + }else if(dim[0] == 1 && dim[1] == 4){ + //Quaternion + DCM = Convert.quat_to_DCM(orientation) + }else if(dim[0] == 3 && dim[1] == 3){ + //DCM + DCM = orientation + }else{ + dde_error("orientation is improperly formatted") + } + + //Please tell me there's a better way to do this: + let pose = [[DCM[0][0], DCM[0][1], DCM[0][2], position[0]], + [DCM[1][0], DCM[1][1], DCM[1][2], position[1]], + [DCM[2][0], DCM[2][1], DCM[2][2], position[2]], + [0, 0, 0, 1]] + return pose + } + //Vector.make_pose() + //Vector.make_pose([10, 20, 30], [Convert.degrees_to_arcseconds(45), 0, 0]) + //Vector.make_pose([10, 20, 30], [Convert.degrees_to_arcseconds(45), 0, 0], "ZX'Z'") + + + + this.identity_matrix = function(size){ + let result = Vector.make_matrix(size, size) + for(var i = 0; i < size; i++){ + result[i][i] = 1 + } + return result + } + //var im = Vector.identity_matrix(4) + //var det = Vector.determinant(im) + + this.rotate_DCM = function(DCM, axis_of_rotation, angle){ + let trans_matrix = Vector.identity_matrix(3) + let x_vector, y_vector, z_vector + switch(axis_of_rotation){ + case "X": + trans_matrix[1][1] = Vector.cos_arcsec(angle) + trans_matrix[2][2] = Vector.cos_arcsec(angle) + trans_matrix[2][1] = Vector.sin_arcsec(angle) + trans_matrix[1][2] = -Vector.sin_arcsec(angle) + break + case "Y": + trans_matrix[0][0] = Vector.cos_arcsec(angle) + trans_matrix[2][2] = Vector.cos_arcsec(angle) + trans_matrix[0][2] = Vector.sin_arcsec(angle) + trans_matrix[2][0] = -Vector.sin_arcsec(angle) + break + case "Z": + trans_matrix[0][0] = Vector.cos_arcsec(angle) + trans_matrix[1][1] = Vector.cos_arcsec(angle) + trans_matrix[1][0] = Vector.sin_arcsec(angle) + trans_matrix[0][1] = -Vector.sin_arcsec(angle) + break + case "X'": + x_vector = [DCM[0][0], DCM[1][0], DCM[2][0]] + Vector.rotate_DCM(DCM, x_vector, angle) + break + case "Y'": + y_vector = [DCM[0][1], DCM[1][1], DCM[2][1]] + Vector.rotate_DCM(DCM, y_vector, angle) + break + case "Z'": + z_vector = [DCM[0][2], DCM[1][2], DCM[2][2]] + Vector.rotate_DCM(DCM, z_vector, angle) + break + default: + x_vector = [DCM[0][0], DCM[1][0], DCM[2][0]] + y_vector = [DCM[0][1], DCM[1][1], DCM[2][1]] + z_vector = [DCM[0][2], DCM[1][2], DCM[2][2]] + x_vector = Vector.rotate(x_vector, axis_of_rotation, angle) + y_vector = Vector.rotate(y_vector, axis_of_rotation, angle) + z_vector = Vector.rotate(z_vector, axis_of_rotation, angle) + DCM = Vector.transpose([x_vector, y_vector, z_vector]) + return DCM + } + return Vector.matrix_multiply(DCM, trans_matrix) + } + /* + var mat = Vector.rotate_DCM(Vector.identity_matrix(3), [1, 0, 0], Convert.degrees_to_arcseconds(90)) + var det = Vector.determinant(mat) + */ + this.rotate_pose = function(pose, axis_of_rotation, angle, point_of_rotation = [0, 0, 0]){ + if(Vector.is_pose(pose) == false){ + dde_error("pose is not properly formatted") + } + let DCM = Vector.pull(pose, [0, 2], [0, 2]) + DCM = Vector.rotate_DCM(DCM, axis_of_rotation, angle) + let axis + switch(axis_of_rotation){ + case "X": + axis = [1, 0, 0] + break + case "Y": + axis = [0, 1, 0] + break + case "Z": + axis = [0, 0, 1] + break + case "X'": + axis = [DCM[0][0], DCM[1][0], DCM[2][0]] + break + case "Y'": + axis = [DCM[0][1], DCM[1][1], DCM[2][1]] + break + case "Z'": + axis = [DCM[0][2], DCM[1][2], DCM[2][2]] + break + default: + axis = axis_of_rotation + } + let position = Vector.transpose(Vector.pull(pose, [0, 2], 3)) + position = Vector.rotate(position, axis, angle, point_of_rotation) + return Vector.make_pose(position, DCM) + } + /* + var my_pose = Vector.make_pose() + var rot_pose = Vector.rotate_pose(my_pose, "Z", Convert.degrees_to_arcseconds(90), [10, 0, 0]) + var center = [10, 0, 0] + var point = [0, 0, 0] + debugger + var result = Vector.rotate(point, [0, 0, 1], 162000, center) + Convert.degrees_to_arcseconds(45) + */ + + this.is_pose = function(pose){ + let dim = Vector.matrix_dimensions(pose) + if (!(dim[0] == 4 && dim[1] == 4)){ + return false + } + + let short, short_mag + short = [pose[0][0], pose[1][0], pose[2][0]] + short_mag = Vector.magnitude(short) + if (Vector.round(short_mag, 10) != 1){ + return false + } + short = [pose[0][1], pose[1][1], pose[2][1]] + short_mag = Vector.magnitude(short) + if (Vector.round(short_mag, 10) != 1){ + return false + } + short = [pose[0][2], pose[1][2], pose[2][2]] + short_mag = Vector.magnitude(short) + if (Vector.round(short_mag, 10) != 1){ + return false + } + + let DCM = Vector.pull(pose, [0, 2], [0, 2]) + if (!Vector.is_equal(Vector.determinant(DCM), 1, 10)){ + return false + } + + if (pose[3][3] != 1){ + return false + } + + if (!((pose[3][0] == 0) && (pose[3][1] == 0) && (pose[3][2] == 0))){ + return false + } + + return true + } + /* + var my_pose = Vector.make_pose() + var state = Vector.is_pose(my_pose) + + var my_pose = Vector.make_pose([10, 20, 30], [Convert.degrees_to_arcseconds(45), 0, 0], "ZX'Z'") + var state = Vector.is_pose(my_pose) + */ + + + /* + this.place = function(matrix, row, column){ + let dim = Vector.matrix_dimensions(matrix) + let row_lower, row_upper, col_lower, col_upper + if (Vector.size(row) == 1){ + row_lower = row + row_upper = row + }else if (Vector.size(row) == 2){ + row_lower = row[0] + row_upper = row[1] + }else{ + dde_error("row has invalid dimensions") + } + if (Vector.size(column) == 1){ + col_lower = column + col_upper = column + }else if (Vector.size(column) == 2){ + col_lower = column[0] + col_upper = column[1] + }else{ + dde_error("column has invalid dimensions") + } + if ((row_lower < 0) || (row_upper > dim[0]) || (col_lower < 0) || (col_upper > dim[1])){ + dde_error("indeces exceed matrix dimensions") + } + + let result = Vector.make_matrix(row_upper-row_lower+1, col_upper-col_lower+1) + for(var i = row_lower; i < row_upper+1; i++){ + for(var j = col_lower; j < col_upper+1; j++){ + result[i-row_lower][j-col_lower] = matrix[i][j] + } + } + return result + } + */ + + + this.pull = function(matrix, row, column){ + let dim = Vector.matrix_dimensions(matrix) + if(dim[0] == 1){ + matrix = [matrix] + } + let row_lower, row_upper, col_lower, col_upper + if (Vector.size(row) == 1){ + row_lower = row + row_upper = row + }else if (Vector.size(row) == 2){ + row_lower = row[0] + row_upper = row[1] + }else{ + dde_error("row has invalid dimensions") + } + if (Vector.size(column) == 1){ + col_lower = column + col_upper = column + }else if (Vector.size(column) == 2){ + col_lower = column[0] + col_upper = column[1] + }else{ + dde_error("column has invalid dimensions") + } + if ((row_lower < 0) || (row_upper > dim[0]) || (col_lower < 0) || (col_upper > dim[1])){ + dde_error("indeces exceed matrix dimensions") + } + + let result = Vector.make_matrix(row_upper-row_lower+1, col_upper-col_lower+1) + for(var i = row_lower; i < row_upper+1; i++){ + for(var j = col_lower; j < col_upper+1; j++){ + result[i-row_lower][j-col_lower] = matrix[i][j] + } + } + if(Vector.matrix_dimensions(result)[0] == 1){ + return result[0] + } + return result + } + //Vector.pull([[1, 2, 3], [4, 5, 6], [7, 8, 9]], [1, 2], [1, 2]) + + + this.insert = function(big_matrix, small_matrix, location = [0, 0]){ + let big_dim = Vector.matrix_dimensions(big_matrix) + let small_dim = Vector.matrix_dimensions(small_matrix) + let result = big_matrix + let small_i, small_j + + for(let i = location[0]; (i < location[0]+small_dim[0]) && (i < big_dim[0]); i++){ + for(let j = location[1]; (j < location[1]+small_dim[1]) && (j < big_dim[1]); j++){ + small_i = i-location[0] + small_j = j-location[1] + result[i][j] = small_matrix[small_i][small_j] + } + } + return result + } + /* + + var my_big = Vector.make_matrix(10) + var my_small = Vector.make_matrix(3, 2, 6) + debugger + var result = Vector.insert(my_big, my_small, [3, 4]) + */ + + + + /* + this.round(vector, decimal_places = 1){ + let result = Vector.multiply(vector, decimal_places) + let dim = Vector.matrix_dimensions(result) + for(var i = 0; i < dim[0]; i++){ + for(var j = 0; j < dim[1]; j++){ + result[i][j] = Math.round(result[i][j]) + } + } + result = Vector.divide(result, decimal_places) + }*/ +//debugger +//Vector.properly_define_point([[1, 2, 3], [1, 2, 3], [1, 2, 3], [1, 2, 3]]) +// +/*1 +var point_list = [[0, 0, 0], [100000, 0, 0], [100000, 100000, 0], [0, 100000, 0]] +//Vector.add(point_list, [10000, 0]) +var pointA = [441543, 143953, 612665] +var pointB = [447231, 271541, 604905] +var pointC = [451538, 283335, 607526] +var results = Vector.three_points_to_transformation(point_list, pointA, pointB, pointC, [651538, 283335, 607526]) +let plane_1 = Vector.points_to_plane(pointA, pointB, pointC) +let plane_2 = Vector.points_to_plane(results[0], results[1], results[2]) +out(plane_1) +out(plane_2) +*/ +} + +/* +rot_plane = cross(points_plane, [0, 0, 1]) +debugger +Vector.cross([Px, Py, Pz], [0, 0, 1]) +rot_plane = [Py, -Pz, 0] + +[x*cos(theta), y*cos(theta), 0] + cross([Py, -Pz, 0], [x, y, 0])*sin(theta) +[0, 0, Py*y + Pz*x] + +[x*cos(theta)+dx, y*cos(theta)+dy, (Py*y + Pz*x)*sin(theta)+dz] +*/ + + + + + + + +//Public +//This class contains functions for the kinematics of the robot +//As of now it only does kinematcs for a non-moving points +var Kin = new function(){ + + + + this.IK = function({xyz, normal = [0, 0, -1], config = [1, 1, 1], base_coor, ref_coor}){ + if(base_coor === undefined){ + + }else{ + U[0] = base_coor.get_position() + P[0] = base_coor.get_pose() + } + + var J = new Array(5) // Joint Angles + var U = new Array(5).fill(new Array(3)) //Point Locations + var P = new Array(3).fill(new Array(4)) //Planes + var L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] //Link Lengths + + //Default Values: + U[0] = U0 //Base Location + var V10 = V10 //Base Orientation + P[0] = P0 //Base Plane + var right_arm = state[0]; + var elbow_up = state[1]; + var wrist_out = state[2]; + + + + //Calculations: + var V54 = Vector.multiply(-1, Vector.normalize(normal)) //Direction of + if(Vector.is_equal(tool_xyz, [0, 0, Dexter.LINK5]) && (Vector.is_equal(tool_normal, [0, 0, 1]))){ + //Desired End Effector Position + U[5] = xyz + U[4] = Vector.add(U[5], Vector.multiply(L[4], V54)) + }else{ + /* + let tool_V54 = Vector.multiply(-1, tool_xyz) + let tool_U5 = xyz + let tool_U4 = Vector.add(xyz, tool_V54) + let theta = Vector.signed_angle(normal, tool_normal, Vector.cross(normal, tool_normal)) + let tool_U4 = Vector.rotate(tool_U4, Vector.cross(normal, tool_normal), theta, tool_U5) + let tool_plane = Vector.complete_plane(tool_V54, tool_U4) + //let proj_U1 = + + local_xyz + local_normal + U[4] = Vector.add(U[5], Vector.multiply(L[4], V54)) + */ + } + + + //Easy points + U[1] = Vector.add(U[0], Vector.multiply(L[0], V10)) + + + //Solving for P1 + //debugger + P[1] = Vector.points_to_plane(U[1], U[0], U[4]) + + //Solving for U3 + var U54_Proj = Vector.project_vector_onto_plane(V54, P[1]) + var U3_a = Vector.add(U[4], Vector.multiply(L[3], Vector.rotate(Vector.normalize(U54_Proj), P[1], 324000))) + var U3_b = Vector.add(U[4], Vector.multiply(L[3], Vector.rotate(Vector.normalize(U54_Proj), P[1], -324000))) + var dist_a = Vector.distance(U3_a, U[1], U[0]) + var dist_b = Vector.distance(U3_b, U[1], U[0]) + if (wrist_out){ + if (dist_a < dist_b){ + U[3] = U3_a + }else{ + U[3] = U3_b + } + }else{ + if (dist_a > dist_b){ + U[3] = U3_a + }else{ + U[3] = U3_b + } + } + + + //Solving for P2 + P[2] = Vector.points_to_plane(U[5], U[4], U[3]) + + //Solving for U2 + var D3 = Vector.distance(U[3], U[1]) + + //Checking if in reach + //debugger + if (D3 > Dexter.LINK2 + Dexter.LINK3){ + let out_of_reach_dist = Vector.round(Convert.microns_to_mms(D3 - (Dexter.LINK2 + Dexter.LINK3)), 3) + + dde_error(Vector.round(Convert.microns_to_mms(xyz), 1) + 'Location is ' + out_of_reach_dist + ' mm out of reach') + } + + + var Beta = Vector.acos_arcsec((-Math.pow(L[2], 2) + Math.pow(L[1], 2) + Math.pow(D3, 2)) / (2 * D3 * L[1])) // Law of Cosines + var V31 = Vector.normalize(Vector.subtract(U[3], U[1])) + var V23 + + var U2_a = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], Beta))) + var U2_b = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], -Beta))) + var U2_a_dist = Vector.distance(U2_a, P[0]) + var U2_b_dist = Vector.distance(U2_b, P[0]) + + if (elbow_up){ + if(U2_a_dist > U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + }else{ + if(U2_a_dist < U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + } + + + //Solving for joint angles + + //var V10 = minus(U[1], U[0]) + var V21 = Vector.normalize(Vector.subtract(U[2], U[1])) + var V32 = Vector.normalize(Vector.subtract(U[3], U[2])) + var V43 = Vector.normalize(Vector.subtract(U[4], U[3])) + //var V54 = minus(U[5], U[3]) + + J[0] = Vector.signed_angle(P[1], P[0], V10) //648000 = 180 degrees + J[1] = Vector.signed_angle(V21, V10, P[1]) + J[2] = Vector.signed_angle(V32, V21, P[1]) + J[3] = Vector.signed_angle(V43, V32, P[1]) + J[4] = Vector.signed_angle(P[2], P[1], V43) //648000 = 180 degrees + + return [J, U, P] + } + + + + + + + + //Private + this.inverse_kinematics = function (xyz, normal = [0, 0, -1], state = [1, 1, 1], U0 = [0, 0, 0], V10 = [0, 0, 1], P0 = [1, 0, 0, 0], tool_xyz = [0, 0, Dexter.LINK5], tool_normal = [0, 0, 1]){ + var J = new Array(5) // Joint Angles + var U = new Array(5).fill(new Array(3)) //Point Locations + var P = new Array(3).fill(new Array(4)) //Planes + var L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] //Link Lengths + + //Default Values: + U[0] = U0 //Base Location + var V10 = V10 //Base Orientation + P[0] = P0 //Base Plane + var right_arm = state[0]; + var elbow_up = state[1]; + var wrist_out = state[2]; + + + + //Calculations: + var V54 = Vector.multiply(-1, Vector.normalize(normal)) //Direction of + if(Vector.is_equal(tool_xyz, [0, 0, Dexter.LINK5]) && (Vector.is_equal(tool_normal, [0, 0, 1]))){ + //Desired End Effector Position + U[5] = xyz + U[4] = Vector.add(U[5], Vector.multiply(L[4], V54)) + }else{ + /* + let tool_V54 = Vector.multiply(-1, tool_xyz) + let tool_U5 = xyz + let tool_U4 = Vector.add(xyz, tool_V54) + let theta = Vector.signed_angle(normal, tool_normal, Vector.cross(normal, tool_normal)) + let tool_U4 = Vector.rotate(tool_U4, Vector.cross(normal, tool_normal), theta, tool_U5) + let tool_plane = Vector.complete_plane(tool_V54, tool_U4) + //let proj_U1 = + + local_xyz + local_normal + U[4] = Vector.add(U[5], Vector.multiply(L[4], V54)) + */ + } + + + //Easy points + U[1] = Vector.add(U[0], Vector.multiply(L[0], V10)) + + + //Solving for P1 + //debugger + P[1] = Vector.points_to_plane(U[1], U[0], U[4]) + + //Solving for U3 + var U54_Proj = Vector.project_vector_onto_plane(V54, P[1]) + var U3_a = Vector.add(U[4], Vector.multiply(L[3], Vector.rotate(Vector.normalize(U54_Proj), P[1], 324000))) + var U3_b = Vector.add(U[4], Vector.multiply(L[3], Vector.rotate(Vector.normalize(U54_Proj), P[1], -324000))) + var dist_a = Vector.distance(U3_a, U[1], U[0]) + var dist_b = Vector.distance(U3_b, U[1], U[0]) + if (wrist_out){ + if (dist_a < dist_b){ + U[3] = U3_a + }else{ + U[3] = U3_b + } + }else{ + if (dist_a > dist_b){ + U[3] = U3_a + }else{ + U[3] = U3_b + } + } + + + //Solving for P2 + P[2] = Vector.points_to_plane(U[5], U[4], U[3]) + + //Solving for U2 + var D3 = Vector.distance(U[3], U[1]) + + //Checking if in reach + //debugger + if (D3 > Dexter.LINK2 + Dexter.LINK3){ + let out_of_reach_dist = Vector.round(Convert.microns_to_mms(D3 - (Dexter.LINK2 + Dexter.LINK3)), 3) + out(V54) + dde_error(Vector.round(Convert.microns_to_mms(xyz), 1) + 'Location is ' + out_of_reach_dist + ' mm out of reach') + } + + + var Beta = Vector.acos_arcsec((-Math.pow(L[2], 2) + Math.pow(L[1], 2) + Math.pow(D3, 2)) / (2 * D3 * L[1])) // Law of Cosines + var V31 = Vector.normalize(Vector.subtract(U[3], U[1])) + var V23 + + var U2_a = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], Beta))) + var U2_b = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], -Beta))) + var U2_a_dist = Vector.distance(U2_a, P[0]) + var U2_b_dist = Vector.distance(U2_b, P[0]) + + if (elbow_up){ + if(U2_a_dist > U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + }else{ + if(U2_a_dist < U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + } + + + //Solving for joint angles + + //var V10 = minus(U[1], U[0]) + var V21 = Vector.normalize(Vector.subtract(U[2], U[1])) + var V32 = Vector.normalize(Vector.subtract(U[3], U[2])) + var V43 = Vector.normalize(Vector.subtract(U[4], U[3])) + //var V54 = minus(U[5], U[3]) + + if(right_arm == 1){ + J[0] = Vector.signed_angle(P[1], P[0], V10) //648000 = 180 degrees + J[1] = Vector.signed_angle(V21, V10, P[1]) + J[2] = Vector.signed_angle(V32, V21, P[1]) + J[3] = Vector.signed_angle(V43, V32, P[1]) + J[4] = Vector.signed_angle(P[2], P[1], V43) //648000 = 180 degrees + }else{ + J[0] = Vector.signed_angle(P[1], P[0], V10) + 648000 //648000 = 180 degrees + J[1] = -Vector.signed_angle(V21, V10, P[1]) + J[2] = -Vector.signed_angle(V32, V21, P[1]) + J[3] = -Vector.signed_angle(V43, V32, P[1]) + J[4] = -Vector.signed_angle(P[2], P[1], V43) + } + + return [J, U, P] + } + /* + Kin.inverse_kinematics([0, 400000, 100000], [0, 0, -1], Dexter.RIGHT_UP_OUT) + debugger + Kin.inverse_kinematics([0, 400000, 100000], [0, 0, -1], Dexter.LEFT_UP_OUT) + */ + //Public + //convention for declaring local link reference frames: + //origin is located at intersection of axis of rotation and plane of rotation + //z axis is along link length + //x axis is along axis of rotation + /* + this.new_end_effector_transform = function(xyz, normal, local_xyz = [0, 0, Dexter.LINK5], local_normal = [0, 0, 1]){ + let default_tool_tip = [0, 0, Dexter.LINK5] + let default_tool_normal = [0, 0, 1] + let theta = Vector.signed_angle(local_normal, default_tool_normal, [1, 0, 0]) + let result_normal = Vector.rotate(normal, [1, 0, 0], theta) + let delta = Vector.subtract(default_tool_tip, local_xyz) + let result_xyz = Vector.add(xyz, Vector.multiply(delta, normal)) + return [result_xyz, result_normal] + } + + debugger + Kin.new_end_effector_transform([500000, 500000, 500000], [0, 0, -1], [10000, 10000, 82550 + 10000], [0, 0, 1]) + */ + + + //Private + //Calculates point positions given joint angles + this.forward_kinematics = function(joint_angles, U0 = [0, 0, 0], V10 = [0, 0, 1], P0 = [1, 0, 0, 0]){ + var J = Convert.deep_copy(joint_angles) //Joint Angles + var U = new Array(5).fill(new Array(3)) //Point Locations + var L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] //Link Lengths + var P = new Array(3).fill(new Array(4)) //Planes + + var U21, U32, U43, U54, V21, V32, V43, V54 + + //Calculates all vectors first + P[0] = P0 + P[1] = Vector.rotate(P[0], V10, 648000 - J[0]) + V21 = Vector.rotate(V10, P[1], J[1]) + V32 = Vector.rotate(V21, P[1], J[2]) + V43 = Vector.rotate(V32, P[1], J[3]) + P[3] = Vector.rotate(P[1], V43, 648000 - J[4]) + V54 = Vector.rotate(V43, P[3], -324000) // 324000 = 90 degrees + let V = [V10, V21, V32, V43, V54] + + //Dimensionalizes vectors by multiplying by link lengths + U[0] = U0 + U[1] = Vector.add(U[0], Vector.multiply(L[0], V10)) + U[2] = Vector.add(U[1], Vector.multiply(L[1], V21)) + U[3] = Vector.add(U[2], Vector.multiply(L[2], V32)) + U[4] = Vector.add(U[3], Vector.multiply(L[3], V43)) + U[5] = Vector.add(U[4], Vector.multiply(L[4], V54)) + + return [U, V] + } + + + + + //This allows the base rotation to be represented as an angle + //Returns the base plane given that angle + this.base_rotation_to_plane = function(base_rotation, base_plane){ + let temp_plane + if(Vector.is_equal(base_plane, [0, 1, 0])){ + temp_plane = [0, 0, -1] + }else{ + temp_plane = [0, 1, 0] + } + let temp_vector = Vector.cross(temp_plane, base_plane) + return Vector.rotate(temp_vector, base_plane, base_rotation) + } + + + this.L3_inverse_kinematics = function(J3_xyz, orientation = [0, 0, -1], config = Dexter.RIGHT_DOWN_OUT){ + var J = new Array(5) // Joint Angles + var U = new Array(5).fill(new Array(3)) //Point Locations + var P = new Array(3).fill(new Array(4)) //Planes + var L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] //Link Lengths + + //Default Values: + U[0] = [0, 0, 0] //Base Location + var V10 = [0, 0, 1] //Base Orientation + P[0] = [1, 0, 0] //Base Plane + var right_arm = config[0]; + var elbow_up = config[1]; + var wrist_out = config[2]; + + + //Easy points + U[1] = Vector.add(U[0], Vector.multiply(L[0], V10)) + var V54 = Vector.multiply(-1, Vector.normalize(normal)) //Direction of + U[3] = J3_xyz + + //Solving for P1 + P[1] = Vector.points_to_plane(U[1], U[0], U[3]) + + //Solving for U2 + var D3 = Vector.distance(U[3], U[1]) + + //Checking if in reach + //debugger + if (D3 > Dexter.LINK2 + Dexter.LINK3){ + let out_of_reach_dist = Vector.round(Convert.microns_to_mms(D3 - (Dexter.LINK2 + Dexter.LINK3)), 3) + out(V54) + dde_error(Vector.round(Convert.microns_to_mms(xyz), 1) + 'Location is ' + out_of_reach_dist + ' mm out of reach') + } + + + var Beta = Vector.acos_arcsec((-Math.pow(L[2], 2) + Math.pow(L[1], 2) + Math.pow(D3, 2)) / (2 * D3 * L[1])) // Law of Cosines + var V31 = Vector.normalize(Vector.subtract(U[3], U[1])) + var V23 + + var U2_a = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], Beta))) + var U2_b = Vector.add(U[1], Vector.multiply(L[1], Vector.rotate(V31, P[1], -Beta))) + var U2_a_dist = Vector.distance(U2_a, P[0]) + var U2_b_dist = Vector.distance(U2_b, P[0]) + + if (elbow_up){ + if(U2_a_dist > U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + }else{ + if(U2_a_dist < U2_b_dist){ + U[2] = U2_a + }else{ + U[2] = U2_b + } + } + + + //Solving for joint angles + + //var V10 = minus(U[1], U[0]) + var V21 = Vector.normalize(Vector.subtract(U[2], U[1])) + var V32 = Vector.normalize(Vector.subtract(U[3], U[2])) + Vector.signed_angle(V54, V32) + var V43 = Vector.normalize(Vector.subtract(U[4], U[3])) + + if(right_arm == 1){ + J[0] = Vector.signed_angle(P[1], P[0], V10) //648000 = 180 degrees + J[1] = Vector.signed_angle(V21, V10, P[1]) + J[2] = Vector.signed_angle(V32, V21, P[1]) + J[3] = Vector.signed_angle(V43, V32, P[1]) + J[4] = Vector.signed_angle(P[2], P[1], V43) //648000 = 180 degrees + }else{ + J[0] = Vector.signed_angle(P[1], P[0], V10) + 648000 //648000 = 180 degrees + J[1] = -Vector.signed_angle(V21, V10, P[1]) + J[2] = -Vector.signed_angle(V32, V21, P[1]) + J[3] = -Vector.signed_angle(V43, V32, P[1]) + J[4] = -Vector.signed_angle(P[2], P[1], V43) + } + + return [J, U, P] + } + + + + //Public + this.is_in_reach = function(xyz, J5_direction = [0, 0, -1], base_xyz = [0, 0, 0], base_plane = [0, 0, 1]){ + let U1 = Vector.add(base_xyz, Vector.multiply(base_plane, Dexter.LINK1)) + let U4 = Vector.add(xyz, Vector.multiply(-1, Vector.normalize(J5_direction))) + if (Vector.distance(U1, U4) <= Dexter.LINK2 + Dexter.LINK3 + Dexter.LINK4){ + return true + }else{ + return false + } + } + + + + + //Public + this.table_intersection = function(joint_points){ + let margins = [null, null, 40000, 30000, 30000, 0] //These are the distances each point can be from the table + + let U_Copy = Convert.deep_copy(joint_points) + let base_plane = Vector.normalize(Vector.subtract(U_Copy[1], U_Copy[0])) + base_plane.push(0) + let bad_points = [] + let point_dist + + for(var i = 2; i <= 5; i++){ + point_dist = -Vector.distance(U_Copy[i], base_plane) + if (margins[i] > point_dist){ + bad_points.push([i, point_dist]) + } + } + if (bad_points.length === 1){ + bad_points = [bad_points] + } + + if (bad_points.length === 0){ + return false + }else{ + return bad_points + } + } + + //Private + //converts keywords in config to array + //example: parse_config("right_down_in") returns [1, 0, 0] + function parse_config(config){ + var state = [1, 1, 1] + var config_words = config.split("_") + + if (config_words.includes("right")){ + state[0] = 1 + }else{ + if (config_words.includes("left")){ + state[0] = 0 + } + } + if (config_words.includes("up")){ + state[1] = 1 + }else{ + if (config_words.includes("down")){ + state[1] = 0 + } + } + if (config_words.includes("out")){ + state[2] = 1 + }else{ + if (config_words.includes("in")){ + state[2] = 0 + } + } + return state + } + + //Public + this.J_angles_to_config = function(joint_angles){ + let J = Convert.deep_copy(joint_angles) + let U = Kin.forward_kinematics(J) + } + + //Public + this.point_at_xyz = function(xyz, current_J5_xyz, current_config, base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + let pointing_direction = Vector.subtract(xyz, current_J5_xyz) + Kin.xyz_to_J_angles(current_J5_xyz, pointing_direction, current_config, base_xyz, base_plane, base_rotation) + } + + + //Torque: + this.gravity_torques = function(J_angles, base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + //This will return the torques expected due to the forces of gravity + //As of now the output units are in Newton-meters but are subject to change + + + //These will change once measurements are taken + let L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] //Link Lengths + let CM_L = Vector.multiply(.5, L) // Center of mass as distance along the link + let M = [5, 2, 2, .5, .5] //Link masses (kg) (guesses) + let g = [0, 0, -9.80665] // (micron/millisecond^2 or m/s^2, they are equivalent) + var T_vector = new Array(5).fill(new Array(3)) + var T = new Array(5) + var F_vector = new Array(5).fill(new Array(3)) + var CM_r = new Array(5).fill(new Array(3)) + + let P0 = Kin.base_rotation_to_plane(base_rotation, base_plane) + let fk_result = Kin.forward_kinematics(J_angles, base_xyz, base_plane, P0) + let U = fk_result[0] + let V = (fk_result[1]) + let Vn = new Array(3).fill(new Array(5)) + for(var i = 0; i < 5; i++){ + Vn[i] = Vector.normalize(V[i]) + F_vector[i] = Vector.multiply(M[i], g) + CM_r[i] = Vector.multiply(CM_L[i], Vn[i]) + } + //var P1 = Vector.points_to_plane(U[1], U[0], U[4]) + var P1 = Vector.rotate(P0, base_plane, J_angles[0]) + + + + + //Torques are calculated backwards from the end effector + //The system is stationary so the sum of the torques equal zero + //the torque vector is found by crossing the radius (distance from joint to link's center of mass) and the weight vector + //that torque vector may only have some components that affect the actual joint's torque reading + //This is dealt with by projecting the torque vector onto the axis of the joint's rotation + let T_sum = [0, 0, 0] + let F_sum = 0 + let radius + let planes_of_rotation = [Vn[0], P1, P1, P1, Vn[3]] + + //Joints 1 and 2 + T_sum = [0, 0, 0] + T_sum = Vector.add(T_sum, Vector.cross(Vector.multiply(CM_L[1], Vn[1]), F_vector[1])) + radius = Vector.add(V[1], CM_r[2]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[2])) + radius = Vector.add(V[1], V[2], CM_r[3]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[3])) + radius = Vector.add(V[1], V[2], V[2], CM_r[4]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[4])) + T_vector[0] = T_sum + T_vector[1] = T_sum + + //Joint 3 + T_sum = [0, 0, 0] + radius = CM_r[2] + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[2])) + radius = Vector.add(V[2], CM_r[3]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[3])) + radius = Vector.add(V[2], V[3], CM_r[4]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[4])) + T_vector[2] = T_sum + + //Joint 4 + T_sum = [0, 0, 0] + radius = CM_r[3] + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[3])) + radius = Vector.add(V[3], CM_r[4]) + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[4])) + T_vector[3] = T_sum + + //Joint 5 + T_sum = [0, 0, 0] + radius = CM_r[4] + T_sum = Vector.add(T_sum, Vector.cross(radius, F_vector[4])) + T_vector[4] = T_sum + + for(var i = 0; i < 5; i++){ + T[i] = Vector.dot(planes_of_rotation[i], T_vector[i]) + } + + T = Vector.multiply(.000001, T) // Converting to Nm (will change) + return [T, T_vector, U, planes_of_rotation] + } + + + this.torques_to_force = function(measured_torques = [0, 0, 0, 0, 0], J_angles = [0, 0, 0, 0, 0], base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + let P0 = Kin.base_rotation_to_plane(base_rotation, base_plane) + let fk_result = Kin.forward_kinematics(J_angles, base_xyz, base_plane, P0) + let et_result = Kin.expected_torques(J_angles, base_xyz, base_plane, base_rotation) + let gravity_torques = et_result[0] + let U = et_result[2] + let planes_of_rotation = et_result[3] + let forces = new Array(5).fill(new Array(3)) + let force = [0, 0, 0] + + let Fi = [0, 0, 0] //Force that lie along the intersection of P1 and P0 + let Fj = [0, 0, 0] //Force perpendicular to P1 + let Fk = [0, 0, 0] //Force perpendicular to P0 + + let applied_torques = Vector.subtract(measured_torques, gravity_torques) + let applied_torque_vectors = new Array(5).fill(new Array(3)) + for(var i = 0; i < 5; i++){ + applied_torque_vectors[i] = Vector.multiply(applied_torques[i], planes_of_rotation[i]) + } + + for(var i = 0; i < 5; i++){ + forces[i] = Vector.cross(applied_torque_vectors[i], Vector.subtract(U[5], U[i])) + } + + + + //assuming there are only forces acting on the end effector + //no torques and no other forcing points + let count = [0, 0, 0] + let total_force = [0, 0, 0] + let force_comp + for(var i = 0; i < 5; i++){ + for(var j = 0; j <3; j++){ + force_comp = forces[i][j] + if(force_comp !== 0){ + count[j]++ + total_force[j] += force_comp + } + } + } + for(var i = 0; i < 3; i++){ + force[i] = total_force[i] / count[i] + } + + + return [force, forces] + } + + + this.check_J_ranges = function(J_angles){ + let lower_limit = [Dexter.J1_ANGLE_MIN, Dexter.J2_ANGLE_MIN, Dexter.J3_ANGLE_MIN, Dexter.J4_ANGLE_MIN, Dexter.J5_ANGLE_MIN] + let upper_limit = [Dexter.J1_ANGLE_MAX, Dexter.J2_ANGLE_MAX, Dexter.J3_ANGLE_MAX, Dexter.J4_ANGLE_MAX, Dexter.J5_ANGLE_MAX] + let angle + for(var i = 0; i < J_angles.length; i++){ + angle = J_angles[i] + if((angle != null) && ((lower_limit[i] > angle) || (upper_limit[i] < angle))){ + return false + } + } + return true + } + + + + + /************************************************************** + Wrapper Functions: + ***************************************************************/ + + //Public + //Wrapper function for inverse kinematics + //Returns joint angles + this.xyz_to_J_angles = function(xyz, J5_direction = [0, 0, -1], config = Dexter.RIGHT_DOWN_OUT, base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + var P0 = Kin.base_rotation_to_plane(base_rotation, base_plane) + + if(typeof(config) === "string"){ + state = parse_config(config) + }else{ + var state = config + } + + //xyz = Convert.mms_to_microns(xyz) + //return Convert.arcseconds_to_degrees(inverse_kinematics(xyz, normal, state, U0, V10, P0)) + var result = Kin.inverse_kinematics(xyz, J5_direction, state, base_xyz, base_plane, P0) + return result[0] + } + + + //Public + //Wrapper function for inverse kinematics + //Returns joint points + this.xyz_to_J_points = function(xyz, J5_direction = [0, 0, -1], config = "right_up_out", base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + var P0 = base_rotation_to_plane(base_rotation, base_plane) + + if(typeof(config) === "string"){ + state = parse_config(config) + }else{ + var state = config + } + var result = Kin.inverse_kinematics(xyz, J5_direction, state, base_xyz, base_plane, P0) + return result[1] + } + + + + //Public + //Wrapper function for inverse kinematics + //Returns joint points + this.xyz_to_J_planes = function(xyz, J5_direction = [0, 0, -1], config = "right_up_out", base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + var P0 = base_rotation_to_plane(base_rotation, base_plane) + + if(typeof(config) === "string"){ + state = parse_config(config) + }else{ + var state = config + } + + var result = Kin.inverse_kinematics(xyz, J5_direction, state, base_xyz, base_plane, P0) + return result[2] + } + + + //Public + //Wrapper function for forward kinematics + this.J_angles_to_xyz = function(joint_angles, base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + //var temp_angles = Convert.degrees_to_arcseconds(joint_angles) + var P0 = Kin.base_rotation_to_plane(base_rotation, base_plane) + var temp_angles = Convert.deep_copy(joint_angles) + let result = Kin.forward_kinematics(temp_angles, base_xyz, base_plane, P0) + return result[0] + } + + + this.J_angles_to_coor = function(joint_angles, L0_pose){ + //let L0, L1, L2, L3, L4, L5 + //var L0, L1, L2, L3, L4, L5 + if(Object.isNewObject(L0_pose)){ + L0 = L0_pose + //L0.name = undefined + }else if(Vector.is_pose(L0_pose)){ + L0 = Table.create_child(L0_pose) + }else if(L0_pose == undefined){ // this should get replaced with is_Coor() + L0 = Table.create_child(Vector.make_pose()) + }else{ + dde_error("L0_pose input arg must be a Coordinate System Object, a pose, or undefined") + } + let L = [Dexter.LINK1, Dexter.LINK2, Dexter.LINK3, Dexter.LINK4, Dexter.LINK5] + let J = joint_angles + + + L1 = L0.create_child(Vector.make_pose([0, 0, L[0]]), "L1") + L2 = L1.create_child(Vector.make_pose([0, 0, L[1]]), "L2") + L3 = L2.create_child(Vector.make_pose([0, 0, L[2]]), "L3") + L4 = L3.create_child(Vector.make_pose([0, 0, L[3]]), "L4") + L5 = L4.create_child(Vector.make_pose([0, 0, 0]), "L5") + + + + L1.rotate("Z", J[0], [0, 0, 0]) + L2.rotate("X", J[1], [0, 0, 0]) + L3.rotate("X", J[2], [0, 0, 0]) + L4.rotate("X", J[3], [0, 0, 0]) + L5.rotate("Z", J[4], [0, 0, 0]) + + return [L0.get_pose(Table), L1.get_pose(Table), L2.get_pose(Table), L3.get_pose(Table), L4.get_pose(Table), L5.get_pose(Table)] + //return [L0, L1, L2, L3, L4, L5] //this crashes DDE + //return null + } + /* + debugger + var my_poses = Kin.J_angles_to_coor([162000, 0, 0, 0, 162000], Table) + [L1] + */ + + this.set_Dexter_coor = function(poses){ + + L0.set_pose(poses[0], Table) + L1.set_pose(poses[1], Table) + L2.set_pose(poses[2], Table) + L3.set_pose(poses[3], Table) + L4.set_pose(poses[4], Table) + L5.set_pose(poses[5], Table) + + return null + } + /* + debugger + var my_poses = Kin.J_angles_to_coor([162000, 0, 0, 0, 162000], Table) + Kin.set_Dexter_coor(my_poses) + out(L0) + [L1] + */ + + this.three_positions_to_pose = function(J_angles_1, J_angles_2, J_angles_3){ + let points_A, points_B, points_C, UA5, UA4, UB5, UB4, UC5, UC4, U5_ave, U4_ave, U45 + let point, x_vector, y_vector, z_vector, pose, angleA, angleB, angleC, vector_1, vector_2 + + points_A = Kin.J_angles_to_xyz(J_angles_1) + points_B = Kin.J_angles_to_xyz(J_angles_2) + points_C = Kin.J_angles_to_xyz(J_angles_3) + + UA5 = points_A[5] + UA4 = points_A[4] + UB5 = points_B[5] + UB4 = points_B[4] + UC5 = points_C[5] + UC4 = points_C[4] + + U5_ave = Vector.average(UA5, UB5, UC5) + U4_ave = Vector.average(UA4, UB4, UC4) + U45 = Vector.subtract(U4_ave, U5_ave) + + angleA = Vector.angle(Vector.subtract(UB5, UA5), Vector.subtract(UC5, UA5)) + angleB = Vector.angle(Vector.subtract(UA5, UB5), Vector.subtract(UC5, UB5)) + angleC = Vector.angle(Vector.subtract(UB5, UC5), Vector.subtract(UA5, UC5)) + + switch(Math.max(angleA, angleB, angleC)){ + case angleA: + point = UA5 + vector_1 = Vector.subtract(UB5, UA5) + vector_2 = Vector.subtract(UC5, UA5) + break + + case angleB: + point = UB5 + vector_1 = Vector.subtract(UA5, UB5) + vector_2 = Vector.subtract(UC5, UB5) + break + + case angleC: + point = UC5 + vector_1 = Vector.subtract(UB5, UC5) + vector_2 = Vector.subtract(UA5, UC5) + break + } + + if(0 < Vector.dot(Vector.cross(vector_1, vector_2), U45)){ + x_vector = Vector.normalize(vector_1) + }else{ + x_vector = Vector.normalize(vector_2) + } + + z_vector = Vector.pull(Vector.points_to_plane(UA5, UB5, UC5), 0, [0, 2]) + if(0 > Vector.dot(z_vector, U45)){ + z_vector = Vector.multiply(-1, z_vector) + } + + pose = Vector.make_pose(point, Vector.make_dcm(x_vector, undefined, z_vector)) + return pose + } + /* + debugger + var result = Kin.three_positions_to_pose([0, 0, 0, 0, 0], [0, 0, 0, 162000, 0], [0, 0, 0, 0, 162000]) + Vector.round(result, 0) + Convert.degrees_to_arcseconds(360) + */ + + + this.J_angles_L5_Direction = function(joint_angles, base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + let J_points = Kin.J_angles_to_xyz(joint_angles, base_xyz, base_plane, base_rotation) + return Vector.normalize(Vector.subtract(J_points[5], J_points[4])) + } + //J_angles_J5_Direction([0, 0, 0, 0, 0]) + // Kin.J_angles_to_xyz([0, 0, 0, 0, 0]) + + + + + //Public + //Wrapper function for inverse kinematics + //Returns joint angles + this.xyz_check = function(xyz, J5_direction = [0, 0, -1], config = "right_up_out", base_xyz = [0, 0, 0], base_plane = [0, 0, 1], base_rotation = 0){ + var P0 = Kin.base_rotation_to_plane(base_rotation, base_plane) + + if(typeof(config) === "string"){ + state = parse_config(config) + }else{ + var state = config + } + + + let ir_result = Kin.is_in_reach(xyz, J5_direction, base_xyz, base_plane) + if(ir_result !== true){ + return ir_result + } + + var ik_result = Kin.inverse_kinematics(xyz, J5_direction, state, base_xyz, base_plane, P0) + var table_result = Kin.table_intersection(ik_result[1]) + if (table_result !== false){ + let intersection_strn = "Joint" + + switch (table_result.length){ + case 1: + intersection_strn += table_result[0][0] + " is below the base plane by " + + Vector.round(Convert.microns_to_mms(table_result[0][1]), 3) + " mm." + break + case 2: + intersection_strn += table_result[0][0] + " and Joint" + table_result[1][0] + " are below the base plane by " + + Vector.round(Convert.microns_to_mms(table_result[0][1]), 3) + "mm and " + + Vector.round(Convert.microns_to_mms(table_result[1][1]), 3) + "mm, respectively " + break + case 3: + intersection_strn += table_result[0][0] + ", Joint" + table_result[1][0] + ", and Joint" + table_result[2][0] + " are below the base plane by " + + Vector.round(Convert.microns_to_mms(table_result[0][1]), 3) + "mm, " + + Vector.round(Convert.microns_to_mms(table_result[1][1]), 3) + "mm, and " + + Vector.round(Convert.microns_to_mms(table_result[2][1]), 3) + "mm, respectively " + break + case 4: + intersection_strn += table_result[0][0] + ", Joint" + table_result[1][0] + + ", Joint" + table_result[2][0] + ", and Joint" + table_result[3][0] + " are below the base plane by " + + Vector.round(Convert.microns_to_mms(table_result[0][1]), 3) + "mm, " + + Vector.round(Convert.microns_to_mms(table_result[1][1]), 3) + "mm, " + + Vector.round(Convert.microns_to_mms(table_result[2][1]), 3) + "mm, and " + + Vector.round(Convert.microns_to_mms(table_result[3][1]), 3) + "mm, respectively " + break + default: + } + //out(intersection_strn) + return false // + } + + return true + } + + +} + + + +new TestSuite("Inverse to Forward Kinematics and Back", + ["var point_1 = [100000, 200000, 300000]"], + ["var myJangles = Kin.xyz_to_J_angles(point_1, [0, 1, -1], Dexter.RIGHT_DOWN_OUT, [0, 0, 500000], [0, 0, -1], 45)"], + ["var myPoints = Kin.J_angles_to_xyz(myJangles, [0, 0, 500000], [0, 0, -1], 45)"], + ["myPoints[5]"], + ["point_1"] +) + +new TestSuite("Checking xyz", + ["Kin.xyz_check([500000, 0, 500000])", "true"], + ["Kin.xyz_check([500000, 0, -500000])", "false"], + ["Kin.xyz_check([5000000, 0, 0])", "false"], + ["Kin.check_J_ranges([0, 0, 0, 0, 0])", "true"], + ["Kin.check_J_ranges([0, 0, 0, 648000, 0])", "false"], + ["Kin.gravity_torques([0, 0, 0, 0, 0])", "[[0, -0.20238473937499996, -0.20238473937499996, -0.20238473937499996, 0], [ [-202384.73937499998, 4.956996465437301e-11, 0], [-202384.73937499998, 4.956996465437301e-11, 0], [-202384.73937499998, 4.956996465437301e-11, 0], [-202384.73937499998, 4.956996465437301e-11, 0], [-202384.73937499998, 4.956996465437301e-11, 0]], [ [0, 0, 0], [0, 0, 165100], [0, 0, 485775], [0, 0, 815975], [0, 0, 866775], [2.02189186539228e-11, 82550, 866775]], [[0, 0, 1], [1, 0, 0], [1, 0, 0], [1, 0, 0], [0, 0, 1]]]"], + ["Kin.gravity_torques([0, 324000, 0, 0, 0])", "[ [ 0, -6.507491903675, -3.3627149949749997, -0.124549358325, 2.47849823271865e-17], [ [-6507491.903675, 8.217228953584769e-10, 0], [-6507491.903675, 8.217228953584769e-10, 0], [-3362714.9949749997, 4.3659879783128066e-10, 0], [-124549.35832500001, 4.00378796280433e-11, 0], [-2.47849823271865e-11, 2.4784982327186504e-11, 0]], [ [0, 0, 0], [0, 0, 165100], [3.927136123165775e-11, 320675, 165100.00000000003], [7.970919853950334e-11, 650875, 165100.00000000006], [8.59304042791719e-11, 701675, 165100.00000000006], [9.60398636061333e-11, 701675, 82550.00000000006]], [ [0, 0, 1], [1, 0, 0], [1, 0, 0], [1, 0, 0], [1.2246467991473532e-16, 1, 6.123233995736766e-17]]]"] +) + + +new TestSuite("Vector Library", + ["Vector.add([1, 2, 3], [4, 5, 6])", "[5, 7, 9]"], + ["Vector.add([1, 2, 3], [4, 5, 6], 10, 50)", "[65, 67, 69]"], + ["Vector.subtract([4, 5, 6], [1, 2, 3])", "[3, 3, 3]"], + ["Vector.subtract([4, 5, 6], 1, [1, 2, 3])", "[2, 2, 2]"], + ["Vector.multiply([1, 2, 3], [4, 5, 6])", "[4, 10, 18]"], + ["Vector.multiply([1, 2, 3], [4, 5, 6], 10, 50)", "[2000, 5000, 9000]"], + ["Vector.size([1, 2, 3])", "3"], + ["Vector.size([10])", "1"], + ["Vector.size(10)", "1"], + ["Vector.normalize([1, 1, 0])", "[0.7071067811865475, 0.7071067811865475, 0]"], + ["Vector.dot([1, 2, 3], [4, 5, 6])", "32"], + ["Vector.cross([1, 2, 3], [4, 5, 6])", "[-3, 6, -3]"], + ["Vector.transpose(Vector.transpose([1, 2, 3, 4, 5]))", "[1, 2, 3, 4, 5]"] +) + + +new TestSuite("Vector Library - Matrix Math", + ["Vector.matrix_divide([[1, 0, 0, 10], [0, 1, 0, 20], [0, 0, 1, 30], [0, 0, 0, 1]], [[1, 0, 0, 100], [0, 1, 0, 200], [0, 0, 1, 300], [0, 0, 0, 1]])", "[[1, 0, 0, -90], [0, 1, 0, -180], [0, 0, 1, -270], [0, 0, 0, 1]]"], + ["Vector.matrix_divide([[1, 0, 0, 100], [0, 1, 0, 200], [0, 0, 1, 300], [0, 0, 0, 1]], [[1, 0, 0, 10], [0, 1, 0, 20], [0, 0, 1, 30], [0, 0, 0, 1]])", "[[1, 0, 0, 90], [0, 1, 0, 180], [0, 0, 1, 270], [0, 0, 0, 1]]"], + ["Vector.make_matrix(10, 7)", "[[0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0]]"], + ["Vector.make_matrix(3)", "[[0, 0, 0], [0, 0, 0], [0, 0, 0]]"], + ["Vector.make_matrix(3, 2, 1)", "[[1, 1], [1, 1], [1, 1]]"], + ["Vector.transpose(Vector.transpose([1, 2, 3]))", "[1, 2, 3]"], + ["Vector.matrix_multiply([1, 2, 3], [[1], [2], [3]])", "[[14]]"], + ["Vector.matrix_multiply([[1], [2], [3]], [1, 2, 3])", "[[NaN, NaN, NaN], [NaN, NaN, NaN], [NaN, NaN, NaN]]"], + ["Vector.matrix_multiply([[1, 0, 0, 10], [0, 1, 0, 20], [0, 0, 1, 30], [0, 0, 0, 1]], [[1], [2], [3], [1]])", "[[11], [22], [33], [1]]"], + ["Vector.transpose([1, 2, 3])", "[[1], [2], [3]]"], + ["Vector.transpose([[1, 2, 3], [4, 5, 6]])", "[[1, 4], [2, 5], [3, 6]]"], + ["Vector.determinant([[1, 0, 0], [0, 1, 0], [0, 0, 1]])", "1"], + ["Vector.determinant([[0, 0, 0], [0, 1, 0], [0, 0, 1]])", "0"], + ["Vector.determinant([[ 0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]])", "1"], + ["Vector.inverse([[1, 0, 0], [0, 1, 0], [0, 0, 1]])", "[[1, 0, 0], [0, 1, 0], [0, 0, 1]]"], + ["Vector.inverse([[3, 2, 1.7, 1.5], [4.5, 5, 4.1, 1.9], [1.1, 8.5, 9, 8], [3, 9, 9, 10]])", "[ [ 0.7319863743922018, -0.18592193878878188, 0.10453233481551132, -0.1580986556413699], [ -2.6473723899420833, 1.3118547988663025, -1.2270847960059614, 1.1295212835114843], [ 2.4406479964636474, -1.081727643862005, 1.5172790389266348, -1.374392178277074], [ -0.03354395818706807, -0.1513378578672326, -0.29253451907325856, 0.3678134019814442]]"], + ["Vector.matrix_divide([[1, 0, 0, 10], [0, 1, 0, 20], [0, 0, 1, 30], [0, 0, 0, 1]], [[1, 0, 0, 100], [0, 1, 0, 200], [0, 0, 1, 300], [0, 0, 0, 1]])", "[[1, 0, 0, -90], [0, 1, 0, -180], [0, 0, 1, -270], [0, 0, 0, 1]]"], + ["Vector.matrix_dimensions([10, 20, 30])", "[1, 3]"], + ["Vector.matrix_dimensions([[10], [20], [30]])", "[3, 1]"], + ["Vector.properly_define_point([10, 20, 30])", "[[10], [20], [30], [1]]"], + ["Vector.properly_define_point([[10], [20], [30]])", "[[10], [20], [30], [1]]"], + ["Vector.properly_define_point([[10, 20, 30], [10, 20, 30], [10, 20, 30]])", "[[10, 10, 10], [20, 20, 20], [30, 30, 30], [1, 1, 1]]"], + ["Vector.make_pose()", "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]"], + ["Vector.make_pose([10, 20, 30], [Convert.degrees_to_arcseconds(45), 0, 0])", "[ [1, 0, 0, 10], [0, 0.7071067811865476, 0.7071067811865475, 20], [0, -0.7071067811865475, 0.7071067811865476, 30], [0, 0, 0, 1]]"], + ["Vector.identity_matrix(4)", "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]"], + ["Vector.identity_matrix(2)", "[[1, 0], [0, 1]]"], + ["Vector.rotate_DCM(Vector.identity_matrix(3), [1, 0, 0], Convert.degrees_to_arcseconds(90))", "[[1, 0, 0], [0, 6.123233995736766e-17, -1], [0, 1, 6.123233995736766e-17]]"], + ['Vector.rotate_pose(Vector.make_pose(), "Z", Convert.degrees_to_arcseconds(90), [10, 0, 0])', "[ [6.123233995736766e-17, -1, 0, 10], [1, 6.123233995736766e-17, 0, -10], [0, 0, 1, 0], [0, 0, 0, 1]]"], + ["Vector.is_pose(Vector.make_pose())", "true"], + ["Vector.pull([[1, 2, 3], [4, 5, 6], [7, 8, 9]], [1, 2], [1, 2])", "[[5, 6], [8, 9]]"] +) \ No newline at end of file