diff --git a/01_Code/physical_computing_interface/assembly/InverseKinematic.js b/01_Code/physical_computing_interface/assembly/InverseKinematic.js
index 77e099f57c21f61a4516b9954dc2418f4b06c56f..6a417ddade8d1abcd7b4042f88a7b2711f41387b 100644
--- a/01_Code/physical_computing_interface/assembly/InverseKinematic.js
+++ b/01_Code/physical_computing_interface/assembly/InverseKinematic.js
@@ -1,465 +1,452 @@
-// define((require, exports, module) => {
- var debug=false;
+
- const Serial = {
- println(text) {
- // // console.log(text)
- },
- }
+function InverseKinematic(geometry){
+ this.debug=false;
+ this.OK = 0
+ this.OUT_OF_RANGE = 1
+ this.OUT_OF_BOUNDS = 2
- const arrows = {}
+ this.V1_length_x_z = Math.sqrt(Math.pow(geometry[1][0], 2) + Math.pow(geometry[1][2], 2))
+ this.V4_length_x_y_z = Math.sqrt(Math.pow(geometry[4][0], 2) + Math.pow(geometry[4][2], 2) + Math.pow(-geometry[4][1], 2))
- function addArrow(name, from, to, color = 0xffff00, length = 10) {
- if (arrows.hasOwnProperty(name)) {
- window.debug.scene.remove(arrows[name])
- }
- if (!debug) return
- const toPoint = new THREE.Vector3(to[0], to[1], to[2])
- const origin = new THREE.Vector3(from[0], from[1], from[2])
- // length = length || toPoint.sub(origin).length()
- // toPoint.normalize()
- arrows[name] = new THREE.ArrowHelper(toPoint.sub(origin).normalize(), origin, length, color, 2, 1)
- window.debug.scene.add(arrows[name])
- }
- function addVectorArrow(name, from, vector, color, length) {
- addArrow(name, from, [from[0] + vector[0], from[1] + vector[1], from[2] + vector[2]], color, length)
+ this.J_initial_absolute = []
+ const tmpPos = [0, 0, 0]
+ for (let i = 0; i < geometry.length; i++) {
+ this.J_initial_absolute.push([tmpPos[0], tmpPos[1], tmpPos[2]])
+ tmpPos[0] += geometry[i][0]
+ tmpPos[1] += geometry[i][1]
+ tmpPos[2] += geometry[i][2]
}
- const spheres = {}
-
- function addSphere(name, position, color = 0xffff00, diameter = 1) {
- if (spheres.hasOwnProperty(name)) {
- window.debug.scene.remove(spheres[name])
- }
- // if (!debug) return// Amira changed
- if (!debug) return
-
- const geometry = new THREE.SphereGeometry(diameter, 32, 32)
- const material = new THREE.MeshBasicMaterial({
- color,
- })
-
- spheres[name] = new THREE.Mesh(geometry, material)
- spheres[name].position.set(position[0], position[1], position[2])
- window.debug.scene.add(spheres[name])
- }
-
- class InverseKinematic {
- constructor(geometry) {
- this.OK = 0
- this.OUT_OF_RANGE = 1
- this.OUT_OF_BOUNDS = 2
-
- this.V1_length_x_z = Math.sqrt(Math.pow(geometry[1][0], 2) + Math.pow(geometry[1][2], 2))
- this.V4_length_x_y_z = Math.sqrt(Math.pow(geometry[4][0], 2) + Math.pow(geometry[4][2], 2) + Math.pow(-geometry[4][1], 2))
-
-
- this.J_initial_absolute = []
- const tmpPos = [0, 0, 0]
- for (let i = 0; i < geometry.length; i++) {
- this.J_initial_absolute.push([tmpPos[0], tmpPos[1], tmpPos[2]])
- tmpPos[0] += geometry[i][0]
- tmpPos[1] += geometry[i][1]
- tmpPos[2] += geometry[i][2]
- }
-
- this.R_corrected = [0, 0, 0, 0, 0, 0]
-
- this.R_corrected[1] += Math.PI / 2
- this.R_corrected[1] -= Math.atan2(geometry[1][0], geometry[1][2]) // correct offset bone
-
- this.R_corrected[2] += Math.PI / 2
- this.R_corrected[2] += Math.atan2((geometry[2][2] + geometry[3][2]), (geometry[2][0] + geometry[3][0])) // correct offset bone V2,V3
- this.R_corrected[2] += Math.atan2(geometry[1][0], geometry[1][2]) // correct bone offset of V1
-
- this.R_corrected[4] += Math.PI
-
- this.geometry = geometry
- }
-
- calculateAngles(x, y, z, a, b, c, angles, config = [false, false, false]) {
- const cc = Math.cos(c)
- const sc = Math.sin(c)
- const cb = Math.cos(b)
- const sb = Math.sin(b)
- const ca = Math.cos(a)
- const sa = Math.sin(a)
-
- const targetVectorZ = [
- sb, -sa * cb,
- ca * cb,
- ]
-
- const R = [
- this.R_corrected[0],
- this.R_corrected[1],
- this.R_corrected[2],
- this.R_corrected[3],
- this.R_corrected[4],
- this.R_corrected[5],
- ]
-
- const J = [
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- ]
-
-
- // ---- J5 ----
-
- J[5][0] = x
- J[5][1] = y
- J[5][2] = z
- addSphere('J5', J[5])
+ this.R_corrected = [0, 0, 0, 0, 0, 0]
- // ---- J4 ----
- // vector
+ this.R_corrected[1] += Math.PI / 2
+ this.R_corrected[1] -= Math.atan2(geometry[1][0], geometry[1][2]) // correct offset bone
- J[4][0] = x - this.V4_length_x_y_z * targetVectorZ[0]
- J[4][1] = y - this.V4_length_x_y_z * targetVectorZ[1]
- J[4][2] = z - this.V4_length_x_y_z * targetVectorZ[2]
- addSphere('J4', J[4])
+ this.R_corrected[2] += Math.PI / 2
+ this.R_corrected[2] += Math.atan2((geometry[2][2] + geometry[3][2]), (geometry[2][0] + geometry[3][0])) // correct offset bone V2,V3
+ this.R_corrected[2] += Math.atan2(geometry[1][0], geometry[1][2]) // correct bone offset of V1
+ this.R_corrected[4] += Math.PI
- // ---- R0 ----
- // # J4
+ this.geometry = geometry
+ this.spheres = {}
+ this.arrows = {}
- const alphaR0 = Math.asin(this.J_initial_absolute[4][1] / this.length2(J[4][1], J[4][0]))
- R[0] += Math.atan2(J[4][1], J[4][0])
- R[0] += -alphaR0
+}
- if (config[0]) {
- R[0] += 2 * alphaR0 - Math.PI
- }
+InverseKinematic.prototype.calculateAngles=function (x, y, z, a, b, c, angles, config = [false, false, false]) {
+ const cc = Math.cos(c)
+ const sc = Math.sin(c)
+ const cb = Math.cos(b)
+ const sb = Math.sin(b)
+ const ca = Math.cos(a)
+ const sa = Math.sin(a)
- if (-this.J_initial_absolute[4][1] > this.length2(J[4][2], J[4][0])) {
- Serial.println('out of reach')
- }
+ const targetVectorZ = [
+ sb, -sa * cb,
+ ca * cb,
+ ]
+ const R = [
+ this.R_corrected[0],
+ this.R_corrected[1],
+ this.R_corrected[2],
+ this.R_corrected[3],
+ this.R_corrected[4],
+ this.R_corrected[5],
+ ]
- // ---- J1 ----
- // # R0
+ const J = [
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ ]
- J[1][0] = Math.cos(R[0]) * this.geometry[0][0] + Math.sin(R[0]) * -this.geometry[0][1]
- J[1][1] = Math.sin(R[0]) * this.geometry[0][0] + Math.cos(R[0]) * this.geometry[0][1]
- J[1][2] = this.geometry[0][2]
- addSphere('J1', J[1], 0x00ff00)
+ // ---- J5 ----
- // ---- rotate J4 into x,z plane ----
- // # J4 R0
+ J[5][0] = x
+ J[5][1] = y
+ J[5][2] = z
+ this.addSphere('J5', J[5])
- const J4_x_z = []
- J4_x_z[0] = Math.cos(R[0]) * J[4][0] + Math.sin(R[0]) * J[4][1]
- J4_x_z[1] = Math.sin(R[0]) * J[4][0] + Math.cos(R[0]) * -J[4][1] // 0
- J4_x_z[2] = J[4][2]
- addSphere('J4_x_z', J4_x_z, 0xff0000)
+ // ---- J4 ----
+ // vector
+ J[4][0] = x - this.V4_length_x_y_z * targetVectorZ[0]
+ J[4][1] = y - this.V4_length_x_y_z * targetVectorZ[1]
+ J[4][2] = z - this.V4_length_x_y_z * targetVectorZ[2]
+ this.addSphere('J4', J[4])
- // ---- J1J4_projected_length_square ----
- // # J4 R0
- const J1J4_projected_length_square = Math.pow(J4_x_z[0] - this.J_initial_absolute[1][0], 2) + Math.pow(J4_x_z[2] - this.J_initial_absolute[1][2], 2) // not using Math.sqrt
+ // ---- R0 ----
+ // # J4
+ const alphaR0 = Math.asin(this.J_initial_absolute[4][1] / this.length2(J[4][1], J[4][0]))
+ R[0] += Math.atan2(J[4][1], J[4][0])
+ R[0] += -alphaR0
- // ---- R2 ----
- // # J4 R0
-
- const J2J4_length_x_z = this.length2(this.geometry[2][0] + this.geometry[3][0], this.geometry[2][2] + this.geometry[3][2])
- R[2] += ((config[1] ? !config[0] : config[0]) ? 1.0 : -1.0) * Math.acos((-J1J4_projected_length_square + Math.pow(J2J4_length_x_z, 2) + Math.pow(this.V1_length_x_z, 2)) / (2.0 * (J2J4_length_x_z) * this.V1_length_x_z))
- R[2] -= 2 * Math.PI
-
- R[2] = ((R[2] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI // clamp -180/180 degree
-
-
- // ---- R1 ----
- // # J4 R0
-
- const J1J4_projected_length = Math.sqrt(J1J4_projected_length_square)
- R[1] -= Math.atan2((J4_x_z[2] - this.J_initial_absolute[1][2]), (J4_x_z[0] - this.J_initial_absolute[1][0])) // a''
- R[1] += ((config[1] ? !config[0] : config[0]) ? 1.0 : -1.0) * Math.acos((J1J4_projected_length_square - Math.pow(J2J4_length_x_z, 2) + Math.pow(this.V1_length_x_z, 2)) / (2.0 * J1J4_projected_length * this.V1_length_x_z)) // a
-
- R[1] = ((R[1] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+ if (config[0]) {
+ R[0] += 2 * alphaR0 - Math.PI
+ }
+ if (-this.J_initial_absolute[4][1] > this.length2(J[4][2], J[4][0])) {
+ Serial.println('out of reach')
+ }
- // ---- J2 ----
- // # R1 R0
- const ta = Math.cos(R[0])
- const tb = Math.sin(R[0])
- const tc = this.geometry[0][0]
- const d = this.geometry[0][2]
- const e = -this.geometry[0][1]
- const f = Math.cos(R[1])
- const g = Math.sin(R[1])
- const h = this.geometry[1][0]
- const i = this.geometry[1][2]
- const j = -this.geometry[1][1]
- const k = Math.cos(R[2])
- const l = Math.sin(R[2])
- const m = this.geometry[2][0]
- const n = this.geometry[2][2]
- const o = -this.geometry[2][1]
+ // ---- J1 ----
+ // # R0
- J[2][0] = ta * tc + tb * e + ta * f * h - ta * -g * i + tb * j
- J[2][1] = -(-tb * tc + ta * e - tb * f * h + tb * -g * i + ta * j)
- J[2][2] = d + -g * h + f * i
- addSphere('J2', J[2], 0x0000ff)
+ J[1][0] = Math.cos(R[0]) * this.geometry[0][0] + Math.sin(R[0]) * -this.geometry[0][1]
+ J[1][1] = Math.sin(R[0]) * this.geometry[0][0] + Math.cos(R[0]) * this.geometry[0][1]
+ J[1][2] = this.geometry[0][2]
+ this.addSphere('J1', J[1], 0x00ff00)
- J[3][0] = J[2][0] + ta * f * k * m - ta * -g * -l * m - ta * -g * k * n - ta * f * -l * n + tb * o
- J[3][1] = J[2][1] - (-tb * f * k * m + tb * -g * -l * m + tb * -g * k * n + tb * f * -l * n + ta * o)
- J[3][2] = J[2][2] + -g * k * m + f * -l * m + f * k * n + g * -l * n
- addSphere('J3', J[3], 0x0000ff)
+ // ---- rotate J4 into x,z plane ----
+ // # J4 R0
- // ---- J4J3 J4J5 ----
- // # J3 J4 J5
+ const J4_x_z = []
- const J4J5_vector = [J[5][0] - J[4][0], J[5][1] - J[4][1], J[5][2] - J[4][2]]
- const J4J3_vector = [J[3][0] - J[4][0], J[3][1] - J[4][1], J[3][2] - J[4][2]]
+ J4_x_z[0] = Math.cos(R[0]) * J[4][0] + Math.sin(R[0]) * J[4][1]
+ J4_x_z[1] = Math.sin(R[0]) * J[4][0] + Math.cos(R[0]) * -J[4][1] // 0
+ J4_x_z[2] = J[4][2]
+ this.addSphere('J4_x_z', J4_x_z, 0xff0000)
- // ---- R3 ----
- // # J3 J4 J5
+ // ---- J1J4_projected_length_square ----
+ // # J4 R0
- const J4J5_J4J3_normal_vector = this.cross(J4J5_vector, J4J3_vector)
+ const J1J4_projected_length_square = Math.pow(J4_x_z[0] - this.J_initial_absolute[1][0], 2) + Math.pow(J4_x_z[2] - this.J_initial_absolute[1][2], 2) // not using Math.sqrt
- addVectorArrow('normal J4', J[4], J4J5_J4J3_normal_vector, 0xbada55, 8)
- const ZY_parallel_aligned_vector = [
- 10 * -Math.sin(R[0]),
- 10 * Math.cos(R[0]),
- 0,
- ]
+ // ---- R2 ----
+ // # J4 R0
- const ZY_aligned_J4J3_normal_vector = this.cross(ZY_parallel_aligned_vector, J4J3_vector)
+ const J2J4_length_x_z = this.length2(this.geometry[2][0] + this.geometry[3][0], this.geometry[2][2] + this.geometry[3][2])
+ R[2] += ((config[1] ? !config[0] : config[0]) ? 1.0 : -1.0) * Math.acos((-J1J4_projected_length_square + Math.pow(J2J4_length_x_z, 2) + Math.pow(this.V1_length_x_z, 2)) / (2.0 * (J2J4_length_x_z) * this.V1_length_x_z))
+ R[2] -= 2 * Math.PI
- addVectorArrow('normal J4 Y_vectors', J[4], ZY_aligned_J4J3_normal_vector, 0xff00ff)
- addVectorArrow('XZ_parallel_aligned_vector', J[4], ZY_parallel_aligned_vector, 0x11ff11)
+ R[2] = ((R[2] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI // clamp -180/180 degree
- R[3] = this.angleBetween(J4J5_J4J3_normal_vector, ZY_parallel_aligned_vector, ZY_aligned_J4J3_normal_vector)
- if (config[2]) // rotate 180 and clamp -180,180
- {
- R[3] += Math.PI
- }
- R[3] = ((R[3] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+ // ---- R1 ----
+ // # J4 R0
+ const J1J4_projected_length = Math.sqrt(J1J4_projected_length_square)
+ R[1] -= Math.atan2((J4_x_z[2] - this.J_initial_absolute[1][2]), (J4_x_z[0] - this.J_initial_absolute[1][0])) // a''
+ R[1] += ((config[1] ? !config[0] : config[0]) ? 1.0 : -1.0) * Math.acos((J1J4_projected_length_square - Math.pow(J2J4_length_x_z, 2) + Math.pow(this.V1_length_x_z, 2)) / (2.0 * J1J4_projected_length * this.V1_length_x_z)) // a
- // ---- R4 ----
- // # J4 J3 J5 R3
+ R[1] = ((R[1] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
- R[4] += ((config[2]) ? 1 : -1) * this.angleBetween2(J4J5_vector, J4J3_vector)
- // clamp -180,180
- R[4] = ((R[4] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+ // ---- J2 ----
+ // # R1 R0
+ const ta = Math.cos(R[0])
+ const tb = Math.sin(R[0])
+ const tc = this.geometry[0][0]
+ const d = this.geometry[0][2]
+ const e = -this.geometry[0][1]
+ const f = Math.cos(R[1])
+ const g = Math.sin(R[1])
+ const h = this.geometry[1][0]
+ const i = this.geometry[1][2]
+ const j = -this.geometry[1][1]
+ const k = Math.cos(R[2])
+ const l = Math.sin(R[2])
+ const m = this.geometry[2][0]
+ const n = this.geometry[2][2]
+ const o = -this.geometry[2][1]
- // ---- R5 ----
- // # J4 J5 J3
+ J[2][0] = ta * tc + tb * e + ta * f * h - ta * -g * i + tb * j
+ J[2][1] = -(-tb * tc + ta * e - tb * f * h + tb * -g * i + ta * j)
+ J[2][2] = d + -g * h + f * i
+ this.addSphere('J2', J[2], 0x0000ff)
- const targetVectorY = [-cb * sc,
- ca * cc - sa * sb * sc,
- sa * cc + ca * sb * sc,
- ]
+ J[3][0] = J[2][0] + ta * f * k * m - ta * -g * -l * m - ta * -g * k * n - ta * f * -l * n + tb * o
+ J[3][1] = J[2][1] - (-tb * f * k * m + tb * -g * -l * m + tb * -g * k * n + tb * f * -l * n + ta * o)
+ J[3][2] = J[2][2] + -g * k * m + f * -l * m + f * k * n + g * -l * n
+ this.addSphere('J3', J[3], 0x0000ff)
- R[5] -= this.angleBetween(J4J5_J4J3_normal_vector, targetVectorY, this.cross(targetVectorZ, targetVectorY))
- if (config[2]) R[5] += Math.PI
+ // ---- J4J3 J4J5 ----
+ // # J3 J4 J5
- R[5] = ((R[5] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
- // ---- Error handling ----
+ const J4J5_vector = [J[5][0] - J[4][0], J[5][1] - J[4][1], J[5][2] - J[4][2]]
+ const J4J3_vector = [J[3][0] - J[4][0], J[3][1] - J[4][1], J[3][2] - J[4][2]]
- const error = false
- const outOfBounds = [false, false, false, false, false, false]
+ // ---- R3 ----
+ // # J3 J4 J5
- angles[0] = R[0]
- angles[1] = R[1]
- angles[2] = R[2]
- angles[3] = R[3]
- angles[4] = R[4]
- angles[5] = R[5]
- }
+ const J4J5_J4J3_normal_vector = this.cross(J4J5_vector, J4J3_vector)
- calculateTCP(R0, R1, R2, R3, R4, R5, jointsResult) {
- const joints = [
- [],
- [],
- [],
- [],
- [],
- [],
- ]
- this.calculateCoordinates(R0, R1, R2, R3, R4, R5, joints)
- jointsResult[0] = joints[5][0]
- jointsResult[1] = joints[5][1]
- jointsResult[2] = joints[5][2]
- jointsResult[3] = joints[5][3]
- jointsResult[4] = joints[5][4]
- jointsResult[5] = joints[5][5]
- }
+ this.addVectorArrow('normal J4', J[4], J4J5_J4J3_normal_vector, 0xbada55, 8)
- calculateCoordinates(R0, R1, R2, R3, R4, R5, jointsResult, config = [false, false, false]) {
- // todo detect config
-
- const a = Math.cos(R0)
- const b = Math.sin(R0)
- const c = this.geometry[0][0]
- const d = this.geometry[0][1]
- const e = this.geometry[0][2]
- const f = Math.cos(R1)
- const g = Math.sin(R1)
- const h = this.geometry[1][0]
- const i = this.geometry[1][1]
- const j = this.geometry[1][2]
- const k = Math.cos(R2)
- const l = Math.sin(R2)
- const m = this.geometry[2][0]
- const n = this.geometry[2][1]
- const o = this.geometry[2][2]
- const p = Math.cos(R3)
- const q = Math.sin(R3)
- const r = this.geometry[3][0]
- const s = this.geometry[3][1]
- const t = this.geometry[3][2]
- const u = Math.cos(R4)
- const v = Math.sin(R4)
- const w = this.geometry[4][0]
- const x = this.geometry[4][1]
- const y = this.geometry[4][2]
- const A = Math.cos(R5)
- const B = Math.sin(R5)
-
- jointsResult[0][0] = 0
- jointsResult[0][1] = 0
- jointsResult[0][2] = 0
-
- jointsResult[1][0] = jointsResult[0][0] + a * c - b * d
- jointsResult[1][1] = jointsResult[0][1] + b * c + a * d
- jointsResult[1][2] = jointsResult[0][2] + e
-
- jointsResult[2][0] = jointsResult[1][0] + a * f * h - b * i + a * g * j
- jointsResult[2][1] = jointsResult[1][1] + b * f * h + a * i + b * g * j
- jointsResult[2][2] = jointsResult[1][2] + -g * h + f * j
-
- jointsResult[3][0] = jointsResult[2][0] + a * f * k * m - a * g * l * m - b * n + a * g * k * o + a * f * l * o
- jointsResult[3][1] = jointsResult[2][1] + b * f * k * m - b * g * l * m + a * n + b * g * k * o + b * f * l * o
- jointsResult[3][2] = jointsResult[2][2] - g * k * m - f * l * m + f * k * o - g * l * o
-
- jointsResult[4][0] = jointsResult[3][0] + a * f * k * r - a * g * l * r - b * p * s + a * g * k * q * s + a * f * l * q * s + a * g * k * p * t + a * f * l * p * t + b * q * t
- jointsResult[4][1] = jointsResult[3][1] + b * f * k * r - b * g * l * r + a * p * s + b * g * k * q * s + b * f * l * q * s + b * g * k * p * t + b * f * l * p * t - a * q * t
- jointsResult[4][2] = jointsResult[3][2] - g * k * r - f * l * r + f * k * q * s - g * l * q * s + f * k * p * t - g * l * p * t
-
- jointsResult[5][0] = jointsResult[4][0] + a * f * k * u * w - a * g * l * u * w - a * g * k * p * v * w - a * f * l * p * v * w - b * q * v * w - b * p * x + a * g * k * q * x + a * f * l * q * x + a * g * k * p * u * y + a * f * l * p * u * y + b * q * u * y + a * f * k * v * y - a * g * l * v * y
- jointsResult[5][1] = jointsResult[4][1] + b * f * k * u * w - b * g * l * u * w - b * g * k * p * v * w - b * f * l * p * v * w + a * q * v * w + a * p * x + b * g * k * q * x + b * f * l * q * x + b * g * k * p * u * y + b * f * l * p * u * y - a * q * u * y + b * f * k * v * y - b * g * l * v * y
- jointsResult[5][2] = jointsResult[4][2] - g * k * u * w - f * l * u * w - f * k * p * v * w + g * l * p * v * w + f * k * q * x - g * l * q * x + f * k * p * u * y - g * l * p * u * y - g * k * v * y - f * l * v * y
-
- const M = [
- [-B * b * p + B * a * g * k * q + B * a * f * l * q - A * a * g * k * p * u - A * a * f * l * p * u - A * b * q * u - A * a * f * k * v + A * a * g * l * v, -A * b * p + A * a * g * k * q + A * a * f * l * q + B * a * g * k * p * u + B * a * f * l * p * u + B * b * q * u + B * a * f * k * v - B * a * g * l * v, a * f * k * u - a * g * l * u - a * g * k * p * v - a * f * l * p * v - b * q * v],
- [B * a * p + B * b * g * k * q + B * b * f * l * q - A * b * g * k * p * u - A * b * f * l * p * u + A * a * q * u - A * b * f * k * v + A * b * g * l * v, A * a * p + A * b * g * k * q + A * b * f * l * q + B * b * g * k * p * u + B * b * f * l * p * u - B * a * q * u + B * b * f * k * v - B * b * g * l * v, b * f * k * u - b * g * l * u - b * g * k * p * v - b * f * l * p * v + a * q * v],
- [B * f * k * q - B * g * l * q - A * f * k * p * u + A * g * l * p * u + A * g * k * v + A * f * l * v, A * f * k * q - A * g * l * q + B * f * k * p * u - B * g * l * p * u - B * g * k * v - B * f * l * v, -g * k * u - f * l * u - f * k * p * v + g * l * p * v],
- ]
-
- // https://www.geometrictools.com/Documentation/EulerAngles.pdf
- let thetaY,
- thetaX,
- thetaZ
- if (M[0][2] < 1) {
- if (M[0][2] > -1) {
- thetaY = Math.asin(M[0][2])
- thetaX = Math.atan2(-M[1][2], M[2][2])
- thetaZ = Math.atan2(-M[0][1], M[0][0])
- } else {
- thetaY = -Math.PI / 2
- thetaX = -Math.atan2(M[1][0], M[1][1])
- thetaZ = 0
- }
- } else {
- thetaY = +Math.PI / 2
- thetaX = Math.atan2(M[1][0], M[1][1])
- thetaZ = 0
- }
-
-
- jointsResult[5][3] = thetaX
- jointsResult[5][4] = thetaY
- jointsResult[5][5] = thetaZ
- }
+ const ZY_parallel_aligned_vector = [
+ 10 * -Math.sin(R[0]),
+ 10 * Math.cos(R[0]),
+ 0,
+ ]
- cross(vectorA, vectorB, result = []) {
- result[0] = vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1]
- result[1] = vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2]
- result[2] = vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0]
- return result
- }
+ const ZY_aligned_J4J3_normal_vector = this.cross(ZY_parallel_aligned_vector, J4J3_vector)
- dot(vectorA, vectorB) {
- return vectorA[0] * vectorB[0] + vectorA[1] * vectorB[1] + vectorA[2] * vectorB[2]
- }
+ this.addVectorArrow('normal J4 Y_vectors', J[4], ZY_aligned_J4J3_normal_vector, 0xff00ff)
+ this.addVectorArrow('XZ_parallel_aligned_vector', J[4], ZY_parallel_aligned_vector, 0x11ff11)
- /**
- * @param {array} vectorA angle from
- * @param {array} vectorB angle to
- * @param {array} referenceVector angle to set 0 degree from. coplanar with vecA and vecB
- * @return {number} description
- * @example angleBetween([1,0,0],[0,1,0],[0,0,1]) // PI/2
- */
- angleBetween(vectorA, vectorB, referenceVector) {
- // angle = atan2(norm(cross(a, b)), dot(a, b))
+ R[3] = this.angleBetween(J4J5_J4J3_normal_vector, ZY_parallel_aligned_vector, ZY_aligned_J4J3_normal_vector)
- const norm = this.length3(this.cross(vectorA, vectorB))
-
- const angle = Math.atan2(norm, (vectorB[0] * vectorA[0] + vectorB[1] * vectorA[1] + vectorB[2] * vectorA[2]))
-
- const tmp = referenceVector[0] * vectorA[0] + referenceVector[1] * vectorA[1] + referenceVector[2] * vectorA[2]
-
- const sign = (tmp > 0.0001) ? 1.0 : -1.0
-
- return angle * sign
- }
-
- length3(vector) {
- return Math.sqrt(Math.pow(vector[0], 2) + Math.pow(vector[1], 2) + Math.pow(vector[2], 2))
+ if (config[2]) // rotate 180 and clamp -180,180
+ {
+ R[3] += Math.PI
+ }
+ R[3] = ((R[3] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+
+
+ // ---- R4 ----
+ // # J4 J3 J5 R3
+
+ R[4] += ((config[2]) ? 1 : -1) * this.angleBetween2(J4J5_vector, J4J3_vector)
+
+ // clamp -180,180
+ R[4] = ((R[4] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+
+
+ // ---- R5 ----
+ // # J4 J5 J3
+
+ const targetVectorY = [-cb * sc,
+ ca * cc - sa * sb * sc,
+ sa * cc + ca * sb * sc,
+ ]
+
+ R[5] -= this.angleBetween(J4J5_J4J3_normal_vector, targetVectorY, this.cross(targetVectorZ, targetVectorY))
+
+ if (config[2]) R[5] += Math.PI
+
+ R[5] = ((R[5] + 3 * Math.PI) % (2 * Math.PI)) - Math.PI
+ // ---- Error handling ----
+
+ const error = false
+ const outOfBounds = [false, false, false, false, false, false]
+
+
+ angles[0] = R[0]
+ angles[1] = R[1]
+ angles[2] = R[2]
+ angles[3] = R[3]
+ angles[4] = R[4]
+ angles[5] = R[5]
+}
+
+InverseKinematic.prototype.calculateTCP=function(R0, R1, R2, R3, R4, R5, jointsResult) {
+ const joints = [
+ [],
+ [],
+ [],
+ [],
+ [],
+ [],
+ ]
+ this.calculateCoordinates(R0, R1, R2, R3, R4, R5, joints)
+ jointsResult[0] = joints[5][0]
+ jointsResult[1] = joints[5][1]
+ jointsResult[2] = joints[5][2]
+ jointsResult[3] = joints[5][3]
+ jointsResult[4] = joints[5][4]
+ jointsResult[5] = joints[5][5]
+}
+
+InverseKinematic.prototype.calculateCoordinates=function(R0, R1, R2, R3, R4, R5, jointsResult, config = [false, false, false]) {
+ // todo detect config
+
+ const a = Math.cos(R0)
+ const b = Math.sin(R0)
+ const c = this.geometry[0][0]
+ const d = this.geometry[0][1]
+ const e = this.geometry[0][2]
+ const f = Math.cos(R1)
+ const g = Math.sin(R1)
+ const h = this.geometry[1][0]
+ const i = this.geometry[1][1]
+ const j = this.geometry[1][2]
+ const k = Math.cos(R2)
+ const l = Math.sin(R2)
+ const m = this.geometry[2][0]
+ const n = this.geometry[2][1]
+ const o = this.geometry[2][2]
+ const p = Math.cos(R3)
+ const q = Math.sin(R3)
+ const r = this.geometry[3][0]
+ const s = this.geometry[3][1]
+ const t = this.geometry[3][2]
+ const u = Math.cos(R4)
+ const v = Math.sin(R4)
+ const w = this.geometry[4][0]
+ const x = this.geometry[4][1]
+ const y = this.geometry[4][2]
+ const A = Math.cos(R5)
+ const B = Math.sin(R5)
+
+ jointsResult[0][0] = 0
+ jointsResult[0][1] = 0
+ jointsResult[0][2] = 0
+
+ jointsResult[1][0] = jointsResult[0][0] + a * c - b * d
+ jointsResult[1][1] = jointsResult[0][1] + b * c + a * d
+ jointsResult[1][2] = jointsResult[0][2] + e
+
+ jointsResult[2][0] = jointsResult[1][0] + a * f * h - b * i + a * g * j
+ jointsResult[2][1] = jointsResult[1][1] + b * f * h + a * i + b * g * j
+ jointsResult[2][2] = jointsResult[1][2] + -g * h + f * j
+
+ jointsResult[3][0] = jointsResult[2][0] + a * f * k * m - a * g * l * m - b * n + a * g * k * o + a * f * l * o
+ jointsResult[3][1] = jointsResult[2][1] + b * f * k * m - b * g * l * m + a * n + b * g * k * o + b * f * l * o
+ jointsResult[3][2] = jointsResult[2][2] - g * k * m - f * l * m + f * k * o - g * l * o
+
+ jointsResult[4][0] = jointsResult[3][0] + a * f * k * r - a * g * l * r - b * p * s + a * g * k * q * s + a * f * l * q * s + a * g * k * p * t + a * f * l * p * t + b * q * t
+ jointsResult[4][1] = jointsResult[3][1] + b * f * k * r - b * g * l * r + a * p * s + b * g * k * q * s + b * f * l * q * s + b * g * k * p * t + b * f * l * p * t - a * q * t
+ jointsResult[4][2] = jointsResult[3][2] - g * k * r - f * l * r + f * k * q * s - g * l * q * s + f * k * p * t - g * l * p * t
+
+ jointsResult[5][0] = jointsResult[4][0] + a * f * k * u * w - a * g * l * u * w - a * g * k * p * v * w - a * f * l * p * v * w - b * q * v * w - b * p * x + a * g * k * q * x + a * f * l * q * x + a * g * k * p * u * y + a * f * l * p * u * y + b * q * u * y + a * f * k * v * y - a * g * l * v * y
+ jointsResult[5][1] = jointsResult[4][1] + b * f * k * u * w - b * g * l * u * w - b * g * k * p * v * w - b * f * l * p * v * w + a * q * v * w + a * p * x + b * g * k * q * x + b * f * l * q * x + b * g * k * p * u * y + b * f * l * p * u * y - a * q * u * y + b * f * k * v * y - b * g * l * v * y
+ jointsResult[5][2] = jointsResult[4][2] - g * k * u * w - f * l * u * w - f * k * p * v * w + g * l * p * v * w + f * k * q * x - g * l * q * x + f * k * p * u * y - g * l * p * u * y - g * k * v * y - f * l * v * y
+
+ const M = [
+ [-B * b * p + B * a * g * k * q + B * a * f * l * q - A * a * g * k * p * u - A * a * f * l * p * u - A * b * q * u - A * a * f * k * v + A * a * g * l * v, -A * b * p + A * a * g * k * q + A * a * f * l * q + B * a * g * k * p * u + B * a * f * l * p * u + B * b * q * u + B * a * f * k * v - B * a * g * l * v, a * f * k * u - a * g * l * u - a * g * k * p * v - a * f * l * p * v - b * q * v],
+ [B * a * p + B * b * g * k * q + B * b * f * l * q - A * b * g * k * p * u - A * b * f * l * p * u + A * a * q * u - A * b * f * k * v + A * b * g * l * v, A * a * p + A * b * g * k * q + A * b * f * l * q + B * b * g * k * p * u + B * b * f * l * p * u - B * a * q * u + B * b * f * k * v - B * b * g * l * v, b * f * k * u - b * g * l * u - b * g * k * p * v - b * f * l * p * v + a * q * v],
+ [B * f * k * q - B * g * l * q - A * f * k * p * u + A * g * l * p * u + A * g * k * v + A * f * l * v, A * f * k * q - A * g * l * q + B * f * k * p * u - B * g * l * p * u - B * g * k * v - B * f * l * v, -g * k * u - f * l * u - f * k * p * v + g * l * p * v],
+ ]
+
+ // https://www.geometrictools.com/Documentation/EulerAngles.pdf
+ let thetaY,
+ thetaX,
+ thetaZ
+ if (M[0][2] < 1) {
+ if (M[0][2] > -1) {
+ thetaY = Math.asin(M[0][2])
+ thetaX = Math.atan2(-M[1][2], M[2][2])
+ thetaZ = Math.atan2(-M[0][1], M[0][0])
+ } else {
+ thetaY = -Math.PI / 2
+ thetaX = -Math.atan2(M[1][0], M[1][1])
+ thetaZ = 0
}
+ } else {
+ thetaY = +Math.PI / 2
+ thetaX = Math.atan2(M[1][0], M[1][1])
+ thetaZ = 0
+ }
- length2(a, b) {
- return Math.sqrt(Math.pow(a, 2) + Math.pow(b, 2))
- }
- angleBetween2(v1, v2) {
- let angle
- // turn vectors into unit vectors
- // this.normalize(v1,v1)
- // this.normalize(v2,v2)
- //
- // var angle = Math.acos(this.dot(v1, v2))
- // // if no noticable rotation is available return zero rotation
- // // this way we avoid Cross product artifacts
- // if (Math.abs(angle) < 0.0001) return 0
- // // in this case there are 2 lines on the same axis
- // // // angle = atan2(norm(cross(a, b)), dot(a, b))
- const cross = this.cross(v1, v2)
-
- return Math.atan2(this.length3(cross), this.dot(v1, v2))
- }
- normalize(vector, result) {
- const length = Math.sqrt((vector[0] * vector[0]) + (vector[1] * vector[1]) + (vector[2] * vector[2]))
- result[0] = vector[0] / length
- result[1] = vector[1] / length
- result[2] = vector[2] / length
- }
+ jointsResult[5][3] = thetaX
+ jointsResult[5][4] = thetaY
+ jointsResult[5][5] = thetaZ
+}
+
+InverseKinematic.prototype.cross=function(vectorA, vectorB, result = []) {
+ result[0] = vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1]
+ result[1] = vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2]
+ result[2] = vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0]
+ return result
+}
+
+InverseKinematic.prototype.dot=function(vectorA, vectorB) {
+ return vectorA[0] * vectorB[0] + vectorA[1] * vectorB[1] + vectorA[2] * vectorB[2]
+}
+
+/**
+ * @param {array} vectorA angle from
+ * @param {array} vectorB angle to
+ * @param {array} referenceVector angle to set 0 degree from. coplanar with vecA and vecB
+ * @return {number} description
+ * @example angleBetween([1,0,0],[0,1,0],[0,0,1]) // PI/2
+ */
+InverseKinematic.prototype.angleBetween=function(vectorA, vectorB, referenceVector) {
+ // angle = atan2(norm(cross(a, b)), dot(a, b))
+
+ const norm = this.length3(this.cross(vectorA, vectorB))
+
+ const angle = Math.atan2(norm, (vectorB[0] * vectorA[0] + vectorB[1] * vectorA[1] + vectorB[2] * vectorA[2]))
+
+ const tmp = referenceVector[0] * vectorA[0] + referenceVector[1] * vectorA[1] + referenceVector[2] * vectorA[2]
+
+ const sign = (tmp > 0.0001) ? 1.0 : -1.0
+
+ return angle * sign
+}
+
+InverseKinematic.prototype.length3=function(vector) {
+ return Math.sqrt(Math.pow(vector[0], 2) + Math.pow(vector[1], 2) + Math.pow(vector[2], 2))
+}
+
+InverseKinematic.prototype.length2=function(a, b) {
+ return Math.sqrt(Math.pow(a, 2) + Math.pow(b, 2))
+}
+
+InverseKinematic.prototype.angleBetween2=function(v1, v2) {
+ let angle
+ // turn vectors into unit vectors
+ // this.normalize(v1,v1)
+ // this.normalize(v2,v2)
+ //
+ // var angle = Math.acos(this.dot(v1, v2))
+ // // if no noticable rotation is available return zero rotation
+ // // this way we avoid Cross product artifacts
+ // if (Math.abs(angle) < 0.0001) return 0
+ // // in this case there are 2 lines on the same axis
+ // // // angle = atan2(norm(cross(a, b)), dot(a, b))
+ const cross = this.cross(v1, v2)
+
+ return Math.atan2(this.length3(cross), this.dot(v1, v2))
+}
+
+InverseKinematic.prototype.normalize=function(vector, result) {
+ const length = Math.sqrt((vector[0] * vector[0]) + (vector[1] * vector[1]) + (vector[2] * vector[2]))
+ result[0] = vector[0] / length
+ result[1] = vector[1] / length
+ result[2] = vector[2] / length
+}
+
+InverseKinematic.prototype.addSphere=function(name, position, color = 0xffff00, diameter = 1) {
+ if (this.spheres.hasOwnProperty(name)) {
+ window.debug.scene.remove(this.spheres[name])
}
-
- function kinematic() {
- return InverseKinematic
+ // if (!debug) return// Amira changed
+ if (!this.debug) return
+
+ const geometry = new THREE.SphereGeometry(diameter, 32, 32)
+ const material = new THREE.MeshBasicMaterial({
+ color,
+ })
+
+ this.spheres[name] = new THREE.Mesh(geometry, material)
+ this.spheres[name].position.set(position[0], position[1], position[2])
+ window.debug.scene.add(this.spheres[name])
+}
+
+InverseKinematic.prototype.addArrow=function(name, from, to, color = 0xffff00, length = 10) {
+ if (this.arrows.hasOwnProperty(name)) {
+ window.debug.scene.remove(this.arrows[name])
}
+ if (!this.debug) return
+ const toPoint = new THREE.Vector3(to[0], to[1], to[2])
+ const origin = new THREE.Vector3(from[0], from[1], from[2])
+ // length = length || toPoint.sub(origin).length()
+ // toPoint.normalize()
+ this.arrows[name] = new THREE.ArrowHelper(toPoint.sub(origin).normalize(), origin, length, color, 2, 1)
+ window.debug.scene.add(this.arrows[name])
+}
+
+InverseKinematic.prototype.addVectorArrow=function(name, from, vector, color, length) {
+ this.addArrow(name, from, [from[0] + vector[0], from[1] + vector[1], from[2] + vector[2]], color, length)
+}
-// module.exports = InverseKinematic
-// })
diff --git a/01_Code/physical_computing_interface/assembly/main.js b/01_Code/physical_computing_interface/assembly/assembly.js
similarity index 100%
rename from 01_Code/physical_computing_interface/assembly/main.js
rename to 01_Code/physical_computing_interface/assembly/assembly.js
diff --git a/01_Code/physical_computing_interface/assembly/index.html b/01_Code/physical_computing_interface/assembly/index.html
index fedf4cec6f1ddb15eb5ff91b317cfaf3872e8d22..1fdfe7c4b6a6e3db8bee9fffce54cc17bb1881ea 100644
--- a/01_Code/physical_computing_interface/assembly/index.html
+++ b/01_Code/physical_computing_interface/assembly/index.html
@@ -18,7 +18,7 @@
<script src="./InverseKinematic.js"></script>
<script src="./voxel.js"></script>
- <script src="./main.js"></script>
+ <script src="./standAloneAssembly.js"></script>
diff --git a/01_Code/physical_computing_interface/assembly/standAloneAssembly.js b/01_Code/physical_computing_interface/assembly/standAloneAssembly.js
new file mode 100644
index 0000000000000000000000000000000000000000..d9be7bb364d95e46feb2547fba9b38897a01ba96
--- /dev/null
+++ b/01_Code/physical_computing_interface/assembly/standAloneAssembly.js
@@ -0,0 +1,2231 @@
+// Amira Abdel-Rahman
+// (c) Massachusetts Institute of Technology 2018
+
+///////////////////////////globals///////////////////////////////
+
+
+///////////////////////////THREE///////////////////////////////
+var container, camera, scene, renderer;
+
+var guiControls =[];
+var geometryGui,targetGui,jointsGui, jointsParams, targetParams, geometryParams,end, currentStep;
+
+var DEG_TO_RAD = Math.PI / 180;
+var RAD_TO_DEG = 180 / Math.PI;
+
+///////////////////////////voxels///////////////////////////////
+var voxel;
+var voxelSpacing=1.0;
+var voxelLocations=[];
+var voxelSlices=[];
+var voxelSlicesCount=[];
+
+var currentVoxelCount=0;
+var build=[];
+
+var globalZ=0;
+var globalRank=0;
+
+var voxelNum=0;
+var gridSize=6+4;//change 10 to changecube size
+
+var stepsCap=100; //change with very big simulations
+
+var grid=[];
+
+
+///////////////////////////robot///////////////////////////////
+var numberOfRobots=1; //change number of robots
+var speed=20; //change to speed up the simulation
+var THREE1=[];
+var robotBones = [];
+var joints = [];
+var angles = [];
+var IK=[];
+
+var robotState=[];
+
+var THREERobot;
+var VisualRobot= [];
+var THREESimulationRobot= [];
+var geo=[];
+var defaultRobotState= [];
+var target = [];
+var control= [];
+var leg= [];
+
+var voxelNormal= [];
+var normalAdjustmentVector= [];
+
+var startLocations= [];
+
+var THREE1dir= [];
+var THREE1Pos= [];
+
+//path
+var targetPositionMesh= [];
+var carriedVoxel= [];
+
+var goHome= [];
+
+//////////////////////
+
+var steps=[];
+var path= [];
+
+var totalNumberofSteps=[];
+
+var locations=[]
+
+
+///////////////////////function calls///////////////////////////////
+declareGlobals();
+init();
+// frepVoxel(10,10,10,"");//later change string for frep
+
+
+for( i=0;i<numberOfRobots;i++)
+{
+ setupGUI(i);
+ THREERobotCall(i);
+ defaultRobot(i);
+ buildHelperMeshes(i);
+ assemble(i);
+
+}
+
+///////////////////////////////scene///////////////////////////////
+function declareGlobals(){
+ for( i=0;i<numberOfRobots;i++)
+ {
+ guiControls.push(null);
+
+ THREE1.push(new THREE.Vector3(0,0,0) );
+ robotBones.push([]);
+ joints.push([]);
+ angles.push([0, 0, 0, 0, 0, 0]);
+ robotState.push({
+ leg1Pos:new THREE.Vector3(1,0,0),
+ leg2Pos:new THREE.Vector3(0,0,0),
+ up:new THREE.Vector3(0,0,1),
+ forward:new THREE.Vector3(1,0,0),
+ Z:0,
+ rank:0
+ });
+
+ // THREERobot.push(null);
+ VisualRobot.push(null);
+ THREESimulationRobot.push(null);
+ geo.push(null);
+ defaultRobotState.push(null);
+ target.push(null);
+ control.push(null);
+ leg.push(1);
+ voxelNormal.push(180);
+ normalAdjustmentVector.push(new THREE.Vector3( 0, 0, 0));
+ startLocations.push(new THREE.Vector3(0*voxelSpacing,10*voxelSpacing,0) );
+ THREE1dir.push(new THREE.Vector3(1,0,0) );
+ THREE1Pos.push(new THREE.Vector3(0,0,0) );
+
+ //path
+ targetPositionMesh.push(startLocations[i]);
+ carriedVoxel.push(null);
+
+ goHome.push(false);
+ totalNumberofSteps.push(0);
+
+ //////////////////////
+
+ steps.push([]);
+ path.push({
+ curve: null,
+ currentPoint: 0,
+ points:[],
+ number:20,
+ delay:1000/speed,
+ timeout:0,
+ cHeight:5.0*voxelSpacing,
+ showPath:true,
+ normals:[],
+ changeLegs:[],
+ changeRotation:[],
+ normalAdjustments:[]
+ });
+ IK.push([]);
+
+ }
+ //starting positions and pickup stations
+ if(numberOfRobots>1){
+ robotState[1].leg1Pos=new THREE.Vector3((gridSize-1),(gridSize-2),0);
+ robotState[1].leg2Pos=new THREE.Vector3((gridSize-1),(gridSize-1),0);
+ startLocations[1]=new THREE.Vector3((gridSize-1)*voxelSpacing,(gridSize-1)*voxelSpacing,0*voxelSpacing);
+
+ }
+ if(numberOfRobots>2){
+ robotState[2].leg1Pos=new THREE.Vector3((gridSize-1),1,0);
+ robotState[2].leg2Pos=new THREE.Vector3((gridSize-1),0,0);
+ startLocations[2]=new THREE.Vector3((gridSize-1)*voxelSpacing,0*voxelSpacing,0*voxelSpacing);
+
+ }
+ if(numberOfRobots>3){
+ robotState[3].leg1Pos=new THREE.Vector3(1,gridSize-1,0);
+ robotState[3].leg2Pos=new THREE.Vector3(0,gridSize-1,0);
+ startLocations[3]=new THREE.Vector3(0*voxelSpacing,(gridSize-1)*voxelSpacing,0*voxelSpacing);
+ }
+
+}
+
+function init() {
+ container = document.getElementById( 'webgl' );
+
+ renderer = new THREE.WebGLRenderer({
+ antialias: true, // to get smoother output
+ preserveDrawingBuffer: false, // no screenshot -> faster?
+ });
+ renderer.setClearColor(0x333333);
+
+ renderer.setSize(window.innerWidth, window.innerHeight)
+ container.appendChild( renderer.domElement );
+
+ scene = new THREE.Scene();
+
+ camera = new THREE.PerspectiveCamera(35, window.innerWidth / window.innerHeight, 1, 10000)
+
+ camera.up.set(0, 0, 0.5);
+ camera.position.set(75, 75, 75);
+ scene.add(camera);
+
+ // lights
+ var light = new THREE.AmbientLight(0xaaaaaa);
+ scene.add(light);
+ var light2 = new THREE.DirectionalLight(0xaaaaaa);
+ light2.position.set(1, 1.3, 1).normalize();
+ scene.add(light2);
+
+ cameraControls = new THREE.OrbitControls(camera, renderer.domElement);
+ cameraControls.addEventListener('change', () => renderer.render(scene, camera));
+
+ function onWindowResize()
+ {
+ camera.aspect = window.innerWidth / window.innerHeight;
+ camera.updateProjectionMatrix();
+
+ renderer.setSize(window.innerWidth, window.innerHeight);
+ renderer.render(scene, camera);
+ }
+
+ window.addEventListener('resize', onWindowResize, false);
+
+ var size = 20;
+ var step = 20;
+
+ var gridHelper = new THREE.GridHelper(size, step);
+ gridHelper.rotation.x = Math.PI / 2;
+ scene.add(gridHelper);
+
+ var axisHelper = new THREE.AxesHelper(5);
+ var colors = axisHelper.geometry.attributes.color.array;
+
+ colors.set( [
+ 0, 1, 0, 0, 1, 0, // x-axis rgb at origin; rgb at end of segment
+ 1, 0, 0, 1, 0, 0, // y-axis
+ 0, 0, 1, 0, 0, 1 // z-axis
+ ] );
+
+ scene.add(axisHelper);
+ animate();
+
+ ////////////////////load voxel/////////////////////////
+ loadVoxel();
+}
+
+function animate() {
+ requestAnimationFrame( animate );
+ render();
+}
+
+function render() {
+ renderer.render(scene, camera);
+
+}
+
+///////////////////////////geometry///////////////////////////////
+//implemented based on https://github.com/glumb/robot-gui
+function THREERobotCall(robotIndex) {
+ THREERobot= function (robotIndex,V_initial, limits, scene) {
+
+ THREE1[robotIndex] = new THREE.Group();
+
+ var colors = [
+ 0xaaaaaa,
+ 0xbbbbbb,
+ 0xbcbcbc,
+ 0xcbcbcb,
+ 0xcccccc,
+ 0x0,
+ ];
+
+ let parentObject = THREE1[robotIndex];
+ robotBones[robotIndex] = [];
+ joints[robotIndex] = [];
+
+ function createCube(x, y, z, w, h, d, min, max, jointNumber) {
+ var thicken = 1;
+
+ var w_thickened = Math.abs(w) + thicken;
+ var h_thickened = Math.abs(h) + thicken;
+ var d_thickened = Math.abs(d) + thicken;
+
+ var material = new THREE.MeshLambertMaterial({ color: colors[jointNumber],});
+ var geometry = new THREE.CubeGeometry(w_thickened, h_thickened, d_thickened);
+ var mesh = new THREE.Mesh(geometry, material);
+
+
+
+ mesh.position.set(w / 2, h / 2, d / 2);
+ var group = new THREE.Object3D();
+ group.position.set(x, y, z);
+ group.add(mesh);
+
+ // min = min / 180 * Math.PI
+ // max = max / 180 * Math.PI
+
+ var jointGeo1 = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, -min, 2 * Math.PI - max + min)
+ var jointGeoMax = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, -max, max)
+ var jointGeoMin = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, 0, -min)
+ var jointMesh1 = new THREE.Mesh(jointGeo1, new THREE.MeshBasicMaterial({
+ color: 0x000000,//0xffbb00,
+ }));
+ var jointMeshMax = new THREE.Mesh(jointGeoMax, new THREE.MeshBasicMaterial({
+ color: 0x000000,//0x009900,
+ }));
+ var jointMeshMin = new THREE.Mesh(jointGeoMin, new THREE.MeshBasicMaterial({
+ color: 0x000000,//0xdd2200,
+ }));
+
+ var joint = new THREE.Group();
+ joint.add(jointMeshMax, jointMeshMin, jointMesh1);
+
+ joints[robotIndex].push(joint);
+
+ switch (jointNumber) {
+ // case 0:
+ // joint.rotation.x = Math.PI / 2
+ // break
+ // case 1:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI / 2 //changed
+ // break
+ // case 2:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI / 2 //changed
+ // // joint.rotation.x = Math.PI / 2
+ // break
+ // case 3:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI
+ // break
+ // case 4:
+ // joint.rotation.x = Math.PI / 2 //changed
+ // // joint.rotation.x = Math.PI / 2
+ // // joint.rotation.y = Math.PI / 2
+ // break
+ // case 5:
+ // joint.rotation.x = Math.PI / 2
+ // group.rotation.y = Math.PI / 2
+ // break;
+ case 0:
+ joint.rotation.x = Math.PI / 2
+ break
+ case 1:
+ // joint.rotation.x = Math.PI / 2
+ break
+ case 2:
+ // joint.rotation.x = Math.PI / 2
+ break
+ case 3:
+ joint.rotation.z = Math.PI / 2
+ // joint.rotation.y = Math.PI
+ break
+ case 4:
+ // joint.rotation.x = Math.PI / 2
+ joint.rotation.y = Math.PI / 2
+ break
+ case 5:
+ joint.rotation.x = Math.PI / 2
+ group.rotation.y = Math.PI / 2
+ break;
+ }
+
+ group.add(joint);
+ return group;
+ }
+
+ let x = 0,
+ y = 0,
+ z = 0;
+
+ V_initial.push([0, 0, 0]) // add a 6th pseudo link for 6 axis
+
+ for (let i = 0; i < V_initial.length; i++) {
+ var link = V_initial[i];
+
+
+
+ var linkGeo = createCube(x, y, z, link[0], link[1], link[2], limits[i][0], limits[i][1], i);
+ x = link[0];
+ y = link[1];
+ z = link[2];
+
+ if(i==4)//todo change later
+ {
+
+ carriedVoxel[robotIndex]=voxel.clone();
+ carriedVoxel[robotIndex].position.x=x+2.5*voxelSpacing;
+ carriedVoxel[robotIndex].position.y=y;
+ carriedVoxel[robotIndex].position.z=z ;
+ linkGeo.add( carriedVoxel[robotIndex] );
+ }
+
+ parentObject.add(linkGeo);
+ parentObject = linkGeo;
+ robotBones[robotIndex].push(linkGeo);
+ }
+
+ scene.add(THREE1[robotIndex]);
+ };
+
+ THREERobot.prototype = {
+ setAngles(robotIndex,angles1) {
+ // robotBones[robotIndex][0].rotation.z = angles[robotIndex][0];
+ // robotBones[robotIndex][1].rotation.y = angles[robotIndex][1];
+ // robotBones[robotIndex][2].rotation.y = angles[robotIndex][2];
+ // robotBones[robotIndex][3].rotation.y = angles[robotIndex][3];
+ // robotBones[robotIndex][4].rotation.z = angles[robotIndex][4];
+ // robotBones[robotIndex][5].rotation.y = angles[robotIndex][5];
+
+ robotBones[robotIndex][0].rotation.z = angles[robotIndex][0];
+ robotBones[robotIndex][1].rotation.y = angles[robotIndex][1];
+ robotBones[robotIndex][2].rotation.y = angles[robotIndex][2];
+ robotBones[robotIndex][3].rotation.x = angles[robotIndex][3];
+ robotBones[robotIndex][4].rotation.y = angles[robotIndex][4];
+ robotBones[robotIndex][5].rotation.z = angles[robotIndex][5];
+
+ // robotBones[robotIndex][0].rotation.z =angles[robotIndex][0];
+ // robotBones[robotIndex][1].rotation.x =angles[robotIndex][1];
+ // robotBones[robotIndex][2].rotation.x =angles[robotIndex][2];
+ // robotBones[robotIndex][3].rotation.x =angles[robotIndex][3];
+ // robotBones[robotIndex][4].rotation.z =angles[robotIndex][4];
+ // robotBones[robotIndex][5].rotation.z =angles[robotIndex][5];
+ },
+
+ setAngle(robotIndex,index, angle) {
+ angles[robotIndex][index] = angle;
+ this.setAngles(robotIndex,angles[robotIndex].slice());
+ },
+ };
+}
+
+///////////////////////initialization/////////////////////////////
+function defaultRobot(robotIndex) {
+ localState = {
+ jointOutOfBound: [false, false, false, false, false, false],
+ };
+ var maxAngleVelocity = 90.0 / (180.0 * Math.PI) / 1000.0;
+
+ d1 = 0.1273;
+ a2 = -0.612;
+ a3 = -0.5723;
+ d4 = 0.163941;
+ d5 = 0.1157;
+ d6 = 0.0922;
+
+ shoulder_offset = 0.220941;
+ elbow_offset = -0.1719;
+
+ var scale=0.01;
+ shoulder_height =scale* d1;
+ upper_arm_length =scale* -a2;
+ forearm_length =scale* -a3;
+ wrist_1_length =scale* d4 - elbow_offset - shoulder_offset;
+ wrist_2_length =scale* d5;
+ wrist_3_length =scale* d6;
+ geo = [
+ // [scale*200, 0, scale*50],
+ // [-scale*100, 0, scale*600],
+ // [0, 0, scale*600],
+ // [scale*200, 0,0],
+ // [0, 0, scale*200],
+ // [scale*200, 0, 0],
+ [scale*200, 0, scale*100],
+ [0, 0, scale*800],
+ [0, 0,scale*200],
+ [scale*800, 0, 0],
+ [scale*200, 0, 0],
+ [scale*200, 0, 0],
+
+ ];
+ defaultRobotState[robotIndex] = {
+ target: {
+ position: {
+ x: startLocations[robotIndex].x,
+ y: startLocations[robotIndex].y,
+ z: startLocations[robotIndex].z,
+ },
+ rotation: {
+ x: Math.PI,
+ y: 0,
+ z: 0,
+ },
+ },
+ angles: {
+ A0: 0,
+ A1: 0,
+ A2: 0,
+ A3: 0,
+ A4: 0,
+ A5: 0,
+ },
+ jointOutOfBound: [false, false, false, false, false, false],
+ maxAngleVelocities: {
+ J0: maxAngleVelocity,
+ J1: maxAngleVelocity,
+ J2: maxAngleVelocity,
+ J3: maxAngleVelocity,
+ J4: maxAngleVelocity,
+ J5: maxAngleVelocity,
+ },
+ jointLimits: {
+ J0: [-190 / 180 * Math.PI, 190 / 180 * Math.PI],
+ J1: [-90 / 180 * Math.PI, 90 / 180 * Math.PI],
+ J2: [-135 / 180 * Math.PI, 45 / 180 * Math.PI],
+ J3: [-90 / 180 * Math.PI, 75 / 180 * Math.PI],
+ J4: [-139 / 180 * Math.PI, 90 / 180 * Math.PI],
+ J5: [-188 / 180 * Math.PI, 181 / 180 * Math.PI],
+ },
+ configuration: [false, false, false],
+ geometry: {
+ V0: {
+ x: geo[0][0],
+ y: geo[0][1],
+ z: geo[0][2],
+ },
+ V1: {
+ x: geo[1][0],
+ y: geo[1][1],
+ z: geo[1][2],
+ },
+ V2: {
+ x: geo[2][0],
+ y: geo[2][1],
+ z: geo[2][2],
+ },
+ V3: {
+ x: geo[3][0],
+ y: geo[3][1],
+ z: geo[3][2],
+ },
+ V4: {
+ x: geo[4][0],
+ y: geo[4][1],
+ z: geo[4][2],
+ },
+ },
+ };
+
+ THREESimulationRobot[robotIndex] = new THREE.Group();
+ scene.add(THREESimulationRobot[robotIndex]);
+
+ ////////////////
+
+ //////////////////
+
+ updateIK(robotIndex);
+
+ console.log(defaultRobotState[robotIndex].angles);
+
+ var geometry = Object.values(defaultRobotState[robotIndex].geometry).map((val, i, array) => [val.x, val.y, val.z]);
+ var jointLimits = Object.values(defaultRobotState[robotIndex].jointLimits);
+
+ VisualRobot[robotIndex] = new THREERobot(robotIndex,geometry, jointLimits, THREESimulationRobot[robotIndex]);
+ // var angles = Object.values(defaultRobotState.angles);
+ // VisualRobot[robotIndex].setAngles(angles);
+ var restAngle=60;
+ // VisualRobot[robotIndex].setAngle(0,restAngle/ 180 * Math.PI);
+ VisualRobot[robotIndex].setAngle(robotIndex,1,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,2,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,3,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,4,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,5,restAngle*RAD_TO_DEG);
+
+ // var target= new THREE.Vector3(guiControls[robotIndex].x,guiControls[robotIndex].y,guiControls[robotIndex].z);
+ // updateGUI();
+
+ targetControl(robotIndex);
+ THREE1[robotIndex].position.x=0;
+ THREE1[robotIndex].position.y=0;
+ THREE1[robotIndex].position.z=0;
+ updateAngles(robotIndex);
+
+}
+
+function updateRobotGeometry1(robotIndex){
+ geo = [
+ [2.5 + 2.3, 0, 7.3],
+ [0, 0, 13.0],
+ [1, 0, 2],
+ [12.6, 0, 0],
+ [3.6, 0, 0],
+ [0, 0, 0],
+ ];
+
+ defaultRobotState[robotIndex].geometry ={
+ V0: {
+ x: geo[0][0],
+ y: geo[0][1],
+ z: geo[0][2],
+ },
+ V1: {
+ x: geo[1][0],
+ y: geo[1][1],
+ z: geo[1][2],
+ },
+ V2: {
+ x: geo[2][0],
+ y: geo[2][1],
+ z: geo[2][2],
+ },
+ V3: {
+ x: geo[3][0],
+ y: geo[3][1],
+ z: geo[3][2],
+ },
+ V4: {
+ x: geo[4][0],
+ y: geo[4][1],
+ z: geo[4][2],
+ },
+ };
+
+ updateIK(robotIndex);
+
+ var geometry = Object.values(defaultRobotState[robotIndex].geometry).map((val, i, array) => [val.x, val.y, val.z]);
+ var jointLimits = Object.values(defaultRobotState[robotIndex].jointLimits);
+
+ while (THREESimulationRobot[robotIndex].children.length) {
+ THREESimulationRobot[robotIndex].remove(THREESimulationRobot[robotIndex].children[0]);
+ }
+
+ VisualRobot[robotIndex] = new THREERobot(robotIndex,geometry, jointLimits, THREESimulationRobot[robotIndex]);
+
+ updateAngles(robotIndex);
+}
+
+
+////////////////////////////////GUI///////////////////////////////
+function setupGUI(robotIndex){
+ //Parameters that can be modified.
+ guiControls[robotIndex] = new function() {
+ this.x = startLocations[robotIndex].x;
+ this.y = startLocations[robotIndex].y;
+ this.z = startLocations[robotIndex].z;
+ this.j1 = 0.0;
+ this.j2 = 0.0;
+ this.j3 = 0.0;
+ this.j4 = 0.0;
+ this.j5 = 0.0;
+ this.j6 = 0.0;
+ this.leg1 = 5.0;
+ this.leg2 = 5.0;
+ this.offset = 2.0;
+ this.targetEnd="end 1";
+ this.step=0;
+ };
+
+ var gui = new dat.GUI();
+
+ geometryGui = gui.addFolder('Robot Geometry');
+ geometryParams=[];
+ geometryParams.push(geometryGui.add(guiControls[robotIndex], 'leg1', 1.00, 10.0).step(0.1).listen());
+ geometryParams.push(geometryGui.add(guiControls[robotIndex], 'leg2', 1.00, 10.0).step(0.1).listen());
+ geometryParams.push(geometryGui.add(guiControls[robotIndex], 'offset', 1.00, 10.0).step(0.1).listen());
+
+ jointsGui = gui.addFolder('Robot Joints');
+ jointsParams=[];
+ jointsParams.push(jointsGui.add(guiControls[robotIndex], 'j1', 0.00, 360.0).step(0.5).listen());
+ jointsParams.push(jointsGui.add(guiControls[robotIndex], 'j2', 0.00, 360.0).step(0.5).listen());
+ jointsParams.push(jointsGui.add(guiControls[robotIndex], 'j3', 0.00, 360.0).step(0.5).listen());
+ jointsParams.push(jointsGui.add(guiControls[robotIndex], 'j4', 0.00, 360.0).step(0.5).listen());
+ jointsParams.push(jointsGui.add(guiControls[robotIndex], 'j5', 0.00, 360.0).step(0.5).listen());
+
+ targetGui = gui.addFolder('Target');
+ targetParams=[];
+ targetParams.push(targetGui.add(guiControls[robotIndex], 'x', -10.0, 10.0).step(0.1).listen());
+ targetParams.push(targetGui.add(guiControls[robotIndex], 'y', -10.0, 10.0).step(0.1).listen());
+ targetParams.push(targetGui.add(guiControls[robotIndex], 'z', -10.0, 10.0).step(0.1).listen());
+
+ var endtarget= gui.addFolder('Target End');
+ end=endtarget.add(guiControls[robotIndex],'targetEnd',["end 1","end 2"]).listen();
+
+ currentStep=gui.add(guiControls[robotIndex], 'step', 0, path[robotIndex].number).step(1.0).listen();
+
+
+}
+
+function updateGUI(robotIndex){
+ for (var i=0;i<targetParams.length;i++) {
+ targetParams[i].onChange(function(value) {
+ updateAngles(robotIndex);
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+ });
+ }
+
+ for (var i=0;i<jointsParams.length;i++) {
+ jointsParams[i].onChange(function(value) {
+ if(leg[robotIndex]==1)
+ {
+ VisualRobot[robotIndex].setAngle(robotIndex,0,guiControls[robotIndex].j1*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,1,guiControls[robotIndex].j2*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,2,guiControls[robotIndex].j3*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,3,guiControls[robotIndex].j4*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,4,guiControls[robotIndex].j5*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,5,guiControls[robotIndex].j6*DEG_TO_RAD);
+
+ }else
+ {
+ VisualRobot[robotIndex].setAngle(robotIndex,5,guiControls[robotIndex].j1*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,4,guiControls[robotIndex].j2*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,3,guiControls[robotIndex].j3*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,2,guiControls[robotIndex].j4*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,1,guiControls[robotIndex].j5*DEG_TO_RAD);
+ VisualRobot[robotIndex].setAngle(robotIndex,0,guiControls[robotIndex].j6*DEG_TO_RAD);
+
+
+ }
+
+ updateTarget(robotIndex);
+ });
+ }
+
+ for (var i=0 ;i<geometryParams.length ;i++) {
+ geometryParams[i].onChange(function(value) {
+ updateRobotGeometry(robotIndex);
+ });
+ }
+
+ currentStep.onChange(function(value) {
+ step(robotIndex,guiControls[robotIndex].step);
+ });
+
+ end.onChange(function(value) {
+
+ changeEnd(robotIndex);
+ });
+
+
+}
+
+///////////////////////Inverse Kinematics////////////////////////
+function updateIK(robotIndex) {
+ const geo = Object.values(defaultRobotState[robotIndex].geometry).map((val, i, array) => [val.x, val.y, val.z])
+ // todo not optimal, since IK is a sideeffect
+ IK[robotIndex] = new InverseKinematic(geo);
+}
+
+function updateAngles(robotIndex){
+ // const calculateAngles = (jointLimits, position, rotation, configuration) => {
+ const angles = []
+ IK[robotIndex].calculateAngles(
+ guiControls[robotIndex].x,
+ guiControls[robotIndex].y,
+ guiControls[robotIndex].z +2.5*voxelSpacing,
+ defaultRobotState[robotIndex].target.rotation.x,
+ defaultRobotState[robotIndex].target.rotation.y,
+ defaultRobotState[robotIndex].target.rotation.z,
+ angles,
+ defaultRobotState[robotIndex].target.configuration
+ )
+ outOfBounds = [false, false, false, false, false, false]
+ // let i = 0
+ // for (const index in jointLimits) {
+ // if (angles[i] < jointLimits[index][0] || angles[i] > jointLimits[index][1]) {
+ // outOfBounds[i] = true
+ // }
+ // i++
+ // }
+ VisualRobot[robotIndex].setAngle(robotIndex,0,angles[0]);
+ VisualRobot[robotIndex].setAngle(robotIndex,1,angles[1]);
+ VisualRobot[robotIndex].setAngle(robotIndex,2,angles[2]);
+ VisualRobot[robotIndex].setAngle(robotIndex,3,angles[3]);
+ VisualRobot[robotIndex].setAngle(robotIndex,4,angles[4]);
+ VisualRobot[robotIndex].setAngle(robotIndex,5,angles[5]);
+
+ angles[robotIndex][0]=angles[0];
+ angles[robotIndex][1]=angles[1];
+ angles[robotIndex][2]=angles[2];
+ angles[robotIndex][3]=angles[3];
+ angles[robotIndex][4]=angles[4];
+ angles[robotIndex][5]=angles[5];
+
+ guiControls[robotIndex].j1=angles[robotIndex][0]*RAD_TO_DEG;
+ guiControls[robotIndex].j2=angles[robotIndex][1]*RAD_TO_DEG;
+ guiControls[robotIndex].j3=angles[robotIndex][2]*RAD_TO_DEG;
+ guiControls[robotIndex].j4=angles[robotIndex][3]*RAD_TO_DEG;
+ guiControls[robotIndex].j5=angles[robotIndex][4]*RAD_TO_DEG;
+ guiControls[robotIndex].j6=angles[robotIndex][5]*RAD_TO_DEG;
+ return angles;
+
+}
+
+function getNormalAdjustment(robotIndex,n,vnormal,forward){//n is normal degree
+
+ var result=new THREE.Vector3(0,0,0);
+ if(n==180)
+ {
+ return result;
+ }
+ var theta=Math.abs(180-n);
+ var base=2*Math.sin(theta/2*DEG_TO_RAD)*guiControls[robotIndex].offset;
+ var x= Math.sin(((180-theta)/2)*DEG_TO_RAD)*base;
+ var y= Math.cos(((180-theta)/2)*DEG_TO_RAD)*base;
+
+ result= vnormal.clone().multiplyScalar(-y);
+
+ if(n > 180)
+ {
+ var tempV=forward.clone().multiplyScalar(x);
+ result.add(tempV);
+ return result;
+ }else
+ {
+ var tempV=forward.clone().multiplyScalar(-x);
+ result.add(tempV);
+ return result;
+ }
+
+}
+
+////////////////////////Taget Control////////////////////////////
+function targetControl(robotIndex){
+ target[robotIndex] = new THREE.Group();
+ scene.add(target[robotIndex]);
+
+ control[robotIndex] = new THREE.TransformControls(camera, renderer.domElement);
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+
+ control[robotIndex].size=0.5;
+ control[robotIndex].space = "local";
+ target[robotIndex].rotation.y=180*DEG_TO_RAD;
+ target[robotIndex].rotation.z=90*DEG_TO_RAD;
+ // control[robotIndex].setSpace( control[robotIndex].space === "local" ? "world" : "local" );
+ control[robotIndex].addEventListener('change', () => {
+ guiControls[robotIndex].x= target[robotIndex].position.x;
+ guiControls[robotIndex].y= target[robotIndex].position.y;
+ guiControls[robotIndex].z= target[robotIndex].position.z;
+ defaultRobotState[robotIndex].target.position.x=target[robotIndex].position.x;
+ defaultRobotState[robotIndex].target.position.y=target[robotIndex].position.y;
+ defaultRobotState[robotIndex].target.position.z=target[robotIndex].position.z;
+ // defaultRobotState[robotIndex].target.rotation.x
+ // defaultRobotState[robotIndex].target.rotation.y
+ // defaultRobotState[robotIndex].target.rotation.z
+ // console.log("here")
+ updateAngles(robotIndex);
+ });
+ control[robotIndex].attach(target[robotIndex]);
+
+ scene.add(control[robotIndex]);
+ // control[robotIndex].visible = false;
+}
+
+function updateTarget(robotIndex){
+ console.log("heree")
+ var tempPosition=new THREE.Vector3(0,0,0);
+
+ parent.updateMatrixWorld();
+
+ var vector = new THREE.Vector3();
+ vector.setFromMatrixPosition( child.matrixWorld );
+
+
+
+ var object=THREE1[robotIndex].children[0].children[2].children[2].children[2].children[2];
+ object.updateMatrixWorld();
+ var vector = object.geometry.vertices[i].clone();
+ vector.applyMatrix4( object.matrixWorld );
+
+ tempPosition.x=THREE1[robotIndex].parent.parent.children[0].position.x;
+ tempPosition.y=THREE1[robotIndex].parent.parent.children[0].position.y;
+ tempPosition.z=THREE1[robotIndex].parent.parent.children[0].position.z;
+ guiControls[robotIndex].x=tempPosition.x;
+ guiControls[robotIndex].y=tempPosition.y;
+ guiControls[robotIndex].z=tempPosition.z;
+
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+}
+
+///////////////////////assembly/////////
+function assemble(robotIndex){
+ resetPath(robotIndex);
+ builtShape();
+ generatePoints(robotIndex);
+
+ // steps[robotIndex]=[startLocations[robotIndex],new THREE.Vector3(10*voxelSpacing,0*voxelSpacing,0)];
+ // showTargetPosition(robotIndex,minIndex,true);
+
+ for(var i=0;i<steps[robotIndex].length;i++){
+ if(i%2!=0){
+ moveRobot(robotIndex,true,steps[robotIndex][i]);
+ }else{
+ moveRobot(robotIndex,false,steps[robotIndex][i+1]);
+ }
+
+ }
+
+}
+
+function createPath(robotIndex,start,end){
+ var snormal=new THREE.Vector3(0,0,1);
+ var enormal=new THREE.Vector3(0,0,1);
+ var robotUp=new THREE.Vector3(0,0,1);
+ var p1=start.clone();
+ p1.add(snormal.clone().multiplyScalar(0));
+ var p2=new THREE.Vector3(0,0,0);
+ var p3=new THREE.Vector3(0,0,0);
+ var p4=end.clone();
+ p4.add(enormal.clone().multiplyScalar(0));
+
+ var nor = snormal.clone();
+ nor.add(enormal);
+ nor.normalize();
+
+ var dir=end.clone().sub(start);
+
+ var temp1=new THREE.Vector3(0,0,0);
+ var temp2=new THREE.Vector3(0,0,0);
+ nor.multiplyScalar(path[robotIndex].cHeight);
+
+
+ temp1.addVectors(start,dir.multiplyScalar(1/3));
+ temp2.addVectors(start,dir.multiplyScalar(2));
+
+
+ p2.addVectors(nor,temp1);
+ p3.addVectors(nor,temp2);
+
+ //create bezier curve
+ path[robotIndex].curve= new THREE.CubicBezierCurve3 (
+ p1,
+ p2,
+ p3,
+ p4
+ );
+ dividePath(robotIndex,snormal.clone(),enormal.clone());
+}
+
+function dividePath(robotIndex,snormal,enormal,robotUp,robotLocation){
+
+ var snormal=new THREE.Vector3(0,0,1);
+ var enormal=new THREE.Vector3(0,0,1);
+ var robotUp=new THREE.Vector3(0,0,1);
+ var robotLocation=new THREE.Vector3(0,0,1);
+
+ //points
+ var d=1/path[robotIndex].number;
+ var tempPoints=path[robotIndex].curve.getSpacedPoints(path[robotIndex].number);
+
+ var forward=new THREE.Vector3(0,0,0);
+ var tempV=tempPoints[0].clone().sub(robotLocation);
+ if(tempV.x!=0 && robotUp.x==0)
+ {
+ forward.x=tempV.x;
+ }else if(tempV.y!=0 && robotUp.y==0)
+ {
+ forward.y=tempV.y;
+ }else if(tempV.z!=0 && robotUp.z==0)
+ {
+ forward.z=tempV.z;
+ }
+ forward.normalize();
+
+ var p1=tempPoints[0];
+ var p2=tempPoints[path[robotIndex].number-1];
+ var diff=p2.clone().sub(p1);
+ diff.multiply(snormal);
+
+ //normals
+ var vnormal1=180;
+ var vnormal2=180;
+ if(!snormal.equals(enormal))
+ {
+
+ if(diff.x>0||diff.y>0||diff.z>0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=90;
+
+ }
+
+ }else if(diff.x<0||diff.y<0||diff.z<0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=180+90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=180+90;
+
+ }
+ }
+ }
+ var dn=(vnormal2-vnormal1)/path[robotIndex].number;
+
+
+
+ for (var i=0;i<=path[robotIndex].number;i++)
+ {
+ path[robotIndex].normals.push(vnormal1+i*dn);
+ path[robotIndex].normalAdjustments.push(getNormalAdjustment(robotIndex,vnormal1+i*dn,robotUp,forward));
+ path[robotIndex].points.push(tempPoints[i].clone());
+
+
+ if(path[robotIndex].showPath)
+ {
+ var material = new THREE.MeshLambertMaterial({ color:0xff7171,});
+ var geometry = new THREE.SphereGeometry(0.05, 0.05, 0.05);
+ var mesh = new THREE.Mesh(geometry, material);
+ mesh.position.x=tempPoints[i].x;
+ mesh.position.y=tempPoints[i].y;
+ mesh.position.z=tempPoints[i].z;
+ scene.add(mesh);
+ }
+ }
+
+}
+
+function moveRobot(robotIndex,leaveVoxel,voxelLoc){
+ var ps= path[robotIndex].points.slice();
+ if(leaveVoxel){
+ setTimeout(function(){ buildVoxelAt(voxelLoc);showTargetPosition(robotIndex,voxelLoc,false) }, path[robotIndex].timeout);
+
+
+ }else{
+ setTimeout(function(){showTargetPosition(robotIndex,voxelLoc,true) }, path[robotIndex].timeout);
+
+ }
+ for(var i=0;i<=path[robotIndex].number;i++)
+ {
+ setTimeout(function(){ move(robotIndex); }, path[robotIndex].timeout+=path[robotIndex].delay);
+ }
+
+
+
+}
+
+function move(robotIndex){
+ guiControls[robotIndex].x=path[robotIndex].points[path[robotIndex].currentPoint].x;
+ guiControls[robotIndex].y=path[robotIndex].points[path[robotIndex].currentPoint].y;
+ guiControls[robotIndex].z=path[robotIndex].points[path[robotIndex].currentPoint].z;
+ voxelNormal[robotIndex]=path[robotIndex].normals[path[robotIndex].currentPoint];
+ normalAdjustmentVector[robotIndex]=path[robotIndex].normalAdjustments[path[robotIndex].currentPoint];
+
+ updateAngles(robotIndex);
+
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+
+ path[robotIndex].currentPoint++;
+ if(path[robotIndex].currentPoint==path[robotIndex].points.length)
+ {
+ // if(goHome[robotIndex])
+ // {
+
+ // buildVoxelAt(robotState[robotIndex].leg1Pos.clone().multiplyScalar(voxelSpacing));
+ // //go Home
+ // // console.log("robotIndex:"+robotIndex+" s2s");
+ // startMovement(robotIndex,true);
+
+ // }else
+ // {
+ // //go to new Voxel
+ // // console.log("robotIndex:"+robotIndex+" s2s");
+ // startMovement(robotIndex,false);
+ // }
+ }
+
+}
+
+function builtShape(){
+ for(var i=0;i<gridSize;i++)
+ {
+ var t=[];
+ grid.push(t)
+ for(var j=0;j<gridSize;j++)
+ {
+ var tt=[];
+ grid[i].push(tt);
+ for(var k=0;k<gridSize;k++)
+ {
+ grid[i][j].push(false);
+ }
+ }
+ }
+ for(var i=gridSize/2.0;i<gridSize;i++)
+ {
+ grid[i][0][0]=true;
+ locations.push(new THREE.Vector3(i*voxelSpacing,0,0)); //TODO CHANGE LATER BASED ON NUMBER OF ROBOTS
+ }
+
+}
+
+function generatePoints(robotIndex){
+ for(var i=0;i<locations.length;i++){
+ steps[robotIndex].push(startLocations[robotIndex]);
+ steps[robotIndex].push(locations[i]);
+
+ }
+ console.log(steps[robotIndex])
+ for(var i=0;i<steps[robotIndex].length-1;i++){
+ createPath(robotIndex,steps[robotIndex][i],steps[robotIndex][i+1]);
+ }
+
+
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////Change Fixed End////////////////////////////
+function changeEnd(robotIndex){
+ //TODO: send fix end and unfix end to code
+ if(leg[robotIndex]==2)
+ {
+ //later output package
+ // console.log("unfix leg 1, fix leg 2");
+ leg[robotIndex]=1;
+ guiControls[robotIndex].targetEnd="end 1";
+ }else
+ {
+ //later output package
+ // console.log("unfix leg 2, fix leg 1");
+ leg[robotIndex]=2;
+ guiControls[robotIndex].targetEnd="end 2";
+ }
+
+ var tempPosition=new THREE.Vector3(0,0,0);
+ tempPosition.x=THREE1[robotIndex].position.x;
+ tempPosition.y=THREE1[robotIndex].position.y;
+ tempPosition.z=THREE1[robotIndex].position.z;
+
+ THREE1[robotIndex].position.x=target[robotIndex].position.x;
+ THREE1[robotIndex].position.y=target[robotIndex].position.y;
+ THREE1[robotIndex].position.z=target[robotIndex].position.z;
+
+
+ guiControls[robotIndex].x=tempPosition.x;
+ guiControls[robotIndex].y=tempPosition.y;
+ guiControls[robotIndex].z=tempPosition.z;
+
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+
+ updateAngles(robotIndex);
+
+}
+
+function rotateRobot(robotIndex,dir){
+
+ var newRotation=getRotation(dir).clone();
+ THREE1[robotIndex].rotation.x=newRotation.x;
+ THREE1[robotIndex].rotation.y=newRotation.y;
+ THREE1[robotIndex].rotation.z=newRotation.z;
+
+ voxelNormal[robotIndex]=path[robotIndex].normals[path[robotIndex].currentPoint+1];
+ normalAdjustmentVector[robotIndex]=path[robotIndex].normalAdjustments[path[robotIndex].currentPoint+1];
+ updateAngles(robotIndex);
+}
+
+function getRotation(dir){
+ var tempRot=new THREE.Vector3();
+ if(dir.equals(new THREE.Vector3(0,0,1)) )
+ {
+ tempRot=new THREE.Vector3(0,0,0);
+ }
+ else if (dir.equals( new THREE.Vector3(0,0,-1)))
+ {
+ tempRot=new THREE.Vector3(180*DEG_TO_RAD,0,0);
+ }
+ else if (dir.equals( new THREE.Vector3(0,-1,0)))
+ {
+ tempRot=new THREE.Vector3(90*DEG_TO_RAD,0,0);
+ }
+ else if (dir.equals( new THREE.Vector3(0,1,0)))
+ {
+ tempRot=new THREE.Vector3(-90*DEG_TO_RAD,0,0);
+ }
+ else if (dir.equals(new THREE.Vector3(-1,0,0)))
+ {
+ tempRot=new THREE.Vector3(0,-90*DEG_TO_RAD,0);
+ }
+ else if (dir.equals( new THREE.Vector3(1,0,0)))
+ {
+ tempRot=new THREE.Vector3(0,90*DEG_TO_RAD,0);
+ }
+ return tempRot;
+}
+
+////////////////////////////////Path////////////////////////////
+
+
+
+function _createPath(robotIndex,start,end,snormal,enormal,robotUp,robotLocation){
+ var p1=start.clone();
+ p1.add(snormal.clone().multiplyScalar(1));
+ var p2=new THREE.Vector3(0,0,0);
+ var p3=new THREE.Vector3(0,0,0);
+ var p4=end.clone();
+ p4.add(enormal.clone().multiplyScalar(1));
+
+ var nor = snormal.clone();
+ nor.add(enormal);
+ nor.normalize();
+
+ var dir=end.clone().sub(start);
+
+ var temp1=new THREE.Vector3(0,0,0);
+ var temp2=new THREE.Vector3(0,0,0);
+ nor.multiplyScalar(path[robotIndex].cHeight);
+
+
+ temp1.addVectors(start,dir.multiplyScalar(1/3));
+ temp2.addVectors(start,dir.multiplyScalar(2));
+
+
+ p2.addVectors(nor,temp1);
+ p3.addVectors(nor,temp2);
+
+ //create bezier curve
+ path[robotIndex].curve= new THREE.CubicBezierCurve3 (
+ p1,
+ p2,
+ p3,
+ p4
+ );
+ _dividePath(robotIndex,snormal.clone(),enormal.clone(),robotUp,robotLocation);
+}
+
+function _dividePath(robotIndex,snormal,enormal,robotUp,robotLocation){
+
+ //points
+ var d=1/path[robotIndex].number;
+ var tempPoints=path[robotIndex].curve.getSpacedPoints(path[robotIndex].number);
+
+ var forward=new THREE.Vector3(0,0,0);
+ var tempV=tempPoints[0].clone().sub(robotLocation);
+ if(tempV.x!=0 && robotUp.x==0)
+ {
+ forward.x=tempV.x;
+ }else if(tempV.y!=0 && robotUp.y==0)
+ {
+ forward.y=tempV.y;
+ }else if(tempV.z!=0 && robotUp.z==0)
+ {
+ forward.z=tempV.z;
+ }
+ forward.normalize();
+
+ var p1=tempPoints[0];
+ var p2=tempPoints[path[robotIndex].number-1];
+ var diff=p2.clone().sub(p1);
+ diff.multiply(snormal);
+
+ //normals
+ var vnormal1=180;
+ var vnormal2=180;
+ if(!snormal.equals(enormal))
+ {
+
+ if(diff.x>0||diff.y>0||diff.z>0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=90;
+
+ }
+
+ }else if(diff.x<0||diff.y<0||diff.z<0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=180+90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=180+90;
+
+ }
+ }
+ }
+ var dn=(vnormal2-vnormal1)/path[robotIndex].number;
+
+
+
+ for (var i=0;i<=path[robotIndex].number;i++)
+ {
+ path[robotIndex].normals.push(vnormal1+i*dn);
+ path[robotIndex].normalAdjustments.push(getNormalAdjustment(robotIndex,vnormal1+i*dn,robotUp,forward));
+ path[robotIndex].points.push(tempPoints[i].clone());
+
+
+ if(path[robotIndex].showPath)
+ {
+ var material = new THREE.MeshLambertMaterial({ color:0xff7171,});
+ var geometry = new THREE.SphereGeometry(0.05, 0.05, 0.05);
+ var mesh = new THREE.Mesh(geometry, material);
+ mesh.position.x=tempPoints[i].x;
+ mesh.position.y=tempPoints[i].y;
+ mesh.position.z=tempPoints[i].z;
+ scene.add(mesh);
+ }
+ }
+
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+function _BilleStep(robotIndex,stepLeg1,stepLeg2,legsNormal){
+ var changeLegBefore=false;
+ var leg1Pos=new THREE.Vector3(0,0,0);
+ var leg2Pos=new THREE.Vector3(0,0,0);
+
+ var pos1=new THREE.Vector3(0,0,0);
+ var pos2=new THREE.Vector3(0,0,0);
+
+ if(path[robotIndex].points.length==0)
+ {
+ pos1.copy(target[robotIndex].position);
+ pos2.copy(THREE1[robotIndex].position);
+ }else{
+ pos1.copy(path[robotIndex].points[path[robotIndex].points.length-1-1-path[robotIndex].number]);
+ pos2.copy(path[robotIndex].points[path[robotIndex].points.length-1]);
+
+ }
+
+ //check starting leg based on distance to target
+ if(pos2.distanceTo(stepLeg1)<pos1.distanceTo(stepLeg1))
+ {
+ changeLegBefore=true;
+ var temp=new THREE.Vector3(0,0,0);
+ temp.copy(pos1);
+ pos1.copy(pos2);
+ pos2.copy(temp);
+ }
+
+ leg1Pos.copy(pos1);
+ leg2Pos.copy(pos2);
+
+ // var robotUp=THREE1dir[robotIndex].clone();
+ var robotUp=THREE1dir[robotIndex].clone();
+ var robotLocation=THREE1Pos[robotIndex].clone();
+
+ _createPath(robotIndex,leg1Pos,stepLeg1,THREE1dir[robotIndex],legsNormal,robotUp,robotLocation);
+
+ robotUp=legsNormal;
+ robotLocation=stepLeg1;
+ _createPath(robotIndex,leg2Pos,stepLeg2,THREE1dir[robotIndex],legsNormal,robotUp,robotLocation);
+
+ //update previous locations/pos
+ THREE1dir[robotIndex] = legsNormal.clone();
+ THREE1Pos[robotIndex]=stepLeg2.clone();
+
+ return [changeLegBefore,legsNormal];
+}
+
+function moveLeg(robotIndex){
+ var ps= path[robotIndex].points.slice();
+
+ for(var i=0;i<=path[robotIndex].number;i++)
+ {
+ setTimeout(function(){ move(robotIndex); }, path[robotIndex].timeout+=path[robotIndex].delay);
+ }
+ setTimeout(function(){ changeEnd(robotIndex); }, path[robotIndex].timeout);
+
+}
+
+function _move(robotIndex){
+ guiControls[robotIndex].x=path[robotIndex].points[path[robotIndex].currentPoint].x;
+ guiControls[robotIndex].y=path[robotIndex].points[path[robotIndex].currentPoint].y;
+ guiControls[robotIndex].z=path[robotIndex].points[path[robotIndex].currentPoint].z;
+ voxelNormal[robotIndex]=path[robotIndex].normals[path[robotIndex].currentPoint];
+ normalAdjustmentVector[robotIndex]=path[robotIndex].normalAdjustments[path[robotIndex].currentPoint];
+ updateAngles(robotIndex);
+
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+
+ path[robotIndex].currentPoint++;
+ if(path[robotIndex].currentPoint==path[robotIndex].points.length)
+ {
+ if(goHome[robotIndex])
+ {
+
+ buildVoxelAt(robotState[robotIndex].leg1Pos.clone().multiplyScalar(voxelSpacing));
+ //go Home
+ // console.log("robotIndex:"+robotIndex+" s2s");
+ startMovement(robotIndex,true);
+
+ }else
+ {
+ //go to new Voxel
+ // console.log("robotIndex:"+robotIndex+" s2s");
+ startMovement(robotIndex,false);
+ }
+ }
+
+}
+
+function getDirVec(direction){
+ var vec = new THREE.Vector3(0,0,0);
+
+ switch(direction)
+ {
+ case 0:
+ vec = new THREE.Vector3(1,0,0);
+ break;
+ case 1:
+ vec = new THREE.Vector3(-1,0,0);
+ break;
+ case 2:
+ vec = new THREE.Vector3(0,1,0);
+ break;
+ case 3:
+ vec = new THREE.Vector3(0,-1,0);
+ break;
+ }
+ return vec;
+
+}
+
+function resetPath(robotIndex){
+ //////////////////////
+ steps[robotIndex]=[];
+ path[robotIndex].curve=null;
+ path[robotIndex].points=[];
+ path[robotIndex].normals=[];
+ path[robotIndex].changeRotation=[];
+ path[robotIndex].normalAdjustments=[];
+ path[robotIndex].currentPoint=0;
+ path[robotIndex].timeout=0;
+}
+
+//////////////////////////commands stack////////////////////////////
+function _generatePoints(robotIndex){
+ for (var i=0; i<steps[robotIndex].length ; i++)
+ {
+ var s=_BilleStep(robotIndex,steps[robotIndex][i][0],steps[robotIndex][i][1],steps[robotIndex][i][2]);
+ path[robotIndex].changeLegs.push(s[0]);
+ path[robotIndex].changeRotation.push(s[1]);
+ }
+}
+
+function step(robotIndex,i){
+ totalNumberofSteps[robotIndex]++;
+ // console.log("robot "+robotIndex +":"+totalNumberofSteps[robotIndex]);
+ // console.log(totalNumberofSteps);
+ if(path[robotIndex].changeLegs[i])
+ {
+ setTimeout(function(){ changeEnd(robotIndex); }, path[robotIndex].timeout);
+ }
+
+ moveLeg(robotIndex);//leg1
+
+ //rotate bill-e
+ if(i>0 && !path[robotIndex].changeRotation[i].equals(path[robotIndex].changeRotation[i-1]))
+ {
+
+ setTimeout(function(){ rotateRobot(robotIndex,path[robotIndex].changeRotation[parseInt((path[robotIndex].currentPoint-path[robotIndex].number)/path[robotIndex].number/2)]);
+ }, path[robotIndex].timeout);
+ }
+
+ moveLeg(robotIndex);//leg2
+
+}
+
+function startMovement(robotIndex,home){
+ // console.log("robot:"+robotIndex+" home:"+home);
+
+ if(!home)
+ {
+ resetPath(robotIndex);
+ voxelBuilder(robotIndex,robotState[robotIndex].leg1Pos, robotState[robotIndex].leg2Pos, robotState[robotIndex].up,robotState[robotIndex].forward);
+ generatePoints(robotIndex);
+
+
+ if(steps[robotIndex].length>0)
+ {
+ for(var i=0;i<steps[robotIndex].length;i++)
+ {
+ step(robotIndex,i);
+ }
+ goHome[robotIndex]=true;
+ }
+
+
+
+ //update z and rank
+ if(robotState[robotIndex].Z>globalZ)
+ {
+ globalZ=robotState[robotIndex].Z;
+ globalRank=robotState[robotIndex].rank;
+ }else if(robotState[robotIndex].Z<globalZ)
+ {
+ robotState[robotIndex].Z=globalZ;
+ robotState[robotIndex].rank=globalRank;
+ }else
+ {
+ if(robotState[robotIndex].rank>globalRank)
+ {
+ globalRank=robotState[robotIndex].rank;
+ }else if(robotState[robotIndex].rank<globalRank)
+ {
+ robotState[robotIndex].rank=globalRank;
+ }
+ }
+
+
+ if(voxelSlices[robotState[robotIndex].Z].length>0)
+ {
+ if(voxelSlicesCount[robotState[robotIndex].Z][robotState[robotIndex].rank] <voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank].length-1)
+ {
+ voxelSlicesCount[robotState[robotIndex].Z][robotState[robotIndex].rank]++;
+ // console.log("robot:"+robotIndex+" f"+" home:"+home);
+ if(!goHome[robotIndex])
+ {
+ startMovement(robotIndex,true);
+ }
+ }
+ else
+ {
+ if(robotState[robotIndex].rank<voxelSlicesCount[robotState[robotIndex].Z].length-1)
+ {
+ robotState[robotIndex].rank++;
+ // globalRank++;
+ // console.log("robot:"+robotIndex+" ff"+" home:"+home);
+ if(!goHome[robotIndex])
+ {
+ startMovement(robotIndex,true);
+ }
+ }
+ else
+ {
+ if(robotState[robotIndex].Z<voxelSlicesCount.length-1)
+ {
+ robotState[robotIndex].Z++;
+ robotState[robotIndex].rank=0;
+
+ // globalZ++;
+ // globalRank=0;
+
+ // console.log("robot:"+robotIndex+" fff"+" home:"+home);
+ if(!goHome[robotIndex])
+ {
+ startMovement(robotIndex,true);
+ }
+
+ }else
+ {
+ console.log("DONEEE!!!");
+
+ }
+ }
+ }
+ }
+ else if(robotState[robotIndex].Z<voxelSlices.length)
+ {
+ // console.log("robot:"+robotIndex+" fffff"+" home:"+home);
+ robotState[robotIndex].Z++;
+ robotState[robotIndex].rank=0;
+
+ // globalZ++;
+ // globalRank=0;
+ // console.log("ss");
+ startMovement(robotIndex,false);
+ }
+
+ }
+ else
+ {
+ resetPath(robotIndex);
+ reloadVoxel(robotIndex,robotState[robotIndex].leg1Pos, robotState[robotIndex].leg2Pos, robotState[robotIndex].up,robotState[robotIndex].forward);
+ generatePoints(robotIndex);
+
+ for(var i=0;i<steps[robotIndex].length;i++)
+ {
+ step(robotIndex,i);
+ }
+ goHome[robotIndex]=false;
+
+
+ if(steps[robotIndex].length==0)
+ {
+ // console.log("nn st")
+ startMovement(robotIndex,false);
+ }
+
+ }
+
+}
+
+function reloadVoxel(robotIndex,leg1Pos, leg2Pos, up ,forward){
+ var succeeded;
+
+ showTargetPosition(robotIndex,leg1Pos.clone(),false);
+
+ state= pathPlan(robotIndex,leg1Pos, leg2Pos, up ,forward,startLocations[robotIndex]);
+
+ robotState[robotIndex].leg1Pos=state[0].clone();
+ robotState[robotIndex].leg2Pos=state[1].clone();
+ robotState[robotIndex].up=state[2].clone();
+ robotState[robotIndex].forward=state[3].clone();
+ succeeded=state[4];
+
+ if(succeeded)
+ {
+ return [state[0],state[1],state[2],state[3]];
+
+ }else
+ {
+ // console.log("COULDN'T FIND PATH!!!");
+ return false;
+ }
+
+
+}
+
+function voxelBuilder(robotIndex,leg1Pos, leg2Pos, up ,forward){
+
+ var start=leg1Pos.clone();
+ var state;
+ var succeeded;
+ var count=0;
+
+ //get point with minimum distance
+ var min=Infinity;
+ var minIndex= new THREE.Vector3(0,0,0);
+
+
+ if(voxelSlices[robotState[robotIndex].Z].length>0)
+ {
+
+ for(var i=0;i<voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank].length;i++)
+ {
+ if(!voxelAt(voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank][i].clone().multiplyScalar(1/voxelSpacing)))
+ {
+
+ if(start.distanceTo(voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank][i])<min)
+ {
+ min=start.distanceTo(voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank][i]);
+ minIndex=voxelSlices[robotState[robotIndex].Z][robotState[robotIndex].rank][i];
+
+ }
+
+ }
+ }
+
+ grid[minIndex.x/voxelSpacing][minIndex.y/voxelSpacing][minIndex.z/voxelSpacing]=true;
+
+ showTargetPosition(robotIndex,minIndex,true);
+
+ // console.log(minIndex);
+
+ state= pathPlan(robotIndex,leg1Pos, leg2Pos, up ,forward,minIndex);
+
+ robotState[robotIndex].leg1Pos=state[0].clone();
+ robotState[robotIndex].leg2Pos=state[1].clone();
+ robotState[robotIndex].up=state[2].clone();
+ robotState[robotIndex].forward=state[3].clone();
+ succeeded=state[4];
+
+ if(succeeded)
+ {
+ return [state[0],state[1],state[2],state[3]];
+
+ }else
+ {
+ // console.log("COULDN'T FIND PATH!!!");
+ return false;
+ }
+
+ }
+
+}
+
+//////////////////////////load Voxels////////////////////////////
+function loadVoxel(){
+ var geometry = new THREE.BufferGeometry();
+ // create a simple square shape. We duplicate the top left and bottom right
+ // vertices because each vertex needs to appear once per triangle.
+ var vertices = voxelData;
+ var normals = voxelNormalData;
+ var uv = voxelUVData;
+
+ // itemSize = 3 because there are 3 values (components) per vertex
+ geometry.setAttribute( 'position', new THREE.BufferAttribute( vertices, 3 ) );
+ geometry.setAttribute( 'normal', new THREE.BufferAttribute( normals, 3 ) );
+ geometry.setAttribute( 'uv', new THREE.BufferAttribute( uv, 2 ) );
+
+ var material = new THREE.MeshLambertMaterial( { color: 0xbbc3ce } );
+ var object = new THREE.Mesh( geometry, material );
+ // object.scale.x=0.04;
+ // object.scale.y=0.04;
+ // object.scale.z=0.04;
+ // object.position.z=-1.5;
+ object.scale.x=0.5/3.0;
+ object.scale.y=0.5/3.0;
+ object.scale.z=0.5/3.0;
+ object.position.x=-15;
+ object.position.y=-15;
+ voxel=object;
+}
+
+function frepVoxel(rangeX,rangeY,rangeZ,stringFunction){
+ //build grid
+ for(var i=0;i<gridSize;i++)
+ {
+ var t=[];
+ grid.push(t)
+ for(var j=0;j<gridSize;j++)
+ {
+ var tt=[];
+ grid[i].push(tt);
+ for(var k=0;k<gridSize;k++)
+ {
+ grid[i][j].push(false);
+ }
+ }
+ }
+ //build first layer
+ for(var i=0;i<gridSize;i++)
+ {
+ for(var j=0;j<gridSize;j++)
+ {
+ buildVoxelAt(new THREE.Vector3(i*voxelSpacing,j*voxelSpacing,0));
+ grid[i][j][0]=true;
+ }
+ }
+
+ // string function= "Math.min(Math.min(Math.min(Math.min(X-(-1),(1)-X),Math.min(Y-(-1),(1)-Y)),Math.min(Z-(-1),(1)-Z)),-(Math.min(Math.min(Math.min(X-(-0.8),(0.8)-X),Math.min(Y-(-0.8),(0.8)-Y)),Math.min(Z-(-0.8),(0.8)-Z))))";
+ var maxZslices=[];
+ var tempVoxelSlices=[];
+ for (var Z=0;Z<gridSize;Z++)
+ {
+ voxelSlices.push([]);
+ tempVoxelSlices.push([]);
+ voxelSlicesCount.push([]);
+ var max=- Infinity;
+ var maxIndex=new THREE.Vector3(0,0,0);
+ for (var Y=0;Y<gridSize;Y++)
+ {
+ for (var X=0;X<gridSize;X++)
+ {
+ var func= frep(X,Y,Z);
+
+ if(func>=0 && !grid[X][Y][Z])
+ {
+ if(func>max)
+ {
+ max=func;
+ maxIndex=new THREE.Vector3(X*voxelSpacing,Y*voxelSpacing, Z*voxelSpacing);
+
+ }
+ var loc=new THREE.Vector3(X*voxelSpacing,Y*voxelSpacing,Z*voxelSpacing);
+ tempVoxelSlices[Z].push(loc);
+ voxelNum++;
+ }
+ }
+ }
+ maxZslices.push(maxIndex);//check if right later
+ }
+
+ for (var Z=0;Z<gridSize;Z++)
+ {
+
+ for(var i=0;i<tempVoxelSlices[Z].length;i++)
+ {
+ var rank = Math.ceil(maxZslices[Z].distanceTo(tempVoxelSlices[Z][i]) /voxelSpacing);
+ while(voxelSlices[Z].length<=rank)
+ {
+ voxelSlicesCount[Z].push([]);
+
+ voxelSlices[Z].push([]);
+
+ voxelSlicesCount[Z][voxelSlices[Z].length-1]=0;
+
+ voxelSlices[Z][voxelSlices[Z].length-1]=[];
+ }
+ voxelSlices[Z][rank].push(tempVoxelSlices[Z][i]);
+ }
+ }
+
+ //////////
+ for(var i=0; i<numberOfRobots;i++)
+ {
+ buildHelperMeshes(i);
+ }
+
+
+}
+
+function frep(X,Y,Z){
+ // return (3*3-((X-(5))*(X-(5))+(Y-(5))*(Y-(5))+(Z-(3))*(Z-(3)))); //sphere FIX!!
+ // return Math.min(Math.min(Math.min(Math.min(X-(2),(7)-X),Math.min(Y-(2),(7)-Y)),Math.min(Z-(1),(5)-Z)),-(Math.min(Math.min(Math.min(X-(4),(8)-X),Math.min(Y-(4),(8)-Y)),Math.min(Z-(0),(4)-Z))));
+ // return Math.min(Math.min(Math.min(Math.min(X-(2),(5)-X),Math.min(Y-(2),(5)-Y)),Math.min(Z-(1),(5)-Z)),-(Math.min(Math.min(Math.min(X-(3),(6)-X),Math.min(Y-(3),(6)-Y)),Math.min(Z-(0),(4)-Z))));
+ // return Math.min(Math.min(Math.min(Math.min(X-(2),(7)-X),Math.min(Y-(2),(7)-Y)),Math.min(Z-(1),(6)-Z)),-(Math.min(Math.min(Math.min(X-(3),(6)-X),Math.min(Y-(3),(6)-Y)),Math.min(Z-(0),(7)-Z))));//empty cube
+ // return Math.min(((6)-Z),(Math.min((Z-(1)),(((3*(5-(Z-(1)))/5)*(3*(5-(Z-(1)))/5)-((X-(5))*(X-(5))+(Y-(5))*(Y-(5))))))));//CONE
+ return Math.min(Math.min(Math.min(X-(2),((gridSize-3))-X),Math.min(Y-(2),((gridSize-3))-Y)),Math.min(Z-(1),((gridSize-4))-Z)); //CUBE
+}
+
+function buildVoxelAt(loc){
+ var object1=voxel.clone();
+ object1.position.x=loc.x;
+ object1.position.y=loc.y;
+ object1.position.z=loc.z+voxelSpacing/2;
+ // if(loc.z==0){
+ // var color1= new THREE.Color( 0, 0, 0 )
+
+ // var material = new THREE.MeshLambertMaterial( { color: 0x000000 } );
+
+ // // var color = new THREE.Color( 1/globalRank, 0, 0 );
+
+ // object1.material=material.clone();
+ // object1.material.color=color1;
+
+ // }
+
+
+
+ scene.add( object1 );
+}
+
+//////////////////////////path planing///////////////////////////
+
+function pathPlan(robotIndex,leg1Pos, leg2Pos, up ,forward,targetPos){
+
+ if(voxelAt(targetPos.clone().multiplyScalar(1/voxelSpacing)))
+ {
+ var succeded=false;
+ var p0=findShortestPath(robotIndex,leg1Pos.clone(), leg2Pos.clone(), up.clone(),forward.clone(), targetPos);
+ succeded=p0[4];
+ steps[robotIndex]=steps[robotIndex].concat(p0[5]);
+ return [p0[0] , p0[1] , p0[2] , p0[3] , true];
+ if(!succeded)
+ {
+ // console.log("see path from neighbours");
+ var neighbours=getNeighbours(targetPos);
+
+ for(var i=0;i<neighbours.length;i++)
+ {
+ var p=findShortestPath(robotIndex,leg1Pos.clone(), leg2Pos.clone(), up.clone(),forward.clone(),neighbours[i]);
+ succeded=p[4];
+ if(succeded)
+ {
+ var stepsFirst=p[5].slice();
+ var p1=findShortestPath(robotIndex,p[0], p[1], p[2],p[3],targetPos);
+ succeded=p[4];
+ if(succeded)
+ {
+ for(var j=0;j<steps[robotIndex].length;j++)
+ {
+ stepsFirst.push(p1[5][j]);
+ }
+
+ steps[robotIndex]=steps[robotIndex].concat(stepsFirst.slice());
+ return [p[0] , p[1] , p[2] , p[3] , true];
+ }
+ }
+ }
+ return [leg1Pos, leg2Pos, up ,forward , false];
+ }
+
+ }
+ else
+ {
+ // console.log("TARGET VOXEL DOESN'T EXIST!");
+ return [leg1Pos, leg2Pos, up ,forward , false];
+
+ }
+
+
+}
+
+function findShortestPath(robotIndex,leg1Pos, leg2Pos, up,forward, targetPos){
+ var stepsTemp=[];
+ var stepsAlternative=[];
+ var orderAlternatives=[[0,1,2],[0,2,1],[1,2,0],[1,0,2],[2,1,0],[2,0,1]];
+ for(var i=0;i<orderAlternatives.length;i++)
+ {
+ stepsAlternative.push(findPath(robotIndex,leg1Pos.clone(), leg2Pos.clone(), up.clone(),forward.clone(), targetPos, orderAlternatives[i]).slice());
+ }
+ stepsAlternative.sort(function(a, b){
+ // ASC -> a.length - b.length
+ // DESC -> b.length - a.length
+ return a[4].length - b[4].length;
+ });
+ stepsTemp=stepsAlternative[0][4].slice();
+ if(stepsTemp.length>stepsCap)
+ {
+ stepsTemp=[];
+ // console.log("CAN'T FIND ANY PATH TO THIS POINT");
+ return [stepsAlternative[0][0],stepsAlternative[0][1],stepsAlternative[0][2],stepsAlternative[0][3],false,stepsTemp];
+ }else
+ {
+ return [stepsAlternative[0][0],stepsAlternative[0][1],stepsAlternative[0][2],stepsAlternative[0][3],true,stepsTemp];
+ }
+}
+
+function findPath(robotIndex,leg1Pos, leg2Pos, up, forward, targetPos, order){
+ var logg=false;
+ if (logg) console.log(order);
+ var stepsTemp=[];
+ // leg1Pos.multiplyScalar(1/voxelSpacing);
+ // leg2Pos.multiplyScalar(1/voxelSpacing);
+ var difference=getDifference(leg1Pos, targetPos);
+ var forwardAlt=new THREE.Vector3(0,0,0);
+ var pos=leg1Pos.clone();
+
+
+ while(difference[0]!=0||difference[1]!=0||difference[2]!=0)
+ {
+ for(var count=0;count<3;count++)
+ {
+ var i= order[count];
+ var startingLeg1Pos=leg1Pos.clone();
+ var startingLeg2Pos=leg2Pos.clone();
+ var exit=false;
+ var previousDifference=Math.pow(10, 1000);//infinity
+
+ if(i==0)
+ {
+ forwardAlt=new THREE.Vector3(-difference[0],0,0);
+ forwardAlt.normalize();
+ if(up.x==0)
+ {
+ forward=forwardAlt.clone();
+ }
+ }else if(i==1)
+ {
+ forwardAlt=new THREE.Vector3(0,-difference[1],0);
+ forwardAlt.normalize();
+ if(up.y==0)
+ {
+ forward=forwardAlt.clone();
+ }
+ }else if(i==2)
+ {
+ forwardAlt=new THREE.Vector3(0,0,-difference[2]);
+ forwardAlt.normalize();
+ if(up.z==0)
+ {
+ forward=forwardAlt.clone();
+ }
+ }
+
+ while (Math.abs(difference[i])>0 && !exit)
+ {
+
+ if(voxelAt(pos.clone().add(forward.clone().add(up))))
+ {
+ if( voxelAt( pos.clone().add( forward.clone().add(up.clone().add(up)) )))
+ {
+ //step there //rotate convex
+ if (logg) console.log("rotate convex");
+ leg1Pos=pos.clone().add( forward.clone().add(up.clone().add(up)));
+ leg2Pos=pos.clone().add(forward.clone().add(up));
+ var temp=up.clone();
+ up= new THREE.Vector3(0,0,0).sub(forward);
+ forward=temp.clone();
+ stepsTemp.push([leg1Pos.clone().multiplyScalar(voxelSpacing),leg2Pos.clone().multiplyScalar(voxelSpacing),up]);
+ difference=getDifference(leg1Pos, targetPos);
+ pos=leg1Pos.clone();
+ }
+ else //if pos+forward+up+up empty
+ {
+ //step there //step up
+ if (logg) console.log("step up");
+ leg2Pos=leg1Pos.clone();
+ leg1Pos=pos.clone().add(forward.clone().add(up));
+ stepsTemp.push([leg1Pos.clone().multiplyScalar(voxelSpacing),leg2Pos.clone().multiplyScalar(voxelSpacing),up]);
+ difference=getDifference(leg1Pos, targetPos);
+ pos=leg1Pos.clone();
+ }
+ }
+ else //if(pos +foward +up) empty
+ {
+ if (voxelAt(pos.clone().add(forward.clone())))
+ {
+ //step there //step forward
+ if (logg) console.log("step forward");
+ leg2Pos=leg1Pos.clone();
+ leg1Pos=pos.clone().add(forward.clone());
+ stepsTemp.push([leg1Pos.clone().multiplyScalar(voxelSpacing),leg2Pos.clone().multiplyScalar(voxelSpacing),up]);
+ difference=getDifference(leg1Pos, targetPos);
+ pos=leg1Pos.clone();
+ }
+ else //if pos+forward empty
+ {
+ if(voxelAt(pos.clone().add(forward.clone().sub(up))))
+ {
+ //step there //step down
+ if (logg) console.log("step down");
+ leg2Pos=leg1Pos.clone();
+ leg1Pos=pos.clone().add(forward.clone().sub(up));
+ stepsTemp.push([leg1Pos.clone().multiplyScalar(voxelSpacing),leg2Pos.clone().multiplyScalar(voxelSpacing),up]);
+ difference=getDifference(leg1Pos, targetPos);
+ pos=leg1Pos.clone();
+ }
+ else if (voxelAt(pos.clone().sub(up))) //pos-up full
+ {
+ if (logg) console.log("rotate concave");
+ leg2Pos=leg1Pos.clone();
+ leg1Pos=pos.clone().sub(up);
+ var temp=up.clone();
+ up= forward.clone();
+ forward=new THREE.Vector3(0,0,0).sub(temp);
+ stepsTemp.push([leg1Pos.clone().multiplyScalar(voxelSpacing),leg2Pos.clone().multiplyScalar(voxelSpacing),up]);
+ difference=getDifference(leg1Pos, targetPos);
+ pos=leg1Pos.clone();
+ }
+ else
+ {
+ if (logg) console.log("fail");
+ exit=true;
+ }
+ }
+ }
+
+ if( previousDifference<0 &&difference[i]<=0 )
+ {
+
+ if(Math.abs(previousDifference)<Math.abs(difference[i]))
+ {
+ exit=true;
+ if (logg) console.log("exit");
+ }
+
+ }
+ else
+ {
+ if(previousDifference<Math.abs(difference[i]))
+ {
+ exit=true;
+ if (logg) console.log("exit");
+ }
+
+ }
+ previousDifference=difference[i];
+ if(stepsTemp.length>stepsCap)//cap number of steps in case of failuer // change later based on complexity of the model
+ {
+ exit=true;
+ if (logg) console.log("Too many steps,choose another alternative");
+ }
+
+ }
+
+
+ }
+ if(stepsTemp.length>stepsCap)//cap number of steps in case of failuer // change later based on complexity of the model
+ {
+ difference[0]=0;
+ difference[1]=0;
+ difference[2]=0;
+ if (logg) console.log("Too many steps,choose another alternative");
+ }
+
+ }
+ return [leg1Pos, leg2Pos, up,forward,stepsTemp];
+
+}
+
+function voxelAt(location){
+ if(location.x<0||location.y<0||location.z<0||location.x>=gridSize||location.y>=gridSize||location.z>=gridSize)
+ {
+ return false;
+ }
+ return grid[location.x][location.y][location.z];
+}
+
+function getDifference(location, targetPos){
+ var diff=[];
+ diff.push(location.x-targetPos.x/voxelSpacing);//0
+ diff.push(location.y-targetPos.y/voxelSpacing);//1
+ diff.push(location.z-targetPos.z/voxelSpacing);//2
+ return diff.slice();
+}
+
+function getNeighbours( targetPos){
+ var n=[];
+ var t=targetPos.clone().multiplyScalar(1/voxelSpacing);
+ // for(var i=-1;i<2;i++)
+ // {
+ // for(var j=-1;j<2;j++)
+ // {
+ // for(var k=-1;k<2;k++)
+ // {
+ // var tt=new THREE.Vector3(t.x+i,t.y+j,t.z+k);
+ // if(!(tt.equals(t)) && voxelAt(tt) )
+ // {
+ // n.push(new THREE.Vector3(tt*voxelSpacing,tt*voxelSpacing,tt*voxelSpacing));
+ // }
+ // }
+ // }
+ // }
+ var tt=new THREE.Vector3(t.x+1,t.y,t.z);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ tt=new THREE.Vector3(t.x-1,t.y,t.z);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ tt=new THREE.Vector3(t.x,t.y+1,t.z);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ tt=new THREE.Vector3(t.x,t.y-1,t.z);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ tt=new THREE.Vector3(t.x,t.y,t.z+1);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ tt=new THREE.Vector3(t.x,t.y,t.z-1);
+ if(voxelAt(tt))
+ {
+ n.push(new THREE.Vector3(tt.x*voxelSpacing,tt.y*voxelSpacing,tt.z*voxelSpacing));
+ }
+ return n;
+}
+
+//////////////////////////utilities////////////////////////////
+function ClosestPointOnLine(s ,d ,vPoint ){
+ var tempvPoint=new THREE.Vector3(0,0,0);
+ tempvPoint.copy(vPoint);
+ var tempD=new THREE.Vector3(0,0,0);
+ tempD.copy(d);
+ var temps=new THREE.Vector3(0,0,0);
+ temps.copy(d);
+
+
+ var vVector1 = tempvPoint.sub(temps) ;
+
+ var vVector2 = tempD.normalize();
+
+ var t = vVector2.dot( vVector1);
+
+ if (t <= 0)
+ {
+ return s;
+ }
+
+ var vVector3 = vVector2.multiplyScalar(t) ;
+
+ var vClosestPoint = temps.add(vVector3);
+
+ return vClosestPoint;
+}
+
+function buildHelperMeshes(robotIndex){
+ var material = new THREE.MeshLambertMaterial({ color:0xff7171,});
+ var geometry = new THREE.SphereGeometry(0.5, 0.5, 0.5);
+ targetPositionMesh[robotIndex] = new THREE.Mesh(geometry, material);
+ targetPositionMesh[robotIndex].scale.x=0.8;
+ targetPositionMesh[robotIndex].scale.y=0.8;
+ targetPositionMesh[robotIndex].scale.z=0.8;
+ scene.add(targetPositionMesh[robotIndex]);
+
+ for (var count=0; count<startLocations.length;count++)
+ {
+ geometry = new THREE.BoxGeometry(voxelSpacing*1.1, voxelSpacing*1.1, voxelSpacing*1.1);
+ mesh = new THREE.Mesh(geometry, material);
+ mesh.position.x=startLocations[count].x;
+ mesh.position.y=startLocations[count].y;
+ mesh.position.z=startLocations[count].z;
+ scene.add(mesh);
+ }
+
+}
+
+function showTargetPosition(robotIndex,targetPos,show){
+ if(show)
+ {
+
+ targetPositionMesh[robotIndex].position.x=targetPos.x;
+ targetPositionMesh[robotIndex].position.y=targetPos.y;
+ targetPositionMesh[robotIndex].position.z=targetPos.z+voxelSpacing/2;
+ carriedVoxel[robotIndex].visible=true;
+ targetPositionMesh[robotIndex].visible=true;
+
+ }else
+ {
+ carriedVoxel[robotIndex].visible=false;
+ targetPositionMesh[robotIndex].visible=false;
+
+
+ }
+
+}
+
+
+
+
+
+
diff --git a/01_Code/physical_computing_interface/globals.js b/01_Code/physical_computing_interface/globals.js
index 20982711ac7710d0a77941c7f073396a51309e91..699946c5910f4cb3da6be801896fbf4658faff0c 100644
--- a/01_Code/physical_computing_interface/globals.js
+++ b/01_Code/physical_computing_interface/globals.js
@@ -89,6 +89,7 @@ globals.prototype.selectNode=function (x,y,z){
//////////////////////utils//////////////////
function utilities(){
};
+
utilities.prototype.getXYZfromName=function(name){
var nums=name.match(/\d+/g);
var x=nums[0];
@@ -96,6 +97,7 @@ utilities.prototype.getXYZfromName=function(name){
var z=nums[2];
return new THREE.Vector3(x,y,z);
}
+
utilities.prototype.getNeighboursList=function(grid,x,y,z){
var list=[];
for(var i=0;i<grid.neighbors.length;i++){
diff --git a/01_Code/physical_computing_interface/threejs/grid.html b/01_Code/physical_computing_interface/threejs/grid.html
index 86908dc8cb6e0863e9288aa8939873891024f380..f48f738825e0661927a43b04313e156bbd4b0894 100644
--- a/01_Code/physical_computing_interface/threejs/grid.html
+++ b/01_Code/physical_computing_interface/threejs/grid.html
@@ -12,7 +12,7 @@
<script src="../lib/OrbitControls.js"></script>
<!-- code -->
-<script src="./grid1.js"></script>
+<script src="./StandAloneGrid.js"></script>
</body>
</html>
\ No newline at end of file