segway kinematics

xy version

by jmchen

HTML

<div id="info">Segway kinematics
    <br/>[W/S][I/K]</div>
<div id="speed"></div>
<script src="http://cdnjs.cloudflare.com/ajax/libs/three.js/r70/three.min.js"></script>
<script src="http://jyunming-chen.github.io/tutsplus/js/KeyboardState.js"></script>

CSS

#info {
    position: absolute;
    top: 0px;
    width: 100%;
    padding: 10px;
    text-align: center;
    color: #ffff00
}
#speed {
    position: absolute;
    bottom: 100px;
    width: 100%;
    color: #ffffff;
    text-align: center;
}
}

JavaScript

var keyboard = new KeyboardState();
var camera, scene, renderer;
var pa = new THREE.Vector3(0, 20, 0);
var pb = new THREE.Vector3(0, -20, 0);
var pc = new THREE.Vector3(0,0,0);
var sa = 2;
var sb = 4;
var pointA, pointB, car;
var speedInfo;
var k = 0;

init();
animate();

function update(dt) {
    var center = new THREE.Vector3();
    var tmp = new THREE.Vector3();
    var l = tmp.subVectors(pa, pb).length();

    k = l * sa / (sb - sa);
    var omega = (sb - sa) / l;
    //if (omega === 0.0) {
    if (Math.abs(sa-sb) < 1e-5) {    // this gives better result (no jerk)
    tmp.subVectors(pa, pb).normalize().cross(new THREE.Vector3(0, 0, 1)).multiplyScalar(sa * dt);
        pa.add(tmp);
        pb.add(tmp);
        pointA.position.copy(pa);
        pointB.position.copy(pb);
        return; // do a pure translation then return
    }

    tmp.subVectors(pa, pb).normalize().multiplyScalar(k);
    center.copy(tmp.add(pa));

    tmp.subVectors(pa, center)
        .applyAxisAngle(new THREE.Vector3(0, 0, 1), omega * dt)
        .add(center);
    pa.copy(tmp);

    tmp.subVectors(pb, center)
        .applyAxisAngle(new THREE.Vector3(0, 0, 1), omega * dt)
        .add(center);
    pb.copy(tmp);

    // normalize ...
    //tmp.subVectors (pb,pa).normalize().multiplyScalar(l);
    //pb.addVectors(tmp,pa);
    
    pointA.position.copy(pa);
    pointB.position.copy(pb);
    pointC.position.copy (center);
}

function init() {

    speedInfo = window.document.getElementById('speed');
    scene = new THREE.Scene();

    camera = new THREE.OrthographicCamera(-100, 100, 100, -100, -10, 10);
    camera.position.z = 5;
    scene.add(camera);

    var gridXZ = new THREE.GridHelper(100, 5);
    gridXZ.setColors(new THREE.Color(0xff0000), new THREE.Color(0xffffff));
    gridXZ.rotation.x = Math.PI / 2;
    scene.add(gridXZ);

    renderer = new THREE.WebGLRenderer();
    renderer.setSize(window.innerWidth, window.innerHeight);
    renderer.setClearColor(0x888888);

   ...