JSFiddle - React, Tailwind, and code Playground

by motioncatch

HTML

<script src="https://cdn.jsdelivr.net/npm/[email protected]/quaternion.min.js"></script>
<script src="https://cdnjs.cloudflare.com/ajax/libs/mathjs/9.3.2/math.min.js"></script>

JavaScript

function rot_axis_angles_zxy(r_prox, r_dist) {

	r_prox_transposed = math.transpose(r_prox);
  
  joint = math.multiply(r_prox_transposed, r_dist);
  
  rx_1 = math.asin(joint[2][1]);
  rx_2 = math.pi - rx_1;
  
  rz_1 = math.atan2(-joint[0][1] / math.cos(rx_1), joint[1][1] / math.cos(rx_1));
  rz_2 = math.atan2(-joint[0][1] / math.cos(rx_2), joint[1][1] / math.cos(rx_2));
  
  ry_1 = math.atan2(-joint[2][0] / math.cos(rx_1), joint[2][2] / math.cos(rx_1));
  ry_2 = math.atan2(-joint[2][0] / math.cos(rx_2), joint[2][2] / math.cos(rx_2));
  
  rot_axis_angles = math.transpose([[rz_1],[rz_2],[rx_1],[rx_2],[ry_1],[ry_2]]);
  
  return rot_axis_angles
}

function angle_diff(theta1, theta2) {
	
  theta_tmp1 = theta1;
  theta_tmp2 = theta2;

	if (theta_tmp1 >= 2 * math.pi) {
  	while (theta_tmp1 >= 2 * math.pi) {
    	theta_tmp1 = theta_tmp1 - (2 * math.pi);
    }
  }
  if (theta_tmp1 < 0) {
  	while (theta_tmp1 < 0) {
    	theta_tmp1 = theta_tmp1 + (2 * math.pi);
    }
  }
  if (theta_tmp2 >= 2 * math.pi) {
  	while (theta_tmp2 >= 2 * math.pi) {
    	theta_tmp2 = theta_tmp2 - (2 * math.pi);
    }
  }
  if (theta_tmp2 < 0) {
  	while (theta_tmp2 < 0) {
    	theta_tmp2 = theta_tmp2 + (2 * math.pi);
    }
  }
  
  val = math.abs(theta_tmp2 - theta_tmp1);
  val2 = 2 * math.pi - val;
  if (val < val2) {
  	return val
  } else {
  	return val2
  }
  
}

function adjust_angle_to_val(prev, theta) {
  result = theta;
  while (math.abs(result - prev) > math.abs(result - prev + (2 * math.pi))) {
  	result = result + (2 * math.pi);
  }
  
  while (math.abs(result - prev) > math.abs(result - prev - (2 * math.pi))) {
  	result = result - (2 * math.pi);
  }
  
  return result

}

function cal_current_angle(prev, theta) {
	
  val = 0
  norm1 = 0
  norm2 = 0
	var result_curr = [];

  val = angle_diff(prev[0], theta[0]);
    
  norm1 = val * val;
  val = angle_diff(prev[1], theta[2]);
  norm1 = norm1 + val * val;
  val = angle_diff(prev[2], theta[4]);
  norm1 = norm1 + val * val;

  val...