| 6 | public class FitCurve { |
| 7 | |
| 8 | public void fit(Vec2[] p, double[] t) { |
| 9 | |
| 10 | int n = p.length; |
| 11 | |
| 12 | Vec2 P0 = p[0]; |
| 13 | Vec2 P3 = p[n - 1]; |
| 14 | Vec2 P1, P2; |
| 15 | |
| 16 | if (n == 1) |
| 17 | |
| 18 | { |
| 19 | P1 = P0; |
| 20 | P2 = P0; |
| 21 | } else if (n == 2) { |
| 22 | |
| 23 | P1 = P0; |
| 24 | P2 = P3; |
| 25 | } |
| 26 | else if (n==3) |
| 27 | { |
| 28 | P1 = p[1]; |
| 29 | P2 = p[1]; |
| 30 | } |
| 31 | else |
| 32 | { |
| 33 | double A1=0, A2=0, A12=0; |
| 34 | Vec2 C1 = new Vec2(); |
| 35 | Vec2 C2 = new Vec2(); |
| 36 | for (int i = 2;i<n -1;i++) |
| 37 | { |
| 38 | double B0 = (1 - t[i])*(1 - t[i])*(1 - t[i]) ; |
| 39 | double B1 = (3 * t[i] * (1 - t[i])*(1 - t[i])); |
| 40 | double B2 = (3 * t[i] *t[i] * (1 - t[i])); |
| 41 | double B3 = t[i] *t[i] *t[i] ; |
| 42 | |
| 43 | A1 = A1 + B1 * B1; |
| 44 | A2 = A2 + B2 * B2; |
| 45 | A12 = A12 + B1 * B2; |
| 46 | Vec2 temp = (new Vec2(p[i]).sub(new Vec2(P0).mul(B0)).sub(new Vec2(P3).mul(B3))); |
| 47 | C1.fma((float) B1, temp); |
| 48 | C2.fma((float) B2, temp); |
| 49 | |
| 50 | } |
| 51 | |
| 52 | double DENOM = (A1 * A2 - A12 * A12); |
| 53 | if (DENOM == 0) { |
| 54 | P1 = P0; |
| 55 | P2 = P3; |
| 56 | } else { |
| 57 | P1 = (new Vec2(C1).mul(A2).sub( new Vec2(C2).mul(A12))).mul(1 / DENOM); |
| 58 | P2 = (new Vec2(C2).mul(A1).sub( new Vec2(C1).mul(A12))).mul(1 / DENOM); |
| 59 | } |
| 60 | |
| 61 | } |
| 62 | } |
| 63 | } |