forked from erhs-53-hackers/arduibot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Math.cpp
195 lines (163 loc) · 6.5 KB
/
Math.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
/******************************************************************************/
/* randn()
*
* Normally (Gaussian) distributed random numbers, using the Box-Muller
* transformation. This transformation takes two uniformly distributed deviates
* within the unit circle, and transforms them into two independently
* distributed normal deviates. Utilizes the internal rand() function; this can
* easily be changed to use a better and faster RNG.
*
* The parameters passed to the function are the mean and standard deviation of
* the desired distribution. The default values used, when no arguments are
* passed, are 0 and 1 - the standard normal distribution.
*
*
* Two functions are provided:
*
* The first uses the so-called polar version of the B-M transformation, using
* multiple calls to a uniform RNG to ensure the initial deviates are within the
* unit circle. This avoids making any costly trigonometric function calls.
*
* The second makes only a single set of calls to the RNG, and calculates a
* position within the unit circle with two trigonometric function calls.
*
* The polar version is generally superior in terms of speed; however, on some
* systems, the optimization of the math libraries may result in better
* performance of the second. Try it out on the target system to see which
* works best for you. On my test machine (Athlon 3800+), the non-trig version
* runs at about 3x10^6 calls/s; while the trig version runs at about
* 1.8x10^6 calls/s (-O2 optimization).
*
*
* Example calls:
* randn_notrig(); //returns normal deviate with mean=0.0, std. deviation=1.0
* randn_notrig(5.2,3.0); //returns deviate with mean=5.2, std. deviation=3.0
*
*
* Dependencies - requires <cmath> for the sqrt(), sin(), and cos() calls, and a
* #defined value for PI.
*/
#include "Math.h"
#include <stdlib.h>
/******************************************************************************/
// "Polar" version without trigonometric calls
double randn_notrig(double mu=0.0, double sigma=1.0) {
static bool deviateAvailable=false; // flag
static float storedDeviate; // deviate from previous calculation
double polar, rsquared, var1, var2;
// If no deviate has been stored, the polar Box-Muller transformation is
// performed, producing two independent normally-distributed random
// deviates. One is stored for the next round, and one is returned.
if (!deviateAvailable) {
// choose pairs of uniformly distributed deviates, discarding those
// that don't fall within the unit circle
do {
var1=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
var2=2.0*( double(rand())/double(RAND_MAX) ) - 1.0;
rsquared=var1*var1+var2*var2;
} while ( rsquared>=1.0 || rsquared == 0.0);
// calculate polar tranformation for each deviate
polar=sqrt(-2.0*log(rsquared)/rsquared);
// store first deviate and set flag
storedDeviate=var1*polar;
deviateAvailable=true;
// return second deviate
return var2*polar*sigma + mu;
}
// If a deviate is available from a previous call to this function, it is
// returned, and the flag is set to false.
else {
deviateAvailable=false;
return storedDeviate*sigma + mu;
}
}
/******************************************************************************/
// Standard version with trigonometric calls
double randn_trig(double mu=0.0, double sigma=1.0) {
static bool deviateAvailable=false; // flag
static float storedDeviate; // deviate from previous calculation
double dist, angle;
// If no deviate has been stored, the standard Box-Muller transformation is
// performed, producing two independent normally-distributed random
// deviates. One is stored for the next round, and one is returned.
if (!deviateAvailable) {
// choose a pair of uniformly distributed deviates, one for the
// distance and one for the angle, and perform transformations
dist=sqrt( -2.0 * log(double(rand()) / double(RAND_MAX)) );
angle=2.0 * PI * (double(rand()) / double(RAND_MAX));
// calculate and store first deviate and set flag
storedDeviate=dist*cos(angle);
deviateAvailable=true;
// calcaulate return second deviate
return dist * sin(angle) * sigma + mu;
}
// If a deviate is available from a previous call to this function, it is
// returned, and the flag is set to false.
else {
deviateAvailable=false;
return storedDeviate*sigma + mu;
}
}
bool lineSegmentIntersection(
double Ax, double Ay,
double Bx, double By,
double Cx, double Cy,
double Dx, double Dy,
double *X, double *Y) {
double distAB, theCos, theSin, newX, ABpos ;
// Fail if either line segment is zero-length.
if (Ax==Bx && Ay==By || Cx==Dx && Cy==Dy) return false;
// Fail if the segments share an end-point.
if (Ax==Cx && Ay==Cy || Bx==Cx && By==Cy
|| Ax==Dx && Ay==Dy || Bx==Dx && By==Dy) {
return false;
}
// (1) Translate the system so that point A is on the origin.
Bx-=Ax;
By-=Ay;
Cx-=Ax;
Cy-=Ay;
Dx-=Ax;
Dy-=Ay;
// Discover the length of segment A-B.
distAB=sqrt(Bx*Bx+By*By);
// (2) Rotate the system so that point B is on the positive X axis.
theCos=Bx/distAB;
theSin=By/distAB;
newX=Cx*theCos+Cy*theSin;
Cy =Cy*theCos-Cx*theSin;
Cx=newX;
newX=Dx*theCos+Dy*theSin;
Dy =Dy*theCos-Dx*theSin;
Dx=newX;
// Fail if segment C-D doesn't cross line A-B.
if (Cy<0. && Dy<0. || Cy>=0. && Dy>=0.) return false;
// (3) Discover the position of the intersection point along line A-B.
ABpos=Dx+(Cx-Dx)*Dy/(Dy-Cy);
// Fail if segment C-D crosses line A-B outside of segment A-B.
if (ABpos<0. || ABpos>distAB) return false;
// (4) Apply the discovered position to line A-B in the original coordinate system.
*X=Ax+ABpos*theCos;
*Y=Ay+ABpos*theSin;
// Success.
return true;
}
double nextDouble() {
return 0.5;
//return TrueRandom.rand() / (32767.0 + 1.0);
}
double distance(double x1, double y1, double x2, double y2) {
return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
}
double Gaussian(double mu, double sigma, double x) {
return exp(-pow(mu - x, 2) / pow(sigma, 2) / 2.0 ) / sqrt(2.0 * PI * pow(sigma, 2));
}
double circle(double num, double length) {
while (num > length - 1.0) {
num -= length;
}
while (num < 0.0) {
num += length;
}
return num;
}