New color sensor
parent
d7d7ff308f
commit
677a5dc01b
|
@ -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
|
||||
|
|
91
sensors.js
91
sensors.js
|
@ -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(', ')
|
||||
|
|
9
utils.js
9
utils.js
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue