4

5

6 extern float rxcopy[];

7 extern float GEstG[3];

8 extern float Q_rsqrt( float number );

9

10

11 float errorvect[3];

12

13 void stick_vector( float maxangle)

14 {

15

16 float stickvector[3];

17 float pitch, roll;

8 def Q_rsqrt(number):

9 float Q_rsqrt( float number )

10 {

11 long i;

12 float x2, y;

13 const float threehalfs = 1.5F;

15 x2 = number * 0.5F;

16 y = number;

17 i = * ( long * ) &y; // evil floating point bit level hacking

18 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

19 y = * ( float * ) &i;

23 }

24

25 print(Q_rsqrt(1.234))

26 ```

27 Inlinec supports gnu-specific c extensions, so you're likely to have reasonable success #includeing glibc headers.

59 }

60

61 const Position scale(const float & f) const

62 {

63 return Position((PositionType)(f * x()), (PositionType)(f * y()));

64 }

65

66 void scalePosition(const float & f)

67 {

68 _x = (PositionType)(f * _x);

120 }

121

122 const float Q_rsqrt( float number ) const

123 {

124 long i;

27 } Float2UInt;

28

29 static m_inline float floatRound(float x) {

30 // Probably slower than std::floor(), also depends of FPU settings,

31 // but we only need this for that special sin/cos() case anyways...

42 }

43

44 static m_inline float floatInvSqrt(float x) {

45 // Modified version of the emblematic Q_rsqrt() from Quake 3.

55 }

56

57 static m_inline float floatSin(float radians) {

58 static const float A = -2.39e-08f;

83 }

84

85 static m_inline float floatCos(float radians) {

86 static const float A = -2.605e-07f;

1 var cppExamples = {

2 "Q_rsqrt": `float Q_rsqrt(float number) {

3 long i;

4 float x2, y;

5 const float threehalfs = 1.5F;

6

7 x2 = number * 0.5F;

9 i = *(long *) &y;

10 i = 0x5f3759df - (i >> 1);

11 y = *(float *) &i;

12 y = y * (threehalfs - (x2 * y * y));

13 y = y * (threehalfs - (x2 * y * y));

54 }

55

56 const Position scale(const float & f) const

57 {

58 return Position((PositionType)(f * x()), (PositionType)(f * y()));

59 }

60

61 void scalePosition(const float & f)

62 {

63 _x = (PositionType)(f * _x);

115 }

116

117 const float Q_rsqrt( float number ) const

118 {

119 long i;

45 * int N = 10;

46 * unsigned int alignment = volk_get_alignment();

47 * float* in = (float*)volk_malloc(sizeof(float)*N, alignment);

48 * float* out = (float*)volk_malloc(sizeof(float)*N, alignment);

71 #include <string.h>

72

73 static inline float Q_rsqrt(float number)

74 {

75 float x2;

94

95 static inline void

96 volk_32f_invsqrt_32f_a_avx(float* cVector, const float* aVector, unsigned int num_points)

97 {

98 unsigned int number = 0;

195

196 static inline void

197 volk_32f_invsqrt_32f_u_avx(float* cVector, const float* aVector, unsigned int num_points)

198 {

199 unsigned int number = 0;

10 extern float GEstG[3];

11 extern float Q_rsqrt( float number );

12 extern char aux[];

13 extern float aux_analog[];

15 // error vector between stick position and quad orientation

16 // this is the output of this function

17 float errorvect[3];

18 // cache the last result so it does not get calculated everytime

19 float last_rx[2] = {13.13f , 12.12f};

20 float stickvector[3] = { 0 , 0 , 1};

21

22

23

24

25 void stick_vector( float rx_input[] , float maxangle)

26 {

27 // only compute stick rotation if values changed

73

74 // 解法三 Quake III 游戏引擎中有一种比 STL 的 sqrt 快 4 倍的实现 https://en.wikipedia.org/wiki/Fast_inverse_square_root

75 // float Q_rsqrt( float number )

76 // {

77 // long i;

78 // float x2, y;

79 // const float threehalfs = 1.5F;

81 // x2 = number * 0.5F;

82 // y = number;

83 // i = * ( long * ) &y; // evil floating point bit level hacking

84 // i = 0x5f3759df - ( i >> 1 ); // what the fuck?

85 // y = * ( float * ) &i;

25 }

26

27 float Length() const { return (float)VectorLength( vec ); }

28 float LengthFast() const { return VectorLengthFast( vec ); }

31 float DistanceTo( const Vec3 &that ) const { return DistanceTo( that.Data() ); }

32 float DistanceTo( const vec3_t that ) const { return (float)Distance( vec, that ); }

33 float FastDistanceTo( const Vec3 &that ) const { return FastDistanceTo( that.Data() ); }

41 float SquareDistance2DTo( const Vec3 &that ) const { return SquareDistanceTo( that.vec ); }

42 float SquareDistance2DTo( const vec3_t that ) const {

43 float dx = vec[0] - that[0];

59 float squareLength = VectorLengthSquared( vec );

60 if( squareLength > 0 ) {

61 float invLength = Q_RSqrt( squareLength );

62 VectorScale( vec, invLength, vec );

63 return squareLength * invLength;

94 // originally from quake3 code

95

96 float Q_rsqrt( float number )

97 {

98

116 void vectorcopy(float *vector1, float *vector2);

117 float atan2approx(float y, float x);

118

119

131

132

133 void vectorcopy(float *vector1, float *vector2)

134 {

135 for (int axis = 0; axis < 3; axis++)

281

282 // +-0.09 deg error

283 float atan2approx(float y, float x)

284 {

285

64 // originally from quake3 code

65

66 float Q_rsqrt( float number )

67 {

68

87

88

89 float atan2approx(float y, float x);

90

91 float calcmagnitude(float vector[3])

191

192 // +-0.09 deg error

193 float atan2approx(float y, float x)

194 {

195

9

10

11 // float Q_rsqrt( float number )

12 // {

13 // long i;

32

33

34 Vector2D(float _x = 0.0f, float _y = 0.0f )

35 {

36 m_x = _x;

211 FLOAT m_Left, m_Top, m_Bottom, m_Right;

212 Rect2D(float _left, float _top, float _right, float _bottom)

213 {

214 m_Left = _left;

243 Vector3D(): m_x(0), m_y(0), m_z(0) {}

244

245 Vector3D(float x1, float y1, float z1): m_x(x1), m_y(y1), m_z(z1) {}

246

247 Vector3D(const Vector3D& v): m_x(v.m_x), m_y(v.m_y), m_z(v.m_z) {}

78 // Magic Number 0x5f3759df !!! So Amazing

79 // 雷神之锤3 源码

80 float Q_rsqrt(float number) {

81 long i;

82 float x2, y;

83 const float threehalfs = 1.5F;

84

85 x2 = number * 0.5F;

86 y = number;

87 i = * ( long * ) &y; // evil floating point bit level hacking

88 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

89 y = * ( float * ) &i;

10

11 // original, as copied from the Quake 3 source, without useless preprocessor code

12 inline float Q_rsqrt(float number)

13 {

14 long i;

26 }

27

28 inline float Q_frsqrt(float number)

29 {

30 long i;

90

91 // Doom 3's rsqrt/invsqrt version

92 inline float D3InvSqrt(float x)

93 {

94

14 Particle* a;

15 Particle* b;

16 float k;

17 bool on;

18 float distanceMin;

19 float distanceMinSquared;

20

21 Attraction( Particle* a, Particle* b, float k, float distanceMin );

22

23 float getMinimumDistance();

45 private:

46 // http://en.wikipedia.org/wiki/Fast_inverse_square_root

47 float Q_rsqrt( float number );

48

49 };

72 }

73

74 Vector4f(float x, float y, float z, float w) : m_x(x), m_y(y), m_z(z), m_w(w)

75 {

76 }

158

159 // create based on standard parameters

160 Quaternion(float x, float y, float z, float w) : m_x(x), m_y(y), m_z(z), m_w(w)

161 {

162 }

201

202 // determine whether a point is in front of or behind a plane (based on its normal vector)

203 int PointPlanePos(float normalX, float normalY, float normalZ, float intercept, const Math::Vector3f &point);

204

205 void Translate(Matrix4f &matrix, float x, float y=0.0f, float z=0.0f);

37 sincos_ps..

38 */

39 void sincos_ps(float32x4_t x,float32x4_t *ysin,float32x4_t *ycos);

40

41 float32x4_t sin_ps(float32x4_t x);

52 float32x4_t inv_sqrt_float32x4( float32x4_t xx );

53

54 float32x4_t q_rsqrt_float32x4( float32x4_t xx);

55

56 float32x4_t atanf_cephes(float32x4_t xx);

57

58 float32x4_t atan2f_cephes(float32x4_t y,float32x4_t x);

59 }

60 #endif // __ARM_NEON__

9 extern float GEstG[3];

10 extern float Q_rsqrt( float number );

11 extern char aux[];

12

13 // error vector between stick position and quad orientation

14 // this is the output of this function

15 float errorvect[3];

16 // cache the last result so it does not get calculated everytime

17 float last_rx[2] = {13.13f , 12.12f};

18 float stickvector[3] = { 0 , 0 , 1};

19

20

21

22

23 void stick_vector( float rx_input[] , float maxangle)

24 {

25 // only compute stick rotation if values changed

24 \end{itemize}

25 \begin{lstlisting}

26 float Q_rsqrt( float number)

27 {

28 int i;

29 float x2, y;

30 const float threehalfs = 1.5F;

31 x2 = number * 0.5F;

32 y = number;

33 i = * ( int * ) &y; // evil floating point bit level hacking

34 i = 0x5f3759df - ( i >> 1 ); // what the f***?

35 y = * ( float * ) &i;

7 #include <string.h>

8

9 static inline float Q_rsqrt( float number )

10 {

11 float x2;

99 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

100 */

101 static inline void volk_32f_invsqrt_32f_neon(float* cVector, const float* aVector, unsigned int num_points){

102 unsigned int number;

103 const unsigned int quarter_points = num_points / 4;

129 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

130 */

131 static inline void volk_32f_invsqrt_32f_generic(float* cVector, const float* aVector, unsigned int num_points){

132 float* cPtr = cVector;

147 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

148 */

149 static inline void volk_32f_invsqrt_32f_u_avx(float* cVector, const float* aVector, unsigned int num_points){

150 unsigned int number = 0;

151 const unsigned int eighthPoints = num_points / 8;

7 namespace traer { namespace physics {

8

9 Attraction::Attraction( Particle* _a, Particle* _b, float _k, float _distanceMin )

10 {

11 a = _a;

74 //a2b *= force;

75

76 a2b *= Q_rsqrt(a2bDistanceSquared) * force;

77

78 // apply

100 }

101

102 float Attraction::Q_rsqrt( float number )

103 {

104 long i;

19 float exp_single(float x);

20

21 float sinf_single(float xx);

22

23 float cosf_single(float xx);

31 float atanf_single(float xx);

32

33 float atan2f_single(float y,float x);

34

35 float exp_remez_single(float x);

37 float inv_sqrt_single(float xx);

38

39 float q_rsqrt_single(float number);

40

41

46 * ----------------------------------------------

47 */

48 #define SQRTFAST(x) ((x) * Q_rsqrt(x))

49

50

54 */

55

56 float Q_rsqrt(float number);

57 inline float u8sin(unsigned char v);

58 inline float u8cos(unsigned char v);

59 inline float u8sinf(float v);

60 inline float u8cosf(float v);

7 #include <string.h>

8

9 static inline float Q_rsqrt( float number )

10 {

11 float x2;

64 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

65 */

66 static inline void volk_32f_invsqrt_32f_a_sse(float* cVector, const float* aVector, unsigned int num_points){

67 unsigned int number = 0;

68 const unsigned int quarterPoints = num_points / 4;

116 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

117 */

118 static inline void volk_32f_invsqrt_32f_u_avx(float* cVector, const float* aVector, unsigned int num_points){

119 unsigned int number = 0;

120 const unsigned int eighthPoints = num_points / 8;

6 #include <math.h>

7

8 static inline float Q_rsqrt( float number )

9 {

10 long i;

14 x2 = number * 0.5F;

15 y = number;

16 i = * ( long * ) &y; // evil floating point bit level hacking

17 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

18 y = * ( float * ) &i;

31 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

32 */

33 static inline void volk_32f_invsqrt_32f_a_sse(float* cVector, const float* aVector, unsigned int num_points){

34 unsigned int number = 0;

35 const unsigned int quarterPoints = num_points / 4;

65 \param num_points The number of values in aVector and bVector to be invsqrted together and stored into cVector

66 */

67 static inline void volk_32f_invsqrt_32f_generic(float* cVector, const float* aVector, unsigned int num_points){

68 float* cPtr = cVector;

9 extern float GEstG[3];

10 extern float Q_rsqrt( float number );

11

12 // error vector between stick position and quad orientation

13 // this is the output of this function

14 float errorvect[3];

15 // cache the last result so it does not get calculated everytime

16 float last_rx[2] = {13.13f , 12.12f};

17 float stickvector[3] = { 0 , 0 , 1};

18

19

20

21

22 void stick_vector( float rx_input[] , float maxangle)

23 {

24 // only compute stick rotation if values changed

447 }

448

449 unsigned ColorBytes4 (float r, float g, float b, float a) {

450 unsigned i;

451

718 #if !idppc

719 /*

720 ** float q_rsqrt( float number )

721 */

722 float Q_rsqrt( float number )

757 ===============

758 */

759 float LerpAngle (float from, float to, float frac) {

760 float a;

841 =================

842 */

843 float AngleDelta ( float angle1, float angle2 ) {

844 return AngleNormalize180( angle1 - angle2 );

845 }

148 static int BitReverse( int x ); -- returns the bit reverse of x

149 static int Abs( int x ); -- returns the absolute value of the integer value (for reference only)

150 static float Fabs( float f ); -- returns the absolute value of the floating point value

151 static float Floor( float f ); -- returns the largest integer that is less than or equal to the given value

161 static signed short ClampShort( int i );

162 static int ClampInt( int min, int max, int value );

163 static float ClampFloat( float min, float max, float value );

164 static float AngleNormalize360( float angle );

165 static float AngleNormalize180( float angle );

166 static float AngleDelta( float angle1, float angle2 );

167 static int FloatToBits( float f, int exponentBits, int mantissaBits );

169 static int FloatHash( const float *array, const int numFloats );

170 static float LerpToWithScale( const float cur, const float dest, const float scale );

171 -------

172 private

1 #ifndef q_rsqrt_H

2 #define q_rsqrt_H

4 //wiki http://en.wikipedia.org/wiki/Fast_inverse_square_root

5

6 static float q_rsqrt(float number) {

7 long i;

8 float x2, y;

9 const float threehalfs = 1.5F;

10

11 x2 = number * 0.5F;

12 y = number;

13 i = *(long *)&y;// evil floating point bit level hacking（对浮点数的邪恶位级hack）

14 i = 0x5f3759df - (i >> 1);// what the fuck?（这他妈的是怎么回事？）

15 y = *(float *)&i;

7 #include <getopt.h>

8

9 float Q_rsqrt( float number );

10

11 uint64_t r;

57 fBegin = (float)atof(optarg);

58 else if(option_index == arg_fStep)

59 fStep = (float)atof(optarg);

60 else if(option_index == arg_fEnd)

61 fEnd = (float)atof(optarg);

143 ** float q_rsqrt( float number )

144 */

145 float Q_rsqrt( float number ) {

146 long i;

147 float x2, y;

98 }

99

100 inline Vec3d RotateX(float a)

101 {

102 float ca=cos(a), sa=sin(a);

103 float zn=z*ca+y*sa;

104 float yn=y*ca-z*sa;

106 }

107

108 inline Vec3d RotateY(float a)

109 {

110 float ca=cos(a), sa=sin(a);

111 float xn=x*ca+z*sa;

112 float zn=z*ca-x*sa;

1 // The following code is the fast inverse square root implementation from Quake III Arena, stripped of C preprocessor directives, but including the exact original comment text:[9]

2

3 float Q_rsqrt( float number )

4 {

5 long i;

6 float x2, y;

7 const float threehalfs = 1.5F;

9 x2 = number * 0.5F;

10 y = number;

11 i = * ( long * ) &y; // evil floating point bit level hacking

12 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

13 y = * ( float * ) &i;

146 long FixedValue( LPRO object );

147 LPRO LPROFromFixed( LPRDATA rdPtr, long fixedvalue );

148 long returnFloat(LPRDATA rdPtr, float value);

149

150 /*

154 void DebugObject(Boid boid);

155 */

156 float RotateTowards(float angle, float targetAngle, float speed);

157 float ToRadians(float degrees);

158 float ToDegrees(float radians);

159

160 int packFloat(float value);

161 void LoopNeighbourBoids(LPRDATA rdPtr, char* loopname, Boid boid, int radius);

162

164 float clamp(float val, float min, float max);

165

166 float Q_rsqrt( float number );

2 * PURPOSE: IEEE sqrt trickery

3 */

4 float Q_rsqrt( float number ) {

5 long i;

6 float x2, y;

7 const float threehalfs = 1.5F;

8

9 x2 = number * 0.5F;

10 y = number;

11 i = * ( long * ) &y; // evil floating point bit level hacking

12 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

13 y = * ( float * ) &i;

1 // Calculates approx. for: 1/(sqrt(number))

2 // 4x speed of normal floating point division

3 // Includes original comments

4 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

5 float Q_rsqrt(float number);

1 float Q_rsqrt( float number )

2 {

3 long i;

4 float x2, y;

5 const float threehalfs = 1.5F;

7 x2 = number * 0.5F;

8 y = number;

9 i = * ( long * ) &y; // evil floating point bit level hacking

10 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

11 y = * ( float * ) &i;

1 // __expected:Q_rsqrt(number => $_retval)

2 float Q_rsqrt( float number )

3 {

4 long i;

5 float x2, y;

6 const float threehalfs = 1.5F;

8 x2 = number * 0.5F;

9 y = number;

10 i = * ( long * ) &y; // evil floating point bit level hacking

11 i = 0x5f3759df - ( i >> 1 ); // what the fuck?

12 y = * ( float * ) &i;