the demo

assets/Scripts/RVO/Agent.ts

@@ -0,0 +1,557 @@
+import { KeyValuePair, Line, Obstacle, RVOMath, Vector2 } from "./Common";
+import { Simulator } from "./Simulator";
+
+
+export class Agent {
+    agentNeighbors_: KeyValuePair<number, Agent>[] = [];
+    obstaclNeighbors_: KeyValuePair<number, Obstacle>[] = [];
+    orcaLines_: Line[] = [];
+    position_: Vector2 = new Vector2(0, 0);
+    prefVelocity_: Vector2 = new Vector2(0, 0);
+    velocity_: Vector2 = new Vector2(0, 0);
+    id: number = 0;
+    maxNeighbors_: number = 0;
+    maxSpeed_: number = 0.0;
+    private _neighborDist: number = 0.0;
+    public get neighborDist(): number {
+        return this._neighborDist;
+    }
+    public set neighborDist(value: number) {
+        this._neighborDist = value;
+    }
+    radius_: number = 0.0;
+    timeHorizon: number = 0.0;
+    timeHorizonObst: number = 0.0;
+    newVelocity_: Vector2 = new Vector2(0, 0);
+    mass: number = 1;
+
+
+    computeNeighbors(sim: Simulator) {
+        this.obstaclNeighbors_.length = 0;
+        let rangeSq = (this.timeHorizonObst * this.maxSpeed_ + this.radius_) ** 2;
+        sim.kdTree.computeObstacleNeighbors(this, rangeSq);
+
+        this.agentNeighbors_.length = 0;
+
+        if (this.maxNeighbors_ > 0) {
+            rangeSq = this.neighborDist ** 2;
+            rangeSq = sim.kdTree.computeAgentNeighbors(this, rangeSq);
+        }
+    }
+
+    /* Search for the best new velocity. */
+    computeNewVelocity(dt: number) {
+        this.orcaLines_.length = 0;
+        let orcaLines = this.orcaLines_;
+
+        let invTimeHorizonObst = 1.0 / this.timeHorizonObst;
+
+        /* Create obstacle ORCA lines. */
+        for (let i = 0; i < this.obstaclNeighbors_.length; ++i) {
+            let obstacle1 = this.obstaclNeighbors_[i].value;
+            let obstacle2 = obstacle1.next;
+
+            let relativePosition1 = obstacle1.point.minus(this.position_);
+            let relativePosition2 = obstacle2.point.minus(this.position_);
+
+            /* 
+             * Check if velocity obstacle of obstacle is already taken care of by
+             * previously constructed obstacle ORCA lines.
+             */
+            let alreadyCovered = false;
+
+            for (let j = 0; j < orcaLines.length; ++j) {
+                if (RVOMath.det(relativePosition1.scale(invTimeHorizonObst).minus(orcaLines[j].point), orcaLines[j].direction) - invTimeHorizonObst * this.radius_ >= -RVOMath.RVO_EPSILON
+                    && RVOMath.det(relativePosition2.scale(invTimeHorizonObst).minus(orcaLines[j].point), orcaLines[j].direction) - invTimeHorizonObst * this.radius_ >= -RVOMath.RVO_EPSILON) {
+
+                    alreadyCovered = true;
+                    break;
+                }
+            }
+
+            if (alreadyCovered) {
+                continue;
+            }
+
+            /* Not yet covered. Check for collisions. */
+
+            let distSq1 = RVOMath.absSq(relativePosition1);
+            let distSq2 = RVOMath.absSq(relativePosition2);
+
+            let radiusSq = RVOMath.sqr(this.radius_);
+
+            let obstacleVector = obstacle2.point.minus(obstacle1.point);
+            let s = relativePosition1.scale(-1).multiply(obstacleVector) / RVOMath.absSq(obstacleVector);
+            let distSqLine = RVOMath.absSq(relativePosition1.scale(-1).minus(obstacleVector.scale(s)));
+
+            let line = new Line();
+            if (s < 0 && distSq1 <= radiusSq) {
+                /* Collision with left vertex. Ignore if non-convex. */
+                if (obstacle1.convex) {
+                    line.point = new Vector2(0, 0);
+                    line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y, relativePosition1.x));
+                    orcaLines.push(line);
+                }
+                continue;
+            }
+            else if (s > 1 && distSq2 <= radiusSq) {
+                /* Collision with right vertex. Ignore if non-convex 
+                 * or if it will be taken care of by neighoring obstace */
+                if (obstacle2.convex && RVOMath.det(relativePosition2, obstacle2.direction) >= 0) {
+                    line.point = new Vector2(0, 0);
+                    line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y, relativePosition2.x));
+                    orcaLines.push(line);
+                }
+                continue;
+            }
+            else if (s >= 0 && s <= 1 && distSqLine <= radiusSq) {
+                /* Collision with obstacle segment. */
+                line.point = new Vector2(0, 0);
+                line.direction = obstacle1.direction.scale(-1);
+                orcaLines.push(line);
+                continue;
+            }
+
+            /* 
+             * No collision.  
+             * Compute legs. When obliquely viewed, both legs can come from a single
+             * vertex. Legs extend cut-off line when nonconvex vertex.
+             */
+            let leftLegDirection: Vector2, rightLegDirection: Vector2;
+
+            if (s < 0 && distSqLine <= radiusSq) {
+                /*
+                 * Obstacle viewed obliquely so that left vertex
+                 * defines velocity obstacle.
+                 */
+                if (!obstacle1.convex) {
+                    /* Ignore obstacle. */
+                    continue;
+                }
+
+                obstacle2 = obstacle1;
+
+                let leg1 = Math.sqrt(distSq1 - radiusSq);
+                leftLegDirection = (new Vector2(relativePosition1.x * leg1 - relativePosition1.y * this.radius_, relativePosition1.x * this.radius_ + relativePosition1.y * leg1)).scale(1 / distSq1);
+                rightLegDirection = (new Vector2(relativePosition1.x * leg1 + relativePosition1.y * this.radius_, -relativePosition1.x * this.radius_ + relativePosition1.y * leg1)).scale(1 / distSq1);
+            }
+            else if (s > 1 && distSqLine <= radiusSq) {
+                /*
+                 * Obstacle viewed obliquely so that
+                 * right vertex defines velocity obstacle.
+                 */
+                if (!obstacle2.convex) {
+                    /* Ignore obstacle. */
+                    continue;
+                }
+
+                obstacle1 = obstacle2;
+
+                let leg2 = Math.sqrt(distSq2 - radiusSq);
+                leftLegDirection = (new Vector2(relativePosition2.x * leg2 - relativePosition2.y * this.radius_, relativePosition2.x * this.radius_ + relativePosition2.y * leg2)).scale(1 / distSq2);
+                rightLegDirection = (new Vector2(relativePosition2.x * leg2 + relativePosition2.y * this.radius_, -relativePosition2.x * this.radius_ + relativePosition2.y * leg2)).scale(1 / distSq2);
+            }
+            else {
+                /* Usual situation. */
+                if (obstacle1.convex) {
+                    let leg1 = Math.sqrt(distSq1 - radiusSq);
+                    leftLegDirection = (new Vector2(relativePosition1.x * leg1 - relativePosition1.y * this.radius_, relativePosition1.x * this.radius_ + relativePosition1.y * leg1)).scale(1 / distSq1);
+                }
+                else {
+                    /* Left vertex non-convex; left leg extends cut-off line. */
+                    leftLegDirection = obstacle1.direction.scale(-1);
+                }
+
+                if (obstacle2.convex) {
+                    let leg2 = Math.sqrt(distSq2 - radiusSq);
+                    rightLegDirection = (new Vector2(relativePosition2.x * leg2 + relativePosition2.y * this.radius_, -relativePosition2.x * this.radius_ + relativePosition2.y * leg2)).scale(1 / distSq2);
+                }
+                else {
+                    /* Right vertex non-convex; right leg extends cut-off line. */
+                    rightLegDirection = obstacle1.direction;
+                }
+            }
+
+            /* 
+             * Legs can never point into neighboring edge when convex vertex,
+             * take cutoff-line of neighboring edge instead. If velocity projected on
+             * "foreign" leg, no constraint is added. 
+             */
+
+            let leftNeighbor = obstacle1.previous;
+
+            let isLeftLegForeign = false;
+            let isRightLegForeign = false;
+
+            if (obstacle1.convex && RVOMath.det(leftLegDirection, leftNeighbor.direction.scale(-1)) >= 0.0) {
+                /* Left leg points into obstacle. */
+                leftLegDirection = leftNeighbor.direction.scale(-1);
+                isLeftLegForeign = true;
+            }
+
+            if (obstacle2.convex && RVOMath.det(rightLegDirection, obstacle2.direction) <= 0.0) {
+                /* Right leg points into obstacle. */
+                rightLegDirection = obstacle2.direction;
+                isRightLegForeign = true;
+            }
+
+            /* Compute cut-off centers. */
+            let leftCutoff = obstacle1.point.minus(this.position_).scale(invTimeHorizonObst);
+            let rightCutoff = obstacle2.point.minus(this.position_).scale(invTimeHorizonObst);
+            let cutoffVec = rightCutoff.minus(leftCutoff);
+
+            /* Project current velocity on velocity obstacle. */
+
+            /* Check if current velocity is projected on cutoff circles. */
+            let t = (obstacle1 == obstacle2) ? 0.5 : this.velocity_.minus(leftCutoff).multiply(cutoffVec) / RVOMath.absSq(cutoffVec);
+            let tLeft = this.velocity_.minus(leftCutoff).multiply(leftLegDirection);
+            let tRight = this.velocity_.minus(rightCutoff).multiply(rightLegDirection);
+
+            if ((t < 0.0 && tLeft < 0.0) || (obstacle1 == obstacle2 && tLeft < 0.0 && tRight < 0.0)) {
+                /* Project on left cut-off circle. */
+                let unitW = RVOMath.normalize(this.velocity_.minus(leftCutoff));
+
+                line.direction = new Vector2(unitW.y, -unitW.x);
+                line.point = leftCutoff.plus(unitW.scale(this.radius_ * invTimeHorizonObst));
+                orcaLines.push(line);
+                continue;
+            }
+            else if (t > 1.0 && tRight < 0.0) {
+                /* Project on right cut-off circle. */
+                let unitW = RVOMath.normalize(this.velocity_.minus(rightCutoff));
+
+                line.direction = new Vector2(unitW.y, -unitW.x);
+                line.point = rightCutoff.plus(unitW.scale(this.radius_ * invTimeHorizonObst));
+                orcaLines.push(line);
+                continue;
+            }
+
+            /* 
+             * Project on left leg, right leg, or cut-off line, whichever is closest
+             * to velocity.
+             */
+            let distSqCutoff = ((t < 0.0 || t > 1.0 || obstacle1 == obstacle2) ? Infinity : RVOMath.absSq(this.velocity_.minus(cutoffVec.scale(t).plus(leftCutoff))));
+            let distSqLeft = ((tLeft < 0.0) ? Infinity : RVOMath.absSq(this.velocity_.minus(leftLegDirection.scale(tLeft).plus(leftCutoff))));
+            let distSqRight = ((tRight < 0.0) ? Infinity : RVOMath.absSq(this.velocity_.minus(rightLegDirection.scale(tRight).plus(rightCutoff))));
+
+            if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
+                /* Project on cut-off line. */
+                line.direction = obstacle1.direction.scale(-1);
+                let aux = new Vector2(-line.direction.y, line.direction.x);
+                line.point = aux.scale(this.radius_ * invTimeHorizonObst).plus(leftCutoff);
+                orcaLines.push(line);
+                continue;
+            }
+            else if (distSqLeft <= distSqRight) {
+                /* Project on left leg. */
+                if (isLeftLegForeign) {
+                    continue;
+                }
+
+                line.direction = leftLegDirection;
+                let aux = new Vector2(-line.direction.y, line.direction.x);
+                line.point = aux.scale(this.radius_ * invTimeHorizonObst).plus(leftCutoff);
+                orcaLines.push(line);
+                continue;
+            }
+            else {
+                /* Project on right leg. */
+                if (isRightLegForeign) {
+                    continue;
+                }
+
+                line.direction = rightLegDirection.scale(-1);
+                let aux = new Vector2(-line.direction.y, line.direction.x);
+                line.point = aux.scale(this.radius_ * invTimeHorizonObst).plus(rightCutoff);
+                orcaLines.push(line);
+                continue;
+            }
+        }
+
+        let numObstLines = orcaLines.length;
+
+        let invTimeHorizon = 1.0 / this.timeHorizon;
+
+        /* Create agent ORCA lines. */
+        for (let i = 0; i < this.agentNeighbors_.length; ++i) {
+            let other = this.agentNeighbors_[i].value;
+
+            let relativePosition = other.position_.minus(this.position_);
+
+            // mass
+            let massRatio = (other.mass / (this.mass + other.mass));
+            let neighborMassRatio = (this.mass / (this.mass + other.mass));
+
+            let velocityOpt = (massRatio >= 0.5 ? (this.velocity_.minus(this.velocity_.scale(massRatio)).scale(2)) : this.prefVelocity_.plus(this.velocity_.minus(this.prefVelocity_).scale(massRatio * 2)));
+            let neighborVelocityOpt = (neighborMassRatio >= 0.5 ? other.velocity_.scale(2).scale(1 - neighborMassRatio) : (other.prefVelocity_.plus(other.velocity_.minus(other.prefVelocity_).scale(2 * neighborMassRatio))));
+
+            let relativeVelocity = velocityOpt.minus(neighborVelocityOpt);//this.velocity.minus(other.velocity);
+            let distSq = RVOMath.absSq(relativePosition);
+            let combinedRadius = this.radius_ + other.radius_;
+            let combinedRadiusSq = RVOMath.sqr(combinedRadius);
+
+            let line = new Line();
+            let u: Vector2;
+
+            if (distSq > combinedRadiusSq) {
+                /* No collision. */
+                let w = relativeVelocity.minus(relativePosition.scale(invTimeHorizon)); // Vector
+                /* Vector from cutoff center to relative velocity. */
+                let wLengthSq = RVOMath.absSq(w);
+
+                let dotProduct1 = w.multiply(relativePosition);
+
+                if (dotProduct1 < 0.0 && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
+                    /* Project on cut-off circle. */
+                    let wLength = Math.sqrt(wLengthSq);
+                    let unitW = w.scale(1 / wLength);
+
+                    line.direction = new Vector2(unitW.y, -unitW.x);
+                    u = unitW.scale(combinedRadius * invTimeHorizon - wLength);
+                }
+                else {
+                    /* Project on legs. */
+                    let leg = Math.sqrt(distSq - combinedRadiusSq);
+
+                    if (RVOMath.det(relativePosition, w) > 0.0) {
+                        /* Project on left leg. */
+                        let aux = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg);
+                        line.direction = aux.scale(1 / distSq);
+                    }
+                    else {
+                        /* Project on right leg. */
+                        let aux = new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg);
+                        line.direction = aux.scale(-1 / distSq);
+                    }
+
+                    let dotProduct2 = relativeVelocity.multiply(line.direction);
+                    u = line.direction.scale(dotProduct2).minus(relativeVelocity);
+                }
+            }
+            else {
+                /* Collision. Project on cut-off circle of time timeStep. */
+                let invTimeStep = 1.0 / dt;
+
+                /* Vector from cutoff center to relative velocity. */
+                let w = relativeVelocity.minus(relativePosition.scale(invTimeStep));
+
+                let wLength = RVOMath.abs(w);
+                let unitW = w.scale(1 / wLength);
+
+                line.direction = new Vector2(unitW.y, -unitW.x);
+                u = unitW.scale(combinedRadius * invTimeStep - wLength);
+            }
+
+
+            // line.point = u.scale(0.5).plus(this.velocity);
+            line.point = velocityOpt.plus(u.scale(massRatio));
+            orcaLines.push(line);
+        }
+
+        let lineFail = this.linearProgram2(orcaLines, this.maxSpeed_, this.prefVelocity_, false, this.newVelocity_);
+
+        if (lineFail < orcaLines.length) {
+            this.linearProgram3(orcaLines, numObstLines, lineFail, this.maxSpeed_, this.newVelocity_);
+        }
+    }
+
+    insertAgentNeighbor(agent: Agent, rangeSq: number) {
+        if (this != agent) {
+            let distSq = RVOMath.absSq(this.position_.minus(agent.position_));
+
+            if (distSq < rangeSq) {
+                if (this.agentNeighbors_.length < this.maxNeighbors_) {
+                    this.agentNeighbors_.push(new KeyValuePair(distSq, agent));
+                }
+                let i = this.agentNeighbors_.length - 1;
+                while (i != 0 && distSq < this.agentNeighbors_[i - 1].key) {
+                    this.agentNeighbors_[i] = this.agentNeighbors_[i - 1];
+                    --i;
+                }
+                this.agentNeighbors_[i] = new KeyValuePair<number, Agent>(distSq, agent);
+
+                if (this.agentNeighbors_.length == this.maxNeighbors_) {
+                    rangeSq = this.agentNeighbors_[this.agentNeighbors_.length - 1].key;
+                }
+            }
+        }
+        return rangeSq;
+    }
+
+    insertObstacleNeighbor(obstacle: Obstacle, rangeSq: number) {
+        let nextObstacle = obstacle.next;
+
+        let distSq = RVOMath.distSqPointLineSegment(obstacle.point, nextObstacle.point, this.position_);
+
+        if (distSq < rangeSq) {
+            this.obstaclNeighbors_.push(new KeyValuePair<number, Obstacle>(distSq, obstacle));
+
+            let i = this.obstaclNeighbors_.length - 1;
+            while (i != 0 && distSq < this.obstaclNeighbors_[i - 1].key) {
+                this.obstaclNeighbors_[i] = this.obstaclNeighbors_[i - 1];
+                --i;
+            }
+            this.obstaclNeighbors_[i] = new KeyValuePair<number, Obstacle>(distSq, obstacle);
+        }
+    }
+
+    update(dt: number) {
+        this.velocity_.copy(this.newVelocity_);
+        this.position_.copy(this.position_.plus(this.velocity_.scale(dt)));
+    };
+
+    linearProgram1(lines: Line[], lineNo: number, radius: number, optVelocity: Vector2, directionOpt: boolean, result: Vector2) {
+        let dotProduct = lines[lineNo].point.multiply(lines[lineNo].direction);
+        let discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);
+
+        if (discriminant < 0.0) {
+            /* Max speed circle fully invalidates line lineNo. */
+            return false;
+        }
+
+        let sqrtDiscriminant = Math.sqrt(discriminant);
+        let tLeft = -dotProduct - sqrtDiscriminant;
+        let tRight = -dotProduct + sqrtDiscriminant;
+
+        for (let i = 0; i < lineNo; ++i) {
+            let denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
+            let numerator = RVOMath.det(lines[i].direction, lines[lineNo].point.minus(lines[i].point));
+
+            if (Math.abs(denominator) <= RVOMath.RVO_EPSILON) {
+                /* Lines lineNo and i are (almost) parallel. */
+                if (numerator < 0.0) {
+                    return false;
+                }
+                else {
+                    continue;
+                }
+            }
+
+            let t = numerator / denominator;
+
+            if (denominator >= 0.0) {
+                /* Line i bounds line lineNo on the right. */
+                tRight = Math.min(tRight, t);
+            }
+            else {
+                /* Line i bounds line lineNo on the left. */
+                tLeft = Math.max(tLeft, t);
+            }
+
+            if (tLeft > tRight) {
+                return false;
+            }
+        }
+
+        if (directionOpt) {
+            if (optVelocity.multiply(lines[lineNo].direction) > 0.0) {
+                // Take right extreme
+                result.copy(lines[lineNo].point.plus(lines[lineNo].direction.scale(tRight)));
+            }
+            else {
+                // Take left extreme.
+                result.copy(lines[lineNo].point.plus(lines[lineNo].direction.scale(tLeft)));
+            }
+        }
+        else {
+            // Optimize closest point
+            let t = lines[lineNo].direction.multiply(optVelocity.minus(lines[lineNo].point));
+            if (t < tLeft) {
+                result.copy(lines[lineNo].point.plus(lines[lineNo].direction.scale(tLeft)));
+            }
+            else if (t > tRight) {
+                result.copy(lines[lineNo].point.plus(lines[lineNo].direction.scale(tRight)));
+            }
+            else {
+                result.copy(lines[lineNo].point.plus(lines[lineNo].direction.scale(t)));
+            }
+        }
+
+        return true;
+    }
+
+    linearProgram2(lines: Line[], radius: number, optVelocity: Vector2, directionOpt: boolean, result: Vector2) {
+        // directionOpt 第一次为false，第二次为true，directionOpt主要用在 linearProgram1 里面
+        if (directionOpt) {
+            /* 
+             * Optimize direction. Note that the optimization velocity is of unit
+             * length in this case.
+             */
+            result.copy(optVelocity.scale(radius));
+        }
+        else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius)) {
+            /* Optimize closest point and outside circle. */
+            result.copy(RVOMath.normalize(optVelocity).scale(radius));
+        }
+        else {
+            /* Optimize closest point and inside circle. */
+            result.copy(optVelocity);
+        }
+
+        for (let i = 0; i < lines.length; ++i) {
+            if (RVOMath.det(lines[i].direction, lines[i].point.minus(result)) > 0.0) {
+                /* Result does not satisfy constraint i. Compute new optimal result. */
+                let tempResult = result.clone();
+                if (!this.linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
+                    result.copy(tempResult);
+                    return i;
+                }
+            }
+        }
+
+        return lines.length;
+    }
+
+    linearProgram3(lines: Line[], numObstLines: number, beginLine: number, radius: number, result: Vector2) {
+        let distance = 0.0;
+        // 遍历所有剩余ORCA线
+        for (let i = beginLine; i < lines.length; ++i) {
+            // 每一条 ORCA 线都需要精确的做出处理，distance 为 最大违规的速度
+            if (RVOMath.det(lines[i].direction, lines[i].point.minus(result)) > distance) {
+                /* Result does not satisfy constraint of line i. */
+                //std::vector<Line> projLines(lines.begin(), lines.begin() + numObstLines);
+                let projLines = []; // new List<Line>();
+                // 1.静态阻挡的orca线直接加到projLines中
+                for (let ii = 0; ii < numObstLines; ++ii) {
+                    projLines.push(lines[ii]);
+                }
+                // 2.动态阻挡的orca线需要重新计算line，从第一个非静态阻挡到当前的orca线
+                for (let j = numObstLines; j < i; ++j) {
+                    let line = new Line();
+
+                    let determinant = RVOMath.det(lines[i].direction, lines[j].direction);
+
+                    if (Math.abs(determinant) <= RVOMath.RVO_EPSILON) {
+                        /* Line i and line j are parallel. */
+                        if (lines[i].direction.multiply(lines[j].direction) > 0.0) {
+                            /* Line i and line j point in the same direction. */
+                            continue;
+                        }
+                        else {
+                            /* Line i and line j point in opposite direction. */
+                            line.point = lines[i].point.plus(lines[j].point).scale(0.5);
+                        }
+                    }
+                    else {
+                        line.point = lines[i].point.plus(lines[i].direction.scale(RVOMath.det(lines[j].direction, lines[i].point.minus(lines[j].point)) / determinant));
+                    }
+
+                    line.direction = RVOMath.normalize(lines[j].direction.minus(lines[i].direction));
+                    projLines.push(line);
+                }
+
+                let tempResult = result.clone();
+                if (this.linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y, lines[i].direction.x), true, result) < projLines.length) {
+                    /* This should in principle not happen.  The result is by definition
+                     * already in the feasible region of this linear program. If it fails,
+                     * it is due to small floating point error, and the current result is
+                     * kept.
+                     */
+                    result.copy(tempResult);
+                }
+
+                distance = RVOMath.det(lines[i].direction, lines[i].point.minus(result));
+            }
+        }
+    }
+}

assets/Scripts/RVO/Agent.ts.meta

@@ -0,0 +1,12 @@
+{
+  "ver": "4.0.23",
+  "importer": "typescript",
+  "imported": true,
+  "uuid": "99d7f5fd-fae6-4106-ba7a-93c1e298d001",
+  "files": [],
+  "subMetas": {},
+  "userData": {
+    "moduleId": "project:///assets/Scripts/GameCore/RVO2/Agent.js",
+    "simulateGlobals": []
+  }
+}

assets/Scripts/RVO/Common.ts

@@ -0,0 +1,117 @@
+import { Vec2 } from "cc";
+
+export class Vector2 {
+    x = 0;
+    y = 0;
+
+    constructor(x: number, y: number) {
+        this.x = x;
+        this.y = y;
+    }
+
+    plus(vector: Vector2) {
+        return new Vector2(this.x + vector.x, this.y + vector.y);
+    }
+
+    minus(vector: Vector2) {
+        return new Vector2(this.x - vector.x, this.y - vector.y);
+    }
+
+    multiply(vector: Vector2) {
+        return this.x * vector.x + this.y * vector.y;
+    }
+
+    scale(k: number) {
+        return new Vector2(this.x * k, this.y * k);
+    }
+
+    copy(v: Vector2 | Vec2) {
+        this.x = v.x;
+        this.y = v.y;
+        return this;
+    }
+
+    clone() {
+        return new Vector2(this.x, this.y);
+    }
+
+    substract(out: Vector2, other: Vector2) {
+        out.x -= other.x;
+        out.y -= other.y;
+        return out;
+    }
+
+    lengthSqr() {
+        return this.x ** 2 + this.y ** 2;
+    }
+}
+
+export class Obstacle {
+    next: Obstacle;
+    previous: Obstacle;
+    direction: Vector2;
+    point: Vector2;
+    id: number;
+    convex: boolean;
+}
+
+export class Line {
+    point: Vector2;
+    direction: Vector2;
+}
+
+export class KeyValuePair<K, V> {
+    key: K;
+    value: V;
+    constructor(key: K, value: V) {
+        this.key = key;
+        this.value = value;
+    }
+}
+
+export class RVOMath {
+
+    static RVO_EPSILON = 0.00001;
+
+    static absSq(v: Vector2) {
+        return v.multiply(v);
+    };
+
+    static normalize(v: Vector2) {
+        return v.scale(1 / RVOMath.abs(v)); // v / abs(v)
+    };
+
+    static distSqPointLineSegment(vector1: Vector2, vector2: Vector2, vector3: Vector2) {
+        let aux1 = vector3.minus(vector1);
+        let aux2 = vector2.minus(vector1);
+
+        let r = aux1.multiply(aux2) / RVOMath.absSq(aux2);
+
+        if (r < 0) {
+            return RVOMath.absSq(aux1);
+        }
+        else if (r > 1) {
+            return RVOMath.absSq(vector3.minus(vector2));
+        }
+        else {
+            return RVOMath.absSq(vector3.minus(vector1.plus(aux2.scale(r))));
+        }
+    };
+
+    static sqr(p: number) {
+        return p * p;
+    };
+
+    static det(v1: Vector2, v2: Vector2) {
+        return v1.x * v2.y - v1.y * v2.x;
+    };
+
+    static abs(v: Vector2) {
+        return Math.sqrt(RVOMath.absSq(v));
+    };
+
+    static leftOf(a: Vector2, b: Vector2, c: Vector2) {
+        return RVOMath.det(a.minus(c), b.minus(a));
+    };
+
+}

assets/Scripts/RVO/Common.ts.meta

@@ -0,0 +1,12 @@
+{
+  "ver": "4.0.23",
+  "importer": "typescript",
+  "imported": true,
+  "uuid": "b21abffd-42c6-4d43-bbe0-0edd2fb59f53",
+  "files": [],
+  "subMetas": {},
+  "userData": {
+    "moduleId": "project:///assets/Scripts/GameCore/RVO2/Common.js",
+    "simulateGlobals": []
+  }
+}

assets/Scripts/RVO/Simulator.ts

@@ -0,0 +1,236 @@
+import { Vec2 } from "cc";
+import { Agent } from "./Agent";
+import { Obstacle, RVOMath, Vector2 } from "./Common";
+import { KdTree } from "./kdtree";
+
+
+export class Simulator {
+    private agentId: number = 0;
+    private agentIdLst: number[] = [];
+    aid2agent: { [key: string]: Agent } = Object.create(null);
+
+    obstacles: Obstacle[] = [];
+    kdTree: KdTree = new KdTree();
+
+
+    defaultAgent: Agent; // Agent
+    time: number = 0.0;
+
+    private static _inst: Simulator;
+    static get instance(): Simulator {
+        if (!Simulator._inst) {
+            Simulator._inst = new Simulator();
+        }
+        return Simulator._inst;
+    }
+
+    getAgent(idx: number) {
+        return this.aid2agent[this.agentIdLst[idx]];
+    }
+
+    getAgentByAid(aid: number) {
+        return this.aid2agent[aid];
+    }
+
+    getGlobalTime() {
+        return this.time;
+    };
+
+    getNumAgents() {
+        // console.log("getNumAgents ::", this.agentIdLst.length, this.agentIdLst)
+        return this.agentIdLst.length;
+    };
+
+    getAgentAidByIdx(idx: number) {
+        return this.agentIdLst[idx]
+    }
+
+
+    setAgentPrefVelocity(aid: number, velocity: Vector2 | Vec2) {
+        this.aid2agent[aid].prefVelocity_.copy(velocity);
+    }
+
+    getAgentPosition(aid: number) {
+        if (this.aid2agent[aid]) {//为什么移除了 还会进入这个aid的检测
+            return this.aid2agent[aid].position_;
+        }
+        return null
+
+    }
+
+    getAgentPrefVelocity(aid: number) {
+        return this.aid2agent[aid].prefVelocity_;
+    }
+
+    getAgentVelocity(aid: number) {
+        return this.aid2agent[aid].velocity_;
+    }
+
+    getAgentRadius(aid: number) {
+        return this.aid2agent[aid].radius_;
+    }
+
+    getAgentOrcaLines(aid: number) {
+        return this.aid2agent[aid].orcaLines_;
+    }
+
+    /**
+     * 添加动态避障管理对象
+     * @param position 初始位置
+     * @param radius  检测半径
+     * @param maxSpeed  最大速度
+     * @param velocity 初始线速度(向量)
+     * @param mass 转向质量
+     * @returns 
+     */
+    addAgent(position: Vector2 | Vec2, radius: number = null, maxSpeed: number = null, velocity: Vector2 = null, mass: number = null) {
+        if (!this.defaultAgent) {
+            throw new Error("no default agent");
+        }
+
+        let agent = new Agent();
+
+        agent.position_.copy(position);
+        agent.maxNeighbors_ = this.defaultAgent.maxNeighbors_;
+        agent.maxSpeed_ = maxSpeed || this.defaultAgent.maxSpeed_;
+        agent.neighborDist = this.defaultAgent.neighborDist;
+        agent.radius_ = radius || this.defaultAgent.radius_;
+        agent.timeHorizon = this.defaultAgent.timeHorizon;
+        agent.timeHorizonObst = this.defaultAgent.timeHorizonObst;
+        agent.velocity_.copy(velocity || this.defaultAgent.velocity_);
+
+        agent.id = this.agentId++;
+
+        if (mass && mass >= 0) {
+            agent.mass = mass
+        }
+        this.aid2agent[agent.id] = agent;
+        this.agentIdLst.push(agent.id);
+
+        return agent.id;
+    }
+
+    removeAgent(aid: number) {
+        if (this.hasAgent(aid)) {
+
+            let idx = this.agentIdLst.indexOf(aid);
+            if (idx >= 0) {
+                // this.agentIdLst.splice(idx, 1) //用高效伪移除
+                this.agentIdLst[idx] = this.agentIdLst[this.agentIdLst.length - 1];
+                this.agentIdLst.length--;
+            }
+            delete this.aid2agent[aid];
+        }
+    }
+
+    hasAgent(aid: number) {
+        return !!this.aid2agent[aid];
+    }
+
+    setAgentMass(agentNo: number, mass: number) {
+        this.aid2agent[agentNo].mass = mass;
+    }
+
+    getAgentMass(agentNo: number) {
+        return this.aid2agent[agentNo].mass;
+    }
+
+    setAgentRadius(agentNo: number, radius: number) {
+        this.aid2agent[agentNo].radius_ = radius;
+    }
+
+    /**
+     * 
+     * @param neighborDist 在寻找周围邻居的搜索距离，这个值设置过大，会让小球在很远距离时做出避障行为
+     * @param maxNeighbors 寻找周围邻居的最大数目，这个值设置越大，最终计算的速度越精确，但会增大计算量
+     * @param timeHorizon 代表计算动态的物体时的时间窗口
+     * @param timeHorizonObst 代表计算静态的物体时的时间窗口，比如在RTS游戏中，小兵向城墙移动时，没必要做出避障，这个值需要 设置得很小
+     * @param radius 代表计算ORCA时的小球的半径，这个值不一定与小球实际显示的半径一样，偏小有利于小球移动顺畅
+     * @param maxSpeed 小球最大速度值
+     * @param velocity 小球初始速度
+     */
+    setAgentDefaults(neighborDist: number, maxNeighbors: number, timeHorizon: number, timeHorizonObst: number, radius: number, maxSpeed: number, velocity: Vector2) {
+        if (!this.defaultAgent) {
+            this.defaultAgent = new Agent();
+        }
+
+        this.defaultAgent.maxNeighbors_ = maxNeighbors;
+        this.defaultAgent.maxSpeed_ = maxSpeed;
+        this.defaultAgent.neighborDist = neighborDist;
+        this.defaultAgent.radius_ = radius;
+        this.defaultAgent.timeHorizon = timeHorizon;
+        this.defaultAgent.timeHorizonObst = timeHorizonObst;
+        this.defaultAgent.velocity_ = velocity;
+    }
+
+    run(dt: number) {
+        this.kdTree.buildAgentTree(this.getNumAgents());
+        let agentNum = this.agentIdLst.length;
+        for (let i = 0; i < agentNum; i++) {
+            this.aid2agent[this.agentIdLst[i]].computeNeighbors(this);
+            this.aid2agent[this.agentIdLst[i]].computeNewVelocity(dt);
+        }
+        for (let i = 0; i < agentNum; i++) {
+            this.aid2agent[this.agentIdLst[i]].update(dt);
+        }
+
+        this.time += dt;
+    }
+
+
+    addObstacle(vertices: Vector2[]) {
+        if (vertices.length < 2) {
+            return -1;
+        }
+
+        let obstacleNo = this.obstacles.length;
+
+        for (let i = 0; i < vertices.length; ++i) {
+            let obstacle = new Obstacle();
+            obstacle.point = vertices[i];
+            if (i != 0) {
+                obstacle.previous = this.obstacles[this.obstacles.length - 1];
+                obstacle.previous.next = obstacle;
+            }
+            if (i == vertices.length - 1) {
+                obstacle.next = this.obstacles[obstacleNo];
+                obstacle.next.previous = obstacle;
+            }
+            obstacle.direction = RVOMath.normalize(vertices[(i == vertices.length - 1 ? 0 : i + 1)].minus(vertices[i]));
+
+            if (vertices.length == 2) {
+                obstacle.convex = true;
+            }
+            else {
+                obstacle.convex = (RVOMath.leftOf(vertices[(i == 0 ? vertices.length - 1 : i - 1)], vertices[i], vertices[(i == vertices.length - 1 ? 0 : i + 1)]) >= 0);
+            }
+
+            obstacle.id = this.obstacles.length;
+
+            this.obstacles.push(obstacle);
+        }
+
+        return obstacleNo;
+    }
+
+    processObstacles() {
+        this.kdTree.buildObstacleTree();
+    };
+
+    queryVisibility(point1: Vector2, point2: Vector2, radius: number) {
+        return this.kdTree.queryVisibility(point1, point2, radius);
+    };
+
+    getObstacles() {
+        return this.obstacles;
+    }
+
+    clear() {
+        this.agentIdLst.length = 0;
+        this.agentId = 0;
+        this.aid2agent = Object.create(null);
+        this.defaultAgent = null;
+        this.kdTree = new KdTree();
+        this.obstacles.length = 0;
+    }
+}

assets/Scripts/RVO/Simulator.ts.meta

@@ -0,0 +1,12 @@
+{
+  "ver": "4.0.23",
+  "importer": "typescript",
+  "imported": true,
+  "uuid": "c9717561-2e5e-441e-b1de-1b3ca6ee543f",
+  "files": [],
+  "subMetas": {},
+  "userData": {
+    "moduleId": "project:///assets/Scripts/GameCore/RVO2/Simulator.js",
+    "simulateGlobals": []
+  }
+}

assets/Scripts/RVO/kdtree.ts

@@ -0,0 +1,391 @@
+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));
+            }
+        }
+    }
+}        

assets/Scripts/RVO/kdtree.ts.meta

@@ -0,0 +1,12 @@
+{
+  "ver": "4.0.23",
+  "importer": "typescript",
+  "imported": true,
+  "uuid": "a7ee4e36-baac-4720-ba0f-7f779a7fd328",
+  "files": [],
+  "subMetas": {},
+  "userData": {
+    "moduleId": "project:///assets/Scripts/GameCore/RVO2/kdtree.js",
+    "simulateGlobals": []
+  }
+}