コード例 #1
0
ファイル: Node.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Constructor.
 /// </summary>
 /// <param name="PositionX">X coordinate.</param>
 /// <param name="PositionY">Y coordinate.</param>
 /// <param name="PositionZ">Z coordinate.</param>
 public Node(double PositionX, double PositionY, double PositionZ)
 {
     _Position = new Point3D(PositionX, PositionY, PositionZ);
     _Passable = true;
     _IncomingArcs = new ArrayList();
     _OutgoingArcs = new ArrayList();
 }
コード例 #2
0
ファイル: Node.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Constructor.
 /// </summary>
 /// <param name="point">Point3D</param>
 public Node(Point3D point)
 {
     _Position = point;
     _Passable = true;
     _IncomingArcs = new ArrayList();
     _OutgoingArcs = new ArrayList();
 }
コード例 #3
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        /// <summary>
        /// Build route based on project any points to GLONASS arcs
        /// </summary>
        /// <param name="point1">First point (will be not included in path)</param>
        /// <param name="point2">Destinations point</param>
        /// <param name="mdist1">max distance from start point to GLONASS route</param>
        /// <param name="mdist2">max distance from destitation to GLONASS route</param>
        /// <param name="radius">radius of interception (projective) points</param>
        /// <returns>List of path points, if route fail list with only one destination point (for direct run)</returns>
        public List<Point3D> GetArcSplitRoute(Point3D point1, Point3D point2, double mdist1, double mdist2, double radius)
        {
            bool pass = true;
            var rdist1 = mdist1; var rdist2 = mdist2;
            var rndist1 = mdist1; var rndist2 = mdist2;
            var node1 = this.G.ClosestNode(point1, out rndist1, pass);
            var node2 = this.G.ClosestNode(point2, out rndist2, pass);
            var arc1 = this.G.ClosestArc(point1, out rdist1, pass);    // TODO optimize
            var arc2 = this.G.ClosestArc(point2, out rdist2, pass);
            //            if (node1 == node2) {
            //                lasterror = GLONASSLastError.DirectRoute;
            //                return new List<Point3D> { point2 };
            //            }
            //            if (arc1 == arc2) {
            //                lasterror = GLONASSLastError.DirectRoute;
            //                return new List<Point3D> { point2 };
            //            }

            if (rdist1 == -1 || rdist2 == -1 || rndist1 == -1 || rndist2 == -1) {
                lasterror = GLONASSLastError.GraphIsEmpty;
                return new List<Point3D> { point2 };
            }
            if (rdist1 > rndist1) rdist1 = rndist1; // node is closest
            if (rdist2 > rndist2) rdist2 = rndist2; // node is closest

            if (rdist1 > point1.distZ(point2) && rdist2 > point1.distZ(point2)) {
                Dbg("Direct route selected");
                return new List<Point3D> { point2 };
            }

            Point3D poststartpoint = null;
            Point3D preendpoint = null;
            if (rdist1 < rndist1) {
                poststartpoint = Point3D.ProjectOnLine(point1, arc1.StartNode.Position, arc1.EndNode.Position);
                poststartpoint.radius = radius;
                node1 = arc1.StartNode;
            }
            if (rdist2 < rndist2) {
                preendpoint = Point3D.ProjectOnLine(point2, arc2.StartNode.Position, arc2.EndNode.Position);
                preendpoint.radius = radius;
                node2 = arc2.StartNode;
            }

            if (poststartpoint != null) {
                if (point1.distZ(poststartpoint) > mdist1) {
                    Dbg("point1.distZ(poststartpoint) =" + point1.distZ(poststartpoint));
                    lasterror = GLONASSLastError.StartPointTooFar;
                    return new List<Point3D> { point2 };
                }
            } else if (point1.distZ(node1.Position) > mdist1) {
                Dbg("point1.distZ(node1.Position) =" + point1.distZ(node1.Position));
                lasterror = GLONASSLastError.StartPointTooFar;
                return new List<Point3D> { point2 };
            }

            if (preendpoint != null) {
                if (point2.distZ(preendpoint) > mdist2) {
                    Dbg("point2.distZ(preendpoint) =" + point2.distZ(preendpoint));
                    lasterror = GLONASSLastError.DestinationPointTooFar;
                    return new List<Point3D> { point2 };
                }
            } else if (point2.distZ(node2.Position) > mdist2) {
                Dbg("point2.distZ(node2.Position) =" + point2.distZ(node2.Position));
                lasterror = GLONASSLastError.DestinationPointTooFar;
                return new List<Point3D> { point2 };
            }

            if (!AS.SearchPath(node1, node2)) {
                lasterror = GLONASSLastError.PathNotFound;
                Dbg("SearchPath " + lasterror + " node1=" + node1 + " node2=" + node2);
                return new List<Point3D> { point2 };
            }

            //Dbg("poststartpoint=" + poststartpoint + " dist=" + me.distZ(poststartpoint));
            List<Point3D> npath = AS.PathByCoordinates.ToList();
            List<Point3D> path2 = new List<Point3D> { };

            var mustpoints = 2;
            if (rdist1 < rndist1) mustpoints++;
            if (rdist2 < rndist2) mustpoints++;
            if (npath.Count < mustpoints) {
                foreach (var np in npath) Dbg("p=" + np);
                //   Dbg("route to " + point2.name + " npath.Count=" + npath.Count + " mustpoints=" + mustpoints);
                //  lasterror = GLONASSLastError.DestinationTooClose;
                //  return new List<Point3D> { point2 };
            }

            // block of removing unneed endpoints
            if (poststartpoint != null && npath.Count > 2 && ((npath[0] == arc1.StartNode.Position && npath[1] == arc1.EndNode.Position)
                    || (npath[1] == arc1.StartNode.Position && npath[0] == arc1.EndNode.Position))) {
                Dbg("start node by arc split");
                npath.RemoveAt(0);
            }
            if (preendpoint != null && npath.Count > 2 && ((npath[npath.Count() - 1] == arc2.StartNode.Position && npath[npath.Count() - 2] == arc2.EndNode.Position)
                || (npath[npath.Count() - 2] == arc2.StartNode.Position && npath[npath.Count() - 1] == arc2.EndNode.Position))) {
                Dbg("end node by arc split");
                npath.RemoveAt(npath.Count - 1);
            }
            // one madness off
            // adding split points and target

            if (poststartpoint != null) path2.Add(poststartpoint);
            path2.AddRange(npath);
            if (preendpoint != null) path2.Add(preendpoint);
            path2.Add(point2);
            Dbg("dists: " + rdist1 + " " + rdist2 + " " + rndist1 + " " + rndist2 + " Path count=" + path2.Count + " lpn=" + point2.name);
            return path2;
        }
コード例 #4
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
 private List<Point3D> GeneratePath(Point3D destination, Point3D startpoint = null, double dist1 = -1, double dist2 = -1)
 {
     if (destination == null) {
         lasterror = GLONASSLastError.DestinationIsEmpty;
         return new List<Point3D> { };
     }
     if (dist1 == -1) dist1 = lazydist / 2;
     if (dist2 == -1) dist2 = lazydist / 2;
     if (startpoint == null) {
         startpoint = this.path.LastOrDefault();
         if (startpoint == null) {
             SetMe();
             startpoint = me;
         }
     }
     Dbg("GeneratePath from " + startpoint + " to " + destination + " current path:" + this.path.Count);
     if (startpoint.distNoZ(destination) < (startpoint.radius + destination.radius + 1)) {
         lasterror = GLONASSLastError.DestinationTooClose;
         return new List<Point3D> { };
     }
     if (G.Arcs.Count < 1) {
         lasterror = GLONASSLastError.GraphIsEmpty;
         return new List<Point3D> { };
     }
     //            Point3D c = _core.me;
     //double dist;
     //var startnode = this.G.ClosestNode(startpoint, out dist, true);
     //Dbg("ClosestNode dist=" + dist);
     //if (startnode == null) {
     //    lasterror = GLONASSLastError.GraphIsEmpty;
     //    return new List<Point3D> { };
     //}
     //if (dist > this.lazydist) {
     //    lasterror = GLONASSLastError.StartPointTooFar;
     //    return new List<Point3D> { };
     //}
     Dbg("destination = " + destination + "  startpoint = " + startpoint + " endpoint = " + destination);
     var newpath = GetArcSplitRoute(startpoint, destination, dist1, dist2, me.radius * 2);
     //Dbg("newpath.Count=" + newpath.Count);
     if (newpath.Count == 1 && startpoint.distZ(newpath[0]) > (dist1 + dist2)) {
         lasterror = GLONASSLastError.DestinationPointTooFar;
         return new List<Point3D> { };
     }
     if (newpath.Count == 1 && startpoint.distZ(newpath[newpath.Count - 1]) < (startpoint.radius + newpath[newpath.Count - 1].radius + 1)) {
         lasterror = GLONASSLastError.DestinationTooClose;
         return new List<Point3D> { };
     }
     //if (newpath.Count == 1) {
     //    return SplitPath(new List<Point3D> { destination });  // direct route
     //}
     //Dbg("AS.PathByCoordinates " + AS.PathByCoordinates.Count());
     return SplitPath(newpath);
 }
コード例 #5
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        private void AutoRotation()
        {
            an = 0;
            double dist = 0;
            double rspd = 22.2; // 22.22 max
            //            Dbg("autorotation " + this.autorotatestatus);
            while (this.autorotatestatus > 0) {
                try {
                    Thread.Sleep(this.autorotate_sleep);
                    SetMe();
                    if (this.autorotatestatus == 3) { StopRotate(0); this.autorotatestatus = 2; Thread.Sleep(10); continue; }
                    if (this.autorotatestatus == 2) { Thread.Sleep(10); continue; }
                    if (this.autorotatestatus == 4) { this.autorotatestatus = 1; }
                    if (!isWorking) {
                        this.autorotatestatus = 3;
                        continue;
                    }

                    this.speed = Math.Sqrt(Math.Pow(_core.me.toX, 2) + Math.Pow(_core.me.toY, 2));
                    StopRotate(an);
                    if (this.ar_target == null) continue;
                    an = dean(_core.angle(_core.me, this.ar_target.X, this.ar_target.Y));
                    dist = me.distNoZ(this.ar_target);
                    if (Math.Abs(an) <= 2) an = 0;
                    if (Math.Abs(an) <= 5 && dist < 5) an = 0;

                    if (this.onslave) {
                        if (Math.Abs(an) > 3 && (Math.Abs(an) > ((dist / this.speed) * rspd) || Math.Abs(an) > 45 || dist < 4)) {
                            this.rotateinstop = 1; if (_core.moveForwardState) _core.MoveForward(false);
                            if (this.isWorking) this.automovestatus = 2;
                        }
                        //if (Math.Abs(an) > 1) Dbg("an=" + an + " turn=" + _core.me.turnAngle);
                        if (an < -3 && !_core.rotateLeftState) {
                            _core.RotateLeft(true);
                            if (Math.Abs(an) > 6) this.rotation = -1;
                            else {
                                Dbg("Fix Left"); Thread.Sleep(Math.Abs(an) * 22);
                                _core.RotateLeft(false);
                                //Dbg("Fix result1: was " + an + " now " + (dean(_core.angle(_core.me, this.ar_target.X, this.ar_target.Y))));
                                Thread.Sleep(200);
                                //Dbg("Fix result2: was " + an + " now " + (dean(_core.angle(_core.me, this.ar_target.X, this.ar_target.Y))));
                            }
                        } else if (an > 3 && !_core.rotateRightState) {
                            _core.RotateRight(true);
                            if (Math.Abs(an) > 6) this.rotation = 1;
                            else {
                                Dbg("Fix Right"); Thread.Sleep(Math.Abs(an) * 22);
                                _core.RotateRight(false);
                                Dbg("Fix result1: was " + an + " now " + (dean(_core.angle(_core.me, this.next.X, this.next.Y))));
                                Thread.Sleep(200);
                                Dbg("Fix result2: was " + an + " now " + (dean(_core.angle(_core.me, this.next.X, this.next.Y))));
                            }
                        }
                    } else {
                        // drift2
                        //Dbg("an=" + Math.Abs(an) + " rds="+((dist / this.speed) * rspd));
                        if (next != null && next2 != null && Math.Abs(an) > 50 && Math.Abs(an) > ((dist / this.speed) * rspd)) {
                            this.rotateinstop = 1; if (_core.moveForwardState) {
                                _core.MoveForward(false);
                                _core.MoveBackward(true); Thread.Sleep(50); _core.MoveBackward(false);
                            }
                            Dbg("EBD on");
                        }
                        HardTurn(an);
                        an = 0;
                        this.lastturn = DateTime.Now;
                    }
                    if (this.ar_target != null && dist <= this.ar_target.radius && Math.Abs(an) < 1) this.ar_target = null;
                } catch (ThreadAbortException) { break; } catch (Exception en) { Err("Steering gear faiure " + en); Thread.Sleep(500); }
            }
            StopRotate();
        }
コード例 #6
0
ファイル: Graph.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// This function will find the closest node from a geographical position in space.
 /// </summary><seealso cref="ClosestNode(double,double,double,out double,bool)"/>
 /// <param name="P">point from which you want the closest node</param>
 /// <param name="Distance">The distance to the closest node.</param>
 /// <param name="IgnorePassableProperty">if 'false', then nodes whose property Passable is set to false will not be taken into account.</param>
 /// <returns></returns>
 public Node ClosestNode(Point3D P, out double Distance, bool IgnorePassableProperty)
 {
     Node NodeMin = null;
     double DistanceMin = -1;
     foreach (Node N in LN)
     {
         if (IgnorePassableProperty && N.Passable == false) continue;
         double DistanceTemp = Point3D.DistanceBetween(N.Position, P);
         if (DistanceMin == -1 || DistanceMin > DistanceTemp)
         {
             DistanceMin = DistanceTemp;
             NodeMin = N;
         }
     }
     Distance = DistanceMin;
     return NodeMin;
 }
コード例 #7
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        private void AutoMoving()
        {
            int stuck = 0;
            double lastdist = 0;
            double na = 0;
            //if (mr == 0) mr = _core.me.modelRadius + 1;
            //if (_core.me.sitOnMount) mr = _core.me.sitMountObj.modelRadius + 1;
            //if (_core.me.sitOnMount) mr = 1.8;
            //if (!_core.me.sitOnMount) HardTurn(dean(_core.angle(_core.me, next.X, next.Y))); todo in unpause state
            while (this.automovestatus > 0) {
                //Dbg("ams=" + this.automovestatus + " left=" + this.path.Count);
                try {

                    SetMe();

                    if (this.automovestatus == 5) {
                        #region PilotMode
                        Dbg("Pilot mode");
                        if (onslave) {
                            Err("Can't use pilot mode on slave");
                            this.automovestatus = 1;
                            PilotMode = false;
                            continue;
                        }
                        if (_core.moveForwardState) StopMoving();
                        StopRotate(); this.autorotatestatus = 2; this.autobuffstatus = 1;
                        if (next == null) { this.automovestatus = 2; continue; }
                        while (GetNextPoint() && PilotMode && isMoving) {
                            this.automovestatus = 6; Dbg("Pilot is" + PilotMode);
                            if (_core.ComeTo(next.X, next.Y, next.Z)) {
                                if (this.path.Count > 0) this.path.RemoveAt(0);
                                else break;
                            } else {
                                var lasterr = _core.GetLastError();
                                if (lasterr == LastError.MovePossibleFullStop) break;
                                Err("Pilot mode error: " + lasterr + ". Try to decrease lazydist and dmax");
                                this.automovestatus = 1;
                                break;
                            }
                        }
                        this.automovestatus = 2;
                        #endregion
                    }

                    Thread.Sleep(this.automove_sleep);

                    #region CheckNextPoint
                    // bit crazy
                    bool nextpoint = false;
                    int skipcount = 5;
                    if (next != null && (isMoving || (onslave && autorotatestatus == 1)))
                        while (next != null && (me.distNoZ(next) <= next.radius + me.radius + speed * 0.2
                        ||
                        (next2 != null
                        && next.distNoZ(next2) >= (me.distNoZ(next2) + next2.radius + me.radius + speed * 0.2)))
                        ) {
                            this.ar_target = null;
                            if (skipcount-- < 0) break;
                            if (next != null && actions.ContainsKey(next)) try {
                                    Dbg("Act detected");
                                    if ((actions[next].Item1 & 2) != 0) {
                                        StopMoving(); StopRotate();
                                    }
                                    try {
                                        actions[next].Item2.Invoke(next);
                                    } catch (Exception acte) { Err("Error in action " + acte); }
                                    if ((actions[next].Item1 & 4) != 0) CallDoMount();
                                    actions.Remove(next); //?
                                } catch { Err("action failed on point " + next); }
                            if (this.path.Count < 1) {
                                Stop();
                                Dbg("destination point reached"); break;
                            } // приехали
                            this.path.RemoveAt(0);
                            this.totaldist -= next.ndist;
                            //na = next.cos_azimut(prev, this.path[0]);
                            nextpoint = true;
                            if (!GetNextPoint()) break;
                        }
                    #endregion
                    #region NextPoint
                    if (nextpoint) {
                        if (onPreMove != null) try { onPreMove(next); } catch (Exception e) {
                                Err("error in onPreMove: " + e);
                                if (this.automovestatus > 1) {
                                    Dbg("set automovestatus = 4 - error in onPreMove");
                                    this.automovestatus = 4;
                                }
                            }
                        //if (_core.me.sitOnMount) mr = _core.me.sitMountObj.modelRadius + 1;
                        //                    if (_core.me.sitOnMount) mr = 1.7;
                        //else mr = _core.me.modelRadius + 1;
                        //lastdist = 0;
                        this.ar_target = next;
                        lastdist = 0; stuck = -10;
                        if (prev != null && next != null && next2 != null) {
                            double x3 = prev.X;
                            double x2 = next.X;
                            double x1 = next2.X;
                            double y3 = prev.Y;
                            double y2 = next.Y;
                            double y1 = next2.Y;
                            na = Math.Atan(((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)) / ((x2 - x1) * (x3 - x1) + (y2 - y1) * (y3 - y1)));
                            na = Math.Abs(RadianToDegree(na));
                            //Dbg("na = " + na);
                        } else na = 0;
                        Dbg("GPS " + this.path.Count + ": " + next.ToString() + " dist=" + _core.me.dist(next.X, next.Y, next.Z) + " na=" + na + " r=" + next.radius + " r2=" + next2.radius);

                        if (this.automovestatus == 6) {
                            this.automovestatus = 5; continue;
                        }
                    }
                    #endregion

                    //////////////////////////////////////////////
                    // states
                    if (this.automovestatus == 2) {
                        // paused state
                        Thread.Sleep(10);
                        if (this.path.Count == 0) {
                            Thread.Sleep(200);
                            continue;
                        }

                        if (!CheckTickets() ||
                            (_core.me.isAfk && this.unpauseonafk)) {
                            ptickets.Clear();
                            //Dbg("set automovestatus = 4 - unpause");
                            this.automovestatus = 4; // не спать, не спать, косить, косить
                        }
                        continue;
                    } else if (this.automovestatus == 3) {
                        // go pause state
                        this.autorotatestatus = 3;
                        lastdist = 0;
                        stuck = -10;
                        this.automovestatus = 2;
                        this.autobuffstatus = 2; // pause buffs too
                        StopMoving(); StopRotate();
                        Thread.Sleep(10);
                        continue;
                    } else if (this.automovestatus == 4) {
                        // unpause state
                        Thread.Sleep(10);
                        if (!GetNextPoint()) {
                            Err("Can't move - path is empty");
                            this.automovestatus = 2;
                            Thread.Sleep(150);
                            continue;
                        }
                        if (onPreMove != null) try { onPreMove(next); } catch (Exception e) { Err("error in onPreMove: " + e); }
                        CallDoMount();
                        //                    if (onslave) {
                        //                        var slv = _core.getSlave();
                        //                        if (slv != null) mr = slv.modelRadius;
                        //                    }
                        if (!onslave) _core.ComeTo(this.next.X, this.next.Y, this.next.Z, 1, this.next.distZ(_core.me));
                        if (me.distZ(next) > this.lazydist) { // crazy solution
                            var newpath = this.GeneratePath(this.path[0], me);
                            if (newpath.Count == 0) {
                                Dbg("left " + this.path.Count);
                                Err("Патирялася я " + me.distZ(next) + " " + next);
                                lasterror = GLONASSLastError.TargetLost;
                                Stop();
                            }
                            newpath.AddRange(this.path);
                            this.path = newpath;
                        }

                        if (PilotMode) { this.automovestatus = 5; this.autobuffstatus = 1; continue; }
                        lastdist = 0; stuck = -10; this.autorotatestatus = 4; this.automovestatus = 1; this.autobuffstatus = 1;
                        Thread.Sleep(100);
                        if (!_core.moveForwardState) _core.MoveForward(true);
                        this.ar_target = next;
                    } else if (this.automovestatus == 1) {
                        if (!_core.moveForwardState) _core.MoveForward(true); // show must go on
                        this.autorotatestatus = 4;
                        this.ar_target = next; // todo
                    }
                    //SetMapPos(next.x,next.y,next.z);

                    // drift
                    // притормаживаем на резких поворотах
                    if (next != null && next2 != null && na > 30 && _core.moveForwardState
                        && pathspeed > 0.4 && me.distZ(next) < pathspeed * 4) {
                        //int stop = (int)Math.Round(na * (2 * pathspeed));
                        int stop = (int)Math.Round(na * (pathspeed));
                        Dbg("@drift@ na=" + na + " pathspeed=" + pathspeed + " sleep=" + stop);
                        //na = 0; // once
                        _core.MoveForward(false);
                        Dbg("ABS on");
                        Thread.Sleep(stop);
                        //Dbg("end stop");
                    }

                    if (next != null) {
                        SetMe();
                        this.pathspeed = lastdist - me.distNoZ(next);

                        if (this.pathspeed <= 0.001 && this.automovestatus == 1 && Math.Abs(an) < 75) {
                            stuck++;
                            //Dbg("stuck=" + stuck + " pathspeed=" + this.pathspeed + " lastdist=" + lastdist);
                            if (this.onslave && _core.moveForwardState) {
                                if (stuck > 3) {
                                    StopRotate();
                                    try { _core.MoveForward(false); Thread.Sleep(1351); } finally { if (!_core.moveForwardState) _core.MoveForward(true); }
                                    lastdist = 0;
                                    stuck = 0;
                                }
                            } else if (stuck == 5) {    // dveri
                                try {
                                    var dveri = _core.getDoodads().Where(d => d.dbAlmighty.groupId == 36 && d.dbFuncGroup.useSkills.Exists(s => s.id == 13439) && _core.me.dist(d) < 3).OrderBy(d => _core.me.dist(d)).FirstOrDefault(); // 2m
                                    //Dbg("dd=" + dveri.dbAlmighty.groupId + " sk="+dveri.getUseSkills().First().id); //  d.dbAlmighty.groupId == 68 &&
                                    if (dveri != null) {
                                        Dbg("door");
                                        _core.MoveForward(false);
                                        _core.UseDoodadSkill(13439, dveri, 0, false, 5);
                                        //_core.UseDoodadSkill(16828, dveri, 0, false, 5);
                                        waitCoolDown();
                                        Thread.Sleep(150);
                                        lastdist = 0;
                                    } // else Dbg("door not found");
                                } catch { }
                            } else if (stuck == 6) {
                                //
                                try { _core.Jump(true); Thread.Sleep(70); } finally { _core.Jump(false); }
                                Dbg("jump");
                                Thread.Sleep(160);

                                //this.pathspeed = lastdist - me.distNoZ(next);
                                //if (this.pathspeed > 0) stuck = 0;
                            } else if (stuck > 12) {
                                _core.MoveTo(this.next.X, this.next.Y, this.next.Z);
                            }
                        } else if (stuck > 0) stuck = 0;
                    }
                    if (next != null) lastdist = me.distNoZ(next);//_core.me.dist(next.x, next.y, next.z);
                    //Dbg("speed " + this.speed);
                } catch (ThreadAbortException) { break; } catch (Exception en) { Err("Primary reactor faiure " + en); Thread.Sleep(500); }

            }
            Dbg("Reactor shotdown");
            this.autorotatestatus = -1;
            this.autobuffstatus = -1;
            Stop();
            return;
        }
コード例 #8
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        /*
        /// <summary>
        /// Sub move inside main move
        /// </summary>
        /// <param name="newpoint">left point</param>
        /// <param name="act">act on this point</param>
        /// <param name="type">type mask of act</param>
        /// <returns></returns>
        public bool SubMove(Point3D newpoint, Action<Point3D> act, ushort type = 0) {
            if (newpoint == null) {
                lasterror = GLONASSLastError.DestinationIsEmpty;
                return false;
            }
            if (next != null && next.distZ(newpoint) < (next.radius + newpoint.radius + 1)) {
                actions.Add(next, Tuple.Create(type, act)); return true;
            }
            var newpath = this.GeneratePath(newpoint, me);

            //Dbg("new subroute to " + newpoint + " cnt:" + newpath.Count + " last is " + newpath[newpath.Count - 1]);

            //if (newpath.Count > 0) newpath.RemoveAt(0); // TODO??
            Dbg("subpath count=" + newpath.Count);
            if (newpath.Count == 0) return false;
            var i = 0;
            for (; i <= newpath.Count; i++) {
                if (newpath[i] == this.path[i] && newpath.Count > 0 && this.path.Count > i) newpath.RemoveAt(0);
                else break;
            }
            if (newpath.Count > 0) {
                this.path.InsertRange(i, newpath);
            }
            Dbg("Merged at " + i);
            actions.Add(next, Tuple.Create(type, act)); // TODO check

            if (newpath.Count > 0 && this.path.Count > i) {
                if (this.path.Count > (i + 1)) i++;
                var wayback = this.GeneratePath(this.path[i], newpoint);
                if (newpath.Count == 0 || wayback.Count == 0) return false; // no way back?
                wayback.RemoveAt(wayback.Count - 1);
                wayback.RemoveAt(0);
                this.path.InsertRange(i, wayback);
            }
            return true;
        }
        */
        /// <summary>
        /// Remove action from point
        /// </summary>
        /// <param name="point"></param>
        public void RemoveAction(Point3D point)
        {
            if (actions.ContainsKey(point)) actions.Remove(point);
        }
コード例 #9
0
ファイル: Vector3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Constructs a Vector3D with two points.
 /// </summary>
 /// <param name="P1">First point of the vector.</param>
 /// <param name="P2">Second point of the vector.</param>
 public Vector3D(Point3D P1, Point3D P2)
 {
     DX = P2.X-P1.X; DY = P2.Y-P1.Y; DZ = P2.Z-P1.Z;
 }
コード例 #10
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// distance between two points
 /// </summary>
 /// <param name="b"></param>
 /// <returns></returns>
 public double distZ(Point3D b)
 {
     return distZ(this, b);
 }
コード例 #11
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
        /// <summary>
        /// Returns the projection of a point on the line defined with two other points.
        /// When the projection is out of the segment, then the closest extremity is returned.
        /// </summary>
        /// <exception cref="ArgumentNullException">None of the arguments can be null.</exception>
        /// <exception cref="ArgumentException">P1 and P2 must be different.</exception>
        /// <param name="Pt">Point to project.</param>
        /// <param name="P1">First point of the line.</param>
        /// <param name="P2">Second point of the line.</param>
        /// <returns>The projected point if it is on the segment / The closest extremity otherwise.</returns>
        public static Point3D ProjectOnLine(Point3D Pt, Point3D P1, Point3D P2)
        {
            if (Pt == null || P1 == null || P2 == null) throw new ArgumentNullException("None of the arguments can be null.");
            if (P1.Equals(P2)) throw new ArgumentException("P1 and P2 must be different.");
            Vector3D VLine = new Vector3D(P1, P2);
            Vector3D V1Pt = new Vector3D(P1, Pt);
            Vector3D Translation = VLine * (VLine | V1Pt) / VLine.SquareNorm;
            Point3D Projection = P1 + Translation;

            Vector3D V1Pjt = new Vector3D(P1, Projection);
            double D1 = V1Pjt | VLine;
            if (D1 < 0) return P1;

            Vector3D V2Pjt = new Vector3D(P2, Projection);
            double D2 = V2Pjt | VLine;
            if (D2 > 0) return P2;

            return Projection;
        }
コード例 #12
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// 
 /// </summary>
 /// <param name="a"></param>
 /// <param name="b"></param>
 /// <returns></returns>
 public static double distZ(Node a, Point3D b)
 {
     return distZ(a.Position, b);
 }
コード例 #13
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Distance from point to node
 /// </summary>
 /// <param name="a">Point</param>
 /// <param name="b">Node</param>
 /// <returns></returns>
 public static double distZ(Point3D a, Node b)
 {
     if (a == null || b == null) return 999999999; return distZ(a, b.Position);
 }
コード例 #14
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// distance between two points
 /// </summary>
 /// <param name="a"></param>
 /// <param name="b"></param>
 /// <returns></returns>
 public static double distZ(Point3D a, Point3D b)
 {
     return Math.Sqrt(Math.Pow((b.X - a.X), 2) + Math.Pow((b.Y - a.Y), 2) + Math.Pow((b.Z - a.Z), 2));
 }
コード例 #15
0
ファイル: Point3D.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Returns the distance between two points.
 /// </summary>
 /// <param name="P1">First point.</param>
 /// <param name="P2">Second point.</param>
 /// <returns>Distance value.</returns>
 public static double DistanceBetween(Point3D P1, Point3D P2)
 {
     return Math.Sqrt(Math.Pow((P2.X - P1.X), 2) +
         Math.Pow((P2.Y - P1.Y), 2) +
         Math.Pow((P2.Z - P1.Z), 2));
     //return Math.Sqrt((P1.X-P2.X)*(P1.X-P2.X)+(P1.Y-P2.Y)*(P1.Y-P2.Y));
 }
コード例 #16
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// GpsMove to point
 /// ATTN! if path can't me generated will go directly to point!
 /// </summary>
 /// <param name="point">target point (also Creature)</param>
 /// <returns></returns>
 public bool GpsMove(Point3D point)
 {
     return MoveTo(point);
 }
コード例 #17
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        /// <summary>
        /// Move to point/Creature/Doodad
        /// start point is last current point or Self if current path is empty
        /// </summary>
        /// <param name="point">destination point</param>
        /// <param name="dist1">distnance from start point to grapth, default (-1) = lazydist/2</param>
        /// <param name="dist2">distnance from grapth to destination, default (-1) = lazydist/2</param>
        /// <param name="act">Action on destination</param>
        /// <param name="type">action type, default 0. Bitmask! 2 - stop moving before, 4 - call doMount after</param>
        /// <returns>true if have route</returns>
        public bool MoveTo(Point3D point, double dist1 = -1, double dist2 = -1, Action<Point3D> act = null, ushort type = 0)
        {
            lasterror = GLONASSLastError.None;
            if (point == null) {
                lasterror = GLONASSLastError.DestinationIsEmpty;
                return false;
            }
            if (dist1 == -1) dist1 = lazydist / 2;
            if (dist2 == -1) dist2 = lazydist / 2;

            var newpath = this.GeneratePath(point, null, dist1, dist2);
            if (newpath.Count == 0) return false;
            //    foreach (var pp in newpath) _core.Log("@ "+pp + " dist="+me.distZ(pp));
            //    return false;
            if (act != null) actions.Add(newpath[newpath.Count - 1], Tuple.Create(type, act));
            this.path.AddRange(newpath);
            this.totaldist = this.path.Sum(x => x.ndist);

            //Dbg("set automovestatus = 4 - start");
            //this.automovestatus = 4;
            Thread.Sleep(250);
            return true;
        }
コード例 #18
0
ファイル: Point3D.bak.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// bred
 /// </summary>
 /// <param name="b"></param>
 /// <returns></returns>
 public double angle(Point3D b)
 {
     return Math.Atan2(b.Y, b.X) - Math.Atan2(this.Y, b.X);
 }
コード例 #19
0
ファイル: GLONASS.cs プロジェクト: TrurlMcByte/aa_glonass
        /// <summary>
        ///  Рубит маршрут на короткие отрезки и прописывает длины
        /// </summary>
        /// <param name="path2">original path</param>
        /// <returns>splitted path</returns>
        public List<Point3D> SplitPath(List<Point3D> path2)
        {
            if (path2.Count < 2) return path2;
            Point3D c = path2[0];
            List<Point3D> rpath = new List<Point3D> { c };

            for (var i = 1; i < path2.Count; i++) {
                Point3D n = path2[i];
                if (c.distNoZ(n) < 0.1) continue; // paranoid mode off - skip duplicate
                //Log("@" + c.ToString() + " => " + n.ToString() + " dist=" + c.dist(n));
                double nd = c.distNoZ(n);
                while (nd > dmax) {
                    nd = c.distNoZ(n);
                    double d = Math.Round(nd / dmax, 0, MidpointRounding.AwayFromZero) - 1;
                    if (d <= n.radius) break;
                    double nx = Math.Round((c.X * d + n.X) / (d + 1), 2);
                    double ny = Math.Round((c.Y * d + n.Y) / (d + 1), 2);
                    c = new Point3D(nx, ny, _core.getZFromHeightMap(nx, ny), "");
                    c.ndist = nd / d;
                    //this.totaldist += c.ndist;
                    //Err("@@" + c + " => " + n + " dist=" + c.distZ(n) + " d=" + d);
                    rpath.Add(c);
                    //Dbg("@" + c);
                }
                c.ndist = c.distNoZ(n);
                //this.totaldist += c.ndist;
                c = n; rpath.Add(c);
            }
            //this.totaldist = path.Sum(x=> x.ndist);
            return rpath;
        }
コード例 #20
0
ファイル: Point3D.bak.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// bred
 /// </summary>
 /// <param name="b"></param>
 /// <returns></returns>
 public double angleD(Point3D b)
 {
     return this.angle(b) * 180 / Math.PI;
 }
コード例 #21
0
ファイル: Node.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// Modifies X, Y and Z coordinates
 /// </summary>
 /// <param name="PositionX">X coordinate.</param>
 /// <param name="PositionY">Y coordinate.</param>
 /// <param name="PositionZ">Z coordinate.</param>
 public void ChangeXYZ(double PositionX, double PositionY, double PositionZ)
 {
     Position = new Point3D(PositionX, PositionY, PositionZ);
 }
コード例 #22
0
ファイル: Point3D.bak.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// cos of angle in current point
 /// </summary>
 /// <param name="b">prevois point</param>
 /// <param name="c">next point</param>
 /// <returns>cos</returns>
 public double cos_azimut(Point3D b, Point3D c)
 {
     if (b == null) return 1;
     if (c == null) return 0;
     return ((Sqr(this.dist(b)) + Sqr(this.dist(c)) - Sqr(b.dist(c))) / (2 * this.dist(b) * this.dist(c)));
 }
コード例 #23
0
ファイル: Point3D.bak.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// dist to point ignoring Z ordinate
 /// </summary>
 /// <param name="b"></param>
 /// <returns></returns>
 public double distZ(Point3D b)
 {
     return Math.Sqrt(Math.Pow((b.X - this.X), 2) + Math.Pow((b.Y - this.Y), 2) + Math.Pow((b.Z - this.Z), 2));
 }
コード例 #24
0
ファイル: Graph.cs プロジェクト: TrurlMcByte/aa_glonass
 /// <summary>
 /// This function will find the closest arc from a geographical position in space using projection.
 /// </summary>
 /// <param name="P">point from which you want the closest arc.</param>
 /// <param name="Distance">The distance to the closest arc.</param>
 /// <param name="IgnorePassableProperty">if 'false', then arcs whose property Passable is set to false will not be taken into account.</param>
 /// <returns>The closest arc that has been found.</returns>
 public Arc ClosestArc(Point3D P, out double Distance, bool IgnorePassableProperty)
 {
     Arc ArcMin = null;
     double DistanceMin = -1;
     foreach (Arc A in LA)
     {
         if (IgnorePassableProperty && A.Passable == false) continue;
         Point3D Projection = Point3D.ProjectOnLine(P, A.StartNode.Position, A.EndNode.Position);
         double DistanceTemp = Point3D.DistanceBetween(P, Projection);
         if (DistanceMin == -1 || DistanceMin > DistanceTemp)
         {
             DistanceMin = DistanceTemp;
             ArcMin = A;
         }
     }
     Distance = DistanceMin;
     return ArcMin;
 }