473 lines
17 KiB
JavaScript
473 lines
17 KiB
JavaScript
/* Lemmings - robot and GUI script.
|
||
*
|
||
* Copyright 2016 Harmen de Weerd
|
||
* 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
|
||
* the Free Software Foundation, either version 3 of the License, or
|
||
* (at your option) any later version.
|
||
*
|
||
* This program is distributed in the hope that it will be useful,
|
||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
* GNU General Public License for more details.
|
||
*
|
||
* You should have received a copy of the GNU General Public License
|
||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
"use strict"
|
||
// Simulation settings; please change anything that you think makes sense.
|
||
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
|
||
};
|
||
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))
|
||
}
|
||
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)
|
||
}
|
||
}
|
||
}
|
||
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);
|
||
}
|
||
}
|
||
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 = []
|
||
}
|
||
init() {
|
||
const arena = document.getElementById("arenaLemming"),
|
||
{height, width} = arena
|
||
this.elem = arena
|
||
arena.style.backgroundColor = 'silver'
|
||
Object.assign(this, {height, width})
|
||
|
||
/* 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;
|
||
|
||
/* 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()
|
||
};
|
||
|
||
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 `<br> <span style="color:${color ? convrgb(color) : 'black'}">id '${id}': ${valueStr}</span>`
|
||
}).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;
|
||
|
||
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;
|
||
};
|
||
|
||
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(sim.world, this.body);
|
||
Matter.Body.setAngle(this.body, robotInfo.init.angle);
|
||
|
||
// instantiate its sensors
|
||
this.sensors = robotInfo.sensors;
|
||
this.sensors.forEach(sensor => sensor.parent = this)
|
||
|
||
// 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
|
||
|
||
// 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);
|
||
|
||
// 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();
|
||
}
|
||
})
|
||
|
||
|
||
function padnumber(number, 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);
|
||
}
|
||
|
||
function toggleSimulation() {
|
||
sim.toggle()
|
||
}
|
||
|