12 changed files with 1063 additions and 633 deletions
|
After Width: | Height: | Size: 2.8 KiB |
|
After Width: | Height: | Size: 2.8 KiB |
|
Before Width: | Height: | Size: 2.7 KiB After Width: | Height: | Size: 2.8 KiB |
@ -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: "<uninit>", |
||||
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 = `</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) + '°' |
||||
} |
||||
} |
||||
@ -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); |
||||
}; |
||||
Loading…
Reference in new issue