mirror of
https://gitee.com/devil_root/snake.git
synced 2025-04-19 05:28:48 +00:00
391 lines
15 KiB
TypeScript
391 lines
15 KiB
TypeScript
import { RVOMath, Obstacle, Vector2 } from "./Common";
|
|
import { Simulator } from "./Simulator";
|
|
import { Agent } from "./Agent";
|
|
|
|
class FloatPair {
|
|
a: number;
|
|
b: number;
|
|
constructor(a: number, b: number) {
|
|
this.a = a;
|
|
this.b = b;
|
|
}
|
|
|
|
lessThan(rhs: FloatPair) {
|
|
return this.a < rhs.a || !(rhs.a < this.a) && this.b < rhs.b;
|
|
}
|
|
|
|
lessEqualThan(rhs: FloatPair) {
|
|
return (this.a == rhs.a && this.b == rhs.b) || this.lessThan(rhs);
|
|
}
|
|
|
|
bigThan(rhs: FloatPair) {
|
|
return !this.lessEqualThan(rhs);
|
|
}
|
|
|
|
bigEqualThan(rhs: FloatPair) {
|
|
return !this.lessThan(rhs);
|
|
}
|
|
}
|
|
|
|
class AgentTreeNode {
|
|
begin: number;
|
|
end: number;
|
|
left: number;
|
|
right: number;
|
|
maxX: number;
|
|
maxY: number;
|
|
minX: number;
|
|
minY: number;
|
|
}
|
|
|
|
class ObstacleTreeNode {
|
|
obstacle: Obstacle;
|
|
left: ObstacleTreeNode;
|
|
right: ObstacleTreeNode;
|
|
}
|
|
|
|
export class KdTree {
|
|
/**
|
|
* The maximum size of an agent k-D tree leaf.
|
|
*/
|
|
MAX_LEAF_SIZE = 10;
|
|
agents: Agent[] = null;
|
|
agentTree: AgentTreeNode[] = [];
|
|
obstacleTree: ObstacleTreeNode = null;
|
|
|
|
|
|
buildAgentTree(agentNum: number) {
|
|
if (!this.agents || this.agents.length != agentNum) {
|
|
this.agents = new Array<Agent>(agentNum);
|
|
for(let i = 0; i < this.agents.length; i++) {
|
|
this.agents[i] = Simulator.instance.getAgent(i);
|
|
}
|
|
|
|
this.agentTree = new Array<AgentTreeNode>(2 * this.agents.length);
|
|
for (let i = 0; i < this.agentTree.length; i++) {
|
|
this.agentTree[i] = new AgentTreeNode();
|
|
}
|
|
}
|
|
|
|
if (this.agents.length != 0) {
|
|
this.buildAgentTreeRecursive(0, this.agents.length, 0);
|
|
}
|
|
}
|
|
|
|
buildObstacleTree() {
|
|
this.obstacleTree = new ObstacleTreeNode();
|
|
let obstacles = new Array<Obstacle>(Simulator.instance.obstacles.length);
|
|
for(let i = 0; i < obstacles.length; i++) {
|
|
obstacles[i] = Simulator.instance.obstacles[i];
|
|
}
|
|
this.obstacleTree = this.buildObstacleTreeRecursive(obstacles);
|
|
}
|
|
|
|
computeAgentNeighbors(agent: Agent, rangeSq: number) {
|
|
return this.queryAgentTreeRecursive(agent, rangeSq, 0);
|
|
}
|
|
|
|
computeObstacleNeighbors(agent: Agent, rangeSq: number) {
|
|
this.queryObstacleTreeRecursive(agent, rangeSq, this.obstacleTree);
|
|
}
|
|
|
|
queryVisibility (q1: Vector2, q2: Vector2, radius: number) {
|
|
return this.queryVisibilityRecursive(q1, q2, radius, this.obstacleTree);
|
|
}
|
|
|
|
buildAgentTreeRecursive(begin: number, end: number, node: number) {
|
|
this.agentTree[node].begin = begin;
|
|
this.agentTree[node].end = end;
|
|
this.agentTree[node].minX = this.agentTree[node].maxX = this.agents[begin].position_.x;
|
|
this.agentTree[node].minY = this.agentTree[node].maxY = this.agents[begin].position_.y;
|
|
|
|
for (let i = begin + 1; i < end; ++i) {
|
|
this.agentTree[node].maxX = Math.max(this.agentTree[node].maxX, this.agents[i].position_.x);
|
|
this.agentTree[node].minX = Math.min(this.agentTree[node].minX, this.agents[i].position_.x);
|
|
this.agentTree[node].maxY = Math.max(this.agentTree[node].maxY, this.agents[i].position_.y);
|
|
this.agentTree[node].minY = Math.min(this.agentTree[node].minY, this.agents[i].position_.y);
|
|
}
|
|
|
|
if (end - begin > this.MAX_LEAF_SIZE) {
|
|
// no leaf node
|
|
let isVertical = (this.agentTree[node].maxX - this.agentTree[node].minX) > (this.agentTree[node].maxY - this.agentTree[node].minY);
|
|
let splitValue = 0.5 * (isVertical ? this.agentTree[node].maxX + this.agentTree[node].minX : this.agentTree[node].maxY + this.agentTree[node].minY);
|
|
|
|
let left = begin;
|
|
let right = end;
|
|
|
|
while (left < right) {
|
|
while (left < right && (isVertical ? this.agents[left].position_.x : this.agents[left].position_.y) < splitValue) {
|
|
++left;
|
|
}
|
|
|
|
while (right > left && (isVertical ? this.agents[right - 1].position_.x : this.agents[right - 1].position_.y) >= splitValue) {
|
|
--right;
|
|
}
|
|
|
|
if (left < right) {
|
|
let tmp = this.agents[left];
|
|
this.agents[left] = this.agents[right - 1];
|
|
this.agents[right - 1] = tmp;
|
|
++left;
|
|
--right;
|
|
}
|
|
}
|
|
|
|
let leftSize = left - begin;
|
|
if (leftSize == 0) {
|
|
++leftSize;
|
|
++left;
|
|
++right;
|
|
}
|
|
|
|
this.agentTree[node].left = node + 1;
|
|
this.agentTree[node].right = node + 2 * leftSize;
|
|
|
|
this.buildAgentTreeRecursive(begin, left, this.agentTree[node].left);
|
|
this.buildAgentTreeRecursive(left, end, this.agentTree[node].right);
|
|
}
|
|
}
|
|
|
|
|
|
buildObstacleTreeRecursive(obstacles: Obstacle[]) {
|
|
if (obstacles.length == 0) {
|
|
return null;
|
|
}
|
|
else {
|
|
let node = new ObstacleTreeNode();
|
|
let optimalSplit = 0;
|
|
let minLeft = obstacles.length;
|
|
let minRight = minLeft;
|
|
|
|
for (let i = 0; i < obstacles.length; ++i) {
|
|
let leftSize = 0;
|
|
let rightSize = 0;
|
|
|
|
let obstacleI1 = obstacles[i];
|
|
let obstacleI2 = obstacleI1.next;
|
|
|
|
for (let j = 0; j < obstacles.length; j++) {
|
|
if (i == j) {
|
|
continue;
|
|
}
|
|
|
|
let obstacleJ1 = obstacles[j];
|
|
let obstacleJ2 = obstacleJ1.next;
|
|
|
|
let j1LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ1.point);
|
|
let j2LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ2.point);
|
|
|
|
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
|
|
++leftSize;
|
|
}
|
|
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
|
|
++rightSize;
|
|
}
|
|
else {
|
|
++leftSize;
|
|
++rightSize;
|
|
}
|
|
|
|
let fp1 = new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize));
|
|
let fp2 = new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight));
|
|
|
|
if (fp1.bigEqualThan(fp2)) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
let fp1 = new FloatPair(Math.max(leftSize, rightSize), Math.min(leftSize, rightSize));
|
|
let fp2 = new FloatPair(Math.max(minLeft, minRight), Math.min(minLeft, minRight));
|
|
|
|
if (fp1.lessThan(fp2)) {
|
|
minLeft = leftSize;
|
|
minRight = rightSize;
|
|
optimalSplit = i;
|
|
}
|
|
}
|
|
|
|
{
|
|
/* Build split node. */
|
|
let leftObstacles: Obstacle[] = [];
|
|
for (let n = 0; n < minLeft; ++n) leftObstacles.push(null);
|
|
|
|
let rightObstacles: Obstacle[] = [];
|
|
for (let n = 0; n < minRight; ++n) rightObstacles.push(null);
|
|
|
|
let leftCounter = 0;
|
|
let rightCounter = 0;
|
|
let i = optimalSplit;
|
|
|
|
let obstacleI1 = obstacles[i];
|
|
let obstacleI2 = obstacleI1.next;
|
|
|
|
for (let j = 0; j < obstacles.length; ++j) {
|
|
if (i == j) {
|
|
continue;
|
|
}
|
|
|
|
let obstacleJ1 = obstacles[j];
|
|
let obstacleJ2 = obstacleJ1.next;
|
|
|
|
let j1LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ1.point);
|
|
let j2LeftOfI = RVOMath.leftOf(obstacleI1.point, obstacleI2.point, obstacleJ2.point);
|
|
|
|
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) {
|
|
leftObstacles[leftCounter++] = obstacles[j];
|
|
}
|
|
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) {
|
|
rightObstacles[rightCounter++] = obstacles[j];
|
|
}
|
|
else {
|
|
/* Split obstacle j. */
|
|
let t = RVOMath.det(obstacleI2.point.minus(obstacleI1.point), obstacleJ1.point.minus(obstacleI1.point)) /
|
|
RVOMath.det(obstacleI2.point.minus(obstacleI1.point), obstacleJ1.point.minus(obstacleJ2.point));
|
|
|
|
let splitpoint = obstacleJ1.point.plus( (obstacleJ2.point.minus(obstacleJ1.point)).scale(t) );
|
|
|
|
let newObstacle = new Obstacle();
|
|
newObstacle.point = splitpoint;
|
|
newObstacle.previous = obstacleJ1;
|
|
newObstacle.next = obstacleJ2;
|
|
newObstacle.convex = true;
|
|
newObstacle.direction = obstacleJ1.direction;
|
|
|
|
newObstacle.id = Simulator.instance.obstacles.length;
|
|
|
|
Simulator.instance.obstacles.push(newObstacle);
|
|
|
|
obstacleJ1.next = newObstacle;
|
|
obstacleJ2.previous = newObstacle;
|
|
|
|
if (j1LeftOfI > 0.0) {
|
|
leftObstacles[leftCounter++] = obstacleJ1;
|
|
rightObstacles[rightCounter++] = newObstacle;
|
|
}
|
|
else {
|
|
rightObstacles[rightCounter++] = obstacleJ1;
|
|
leftObstacles[leftCounter++] = newObstacle;
|
|
}
|
|
}
|
|
}
|
|
|
|
node.obstacle = obstacleI1;
|
|
node.left = this.buildObstacleTreeRecursive(leftObstacles);
|
|
node.right = this.buildObstacleTreeRecursive(rightObstacles);
|
|
return node;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
queryAgentTreeRecursive(agent: Agent, rangeSq: number, node: number) {
|
|
if (this.agentTree[node].end - this.agentTree[node].begin <= this.MAX_LEAF_SIZE) {
|
|
for (let i = this.agentTree[node].begin; i < this.agentTree[node].end; ++i) {
|
|
rangeSq = agent.insertAgentNeighbor(this.agents[i], rangeSq);
|
|
}
|
|
}
|
|
else {
|
|
let distSqLeft = RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].left].minX - agent.position_.x)) +
|
|
RVOMath.sqr(Math.max(0, agent.position_.x - this.agentTree[this.agentTree[node].left].maxX)) +
|
|
RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].left].minY - agent.position_.y)) +
|
|
RVOMath.sqr(Math.max(0, agent.position_.y - this.agentTree[this.agentTree[node].left].maxY));
|
|
|
|
let distSqRight = RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].right].minX - agent.position_.x)) +
|
|
RVOMath.sqr(Math.max(0, agent.position_.x - this.agentTree[this.agentTree[node].right].maxX)) +
|
|
RVOMath.sqr(Math.max(0, this.agentTree[this.agentTree[node].right].minY - agent.position_.y)) +
|
|
RVOMath.sqr(Math.max(0, agent.position_.y - this.agentTree[this.agentTree[node].right].maxY));
|
|
|
|
if (distSqLeft < distSqRight) {
|
|
if (distSqLeft < rangeSq) {
|
|
rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].left);
|
|
|
|
if (distSqRight < rangeSq) {
|
|
rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].right);
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
if (distSqRight < rangeSq) {
|
|
rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].right);
|
|
|
|
if (distSqLeft < rangeSq) {
|
|
rangeSq = this.queryAgentTreeRecursive(agent, rangeSq, this.agentTree[node].left);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
return rangeSq;
|
|
}
|
|
|
|
// pass ref range
|
|
queryObstacleTreeRecursive(agent: Agent, rangeSq: number, node: ObstacleTreeNode) {
|
|
if (node == null) {
|
|
return rangeSq;
|
|
}
|
|
else {
|
|
let obstacle1 = node.obstacle;
|
|
let obstacle2 = obstacle1.next;
|
|
|
|
let agentLeftOfLine = RVOMath.leftOf(obstacle1.point, obstacle2.point, agent.position_);
|
|
|
|
rangeSq = this.queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0 ? node.left : node.right));
|
|
|
|
let distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point.minus(obstacle1.point));
|
|
|
|
if (distSqLine < rangeSq)
|
|
{
|
|
if (agentLeftOfLine < 0)
|
|
{
|
|
/*
|
|
* Try obstacle at this node only if is on right side of
|
|
* obstacle (and can see obstacle).
|
|
*/
|
|
agent.insertObstacleNeighbor(node.obstacle, rangeSq);
|
|
}
|
|
|
|
/* Try other side of line. */
|
|
this.queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0 ? node.right : node.left));
|
|
}
|
|
return rangeSq;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
queryVisibilityRecursive(q1: Vector2, q2: Vector2, radius: number, node: ObstacleTreeNode) {
|
|
if (node == null) {
|
|
return true;
|
|
}
|
|
else {
|
|
let obstacle1 = node.obstacle;
|
|
let obstacle2 = obstacle1.next;
|
|
|
|
let q1LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q1);
|
|
let q2LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q2);
|
|
let invLengthI = 1.0 / RVOMath.absSq(obstacle2.point.minus(obstacle1.point));
|
|
|
|
if (q1LeftOfI >= 0 && q2LeftOfI >= 0)
|
|
{
|
|
return this.queryVisibilityRecursive(q1, q2, radius, node.left) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || this.queryVisibilityRecursive(q1, q2, radius, node.right));
|
|
}
|
|
else if (q1LeftOfI <= 0 && q2LeftOfI <= 0)
|
|
{
|
|
return this.queryVisibilityRecursive(q1, q2, radius, node.right) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || this.queryVisibilityRecursive(q1, q2, radius, node.left));
|
|
}
|
|
else if (q1LeftOfI >= 0 && q2LeftOfI <= 0)
|
|
{
|
|
/* One can see through obstacle from left to right. */
|
|
return this.queryVisibilityRecursive(q1, q2, radius, node.left) && this.queryVisibilityRecursive(q1, q2, radius, node.right);
|
|
}
|
|
else
|
|
{
|
|
let point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point);
|
|
let point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point);
|
|
let invLengthQ = 1.0 / RVOMath.absSq(q2.minus(q1));
|
|
|
|
return (point1LeftOfQ * point2LeftOfQ >= 0 && RVOMath.sqr(point1LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && this.queryVisibilityRecursive(q1, q2, radius, node.left) && this.queryVisibilityRecursive(q1, q2, radius, node.right));
|
|
}
|
|
}
|
|
}
|
|
}
|