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); }