FixedPoint fast_sqrt() const { if (Data <= 0) return 0; int ld = (31-__builtin_clz(Data)) - FracBits; if (ld >= 0) { if (ld >= ((31-FracBits)/2)-1) { unsigned long long sq = (((unsigned long long)Data)*Data); unsigned int a = (6ULL << (ld+FracBits)) ; unsigned long long b = (1ULL << ((ld+FracBits)<<1)); unsigned int c = (1ULL << (ld>>1)); unsigned long long d = (1ULL << (((ld*6)>>2)+FracBits)); if (ld&1) { c *= 23; c >>= 4; d *= 22; d >>= 4; } FixedPoint x; x.Data = div_64_64u(mac_u(a, Data, sq+b), mac_u(c, Data, d)); x >>= 2; return x; } else { FixedPoint sq = (*this)*(*this); unsigned int a = (6U << (ld)); unsigned int b = (1U << (ld<<1)); FixedPoint c = (1U << (ld>>1)); FixedPoint d = (1U << ((ld*6)>>2)); if (ld&1) { c *= 1.4142136; d *= 1.4142136; } FixedPoint x = ( sq + a*(*this) + b) / (FixedPoint(c * (*this)) + d); x >>= 2; return x; } } else { ld = Abs(ld); FixedPoint sq = (*this)*(*this); FixedPoint a = 6; a >>= ld; FixedPoint b = 1; b >>= ld<<1; FixedPoint c = 1; c >>= ld>>1; FixedPoint d = 1; d >>= (ld*6)>>2; if (ld&1) { c *= 0.70710578; d *= 0.70710578; } FixedPoint x = ( sq + a*(*this) + b) / (FixedPoint(c * (*this)) + d); x >>= 2; return x; } }