private void ApplyCone(Vector3d center3d, Vector2d forward, long radius, long angle)
{
Vector2d center = center3d.ToVector2d();
long fastRange = radius * radius;
Scan(center, radius);
for (int i = 0; i < ScanOutput.Count; i++)
{
RTSAgent agent = ScanOutput [i];
Vector2d agentPos = agent.Body._position;
Vector2d difference = agentPos - center;
if (difference.FastMagnitude() > fastRange)
{
continue;
}
if (forward.Dot(difference) < 0)
{
continue;
}
difference.Normalize();
long cross = forward.Cross(difference).Abs();
if (cross > angle)
{
continue;
}
HitAgent(agent);
}
}
public static void PrepareAxisCheck(Vector2d p1, Vector2d p2, bool calculateIntersectionPoints = true)
{
cacheP1 = p1;
cacheP2 = p2;
cacheAxis = p2 - p1;
cacheAxis.Normalize();
cacheAxisNormal = cacheAxis.rotatedLeft;
axisMin = p1.Dot(cacheAxis.x, cacheAxis.y);
axisMax = p2.Dot(cacheAxis.x, cacheAxis.y);
cacheProj = cacheP1.Dot(cacheAxis.x, cacheAxis.y);
cacheProjPerp = cacheP1.Dot(cacheAxisNormal.x, cacheAxisNormal.y);
perpVector = cacheAxisNormal * cacheProjPerp;
calculateIntersections = calculateIntersectionPoints;
}
public bool IsPositionCovered(Vector2d position)
{
//Checks if this body covers a position
//Different techniques for different shapes
switch (this.Shape)
{
case ColliderType.Circle:
long maxDistance = this.Radius + FixedMath.Half;
maxDistance *= maxDistance;
if ((this._position - position).FastMagnitude() > maxDistance)
{
return(false);
}
goto case ColliderType.AABox;
case ColliderType.AABox:
return(position.x + FixedMath.Half > this.XMin && position.x - FixedMath.Half < this.XMax &&
position.y + FixedMath.Half > this.YMin && position.y - FixedMath.Half < this.YMax);
//break;
case ColliderType.Polygon:
for (int i = this.EdgeNorms.Length - 1; i >= 0; i--)
{
Vector2d norm = this.EdgeNorms[i];
long posProj = norm.Dot(position);
long polyMin, polyMax;
CollisionPair.ProjectPolygon(norm.x, norm.y, this, out polyMin, out polyMax);
if (posProj >= polyMin && posProj <= polyMax)
{
}
else
{
return(false);
}
}
return(true);
//break;
}
return(false);
}
public bool Overlaps(FastList <Vector2d> outputIntersectionPoints)
{
outputIntersectionPoints.FastClear();
//Checks if this object overlaps the line formed by p1 and p2
switch (this.Shape)
{
case ColliderType.Circle:
{
bool overlaps = false;
//Check if the circle completely fits between the line
long projPos = this._position.Dot(cacheAxis.x, cacheAxis.y);
//Circle withing bounds?
if (projPos >= axisMin && projPos <= axisMax)
{
long projPerp = this._position.Dot(cacheAxisNormal.x, cacheAxisNormal.y);
long perpDif = (cacheProjPerp - projPerp);
long perpDist = perpDif.Abs();
if (perpDist <= _radius)
{
overlaps = true;
}
if (overlaps)
{
long sin = (perpDif);
long cos = FixedMath.Sqrt(_radius.Mul(_radius) - sin.Mul(sin));
if (cos == 0)
{
outputIntersectionPoints.Add((cacheAxis * projPos) + perpVector);
}
else
{
outputIntersectionPoints.Add(cacheAxis * (projPos - cos) + perpVector);
outputIntersectionPoints.Add(cacheAxis * (projPos + cos) + perpVector);
}
}
}
else
{
//If not, check distances to points
long p1Dist = _position.FastDistance(cacheP1.x, cacheP2.y);
if (p1Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP1);
overlaps = true;
}
long p2Dist = _position.FastDistance(cacheP2.x, cacheP2.y);
if (p2Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP2);
overlaps = true;
}
}
return(overlaps);
}
//break;
case ColliderType.AABox:
{
}
break;
case ColliderType.Polygon:
{
bool intersected = false;
for (int i = 0; i < this.Vertices.Length; i++)
{
int edgeIndex = i;
Vector2d pivot = this.RealPoints [edgeIndex];
Vector2d edge = this.Edges [edgeIndex];
long proj1 = 0;
int nextIndex = edgeIndex + 1 < this.RealPoints.Length ? edgeIndex + 1 : 0;
Vector2d nextPoint = RealPoints [nextIndex];
long proj2 = (nextPoint - pivot).Dot(edge);
long min;
long max;
if (proj1 < proj2)
{
min = proj1;
max = proj2;
}
else
{
min = proj2;
max = proj1;
}
long lineProj1 = (cacheP1 - pivot).Dot(edge);
long lineProj2 = (cacheP2 - pivot).Dot(edge);
long lineMin;
long lineMax;
if (lineProj1 < lineProj2)
{
lineMin = lineProj1;
lineMax = lineProj2;
}
else
{
lineMin = lineProj2;
lineMax = lineProj1;
}
if (CollisionPair.CheckOverlap(min, max, lineMin, lineMax))
{
Vector2d edgeNorm = this.EdgeNorms [edgeIndex];
long normProj = 0;
long normLineProj1 = (cacheP1 - pivot).Dot(edgeNorm);
long normLineProj2 = (cacheP2 - pivot).Dot(edgeNorm);
long normLineMin;
long normLineMax;
if (normLineProj1 < normLineProj2)
{
normLineMin = normLineProj1;
normLineMax = normLineProj2;
}
else
{
normLineMin = normLineProj2;
normLineMax = normLineProj1;
}
if (normProj >= normLineMin && normProj <= normLineMax)
{
long revProj1 = pivot.Dot(LSBody.cacheAxisNormal);
long revProj2 = nextPoint.Dot(cacheAxisNormal);
long revMin;
long revMax;
if (revProj1 < revProj2)
{
revMin = revProj1;
revMax = revProj2;
}
else
{
revMin = revProj2;
revMax = revProj1;
}
if (LSBody.cacheProjPerp >= revMin && LSBody.cacheProjPerp <= revMax)
{
intersected = true;
if (LSBody.calculateIntersections)
{
long fraction = normLineProj1.Abs().Div(normLineMax - normLineMin);
long intersectionProj = FixedMath.Lerp(lineProj1, lineProj2, fraction);
outputIntersectionPoints.Add(edge * intersectionProj + pivot);
if (outputIntersectionPoints.Count == 2)
{
break;
}
}
}
}
}
}
return(intersected);
}
//break;
}
return(false);
}
