public virtual CircleCastHit CircleCastHandle(CircleCast cast)
{
// Already collided at origin position
if (pos.distTo(cast.origin) < cast.r)
{
return(new CircleCastHit
(
isHit: true, isOriginHit: true,
originCirclePos: cast.origin, hitCirclePos: cast.origin, contactPos: pos
));
}
// Detect
var castLine = cast.GetLineBase();
if (castLine.FootOnThis(pos))
{
var dist = castLine.DistToPoint(pos);
if (dist <= cast.r)
{
var distContactFoot = Math.Sqrt(cast.r * cast.r - dist * dist);
var foot1 = castLine.FootPoint(pos);
var distOriginFoot = (foot1 - cast.origin) * cast.dir;
var distOriginContact = distOriginFoot - distContactFoot;
var hit = cast.origin + cast.dir * distOriginContact;
return(new CircleCastHit
(
isHit: true, isOriginHit: false, originCirclePos: cast.origin,
hitCirclePos: hit, contactPos: pos
));
}
}
// No Collision
return(CircleCastHit.NotHit(cast.origin));
}
/// <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 virtual CircleCastHit CircleCastHandle(CircleCast cast)
{
var hit_between = CircleCastHandle_ExceptEnds(cast);
if (hit_between.isHit)
{
return(hit_between);
}
var hit_ends = CircleCastHandle_Ends(cast);
return(hit_ends);
}
protected CircleCastHit CircleCastHandle_Ends(CircleCast cast)
{
Point p1 = new Point(point1);
Point p2 = new Point(point2);
var hit1 = p1.CircleCastHandle(cast);
var hit2 = p2.CircleCastHandle(cast);
if (hit1.EarlierThan(hit2))
{
return(hit1);
}
return(hit2);
}
// public Wall(double a, double b, double c, double margin=0) : base(a, b, c){
// this.margin = margin;
// }
public override CircleCastHit CircleCastHandle(CircleCast cast)
{
CircleCast surrCast = new CircleCast(cast.origin, cast.rad, cast.r + margin);
var hit = base.CircleCastHandle(surrCast);
if (!hit.isHit)
{
return(hit);
}
var contactVec = hit.contactPos - hit.hitCirclePos;
var contact = (contactVec.mag - margin) * contactVec.unit + hit.hitCirclePos;
return(new CircleCastHit(
isHit: true, isOriginHit: hit.isOriginHit,
originCirclePos: cast.origin,
hitCirclePos: hit.hitCirclePos,
contactPos: contact
));
}
protected CircleCastHit CircleCastHandle_ExceptEnds(CircleCast cast)
{
if (this.DistToPoint(cast.origin) < cast.r)
{
// Already collided at origin position
if (this.FootOnThis(cast.origin))
{
return(new CircleCastHit
(
isHit: true, isOriginHit: true, originCirclePos: cast.origin,
hitCirclePos: cast.origin, contactPos: FootPoint(cast.origin)
));
}
// Except Collide Inside out
else
{
return(CircleCastHit.NotHit(cast.origin));
}
}
// Detect collision between ends
var marginLine = this.MovedAlongNormToPointSide(cast.origin, cast.r);
var castLine = cast.GetLineBase();
var sec = castLine.Intersect(marginLine);
if (!sec.isNull)
{
return(new CircleCastHit
(
isHit: true, isOriginHit: false, originCirclePos: cast.origin,
hitCirclePos: sec, contactPos: FootPoint(sec)
));
}
// No Collision
return(CircleCastHit.NotHit(cast.origin));
}