I need this geometry function faster

N

not_a_commie

I have here a function at the crux of my path search heuristic. The
function takes two points, an angle at each point, and a radius and
returns the shortest arc-line-arc triplet connecting those two points.
The function creates four connections between adjacent circles at each
end and returns the shortest of those connections. I need it to run
500k times a second. Currently it runs at a 10th that speed. I'm
grateful for any speedup ideas that you can offer! I think I'm just
allocating too many objects, but I'm not sure what to do about it. A
few notes: SegmentList is the same as List with a function for getting
the summed length of all its members and a length comparison function
with an early exit. Vector and Point are the same as Vector and Point
shipping with .NET 3.0. They have a few extension functions for
getting the current angle (an atan2 call) and creating a vector from
an angle (a sin and cos call). A negative radius is a clockwise arc.
CC is short for counter-clockwise. My arc and line classes have
internal constructors that allow you to set the cached entrance and
exit angles and the length directly rather than having to calculate
those on their first call. NaN means the cache is invalid.

public static double ShortestArcLineArc(Point start, double
startAngle, Point end, double endAngle, double minRadius, out
SegmentList<ISegment> shortestPath)
{
SegmentList<ISegment> tempPath = new SegmentList<ISegment>();
SegmentList<ISegment> shortPath = new SegmentList<ISegment>();
Point startCC = start + Vector.FromLengthAngle(minRadius, startAngle
+ Constants.PI_OVER_TWO);
Point endCC = end + Vector.FromLengthAngle(minRadius, endAngle +
Constants.PI_OVER_TWO);
Point endC = end + Vector.FromLengthAngle(minRadius, endAngle -
Constants.PI_OVER_TWO);
Point startC = start + Vector.FromLengthAngle(minRadius, startAngle -
Constants.PI_OVER_TWO);
Point lineStart, lineEnd;
Vector startCCtoEndCC = endCC - startCC;
Vector startCCtoEndC = endC - startCC;
Vector startCtoEndC = endC - startC;
Vector startCtoEndCC = endCC - startC;

// toss any diagonal on an overlapping circle:
double radius2 = minRadius * minRadius * 4, lastAngle; // diameter
squared

// startCCtoEndCC is our line. it just needs to be connected to the
right start point
lastAngle = startCCtoEndCC.Angle;
lineStart = startCC + Vector.FromLengthAngle(minRadius, lastAngle -
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCCtoEndCC;
if (!start.Equals(lineStart))
shortPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, lastAngle, double.NaN));
shortPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
shortPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
lastAngle, endAngle, double.NaN));

// startCtoEndC is our line. it just needs to be connected to the
right start point
lastAngle = startCtoEndC.Angle;
lineStart = startC + Vector.FromLengthAngle(minRadius, lastAngle +
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCtoEndC;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, lastAngle, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
lastAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 || tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
// in these next two our angle is not exactly Constants.PI_OVER_TWO;
that leads to an unfortunate arccos call and a sqrt on the length
if (startCCtoEndC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCCtoEndC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCCtoEndC.Angle -
diff);
lineStart = startCC + v;
lineEnd = endC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
if (startCtoEndCC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCtoEndCC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCtoEndCC.Angle +
diff);
lineStart = startC + v;
lineEnd = endCC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
shortestPath = shortPath;
return shortestPath.Length;
}
 
N

Nicholas Paldino [.NET/C# MVP]

not_a_commie.

First, Vector is not a class that ships with .NET 3.0.

Second, you should profile your code, and try to determine which calls
are taking the most time. Look at the StopWatch class in the
System.Diagnostics namespace, and also there are profilers that you can get
for free (I believe that certain versions of VS.NET have profiling
capability, but that might only be for Team versions).

Beyond that, without knowing anything else about the code, I can offer
the following advice:

- When making the call to FromLengthAngle, if the call to the static
FromLengthAngle is costly, then when trying to get the point in the other
direction, consider just doing a geometric transformation, reflecting along
the axis created by the angle. The result should be the same.

- You might want to consider setting the Capacity on your SegmentList to
something like 8 or 16 (at least from what I can tell). Initially, it will
be allocated to four elements (when the first element is added), and then
double as needed. I see a few calls to Add and AddRange which might cause
the capacity to grow beyond four, so you can save a little bit of time by
pre-allocating the array.

- You have a few calls where you do this:

shortPath.Clear();
shortPath.AddRange(tempPath);

Instead of doing that, just create a new instance, passing the tempPath
(assuming that you have exposed the constructor that takes an
IEnumerable<T>). The constructor will pre-allocate the internal store of
objects to the size of the list passed in, given that tempPath implements
ICollection<T>. Mind you, this point is based on an implementation detail,
so this is subject to change at any time.

- There is a call to Clear on the tempPath variable at the end of the
routine which just isn't needed.

--
- Nicholas Paldino [.NET/C# MVP]
- (e-mail address removed)


not_a_commie said:
I have here a function at the crux of my path search heuristic. The
function takes two points, an angle at each point, and a radius and
returns the shortest arc-line-arc triplet connecting those two points.
The function creates four connections between adjacent circles at each
end and returns the shortest of those connections. I need it to run
500k times a second. Currently it runs at a 10th that speed. I'm
grateful for any speedup ideas that you can offer! I think I'm just
allocating too many objects, but I'm not sure what to do about it. A
few notes: SegmentList is the same as List with a function for getting
the summed length of all its members and a length comparison function
with an early exit. Vector and Point are the same as Vector and Point
shipping with .NET 3.0. They have a few extension functions for
getting the current angle (an atan2 call) and creating a vector from
an angle (a sin and cos call). A negative radius is a clockwise arc.
CC is short for counter-clockwise. My arc and line classes have
internal constructors that allow you to set the cached entrance and
exit angles and the length directly rather than having to calculate
those on their first call. NaN means the cache is invalid.

public static double ShortestArcLineArc(Point start, double
startAngle, Point end, double endAngle, double minRadius, out
SegmentList<ISegment> shortestPath)
{
SegmentList<ISegment> tempPath = new SegmentList<ISegment>();
SegmentList<ISegment> shortPath = new SegmentList<ISegment>();
Point startCC = start + Vector.FromLengthAngle(minRadius, startAngle
+ Constants.PI_OVER_TWO);
Point endCC = end + Vector.FromLengthAngle(minRadius, endAngle +
Constants.PI_OVER_TWO);
Point endC = end + Vector.FromLengthAngle(minRadius, endAngle -
Constants.PI_OVER_TWO);
Point startC = start + Vector.FromLengthAngle(minRadius, startAngle -
Constants.PI_OVER_TWO);
Point lineStart, lineEnd;
Vector startCCtoEndCC = endCC - startCC;
Vector startCCtoEndC = endC - startCC;
Vector startCtoEndC = endC - startC;
Vector startCtoEndCC = endCC - startC;

// toss any diagonal on an overlapping circle:
double radius2 = minRadius * minRadius * 4, lastAngle; // diameter
squared

// startCCtoEndCC is our line. it just needs to be connected to the
right start point
lastAngle = startCCtoEndCC.Angle;
lineStart = startCC + Vector.FromLengthAngle(minRadius, lastAngle -
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCCtoEndCC;
if (!start.Equals(lineStart))
shortPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, lastAngle, double.NaN));
shortPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
shortPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
lastAngle, endAngle, double.NaN));

// startCtoEndC is our line. it just needs to be connected to the
right start point
lastAngle = startCtoEndC.Angle;
lineStart = startC + Vector.FromLengthAngle(minRadius, lastAngle +
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCtoEndC;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, lastAngle, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
lastAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 || tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
// in these next two our angle is not exactly Constants.PI_OVER_TWO;
that leads to an unfortunate arccos call and a sqrt on the length
if (startCCtoEndC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCCtoEndC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCCtoEndC.Angle -
diff);
lineStart = startCC + v;
lineEnd = endC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
if (startCtoEndCC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCtoEndCC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCtoEndCC.Angle +
diff);
lineStart = startC + v;
lineEnd = endCC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
shortestPath = shortPath;
return shortestPath.Length;
}
Click to expand...
 
F

Family Tree Mike

I'm going to look at the code now because I find the problem intriguing. I'm
guessing your description and the code aren't agreeing however, as it should
be a much smaller code to find the closest pair of points from a set of four
I have here a function at the crux of my path search heuristic. The
function takes two points, an angle at each point, and a radius and
returns the shortest arc-line-arc triplet connecting those two points.
The function creates four connections between adjacent circles at each
end and returns the shortest of those connections. I need it to run
500k times a second. Currently it runs at a 10th that speed. I'm
grateful for any speedup ideas that you can offer! I think I'm just
allocating too many objects, but I'm not sure what to do about it. A
few notes: SegmentList is the same as List with a function for getting
the summed length of all its members and a length comparison function
with an early exit. Vector and Point are the same as Vector and Point
shipping with .NET 3.0. They have a few extension functions for
getting the current angle (an atan2 call) and creating a vector from
an angle (a sin and cos call). A negative radius is a clockwise arc.
CC is short for counter-clockwise. My arc and line classes have
internal constructors that allow you to set the cached entrance and
exit angles and the length directly rather than having to calculate
those on their first call. NaN means the cache is invalid.

public static double ShortestArcLineArc(Point start, double
startAngle, Point end, double endAngle, double minRadius, out
SegmentList<ISegment> shortestPath)
{
SegmentList<ISegment> tempPath = new SegmentList<ISegment>();
SegmentList<ISegment> shortPath = new SegmentList<ISegment>();
Point startCC = start + Vector.FromLengthAngle(minRadius, startAngle
+ Constants.PI_OVER_TWO);
Point endCC = end + Vector.FromLengthAngle(minRadius, endAngle +
Constants.PI_OVER_TWO);
Point endC = end + Vector.FromLengthAngle(minRadius, endAngle -
Constants.PI_OVER_TWO);
Point startC = start + Vector.FromLengthAngle(minRadius, startAngle -
Constants.PI_OVER_TWO);
Point lineStart, lineEnd;
Vector startCCtoEndCC = endCC - startCC;
Vector startCCtoEndC = endC - startCC;
Vector startCtoEndC = endC - startC;
Vector startCtoEndCC = endCC - startC;

// toss any diagonal on an overlapping circle:
double radius2 = minRadius * minRadius * 4, lastAngle; // diameter
squared

// startCCtoEndCC is our line. it just needs to be connected to the
right start point
lastAngle = startCCtoEndCC.Angle;
lineStart = startCC + Vector.FromLengthAngle(minRadius, lastAngle -
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCCtoEndCC;
if (!start.Equals(lineStart))
shortPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, lastAngle, double.NaN));
shortPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
shortPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
lastAngle, endAngle, double.NaN));

// startCtoEndC is our line. it just needs to be connected to the
right start point
lastAngle = startCtoEndC.Angle;
lineStart = startC + Vector.FromLengthAngle(minRadius, lastAngle +
Constants.PI_OVER_TWO);
lineEnd = lineStart + startCtoEndC;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, lastAngle, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd, lastAngle,
lineStart.DistanceTo(lineEnd)));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
lastAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 || tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
// in these next two our angle is not exactly Constants.PI_OVER_TWO;
that leads to an unfortunate arccos call and a sqrt on the length
if (startCCtoEndC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCCtoEndC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCCtoEndC.Angle -
diff);
lineStart = startCC + v;
lineEnd = endC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startCC, minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endC, -minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
if (startCtoEndCC.LengthSquared >= radius2)
{
double diff = Math.Acos(minRadius / (startCtoEndCC.Length * 0.5));
Vector v = Vector.FromLengthAngle(minRadius, startCtoEndCC.Angle +
diff);
lineStart = startC + v;
lineEnd = endCC - v;
if (!start.Equals(lineStart))
tempPath.Add(new ArcSegment(start, lineStart, startC, -minRadius,
startAngle, double.NaN, double.NaN));
tempPath.Add(new LineSegment(lineStart, lineEnd));
if (!lineEnd.Equals(end))
tempPath.Add(new ArcSegment(lineEnd, end, endCC, minRadius,
tempPath[0].EndAngle, endAngle, double.NaN));
if (shortPath.Count <= 0 ||
tempPath.IsShorterThan(shortPath.Length))
{
shortPath.Clear();
shortPath.AddRange(tempPath);
}
tempPath.Clear();
}
shortestPath = shortPath;
return shortestPath.Length;
}
Click to expand...
 
B

Ben Voigt [C++ MVP]

Nicholas said:
not_a_commie.

First, Vector is not a class that ships with .NET 3.0.

Second, you should profile your code, and try to determine which
calls are taking the most time. Look at the StopWatch class in the
System.Diagnostics namespace, and also there are profilers that you
can get for free (I believe that certain versions of VS.NET have
profiling capability, but that might only be for Team versions).

Beyond that, without knowing anything else about the code, I can
offer the following advice:

- When making the call to FromLengthAngle, if the call to the static
FromLengthAngle is costly, then when trying to get the point in the
other direction, consider just doing a geometric transformation,
reflecting along the axis created by the angle. The result should be
the same.
Click to expand...

That reflection is just multiplication by -1.

Or more generally, inline these calls, apply trig identities for
sum-of-angles and difference-of-angles, evaluate constant functions (of
PI/2), and perform common subexpression optimization on (minRadius *
sin(startAngle)), (minRadius * cos(startAngle)), (minRadius *
sin(endAngle)), etc.

For one thing, I think

lineStart == start

All Nicholas's other tips are very good too.
 
N

not_a_commie

and perform common subexpression optimization on (minRadius *
sin(startAngle)), (minRadius * cos(startAngle)), (minRadius *
sin(endAngle)), etc.
Click to expand...

That's a great suggestion. Thanks.
For one thing, I think
lineStart == start
Click to expand...

Um, not true. Let me clarify the problem a little more. You are given
a minimum radius R and two points: (Ax, Ay, At) and (Bx, By, Bt) where
At is the angle at A and Bt is the angle at B. Find the shortest arc-
line-arc triplet connecting A and B such that the first arc is tangent
at A and tangent at the line and the last arc is tangent at the line
and tangent at B.
the closest points on the circle will always be the
intersection of the line from the circle centroids
with the circles.
Click to expand...

I don't understand that statement.
 
F

Family Tree Mike

not_a_commie said:
That's a great suggestion. Thanks.


Um, not true. Let me clarify the problem a little more. You are given
a minimum radius R and two points: (Ax, Ay, At) and (Bx, By, Bt) where
At is the angle at A and Bt is the angle at B. Find the shortest arc-
line-arc triplet connecting A and B such that the first arc is tangent
at A and tangent at the line and the last arc is tangent at the line
and tangent at B.


I don't understand that statement.
Click to expand...

Thanks. Your description helped me. From the original description it
sounded as though the code was to find the closest two points on the two
circles (to each other). That is easily found by connecting the centerpoints
with a line and finding the intersects with the circles.
 
B

Ben Voigt [C++ MVP]

not_a_commie said:
That's a great suggestion. Thanks.


Um, not true. Let me clarify the problem a little more. You are given
Click to expand...

You're right, I was cancelling things but the angle variables were
different.
 
S

sherifffruitfly

Or more generally, inline these calls, apply trig identities for
sum-of-angles and difference-of-angles, evaluate constant functions (of
PI/2), and perform common subexpression optimization on (minRadius *
sin(startAngle)), (minRadius * cos(startAngle)), (minRadius *
sin(endAngle)), etc.
Click to expand...

Avoiding catastrophic cancellation and other numerical instability
issues, of course.
 
Top