add 3d multi end

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
     "cSpell.words": [
-        "FABRIK"
+        "FABRIK",
+        "quats"
     ]
 }

src/3d/MultiEnd.scss

@@ -0,0 +1,39 @@
+.multi-end {
+    width: 100vw;
+    height: 100vh;
+    position: relative;
+
+    .overlay {
+        position: absolute;
+        top: 0;
+        left: 0;
+        display: flex;
+        justify-content: flex-start;
+        align-self: start;
+
+        .overlay-area {
+            margin: 8px;
+            padding: 8px 8px 32px 8px;
+            background-color: #f5f5f5;
+            border: 1px solid #e0e0e0;
+            border-radius: 10px;
+
+            .iteration {
+                pointer-events: none;
+                -webkit-user-select: none;
+                /* Safari */
+                -ms-user-select: none;
+                /* IE 10 and IE 11 */
+                user-select: none;
+                /* Standard syntax */
+            }
+        }
+    }
+
+    .canvas {
+        width: 100vw;
+        height: 100vh;
+        background-color: #f0f0f0;
+        border-radius: 10px;
+    }
+}

src/3d/MultiEnd.tsx

@@ -0,0 +1,121 @@
+import Camera from './Camera';
+import Bone from './character/bone';
+import XYZValue from './value/xyz_value';
+import { Canvas } from '@react-three/fiber';
+import { Component, ReactNode } from 'react';
+import MultiEndFABRIK from './fabrik/multi-end-fabrik';
+
+import './MultiEnd.scss';
+
+import BoneVisualizer from './visualizer/BoneVisualizer';
+import TargetVisualizer from './visualizer/TargetVisualizer';
+
+interface MultiEndState {
+    isLoading: boolean;
+    cameraEnabled: boolean;
+    fabrikIteration: number;
+    root: Bone;
+}
+
+class MultiEnd extends Component<any, MultiEndState> {
+
+    private _fabrik: MultiEndFABRIK = new MultiEndFABRIK();
+
+    private _target1: XYZValue = new XYZValue(0, 0, 0);
+    private _target2: XYZValue = new XYZValue(0, 0, 0);
+
+    constructor(props: any) {
+        super(props);
+        this.state = {
+            isLoading: true,
+            cameraEnabled: true,
+            fabrikIteration: 0,
+            root: new Bone('root', new XYZValue(0, 0, 0), new XYZValue(0, 0, 0))
+        }
+    }
+
+    componentDidMount(): void {
+        const bone1 = new Bone('bone1', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        const bone2 = new Bone('bone2', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        const bone3 = new Bone('bone3', new XYZValue(40, 40, 0), new XYZValue(0, 0, 0));
+        const bone4 = new Bone('bone4', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        const bone5 = new Bone('bone5', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        const bone6 = new Bone('bone6', new XYZValue(-40, 40, 0), new XYZValue(0, 0, 0));
+        const bone7 = new Bone('bone7', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        const bone8 = new Bone('bone8', new XYZValue(0, 60, 0), new XYZValue(0, 0, 0));
+        bone7.addChild(bone8);
+        bone6.addChild(bone7);
+        bone4.addChild(bone5);
+        bone3.addChild(bone4);
+        bone2.addChild(bone6);
+        bone2.addChild(bone3);
+        bone1.addChild(bone2);
+        const root = this.state.root;
+        root.addChild(bone1);
+        this._target1 = bone5.world[0];
+        this._target2 = bone8.world[0];
+        this.setState({
+            root: root,
+            isLoading: false
+        });
+    }
+
+    private _onTarget1Move = (pos: XYZValue): void => {
+        this._target1 = pos;
+        const root = this.state.root;
+        const iteration = this._fabrik.resolve(root, [this._target1, this._target2], ['bone5', 'bone8']);
+        this.setState({
+            root: root,
+            fabrikIteration: iteration
+        });
+    }
+
+    private _onTarget2Move = (pos: XYZValue): void => {
+        this._target2 = pos;
+        const root = this.state.root;
+        const iteration = this._fabrik.resolve(root, [this._target1, this._target2], ['bone5', 'bone8']);
+        this.setState({
+            root: root,
+            fabrikIteration: iteration
+        });
+    }
+
+    render(): ReactNode {
+        if (this.state.isLoading) {
+            return <div>Loading...</div>;
+        }
+        return (
+            <div className="multi-end">
+                <div className="overlay">
+                    <div className="overlay-area">
+                        <span className="iteration">iteration: {this.state.fabrikIteration}</span>
+                    </div>
+                </div>
+                <div className="canvas">
+                    <Canvas>
+                        <Camera disabled={!this.state.cameraEnabled} />
+                        <ambientLight color="#0f0f0f" intensity={1} />
+                        <hemisphereLight color="#ffffff" groundColor="#0f0f0f" intensity={1} />
+                        <gridHelper args={[10000, 1000]} />
+                        <axesHelper args={[100]} />
+                        <BoneVisualizer root={this.state.root} />
+                        <TargetVisualizer
+                            initPos={this._target1}
+                            onTargetMove={this._onTarget1Move}
+                            onDragStart={() => this.setState({ cameraEnabled: false })}
+                            onDragEnd={() => this.setState({ cameraEnabled: true })}
+                        />
+                        <TargetVisualizer
+                            initPos={this._target2}
+                            onTargetMove={this._onTarget2Move}
+                            onDragStart={() => this.setState({ cameraEnabled: false })}
+                            onDragEnd={() => this.setState({ cameraEnabled: true })}
+                        />
+                    </Canvas>
+                </div>
+            </div>
+        )
+    }
+}
+
+export default MultiEnd;

src/3d/SingleEnd.tsx

@@ -29,7 +29,7 @@ class SingleEnd extends Component<any, SingleEndState> {
             isLoading: true,
             cameraEnabled: true,
             fabrikIteration: 0,
-            root: new Bone('root', new XYZValue(0, 0, 0), new XYZValue(4.909412733568189, -0.04645663367750785, 2.9342290124949955))
+            root: new Bone('root', new XYZValue(0, 0, 0), new XYZValue(0, 0, 0))
         }
     }
 

src/3d/fabrik/multi-end-fabrik.ts

@@ -0,0 +1,98 @@
+import Bone from '../character/bone';
+import PointGroup from '../value/point_group';
+import Quat, { applyQuatRotation, averageQuat, xyzRotation } from '../value/quat';
+import XYZValue, { moveXYZAlone, subXYZ } from '../value/xyz_value';
+
+class MultiEndFABRIK {
+    private static MAX_ITERATIONS = 20;
+    private static TOLERANCE = 0.1;
+
+    public resolve(root: Bone, poss: XYZValue[], endNames: string[]): number {
+        for (let i = 0; i < MultiEndFABRIK.MAX_ITERATIONS; i++) {
+            const worlds: Map<string, XYZValue> = new Map();
+            const forwardWorlds: Map<string, PointGroup> = new Map();
+            { // Initialize
+                for (const bone of root) {
+                    worlds.set(bone.name, bone.world[0]);
+                    forwardWorlds.set(bone.name, new PointGroup());
+                }
+            }
+            { // Forward
+                const queue: Bone[] = [];
+                const visited: Set<string> = new Set();
+                for (let i = 0; i < endNames.length; i++) {
+                    queue.push(root.find(endNames[i])!);
+                    forwardWorlds.get(endNames[i])!.addPoint(poss[i]);
+                }
+                let current: Bone | undefined = queue.shift();
+                while (current) {
+                    if (!current.parent) { // skip root
+                        visited.add(current.name);
+                        current = queue.shift();
+                        continue;
+                    }
+                    if (visited.has(current.name)) { // skip visited
+                        current = queue.shift();
+                        continue;
+                    }
+                    let allChildrenVisited = true;
+                    for (const child of current.children) {
+                        if (!visited.has(child.name)) {
+                            allChildrenVisited = false;
+                            break;
+                        }
+                    }
+                    // if one of the children is not visited, push current back
+                    // to the queue and continue
+                    if (!allChildrenVisited) {
+                        queue.push(current);
+                        current = queue.shift();
+                        continue;
+                    }
+                    visited.add(current!.name);
+                    // forward process
+                    const child = forwardWorlds.get(current.name)!;
+                    const parent = worlds.get(current.parent.name)!;
+                    const length = current.pos.length();
+                    const pg = forwardWorlds.get(current.parent.name)!;
+                    let newWorld = moveXYZAlone(child.centroid, parent, length);
+                    pg.addPoint(newWorld);
+                    forwardWorlds.set(current.parent.name, pg);
+                    queue.push(current.parent);
+                    current = queue.shift();
+                }
+            }
+            { // Backward
+                for (const bone of root) {
+                    if (bone.children.length === 0) {
+                        continue;
+                    }
+                    const quats: Quat[] = [];
+                    const [bw, br] = bone.world;
+                    for (const child of bone.children) {
+                        const v1 = subXYZ(child.world[0], bw);
+                        const v2 = subXYZ(forwardWorlds.get(child.name)!.centroid, bw);
+                        const quat = xyzRotation(v1, v2)
+                        quats.push(quat);
+                    }
+                    const avg = averageQuat(quats);
+                    bone.rotation = applyQuatRotation(br, bone.rotation, avg);
+                }
+            }
+            // Early termination when the target is reached
+            let allReached = true;
+            for (let i = 0; i < endNames.length; i++) {
+                if (subXYZ(root.find(endNames[i])!.world[0], poss[i]).length() > MultiEndFABRIK.TOLERANCE) {
+                    allReached = false;
+                    break;
+                }
+            }
+            if (allReached) {
+                return i + 1;
+            }
+        }
+        return MultiEndFABRIK.MAX_ITERATIONS;
+    }
+}
+
+export default MultiEndFABRIK;

src/3d/value/point_group.ts

@@ -0,0 +1,31 @@
+import XYZValue from './xyz_value';
+
+class PointGroup {
+    private _points: XYZValue[] = [];
+    private _centroid = { x: 0, y: 0, z: 0 };
+
+    public get centroid(): XYZValue {
+        return new XYZValue(this._centroid.x, this._centroid.y, this._centroid.z);
+    }
+
+    constructor() { }
+
+    public addPoint(p: XYZValue): void {
+        this._points.push(p);
+        this._updateCentroid();
+    }
+
+    private _updateCentroid(): void {
+        let x = 0;
+        let y = 0;
+        let z = 0;
+        for (const p of this._points) {
+            x += p.x;
+            y += p.y;
+            z += p.z;
+        }
+        this._centroid = { x: x / this._points.length, y: y / this._points.length, z: z / this._points.length };
+    }
+}
+
+export default PointGroup;

src/3d/value/quat.ts

@@ -36,6 +36,10 @@ class Quat {
     public normalize(): Quat {
         const len = Math.hypot(this._w, this._x, this._y, this._z);
 
+        if (len === 1) {
+            return this;
+        }
+
         if (len > 0) {
             const invLen = 1 / len;
             return new Quat(this._w * invLen, this._x * invLen, this._y * invLen, this._z * invLen);
@@ -112,12 +116,27 @@ const xyzRotation = (a: XYZValue, b: XYZValue): Quat => {
 
 // gr: global rotation, r: local rotation, q: quat
 const applyQuatRotation = (gr: Quat, r: XYZValue, q: Quat): XYZValue => {
+    // local rotation to quaternion
     const qr = quatFromDegree(r);
-    const pr = mulQuat(gr, qr.inverse())
+    // extract parent's global rotation from global rotation
+    const pr = mulQuat(gr, qr.inverse());
+    // apply new rotation to global rotation and convert back to local rotation
     const nr = mulQuat(pr.inverse(), mulQuat(q, gr));
     return nr.toEuler();
 }
 
+const averageQuat = (quats: Quat[]): Quat => {
+    let [aw, ax, ay, az] = [0, 0, 0, 0];
+    for (let i = 0; i < quats.length; i++) {
+        aw += quats[i].w;
+        ax += quats[i].x;
+        ay += quats[i].y;
+        az += quats[i].z;
+    }
+    const avg = new Quat(aw / quats.length, ax / quats.length, ay / quats.length, az / quats.length);
+    return avg.normalize();
+}
+
 const quatFromDegree = (value: XYZValue): Quat => {
     // degree to radian
     const roll = value.x * (Math.PI / 180);
@@ -152,5 +171,6 @@ export {
     xyzRotation,
     applyQuatRotation,
     quatFromDegree,
-    transferQuat
+    transferQuat,
+    averageQuat
 }

src/router/Router.tsx

@@ -11,6 +11,7 @@ const MultiEnd2D = withSuspense(lazy(() => import('../2d/MultiEnd')));
 import ThreeDimension from '../3d/index';
 
 const SingleEnd3D = withSuspense(lazy(() => import('../3d/SingleEnd')));
+const MultiEnd3D = withSuspense(lazy(() => import('../3d/MultiEnd')));
 
 class Router extends Component {
     render(): ReactNode {
@@ -23,6 +24,7 @@ class Router extends Component {
                     </Route>
                     <Route path="3d" element={<ThreeDimension />}>
                         <Route path="single-end" element={<SingleEnd3D />} />
+                        <Route path="multi-end" element={<MultiEnd3D />} />
                     </Route>
                 </Routes>
             </BrowserRouter>