243 {
244 float t[3] = {0.0f, 0.0f, 0.0f};
245 constexpr float kStep = 0.01f;
246 constexpr int kIters = 500;
247 for (int it = 0; it < kIters; ++it) {
248 float r[3];
249 r[0] = M[0][0]*
t[0] + M[0][1]*
t[1] + M[0][2]*
t[2] - b[0];
250 r[1] = M[1][0]*
t[0] + M[1][1]*
t[1] + M[1][2]*
t[2] - b[1];
251 r[2] = M[2][0]*
t[0] + M[2][1]*
t[1] + M[2][2]*
t[2] - b[2];
252
254 g[0] = M[0][0]*r[0] + M[1][0]*r[1] + M[2][0]*r[2];
255 g[1] = M[0][1]*r[0] + M[1][1]*r[1] + M[2][1]*r[2];
256 g[2] = M[0][2]*r[0] + M[1][2]*r[1] + M[2][2]*r[2];
257 for (int j = 0; j < 3; ++j) {
258 float v =
t[j] - kStep *
g[j];
259 t[j] = v > 0.0f ? v : 0.0f;
260 }
261 }
262 if (residual_out != nullptr) {
263 float r[3];
264 r[0] = M[0][0]*
t[0] + M[0][1]*
t[1] + M[0][2]*
t[2] - b[0];
265 r[1] = M[1][0]*
t[0] + M[1][1]*
t[1] + M[1][2]*
t[2] - b[1];
266 r[2] = M[2][0]*
t[0] + M[2][1]*
t[1] + M[2][2]*
t[2] - b[2];
267 *residual_out =
fl::sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
268 }
269 t_out[0] =
t[0]; t_out[1] =
t[1]; t_out[2] =
t[2];
270}
constexpr enable_if< is_fixed_point< T >::value, T >::type sqrt(T x) FL_NO_EXCEPT