public virtual Vector Intersect(LineBase line)
{
// Calc. intersection if 2 lines are both infinite.
var cc = a * line.b - line.a * b;
if (cc == 0)
{
return(Vector.Null);
}
var aa = b * line.c - line.b * c;
var bb = line.a * c - a * line.c;
var sec = new Vector(aa / cc, bb / cc);
// Parallel
if (sec.isNull)
{
return(sec);
}
// Intersection on both line
if (this.FootOnThis(sec) && line.FootOnThis(sec))
{
return(sec);
}
// No Intersection
return(Vector.Null);
}
public void Update(bool usePred = false)
{
if (usePred)
{
x = handle.xPred;
y = handle.yPred;
pos = new Vector(x, y);
rad = handle.radPred;
spdL = handle.spdPredL;
spdR = handle.spdPredR;
spd = handle.spdPred;
w = handle.wPred;
v = handle.vPred;
}
else
{
x = handle.x;
y = handle.y;
pos = handle.pos;
rad = handle.rad;
spdL = handle.spdL;
spdR = handle.spdR;
spd = handle.spd;
w = handle.w;
v = handle.v;
}
}
public Vector Run(List <Navigator> others, Vector tarPos)
{
List <Navigator> othersSeen = new List <Navigator>();
foreach (var o in others)
{
if (ego.pos.distTo(o.entity.pos) - margin - o.boids.margin < range &&
ego.Rad2Pos(o.entity.pos) < fov)
{
othersSeen.Add(o);
}
}
var att = Attract(tarPos) * p_weight_attraction;
var sep = Separate(othersSeen) * p_weight_separation;
sep = sep.clip(p_max_separation);
var coh = Cohesion(othersSeen) * p_weight_cohesion;
coh = coh.clip(p_max_cohesion);
var ali = Alignment(othersSeen) * p_weight_alignment;
ali = ali.clip(p_max_alignment);
var f = att + sep + coh + ali;
f = f.clip(p_max_all);
return(f);
}
// public LineBase(double a, double b, double c){
// if (a ==0 && b == 0) throw new ArgumentException("a, b cannot be both 0");
// this.a = a; this.b = b; this.c = c;
// var mag = Math.Sqrt(a*a+b*b);
// this.a /= mag; this.b /= mag; this.c /= mag;
// }
public bool FootOnThis(Vector point)
{
bool insideOfEnd1 = !this.point1end || (point - this.point1) * (this.point2 - this.point1) >= 0;
bool insideOfEnd2 = !this.point2end || (point - this.point2) * (this.point1 - this.point2) >= 0;
return(insideOfEnd1 && insideOfEnd2);
}
public static CircleCastHit NotHit(Vector origin)
{
return(new CircleCastHit(
isHit: false, isOriginHit: false,
originCirclePos: origin,
hitCirclePos: Vector.Null, contactPos: Vector.Null
));
}
protected LineBase MovedAlongNormToPointSide(Vector point, Double displacement)
{
// on which side of wall ego is
var normSign = Math.Sign(norm * point + c);
var step = normSign * displacement * norm;
return(new LineBase(point1 + step, point2 + step, point1end, point2end));
}
public void calcPoints()
{
points = new Vector[rads.Length];
for (int i = 0; i < rads.Length; ++i)
{
points[i] = Vector.fromRadMag(rads[i], Abs(dists[i]));
}
}
/// <summary>
/// Scan one wall.
/// </summary>
private ScanResult _ScanWall(Wall wall, double[] rads)
{
ScanResult res = ScanResult.init(rads, range + 1);
var egopos = ego.pos;
var castTry = new CircleCast(egopos, 0, margin);
var hitTry = castTry.CastTo(wall);
// currently in collision
if (hitTry.isOriginHit)
{
res.isCollision = true;
var dist2wall = wall.DistToPoint_TruncatedByEnds(egopos);
var colSeverity = Max(0, margin + wall.margin - dist2wall) / margin;
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
var repulsionDir = wall.RepulsionDirToPoint(egopos);
if (useSafety)
{
res.safety[i] = Cos(AbsRad(rads[i] - repulsionDir.rad)) * (1 + colSeverity);
}
else
{
var dist = Max(0.1, Vector.fromRadMag(rad, 1) * repulsionDir * 100);
res.dists[i] = dist;
}
}
}
// not in collision
else
{
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
var cast = new CircleCast(egopos, rad, margin);
var hit = cast.CastTo(wall);
// No hit
if (!hit.isHit)
{
continue;
}
// Hit
var dist = hit.dist;
if (dist < res.dists[i])
{
res.dists[i] = dist;
}
}
}
return(res);
}
public CircleCastHit(
bool isHit, bool isOriginHit,
Vector originCirclePos, Vector hitCirclePos, Vector contactPos
)
{
this.isHit = isHit;
this.isOriginHit = isOriginHit;
this.originCirclePos = originCirclePos;
this.hitCirclePos = hitCirclePos;
this.contactPos = contactPos;
}
/// <summary>
/// Create Default Walls (Borders)
/// </summary>
public void AddBorder(int width = 20, int x1 = 40, int x2 = 460, int y1 = 40, int y2 = 460)
{
Vector lt = new Vector(x1, y1);
Vector rt = new Vector(x2, y1);
Vector lb = new Vector(x1, y2);
Vector rb = new Vector(x2, y2);
walls.Add(new Wall(lt, lb, width)); // left
walls.Add(new Wall(rt, rb, width)); // right
walls.Add(new Wall(lt, rt, width)); // top
walls.Add(new Wall(lb, rb, width)); // bottom
}
/// <summary>
/// Create Default Walls (Borders)
/// </summary>
public void AddBorder(int width, RectInt rect)
{
Vector lt = new Vector(rect.xMin, rect.yMin);
Vector rt = new Vector(rect.xMax, rect.yMin);
Vector lb = new Vector(rect.xMin, rect.yMax);
Vector rb = new Vector(rect.xMax, rect.yMax);
walls.Add(new Wall(lt, lb, width)); // left
walls.Add(new Wall(rt, rb, width)); // right
walls.Add(new Wall(lt, rt, width)); // top
walls.Add(new Wall(lb, rb, width)); // bottom
}
Vector Separate(List <Navigator> others)
{
Vector f = Vector.zero;
foreach (var o in others)
{
var dPos = ego.pos - o.entity.pos;
var rawDist = dPos.mag;
var dist = Math.Max(1, rawDist - margin - o.boids.margin);
f += dPos.unit * (range / dist - 1);
}
return(f);
}
public LineBase(Vector point1, Vector point2, bool point1end = true, bool point2end = true)
{
this.point1 = point1;
this.point2 = point2;
this.point1end = point1end;
this.point2end = point2end;
// Calculate a,b,c of ax+by+c=0
if (point1 == point2)
{
throw new ArgumentException("point1 and point2 cannot be the same");
}
else if (point1.x == point2.x)
{
b = 0;
if (point1.x == 0)
{
a = 1; c = 0;
}
else
{
c = -point1.x;
a = 1;
}
}
else if (point1.y == point2.y)
{
a = 0;
if (point1.y == 0)
{
b = 1; c = 0;
}
else
{
c = -point1.y;
b = 1;
}
}
else
{
c = point1.x * point2.y - point2.x * point1.y;
a = point1.y - point2.y;
b = point2.x - point1.x;
var mag = Math.Sqrt(a * a + b * b);
a /= mag; b /= mag; c /= mag;
}
}
public Line(Vector point1, Vector point2)
{
if (point1 == point2)
{
throw new ArgumentException("point1 and point2 cannot be the same");
}
else if (point1.x == point2.x)
{
b = 0;
if (point1.x == 0)
{
a = 1; c = 0;
}
else
{
c = -point1.x;
a = 1;
}
}
else if (point1.y == point2.y)
{
a = 0;
if (point1.y == 0)
{
b = 1; c = 0;
}
else
{
c = -point1.y;
b = 1;
}
}
else
{
c = point1.x * point2.y - point2.x * point1.y;
a = point1.y - point2.y;
b = point2.x - point1.x;
var mag = Math.Sqrt(a * a + b * b);
a /= mag; b /= mag; c /= mag;
}
}
public double DistToPoint_TruncatedByEnds(Vector point)
{
if (FootOnThis(point))
{
return(DistToPoint(point));
}
double d1 = -1, d2 = -1;
if (point1end)
{
d1 = point1.distTo(point);
}
if (point2end)
{
d2 = point2.distTo(point);
}
if (d1 < d2 && d1 >= 0 || d2 < 0)
{
return(d1);
}
return(d2);
}
public Vector RepulsionDirToPoint(Vector point)
{
if (FootOnThis(point))
{
return(Math.Sign(SignedDistToPoint(point)) * norm);
}
double d1 = -1, d2 = -1;
if (point1end)
{
d1 = point1.distTo(point);
}
if (point2end)
{
d2 = point2.distTo(point);
}
if (d1 < d2 && d1 >= 0 || d2 < 0)
{
return((point - point1).unit);
}
return((point - point2).unit);
}
public double p_mv2tar_reach_pred = 0; // weight of posPred for reach condition
/// <summary>
/// Move to target position x, y.
/// </summary>
/// <param name="rotateTime">time(ms) is supposed to rotate to target. i.e. the slowness of rotation.</param>
/// <param name="tolerance">how close should cube be to target, that can be judged "reached".</param>
public Movement Move2Target(double tarX, double tarY, double maxSpd = 50,
int rotateTime = 250, double tolerance = 8)
{
Vector tar = new Vector(tarX, tarY);
double dist = this.posPred.distTo(tar);
double dradPred = Rad((tar - this.posPred).rad - this.radPred);
double drad = Rad((tar - pos).rad - rad);
// Reach condition
if (tar.distTo(this.pos * (1 - p_mv2tar_reach_pred) + this.posPred * p_mv2tar_reach_pred) < tolerance)
{
return(new Movement(this, 0, 0, 100, true));
}
else
{
// Decelerate at turning.
// Consider distance
double endRad = 110 * Min(1, Max(dist, 20) / Max(1, maxSpd) / VDotOverU / 1.5 / (rotateTime / 1000.0));
double translate = maxSpd * clip(1 - (Abs(drad) - Deg2Rad(10)) / Deg2Rad(endRad), 0, 1);
// Decelerate as getting close to target.
translate *= clip(dist, Deadzone * 0.5, Max(maxSpd, Deadzone) * 0.5) / (Max(maxSpd, Deadzone) * 0.5);
// Angular velocity
double w = dradPred / (rotateTime / 1000.0);
double rotate = w * TireWidthDot / VDotOverU;
// Linearly combine turning raidus method and angular velocity method.
// Cause turning radius does not work on low "translate",
// while angular velocity is weak on high "translate".
double miu = (Abs(translate) / MaxSpd); // weight of turning radius method
rotate *= Abs(translate / 50) * miu + 1 * (1 - miu);
return(new Movement(this, translate, rotate, rotateTime, false));
}
}
public NaviResult(Mode mode, Vector egoPos, Vector avoidVector, Vector boidsVector,
double speedLimit = double.MaxValue, double speedRatio = 1, bool isCollision = false)
{
this.mode = mode;
this.avoidVector = avoidVector;
this.boidsVector = boidsVector;
this.isCollision = isCollision;
switch (mode)
{
case Mode.AVOID:
case Mode.BOIDS_AVOID:
this.waypoint = egoPos + avoidVector; break;
case Mode.BOIDS:
this.waypoint = egoPos + boidsVector; break;
default:
this.waypoint = egoPos + Vector.zero; break;
}
this.speedRatio = speedRatio;
this.speedLimit = speedLimit;
}
public Line GetBorder(Vector egoPos, double egoMargin)
{
return(new Line(a, b, c - Math.Sign(norm * egoPos + c) * (egoMargin + margin)));
}
/// <summary>
/// Run towards target.
/// </summary>
public (Vector, bool, double) RunTowards(List <Navigator> others, Entity target, List <Wall> walls)
{
// ==== Scan ====
var rads = SampleRads(target);
var(res, resCol) = Scan(others, target, walls, rads);
res.calcPoints(); resCol.calcPoints();
// ==== Choose Waypoint ====
// find waypoint by res, which is the scan result of objects NOT in collision
waypointIndex = 0;
Vector waypoint = Vector.zero;
// evaluate "distance" for each rad
List <double> dists = new List <double>();
for (int i = 0; i < rads.Length; ++i)
{
// nearest point at certain rad
// var targetDist = ego.pos.distTo(target.pos);
var targetDist = Math.Sqrt((ego.x - target.x) * (ego.x - target.x) + (ego.y - target.y) * (ego.y - target.y));
var nearestMag = Max(Cos((target.pos - ego.pos).rad - rads[i]) * targetDist, 1);
var mv = res.points[i].clip(nearestMag);
res.points[i] = mv;
// distance of waypoint to target
var dist = (ego.pos + mv).distTo(target.pos);
// penalty of turning
// NOTE may cause not turning when target right behind ego
dist += Max(AbsRad(ego.rad - rads[i]), 0.5) * 0.1;
dist += Max((AbsRad(ego.rad - rads[i]) - PI / 2), 0) * 2;
dists.Add(dist);
}
// choose best waypoint
if (useSafety) // combine with safety
{
double minDist = double.MaxValue;
for (int i = 0; i < rads.Length; ++i)
{
var dist = dists[i];
// Search Nearest waypoint away from target, with SAFE direction
if (dist < minDist && resCol.safety[i] > p_waypoint_safety_threshold)
{
minDist = dist; waypointIndex = i;
}
}
if (minDist < double.MaxValue)
{
waypoint = res.points[waypointIndex];
}
else
{
// Debug.Log("Stuck");
}
}
else // with force from collided entity
{
if (!resCol.isCollision) // Yet no collision.
{
double minDist = double.MaxValue;
for (int i = 0; i < rads.Length; ++i)
{
var dist = dists[i];
// Search Nearest waypoint away from target
if (dist < minDist)
{
minDist = dist; waypointIndex = i;
}
}
waypoint = res.points[waypointIndex];
}
else // Now collision
{
var maxDist = resCol.dists[0];
for (int i = 0; i < rads.Length; ++i)
{
var d = resCol.dists[i];
if (d > maxDist)
{
maxDist = d; waypointIndex = i;
}
}
waypoint = resCol.points[waypointIndex];
}
}
// ==== Speed Limit ====
double speedLimit = double.MaxValue;
var waypointRad = rads[waypointIndex];
// slow down when colliders are close around.
{
double minRadius = 1e10;
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
if (res.dists[i] < 50 &&
(Rad(rad - ego.rad) > 0.1 && Rad(waypointRad - rad) > 0.1 ||
Rad(ego.rad - rad) > 0.1 && Rad(rad - waypointRad) > 0.1))
{
// minimal maximal turning radius
minRadius = Min(minRadius, (res.dists[i] + 20) / (2 * Sin(Abs(rad - ego.rad))));
}
}
speedLimit = Max(8, minRadius * Max(1, Abs(ego.w)));
}
// stop when ego.rad is pointing insides colliders.
if (resCol.isCollision)
{
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
if (AbsRad(rad - ego.rad) < PI * 2 / nsample &&
resCol.safety[i] <= 0
)
{
speedLimit = 100 * Max(0, resCol.safety[i] * 2 + 1);
break;
}
}
}
// ==== make result ====
res.rads = rads;
res.isCollision = resCol.isCollision;
if (resCol.isCollision && useSafety)
{
Array.Copy(resCol.safety, res.safety, res.safety.Length);
}
scanResult = res;
return(waypoint, resCol.isCollision, speedLimit);
}
/// <summary>
/// Runaway from target.
/// </summary>
public (Vector, bool, double) RunAway(List <Navigator> others, Entity target, List <Wall> walls)
{
// ==== Scan ====
var rads = SampleRads(target);
double oppoRad = (ego.pos - target.pos).rad;
int oppoIndex = (int)((oppoRad - (ego.rad - fov / 2)) / (fov / (nsample - 1)));
var(res, resCol) = Scan(others, target, walls, rads);
res.calcPoints(); resCol.calcPoints();
// ==== Choose Waypoint ====
// find waypoint by res, which is the scan result of objects NOT in collision
waypointIndex = 0;
Vector waypoint = Vector.zero;
{
double maxDist = 0;
for (int i = 0; i < rads.Length; ++i)
{
// distance of waypoint to target
var point = res.points[i] * p_runaway_range / range + ego.pos;
var dist = (target.pos + target.v * 0.5).distTo(point);
var dist_penalty = dist;
// Soft penalty : away from target (predicted)
dist_penalty -= (-PI + AbsRad(rads[i] - (ego.pos - target.pos - target.v * 0.2).rad)) * p_runaway_penalty_away_k;
// Soft penalty : keep self's direction
dist_penalty -= (-PI + AbsRad(rads[i] - ego.rad)) * p_runaway_penalty_keeprad_k;
// Hard penalty
var drad = AbsRad((rads[i] - (ego.pos - target.pos - target.v * 0.0).rad));
double danger_fov = PI / 4 - 0.15;// + 5/(Max(ego.pos.distTo(target.pos)-20, 20)) ; // greater than pi/4 make ego not able to leave corner
if (drad - PI + danger_fov > 0)
{
dist_penalty = 0;
}
// Search Furthest waypoint away from target, with SAFE direction
if (maxDist < dist_penalty && resCol.safety[i] > p_waypoint_safety_threshold)
{
maxDist = dist_penalty;
waypointIndex = i;
}
}
if (maxDist > 0)
{
waypoint = res.points[waypointIndex];
}
else
{
// Debug.Log("Stuck");
}
}
// ==== Speed Limit ====
double speedLimit = double.MaxValue;
var waypointRad = rads[waypointIndex];
// slow down when colliders are close around.
{
double minRadius = 1e10;
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
if (Rad(rad - ego.rad) > 0.1 && Rad(waypointRad - rad) > 0.1 ||
Rad(ego.rad - rad) > 0.1 && Rad(rad - waypointRad) > 0.1)
{
// minimal maximal turning radius
minRadius = Min(minRadius, (res.dists[i] + 20) / (2 * Sin(Abs(rad - ego.rad))));
}
}
speedLimit = Max(8, minRadius * Max(1, Abs(ego.w)));
}
// stop when ego.rad is pointing insides colliders.
if (resCol.isCollision)
{
for (int i = 0; i < rads.Length; ++i)
{
var rad = rads[i];
if (AbsRad(rad - ego.rad) < PI * 2 / nsample &&
resCol.safety[i] <= 0
)
{
speedLimit = 100 * Max(0, resCol.safety[i] * 2 + 1);
break;
}
}
}
// ==== make result ====
res.rads = rads;
res.isCollision = resCol.isCollision;
if (resCol.isCollision && useSafety)
{
Array.Copy(resCol.safety, res.safety, res.safety.Length);
}
scanResult = res;
return(waypoint, resCol.isCollision, speedLimit);
}
public Vector FootPoint(Vector point)
{
var n = norm;
return(point - (n * point + c) * n);
}
public double DistToPoint(Vector point)
{
return(Math.Abs(norm * point + c));
}
public double Rad2Pos(Vector tarPos)
{
var radAbs = (tarPos - pos).rad;
return(Rad(radAbs - rad));
}
public Entity(Vector _pos, double _margin)
{
pos = _pos; x = _pos.x; y = _pos.y;
margin = _margin;
}
Vector Attract(Vector tarPos)
{
var dPos = tarPos - ego.pos;
return(dPos.unit);
}
/// <summary>
/// Get waypoint away from target position Vector.
/// </summary>
public NaviResult GetWaypointAway(Vector pos)
{
return(GetWaypointAway(new Entity(new Vector(pos.x, pos.y))));
}
public Wall(Vector point1, Vector point2, double margin = 0) : base(point1, point2)
{
this.margin = margin;
}
public Entity(Vector _pos)
{
pos = _pos; x = _pos.x; y = _pos.y;
}
public Line(Vector point1, Vector point2) : base(point1, point2)
{
}
