26 static const double mu04pi = 1.0e-7;
27 static const double minvsq = 10. * 10.;
29 const Eigen::Map<const Eigen::Vector3d> pos(
xyz);
30 Eigen::Map<Eigen::Vector3d> B(B_in);
32 const Eigen::Vector3d r1 = pos -
m_p1;
33 const Eigen::Vector3d r2 = pos -
m_p2;
34 const Eigen::Vector3d v =
m_u.cross(r1);
35 const double vsq = std::max(v.squaredNorm(), minvsq);
37 const double r1u = r1.dot(
m_u);
39 const double r2u = r2.dot(
m_u);
40 const double r1n = r1.norm();
42 const double r2n = r2.norm();
44 const double f0 = mu04pi *
m_curr * scaleFactor / vsq;
45 const double f1 = f0 * (
m_finite ? r1u / r1n - r2u / r2n : 2.0);
46 const double f2 = 2.0 * f1 / vsq;
51 Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> D(deriv);
53 D(0, 1) -= f1 *
m_u(2);
54 D(0, 2) += f1 *
m_u(1);
55 D(1, 0) += f1 *
m_u(2);
56 D(1, 2) -= f1 *
m_u(0);
57 D(2, 0) -= f1 *
m_u(1);
58 D(2, 1) += f1 *
m_u(0);
61 Eigen::Vector3d w = f2 * (
m_u * r1u - r1);
65 w += f0 * ((
m_u - r1 * r1u / (r1n * r1n)) / r1n -
66 (
m_u - r2 * r2u / (r2n * r2n)) / r2n);
69 D += v * w.transpose();