227 {
228 float t[3] = {0.0f, 0.0f, 0.0f};
229 constexpr float kStep = 0.01f;
230 constexpr int kIters = 500;
231 for (int it = 0; it < kIters; ++it) {
232 float r[3];
233 r[0] = M[0][0]*
t[0] + M[0][1]*
t[1] + M[0][2]*
t[2] - b[0];
234 r[1] = M[1][0]*
t[0] + M[1][1]*
t[1] + M[1][2]*
t[2] - b[1];
235 r[2] = M[2][0]*
t[0] + M[2][1]*
t[1] + M[2][2]*
t[2] - b[2];
236
237 float g[3];
238 g[0] = M[0][0]*r[0] + M[1][0]*r[1] + M[2][0]*r[2];
239 g[1] = M[0][1]*r[0] + M[1][1]*r[1] + M[2][1]*r[2];
240 g[2] = M[0][2]*r[0] + M[1][2]*r[1] + M[2][2]*r[2];
241 for (int j = 0; j < 3; ++j) {
242 float v =
t[j] - kStep * g[j];
243 t[j] = v > 0.0f ? v : 0.0f;
244 }
245 }
246 if (residual_out != nullptr) {
247 float r[3];
248 r[0] = M[0][0]*
t[0] + M[0][1]*
t[1] + M[0][2]*
t[2] - b[0];
249 r[1] = M[1][0]*
t[0] + M[1][1]*
t[1] + M[1][2]*
t[2] - b[1];
250 r[2] = M[2][0]*
t[0] + M[2][1]*
t[1] + M[2][2]*
t[2] - b[2];
251 *residual_out =
fl::sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
252 }
253 t_out[0] =
t[0]; t_out[1] =
t[1]; t_out[2] =
t[2];
254}
constexpr enable_if< is_fixed_point< T >::value, T >::type sqrt(T x) FL_NOEXCEPT