1 files changed, 427 insertions, 0 deletions
diff --git a/include/2geom/numeric/symmetric-matrix-fs-trace.h b/include/2geom/numeric/symmetric-matrix-fs-trace.h
new file mode 100644
index 0000000..0e7a28c
--- /dev/null
+++ b/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 :