New color sensor

master
Yorick van Pelt 2017-11-29 01:54:28 +01:00
parent d7d7ff308f
commit 677a5dc01b
3 changed files with 79 additions and 28 deletions

View File

@ -53,9 +53,12 @@ class Lemming extends Robot {
color: [100,100,0]
}),
new ColorSensor('carry', {
attachAngle: 0,
attachAngle: Math.PI/5,
lookAngle: -Math.PI/5,
color: [255, 100, 0],
attachRadius: 5
attachRadius: 5,
dist: 5,
width: 15,
}),
new DistanceSensor('wallR', {
attachAngle: Math.PI/2.5, // where the sensor is mounted on robot body

View File

@ -44,6 +44,32 @@ class Sensor {
}
};
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.
@ -72,30 +98,8 @@ class DistanceSensor extends Sensor {
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
}
}
})
})
const sensorRay = (bodies, rayLength) => {
return this.sensorRay(bodies, startPoint, getEndpoint(rayLength))
};
// call 1x with full length, and check all bodies in the world;
@ -154,13 +158,48 @@ function clamp(x, min, max) {
}
class ColorSensor extends DistanceSensor {
sense() {
const [bodies, rayLength] = this.rayCast()
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) {
color = bodies[0].color.map(x => clamp(x + gaussNoise(), 0, 255))
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(', ')

View File

@ -13,6 +13,15 @@ const Vec2 = {
sub({x:x1, y:y1}, {x:x2, y:y2}) {
return Object.freeze({x:x1-x2, y: y1-y2})
},
angle({x, y}) {
return Math.atan2(y, x)
},
len({x, y}) {
return Math.sqrt((x*x) + (y*y))
},
dist(a, b) {
return Vec2.len(Vec2.sub(a, b))
},
distLess(a, b, radius) {
const {x, y} = Vec2.sub(a, b)
return ((x*x) + (y*y)) < (radius * radius)