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 package ffx.numerics.func1d;
39
40 import static ffx.numerics.math.ScalarMath.modToRange;
41 import static java.lang.String.format;
42 import static org.apache.commons.math3.util.FastMath.PI;
43 import static org.apache.commons.math3.util.FastMath.cos;
44 import static org.apache.commons.math3.util.FastMath.sin;
45
46
47
48
49
50
51
52
53 public class QuasiLinearThetaMap implements UnivariateDiffFunction {
54
55 private final double theta0;
56 private final double r;
57 private final double a;
58 private final double b;
59 private final double c;
60 private final double piMinusTheta0;
61 private final double negPiPlusTheta0;
62 private final double halfR;
63
64
65 public QuasiLinearThetaMap() {
66 this(0.1);
67 }
68
69
70
71
72
73
74
75
76
77 public QuasiLinearThetaMap(double theta0) {
78 if (theta0 <= 0 || theta0 >= PI) {
79 throw new IllegalArgumentException(
80 format(" QuasiLinearThetaMap must receive theta0 from (0 to +pi), received %11.5g",
81 theta0));
82 }
83
84 double sinT = sin(theta0);
85 double cosT = cos(theta0);
86 r = 1.0 / (1 - cosT + (0.5 * sinT * (PI - 2 * theta0)));
87
88 this.theta0 = theta0;
89 piMinusTheta0 = PI - theta0;
90 negPiPlusTheta0 = 0.1 - PI;
91
92 b = r * 0.5 * (1 - cosT - (theta0 * sinT));
93 halfR = 0.5 * r;
94 a = r * sinT * 0.5;
95 double temp = sin(piMinusTheta0 * 0.5);
96 c = (r * 0.5 * sinT * piMinusTheta0) + b - (r * (temp * temp));
97 }
98
99 @Override
100 public double firstDerivative(double x) throws IllegalArgumentException {
101 return fd(modToRange(x, -PI, PI));
102 }
103
104 @Override
105 public double nthDerivative(double x, int order) throws IllegalArgumentException {
106 x = modToRange(x, -PI, PI);
107 return switch (order) {
108 case 0 -> val(x);
109 case 1 -> fd(x);
110 case 2 -> sd(x);
111 default -> nd(x, order);
112 };
113 }
114
115 @Override
116 public double secondDerivative(double x) throws IllegalArgumentException {
117 return sd(modToRange(x, -PI, PI));
118 }
119
120 @Override
121 public double valueAt(double x) throws IllegalArgumentException {
122 return val(modToRange(x, -PI, PI));
123 }
124
125 final double[] getConstants() {
126 return new double[] {r, a, b, c};
127 }
128
129 Branch getBranch(double x) {
130 assert x >= -1.0 * PI && x <= PI;
131 if (x < negPiPlusTheta0) {
132 return Branch.D;
133 } else if (x < -1.0 * theta0) {
134 return Branch.C;
135 } else if (x <= theta0) {
136 return Branch.A;
137 } else if (x < piMinusTheta0) {
138 return Branch.B;
139 } else {
140 return Branch.D;
141 }
142 }
143
144 private double val(double x) throws IllegalArgumentException {
145 return val(x, getBranch(x));
146 }
147
148 double val(double x, Branch branch) throws IllegalArgumentException {
149 switch (branch) {
150 case A -> {
151 double sinT = sin(x * 0.5);
152 return r * sinT * sinT;
153 }
154 case B -> {
155 return b + a * x;
156 }
157 case C -> {
158 return b - a * x;
159 }
160 case D -> {
161 double sinT = sin(x * 0.5);
162 return (r * sinT * sinT) + c;
163 }
164 default ->
165 throw new IllegalArgumentException("Could not pick a branch! Should be impossible!");
166 }
167 }
168
169 private double fd(double x) throws IllegalArgumentException {
170 return fd(x, getBranch(x));
171 }
172
173 double fd(double x, Branch branch) throws IllegalArgumentException {
174 switch (branch) {
175 case A, D -> {
176 return halfR * sin(x);
177 }
178 case B -> {
179 return a;
180 }
181 case C -> {
182 return -1.0 * a;
183 }
184 default ->
185 throw new IllegalArgumentException("Could not pick a branch! Should be impossible!");
186 }
187 }
188
189 private double sd(double x) throws IllegalArgumentException {
190 return sd(x, getBranch(x));
191 }
192
193 double sd(double x, Branch branch) throws IllegalArgumentException {
194 switch (branch) {
195 case A, D -> {
196 return halfR * cos(x);
197 }
198 case B, C -> {
199 return 0;
200 }
201 default ->
202 throw new IllegalArgumentException("Could not pick a branch! Should be impossible!");
203 }
204 }
205
206 private double nd(double x, int order) throws IllegalArgumentException {
207 Branch br = getBranch(x);
208 if (order < 3) {
209 throw new IllegalArgumentException(" Order was " + order + ", must be > 2!");
210 }
211 switch (br) {
212 case B, C -> {
213 return 0;
214 }
215 }
216
217 switch (order % 4) {
218 case 0 -> {
219 return halfR * sin(x);
220 }
221 case 1 -> {
222 return halfR * cos(x);
223 }
224 case 2 -> {
225 return -1.0 * halfR * sin(x);
226 }
227 case 3 -> {
228 return -1.0 * halfR * cos(x);
229 }
230 default -> throw new ArithmeticException(format(" Value %d modulo 4 somehow not 0-3!", order));
231 }
232 }
233
234 enum Branch {
235 A, B, C, D
236 }
237 }