17 #include "analysis/OrcaKinFit/SoftGaussMassConstraint.h"
18 #include "analysis/OrcaKinFit/ParticleFitObject.h"
19 #include <framework/logging/Logger.h>
32 namespace OrcaKinFit {
36 : SoftGaussParticleConstraint(sigma_),
46 double totE[2] = {0, 0};
47 double totpx[2] = {0, 0};
48 double totpy[2] = {0, 0};
49 double totpz[2] = {0, 0};
50 for (
unsigned int i = 0; i <
fitobjects.size(); i++) {
51 int index = (
flags[i] == 1) ? 0 : 1;
57 double result = -
mass;
58 result += std::sqrt(std::abs(totE[0] * totE[0] - totpx[0] * totpx[0] - totpy[0] * totpy[0] - totpz[0] * totpz[0]));
59 result -= std::sqrt(std::abs(totE[1] * totE[1] - totpx[1] * totpx[1] - totpy[1] * totpy[1] - totpz[1] * totpz[1]));
70 double totE[2] = {0, 0};
71 double totpx[2] = {0, 0};
72 double totpy[2] = {0, 0};
73 double totpz[2] = {0, 0};
74 bool valid[2] = {
false,
false};
75 for (
unsigned int i = 0; i <
fitobjects.size(); i++) {
76 int index = (
flags[i] == 1) ? 0 : 1;
84 double m_inv[2] = {0, 0};
85 for (
int index = 0; index < 2; ++index) {
86 m2[index] = totE[index] * totE[index] - totpx[index] * totpx[index]
87 - totpy[index] * totpy[index] - totpz[index] * totpz[index];
88 if (m2[index] < 0 && m2[index] > -1E-9) m2[index] = 0;
89 if (m2[index] < 0 && valid[index]) {
90 B2ERROR(
"SoftGaussMassConstraint::getDerivatives: m2<0!");
91 for (
unsigned int j = 0; j <
fitobjects.size(); j++) {
92 int jndex = (
flags[j] == 1) ? 0 : 1;
98 B2ERROR(
"sum: E=" << totE[index] <<
", px=" << totpx[index]
99 <<
", py=" << totpy[index] <<
", pz=" << totpz[index] <<
", m2=" << m2[index]);
101 if (m2[index] != 0) m_inv[index] = 1 / std::sqrt(std::abs(m2[index]));
104 for (
unsigned int i = 0; i <
fitobjects.size(); i++) {
105 int index = (
flags[i] == 1) ? 0 : 1;
106 for (
int ilocal = 0; ilocal <
fitobjects[i]->getNPar(); ilocal++) {
108 int iglobal =
fitobjects[i]->getGlobalParNum(ilocal);
109 assert(iglobal >= 0 && iglobal < idim);
110 if (m2[index] != 0) {
111 der[iglobal] = totE[index] *
fitobjects[i]->getDE(ilocal)
112 - totpx[index] *
fitobjects[i]->getDPx(ilocal)
113 - totpy[index] *
fitobjects[i]->getDPy(ilocal)
114 - totpz[index] *
fitobjects[i]->getDPz(ilocal);
115 der[iglobal] *= m_inv[index];
116 }
else der[iglobal] = 1;
117 if (index == 1) der[iglobal] *= -1.;
129 for (
unsigned int i = 0; i <
fitobjects.size(); i++) {
130 if (
flags[i] == flag) {
137 return std::sqrt(std::abs(totE * totE - totpx * totpx - totpy * totpy - totpz * totpz));
147 B2DEBUG(14,
"SoftGaussMassConstraint::secondDerivatives: i=" << i <<
", j=" << j);
148 int index = (
flags[i] == 1) ? 0 : 1;
149 int jndex = (
flags[j] == 1) ? 0 : 1;
150 if (index != jndex)
return false;
155 for (
unsigned int k = 0; k <
fitobjects.size(); ++k) {
156 int kndex = (
flags[k] == 1) ? 0 : 1;
159 if (index == kndex) {
161 totpx += fok->
getPx();
162 totpy += fok->
getPy();
163 totpz += fok->
getPz();
168 B2ERROR(
"SoftGaussMassConstraint::secondDerivatives: totE = " << totE);
171 double m2 = std::abs(totE * totE - totpx * totpx - totpy * totpy - totpz * totpz);
172 double m = std::sqrt(m2);
174 double minv3 = 1 / (m * m * m);
176 assert(dderivatives);
177 for (
int k = 0; k < 16; ++k) dderivatives[k] = 0;
178 dderivatives[4 * 0 + 0] = (m2 - totE * totE) * minv3;
179 dderivatives[4 * 0 + 1] = dderivatives[4 * 1 + 0] = totE * totpx * minv3;
180 dderivatives[4 * 0 + 2] = dderivatives[4 * 2 + 0] = totE * totpy * minv3;
181 dderivatives[4 * 0 + 3] = dderivatives[4 * 3 + 0] = totE * totpz * minv3;
182 dderivatives[4 * 1 + 1] = -(m2 + totpx * totpx) * minv3;
183 dderivatives[4 * 1 + 2] = dderivatives[4 * 2 + 1] = -totpx * totpy * minv3;
184 dderivatives[4 * 1 + 3] = dderivatives[4 * 3 + 1] = -totpx * totpz * minv3;
185 dderivatives[4 * 2 + 2] = -(m2 + totpy * totpy) * minv3;
186 dderivatives[4 * 2 + 3] = dderivatives[4 * 3 + 2] = -totpy * totpz * minv3;
187 dderivatives[4 * 3 + 3] = -(m2 + totpz * totpz) * minv3;
197 int index = (
flags[i] == 1) ? 0 : 1;
198 for (
unsigned int j = 0; j <
fitobjects.size(); ++j) {
199 int jndex = (
flags[j] == 1) ? 0 : 1;
202 if (index == jndex) {
204 totpx += foj->getPx();
205 totpy += foj->getPy();
206 totpz += foj->getPz();
211 B2ERROR(
"SoftGaussMassConstraint::firstDerivatives: totE = " << totE);
214 double m = std::sqrt(std::abs(totE * totE - totpx * totpx - totpy * totpy - totpz * totpz));
217 dderivatives[0] = totE / m;
218 dderivatives[1] = -totpx / m;
219 dderivatives[2] = -totpy / m;
220 dderivatives[3] = -totpz / m;