CCD (ccdbox)

detailed example

by jmcjc5u

HTML

<div id="info">
  IK of Two-Link Arm
  <br>
  <br> ( generic CCD Solver )
</div>

<script src="https://cdnjs.cloudflare.com/ajax/libs/three.js/84/three.min.js"></script>
<script src="https://threejs.org/examples/js/controls/OrbitControls.js"></script>
<!--script src="https://rawgit.com/jyunming-chen/game3js/master/js/ccdbox.js"></script-->
<!--script src="https://cdn.jsdelivr.net/gh/jyunming-chen/game3js@13a05103eb51d913dd7815939bf7ad45690e8dc9/js/ccdbox.js"></script-->
<script src="https://jyunming-chen.github.io/game3js/js/ccdsys.js"></script>

CSS

body {
	  background-color: #fff;
	  color: #111;
	  margin: 0px;
	  overflow: hidden;
	font-family: Monospace;
	  font-size: 20px;
	}
	
	#info {
	  position: absolute;
	  top: 0px;
	  width: 100%;
	  padding: 5px;
	  text-align: center;
	  color: #ffff00
	}
	
	a {
	  color: #00ffff
	}
	
	strong {
	  color: red
	}
	
	#container {
	  z-index: 0;
	  left: 0px;
	  top: 0px;
	  overflow: hidden;
	  position: absolute;
	  width: 100%;
	  height: 100%;
	}

JavaScript

/*
//////////////////////////////////
/// HELPER FUNCTIONS
// p: the vector to be projected
// n: the normal defining the projection plane (unit vector)
// clarification: call by reference/pointer or call-by-value
function proj2plane(p, n) {
	return p.clone().projectOnPlane(n);
}

function CLAMP(x, xLo, xHi) {
	return x < xLo ? xLo : ( x > xHi ? xHi : x) ;
}


class CCDSys {
	constructor ( fkFunc ) {
		this.axes = [];
		this.fkFunc = fkFunc
	}
	
  	setCCDAxis (vec, id, angleLo, angleHi) {
  	  let CCD_axis = {axis: vec.clone(), jointid: id};
  	  let thetaLo = angleLo || -1e4 // default: no limits
 	  let thetaHi = angleHi || 1e4 
 	  CCD_axis.limits = new THREE.Vector2 (thetaLo, thetaHi)
 	  
 	  this.axes.push (CCD_axis)
  	}
  
	solve ( target, thetas ) { // (NY) in case base is changing ...
		
		// local variable for iterations
		let end = new THREE.Vector3();
    	let base = new THREE.Vector3();

		// short hand 
		let axes = this.axes;
    
    	// e.g., njoints = 2;
    	// jointid: 0,0,1
		var njoints = axes[axes.length - 1].jointid + 1;
		var joints = [];
		for (var i = 0; i <= njoints; i++) joints[i] = new THREE.Vector3();

		this.fkFunc (thetas, joints);
		end.copy(joints[joints.length - 1]);

		// convergence
		const EPS = 1e-1;
		const MAXITER = 20;

		let t_target = new THREE.Vector3();
		let t_end = new THREE.Vector3();
		let tmpV = new THREE.Vector3();

		// iterations

		for (var iter = 0; iter < MAXITER; iter++) {
		  for (var i = axes.length - 1; i >= 0; i--) {
			base.copy(joints[axes[i].jointid]);

			// this part is quite different from the C counterpart
			var axis = axes[i].axis.clone();
			for (var j = i - 1; j >= 0; j--)
			  axis.applyMatrix4(new THREE.Matrix4().makeRotationAxis(axes[j].axis, thetas[j]));

			// after this manipulation,
			// axis become world coordinate

			tmpV.subVectors(target, base);
			tmpV = proj2plane(tmpV, axis);
			t_target.copy(tmpV.normalize());

			tmpV.subVectors(end, base);
			tmpV = proj2plane(tmpV,...