Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package org.jlab.clas.tracking.utilities;

import java.util.concurrent.ConcurrentLinkedQueue;
import org.jlab.clas.pdg.PhysicsConstants;
import org.jlab.jnp.matrix.*;
import org.jlab.clas.swimtools.Swim;
Expand All @@ -24,11 +23,12 @@ public void SwimToZ(int sector, StateVec vec, Swim swim, double z0, float[] bf)
swim.Bfield(sector, vec.x, vec.y, vec.z, bf);

vec.B = Math.sqrt(bf[0]*bf[0] + bf[1]*bf[1] + bf[2]*bf[2]);
double s = vec.B;
final double travelSign = Math.signum(z0 - vec.z);
double BatMeas = 0;

while(travelSign * vec.z < travelSign * z0) {
double s = travelSign * stepSize;
s = travelSign * stepSize;
if (travelSign*(vec.z+s) > travelSign*z0) s = travelSign*Math.abs(z0-vec.z);

this.RK4transport(sector, s, swim, vec);
Expand Down Expand Up @@ -82,8 +82,8 @@ private double[] RK4step(int sector, double z0, double h, Swim swim, double[] sI
public void RK4transport(int sector, double h, Swim swim, Matrix cMat, StateVec vec) {
// Set initial state and Jacobian.
double qv = vec.Q*v;
RK4Vec s0 = RK4VEC_POOL.poll(vec);
RK4Vec sNull = RK4VEC_POOL.poll();
RK4Vec s0 = new RK4Vec(vec);
RK4Vec sNull = new RK4Vec();

// Perform steps.
RK4Vec s1 = RK4step(sector, vec.z, 0, swim, s0, sNull, qv);
Expand All @@ -94,11 +94,6 @@ public void RK4transport(int sector, double h, Swim swim, Matrix cMat, StateVec
// Compute and set final state and covariance matrix.
RK4Vec sF = computeFinalState(h, s0, s1, s2, s3, s4);

RK4VEC_POOL.offer(s1);
RK4VEC_POOL.offer(s2);
RK4VEC_POOL.offer(s3);
RK4VEC_POOL.offer(s4);

vec.x = sF.x;
vec.y = sF.y;
vec.tx = sF.tx;
Expand All @@ -107,17 +102,12 @@ public void RK4transport(int sector, double h, Swim swim, Matrix cMat, StateVec
vec.B = Math.sqrt(_b[0]*_b[0]+_b[1]*_b[1]+_b[2]*_b[2]);
vec.deltaPath += Math.sqrt((s0.x-vec.x)*(s0.x-vec.x) + (s0.y-vec.y)*(s0.y-vec.y) + h*h);
vec.CM.set(computeCovMat(cMat, sF));

RK4VEC_POOL.offer(sF);

RK4VEC_POOL.offer(s0);
RK4VEC_POOL.offer(sNull);
}

/** 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 = RK4VEC_POOL.poll();
RK4Vec sNext = new RK4Vec();
swim.Bfield(sector, sInit.x + h*sPrev.x, sInit.y + h*sPrev.y, z0 + h, _b);

// State.
Expand Down Expand Up @@ -158,7 +148,7 @@ private RK4Vec RK4step(int sector, double z0, double h, Swim swim,
/** Compute the final state for each entry in the internal state matrix. */
private RK4Vec computeFinalState(double h, RK4Vec s0, RK4Vec s1, RK4Vec s2, RK4Vec s3,
RK4Vec s4) {
RK4Vec sF = RK4VEC_POOL.poll();
RK4Vec sF = new RK4Vec();

sF.x = s0.x + this.RK4(s1.x, s2.x, s3.x, s4.x, h);
sF.dxdtx0 = this.RK4(s1.dxdtx0, s2.dxdtx0, s3.dxdtx0, s4.dxdtx0, h);
Expand Down Expand Up @@ -339,34 +329,8 @@ private double dty_dq0(double qv, double Ay, double dAy_dtx, double dAy_dty,
return v*Ay + qv * (dAy_dtx*dtx_dq0 + dAy_dty*dty_dq0);
}

static class RK4VecPool extends ConcurrentLinkedQueue<RK4Vec> {
public RK4VecPool(int size) {
super();
for (int i=0; i<size; i++) add(new RK4Vec());
}
@Override
public RK4Vec poll() {
RK4Vec v = super.poll();
v.reset();
return v;
}
public RK4Vec poll(StateVec vec) {
RK4Vec v = poll();
v.x = vec.x;
v.y = vec.y;
v.tx = vec.tx;
v.ty = vec.ty;
v.dtxdtx0 = 1;
v.dtydty0 = 1;
return v;
}
}

// At 16 doubles each, 10k is about 10 MB:
static final RK4VecPool RK4VEC_POOL =new RK4VecPool(10000);

/** State vector and its derivatives used internally. */
static class RK4Vec {
private class RK4Vec {
public double x = 0;
public double dxdtx0 = 0;
public double dxdty0 = 0;
Expand All @@ -387,26 +351,15 @@ static class RK4Vec {
public double dtydty0 = 0;
public double dtydq0 = 0;

void reset() {
x = 0;
dxdtx0 = 0;
dxdty0 = 0;
dxdq0 = 0;

y = 0;
dydtx0 = 0;
dydty0 = 0;
dydq0 = 0;

tx = 0;
dtxdtx0 = 0;
dtxdty0 = 0;
dtxdq0 = 0;

ty = 0;
dtydtx0 = 0;
dtydty0 = 0;
dtydq0 = 0;
RK4Vec() {}
RK4Vec(StateVec vec) {
this.x = vec.x;
this.y = vec.y;
this.tx = vec.tx;
this.ty = vec.ty;

this.dtxdtx0 = 1;
this.dtydty0 = 1;
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import java.util.ArrayDeque;
import java.util.List;
import java.util.concurrent.ConcurrentLinkedQueue;

/**
* Static methods for Runge-Kutta 4 integration, including a constant stepsize
Expand Down Expand Up @@ -1063,42 +1062,6 @@ private int driver(double yo[],
// A uniform step size advancer
class UniformAdvance implements IAdvance {

static class State {
double k2[];
double k3[];
double k4[];
double ytemp[];
State(int dim) {
k2 = new double[dim];
k3 = new double[dim];
k4 = new double[dim];
ytemp = new double[dim];
}
State reset() {
for (int i=0; i<k2.length; i++) {
k2[i] = 0;
k3[i] = 0;
k4[i] = 0;
ytemp[i] = 0;
}
return this;
}
}

static class StatePool extends ConcurrentLinkedQueue<State> {
StatePool(int size, int dim) {
for (int i=0; i<size; i++) add(new State(dim));
}
@Override
public State poll() { return super.poll().reset(); }
}

static StatePool[] statePool = new StatePool[]{
null, null, null,
new StatePool(1000,3),new StatePool(1000,4),
new StatePool(1000,5),new StatePool(1000,6)
};

@Override
public void advance(double t,
double[] y,
Expand All @@ -1113,8 +1076,11 @@ public void advance(double t,
double k1[] = dydt; // the current dreivatives
// we need some arrays from the pool

State s = statePool[nDim].poll();

double k2[] = new double[nDim];
double k3[] = new double[nDim];
double k4[] = new double[nDim];
double ytemp[] = new double[nDim];

double hh = h * 0.5; // half step
double h6 = h / 6.0;

Expand All @@ -1124,28 +1090,27 @@ public void advance(double t,
// first step: initial derivs to midpoint
// after this,
for (int i = 0; i < nDim; i++) {
s.ytemp[i] = y[i] + hh * k1[i];
ytemp[i] = y[i] + hh * k1[i];
}
deriv.derivative(tmid, s.ytemp, s.k2);
deriv.derivative(tmid, ytemp, k2);

// second step (like 1st, but use midpoint just computed derivatives
// dyt)
for (int i = 0; i < nDim; i++) {
s.ytemp[i] = y[i] + hh * s.k2[i];
ytemp[i] = y[i] + hh * k2[i];
}
deriv.derivative(tmid, s.ytemp, s.k3);
deriv.derivative(tmid, ytemp, k3);

// third (full) step
for (int i = 0; i < nDim; i++) {
s.ytemp[i] = y[i] + h * s.k3[i];
ytemp[i] = y[i] + h * k3[i];
}
deriv.derivative(t + h, s.ytemp, s.k4);
deriv.derivative(t + h, ytemp, k4);

for (int i = 0; i < nDim; i++) {
yout[i] = y[i] + h6 * (k1[i] + +2.0 * s.k2[i] + 2 * s.k3[i] + s.k4[i]);
yout[i] = y[i] + h6 * (k1[i] + +2.0 * k2[i] + 2 * k3[i] + k4[i]);
}

statePool[nDim].offer(s);
}

@Override
Expand Down
Loading