246 lines
9.5 KiB
JavaScript
246 lines
9.5 KiB
JavaScript
|
|
/* Lemmings - sensor code.
|
|
*
|
|
* Copyright 2016 Harmen de Weerd
|
|
* Copyright 2017 Johannes Keyser, James Cooke, George Kachergis
|
|
* Copyright 2017 Niels Boer, Tanja van Alfen, 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/>.
|
|
*/
|
|
class Sensor {
|
|
constructor(id, props) {
|
|
Object.assign(this, {
|
|
id,
|
|
parent: null,
|
|
lookAngle:0,
|
|
attachRadius: simInfo.robotSize,
|
|
value: null,
|
|
valueStr: "<uninit>",
|
|
minVal: 0,
|
|
maxVal: 50,
|
|
attachAngle: 0,
|
|
}, 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 {
|
|
sensorRay(bodies, start, end) {
|
|
// Cast ray of supplied length and return the bodies that collide with it.
|
|
const rayLength = Vec2.dist(start, end)
|
|
const rayAngle = Vec2.angle(Vec2.sub(end, start))
|
|
const rayWidth = 1e-100,
|
|
ray = Vec2.avg(start, end),
|
|
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
|
|
}
|
|
}
|
|
})
|
|
})
|
|
}
|
|
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)
|
|
};
|
|
const sensorRay = (bodies, rayLength) => {
|
|
return this.sensorRay(bodies, startPoint, getEndpoint(rayLength))
|
|
};
|
|
|
|
// 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() {
|
|
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,
|
|
middle = Vec2.fromPolar(rPos, this.attachRadius + this.dist, robotAngle + this.attachAngle),
|
|
startPoint = Vec2.fromPolar(middle, -this.width / 2, rayAngle + Math.PI/2),
|
|
endPoint = Vec2.fromPolar(middle, this.width / 2, rayAngle + Math.PI/2)
|
|
bodies = this.sensorRay(bodies, startPoint, endPoint)
|
|
|
|
if (simInfo.debugSensors) { // if invisible, check order of object drawing
|
|
const context = document.getElementById('arenaLemming').getContext('2d');
|
|
// 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 color
|
|
if (bodies.length) {
|
|
let r = 0
|
|
let g = 0
|
|
let b = 0
|
|
let len = 0
|
|
bodies.forEach(({color}) => {
|
|
if (color) {
|
|
r += color[0]
|
|
g += color[1]
|
|
b += color[2]
|
|
len++
|
|
}
|
|
})
|
|
if (len) color = [r/len,g/len,b/len]
|
|
else color = [255,255,255]
|
|
} else {
|
|
color = [255,255,255]
|
|
}
|
|
color = color.map(x => Math.max(0, Math.min(x + gaussNoise(), 255)))
|
|
this.value = color
|
|
this.valueStr = `</span><span style="color: ${convrgb(color)}">` +
|
|
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) + '°'
|
|
}
|
|
}
|
|
|
|
class CollisionSensor extends DistanceSensor {
|
|
constructor(id, props) {
|
|
super(id, Object.assign({
|
|
maxVal: 8
|
|
}, props))
|
|
}
|
|
sense() {
|
|
super.sense()
|
|
this.value = this.value != Infinity
|
|
this.valueStr = this.value ? "yes" : "no"
|
|
}
|
|
}
|