/** * @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 */ static angleBetween(vectorA, vectorB, referenceVector) { // angle = atan2(norm(cross(a, b)), dot(a, b)) const norm = Kinematics.length3(Kinematics.cross(vectorA, vectorB)) const angle = Math.atan2(norm, (this.dot(vectorA, vectorB))) const tmp = referenceVector[0] * vectorA[0] + referenceVector[1] * vectorA[1] + referenceVector[2] * vectorA[2] const sign = (tmp > 0) ? 1.0 : -1.0 return angle * sign }
const J4J5_J4J3_normal_vector = Kinematics.cross(J4J5_vector, J4J3_vector) const XZ_parallel_aligned_vector = [ 10 * Math.cos(R[0] + (Math.PI / 2)), const reference = Kinematics.cross(XZ_parallel_aligned_vector, J4J3_vector) const referenceVector = Kinematics.cross(J4J3_vector, J4J5_J4J3_normal_vector) R[5] -= Kinematics.angleBetween(J4J5_J4J3_normal_vector, targetVectorY, Kinematics.cross(targetVectorY, targetVectorX))