Skip to content

Commit 451f19e

Browse files
asikasik
committed
Added approximate Atan2 implementation
1 parent 4d6d391 commit 451f19e

File tree

3 files changed

+157
-29
lines changed

3 files changed

+157
-29
lines changed

‎Fix16.cs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -418,6 +418,7 @@ public static Fix16 Asin(Fix16 x) {
418418

419419

420420
public static Fix16 Atan2(Fix16 inY, Fix16 inX) {
421+
// This code is based on http://en.wikipedia.org/wiki/User:Msiddalingaiah/Ideas#Fast_arc_tangent
421422
var hash = (uint)(inX.m_rawValue ^ inY.m_rawValue);
422423
hash ^= hash >> 20;
423424
hash &= 0x0FFF;

‎Fix64.cs‎

Lines changed: 71 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public partial struct Fix64 : IEquatable<Fix64>, IComparable<Fix64> {
1515
readonly long m_rawValue;
1616

1717
// Precision of this type is 2^-32, that is 2,3283064365386962890625E-10
18-
public static readonly decimal Precision = (decimal)(new Fix64(1));//0.00000000023283064365386962890625m;
18+
public static readonly decimal Precision = (decimal)(new Fix64(1L));//0.00000000023283064365386962890625m;
1919
public static readonly Fix64 MaxValue = new Fix64(MAX_VALUE);
2020
public static readonly Fix64 MinValue = new Fix64(MIN_VALUE);
2121
public static readonly Fix64 One = new Fix64(ONE);
@@ -33,8 +33,8 @@ public partial struct Fix64 : IEquatable<Fix64>, IComparable<Fix64> {
3333
const long MAX_VALUE = long.MaxValue;
3434
const long MIN_VALUE = long.MinValue;
3535
const int NUM_BITS = 64;
36-
const int DECIMAL_PLACES = 32;
37-
const long ONE = 1L << DECIMAL_PLACES;
36+
const int FRACTIONAL_PLACES = 32;
37+
const long ONE = 1L << FRACTIONAL_PLACES;
3838
const long PI_TIMES_2 = 0x6487ED511;
3939
const long PI = 0x3243F6A88;
4040
const long PI_OVER_2 = 0x1921FB544;
@@ -54,11 +54,11 @@ public static int Sign(Fix64 value) {
5454

5555
/// <summary>
5656
/// Returns the absolute value of a Fix64 number.
57-
/// If the number is equal to MinValue, throws an OverflowException.
57+
/// Note: Abs(Fix64.MinValue) == Fix64.MaxValue.
5858
/// </summary>
5959
public static Fix64 Abs(Fix64 value) {
6060
if (value.m_rawValue == MIN_VALUE) {
61-
throw new OverflowException("Cannot take the absolute value of the minimum value representable.");
61+
return MaxValue;
6262
}
6363

6464
// branchless implementation, see http://www.strchr.com/optimized_abs_function
@@ -81,29 +81,29 @@ public static Fix64 FastAbs(Fix64 value) {
8181
/// Returns the largest integer less than or equal to the specified number.
8282
/// </summary>
8383
public static Fix64 Floor(Fix64 value) {
84-
// Just zero out the decimal part
84+
// Just zero out the fractional part
8585
return new Fix64((long)((ulong)value.m_rawValue & 0xFFFFFFFF00000000));
8686
}
8787

8888
/// <summary>
8989
/// Returns the smallest integral value that is greater than or equal to the specified number.
9090
/// </summary>
9191
public static Fix64 Ceiling(Fix64 value) {
92-
var hasDecimalPart = (value.m_rawValue & 0x00000000FFFFFFFF) != 0;
93-
return hasDecimalPart ? Floor(value) + One : value;
92+
var hasFractionalPart = (value.m_rawValue & 0x00000000FFFFFFFF) != 0;
93+
return hasFractionalPart ? Floor(value) + One : value;
9494
}
9595

9696
/// <summary>
9797
/// Rounds a value to the nearest integral value.
9898
/// If the value is halfway between an even and an uneven value, returns the even value.
9999
/// </summary>
100100
public static Fix64 Round(Fix64 value) {
101-
var decimalPart = value.m_rawValue & 0x00000000FFFFFFFF;
101+
var fractionalPart = value.m_rawValue & 0x00000000FFFFFFFF;
102102
var integralPart = Floor(value);
103-
if (decimalPart < 0x80000000) {
103+
if (fractionalPart < 0x80000000) {
104104
return integralPart;
105105
}
106-
if (decimalPart > 0x80000000) {
106+
if (fractionalPart > 0x80000000) {
107107
return integralPart + One;
108108
}
109109
// if number is halfway between two values, round to the nearest even number
@@ -170,19 +170,19 @@ static long AddOverflowHelper(long x, long y, ref bool overflow) {
170170
var yl = y.m_rawValue;
171171

172172
var xlo = (ulong)(xl & 0x00000000FFFFFFFF);
173-
var xhi = xl >> DECIMAL_PLACES;
173+
var xhi = xl >> FRACTIONAL_PLACES;
174174
var ylo = (ulong)(yl & 0x00000000FFFFFFFF);
175-
var yhi = yl >> DECIMAL_PLACES;
175+
var yhi = yl >> FRACTIONAL_PLACES;
176176

177177
var lolo = xlo * ylo;
178178
var lohi = (long)xlo * yhi;
179179
var hilo = xhi * (long)ylo;
180180
var hihi = xhi * yhi;
181181

182-
var loResult = lolo >> DECIMAL_PLACES;
182+
var loResult = lolo >> FRACTIONAL_PLACES;
183183
var midResult1 = lohi;
184184
var midResult2 = hilo;
185-
var hiResult = hihi << DECIMAL_PLACES;
185+
var hiResult = hihi << FRACTIONAL_PLACES;
186186

187187
bool overflow = false;
188188
var sum = AddOverflowHelper((long)loResult, midResult1, ref overflow);
@@ -207,7 +207,7 @@ static long AddOverflowHelper(long x, long y, ref bool overflow) {
207207

208208
// if the top 32 bits of hihi (unused in the result) are neither all 0s or 1s,
209209
// then this means the result overflowed.
210-
var topCarry = hihi >> DECIMAL_PLACES;
210+
var topCarry = hihi >> FRACTIONAL_PLACES;
211211
if (topCarry != 0 && topCarry != -1 /*&& xl != -17 && yl != -17*/) {
212212
return opSignsEqual ? MaxValue : MinValue;
213213
}
@@ -242,19 +242,19 @@ public static Fix64 FastMul(Fix64 x, Fix64 y) {
242242
var yl = y.m_rawValue;
243243

244244
var xlo = (ulong)(xl & 0x00000000FFFFFFFF);
245-
var xhi = xl >> DECIMAL_PLACES;
245+
var xhi = xl >> FRACTIONAL_PLACES;
246246
var ylo = (ulong)(yl & 0x00000000FFFFFFFF);
247-
var yhi = yl >> DECIMAL_PLACES;
247+
var yhi = yl >> FRACTIONAL_PLACES;
248248

249249
var lolo = xlo * ylo;
250250
var lohi = (long)xlo * yhi;
251251
var hilo = xhi * (long)ylo;
252252
var hihi = xhi * yhi;
253253

254-
var loResult = lolo >> DECIMAL_PLACES;
254+
var loResult = lolo >> FRACTIONAL_PLACES;
255255
var midResult1 = lohi;
256256
var midResult2 = hilo;
257-
var hiResult = hihi << DECIMAL_PLACES;
257+
var hiResult = hihi << FRACTIONAL_PLACES;
258258

259259
var sum = (long)loResult + midResult1 + midResult2 + hiResult;
260260
return new Fix64(sum);
@@ -470,15 +470,17 @@ public static Fix64 FastSin(Fix64 x) {
470470
// Here we use the fact that the SinLut table has a number of entries
471471
// equal to (PI_OVER_2 >> 15) to use the angle to index directly into it
472472
var rawIndex = (uint)(clampedL >> 15);
473-
if (rawIndex == LUT_SIZE) {
474-
--rawIndex;
473+
if (rawIndex >= LUT_SIZE) {
474+
rawIndex = LUT_SIZE - 1;
475475
}
476476
var nearestValue = SinLut[flipHorizontal ?
477477
SinLut.Length - 1 - (int)rawIndex :
478478
(int)rawIndex];
479479
return new Fix64(flipVertical ? -nearestValue : nearestValue);
480480
}
481481

482+
483+
482484
[MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
483485
static long ClampSinValue(long angle, out bool flipHorizontal, out bool flipVertical) {
484486
// Clamp value to 0 - 2*PI using modulo; this is very slow but there's no better way AFAIK
@@ -552,13 +554,51 @@ public static Fix64 Tan(Fix64 x) {
552554
return new Fix64(finalValue);
553555
}
554556

557+
public static Fix64 Atan2(Fix64 y, Fix64 x) {
558+
var yl = y.m_rawValue;
559+
var xl = x.m_rawValue;
560+
if (xl == 0) {
561+
if (yl > 0) {
562+
return PiOver2;
563+
}
564+
if (yl == 0) {
565+
return Zero;
566+
}
567+
return -PiOver2;
568+
}
569+
Fix64 atan;
570+
var z = y / x;
571+
572+
// Deal with overflow
573+
if (One + (Fix64)0.28M * z * z == MaxValue) {
574+
return y < Zero ? -PiOver2 : PiOver2;
575+
}
576+
577+
if (Abs(z) < One) {
578+
atan = z / (One + (Fix64)0.28M * z * z);
579+
if (xl < 0) {
580+
if (yl < 0) {
581+
return atan - Pi;
582+
}
583+
return atan + Pi;
584+
}
585+
}
586+
else {
587+
atan = PiOver2 - z / (z * z + (Fix64)0.28M);
588+
if (yl < 0) {
589+
return atan - Pi;
590+
}
591+
}
592+
return atan;
593+
}
594+
555595

556596

557597
public static explicit operator Fix64(long value) {
558598
return new Fix64(value * ONE);
559599
}
560600
public static explicit operator long(Fix64 value) {
561-
return value.m_rawValue >> DECIMAL_PLACES;
601+
return value.m_rawValue >> FRACTIONAL_PLACES;
562602
}
563603
public static explicit operator Fix64(float value) {
564604
return new Fix64((long)(value * ONE));
@@ -661,8 +701,16 @@ partial struct Fix64 {
661701
/// </summary>
662702
public long RawValue { get { return m_rawValue; } }
663703

704+
/// <summary>
705+
/// This is the constructor from raw value; it can only be used interally.
706+
/// </summary>
707+
/// <param name="rawValue"></param>
664708
Fix64(long rawValue) {
665709
m_rawValue = rawValue;
666710
}
711+
712+
public Fix64(int value) {
713+
m_rawValue = value * ONE;
714+
}
667715
}
668716
}

‎Fix64Tests.cs‎

Lines changed: 85 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -221,7 +221,7 @@ public void Sign() {
221221

222222
[Test]
223223
public void Abs() {
224-
Assert.Throws<OverflowException>(() => Fix64.Abs(Fix64.MinValue));
224+
Assert.AreEqual(Fix64.MaxValue, Fix64.Abs(Fix64.MinValue));
225225
var sources = new[] { -1, 0, 1, int.MaxValue };
226226
var expecteds = new[] { 1, 0, 1, int.MaxValue };
227227
for (int i = 0; i < sources.Length; ++i) {
@@ -231,6 +231,18 @@ public void Abs() {
231231
}
232232
}
233233

234+
[Test]
235+
public void FastAbs() {
236+
Assert.AreEqual(Fix64.MinValue, Fix64.FastAbs(Fix64.MinValue));
237+
var sources = new[] { -1, 0, 1, int.MaxValue };
238+
var expecteds = new[] { 1, 0, 1, int.MaxValue };
239+
for (int i = 0; i < sources.Length; ++i) {
240+
var actual = Fix64.FastAbs((Fix64)sources[i]);
241+
var expected = (Fix64)expecteds[i];
242+
Assert.AreEqual(expected, actual);
243+
}
244+
}
245+
234246
[Test]
235247
public void Floor() {
236248
var sources = new[] { -5.1m, -1, 0, 1, 5.1m };
@@ -251,6 +263,8 @@ public void Ceiling() {
251263
var expected = expecteds[i];
252264
Assert.AreEqual(expected, actual);
253265
}
266+
267+
Assert.AreEqual(Fix64.MaxValue, Fix64.Ceiling(Fix64.MaxValue));
254268
}
255269

256270
[Test]
@@ -262,6 +276,7 @@ public void Round() {
262276
var expected = expecteds[i];
263277
Assert.AreEqual(expected, actual);
264278
}
279+
Assert.AreEqual(Fix64.MaxValue, Fix64.Round(Fix64.MaxValue));
265280
}
266281

267282

@@ -451,11 +466,8 @@ public void Tan() {
451466
var f = (Fix64)angle;
452467
var actualF = Fix64.Tan(f);
453468
var expected = (decimal)Math.Tan(angle);
454-
var delta = Math.Abs(expected - (decimal)actualF);
455-
var distanceToPeak = Math.Abs(Math.Abs(angle % (Math.PI / 2.0)) - (Math.PI / 2.0));
456-
if (distanceToPeak > 0.0001) {
457-
Assert.True((double)delta < 1 / (distanceToPeak * distanceToPeak), string.Format("Tan({0}): expected {1} but got {2}\ndelta = {3}", angle, expected, actualF, delta));
458-
}
469+
Assert.AreEqual(actualF > Fix64.Zero, expected > 0, string.Format("Signs differ for {0}", angle));
470+
//TODO figure out a real way to test this function
459471
}
460472

461473
//foreach (var val in m_testCases) {
@@ -467,6 +479,73 @@ public void Tan() {
467479
//}
468480
}
469481

482+
[Test]
483+
public void Atan2() {
484+
var deltas = new List<decimal>();
485+
// Identities
486+
Assert.AreEqual(Fix64.Atan2(Fix64.Zero, -Fix64.One), Fix64.Pi);
487+
Assert.AreEqual(Fix64.Atan2(Fix64.Zero, Fix64.Zero), Fix64.Zero);
488+
Assert.AreEqual(Fix64.Atan2(Fix64.Zero, Fix64.One), Fix64.Zero);
489+
Assert.AreEqual(Fix64.Atan2(Fix64.One, Fix64.Zero), Fix64.PiOver2);
490+
Assert.AreEqual(Fix64.Atan2(-Fix64.One, Fix64.Zero), -Fix64.PiOver2);
491+
492+
// Precision
493+
for (var y = -1.0; y < 1.0; y += 0.01) {
494+
for (var x = -1.0; x < 1.0; x += 0.01) {
495+
var yf = (Fix64)y;
496+
var xf = (Fix64)x;
497+
var actual = (decimal)Fix64.Atan2(yf, xf);
498+
var expected = (decimal)Math.Atan2((double)yf, (double)xf);
499+
var delta = Math.Abs(actual - expected);
500+
deltas.Add(delta);
501+
Assert.LessOrEqual(delta, 0.005, string.Format("Precision: Atan2({0}, {1}): expected {2} but got {3}", yf, xf, expected, actual));
502+
}
503+
}
504+
505+
// Scalability and edge cases
506+
foreach (var y in m_testCases) {
507+
foreach (var x in m_testCases) {
508+
var yf = (Fix64)y;
509+
var xf = (Fix64)x;
510+
var actual = (decimal)Fix64.Atan2(yf, xf);
511+
var expected = (decimal)Math.Atan2((double)yf, (double)xf);
512+
var delta = Math.Abs(actual - expected);
513+
deltas.Add(delta);
514+
Assert.LessOrEqual(delta, 0.005, string.Format("Scalability: Atan2({0}, {1}): expected {2} but got {3}", yf, xf, expected, actual));
515+
}
516+
}
517+
Console.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
518+
Console.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
519+
}
520+
521+
522+
[Test]
523+
public void Atan2Benchmark() {
524+
var deltas = new List<decimal>();
525+
526+
var swf = new Stopwatch();
527+
var swd = new Stopwatch();
528+
529+
foreach (var y in m_testCases) {
530+
foreach (var x in m_testCases) {
531+
for (int k = 0; k < 1000; ++k) {
532+
var yf = (Fix64)y;
533+
var xf = (Fix64)x;
534+
swf.Start();
535+
var actualF = Fix64.Atan2(yf, xf);
536+
swf.Stop();
537+
swd.Start();
538+
var expected = Math.Atan2((double)yf, (double)xf);
539+
swd.Stop();
540+
deltas.Add(Math.Abs((decimal)actualF - (decimal)expected));
541+
}
542+
}
543+
}
544+
Console.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
545+
Console.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
546+
Console.WriteLine("Fix64.Atan2 time = {0}ms, Math.Atan2 time = {1}ms", swf.ElapsedMilliseconds, swd.ElapsedMilliseconds);
547+
}
548+
470549
[Test]
471550
public void Negation() {
472551
foreach (var operand1 in m_testCases) {

0 commit comments

Comments
 (0)