diff options
Diffstat (limited to '')
-rw-r--r-- | src/3rdparty/2geom/include/2geom/numeric/symmetric-matrix-fs-trace.h | 427 |
1 files changed, 427 insertions, 0 deletions
diff --git a/src/3rdparty/2geom/include/2geom/numeric/symmetric-matrix-fs-trace.h b/src/3rdparty/2geom/include/2geom/numeric/symmetric-matrix-fs-trace.h new file mode 100644 index 0000000..0e7a28c --- /dev/null +++ b/src/3rdparty/2geom/include/2geom/numeric/symmetric-matrix-fs-trace.h @@ -0,0 +1,427 @@ +/* + * SymmetricMatrix trace + * + * Authors: + * Marco Cecchetti <mrcekets at gmail.com> + * + * Copyright 2009 authors + * + * This library is free software; you can redistribute it and/or + * modify it either under the terms of the GNU Lesser General Public + * License version 2.1 as published by the Free Software Foundation + * (the "LGPL") or, at your option, under the terms of the Mozilla + * Public License Version 1.1 (the "MPL"). If you do not alter this + * notice, a recipient may use your version of this file under either + * the MPL or the LGPL. + * + * You should have received a copy of the LGPL along with this library + * in the file COPYING-LGPL-2.1; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * You should have received a copy of the MPL along with this library + * in the file COPYING-MPL-1.1 + * + * The contents of this file are subject to the Mozilla Public License + * Version 1.1 (the "License"); you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://www.mozilla.org/MPL/ + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY + * OF ANY KIND, either express or implied. See the LGPL or the MPL for + * the specific language governing rights and limitations. + */ + + +#ifndef _NL_TRACE_H_ +#define _NL_TRACE_H_ + + +#include <2geom/numeric/matrix.h> +#include <2geom/numeric/symmetric-matrix-fs.h> + + + + + +namespace Geom { namespace NL { + + +namespace detail +{ + +/* + * helper routines + */ + +inline +int sgn_prod (int x, int y) +{ + if (x == 0 || y == 0) return 0; + if (x == y) return 1; + return -1; +} + +inline +bool abs_less (double x, double y) +{ + return (std::fabs(x) < std::fabs(y)); +} + + +/* + * trace K-th of symmetric matrix S of order N + */ +template <size_t K, size_t N> +struct trace +{ + static double evaluate(const ConstBaseSymmetricMatrix<N> &S); +}; + +template <size_t N> +struct trace<1,N> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<N> & S) + { + double t = 0; + for (size_t i = 0; i < N; ++i) + { + t += S(i,i); + } + return t; + } +}; + +template <size_t N> +struct trace<N,N> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<N> & S) + { + Matrix M(S); + return det(M); + } +}; + +/* + * trace for symmetric matrix of order 2 + */ +template <> +struct trace<1,2> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<2> & S) + { + return (S.get<0,0>() + S.get<1,1>()); + } +}; + +template <> +struct trace<2,2> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<2> & S) + { + return (S.get<0,0>() * S.get<1,1>() - S.get<0,1>() * S.get<1,0>()); + } +}; + + +/* + * trace for symmetric matrix of order 3 + */ +template <> +struct trace<1,3> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<3> & S) + { + return (S.get<0,0>() + S.get<1,1>() + S.get<2,2>()); + } +}; + +template <> +struct trace<2,3> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<3> & S) + { + double a00 = S.get<1,1>() * S.get<2,2>() - S.get<1,2>() * S.get<2,1>(); + double a11 = S.get<0,0>() * S.get<2,2>() - S.get<0,2>() * S.get<2,0>(); + double a22 = S.get<0,0>() * S.get<1,1>() - S.get<0,1>() * S.get<1,0>(); + return (a00 + a11 + a22); + } +}; + +template <> +struct trace<3,3> +{ + static + double evaluate (const ConstBaseSymmetricMatrix<3> & S) + { + double d = S.get<0,0>() * S.get<1,1>() * S.get<2,2>(); + d += (2 * S.get<1,0>() * S.get<2,0>() * S.get<2,1>()); + d -= (S.get<0,0>() * S.get<2,1>() * S.get<2,1>()); + d -= (S.get<1,1>() * S.get<2,0>() * S.get<2,0>()); + d -= (S.get<2,2>() * S.get<1,0>() * S.get<1,0>()); + return d; + } +}; + + +/* + * sign of trace K-th + */ +template <size_t K, size_t N> +struct trace_sgn +{ + static + int evaluate (const ConstBaseSymmetricMatrix<N> & S) + { + double d = trace<K, N>::evaluate(S); + return sgn(d); + } +}; + + +/* + * sign of trace for symmetric matrix of order 2 + */ +template <> +struct trace_sgn<2,2> +{ + static + int evaluate (const ConstBaseSymmetricMatrix<2> & S) + { + double m00 = S.get<0,0>(); + double m10 = S.get<1,0>(); + double m11 = S.get<1,1>(); + + int sm00 = sgn (m00); + int sm10 = sgn (m10); + int sm11 = sgn (m11); + + if (sm10 == 0) + { + return sgn_prod (sm00, sm11); + } + else + { + int sm00m11 = sgn_prod (sm00, sm11); + if (sm00m11 == 1) + { + int e00, e10, e11; + double f00 = std::frexp (m00, &e00); + double f10 = std::frexp (m10, &e10); + double f11 = std::frexp (m11, &e11); + + int e0011 = e00 + e11; + int e1010 = e10 << 1; + int ed = e0011 - e1010; + + if (ed > 1) + { + return 1; + } + else if (ed < -1) + { + return -1; + } + else + { + double d = std::ldexp (f00 * f11, ed) - f10 * f10; + //std::cout << "trace_sgn<2,2>: det = " << d << std::endl; + double eps = std::ldexp (1, -50); + if (std::fabs(d) < eps) return 0; + return sgn (d); + } + } + return -1; + } + } +}; + + +/* + * sign of trace for symmetric matrix of order 3 + */ +template <> +struct trace_sgn<2,3> +{ + static + int evaluate (const ConstBaseSymmetricMatrix<3> & S) + { + double eps = std::ldexp (1, -50); + double t[6]; + + t[0] = S.get<1,1>() * S.get<2,2>(); + t[1] = - S.get<1,2>() * S.get<2,1>(); + t[2] = S.get<0,0>() * S.get<2,2>(); + t[3] = - S.get<0,2>() * S.get<2,0>(); + t[4] = S.get<0,0>() * S.get<1,1>(); + t[5] = - S.get<0,1>() * S.get<1,0>(); + + + double* maxp = std::max_element (t, t+6, abs_less); + int em; + std::frexp(*maxp, &em); + double d = 0; + for (double i : t) + { + d += i; + } + double r = std::fabs (std::ldexp (d, -em)); // relative error + //std::cout << "trace_sgn<2,3>: d = " << d << std::endl; + //std::cout << "trace_sgn<2,3>: r = " << r << std::endl; + if (r < eps) return 0; + if (d > 0) return 1; + return -1; + } +}; + +template <> +struct trace_sgn<3,3> +{ + static + int evaluate (const ConstBaseSymmetricMatrix<3> & S) + { + + double eps = std::ldexp (1, -48); + double t[5]; + + t[0] = S.get<0,0>() * S.get<1,1>() * S.get<2,2>(); + t[1] = 2 * S.get<1,0>() * S.get<2,0>() * S.get<2,1>(); + t[2] = -(S.get<0,0>() * S.get<2,1>() * S.get<2,1>()); + t[3] = -(S.get<1,1>() * S.get<2,0>() * S.get<2,0>()); + t[4] = -(S.get<2,2>() * S.get<1,0>() * S.get<1,0>()); + + double* maxp = std::max_element (t, t+5, abs_less); + int em; + std::frexp(*maxp, &em); + double d = 0; + for (double i : t) + { + d += i; + } + //std::cout << "trace_sgn<3,3>: d = " << d << std::endl; + double r = std::fabs (std::ldexp (d, -em)); // relative error + //std::cout << "trace_sgn<3,3>: r = " << r << std::endl; + + if (r < eps) return 0; + if (d > 0) return 1; + return -1; + } +}; // end struct trace_sgn<3,3> + +} // end namespace detail + + +template <size_t K, size_t N> +inline +double trace (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace<K, N>::evaluate(_matrix); +} + +template <size_t N> +inline +double trace (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace<1, N>::evaluate(_matrix); +} + +template <size_t N> +inline +double det (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace<N, N>::evaluate(_matrix); +} + + +template <size_t K, size_t N> +inline +int trace_sgn (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace_sgn<K, N>::evaluate(_matrix); +} + +template <size_t N> +inline +int trace_sgn (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace_sgn<1, N>::evaluate(_matrix); +} + +template <size_t N> +inline +int det_sgn (const ConstBaseSymmetricMatrix<N> & _matrix) +{ + return detail::trace_sgn<N, N>::evaluate(_matrix); +} + +/* +template <size_t N> +inline +size_t rank (const ConstBaseSymmetricMatrix<N> & S) +{ + THROW_NOTIMPLEMENTED(); + return 0; +} + +template <> +inline +size_t rank<2> (const ConstBaseSymmetricMatrix<2> & S) +{ + if (S.is_zero()) return 0; + double d = S.get<0,0>() * S.get<1,1>() - S.get<0,1>() * S.get<1,0>(); + if (d != 0) return 2; + return 1; +} + +template <> +inline +size_t rank<3> (const ConstBaseSymmetricMatrix<3> & S) +{ + if (S.is_zero()) return 0; + + double a20 = S.get<0,1>() * S.get<1,2>() - S.get<0,2>() * S.get<1,1>(); + double a21 = S.get<0,2>() * S.get<1,0>() - S.get<0,0>() * S.get<1,2>(); + double a22 = S.get<0,0>() * S.get<1,1>() - S.get<0,1>() * S.get<1,0>(); + double d = a20 * S.get<2,0>() + a21 * S.get<2,1>() + a22 * S.get<2,2>(); + + if (d != 0) return 3; + + if (a20 != 0 || a21 != 0 || a22 != 0) return 2; + + double a00 = S.get<1,1>() * S.get<2,2>() - S.get<1,2>() * S.get<2,1>(); + if (a00 != 0) return 2; + + double a10 = S.get<0,2>() * S.get<2,1>() - S.get<0,1>() * S.get<2,2>(); + if (a10 != 0) return 2; + + double a11 = S.get<0,0>() * S.get<2,2>() - S.get<0,2>() * S.get<2,0>(); + if (a11 != 0) return 2; + + return 1; +} +*/ + +} /* end namespace NL*/ } /* end namespace Geom*/ + + + + +#endif // _NL_TRACE_H_ + + + + +/* + Local Variables: + mode:c++ + c-file-style:"stroustrup" + c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +)) + indent-tabs-mode:nil + fill-column:99 + End: +*/ +// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 : |