/// <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();
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name="point">Point3D</param>
public Node(Point3D point)
{
_Position = point;
_Passable = true;
_IncomingArcs = new ArrayList();
_OutgoingArcs = new ArrayList();
}
/// <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;
}
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);
}
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();
}
/// <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;
}
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;
}
/*
/// <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);
}
/// <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;
}
/// <summary>
/// distance between two points
/// </summary>
/// <param name="b"></param>
/// <returns></returns>
public double distZ(Point3D b)
{
return distZ(this, b);
}
/// <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;
}
/// <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);
}
/// <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);
}
/// <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));
}
/// <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));
}
/// <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);
}
/// <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;
}
/// <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);
}
/// <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;
}
/// <summary>
/// bred
/// </summary>
/// <param name="b"></param>
/// <returns></returns>
public double angleD(Point3D b)
{
return this.angle(b) * 180 / Math.PI;
}
/// <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);
}
/// <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)));
}
/// <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));
}
/// <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;
}
