463 lines
16 KiB
JavaScript
463 lines
16 KiB
JavaScript
|
define((require, exports, module) => {
|
||
|
const Serial = {
|
||
|
println(text) {
|
||
|
// // console.log(text)
|
||
|
},
|
||
|
}
|
||
|
|
||
|
const arrows = {}
|
||
|
|
||
|
function addArrow(name, from, to, color = 0xffff00, length = 10) {
|
||
|
if (arrows.hasOwnProperty(name)) {
|
||
|
window.debug.scene.remove(arrows[name])
|
||
|
}
|
||
|
if (!window.debug.show) 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)
|
||
|
}
|
||
|
const spheres = {}
|
||
|
|
||
|
function addSphere(name, position, color = 0xffff00, diameter = 1) {
|
||
|
if (spheres.hasOwnProperty(name)) {
|
||
|
window.debug.scene.remove(spheres[name])
|
||
|
}
|
||
|
if (!window.debug.show) 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])
|
||
|
|
||
|
|
||
|
// ---- 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]
|
||
|
addSphere('J4', J[4])
|
||
|
|
||
|
|
||
|
// ---- 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
|
||
|
|
||
|
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')
|
||
|
}
|
||
|
|
||
|
|
||
|
// ---- J1 ----
|
||
|
// # R0
|
||
|
|
||
|
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)
|
||
|
|
||
|
|
||
|
// ---- rotate J4 into x,z plane ----
|
||
|
// # J4 R0
|
||
|
|
||
|
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)
|
||
|
|
||
|
|
||
|
// ---- 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
|
||
|
|
||
|
|
||
|
// ---- 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
|
||
|
|
||
|
|
||
|
// ---- 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]
|
||
|
|
||
|
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[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)
|
||
|
|
||
|
|
||
|
// ---- J4J3 J4J5 ----
|
||
|
// # J3 J4 J5
|
||
|
|
||
|
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]]
|
||
|
|
||
|
|
||
|
// ---- R3 ----
|
||
|
// # J3 J4 J5
|
||
|
|
||
|
const J4J5_J4J3_normal_vector = this.cross(J4J5_vector, J4J3_vector)
|
||
|
|
||
|
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,
|
||
|
]
|
||
|
|
||
|
const ZY_aligned_J4J3_normal_vector = this.cross(ZY_parallel_aligned_vector, J4J3_vector)
|
||
|
|
||
|
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[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
|
||
|
|
||
|
|
||
|
// ---- 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]
|
||
|
}
|
||
|
|
||
|
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]
|
||
|
}
|
||
|
|
||
|
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
|
||
|
}
|
||
|
|
||
|
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
|
||
|
}
|
||
|
|
||
|
dot(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
|
||
|
*/
|
||
|
angleBetween(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
|
||
|
}
|
||
|
|
||
|
length3(vector) {
|
||
|
return Math.sqrt(Math.pow(vector[0], 2) + Math.pow(vector[1], 2) + Math.pow(vector[2], 2))
|
||
|
}
|
||
|
|
||
|
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
|
||
|
}
|
||
|
}
|
||
|
|
||
|
function kinematic() {
|
||
|
return InverseKinematic
|
||
|
}
|
||
|
|
||
|
module.exports = InverseKinematic
|
||
|
})
|