Skip to content

Commit 9603185

Browse files
RealAntEngineerRealAntEngineer
committed
update multiblock API
1 parent b5de7e9 commit 9603185

File tree

2 files changed

+178
-47
lines changed

2 files changed

+178
-47
lines changed

‎src/main/java/com/rae/formicapi/math/data/TwoDTabulatedFunction.java‎

Lines changed: 73 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -74,69 +74,95 @@ public float evaluate(float xInput, float yInput) {
7474
throw new IllegalStateException("Function table is empty");
7575
}
7676

77-
float xIndex = (float) (xMode.forward.applyAsDouble(xInput) / xStep);
78-
int xLowerIndex = (int) Math.floor(xIndex);
79-
float xFrac = xIndex - xLowerIndex;
80-
81-
float X1 = (float) xMode.inverse.applyAsDouble(xLowerIndex * xStep);
82-
float X2 = (float) xMode.inverse.applyAsDouble((xLowerIndex + 1) * xStep);
83-
84-
TreeMap<Float, Float> row1 = table.get(X1);
85-
TreeMap<Float, Float> row2 = table.get(X2);
86-
87-
if (row1 == null || row2 == null) {
88-
if (clamp) {
89-
Map.Entry<Float, TreeMap<Float, Float>> nearest = table.floorEntry(xInput);
90-
if (nearest == null) nearest = table.ceilingEntry(xInput);
91-
if (nearest == null) return table.firstEntry().getValue().firstEntry().getValue();
92-
return evaluate1D(yInput, nearest.getValue());
93-
} else {
94-
return extrapolateZ(xInput, yInput);
95-
}
96-
}
77+
// Get nearest X bounds
78+
Map.Entry<Float, TreeMap<Float, Float>> lowerX = table.floorEntry(xInput);
79+
Map.Entry<Float, TreeMap<Float, Float>> upperX = table.ceilingEntry(xInput);
9780

98-
float v1 = evaluate1D(yInput, row1);
99-
float v2 = evaluate1D(yInput, row2);
81+
if (lowerX == null && upperX == null) {
82+
throw new IllegalStateException("No data in table at all");
83+
}
84+
if (lowerX == null) {
85+
return clamp ? evaluate1D(yInput, upperX.getValue())
86+
: extrapolateZ(xInput, yInput);
87+
}
88+
if (upperX == null) {
89+
return clamp ? evaluate1D(yInput, lowerX.getValue())
90+
: extrapolateZ(xInput, yInput);
91+
}
92+
if (lowerX.getKey().equals(upperX.getKey())) {
93+
return evaluate1D(yInput, lowerX.getValue());
94+
}
10095

101-
return v1 * (1 - xFrac) + v2 * xFrac;
96+
// Interpolate across X
97+
float x1 = lowerX.getKey();
98+
float x2 = upperX.getKey();
99+
float v1 = evaluate1D(yInput, lowerX.getValue());
100+
float v2 = evaluate1D(yInput, upperX.getValue());
101+
float t = (xInput - x1) / (x2 - x1);
102+
return v1 * (1 - t) + v2 * t;
102103
}
103104

104105
private float evaluate1D(float yInput, TreeMap<Float, Float> row) {
105106
if (row.isEmpty()) {
106107
throw new IllegalStateException("Row table is empty");
107108
}
108109

109-
float yIndex = (float) (yMode.forward.applyAsDouble(yInput) / yStep);
110-
int yLowerIndex = (int) Math.floor(yIndex);
111-
float yFrac = yIndex - yLowerIndex;
112-
113-
float Y1 = (float) yMode.inverse.applyAsDouble(yLowerIndex * yStep);
114-
float Y2 = (float) yMode.inverse.applyAsDouble((yLowerIndex + 1) * yStep);
110+
// Find nearest Y bounds
111+
Map.Entry<Float, Float> lowerY = row.floorEntry(yInput);
112+
Map.Entry<Float, Float> upperY = row.ceilingEntry(yInput);
115113

116-
Float Z1 = row.get(Y1);
117-
Float Z2 = row.get(Y2);
118-
119-
if (Z1 == null || Z2 == null) {
120-
Map.Entry<Float, Float> lower = row.floorEntry(yInput);
121-
Map.Entry<Float, Float> upper = row.ceilingEntry(yInput);
114+
if (lowerY == null && upperY == null) {
115+
throw new IllegalStateException("No data in row at all");
116+
}
117+
if (lowerY == null) {
118+
return clamp ? upperY.getValue() : extrapolateY(yInput, row);
119+
}
120+
if (upperY == null) {
121+
return clamp ? lowerY.getValue() : extrapolateY(yInput, row);
122+
}
123+
if (lowerY.getKey().equals(upperY.getKey())) {
124+
return lowerY.getValue();
125+
}
122126

123-
if (lower == null || upper == null) {
124-
return row.firstEntry().getValue();
125-
}
127+
// Interpolate across Y
128+
float y1 = lowerY.getKey();
129+
float y2 = upperY.getKey();
130+
float v1 = lowerY.getValue();
131+
float v2 = upperY.getValue();
132+
float t = (yInput - y1) / (y2 - y1);
133+
return v1 * (1 - t) + v2 * t;
134+
}
126135

127-
float T_lower = lower.getKey();
128-
float T_upper = upper.getKey();
129-
if (T_lower == T_upper) {
130-
return lower.getValue();
131-
}
132-
float fracAlt = (yInput - T_lower) / (T_upper - T_lower);
136+
private float extrapolateY(float yInput, TreeMap<Float, Float> row) {
137+
Map.Entry<Float, Float> lower = row.floorEntry(yInput);
138+
Map.Entry<Float, Float> upper = row.ceilingEntry(yInput);
133139

134-
return lower.getValue() * (1 - fracAlt) + upper.getValue() * fracAlt;
140+
if (lower == null) {
141+
// extrapolate below using first two points
142+
Map.Entry<Float, Float> first = row.firstEntry();
143+
Map.Entry<Float, Float> next = row.higherEntry(first.getKey());
144+
if (next == null) return first.getValue();
145+
return linear(yInput, first, next);
135146
}
136-
137-
return Z1 * (1 - yFrac) + Z2 * yFrac;
147+
if (upper == null) {
148+
// extrapolate above using last two points
149+
Map.Entry<Float, Float> last = row.lastEntry();
150+
Map.Entry<Float, Float> prev = row.lowerEntry(last.getKey());
151+
if (prev == null) return last.getValue();
152+
return linear(yInput, prev, last);
153+
}
154+
// Already handled in evaluate1D, should not reach here
155+
return lower.getValue();
138156
}
139157

158+
private float linear(float query, Map.Entry<Float, Float> a, Map.Entry<Float, Float> b) {
159+
float x1 = a.getKey();
160+
float x2 = b.getKey();
161+
float y1 = a.getValue();
162+
float y2 = b.getValue();
163+
float t = (query - x1) / (x2 - x1);
164+
return y1 * (1 - t) + y2 * t;
165+
}
140166
private float extrapolateZ(float xInput, float yInput) {
141167
Map.Entry<Float, TreeMap<Float, Float>> lower = table.floorEntry(xInput);
142168
Map.Entry<Float, TreeMap<Float, Float>> upper = table.ceilingEntry(xInput);

‎src/test/java/com/rae/formicapi/TwoDTabulatedFunctionTest.java‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
package com.rae.formicapi;
2+
3+
import com.rae.formicapi.math.data.StepMode;
4+
import com.rae.formicapi.math.data.TwoDTabulatedFunction;
5+
import org.junit.jupiter.api.Test;
6+
7+
import java.util.Random;
8+
import java.util.TreeMap;
9+
10+
import static org.junit.jupiter.api.Assertions.*;
11+
12+
class TwoDTabulatedFunctionTest {
13+
14+
private TwoDTabulatedFunction makeFunc(boolean clamp) {
15+
// z(x,y) = x + y, simple additive function
16+
TreeMap<Float, TreeMap<Float, Float>> table = new TreeMap<>();
17+
for (int xi = 0; xi <= 5; xi++) {
18+
TreeMap<Float, Float> row = new TreeMap<>();
19+
for (int yi = 0; yi <= 5; yi++) {
20+
row.put((float) yi, (float) (xi + yi));
21+
}
22+
table.put((float) xi, row);
23+
}
24+
return new TwoDTabulatedFunction(table, 1f, 1f, StepMode.LINEAR, StepMode.LINEAR, clamp);
25+
}
26+
27+
private TwoDTabulatedFunction makeFuncWithHoles(boolean clamp) {
28+
// z(x,y) = x + y, but missing some rows and entries
29+
TreeMap<Float, TreeMap<Float, Float>> table = new TreeMap<>();
30+
for (int xi = 0; xi <= 5; xi++) {
31+
if (xi == 2) continue; // skip entire row (hole in X)
32+
TreeMap<Float, Float> row = new TreeMap<>();
33+
for (int yi = 0; yi <= 5; yi++) {
34+
if (yi == 3) continue; // skip inner point (hole in Y)
35+
row.put((float) yi, (float) (xi + yi));
36+
}
37+
table.put((float) xi, row);
38+
}
39+
return new TwoDTabulatedFunction(table, 1f, 1f, StepMode.LINEAR, StepMode.LINEAR, clamp);
40+
}
41+
42+
@Test
43+
void testExactPoints() {
44+
TwoDTabulatedFunction f = makeFunc(false);
45+
for (int x = 0; x <= 5; x++) {
46+
for (int y = 0; y <= 5; y++) {
47+
assertEquals(x + y, f.evaluate(x, y), 1e-6,
48+
"Exact point failed at (" + x + "," + y + ")");
49+
}
50+
}
51+
}
52+
53+
@Test
54+
void testMidpoints() {
55+
TwoDTabulatedFunction f = makeFunc(false);
56+
float val = f.evaluate(2.5f, 3.5f);
57+
assertEquals(2.5f + 3.5f, val, 1e-6, "Midpoint interpolation failed");
58+
}
59+
60+
@Test
61+
void testExtrapolationBelowAndAbove() {
62+
TwoDTabulatedFunction f = makeFunc(false);
63+
assertEquals(-1f, f.evaluate(-1f, 0f), 1e-6, "Extrapolation below X failed");
64+
assertEquals(11f, f.evaluate(5f, 6f), 1e-6, "Extrapolation above Y failed");
65+
assertEquals(12f, f.evaluate(6f, 6f), 1e-6, "Extrapolation above both failed");
66+
}
67+
68+
@Test
69+
void testClampMode() {
70+
TwoDTabulatedFunction f = makeFunc(true);
71+
assertEquals(0f, f.evaluate(-10f, 0f), 1e-6, "Clamp below X failed");
72+
assertEquals(5f, f.evaluate(5f, -10f), 1e-6, "Clamp below Y failed");
73+
assertEquals(10f, f.evaluate(5f, 5f), 1e-6, "Clamp at top-right failed");
74+
assertEquals(10f, f.evaluate(100f, 100f), 1e-6, "Clamp far outside failed");
75+
}
76+
77+
@Test
78+
void testHoles() {
79+
TwoDTabulatedFunction f = makeFuncWithHoles(false);
80+
81+
// Hole in X row: x=2 missing → should interpolate between x=1 and x=3
82+
float val = f.evaluate(2f, 4f);
83+
assertEquals(6f, val, 1e-6, "Interpolation across missing row failed");
84+
85+
// Hole in Y entry: y=3 missing → should interpolate between y=2 and y=4
86+
val = f.evaluate(4f, 3f);
87+
assertEquals(7f, val, 1e-6, "Interpolation across missing column entry failed");
88+
}
89+
90+
@Test
91+
void stressRandomInputs() {
92+
TwoDTabulatedFunction f = makeFuncWithHoles(false);
93+
Random rnd = new Random(42);
94+
95+
for (int i = 0; i < 5000; i++) {
96+
float x = rnd.nextFloat() * 7f - 1f; // [-1,6]
97+
float y = rnd.nextFloat() * 7f - 1f; // [-1,6]
98+
float expected = x + y; // true function
99+
float actual = f.evaluate(x, y);
100+
101+
assertEquals(expected, actual, 1e-4,
102+
"Stress test failed at (" + x + "," + y + ")");
103+
}
104+
}
105+
}

0 commit comments

Comments
 (0)