From d7d7ff308fecd2346a68d206b036002fdca8654a Mon Sep 17 00:00:00 2001 From: Yorick van Pelt Date: Mon, 27 Nov 2017 03:05:27 +0100 Subject: [PATCH] initial rewrite --- LemmingSim.html | 5 +- bodies/robotbody_longerleft.svg | 65 ++ bodies/robotbody_longerleft_mediumright.svg | 65 ++ bodies/robotbody_longleft.svg | 65 ++ bodies/robotbody_longleft_mediumright.svg | 65 ++ bodies/robotbody_longleft_rabbit.svg | 147 +++ lemming_sim_student.js | 1027 ++++++++----------- mouse.js | 14 +- pathseg.js | 4 +- robotbody.svg | 30 +- sensors.js | 174 ++++ utils.js | 41 + 12 files changed, 1066 insertions(+), 636 deletions(-) create mode 100644 bodies/robotbody_longerleft.svg create mode 100644 bodies/robotbody_longerleft_mediumright.svg create mode 100644 bodies/robotbody_longleft.svg create mode 100644 bodies/robotbody_longleft_mediumright.svg create mode 100644 bodies/robotbody_longleft_rabbit.svg create mode 100644 sensors.js create mode 100644 utils.js diff --git a/LemmingSim.html b/LemmingSim.html index 66f993e..95c31ae 100644 --- a/LemmingSim.html +++ b/LemmingSim.html @@ -20,12 +20,13 @@ along with this program. If not, see . Lemmings - + + - + diff --git a/bodies/robotbody_longerleft.svg b/bodies/robotbody_longerleft.svg new file mode 100644 index 0000000..db17d89 --- /dev/null +++ b/bodies/robotbody_longerleft.svg @@ -0,0 +1,65 @@ + + + +image/svg+xmlimage/svg+xml \ No newline at end of file diff --git a/bodies/robotbody_longerleft_mediumright.svg b/bodies/robotbody_longerleft_mediumright.svg new file mode 100644 index 0000000..5d24868 --- /dev/null +++ b/bodies/robotbody_longerleft_mediumright.svg @@ -0,0 +1,65 @@ + + + +image/svg+xmlimage/svg+xml \ No newline at end of file diff --git a/bodies/robotbody_longleft.svg b/bodies/robotbody_longleft.svg new file mode 100644 index 0000000..ec4f225 --- /dev/null +++ b/bodies/robotbody_longleft.svg @@ -0,0 +1,65 @@ + + + +image/svg+xmlimage/svg+xml \ No newline at end of file diff --git a/bodies/robotbody_longleft_mediumright.svg b/bodies/robotbody_longleft_mediumright.svg new file mode 100644 index 0000000..c601e78 --- /dev/null +++ b/bodies/robotbody_longleft_mediumright.svg @@ -0,0 +1,65 @@ + + + +image/svg+xmlimage/svg+xml \ No newline at end of file diff --git a/bodies/robotbody_longleft_rabbit.svg b/bodies/robotbody_longleft_rabbit.svg new file mode 100644 index 0000000..be4337f --- /dev/null +++ b/bodies/robotbody_longleft_rabbit.svg @@ -0,0 +1,147 @@ + + + +image/svg+xmlimage/svg+xml \ No newline at end of file diff --git a/lemming_sim_student.js b/lemming_sim_student.js index 0849428..25b9d2b 100644 --- a/lemming_sim_student.js +++ b/lemming_sim_student.js @@ -1,7 +1,7 @@ /* Lemmings - robot and GUI script. * * Copyright 2016 Harmen de Weerd - * Copyright 2017 Johannes Keyser, James Cooke, George Kachergis + * Copyright 2017 Johannes Keyser, James Cooke, George Kachergis, Yorick van Pelt * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -16,648 +16,457 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ - -// Description of robot(s), and attached sensor(s) used by InstantiateRobot() -RobotInfo = [ - {body: null, // for MatterJS body, added by InstantiateRobot() - color: [255, 255, 255], // color of the robot shape - init: {x: 50, y: 50, angle: 0}, // initial position and orientation - sensors: [ // define an array of sensors on the robot - // define one sensor - {sense: senseDistance, // function handle, determines type of sensor - minVal: 0, // minimum detectable distance, in pixels - maxVal: 50, // maximum detectable distance, in pixels - attachAngle: Math.PI/4, // where the sensor is mounted on robot body - lookAngle: 0, // direction the sensor is looking (relative to center-out) - id: 'distR', // a unique, arbitrary ID of the sensor, for printing/debugging - color: [150, 0, 0], // sensor color [in RGB], to distinguish them - parent: null, // robot object the sensor is attached to, added by InstantiateRobot - value: null // sensor value, i.e. distance in pixels; updated by sense() function - }, - // define another sensor - {sense: senseDistance, minVal: 0, maxVal: 50, attachAngle: -Math.PI/4, - lookAngle: 0, id: 'distL', color: [0, 150, 0], parent: null, value: null - } - ] - } -]; - +"use strict" // Simulation settings; please change anything that you think makes sense. -simInfo = { - maxSteps: 50000, // maximal number of simulation steps to run - airDrag: 0.1, // "air" friction of enviroment; 0 is vacuum, 0.9 is molasses - boxFric: 0.005, // friction between boxes during collisions - boxMass: 0.01, // mass of boxes - boxSize: 20, // size of the boxes, in pixels - robotSize: 13, // approximate robot radius, in pixels (note the SVG gets scaled down) - robotMass: 0.4, // robot mass (a.u) - gravity: 0, // constant acceleration in Y-direction - bayRobot: null, // currently selected robot - baySensor: null, // currently selected sensor - bayScale: 3, // scale within 2nd, inset canvas showing robot in it's "bay" - doContinue: true, // whether to continue simulation, set in HTML - debugSensors: false, // plot sensor rays and mark detected objects - debugMouse: false, // allow dragging any object with the mouse - engine: null, // MatterJS 2D physics engine - world: null, // world object (composite of all objects in MatterJS engine) - runner: null, // object for running MatterJS engine - height: null, // set in HTML file; height of arena (world canvas), in pixels - width: null, // set in HTML file; width of arena (world canvas), in pixels - curSteps: 0 // increased by simStep() +var simInfo = { + maxSteps: 50000, // maximal number of simulation steps to run + airDrag: 0.1, // "air" friction of enviroment; 0 is vacuum, 0.9 is molasses + boxFric: 0.005, // friction between boxes during collisions + boxMass: 0.01, // mass of boxes + boxSize: 15, // size of the boxes, in pixels + robotSize: 13, // approximate robot radius, in pixels (note the SVG gets scaled down) + robotMass: 0.4, // robot mass (a.u) + gravity: 0, // constant acceleration in Y-direction + bayScale: 2, // scale within 2nd, inset canvas showing robot in it's "bay" + debugSensors: true, // plot sensor rays and mark detected objects + debugMouse: true, // allow dragging any object with the mouse }; - -robots = new Array(); -sensors = new Array(); - -function init() { // called once when loading HTML file - const robotBay = document.getElementById("bayLemming"), - arena = document.getElementById("arenaLemming"), - height = arena.height, - width = arena.width; - simInfo.height = height; - simInfo.width = width; - - /* Create a MatterJS engine and world. */ - simInfo.engine = Matter.Engine.create(); - simInfo.world = simInfo.engine.world; - simInfo.world.gravity.y = simInfo.gravity; - simInfo.engine.timing.timeScale = 1; - - /* Create walls and boxes, and add them to the world. */ - // note that "roles" are custom properties for rendering (not from MatterJS) - function getWall(x, y, width, height) { - return Matter.Bodies.rectangle(x, y, width, height, - {isStatic: true, role: 'wall', - color:[150, 150, 150]}); - }; - const wall_lo = getWall(width/2, height-5, width-5, 5), - wall_hi = getWall(width/2, 5, width-5, 5), - wall_le = getWall(5, height/2, 5, height-15), - wall_ri = getWall(width-5, height/2, 5, height-15); - Matter.World.add(simInfo.world, [wall_lo, wall_hi, wall_le, wall_ri]); - - /* Add a bunch of boxes in a neat grid. */ - function getBox(x, y) { - // flip coin for red vs blue and add rgb - colFlag = Math.round(Math.random()); // random 0,1 variable for box color - if (colFlag == 1 ){ - color = [0, 0, 200]; +class Lemming extends Robot { + constructor(props) { + super(Object.assign({ + sensors: [ + new DistanceSensor('distR', { + attachAngle: Math.PI/(2.5), // where the sensor is mounted on robot body + color: [150, 0, 0], // sensor color [in RGB], to distinguish them + lookAngle: (Math.PI/7 - Math.PI/(2.5)), + attachRadius: 20 + }), + // define another sensor + new DistanceSensor('distL', { + attachAngle: -Math.PI/7, + color: [0, 150, 0], + attachRadius: 20 + }), + new Gyroscope('gyro', { + attachAngle: 0, + attachRadius: 5, + color: [100,100,0] + }), + new ColorSensor('carry', { + attachAngle: 0, + color: [255, 100, 0], + attachRadius: 5 + }), + new DistanceSensor('wallR', { + attachAngle: Math.PI/2.5, // where the sensor is mounted on robot body + color: [150, 0, 0], // sensor color [in RGB], to distinguish them + filter: x => x.role == 'wall', + lookAngle: (Math.PI/7 - Math.PI/2.5), + attachRadius: 20, + + }), + // define another sensor + new DistanceSensor('wallL', { + attachAngle: -Math.PI/7, + color: [0, 150, 0], + filter: x => x.role == 'wall', + attachRadius: 20 + }), + ].concat(props.sensors || []) + }, props)) } - else { - color = [200, 0, 0]; - } - box = Matter.Bodies.rectangle(x, y, simInfo.boxSize, simInfo.boxSize, - {frictionAir: simInfo.airDrag, - friction: simInfo.boxFric, - mass: simInfo.boxMass, - role: 'box', - color: color}); - return box; - }; - - const startX = 100, startY = 100, - nBoxX = 5, nBoxY = 5, - gapX = 40, gapY = 30, - stack = Matter.Composites.stack(startX, startY, - nBoxX, nBoxY, - gapX, gapY, getBox); - Matter.World.add(simInfo.world, stack); - - /* Add debugging mouse control for dragging objects. */ - if (simInfo.debugMouse){ - const mouseConstraint = Matter.MouseConstraint.create(simInfo.engine, - {mouse: Matter.Mouse.create(arena), - // spring stiffness mouse ~ object - constraint: {stiffness: 0.5}}); - Matter.World.add(simInfo.world, mouseConstraint); - } - // Add the tracker functions from mouse.js - addMouseTracker(arena); - addMouseTracker(robotBay); - - /* Running the MatterJS physics engine (without rendering). */ - simInfo.runner = Matter.Runner.create({fps: 60, isFixed: false}); - Matter.Runner.start(simInfo.runner, simInfo.engine); - // register function simStep() as callback to MatterJS's engine events - Matter.Events.on(simInfo.engine, 'tick', simStep); - - /* Create robot(s). */ - setRobotNumber(1); // requires defined simInfo.world - loadBay(robots[0]); - -}; - -function convrgb(values) { - return 'rgb(' + values.join(', ') + ')'; -}; - - -function rotate(robot, torque=0) { - /* Apply a torque to the robot to rotate it. - * - * Parameters - * torque - rotational force to apply to the body. - * Try values around +/- 0.005. - */ - robot.body.torque = torque; - }; - -function drive(robot, force=0) { - /* Apply a force to the robot to move it. - * - * Parameters - * force - force to apply to the body. - * Try values around +/- 0.0005. - */ - const orientation = robot.body.angle, - force_vec = Matter.Vector.create(force, 0), - move_vec = Matter.Vector.rotate(force_vec, orientation); - Matter.Body.applyForce(robot.body, robot.body.position , move_vec); -}; - - -function senseDistance() { - /* Distance sensor simulation based on ray casting. Called from sensor - * object, returns nothing, updates a new reading into this.value. - * - * Idea: Cast a ray with a certain length from the sensor, and check - * via collision detection if objects intersect with the ray. - * To determine distance, run a Binary search on ray length. - * Note: Sensor ray needs to ignore robot (parts), or start outside of it. - * The latter is easy with the current circular shape of the robots. - * Note: Order of tests are optimized by starting with max ray length, and - * then only testing the maximal number of initially resulting objects. - * Note: The sensor's "ray" could have any other (convex) shape; - * currently it's just a very thin rectangle. - */ - - const context = document.getElementById('arenaLemming').getContext('2d'); - var bodies = Matter.Composite.allBodies(simInfo.engine.world); - - const robotAngle = this.parent.body.angle, - attachAngle = this.attachAngle, - rayAngle = robotAngle + attachAngle + this.lookAngle; - - const rPos = this.parent.body.position, - rSize = simInfo.robotSize, - startPoint = {x: rPos.x + (rSize+1) * Math.cos(robotAngle + attachAngle), - y: rPos.y + (rSize+1) * Math.sin(robotAngle + attachAngle)}; - - function getEndpoint(rayLength) { - return {x: rPos.x + (rSize + rayLength) * Math.cos(rayAngle), - y: rPos.y + (rSize + rayLength) * Math.sin(rayAngle)}; - }; - - function sensorRay(bodies, rayLength) { - // Cast ray of supplied length and return the bodies that collide with it. - const rayWidth = 1e-100, - endPoint = getEndpoint(rayLength); - rayX = (endPoint.x + startPoint.x) / 2, - rayY = (endPoint.y + startPoint.y) / 2, - rayRect = Matter.Bodies.rectangle(rayX, rayY, rayLength, rayWidth, - {isSensor: true, isStatic: true, - angle: rayAngle, role: 'sensor'}); - - var collidedBodies = []; - for (var bb = 0; bb < bodies.length; bb++) { - var body = bodies[bb]; - // coarse check on body boundaries, to increase performance: - if (Matter.Bounds.overlaps(body.bounds, rayRect.bounds)) { - for (var pp = body.parts.length === 1 ? 0 : 1; pp < body.parts.length; pp++) { - var part = body.parts[pp]; - // finer, more costly check on actual geometry: - if (Matter.Bounds.overlaps(part.bounds, rayRect.bounds)) { - const collision = Matter.SAT.collides(part, rayRect); - if (collision.collided) { - collidedBodies.push(body); - break; + turnDeg(rads, cb) { + if (Math.abs(rads) >= 320) + return this.turnDeg(320, x => this.turnDeg(rads -320, cb)) + const torque = Math.sign(rads) * 0.01 + const start = this.getSensorValById('gyro') + this.move = function() { + const curAngle = this.getSensorValById('gyro') + const turned = ((curAngle - start) * Math.sign(rads) + 360) % 360 + if (turned < 340 && (turned - Math.abs(rads) > 0)) { + delete this.move + if (cb) cb() + } else { + this.rotate(torque) } - } } - } } - return collidedBodies; - }; - - // call 1x with full length, and check all bodies in the world; - // in subsequent calls, only check the bodies resulting here - var rayLength = this.maxVal; - bodies = sensorRay(bodies, rayLength); - // if some collided, search for maximal ray length without collisions - if (bodies.length > 0) { - var lo = 0, - hi = rayLength; - while (lo < rayLength) { - if (sensorRay(bodies, rayLength).length > 0) { - hi = rayLength; - } - else { - lo = rayLength; - } - rayLength = Math.floor(lo + (hi-lo)/2); + move() { + //if (sim.curSteps % 250 == 0) this.turnDeg(-90) + //return + // TODO: Define Lemming program here. + const vals = ['distL', 'distR', 'carry', 'wallL', 'wallR'] + .reduce(((p,c) => ((p[c] = this.getSensorValById(c)), p)), {}) + const [r,g,b] = vals.carry + let block = 0 + if (r > (g+b)) { + block = 'red' + } else if (b > r+g) { + block = 'blue' + } + const {distL, distR, wallL, wallR} = vals + if (distL < wallL - 5 || distR < wallR - 5) { + // if it senses a block + if (!block) return this.drive(2e-4) // no block: drive towards block? + if (block == 'blue') void 0; // if blue: ignore + if (block == 'red') return this.turnDeg(-90) // if red: leave block + } else if (wallL < Infinity || wallR < Infinity) { + // no block: turn left or right + if (!block) return this.turnDeg(90 * (Math.random() < 0.5 ? -1 : 1)) + if (block == 'blue') return this.turnDeg(-90) // blue: leave + if (block == 'red') return this.turnDeg(90) // red: keep + } + // by default: wander + this.rotate(+0.002); + this.drive(0.0002); } - } - // increase length to (barely) touch closest body (if any) - rayLength += 1; - bodies = sensorRay(bodies, rayLength); - - if (simInfo.debugSensors) { // if invisible, check order of object drawing - // draw the resulting ray - endPoint = getEndpoint(rayLength); - context.beginPath(); - context.moveTo(startPoint.x, startPoint.y); - context.lineTo(endPoint.x, endPoint.y); - context.strokeStyle = this.parent.info.color; - context.lineWidth = 0.5; - context.stroke(); - // mark all objects's lines intersecting with the ray - for (var bb = 0; bb < bodies.length; bb++) { - var vertices = bodies[bb].vertices; - context.moveTo(vertices[0].x, vertices[0].y); - for (var vv = 1; vv < vertices.length; vv += 1) { - context.lineTo(vertices[vv].x, vertices[vv].y); - } - context.closePath(); +} +var sim = null +class Simulation { + constructor() { + this.bay = null + this.robots = null + this.runner = null + this.world = null + this.engine = null + this.curSteps = 0 + this.doContinue = false + this.robots = [] } - context.stroke(); - } + init() { + const arena = document.getElementById("arenaLemming"), + {height, width} = arena + this.elem = arena + arena.style.backgroundColor = 'silver' + Object.assign(this, {height, width}) - // indicate if the sensor exceeded its maximum length by returning infinity - if (rayLength > this.maxVal) { - rayLength = Infinity; - } - else { - // apply mild noise on the sensor reading, and clamp between valid values - function gaussNoise(sigma=1) { - const x0 = 1.0 - Math.random(); - const x1 = 1.0 - Math.random(); - return sigma * Math.sqrt(-2 * Math.log(x0)) * Math.cos(2 * Math.PI * x1); - }; - rayLength = Math.floor(rayLength + gaussNoise(3)); - rayLength = Matter.Common.clamp(rayLength, this.minVal, this.maxVal); - } + /* Create a MatterJS engine and world. */ + this.engine = Matter.Engine.create(); + this.world = this.engine.world; + this.world.gravity.y = simInfo.gravity; + this.engine.timing.timeScale = 1; - this.value = rayLength; + /* Create walls and boxes, and add them to the world. */ + // note that "roles" are custom properties for rendering (not from MatterJS) + function getWall(x, y, width, height) { + return Matter.Bodies.rectangle(x, y, width, height, { + isStatic: true, role: 'wall', + color:[150, 150, 150] + }); + }; + const wall_lo = getWall(width/2, height-5, width-5, 5), + wall_hi = getWall(width/2, 5, width-5, 5), + wall_le = getWall(5, height/2, 5, height-15), + wall_ri = getWall(width-5, height/2, 5, height-15); + Matter.World.add(this.world, [wall_lo, wall_hi, wall_le, wall_ri]); + + /* Add a bunch of boxes in a neat grid. */ + function getBox(x, y) { + // flip coin for red vs blue and add rgb + const color = (Math.random() < 0.5) ? [0, 0, 200] : [200, 0, 0] + return Matter.Bodies.rectangle(x, y, simInfo.boxSize, simInfo.boxSize, { + frictionAir: simInfo.airDrag, + friction: simInfo.boxFric, + mass: simInfo.boxMass, + role: 'box', + color + }); + } + + const startX = 100, startY = 100, + nBoxX = 5, nBoxY = 5, + gapX = 40, gapY = 30, + stack = Matter.Composites.stack(startX, startY, + nBoxX, nBoxY, + gapX, gapY, getBox); + Matter.World.add(this.world, stack); + + /* Add debug ging mouse control for dragging objects. */ + if (simInfo.debugMouse){ + const mouseConstraint = Matter.MouseConstraint.create(this.engine, { + mouse: Matter.Mouse.create(arena), + // spring stiffness mouse ~ object + constraint: {stiffness: 0.5} + }); + Matter.World.add(this.world, mouseConstraint); + } + // Add the tracker functions from mouse.js + addMouseTracker(arena); + + /* Running the MatterJS physics engine (without rendering). */ + this.runner = Matter.Runner.create({fps: 60, isFixed: false}); + // register function simStep() as callback to MatterJS's engine events + Matter.Events.on(this.engine, 'tick', this.step.bind(this)); + + this.bay = new Bay() + } + step() { + // advance simulation by one step (except MatterJS engine's physics) + if (this.curSteps < simInfo.maxSteps) { + this.bay.repaint(); + this.draw(); + this.robots.forEach(robot => { + robot.updateSensors(); + robot.move(); + // To enable selection by clicking (via mouse.js/graphics.js), + // the position on the canvas needs to be defined in (x, y): + const rSize = simInfo.robotSize; + robot.x = robot.body.position.x - rSize; + robot.y = robot.body.position.y - rSize; + }) + // count and display number of steps + this.curSteps += 1; + document.getElementById("SimStepLabel").innerHTML = + padnumber(this.curSteps, 5) + + ' of ' + + padnumber(simInfo.maxSteps, 5); + } + else { + this.toggle() + } + } + draw() { + const context = this.elem.getContext('2d') + context.clearRect(0, 0, this.width, this.height); + // draw objects within world + const Composite = Matter.Composite, + bodies = Composite.allBodies(this.world); + bodies.forEach(({role, vertices, color}) => { + if (role == 'robot') return + if (color) { + context.strokeStyle = convrgb(color); + } + drawVertices(context, vertices); + }) + context.lineWidth = 1; + + // draw all robots + this.robots.forEach(robot => robot.plotRobot(context)) + } + addRobot(robot) { + this.robots.push( + makeInteractiveElement(robot, this.elem)) + if (!this.bay.robot) this.bay.load(robot) + } + removeRobot(robot) { + this.robots = this.robots.filter(x => x == robot) + Matter.World.remove(this.world, robot.body) + } + start() { + if (!this.doContinue) Matter.Runner.start(this.runner, this.engine); + this.doContinue = true + } + stop() { + if (this.doContinue) Matter.Runner.stop(this.runner) + this.doContinue = false + } + toggle() { + if (this.doContinue) this.stop() + else this.start() + } +} +function init() { // called once when loading HTML file + // the pathseg polyfill will run in the html document + // but we need it in the svg. I changed the IIFE in to a + // regular function and inject it into the svg using eval + const svg = document.getElementById('robotbodySVG') + svg.contentWindow.eval('(' + window.pathseg.toString() + ')()') + sim = new Simulation() + sim.init() + sim.addRobot(new Lemming({ + color: [255, 255, 255], // color of the robot shape + init: {x: 50, y: 50, angle: 0}, // initial position and orientation + })) + sim.start() }; -function dragSensor(sensor, event) { - const robotBay = document.getElementById('bayLemming'), - bCenter = {x: robotBay.width/2, - y: robotBay.height/2}, - rSize = simInfo.robotSize, - bScale = simInfo.bayScale, - sSize = sensor.getWidth(), - mAngle = Math.atan2( event.mouse.x - bCenter.x, - -(event.mouse.y - bCenter.y)); - sensor.info.attachAngle = mAngle; - sensor.x = bCenter.x - sSize - bScale * rSize * Math.sin(-mAngle); - sensor.y = bCenter.y - sSize - bScale * rSize * Math.cos( mAngle); - repaintBay(); -} - -function loadSensor(sensor, event) { - loadSensorInfo(sensor.sensor); -} - -function loadSensorInfo(sensorInfo) { - simInfo.baySensor = sensorInfo; -} - -function loadBay(robot) { - simInfo.bayRobot = robot; - sensors = new Array(); - const robotBay = document.getElementById("bayLemming"); - const bCenter = {x: robotBay.width/2, - y: robotBay.height/2}, - rSize = simInfo.robotSize, - bScale = simInfo.bayScale; - - for (var ss = 0; ss < robot.info.sensors.length; ++ss) { - const curSensor = robot.sensors[ss], - attachAngle = curSensor.attachAngle; - // put current sensor into global variable, make mouse-interactive - sensors[ss] = makeInteractiveElement(new SensorGraphics(curSensor), - document.getElementById("bayLemming")); - const sSize = sensors[ss].getWidth(); - sensors[ss].x = bCenter.x - sSize - bScale * rSize * Math.sin(-attachAngle); - sensors[ss].y = bCenter.y - sSize - bScale * rSize * Math.cos( attachAngle); - sensors[ss].onDragging = dragSensor; - sensors[ss].onDrag = loadSensor; - } - repaintBay(); -} - -function SensorGraphics(sensorInfo) { - this.info = sensorInfo; - this.plotSensor = plotSensor; - // add functions getWidth/getHeight for graphics.js & mouse.js, - // to enable dragging the sensor in the robot bay - this.getWidth = function() { return 6; }; - this.getHeight = function() { return 6; }; +class Bay { + constructor() { + this.elem = document.getElementById("bayLemming") + this.context = this.elem.getContext('2d') + this.center = {x: this.elem.width / simInfo.bayScale / 2, + y: this.elem.height / simInfo.bayScale / 2} + this.robot = null + this.elem.style.backgroundColor = 'silver' + addMouseTracker(this.elem); + } + load(robot) { + this.robot = robot + robot.sensors.forEach(sensor => { + makeInteractiveElement(sensor, this.elem) + }) + // todo: removeinteractiveelement? + this.repaint() + } + repaint() { + const {context, robot, elem: robotBay} = this + // update inset canvas showing information about selected robot + context.clearRect(0, 0, this.elem.width, this.elem.height); + if (!robot) return + context.save() + context.scale(simInfo.bayScale, simInfo.bayScale) + context.translate(this.center.x, this.center.y) + context.rotate(-Math.PI/2) + robot.plotRobot(context, 0, 0, 0); + context.restore() + // print sensor values of selected robot next to canvas + if (!(sim.curSteps % 5)) { // update slow enough to read + const sensorString = robot.sensors.map(({id, valueStr, color}) => { + return `
id '${id}': ${valueStr}` + }).join('') + + document.getElementById('SensorLabel').innerHTML = sensorString; + } + } + transformMouse({x,y}) { + // scale, translate, rotate by 90 degrees, + return { y: (x / simInfo.bayScale) - this.center.x, + x: -((y / simInfo.bayScale) - this.center.y) } + } + } function loadFromSVG() { - var vertexSets = []; - const svg = document.getElementById('robotbodySVG'), - data = svg.contentDocument; + var vertexSets = []; + const svg = document.getElementById('robotbodySVG'), + data = svg.contentDocument; - jQuery(data).find('path').each(function(_, path) { - var points = Matter.Svg.pathToVertices(path, 30); - vertexSets.push(Matter.Vertices.scale(points, 0.2, 0.2)); - }); + for (const path of data.getElementsByTagName('path')) { + const points = Matter.Svg.pathToVertices(path, 30); + vertexSets.push(Matter.Vertices.scale(points, 0.2, 0.2)); + } - return vertexSets; + return vertexSets; }; -function InstantiateRobot(robotInfo) { - // load robot's body shape from SVG file - const bodySVGpoints = loadFromSVG(); - this.body = Matter.Bodies.fromVertices(robotInfo.init.x, - robotInfo.init.y, - bodySVGpoints, - {frictionAir: simInfo.airDrag, - mass: simInfo.robotMass, - color: [255, 255, 255], - role: 'robot'}, true); +function Robot(robotInfo) { + // load robot's body shape from SVG file + const bodySVGpoints = loadFromSVG(); + this.body = Matter.Bodies.fromVertices(robotInfo.init.x, + robotInfo.init.y, + bodySVGpoints, { + frictionAir: simInfo.airDrag, + mass: simInfo.robotMass, + color: [255, 255, 255], + role: 'robot' + }, true); - Matter.World.add(simInfo.world, this.body); - Matter.Body.setAngle(this.body, robotInfo.init.angle); + Matter.World.add(sim.world, this.body); + Matter.Body.setAngle(this.body, robotInfo.init.angle); - // instantiate its sensors - this.sensors = robotInfo.sensors; - for (var ss = 0; ss < this.sensors.length; ++ss) { - this.sensors[ss].parent = this; - } + // instantiate its sensors + this.sensors = robotInfo.sensors; + this.sensors.forEach(sensor => sensor.parent = this) - // attach its helper functions - this.rotate = rotate; - this.drive = drive; - this.info = robotInfo; - this.plotRobot = plotRobot; - - // add functions getWidth/getHeight for graphics.js & mouse.js, - // to enable selection by clicking the robot in the arena - this.getWidth = function() { return 2 * simInfo.robotSize; }; - this.getHeight = function() { return 2 * simInfo.robotSize; }; + // attach its helper functions + this.info = robotInfo; } +Object.assign(Robot.prototype, { + rotate(torque=0) { + /* Apply a torque to the robot to rotate it. + * + * Parameters + * torque - rotational force to apply to the body. + * Try values around +/- 0.005. + */ + this.body.torque = torque; + }, + + drive(force=0) { + /* Apply a force to the robot to move it. + * + * Parameters + * force - force to apply to the body. + * Try values around +/- 0.0005. + */ + const orientation = this.body.angle, + force_vec = Matter.Vector.create(force, 0), + move_vec = Matter.Vector.rotate(force_vec, orientation); + Matter.Body.applyForce(this.body, this.body.position , move_vec); + }, + updateSensors() { + this.sensors.forEach(sensor => sensor.sense()) + }, + getSensorValById(id) { + const sensor = this.sensors.find(sensor => sensor.id == id) + return sensor ? sensor.value : undefined + }, + move() { + // TODO: Define Lemming program here. + const distL = this.getSensorValById('distL'), + distR = this.getSensorValById('distR'); + + //robot.rotate(+0.005); + //robot.drive(0.0005); + }, + mouseHit(x, y) { + return Vec2.distLess(this.body.position, {x,y}, this.getWidth()/2 + 1) + }, + getWidth() { return 2 * simInfo.robotSize }, + getHeight() { return 2 * simInfo.robotSize }, + onDrop(robot, event) { + this.isDragged = false + }, + onDrag(robot, event) { + this.isDragged = true + sim.bay.load(this) + return true + }, + plotRobot(context, + x = this.body.position.x, + y = this.body.position.y, + angle = this.body.angle) { + const showInternalEdges = false; + const body = this.body -function robotUpdateSensors(robot) { - // update all sensors of robot; puts new values into sensor.value - for (var ss = 0; ss < robot.sensors.length; ss++) { - robot.sensors[ss].sense(); - } -}; + // MatterJS thinks in world coords + // translate the world canvas to compensate + context.save(); + context.rotate(-body.angle + angle); + context.translate(-body.position.x + x, -body.position.y + y); -function getSensorValById(robot, id) { - for (var ss = 0; ss < robot.sensors.length; ss++) { - if (robot.sensors[ss].id == id) { - return robot.sensors[ss].value; + // handle compound parts + context.beginPath(); + context.strokeStyle = convrgb(body.color); + context.lineWidth = 1.5; + body.parts.forEach(({vertices}, k) => { + if (k == 0 && body.parts.length > 1) return + context.moveTo(vertices[0].x, vertices[0].y); + let fn, wasInternal = true + vertices.forEach(fn = ({x,y, isInternal}, j) => { + if (wasInternal) context.moveTo(x, y) + else context.lineTo(x, y) + wasInternal = isInternal && !showInternalEdges + }) + fn(vertices[0]) + }) + context.stroke() + + // to draw the rest, rotate & translate again + context.restore() + + context.save(); + context.translate(x, y); + context.rotate(angle); + + // Plot sensor positions into world canvas. + this.sensors.forEach(sensor => sensor.plotSensor(context)) + context.restore(); } - } - return undefined; // if not returned yet, id doesn't exist -}; - -function robotMove(robot) { - // TODO: Define Lemming program here. - const distL = getSensorValById(robot, 'distL'), - distR = getSensorValById(robot, 'distR'); - - robot.rotate(robot, +0.005); - robot.drive(robot, 0.0005); -}; - -function plotSensor(context, x = this.x, y = this.y) { - context.beginPath(); - context.arc(x + this.getWidth()/2, - y + this.getHeight()/2, - this.getWidth()/2, 0, 2*Math.PI); - context.closePath(); - context.fillStyle = 'black'; - context.strokeStyle = 'black'; - context.fill(); - context.stroke(); -} - -function plotRobot(context, - xTopLeft = this.body.position.x, - yTopLeft = this.body.position.y) { - var x, y, scale, angle, i, half, full, - rSize = simInfo.robotSize; - const showInternalEdges = false; - - if (context.canvas.id == "bayLemming") { - scale = simInfo.bayScale; - half = Math.floor(rSize/2*scale); - full = half * 2; - x = xTopLeft + full; - y = yTopLeft + full; - angle = -Math.PI / 2; - } else { - scale = 1; - half = Math.floor(rSize/2*scale); - full = half * 2; - x = xTopLeft; - y = yTopLeft; - angle = this.body.angle; - } - context.save(); - context.translate(x, y); - context.rotate(angle); - - if (context.canvas.id == "arenaLemming") { - // draw into world canvas without transformations, - // because MatterJS thinks in world coords... - context.restore(); - - const body = this.body; - // handle compound parts - - context.beginPath(); - for (k = body.parts.length > 1 ? 1 : 0; k < body.parts.length; k++) { - part = body.parts[k]; - context.moveTo(part.vertices[0].x, - part.vertices[0].y); - for (j = 1; j < part.vertices.length; j++) { - if (!part.vertices[j - 1].isInternal || showInternalEdges) { - context.lineTo(part.vertices[j].x, - part.vertices[j].y); - } else { - context.moveTo(part.vertices[j].x, - part.vertices[j].y); - } - - if (part.vertices[j].isInternal && !showInternalEdges) { - context.moveTo(part.vertices[(j + 1) % part.vertices.length].x, - part.vertices[(j + 1) % part.vertices.length].y); - } - } - context.lineTo(part.vertices[0].x, - part.vertices[0].y); - } - context.strokeStyle = convrgb(body.color); - context.lineWidth = 1.5; - context.stroke(); - - // to draw the rest, rotate & translate again - context.save(); - context.translate(x, y); - context.rotate(angle); - } - - // Plot sensor positions into world canvas. - if (context.canvas.id == "arenaLemming") { - for (ss = 0; ss < this.info.sensors.length; ++ss) { - context.beginPath(); - context.arc(full * Math.cos(this.info.sensors[ss].attachAngle), - full * Math.sin(this.info.sensors[ss].attachAngle), - scale, 0, 2*Math.PI); - context.closePath(); - context.fillStyle = 'black'; - context.strokeStyle = 'black'; - context.fill(); - context.stroke(); - } - } - context.restore(); -} - -function simStep() { - // advance simulation by one step (except MatterJS engine's physics) - if (simInfo.curSteps < simInfo.maxSteps) { - repaintBay(); - drawBoard(); - for (var rr = 0; rr < robots.length; ++rr) { - robotUpdateSensors(robots[rr]); - robotMove(robots[rr]); - // To enable selection by clicking (via mouse.js/graphics.js), - // the position on the canvas needs to be defined in (x, y): - const rSize = simInfo.robotSize; - robots[rr].x = robots[rr].body.position.x - rSize; - robots[rr].y = robots[rr].body.position.y - rSize; - } - // count and display number of steps - simInfo.curSteps += 1; - document.getElementById("SimStepLabel").innerHTML = - padnumber(simInfo.curSteps, 5) + - ' of ' + - padnumber(simInfo.maxSteps, 5); - } - else { - toggleSimulation(); - } -} - -function drawBoard() { - var context = document.getElementById('arenaLemming').getContext('2d'); - context.fillStyle = "#444444"; - context.fillRect(0, 0, simInfo.width, simInfo.height); - - // draw objects within world - const Composite = Matter.Composite, - bodies = Composite.allBodies(simInfo.world); - - for (var bb = 0; bb < bodies.length; bb += 1) { - var vertices = bodies[bb].vertices, - vv; - - // draw all non-robot bodies here (walls and boxes) - // don't draw robot's bodies here; they're drawn in plotRobot() - if (bodies[bb].role != 'robot') { - context.beginPath(); - context.moveTo(vertices[0].x, vertices[0].y); - for (vv = 1; vv < vertices.length; vv += 1) { - context.lineTo(vertices[vv].x, vertices[vv].y); - } - if (bodies[bb].color) { - context.strokeStyle = convrgb(bodies[bb].color); - context.closePath(); - context.stroke(); - } - } - } - context.lineWidth = 1; - - // draw all robots - for (var rr = 0; rr < robots.length; ++rr) { - robots[rr].plotRobot(context); - } -} - -function repaintBay() { - // update inset canvas showing information about selected robot - const robotBay = document.getElementById('bayLemming'), - context = robotBay.getContext('2d'); - context.clearRect(0, 0, robotBay.width, robotBay.height); - simInfo.bayRobot.plotRobot(context, 10, 10); - for (var ss = 0; ss < sensors.length; ss++) { - sensors[ss].plotSensor(context); - } - - // print sensor values of selected robot next to canvas - if (!(simInfo.curSteps % 5)) { // update slow enough to read - var sensorString = ''; - const rsensors = simInfo.bayRobot.sensors; - for (ss = 0; ss < rsensors.length; ss++) { - sensorString += '
id \'' + rsensors[ss].id + '\': ' + - padnumber(rsensors[ss].value, 2); - } - document.getElementById('SensorLabel').innerHTML = sensorString; - } -} - -function setRobotNumber(newValue) { - var n; - while (robots.length > newValue) { - n = robots.length - 1; - Matter.World.remove(simInfo.world, robots[n].body); - robots[n] = null; - robots.length = n; - } - - while (robots.length < newValue) { - if (newValue > RobotInfo.length) { - console.warn('You request '+newValue+' robots, but only ' + RobotInfo.length + - ' are defined in RobotInfo!'); - toggleSimulation(); - return; - } - n = robots.length; - robots[n] = makeInteractiveElement(new InstantiateRobot(RobotInfo[n]), - document.getElementById("arenaLemming")); - - robots[n].onDrop = function(robot, event) { - robot.isDragged = false; - }; - - robots[n].onDrag = function(robot, event) { - robot.isDragged = true; - loadBay(robot); - return true; - }; - } -} +}) function padnumber(number, size) { - if (number == Infinity) { - return 'inf'; - } - const s = "000000" + number; - return s.substr(s.length - size); + if (number == 'Infinity') return 'inf' + return (''+number).padStart(size, '0') } function format(number) { - // prevent HTML elements to jump around at sign flips etc - return (number >= 0 ? '+' : '−') + Math.abs(number).toFixed(1); + // prevent HTML elements to jump around at sign flips etc + return (number >= 0 ? '+' : '−') + Math.abs(number).toFixed(1); } function toggleSimulation() { - simInfo.doContinue = !simInfo.doContinue; - if (simInfo.doContinue) { - Matter.Runner.start(simInfo.runner, simInfo.engine); - } - else { - Matter.Runner.stop(simInfo.runner); - } + sim.toggle() } diff --git a/mouse.js b/mouse.js index 76e68ad..f49af96 100644 --- a/mouse.js +++ b/mouse.js @@ -58,17 +58,15 @@ function addMouseTracker(elem) { this.mouse.hasFocus = false; }); elem.addInteractiveElement = function(obj) { + if (!obj.mouseHit) obj.mouseHit = mouseHit this.mouse.interactiveElements.push(obj); }; } - +function mouseHit(x, y) { + return (x < this.x + this.getWidth() && x > this.x && + y < this.y + this.getHeight() && y > this.y) +} function findElement(x, y) { - for (var i = this.mouse.interactiveElements.length - 1; i >= 0; --i) { - if (x < this.mouse.interactiveElements[i].x + this.mouse.interactiveElements[i].getWidth() && x > this.mouse.interactiveElements[i].x && - y < this.mouse.interactiveElements[i].y + this.mouse.interactiveElements[i].getHeight() && y > this.mouse.interactiveElements[i].y) { - return this.mouse.interactiveElements[i]; - } - } - return null; + return this.mouse.interactiveElements.find(elem => elem.mouseHit(x, y)) } diff --git a/pathseg.js b/pathseg.js index 21e883f..ca490e3 100644 --- a/pathseg.js +++ b/pathseg.js @@ -5,7 +5,7 @@ // SVG2 (https://lists.w3.org/Archives/Public/www-svg/2015Jun/0044.html), including the latest spec // changes which were implemented in Firefox 43 and Chrome 46. -(function() { "use strict"; +window.pathseg = (function() { "use strict"; if (!("SVGPathSeg" in window)) { // Spec: http://www.w3.org/TR/SVG11/single-page.html#paths-InterfaceSVGPathSeg window.SVGPathSeg = function(type, typeAsLetter, owningPathSegList) { @@ -841,4 +841,4 @@ return builder.pathSegList; } } -}()); +}); diff --git a/robotbody.svg b/robotbody.svg index 4991cda..c601e78 100644 --- a/robotbody.svg +++ b/robotbody.svg @@ -13,13 +13,13 @@ version="1.1" x="0px" y="0px" - width="300" - height="300" - viewBox="0 0 300 300" + width="239.19885" + height="186.78011" + viewBox="0 0 239.19885 186.78011" xml:space="preserve" id="svg11" - sodipodi:docname="robotbody.svg" - inkscape:version="0.92.2 5c3e80d, 2017-08-06">image/svg+xml \ No newline at end of file diff --git a/sensors.js b/sensors.js new file mode 100644 index 0000000..2190083 --- /dev/null +++ b/sensors.js @@ -0,0 +1,174 @@ + +// Description of robot(s), and attached sensor(s) used by InstantiateRobot() +class Sensor { + constructor(id, props) { + Object.assign(this, { + id, + parent: null, + lookAngle:0, + attachRadius: simInfo.robotSize, + value: null, + valueStr: "", + minVal: 0, + maxVal: 50 + }, props) + this.info = this + this.setAngle(this.attachAngle) + } + setAngle(attachAngle) { + this.attachAngle = attachAngle + Object.assign(this, Vec2.fromPolar(Vec2.zero, this.attachRadius, this.attachAngle)) + } + plotSensor(context, x = this.x, y = this.y) { + context.beginPath(); + context.arc(x, y, this.getWidth()/2, 0, 2*Math.PI); + context.closePath(); + context.fillStyle = 'black'; + context.strokeStyle = this.color ? convrgb(this.color) : 'black'; + context.fill(); + context.stroke(); + } + onDragging(sensor, event) { + const {x, y} = sim.bay.transformMouse(event.mouse), + mAngle = Math.atan2(y, x), + mRadius = Math.sqrt(x*x + y*y); + this.attachRadius = mRadius + this.setAngle(mAngle) + sim.bay.repaint() + } + getWidth() { return 2 } + getHeight() { return 2 } + mouseHit(x, y) { + const mouse = sim.bay.transformMouse({x,y}) + return Vec2.distLess(this, mouse, this.getWidth()/2 + 1) + } +}; +class DistanceSensor extends Sensor { + rayCast() { + /* Distance sensor simulation based on ray casting. Called from sensor + * object, returns nothing, updates a new reading into this.value. + * + * Idea: Cast a ray with a certain length from the sensor, and check + * via collision detection if objects intersect with the ray. + * To determine distance, run a Binary search on ray length. + * Note: Sensor ray needs to ignore robot (parts), or start outside of it. + * The latter is easy with the current circular shape of the robots. + * Note: Order of tests are optimized by starting with max ray length, and + * then only testing the maximal number of initially resulting objects. + * Note: The sensor's "ray" could have any other (convex) shape; + * currently it's just a very thin rectangle. + */ + + var bodies = Matter.Composite.allBodies(sim.engine.world); + if (this.filter) bodies = bodies.filter(this.filter) + + const robotAngle = this.parent.body.angle, + rayAngle = robotAngle + this.attachAngle + this.lookAngle; + + const rPos = this.parent.body.position, + rSize = this.attachRadius, + startPoint = Vec2.fromPolar(rPos, this.attachRadius, robotAngle+this.attachAngle) + + function getEndpoint(rayLength) { + return Vec2.fromPolar(startPoint, rayLength, rayAngle) + }; + + function sensorRay(bodies, rayLength) { + // Cast ray of supplied length and return the bodies that collide with it. + const rayWidth = 1e-100, + ray = Vec2.avg(startPoint, getEndpoint(rayLength)), + rayRect = Matter.Bodies.rectangle(ray.x, ray.y, rayLength, rayWidth, + {isSensor: true, isStatic: true, + angle: rayAngle, role: 'sensor'}); + + return bodies.filter(body => { + // coarse check on body boundaries, to increase performance: + return Matter.Bounds.overlaps(body.bounds, rayRect.bounds) && + body.parts.some((part, pp) => { + // skip the first part, if it's not the only one + if (pp == 0 && body.parts.length > 1) return false + // finer, more costly check on actual geometry: + if (Matter.Bounds.overlaps(part.bounds, rayRect.bounds)) { + const collision = Matter.SAT.collides(part, rayRect); + if (collision.collided) { + return true + } + } + }) + }) + }; + + // call 1x with full length, and check all bodies in the world; + // in subsequent calls, only check the bodies resulting here + var rayLength = this.maxVal; + bodies = sensorRay(bodies, rayLength); + function binarySearch(lo, hi, cond) { + let x = hi + while (lo < x) { + if (cond(x)) { + hi = x + } else { + lo = x + } + x = Math.floor(lo + (hi-lo)/2) + } + return x + } + // if some collided, search for maximal ray length without collisions + if (bodies.length > 0) { + rayLength = binarySearch(0, rayLength, len => sensorRay(bodies, len).length > 0) + } + // increase length to (barely) touch closest body (if any) + rayLength += 1; + bodies = sensorRay(bodies, rayLength); + return [bodies, rayLength, startPoint, getEndpoint(rayLength)] + } + sense() { + const context = document.getElementById('arenaLemming').getContext('2d'); + const [bodies, rayLength, startPoint, endPoint] = this.rayCast() + if (simInfo.debugSensors) { // if invisible, check order of object drawing + // draw the resulting ray + context.strokeStyle = convrgb(this.parent.info.color) + context.lineWidth = 0.5; + drawVertices(context, [startPoint, endPoint]) + // mark all objects's lines intersecting with the ray + bodies.forEach(({vertices}) => drawVertices(context, vertices)) + } + let rl + + // indicate if the sensor exceeded its maximum length by returning infinity + if (rayLength > this.maxVal) { + rl = Infinity; + } + else { + rl = Math.floor(rayLength + gaussNoise(3)); + rl = Matter.Common.clamp(rl, this.minVal, this.maxVal); + } + + this.value = rl; + this.valueStr = padnumber(this.value, 2); + } +} +function clamp(x, min, max) { + return x < min ? min : x > max ? max : x +} +class ColorSensor extends DistanceSensor { + sense() { + const [bodies, rayLength] = this.rayCast() + let color + if (bodies.length) { + color = bodies[0].color.map(x => clamp(x + gaussNoise(), 0, 255)) + } else { + color = [255,255,255] + } + this.value = color + this.valueStr = `` + + color.map(x => format(x)).join(', ') + } +} +class Gyroscope extends Sensor { + sense() { + this.value = (this.parent.body.angle * (180/Math.PI) + gaussNoise()) % 360 + this.valueStr = format(this.value) + '°' + } +} diff --git a/utils.js b/utils.js new file mode 100644 index 0000000..5d1b683 --- /dev/null +++ b/utils.js @@ -0,0 +1,41 @@ +"use strict" +const Vec2 = { + add({x:x1, y:y1}, {x:x2, y:y2}) { + return Object.freeze({x:x1+x2, y:y1+y2}) + }, + avg({x:x1, y:y1}, {x:x2, y:y2}) { + return Object.freeze({x:(x1+x2)/2, y:(y1+y2)/2}) + }, + fromPolar({x,y}, radius, angle) { + return Object.freeze({x: x + radius * Math.cos(angle), + y: y + radius * Math.sin(angle)}) + }, + sub({x:x1, y:y1}, {x:x2, y:y2}) { + return Object.freeze({x:x1-x2, y: y1-y2}) + }, + distLess(a, b, radius) { + const {x, y} = Vec2.sub(a, b) + return ((x*x) + (y*y)) < (radius * radius) + }, + zero: Object.freeze({x:0, y:0}) +}; + +function drawVertices(context, vertices, draw=true, close=true) { + if (draw) context.beginPath(); + context.moveTo(vertices[0].x, vertices[0].y); + vertices.slice(1).forEach(({x, y}) => + context.lineTo(x, y)) + if (close) context.lineTo(vertices[0].x, vertices[0].y); + if (draw) context.stroke(); +} + +function convrgb(values) { + return 'rgb(' + values.map(x=>x|0).join(', ') + ')'; +}; + +// apply mild noise on the sensor reading, and clamp between valid values +function gaussNoise(sigma=1) { + const x0 = 1.0 - Math.random(); + const x1 = 1.0 - Math.random(); + return sigma * Math.sqrt(-2 * Math.log(x0)) * Math.cos(2 * Math.PI * x1); +};