class CCDRobotArm{
constructor(ThreeEnv, fixedPoint, segments) {
this.env = ThreeEnv;
this.boxGeometry = new THREE.BoxBufferGeometry(1, 1, 1);
this.color = new THREE.MeshLambertMaterial({ color: 0x888888 });
this.fixedPoint = fixedPoint;
this.joints = [];
for (let i = 0; i < segments.length; ++i) {
const base = this.joints.length > 0 ? this.joints[this.joints.length - 1]: this.env.scene;
this.addJoint(base, segments[i].pos, segments[i].axis, segments[i].limits, segments[i].boxSize, segments[i].boxPos);
}
this.endEffector = new THREE.Group();
this.joints[this.joints.length - 1].add(this.endEffector);
this.endEffector.position.set(0,1,0);
}
addJoint(base, pos, axis, limits, sizes, boxPos){
let joint = new THREE.Group();
this.base.add(joint);
joint.position.set(pos.x, pos.y, pos.z);
joint.axis = new THREE.Vector3(axis.x, axis.y, axis.z);
joint.minLimit = -1 * limits[0];
joint.maxLimit = limits[1];
this.joints.push(joint);
let box = new THREE.Mesh(this.boxGeometry, this.color);
box.scale.set(sizes.x, sizes.y, sizes.z);
box.position.set(boxPos.x, boxPos.y, boxPos.z);
box.castShadow = true;
joint.add(box);
return joint;
}
CCDIK(targetPos){
let tipPos = new THREE.Vector3();
let rotation = new THREE.Quaternion();
let quaternionDiff = new THREE.Quaternion();
for (let i = this.joints.length - 1; i > -1; --i) {
this.joints[i].updateMatrixWorld();
this.endEffector.getWorldPosition(tipPos);
let direction = this.joints[i].worldToLocal(tipPos.clone()).normalize();
let targetDirection = this.joints[i].worldToLocal(targetPos.clone()).normalize();
quaternionDiff.setFromUnitVectors(direction, targetDirection);
this.joints[i].quaternion.multiply(quaternionDiff);
let inversedQuaternion = this.joints[i].quaternion.inverse();
let parentAxis = this.joints[i].axis.clone.applyQuaternion(inversedQuaternion);
quaternionDiff.setFromUnitVectors(this.joints[i].axis, parentAxis);
this.joints[i].quaternion.multiply(quaternionDiff);
let clampedRotation = this.joints[i].rotation.toVector3().clampScalar(this.joints[i].minLimit, this.joints[i].maxLimit);
this.joints[i].rotation.setFromVector3(clampedRotation);
this.joints[i].updateMatrixWorld();
}
}
}