Skip to content

fix: little DC speedups, finals for clarity #1258

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
baltzell wants to merge 5 commits into
base: development
Choose a base branch
from
Draft

fix: little DC speedups, finals for clarity #1258

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c1001eb
minor optimizations, finals for clarity
baltzell May 13, 2026
25a8766
remove unnecessary check
baltzell May 13, 2026
edcdbb1
break it down
baltzell May 13, 2026
9d1faf8
make methods static
baltzell May 13, 2026
04475cf
split up
baltzell May 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 20 additions & 28 deletions reconstruction/dc/src/main/java/org/jlab/rec/dc/cluster/ClusterCleanerUtilities.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,34 +76,29 @@ public List<FittedCluster> ClusterSplitter(FittedCluster clus, int nextClsStartI
/// determined.
/// This is a preliminary pattern recognition method used to identify
/// reconstructed hits belonging to the same track-segment.
int N_t = 180;
final int N_t = 180;

// From this calculate the bin size in the theta accumulator array
double ThetaMin = 0.;
double ThetaMax = 2. * Math.PI;
double SizeThetaBin = (ThetaMax - ThetaMin) / ((double) N_t);
final double ThetaMin = 0.;
final double ThetaMax = 2. * Math.PI;
final double SizeThetaBin = (ThetaMax - ThetaMin) / ((double) N_t);

// Define the dimension of the r accumulator array
int N_r = 130;
final int N_r = 130;
// From this calculate the bin size in the theta accumulator array
double RMin = -130;
double RMax = 130;
final double RMin = -130;
final double RMax = 130;
final double dR = RMax - RMin;

int[][] R_Phi_Accumul;
R_Phi_Accumul = new int[N_r][N_t];
final int[][] R_Phi_Accumul = new int[N_r][N_t];

// cache the cos and sin theta values [for performance improvement]
double[] cosTheta_RPhi_array;
double[] sinTheta_RPhi_array;
final double[] cosTheta_RPhi_array = new double[N_t];
final double[] sinTheta_RPhi_array = new double[N_t];

// the values corresponding to the peaks in the array
double[] binrMaxR_Phi;
double[] bintMaxR_Phi;
binrMaxR_Phi = new double[N_r * N_t];
bintMaxR_Phi = new double[N_r * N_t];

cosTheta_RPhi_array = new double[N_t];
sinTheta_RPhi_array = new double[N_t];
final double[] binrMaxR_Phi = new double[N_r * N_t];
final double[] bintMaxR_Phi = new double[N_r * N_t];

for (int j_t = 0; j_t < N_t; j_t++) {
// theta_j in the middle of the bin :
Expand All @@ -125,7 +120,7 @@ public List<FittedCluster> ClusterSplitter(FittedCluster clus, int nextClsStartI
// r_j corresponding to that theta_j:
double r_j = rho * cosTheta_RPhi_array[j_t] + phi * sinTheta_RPhi_array[j_t];
// this value of r_j falls into the following bin in the r array:
int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (RMax - RMin));
int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (dR));

// increase this accumulator cell:
R_Phi_Accumul[j_r][j_t]++;
Expand Down Expand Up @@ -179,11 +174,12 @@ public List<FittedCluster> ClusterSplitter(FittedCluster clus, int nextClsStartI
// r_j corresponding to that theta_j:
double r_j = rho * cosTheta_RPhi_array[j_t] + phi * sinTheta_RPhi_array[j_t];
// this value of r_j falls into the following bin in the r array:
int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (RMax - RMin));
int j_r = (int) Math.floor(N_r * (r_j - RMin) / (float) (dR));

// match bins:
if (j_r == binrMaxR_Phi[p] && j_t == bintMaxR_Phi[p]) {
newClus.add(clus.get(i)); // add this hit
break; // it cannot be in any other theta bin
}
}
}
Expand Down Expand Up @@ -242,16 +238,15 @@ public List<FittedCluster> ClusterSplitter(FittedCluster clus, int nextClsStartI
}

int splitclusId = 1;
if (!selectedClusList2.isEmpty()) {
if (selectedClusList2.isEmpty()) {
selectedClusList2.add(clus); // if the splitting fails, then return the original cluster
}
else {
for (FittedCluster cl : selectedClusList2) {
cl.set_Id(clus.get_Id() * 1000 + splitclusId);
splitclusId++;
}
}

if (selectedClusList2.isEmpty()) {
selectedClusList2.add(clus); // if the splitting fails, then return the original cluster
}
return selectedClusList2;
}

Expand Down Expand Up @@ -328,11 +323,8 @@ public FittedCluster LRAmbiguityResolver(DataEvent event, FittedCluster fClus,

int index = 0;
for(FittedHit hit: fClus) {

if (hit.get_Doca() < 0.4 * hit.get_CellSize()) {
hit.set_LeftRightAmb(0);
}
if (hit.get_LeftRightAmb() == 0) {
index++;
}
}
Expand Down
192 changes: 80 additions & 112 deletions reconstruction/dc/src/main/java/org/jlab/rec/dc/track/fit/RungeKuttaDoca.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
package org.jlab.rec.dc.track.fit;

import java.util.ArrayList;
import org.jlab.jnp.matrix.*;
import org.jlab.clas.swimtools.Swim;
import org.jlab.jnp.matrix.Matrix;

/**
* Swims a given state vector to a given Z position using Runge Kutta 4 transport.
Expand Down Expand Up @@ -103,43 +102,46 @@ void RK4transport(int sector, double h, Swim swim, StateVecsDoca.CovMat covMat,
fCov.covMat.set(computeCovMat(covMat.covMat, sF));
}

private void linStep(double h, RK4Vec next, RK4Vec init, RK4Vec prev) {
next.x = init.tx + h*prev.tx;
next.y = init.ty + h*prev.ty;
next.dxdtx0 = init.dtxdtx0 + h*prev.dtxdtx0;
next.dxdty0 = init.dtxdty0 + h*prev.dtxdty0;
next.dxdq0 = init.dtxdq0 + h*prev.dtxdq0;
next.dydtx0 = init.dtydtx0 + h*prev.dtydtx0;
next.dydty0 = init.dtydty0 + h*prev.dtydty0;
next.dydq0 = init.dtydq0 + h*prev.dtydq0;
}

private void covStep(RK4Vec next, double qv) {
double Csq = Csq(next.x, next.y);
double C = C(Csq);
double Ax = Ax(C, next.x, next.y);
double Ay = Ay(C, next.x, next.y);
next.tx = qv * Ax(C, next.x, next.y);
next.ty = qv * Ay(C, next.x, next.y);
double dAx_dtx = dAx_dtx(C, Csq, Ax, next.x, next.y);
double dAx_dty = dAx_dty(C, Csq, Ax, next.x, next.y);
double dAy_dtx = dAy_dtx(C, Csq, Ay, next.x, next.y);
double dAy_dty = dAy_dty(C, Csq, Ay, next.x, next.y);
next.dtxdtx0 = dtx_dtx0(qv, dAx_dtx, dAx_dty, next.dxdtx0, next.dydtx0);
next.dtxdty0 = dtx_dty0(qv, dAx_dty, next.dxdty0, next.dydty0);
next.dtxdq0 = dtx_dq0( qv, Ax, dAx_dtx, dAx_dty, next.dxdq0, next.dydq0);
next.dtydtx0 = dty_dtx0(qv, dAy_dtx, dAy_dty, next.dxdtx0, next.dydtx0);
next.dtydty0 = dty_dty0(qv, dAy_dty, next.dxdty0, next.dydty0);
next.dtydq0 = dty_dq0( qv, Ay, dAy_dtx, dAy_dty, next.dxdq0, next.dydq0);
}

/** Perform one RK4 step, updating the covariance matrix. */
private RK4Vec RK4step(int sector, double z0, double h, Swim swim,
RK4Vec sInit, RK4Vec sPrev, double qv) {

RK4Vec sNext = new RK4Vec();
swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b);

// State.
sNext.x = sInit.tx + h*sPrev.tx;
sNext.y = sInit.ty + h*sPrev.ty;
swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b);

double Csq = Csq(sNext.x, sNext.y);
double C = C(Csq);
double Ax = Ax(C, sNext.x, sNext.y);
double Ay = Ay(C, sNext.x, sNext.y);

sNext.tx = qv * Ax;
sNext.ty = qv * Ay;

// Jacobian.
sNext.dxdtx0 = sInit.dtxdtx0 + h*sPrev.dtxdtx0;
sNext.dxdty0 = sInit.dtxdty0 + h*sPrev.dtxdty0;
sNext.dxdq0 = sInit.dtxdq0 + h*sPrev.dtxdq0;
sNext.dydtx0 = sInit.dtydtx0 + h*sPrev.dtydtx0;
sNext.dydty0 = sInit.dtydty0 + h*sPrev.dtydty0;
sNext.dydq0 = sInit.dtydq0 + h*sPrev.dtydq0;

double dAx_dtx = dAx_dtx(C, Csq, Ax, sNext.x, sNext.y);
double dAx_dty = dAx_dty(C, Csq, Ax, sNext.x, sNext.y);
double dAy_dtx = dAy_dtx(C, Csq, Ay, sNext.x, sNext.y);
double dAy_dty = dAy_dty(C, Csq, Ay, sNext.x, sNext.y);

sNext.dtxdtx0 = this.dtx_dtx0(qv, dAx_dtx, dAx_dty, sNext.dxdtx0, sNext.dydtx0);
sNext.dtxdty0 = this.dtx_dty0(qv, dAx_dty, sNext.dxdty0, sNext.dydty0);
sNext.dtxdq0 = this.dtx_dq0( qv, Ax, dAx_dtx, dAx_dty, sNext.dxdq0, sNext.dydq0);
sNext.dtydtx0 = this.dty_dtx0(qv, dAy_dtx, dAy_dty, sNext.dxdtx0, sNext.dydtx0);
sNext.dtydty0 = this.dty_dty0(qv, dAy_dty, sNext.dxdty0, sNext.dydty0);
sNext.dtydq0 = this.dty_dq0( qv, Ay, dAy_dtx, dAy_dty, sNext.dxdq0, sNext.dydq0);
linStep(h, sNext, sInit, sPrev);
covStep(sNext, qv);

return sNext;
}
Expand Down Expand Up @@ -172,97 +174,63 @@ private RK4Vec computeFinalState(double h, RK4Vec s0, RK4Vec s1, RK4Vec s2, RK4V
return sF;
}

/** Compute the final covariance matrix. covMat = FCF^T. */
private double[][] computeCovMat(Matrix C, RK4Vec sF) {
double[][] cNext = new double[5][5];
cNext[0][0] = C.get(0,0)+C.get(2,0)*sF.dxdtx0+C.get(3,0)*sF.dxdty0+C.get(4,0)*sF.dxdq0
+ sF.dxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

cNext[0][1] = C.get(0,1)+C.get(2,1)*sF.dxdtx0+C.get(3,1)*sF.dxdty0+C.get(4,1)*sF.dxdq0
+ sF.dydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

cNext[0][2] = sF.dtxdq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dtxdtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dtxdty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

cNext[0][3] = sF.dtydq0 *(C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dtydtx0*(C.get(0,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dtydty0*(C.get(0,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

cNext[0][4] = C.get(0,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0;

cNext[1][0] = C.get(1,0)+C.get(2,0)*sF.dydtx0+C.get(3,0)*sF.dydty0+C.get(4,0)*sF.dydq0
+ sF.dxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0)
+ sF.dxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0)
+ sF.dxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0);

cNext[1][1] = C.get(1,1)+C.get(2,1)*sF.dydtx0+C.get(3,1)*sF.dydty0+C.get(4,1)*sF.dydq0
+ sF.dydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0)
+ sF.dydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0)
+ sF.dydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0);

cNext[1][2] = sF.dtxdq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0)
+ sF.dtxdtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0)
+ sF.dtxdty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0);

cNext[1][3] = sF.dtydq0 *(C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0)
+ sF.dtydtx0*(C.get(1,2)+C.get(2,2)*sF.dydtx0+C.get(3,2)*sF.dydty0+C.get(4,2)*sF.dydq0)
+ sF.dtydty0*(C.get(1,3)+C.get(2,3)*sF.dydtx0+C.get(3,3)*sF.dydty0+C.get(4,3)*sF.dydq0);

cNext[1][4] = C.get(1,4)+C.get(2,4)*sF.dydtx0+C.get(3,4)*sF.dydty0+C.get(4,4)*sF.dydq0;

cNext[2][0] = C.get(2,0)*sF.dtxdtx0+C.get(3,0)*sF.dtxdty0+C.get(4,0)*sF.dtxdq0
private static void doit(int i, double[][] next, Matrix C, RK4Vec sF) {
next[i][0] = C.get(i,0)+C.get(2,0)*sF.dxdtx0+C.get(3,0)*sF.dxdty0+C.get(4,0)*sF.dxdq0
+ sF.dxdq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dxdtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dxdty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

next[i][1] = C.get(i,1)+C.get(2,1)*sF.dxdtx0+C.get(3,1)*sF.dxdty0+C.get(4,1)*sF.dxdq0
+ sF.dydq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dydtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dydty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

next[i][2] = sF.dtxdq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dtxdtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dtxdty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

next[i][3] = sF.dtydq0 *(C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0)
+ sF.dtydtx0*(C.get(i,2)+C.get(2,2)*sF.dxdtx0+C.get(3,2)*sF.dxdty0+C.get(4,2)*sF.dxdq0)
+ sF.dtydty0*(C.get(i,3)+C.get(2,3)*sF.dxdtx0+C.get(3,3)*sF.dxdty0+C.get(4,3)*sF.dxdq0);

next[i][4] = C.get(i,4)+C.get(2,4)*sF.dxdtx0+C.get(3,4)*sF.dxdty0+C.get(4,4)*sF.dxdq0;
}

private static void doit2(int i, double[][] next, Matrix C, RK4Vec sF) {
next[i][0] = C.get(2,0)*sF.dtxdtx0+C.get(3,0)*sF.dtxdty0+C.get(4,0)*sF.dtxdq0
+ sF.dxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
+ sF.dxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0)
+ sF.dxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0);

cNext[2][1] = C.get(2,1)*sF.dtxdtx0+C.get(3,1)*sF.dtxdty0+C.get(4,1)*sF.dtxdq0
next[i][1] = C.get(2,1)*sF.dtxdtx0+C.get(3,1)*sF.dtxdty0+C.get(4,1)*sF.dtxdq0
+ sF.dydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
+ sF.dydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0)
+ sF.dydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0);

cNext[2][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
next[i][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
+ sF.dtxdtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0)
+ sF.dtxdty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0);

cNext[2][3] = sF.dtydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
next[i][3] = sF.dtydq0 *(C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0)
+ sF.dtydtx0*(C.get(2,2)*sF.dtxdtx0+C.get(3,2)*sF.dtxdty0+C.get(4,2)*sF.dtxdq0)
+ sF.dtydty0*(C.get(2,3)*sF.dtxdtx0+C.get(3,3)*sF.dtxdty0+C.get(4,3)*sF.dtxdq0);

cNext[2][4] = C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0;

cNext[3][0] = C.get(2,0)*sF.dtydtx0+C.get(3,0)*sF.dtydty0+C.get(4,0)*sF.dtydq0
+ sF.dxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0)
+ sF.dxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0)
+ sF.dxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0);

cNext[3][1] = C.get(2,1)*sF.dtydtx0+C.get(3,1)*sF.dtydty0+C.get(4,1)*sF.dtydq0
+ sF.dydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0)
+ sF.dydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0)
+ sF.dydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0);

cNext[3][2] = sF.dtxdq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0)
+ sF.dtxdtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0)
+ sF.dtxdty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0);

cNext[3][3] = sF.dtydq0 *(C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0)
+ sF.dtydtx0*(C.get(2,2)*sF.dtydtx0+C.get(3,2)*sF.dtydty0+C.get(4,2)*sF.dtydq0)
+ sF.dtydty0*(C.get(2,3)*sF.dtydtx0+C.get(3,3)*sF.dtydty0+C.get(4,3)*sF.dtydq0);

cNext[3][4] = C.get(2,4)*sF.dtydtx0+C.get(3,4)*sF.dtydty0+C.get(4,4)*sF.dtydq0;
next[i][4] = C.get(2,4)*sF.dtxdtx0+C.get(3,4)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0;
}

/** Compute the final covariance matrix. covMat = FCF^T. */
private double[][] computeCovMat(Matrix C, RK4Vec sF) {
double[][] cNext = new double[5][5];

doit(0, cNext, C, sF);
doit(1, cNext, C, sF);
doit2(2, cNext, C, sF);
doit2(3, cNext, C, sF);

cNext[4][0] = C.get(4,0)+C.get(4,2)*sF.dxdtx0+C.get(4,3)*sF.dxdty0+C.get(4,4)*sF.dxdq0;

cNext[4][1] = C.get(4,1)+C.get(4,2)*sF.dydtx0+C.get(4,3)*sF.dydty0+C.get(4,4)*sF.dydq0;

cNext[4][2] = C.get(4,2)*sF.dtxdtx0+C.get(4,3)*sF.dtxdty0+C.get(4,4)*sF.dtxdq0;

cNext[4][3] = C.get(4,2)*sF.dtydtx0+C.get(4,3)*sF.dtydty0+C.get(4,4)*sF.dtydq0;

cNext[4][4] = C.get(4,4);

return cNext;
Expand All @@ -274,13 +242,13 @@ private double RK4(double k1, double k2, double k3, double k4, double h) {
}

// Auxiliary calculations.
private double C(double tx, double ty) {
private static double C(double tx, double ty) {
return Math.sqrt(1 + tx*tx + ty*ty);
}
private double C(double Csq) {
private static double C(double Csq) {
return Math.sqrt(Csq);
}
private double Csq(double tx, double ty) {
private static double Csq(double tx, double ty) {
return 1 + tx*tx + ty*ty;
}

Expand All @@ -305,22 +273,22 @@ private double dAy_dty(double C, double C2, double Ay, double tx, double ty) {
}

// Total derivatives.
private double dtx_dtx0(double qv, double dAx_dtx, double dAx_dty,
private static double dtx_dtx0(double qv, double dAx_dtx, double dAx_dty,
double dtx_dtx0, double dty_dtx0) {
return qv * (dAx_dtx*dtx_dtx0 + dAx_dty*dty_dtx0);
}
private double dtx_dty0(double qv, double dAx_dty, double dtx_dty0, double dty_dty0) {
private static double dtx_dty0(double qv, double dAx_dty, double dtx_dty0, double dty_dty0) {
return qv * (dAx_dty*dtx_dty0 + dAx_dty*dty_dty0);
}
private double dtx_dq0(double qv, double Ax, double dAx_dtx, double dAx_dty,
double dtx_dq0, double dty_dq0) {
return v*Ax + qv * (dAx_dtx*dtx_dq0 + dAx_dty*dty_dq0);
}
private double dty_dtx0(double qv, double dAy_dtx, double dAy_dty,
private static double dty_dtx0(double qv, double dAy_dtx, double dAy_dty,
double dtx_dtx0, double dty_dtx0) {
return qv * (dAy_dtx*dtx_dtx0 + dAy_dty*dty_dtx0);
}
private double dty_dty0(double qv, double dAy_dty, double dtx_dty0, double dty_dty0) {
private static double dty_dty0(double qv, double dAy_dty, double dtx_dty0, double dty_dty0) {
return qv * (dAy_dty*dtx_dty0 + dAy_dty*dty_dty0);
}
private double dty_dq0(double qv, double Ay, double dAy_dtx, double dAy_dty,
Expand Down
Loading