private bool CheckVisibilityRecursive(Vector3 q1, Vector3 q2, float radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
else
{
Obstacle obstacle1 = node.m_Obstacle;
Obstacle obstacle2 = obstacle1.m_NextObstacle;
float q1LeftOfI = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q1);
float q2LeftOfI = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q2);
float invLengthI = 1.0f / VectorMath.absSq(obstacle2.m_Point - obstacle1.m_Point);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f)
{
return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Right)));
}
else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f)
{
return(CheckVisibilityRecursive(q1, q2, radius, node.m_Right) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Left)));
}
else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f)
{
/* One can see through obstacle from left to right. */
return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right));
}
else
{
float point1LeftOfQ = VectorMath.leftOf(q1, q2, obstacle1.m_Point);
float point2LeftOfQ = VectorMath.leftOf(q1, q2, obstacle2.m_Point);
float invLengthQ = 1.0f / VectorMath.absSq(q2 - q1);
return(point1LeftOfQ * point2LeftOfQ >= 0.0f && VectorMath.sqr(point1LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && VectorMath.sqr(point2LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right));
}
}
}
private ObstacleTreeNode BuildRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
else
{
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
int leftSize = 0;
int rightSize = 0;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.m_NextObstacle;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle;
float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point);
float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point);
if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.m_NextObstacle;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle;
float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point);
float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point);
if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
float t = VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleI1.m_Point) / VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleJ2.m_Point);
Vector3 splitpoint = obstacleJ1.m_Point + t * (obstacleJ2.m_Point - obstacleJ1.m_Point);
Obstacle newObstacle = new Obstacle();
newObstacle.m_Point = splitpoint;
newObstacle.m_PrevObstacle = obstacleJ1;
newObstacle.m_NextObstacle = obstacleJ2;
newObstacle.m_IsConvex = true;
newObstacle.m_UnitDir = obstacleJ1.m_UnitDir;
m_Obstacles.Add(newObstacle);
obstacleJ1.m_NextObstacle = newObstacle;
obstacleJ2.m_PrevObstacle = newObstacle;
if (j1LeftOfI > 0.0f)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.m_Obstacle = obstacleI1;
node.m_Left = BuildRecursive(leftObstacles);
node.m_Right = BuildRecursive(rightObstacles);
return(node);
}
}
}
