private void WeightWayPointsAlongSegments()
{
for (var i = 0; i < _vertices.Count; i++)
{
var firstVertex = _vertices[i];
var nextVertex = i == _vertices.Count - 1 ? _vertices[0] : _vertices[i + 1];
_wayPoints.Add(firstVertex);
var distanceBetween = Vector2.Distance(firstVertex.ProjectedPoint, nextVertex.ProjectedPoint);
var pointsToAdd = Mathf.FloorToInt(distanceBetween / _minDistanceBetweenNewPointsBetweenVertices) <= 0
? 1
: Mathf.FloorToInt(distanceBetween / _minDistanceBetweenNewPointsBetweenVertices);
if (pointsToAdd == 1)
{
var obstaclePoint = new ObstaclePoint
{
IsSidePoint = true,
PreviousVertexPoint = firstVertex,
NextVertexPoint = nextVertex,
PercentOfSegment = 0.5f,
ProjectedPoint = Vector2.Lerp(firstVertex.ProjectedPoint, nextVertex.ProjectedPoint, 0.5f)
};
_wayPoints.Add(obstaclePoint);
}
else
{
for (var j = 0; j < pointsToAdd - 1; j++)
{
var percentOfSegment = (j + 1) / (float)pointsToAdd;
var pointPosition =
Vector2.Lerp(firstVertex.ProjectedPoint, nextVertex.ProjectedPoint, percentOfSegment);
var projectedPointFrom = pointPosition;
var projectionDirection = firstVertex.Normal;
var obstaclePoint = new ObstaclePoint
{
IsSidePoint = true,
StartingPosition = pointPosition,
ProjectedPointFrom = projectedPointFrom,
ProjectionDirection = projectionDirection,
ProjectedPoint = pointPosition,
NextVertexPoint = nextVertex,
PreviousVertexPoint = firstVertex,
PercentOfSegment = percentOfSegment
};
_wayPoints.Add(obstaclePoint);
}
}
}
}
private void ResetForNewDraw()
{
_pointIndex = 0;
_startingPoint = null;
_shouldNotCheckForCorners = false;
_wayPoints.Clear();
_obstaclePoints.Clear();
_vertices.Clear();
_debugIntersections.Clear();
_debugTangent.Clear();
_debugReverseNormals.Clear();
_debugReverseTangents.Clear();
}
public Queue <ObstaclePoint> BestPath(ObstaclePoint closestToStart, ObstaclePoint closestToEnd, Vector2 destination)
{
var forwardPath = new Queue <ObstaclePoint>();
var reversePath = new Queue <ObstaclePoint>();
var closestPointToDestinationIndex = closestToEnd.WayPointIndex;
var currentObstaclePoint = closestToStart;
forwardPath.Enqueue(currentObstaclePoint);
var forwardDistanceSum = 0f;
while (currentObstaclePoint.WayPointIndex != closestPointToDestinationIndex)
{
forwardDistanceSum += currentObstaclePoint.DistanceToNextWayPoint;
currentObstaclePoint = currentObstaclePoint.NextWayPoint;
forwardPath.Enqueue(currentObstaclePoint);
}
var closestToFinalDestination = Vector2.Distance(closestToEnd.ProjectedPoint, destination);
currentObstaclePoint = closestToStart;
reversePath.Enqueue(currentObstaclePoint);
var reverseDistanceSum = 0f;
while (currentObstaclePoint.WayPointIndex != closestPointToDestinationIndex)
{
reverseDistanceSum += currentObstaclePoint.DistanceToPreviousWayPoint;
currentObstaclePoint = currentObstaclePoint.PreviousWayPoint;
reversePath.Enqueue(currentObstaclePoint);
}
forwardDistanceSum += closestToFinalDestination;
reverseDistanceSum += closestToFinalDestination;
var selectedPath = forwardDistanceSum < reverseDistanceSum ? forwardPath : reversePath;
_selectedPath = selectedPath;
return(selectedPath);
}
private void FindNextPoint(Vector2 point, Vector2 normal)
{
var tangent = (Vector2)Vector3.Cross(normal, Vector3.forward);
var offsetPoint = point + normal * OffsetDistance;
var tangentCheckPoint = offsetPoint + tangent * FixedStep;
var reverseNormalCheckPoint = tangentCheckPoint - normal * (OffsetDistance + .005f);
var reverseTangentCheckPoint = reverseNormalCheckPoint - tangent * (FixedStep * 2);
var found = false;
var obstaclePoint = new ObstaclePoint {
StartingPosition = point, Normal = normal, ObstaclePointIndex = _pointIndex++, Tangent = tangent
};
if (_startingPoint == null)
{
_startingPoint = obstaclePoint;
}
if (_obstaclePoints.Count > 1)
{
if (Vector2.Distance(_startingPoint.StartingPosition, point) < FixedStep)
{
return;
}
if (_vertices.Count > 0)
{
if (Vector2.Distance(_startingPoint.StartingPosition, obstaclePoint.StartingPosition) <= FixedStep && _vertices[0].Normal == normal)
{
_shouldNotCheckForCorners = true;
}
}
if (Vector2.Dot(_obstaclePoints[_obstaclePoints.Count - 1].Normal, normal) < 0.95f && !_shouldNotCheckForCorners)
{
if (_obstaclePoints[_obstaclePoints.Count - 1].NextIsCorner)
{
obstaclePoint.IsInsideCorner = true;
}
_vertices.Add(obstaclePoint);
}
}
_obstaclePoints.Add(obstaclePoint);
var hit = Physics2D.Raycast(offsetPoint, tangent, FixedStep);
if (hit)
{
obstaclePoint.NextIsCorner = true;
_debugReverseTangents.Add(new [] { offsetPoint, hit.point });
found = true;
}
else
{
_debugReverseTangents.Add(new [] { offsetPoint, tangentCheckPoint });
hit = Physics2D.Raycast(tangentCheckPoint, -normal, OffsetDistance + FixedStep);
if (hit)
{
_debugReverseNormals.Add(new [] { tangentCheckPoint, hit.point });
found = true;
}
else
{
_debugReverseTangents.Add(new [] { tangentCheckPoint, reverseNormalCheckPoint });
hit = Physics2D.Raycast(reverseNormalCheckPoint, -tangent, FixedStep * 2);
if (hit)
{
_debugReverseTangents.Add(new [] { reverseNormalCheckPoint, hit.point });
found = true;
}
else
{
_debugReverseTangents.Add(new [] { reverseNormalCheckPoint, reverseTangentCheckPoint });
}
}
}
if (found && _obstaclePoints.Count < MaxChecks)
{
FindNextPoint(hit.point, hit.normal);
}
}
public override void MarkIntersectWithSurface(ObstacleSurface otherSurface, MovementMechanism forMechanism)
{
Hitbox hitbox = forMechanism.Hitbox;
switch (hitbox.Type)
{
case Hitbox.HitboxType.Square:
{
//SquareHitbox square = hitbox as SquareHitbox;
}
break;
default:
throw new NotImplementedException("Unknown hitbox type");
}
SimpleVolume vol = MyVolume;
Orientation offset = forMechanism.HitboxOffset(otherSurface);
//Reverse changes to find how this object intersects with original surface instead of the other way around.
vol.MinX = vol.MinX - offset.x - hitbox.MaxXIncrease((Rotation)offset);
vol.MaxX = vol.MaxX - offset.x + hitbox.MaxXDecrease((Rotation)offset);
vol.MinY = vol.MinY - offset.y - hitbox.MaxYIncrease((Rotation)offset);
vol.MaxY = vol.MaxY - offset.y + hitbox.MaxYDecrease((Rotation)offset);
vol.MinZ = vol.MinZ - offset.z - hitbox.MaxZIncrease((Rotation)offset);
vol.MaxZ = vol.MaxZ - offset.z + hitbox.MaxZDecrease((Rotation)offset);
Vector normal = otherSurface.GetNormal();
int height = otherSurface.GetHeight();
//Important: These indices match the obstacle point indices.
int[] differences = new int[8];
differences[0] = (int)normal.DotProduct(vol.MinX, vol.MinY, vol.MinZ) - height;
differences[1] = (int)normal.DotProduct(vol.MaxX, vol.MinY, vol.MinZ) - height;
differences[2] = (int)normal.DotProduct(vol.MaxX, vol.MaxY, vol.MinZ) - height;
differences[3] = (int)normal.DotProduct(vol.MinX, vol.MaxY, vol.MinZ) - height;
differences[4] = (int)normal.DotProduct(vol.MinX, vol.MinY, vol.MaxZ) - height;
differences[5] = (int)normal.DotProduct(vol.MaxX, vol.MinY, vol.MaxZ) - height;
differences[6] = (int)normal.DotProduct(vol.MaxX, vol.MaxY, vol.MaxZ) - height;
differences[7] = (int)normal.DotProduct(vol.MinX, vol.MaxY, vol.MaxZ) - height;
List <int> negativeHeights = new List <int>(8);
List <int> positiveHeights = new List <int>(8);
if (differences[0] > 0)
{
positiveHeights.Add(0);
}
else
{
negativeHeights.Add(0);
}
if (differences[1] > 0)
{
positiveHeights.Add(1);
}
else
{
negativeHeights.Add(1);
}
if (differences[2] > 0)
{
positiveHeights.Add(2);
}
else
{
negativeHeights.Add(2);
}
if (differences[3] > 0)
{
positiveHeights.Add(3);
}
else
{
negativeHeights.Add(3);
}
if (differences[4] > 0)
{
positiveHeights.Add(4);
}
else
{
negativeHeights.Add(4);
}
if (differences[5] > 0)
{
positiveHeights.Add(5);
}
else
{
negativeHeights.Add(5);
}
if (differences[6] > 0)
{
positiveHeights.Add(6);
}
else
{
negativeHeights.Add(6);
}
if (differences[7] > 0)
{
positiveHeights.Add(7);
}
else
{
negativeHeights.Add(7);
}
if (positiveHeights.Count == 0 || negativeHeights.Count == 0)
{
return;
}
for (int i = 0; i < negativeHeights.Count; i++)
{
int pointId = negativeHeights[i];
for (int j = 0; j < 3; j++)
{
SurfaceLine line = PointLine(pointId, j);
ObstaclePoint otherPoint = line.Start;
if (otherPoint.id == pointId)
{
otherPoint = line.End;
}
if (differences[otherPoint.id] > 0)
{
double ratio = differences[pointId] / (differences[pointId] - differences[otherPoint.id]);
Point start = PointPosition(pointId);
Point end = otherPoint.Position;
Point intersectPoint;
intersectPoint.x = start.x + (int)((end.x - start.x) * ratio);
intersectPoint.y = start.y + (int)((end.y - start.y) * ratio);
intersectPoint.z = start.z + (int)((end.z - start.z) * ratio);
List <Point> IntersectingPoints = new List <Point>();
IntersectingPoints.Add(intersectPoint);
SurfaceLine previousLine = line;
SurfaceLine nextLine = new SurfaceLine(); //compiler has failed me for once, this shouldn't need initialization here.
int previousSurfaceId = -1;
do
{
ObstacleSurface nextSurface = previousLine.Left;
if (previousSurfaceId == nextSurface.surfaceIndex)
{
nextSurface = previousLine.Right;
}
for (int k = 0; k < 4; k++)
{
nextLine = nextSurface.GetLine(k);
if (nextLine.id == previousLine.id)
{
continue;
}
ObstaclePoint nextStart = nextLine.Start;
ObstaclePoint nextEnd = nextLine.End;
if ((differences[nextStart.id] > 0 && differences[nextEnd.id] <= 0) ||
(differences[nextStart.id] <= 0 && differences[nextEnd.id] > 0))
{
start = nextStart.Position;
end = nextEnd.Position;
ratio = differences[nextStart.id] / (differences[nextStart.id] - differences[nextEnd.id]);
intersectPoint.x = start.x + (int)((end.x - start.x) * ratio);
intersectPoint.y = start.y + (int)((end.y - start.y) * ratio);
intersectPoint.z = start.z + (int)((end.z - start.z) * ratio);
IntersectingPoints.Add(intersectPoint);
break;
}
}
previousSurfaceId = nextSurface.surfaceIndex;
previousLine = nextLine;
}while (nextLine.id != line.id);
otherSurface.AddIntersectLines(IntersectingPoints, this, forMechanism);
return;
}
}
}
}
