<!DOCTYPE html>
<html>
<head>
<title>我的网页</title>
</head>
<body>
<div>
<script type="module">
const PI = 3.141592653589793238;
import init from './pkg/pi_orca.js';
import { RVOSimulator, Vector2, Vertices, TileMap, TileObstacle, AStar, NodeIndex } from './pkg/pi_orca.js';
const DefaultMaxSpeed = 2.0;
const RAND_MAX = 0x7fff;
const mapwidth = 10;
const mapHeight = 10;
const scaling = 20;
let paths = [];
let goals = [];
let divs = [];
let agents = [];
let lastpotion = [];
let velocitys = [];
class Path {
constructor(path) {
this.path = path;
this.index = 0;
this.lengths = [];
for (let i = 0; i < path.length - 1; i++) {
this.lengths.push(Vector2.abs_sq(path[i].sub(path[i + 1])));
}
}
next() {
return this.path[this.index++];
}
reset() {
this.index = 0;
}
surplusLengthSq() {
let length = 0;
for (let i = this.index; i < this.lengths.length; i++) {
let inedx = i - 1;
if (inedx < 0) {
inedx = 0;
}
length += this.lengths[inedx];
}
return length;
}
}
function update_divs(sim, divs) {
for (let i = 0; i < agents.length; i++) {
let position = sim.get_agent_position(agents[i]);
let x = Math.floor(position.x());
let y = Math.floor(position.y());
divs[agents[i]].style.left = x + "px";
divs[agents[i]].style.top = y + "px";
}
}
function set_preferred_velocities(sim, goals) {
let goal_vectors = [];
for (let i = 0; i < agents.length; i++) {
let goal_vector = goals[agents[i]].sub(sim.get_agent_position(agents[i]));
let temp = Vector2.abs_sq(goal_vector);
goal_vectors.push(temp);
if (temp > 1) {
goal_vector = Vector2.normalize(goal_vector);
}
sim.set_agent_pref_velocity(agents[i], goal_vector);
velocitys[agents[i]] = goal_vector;
}
}
function reached_goal(sim, a_star, map) {
let reached_goal = true;
for (let i = 0; i < agents.length; ++i) {
let cur_postion = sim.get_agent_position(agents[i]);
if (Vector2.abs_sq(cur_postion.sub(goals[agents[i]])) > 2.0 * 2.0) {
reached_goal = false;
reset_path(sim, a_star, map, cur_postion, agents[i]);
} else {
let path = paths[agents[i]].next();
if (path) {
console.log("end_pos" + agents[i] + ": " + path.x() + " " + path.y());
goals[agents[i]] = path;
reached_goal = false;
}
}
}
return reached_goal;
}
function reset_path(sim, a_star, map, cur_postion, i) {
let dist_sq = Vector2.abs_sq(cur_postion.sub(lastpotion[i][0]));
if (dist_sq < 0.0001) {
if (lastpotion[i][1] < 10) {
lastpotion[i][1] += 1;
} else {
lastpotion[i][1] = 0;
let start_pos = cur_postion.div(scaling);
let start_index = NodeIndex.new(Math.floor(start_pos.y()) * mapHeight + Math.floor(start_pos.x()));
let end_index;
while (true) {
let end_pos = paths[i].next();
if (end_pos) {
end_index = NodeIndex.new(end_pos.y() * mapHeight + end_pos.x());
} else {
break;
}
}
if (!end_index) {
let end_pos = goals[i].div(scaling);
end_index = NodeIndex.new(Math.floor(end_pos.y()) * mapHeight + Math.floor(end_pos.x()));
}
let res = a_star.find_path(map, 2000, start_index, end_index);
paths[i] = a_star.result(res, map);
goals[i] = paths[i].next().mul_number(scaling).add(Vector2.new(scaling / 2, scaling / 2));
}
}
lastpotion[i][0] = cur_postion;
}
function setObstacle(map, sim, ctx, min, max) {
for (let i = min.x(); i < max.x(); i++) {
for (let j = min.y(); j < max.y(); j++) {
let index = j * mapHeight + i;
map.set_obstacle(NodeIndex.new(index), TileObstacle.Center);
}
}
let obstacle1 = Vertices.new();
obstacle1.add(min.mul_number(scaling));
obstacle1.add(Vector2.new(max.x(), min.y()).mul_number(scaling));
obstacle1.add(max.mul_number(scaling));
obstacle1.add(Vector2.new(min.x(), max.y()).mul_number(scaling));
sim.add_obstacle(obstacle1);
ctx.fillRect(min.x() * scaling, min.y() * scaling, (max.x() - min.x()) * scaling, (max.y() - min.y()) * scaling);
}
function find_path(a_star, map, sim, start, end) {
let start_index = NodeIndex.new(start.y() * mapHeight + start.x());
let end_index = NodeIndex.new(end.y() * mapHeight + end.x());
let res = a_star.find_path(map, 1000, start_index, end_index);
let begin = end_index;
if (!res) {
throw Error("no path!!! start: " + start_index.index() + " end: " + end_index.index() + "");
}
if (res.index() != end_index.index()) {
console.error("node number not enough!!!");
begin = res;
}
return a_star.result(begin, map);
}
function addAgent(a_star, map, sim, start, end,) {
let path_iter = find_path(a_star, map, sim, start, end);
let path_arr = [];
while (true) {
let p = path_iter.next();
if (p) {
path_arr.push(p.mul_number(scaling).add(Vector2.new(scaling / 2, scaling / 2)));
} else {
break;
}
}
let path = new Path(path_arr);
let p = path.next();
let id = sim.add_agent(p);
agents.push(id) ;
goals[id] = path.next();
lastpotion[id] = [p, 0];
paths[id] = path;
let div = document.createElement("div")
div.id = id;
div.style.left = Math.floor(p.x()) + "px";
div.style.top = Math.floor(p.y()) + "px";
div.style.width = "4px";
div.style.height = "4px";
div.style.borderRadius = "50%";
div.style.backgroundColor = "rgb(255, 0, 0)";
div.style.position = "absolute";
document.body.appendChild(div);
divs[id] = div;
}
init().then(module => {
let map = TileMap.new(mapwidth, mapHeight);
let a_star = AStar.new(mapwidth, mapHeight, 1000);
let sim = RVOSimulator.default(2000, );
sim.set_time_step(0.25);
sim.set_agent_defaults(10.0, 10, 5.0, 5.0, 1.5, DefaultMaxSpeed, Vector2.default());
let canvas = document.createElement("canvas");
canvas.width = mapwidth * scaling;
canvas.height = mapHeight * scaling;
document.body.appendChild(canvas);
let ctx = canvas.getContext('2d');
ctx.strokeStyle = 'rgb(255,0,0)';
ctx.fillStyle = "rgba(100, 100, 100, 0.5)";
ctx.lineWidth = 1;
for (let i = 0; i <= canvas.width; i += scaling) {
ctx.beginPath();
ctx.moveTo(i, 0);
ctx.lineTo(i, canvas.height);
ctx.stroke();
}
for (let j = 0; j <= canvas.height; j += scaling) {
ctx.beginPath();
ctx.moveTo(0, j);
ctx.lineTo(canvas.width, j);
ctx.stroke();
}
setObstacle(map, sim, ctx, Vector2.new(4, 0), Vector2.new(6, 4));
setObstacle(map, sim, ctx, Vector2.new(4, 5), Vector2.new(6, 10));
addAgent(a_star, map, sim, Vector2.new(9, 9), Vector2.new(1, 9));
addAgent(a_star, map, sim, Vector2.new(8, 9), Vector2.new(0, 9));
addAgent(a_star, map, sim, Vector2.new(9, 8), Vector2.new(1, 8));
addAgent(a_star, map, sim, Vector2.new(8, 8), Vector2.new(0, 8));
let r = 0;
let id = setInterval(() => {
update_divs(sim, divs);
set_preferred_velocities(sim, goals);
sim.do_step();
if (reached_goal(sim, a_star, map)) {
clearInterval(id);
}
}, 16)
});
</script>
</body>
<style>
canvas {
position: absolute;
left: 0px;
top: 0px;
}
</style>
</html>