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();