1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora
13 // =============================================================================
14
15 #include "chrono/core/ChMathematics.h"
16
17 namespace chrono {
18
19 // ANGLE UNITS CONVERSIONS
20
ChAtan2(double mcos,double msin)21 double ChAtan2(double mcos, double msin) {
22 double ret;
23 if (fabs(mcos) < 0.707) {
24 ret = acos(mcos);
25 if (msin < 0.0)
26 ret = -ret;
27 } else {
28 ret = asin(msin);
29 if (mcos < 0.0)
30 ret = CH_C_PI - ret;
31 }
32 return ret;
33 }
34
35 // OTHER
36
37 // Park-Miller hi-quality random generator
38
39 #define IA 16807
40 #define IM 2147483647
41 #define AM (1.0 / IM)
42 #define IQ 127773
43 #define IR 2836
44 #define MASK 123459876
45
46 static long CH_PAseed = 123;
47
ChSetRandomSeed(long newseed)48 void ChSetRandomSeed(long newseed) {
49 if (CH_PAseed)
50 CH_PAseed = newseed;
51 }
52
ChRandom()53 double ChRandom() {
54 long k;
55 double ans;
56 CH_PAseed ^= MASK;
57 k = (CH_PAseed) / IQ;
58 CH_PAseed = IA * (CH_PAseed - k * IQ) - IR * k;
59 if (CH_PAseed < 0)
60 CH_PAseed += IM;
61 ans = AM * (CH_PAseed);
62 CH_PAseed ^= MASK;
63 return ans;
64 }
65
ChPeriodicPar(double & u,int closed)66 void ChPeriodicPar(double& u, int closed) {
67 if (u < 0) {
68 if (closed)
69 u = u + 1;
70 else
71 u = 0;
72 }
73 if (u > 1) {
74 if (closed)
75 u = u - 1;
76 else
77 u = 1;
78 }
79 }
80
ChNoise(double x,double amp,double freq,int octaves,double amp_ratio)81 double ChNoise(double x, double amp, double freq, int octaves, double amp_ratio) {
82 double ret = 0;
83 long oldseed = CH_PAseed;
84 double o_freq, o_amp, xA, xB, yA, yB, period;
85 int iA, iB;
86
87 o_freq = freq;
88 o_amp = amp;
89
90 for (int i = 1; i <= octaves; i++) {
91 period = 1.0 / o_freq;
92 xA = period * floor(x / period);
93 xB = xA + period;
94 iA = int(floor(x / period));
95 iB = iA + 1;
96 ChSetRandomSeed((long)(iA + 12345));
97 ChRandom();
98 ChRandom();
99 ChRandom(); // just to puzzle the seed..
100 yA = (ChRandom() - 0.5) * o_amp;
101 ChSetRandomSeed((long)(iB + 12345));
102 ChRandom();
103 ChRandom();
104 ChRandom(); // just to puzzle the seed..
105 yB = (ChRandom() - 0.5) * o_amp;
106 // cubic noise interpolation from (xA,yA) to (xB,yB), with flat extremal derivatives
107 ret += yA + (yB - yA) * ((3 * (pow(((x - xA) / (xB - xA)), 2))) - 2 * (pow(((x - xA) / (xB - xA)), 3)));
108 // for following octave, reduce amplitude...
109 o_amp *= amp_ratio;
110 o_freq *= 2.0;
111 }
112 // restore previous seed
113 CH_PAseed = oldseed;
114 return ret;
115 }
116
ChSineStep(double x,double x1,double y1,double x2,double y2)117 double ChSineStep(double x, double x1, double y1, double x2, double y2) {
118 if (x <= x1)
119 return y1;
120 if (x >= x2)
121 return y2;
122 double dx = x2 - x1;
123 double dy = y2 - y1;
124 double y = y1 + dy * (x - x1) / dx - (dy / CH_C_2PI) * std::sin(CH_C_2PI * (x - x1) / dx);
125 return y;
126 }
127
128 } // end namespace chrono
129