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