3 #include <HelixHough/SimpleHit3D.h>
4 #include <HelixHough/HelixKalmanState.h>
13 using namespace Eigen;
15 static inline float sign(
float x) {
16 return ((
float)(x > 0.)) - ((float)(x < 0.));
20 std::vector<float>& detector_material,
float B) :
21 det_radii(detector_radii),
22 det_scatter_variance(detector_material),
23 nlayers(detector_radii.size()),
37 Matrix<float, 2, 5>
H = Matrix<float, 2, 5>::Zero(2, 5);
39 Matrix<float, 2, 1>
ha = Matrix<float, 2, 1>::Zero(2, 1);
43 Matrix<float, 2, 1>
m = Matrix<float, 2, 1>::Zero(2, 1);
45 Matrix<float, 2, 2>
G = Matrix<float, 2, 2>::Zero(2, 2);
49 Matrix<float, 5, 5>
Q = Matrix<float, 5, 5>::Zero(5, 5);
52 Matrix<float, 5, 2> Ht = H.transpose();
54 Matrix<float, 5, 5> C_proj = state.
C +
Q;
56 Matrix<float, 5, 5> C_proj_inv = C_proj.fullPivLu().inverse();
58 Matrix<float, 5, 5> Cadd = Ht * G * H;
60 Matrix<float, 5, 5> Cinv = (C_proj_inv + Cadd);
62 state.
C = Cinv.fullPivLu().inverse();
65 Matrix<float, 5, 2> K = state.
C * Ht * G;
67 Matrix<float, 2, 1> proj_diff = Matrix<float, 2, 1>::Zero(2, 1);
70 Matrix<float, 5, 1> state_add = K * proj_diff;
72 Matrix<float, 5, 1> old_state = Matrix<float, 5, 1>::Zero(5, 1);
73 old_state(0, 0) = state.
phi;
74 old_state(1, 0) = state.
d;
76 old_state(2, 0) = state.
nu;
77 old_state(3, 0) = state.
z0;
78 old_state(4, 0) = state.
dzdl;
80 Matrix<float, 5, 1> new_state = old_state + state_add;
82 state.
phi = new_state(0, 0);
83 state.
d = new_state(1, 0);
84 state.
nu = new_state(2, 0);
86 state.
z0 = new_state(3, 0);
87 state.
dzdl = new_state(4, 0);
91 Matrix<float, 5, 1> state_diff = (new_state - old_state);
92 Matrix<float, 1, 5> state_diff_T = state_diff.transpose();
94 Matrix<float, 1, 1> chi2_add = (state_diff_T * C_proj_inv * state_diff);
95 state.
chi2 += chi2_add(0, 0);
100 Matrix<float, 2, 5>& H,
101 Matrix<float, 2, 1>&
ha) {
107 Matrix<float, 3, 5> dxda = Matrix<float, 3, 5>::Zero(3, 5);
114 Matrix<float, 2, 3> dmdx = Matrix<float, 2, 3>::Zero(2, 3);
116 float r2_inv = 1. / (x * x + y *
y);
117 dmdx(0, 0) = -y * r2_inv;
118 dmdx(0, 1) = x * r2_inv;
123 ha(0, 0) = atan2(y, x);
131 Matrix<float, 2, 1>&
m,
132 Matrix<float, 2, 2>& G) {
133 Matrix<float, 2, 2> V = Matrix<float, 2, 2>::Zero(2, 2);
136 V(0, 0) = 3.33333333333333426e-01 *
145 V(1, 1) = 3.33333333333333426e-01 *
149 G = V.fullPivLu().inverse();
151 m = Matrix<float, 2, 1>::Zero(2, 1);
160 Matrix<float, 5, 5>&
Q) {
161 Matrix<float, 5, 3> dAdAp = Matrix<float, 5, 3>::Zero(5, 3);
173 Matrix<float, 3, 3> dApdp = Matrix<float, 3, 3>::Zero(3, 3);
174 Vector3f
p = Vector3f::Zero(3);
177 Matrix<float, 3, 3> dpdb = Matrix<float, 3, 3>::Zero(3, 3);
180 Matrix<float, 3, 2> dbdt = Matrix<float, 3, 2>::Zero(3, 2);
183 Matrix<float, 5, 2> dAdt = dAdAp * dApdp * dpdb * dbdt;
185 for (
unsigned int j = 0; j < 5; ++j) {
186 for (
unsigned int i = 0; i < 5; ++i) {
187 Q(i, j) = (dAdt(i, 0) * dAdt(j, 0) + dAdt(i, 1) * dAdt(j, 1)) * var;
193 Matrix<float, 2, 1>&
ha,
194 Matrix<float, 2, 1>& diff) {
196 if (diff(0, 0) >
M_PI) {
197 diff(0, 0) -= (2. *
M_PI);
199 if (diff(0, 0) < (-
M_PI)) {
200 diff(0, 0) += (2. *
M_PI);
214 float cosphi = cos(phi);
215 float sinphi = sin(phi);
220 float kd = (d * k + 1.);
221 float kcx = kd * cosphi;
222 float kcy = kd * sinphi;
223 float kd_inv = 1. / kd;
224 float R2 = rad_det * rad_det;
225 float a = 0.5 * (k * R2 + (d * d * k + 2. *
d)) * kd_inv;
226 float tmp1 = a * kd_inv;
227 float P2x = kcx *
tmp1;
228 float P2y = kcy *
tmp1;
230 float h = sqrt(R2 - a * a);
232 float ux = -kcy * kd_inv;
233 float uy = kcx * kd_inv;
236 state.
x_int = P2x + signk * ux *
h;
237 state.
y_int = P2y + signk * uy *
h;
239 float sign_dzdl =
sign(dzdl);
240 float startx = d * cosphi;
241 float starty = d * sinphi;
242 float D = sqrt((startx - state.
x_int) * (startx - state.
x_int) +
243 (starty - state.
y_int) * (starty - state.
y_int));
244 float v = 0.5 * k *
D;
249 float s = 2. * asin(v) /
k;
250 float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
251 state.
z_int = z0 + sign_dzdl *
dz;
254 float temp1 = k * D * 0.5;
256 float temp2 = D * 0.5;
261 s += (3. / 20.) *
temp2;
263 s += (5. / 56.) *
temp2;
264 float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
265 state.
z_int = z0 + sign_dzdl *
dz;
278 float p_inv = 3.33333333333333314e+02 * k *
Bfield_inv *
289 Matrix<float, 5, 3>& dAdAp,
float& phi_p,
290 float& cosphi_p,
float& sinphi_p) {
297 float cosphi = cos(phi);
298 float sinphi = sin(phi);
305 float x0 = state.
x_int;
306 float y0 = state.
y_int;
308 phi_p = atan2((1. + k * d) * sinphi - k * y0, (1. + k * d) * cosphi - k * x0);
309 cosphi_p = cos(phi_p);
310 sinphi_p = sin(phi_p);
330 float tx = cosphi_p + k * x0;
331 float ty = sinphi_p + k * y0;
332 float tx2ty2_inv = 1. / (tx * tx + ty * ty);
333 float dphidtx = -ty * tx2ty2_inv;
334 float dphidty = tx * tx2ty2_inv;
335 float dtxdA_p = -sinphi_p;
336 float dtydA_p = cosphi_p;
337 dAdAp(0, 0) = dphidtx * dtxdA_p + dphidty * dtydA_p;
341 dAdAp(0, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p;
344 float k_inv = 1. /
k;
345 float tx2ty2sqrtinv = sqrt(tx2ty2_inv);
346 float dddtx = tx2ty2sqrtinv * tx * k_inv;
347 float dddty = tx2ty2sqrtinv * ty * k_inv;
351 dAdAp(1, 0) = dddtx * dtxdA_p + dddty * dtydA_p;
355 dAdAp(1, 1) = dphidtx * dtxdA_p + dphidty * dtydA_p -
356 (sqrt(tx * tx + ty * ty) - 1.) * k_inv * k_inv;
359 dAdAp(1, 0) = y0 * cosphi_p - x0 * sinphi_p;
360 dAdAp(1, 1) = (dAdAp(1, 0)) * (dAdAp(1, 0)) * 0.5;
367 float sign_dzdl =
sign(dzdl);
369 float dx = d * cosphi;
370 float dy = d * sinphi;
371 float D = sqrt((x0 - dx) * (x0 - dx) + (y0 - dy) * (y0 - dy));
372 float D_inv = 1. /
D;
373 float v = 0.5 * k *
D;
375 float k_inv = 1. /
k;
376 float s = 2. * asin(v) * k_inv;
377 float s_inv = 1. /
s;
378 float dsdv = 2. / (k * sqrt(1 - v * v));
379 float dsdk = -s * k_inv;
383 float dz = sqrt(tmp1 / (1. - dzdl * dzdl));
384 float dz3 = dz * dz *
dz;
387 float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
388 float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
393 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
394 float dvdA = 0.5 * (k * dDdA + D * dkdA);
395 float dsdA = dsdv * dvdA + dsdk * dkdA;
396 float ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
397 dAdAp(3, 0) = -sign_dzdl * ddzdA;
405 dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
406 dvdA = 0.5 * (k * dDdA + D * dkdA);
407 dsdA = dsdv * dvdA + dsdk * dkdA;
408 ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
409 dAdAp(3, 1) = -sign_dzdl * ddzdA;
417 dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
418 dvdA = 0.5 * (k * dDdA + D * dkdA);
419 dsdA = dsdv * dvdA + dsdk * dkdA;
420 ddzdA = (dz * s_inv) * dsdA + (dz3 * tmp3) * ddzdldA;
421 dAdAp(3, 2) = -sign_dzdl * ddzdA;
424 float temp1 = k * D * 0.5;
426 float temp2 = D * 0.5;
431 s += (3. / 20.) *
temp2;
433 s += (5. / 56.) *
temp2;
434 float s_inv = 1. /
s;
435 float onedzdl2_inv = 1. / (1. - dzdl *
dzdl);
436 float dz = sqrt(s * s * dzdl * dzdl * onedzdl2_inv);
437 float dz_inv = 1. /
dz;
453 float ddxdA = -d * sinphi * dAdAp(0, 0) + cosphi * dAdAp(1, 0);
454 float ddydA = d * cosphi * dAdAp(0, 0) + sinphi * dAdAp(1, 0);
459 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
461 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
462 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
463 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
464 float ddzdA = 0.5 * dz_inv *
465 (2. * (dsdA)*dz2 * s_inv +
466 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
467 dAdAp(3, 0) = -sign_dzdl * ddzdA;
475 dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
477 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
478 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
479 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
480 ddzdA = 0.5 * dz_inv *
481 (2. * (dsdA)*dz2 * s_inv +
482 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
483 dAdAp(3, 1) = -sign_dzdl * ddzdA;
491 dDdA = 0.5 * D_inv * (2. * (dx - x0) * ddxdA + 2. * (dy - y0) * ddydA);
493 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
494 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
495 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
496 ddzdA = 0.5 * dz_inv *
497 (2. * (dsdA)*dz2 * s_inv +
498 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
499 dAdAp(3, 2) = -sign_dzdl * ddzdA;
507 dAdAp(0, 1) *= 2. * state.
nu;
508 dAdAp(1, 1) *= 2. * state.
nu;
509 dAdAp(3, 1) *= 2. * state.
nu;
510 dAdAp(4, 1) *= 2. * state.
nu;
515 Matrix<float, 3, 3>& dApdp, Vector3f&
p,
516 float ,
float cosphi,
float sinphi) {
525 float pT_inv = 1. /
pT;
526 float pT_inv2 = pT_inv * pT_inv;
528 float px = -sinphi *
pT;
529 float py = cosphi *
pT;
530 float pz = dzdl * pT / sqrt(1. - dzdl * dzdl);
533 dApdp(0, 0) = -py * pT_inv2;
535 dApdp(0, 1) = px * pT_inv2;
538 float pTneg3 = (pT_inv * pT_inv2);
539 dApdp(1, 0) = -0.003 *
Bfield * px * pTneg3;
541 dApdp(1, 1) = -0.003 *
Bfield * py * pTneg3;
543 float pmag = sqrt(pT * pT + pz * pz);
544 float pmag_inv = 1. / pmag;
545 float pmag_inv_3 = pmag_inv * pmag_inv * pmag_inv;
547 dApdp(2, 0) = -pz * pmag_inv_3 * px;
548 dApdp(2, 1) = -pz * pmag_inv_3 * py;
549 dApdp(2, 2) = -pz * pmag_inv_3 * pz + pmag_inv;
557 dApdp(1, 0) *= 0.5 / state.
nu;
558 dApdp(1, 1) *= 0.5 / state.
nu;
565 Vector3f
b = Vector3f::Zero(3);
568 float p_mag = sqrt(p.dot(p));
570 Vector3f p_unit = p / p_mag;
572 Vector3f axis = b.cross(p_unit);
573 float angle = acos(p_unit.dot(b));
575 axis /= sqrt(axis.dot(axis));
577 Matrix<float, 3, 3> rot_p = Matrix<float, 3, 3>::Zero(3, 3);
578 float cos_p = cos(angle);
579 float sin_p = sin(angle);
580 rot_p(0, 0) = cos_p + axis(0) * axis(0) * (1. - cos_p);
581 rot_p(0, 1) = axis(0) * axis(1) * (1. - cos_p) - axis(2) * sin_p;
582 rot_p(0, 2) = axis(0) * axis(2) * (1. - cos_p) + axis(1) * sin_p;
583 rot_p(1, 0) = axis(1) * axis(0) * (1. - cos_p) + axis(2) * sin_p;
584 rot_p(1, 1) = cos_p + axis(1) * axis(1) * (1. - cos_p);
585 rot_p(1, 2) = axis(1) * axis(2) * (1. - cos_p) - axis(0) * sin_p;
586 rot_p(2, 0) = axis(2) * axis(0) * (1. - cos_p) - axis(1) * sin_p;
587 rot_p(2, 1) = axis(2) * axis(1) * (1. - cos_p) + axis(0) * sin_p;
588 rot_p(2, 2) = cos_p + axis(2) * axis(2) * (1. - cos_p);
590 dpdb = rot_p * p_mag;
637 Matrix<float, 3, 5>& dxda,
float&
x,
638 float&
y,
float&
z) {
647 float cosphi = cos(phi);
648 float sinphi = sin(phi);
653 float kd = (d * k + 1.);
654 float kcx = kd * cosphi;
655 float kcy = kd * sinphi;
656 float kd_inv = 1. / kd;
657 float R2 = rad_det * rad_det;
658 float a = 0.5 * (k * R2 + (d * d * k + 2. *
d)) * kd_inv;
659 float tmp1 = a * kd_inv;
660 float P2x = kcx *
tmp1;
661 float P2y = kcy *
tmp1;
663 float h = sqrt(R2 - a * a);
665 float ux = -kcy * kd_inv;
666 float uy = kcx * kd_inv;
668 float x1 = P2x + ux *
h;
669 float y1 = P2y + uy *
h;
670 float x2 = P2x - ux *
h;
671 float y2 = P2y - uy *
h;
672 float diff1 = (x1 - hit.
get_x()) * (x1 - hit.
get_x()) +
674 float diff2 = (x2 - hit.
get_x()) * (x2 - hit.
get_x()) +
683 x = P2x + signk * ux *
h;
684 y = P2y + signk * uy *
h;
689 float dkcxdA = -kd * sinphi;
691 float dkcydA = kd * cosphi;
693 float dkd_invdA = 0.;
697 float dtmp1dA = dadA * kd_inv + a * dkd_invdA;
698 float dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
699 float duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
700 float dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
701 dxda(0, 0) = dP2xdA + signk * (duxdA * h + ux * dhdA);
705 float duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
706 float dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
707 dxda(1, 0) = dP2ydA + signk * (duydA * h + uy * dhdA);
716 dkd_invdA = -kd_inv * kd_inv *
k;
720 0.5 * ((k * R2 + (d * d * k + 2. *
d)) * dkd_invdA + kd_inv * (2. * kd));
721 dtmp1dA = dadA * kd_inv + a * dkd_invdA;
722 dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
723 duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
724 dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
725 dxda(0, 1) = dP2xdA + signk * (duxdA * h + ux * dhdA);
729 duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
730 dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
731 dxda(1, 1) = dP2ydA + signk * (duydA * h + uy * dhdA);
740 dkd_invdA = -kd_inv * kd_inv *
d;
744 ((k * R2 + (d * d * k + 2. *
d)) * dkd_invdA + kd_inv * (R2 + d *
d));
745 dtmp1dA = dadA * kd_inv + a * dkd_invdA;
746 dP2xdA = tmp1 * dkcxdA + kcx * dtmp1dA;
747 duxdA = -kd_inv * dkcydA - kcy * dkd_invdA;
748 dhdA = (0.5 / sqrt(R2 - a * a)) * (-2. * a * dadA);
749 dxda(0, 2) = dP2xdA + signk * (duxdA * h + ux * dhdA);
753 duydA = kd_inv * dkcxdA + kcx * dkd_invdA;
754 dP2ydA = tmp1 * dkcydA + kcy * dtmp1dA;
755 dxda(1, 2) = dP2ydA + signk * (duydA * h + uy * dhdA);
759 float sign_dzdl =
sign(dzdl);
760 float onedzdl2_inv = 1. / (1. - dzdl *
dzdl);
761 float startx = d * cosphi;
762 float starty = d * sinphi;
763 float D = sqrt((startx - x) * (startx - x) + (starty - y) * (starty - y));
764 float D_inv = 1. /
D;
765 float v = 0.5 * k *
D;
771 float s = 2. * asin(v) /
k;
772 float s_inv = 1. /
s;
773 float sqrtvv = sqrt(1 - v * v);
774 float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
775 z = z0 + sign_dzdl *
dz;
776 float dz_inv = 1. /
dz;
780 float dstartxdA = -d * sinphi;
781 float dstartydA = d * cosphi;
786 float dDdA = 0.5 * D_inv * (2. * (startx -
x) * dstartxdA +
787 2. * (starty - y) * dstartydA);
788 float dvdA = 0.5 * (k * dDdA + D * dkdA);
789 float dsdA = (2. / (k * sqrtvv)) * dvdA - (s /
k) * dkdA;
790 float ddzdA = 0.5 * dz_inv *
791 (2. * (dsdA)*dz2 * s_inv +
792 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
793 dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
803 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
804 dvdA = 0.5 * (k * dDdA + D * dkdA);
805 dsdA = (2. / (k * sqrtvv)) * dvdA - (s /
k) * dkdA;
806 ddzdA = 0.5 * dz_inv *
807 (2. * (dsdA)*dz2 * s_inv +
808 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
809 dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
819 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
820 dvdA = 0.5 * (k * dDdA + D * dkdA);
821 dsdA = (2. / (k * sqrtvv)) * dvdA - (s /
k) * dkdA;
822 ddzdA = 0.5 * dz_inv *
823 (2. * (dsdA)*dz2 * s_inv +
824 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
825 dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
835 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
836 dvdA = 0.5 * (k * dDdA + D * dkdA);
837 dsdA = (2. / (k * sqrtvv)) * dvdA - (s /
k) * dkdA;
838 ddzdA = 0.5 * dz_inv *
839 (2. * (dsdA)*dz2 * s_inv +
840 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
841 dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
851 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
852 dvdA = 0.5 * (k * dDdA + D * dkdA);
853 dsdA = (2. / (k * sqrtvv)) * dvdA - (s /
k) * dkdA;
854 ddzdA = 0.5 * dz_inv *
855 (2. * (dsdA)*dz2 * s_inv +
856 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
857 dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
860 float temp1 = k * D * 0.5;
862 float temp2 = D * 0.5;
867 s += (3. / 20.) *
temp2;
869 s += (5. / 56.) *
temp2;
870 float s_inv = 1. /
s;
871 float dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
872 z = z0 + sign_dzdl *
dz;
873 float dz_inv = 1. /
dz;
889 float dstartxdA = -d * sinphi;
890 float dstartydA = d * cosphi;
895 float dDdA = 0.5 * D_inv * (2. * (startx -
x) * dstartxdA +
896 2. * (starty - y) * dstartydA);
898 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
899 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
900 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
901 float ddzdA = 0.5 * dz_inv *
902 (2. * (dsdA)*dz2 * s_inv +
903 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
904 dxda(2, 0) = dz0dA + sign_dzdl * ddzdA;
914 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
916 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
917 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
918 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
919 ddzdA = 0.5 * dz_inv *
920 (2. * (dsdA)*dz2 * s_inv +
921 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
922 dxda(2, 1) = dz0dA + sign_dzdl * ddzdA;
932 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
934 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
935 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
936 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
937 ddzdA = 0.5 * dz_inv *
938 (2. * (dsdA)*dz2 * s_inv +
939 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
940 dxda(2, 2) = dz0dA + sign_dzdl * ddzdA;
950 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
952 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
953 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
954 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
955 ddzdA = 0.5 * dz_inv *
956 (2. * (dsdA)*dz2 * s_inv +
957 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
958 dxda(2, 3) = dz0dA + sign_dzdl * ddzdA;
968 (2. * (startx -
x) * dstartxdA + 2. * (starty - y) * dstartydA);
970 dsdA += (1. / 24.) * (3. * k2 * D2 * dDdA + 2. * k * D3 * dkdA);
971 dsdA += (3. / 640.) * (5. * D4 * k4 * dDdA + 4. * k3 * D5 * dkdA);
972 dsdA += (5. / 7168.) * (7. * D6 * k6 * dDdA + 6. * k5 * D7 * dkdA);
973 ddzdA = 0.5 * dz_inv *
974 (2. * (dsdA)*dz2 * s_inv +
975 s * s * (2. * dzdl * ddzdldA * onedzdl2_inv * onedzdl2_inv));
976 dxda(2, 4) = dz0dA + sign_dzdl * ddzdA;
983 for (
int i = 0; i < 3; ++i) {
984 dxda(i, 2) *= 2. * state.
nu;