<commit_before>/*
* Copyright (C) 2019 German Aerospace Center (DLR/SC)
*
* Created: 2019-09-30 Martin Siggel <Martin.Siggel@dlr.de>
*
* Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include "CTiglIntersectBSplines.h"
#include "CTiglBSplineAlgorithms.h"
#include "CTiglLineSegment.h"
#include "tiglcommonfunctions.h"

#include <math_MultipleVarFunctionWithGradient.hxx>
#include <math_BFGS.hxx>

#include <limits>
#include <list>
#include <algorithm>

#include <cassert>

#include <Standard_Version.hxx>

namespace
{
    template <typename T>
    T maxval(const T& v1, const T& v2)
    {
        return v1 > v2 ? v1 : v2;
    }

    template <typename T>
    T sqr(const T& v)
    {
        return v*v;
    }

    tigl::CTiglPoint minCoords(const tigl::CTiglPoint& p1, const tigl::CTiglPoint& p2)
    {
        tigl::CTiglPoint result = p1;
        result.x = p1.x < p2.x? p1.x : p2.x;
        result.y = p1.y < p2.y? p1.y : p2.y;
        result.z = p1.z < p2.z? p1.z : p2.z;
        return result;

    }

    tigl::CTiglPoint maxCoords(const tigl::CTiglPoint& p1, const tigl::CTiglPoint& p2)
    {
        tigl::CTiglPoint result = p1;
        result.x = p1.x > p2.x? p1.x : p2.x;
        result.y = p1.y > p2.y? p1.y : p2.y;
        result.z = p1.z > p2.z? p1.z : p2.z;
        return result;

    }

    class Intervall
    {
    public:
        Intervall(double mmin, double mmax)
            : min(mmin), max(mmax)
        {}
        double min;
        double max;

        bool operator==(const Intervall& other) const
        {
            const double EPS = 1e-15;
            return fabs(min - other.min) < EPS && fabs(max - other.max) < EPS;
        }
    };

    class BoundingBox
    {
    public:
        BoundingBox(const Handle(Geom_BSplineCurve) curve)
            : range(curve->FirstParameter(), curve->LastParameter())
        {
            low.x = low.y = low.z = std::numeric_limits<double>::max();
            high.x = high.y = high.z = -std::numeric_limits<double>::max();
            // compute min / max from control points
            for (Standard_Integer i = 1; i <= curve->NbPoles(); ++i) {
                gp_XYZ p = curve->Pole(i).XYZ();
                low = minCoords(low, p);
                high = maxCoords(high, p);
            }
        }

        bool Intersects(const BoundingBox& other, double eps) const
        {
            tigl::CTiglPoint min = maxCoords(low, other.low);
            tigl::CTiglPoint max = minCoords(high, other.high);
            return (min.x < max.x + eps) && (min.y < max.y + eps) && (min.z < max.z + eps);
        }

        BoundingBox& Merge(const BoundingBox& other)
        {
            assert(range.max == other.range.min);
            range.max = other.range.max;
            high = maxCoords(high, other.high);
            low = minCoords(low, other.low);

            return *this;
        }

        bool operator==(const BoundingBox& other) const
        {
            return range == other.range;
        }

        tigl::CTiglPoint low, high;
        Intervall range;
    };

    // Computes the total curvature of the curve
    // A curvature of 1 is equivalent to a straight line
    double curvature(const Handle(Geom_BSplineCurve)& curve)
    {
        double len = curve->Pole(1).Distance(curve->Pole(curve->NbPoles()));
        double total = 0.;
        for (Standard_Integer i = 1; i < curve->NbPoles(); ++i) {
            gp_Pnt p1 = curve->Pole(i);
            gp_Pnt p2 = curve->Pole(i+1);
            double dist = p1.Distance(p2);
            total += dist;
        }

        return total / len;
    }

    struct BoundingBoxPair
    {
        BoundingBoxPair (const BoundingBox& i1, const BoundingBox& i2)
            : b1(i1), b2(i2)
        {}
        BoundingBox b1;
        BoundingBox b2;
    };

    /// Computes possible ranges of intersections by a bracketing approach
    std::list<BoundingBoxPair> getRangesOfIntersection(const Handle(Geom_BSplineCurve) curve1, const Handle(Geom_BSplineCurve) curve2, double tolerance)
    {
        BoundingBox h1(curve1);
        BoundingBox h2(curve2);

        if (!h1.Intersects(h2, tolerance)) {
            return {};
        }

        double c1_curvature = curvature(curve1);
        double c2_curvature = curvature(curve2);
        double max_curvature = 1.0005;

        if (c1_curvature <= max_curvature && c2_curvature <= max_curvature) {
            // both curves are now almost linear. check approximate distance between line segments
            tigl::CTiglLineSegment l1(curve1->Pole(1).XYZ(), curve1->Pole(curve1->NbPoles()).XYZ());
            tigl::CTiglLineSegment l2(curve2->Pole(1).XYZ(), curve2->Pole(curve2->NbPoles()).XYZ());

            if (l1.distance(l2) < tolerance){
                return {BoundingBoxPair(h1, h2)};
            }
            else {
                return {};
            }
        }

        double curve1MidParm = 0.5*(curve1->FirstParameter() + curve1->LastParameter());
        double curve2MidParm = 0.5*(curve2->FirstParameter() + curve2->LastParameter());

        if (c1_curvature > max_curvature && c2_curvature > max_curvature) {
            Handle_Geom_BSplineCurve c11 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1->FirstParameter(), curve1MidParm);
            Handle_Geom_BSplineCurve c12 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1MidParm, curve1->LastParameter());

            Handle_Geom_BSplineCurve c21 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2->FirstParameter(), curve2MidParm);
            Handle_Geom_BSplineCurve c22 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2MidParm, curve2->LastParameter());

            auto result1 = getRangesOfIntersection(c11, c21, tolerance);
            auto result2 = getRangesOfIntersection(c11, c22, tolerance);
            auto result3 = getRangesOfIntersection(c12, c21, tolerance);
            auto result4 = getRangesOfIntersection(c12, c22, tolerance);

            // append all results
            result1.splice(std::begin(result1), result2);
            result1.splice(std::begin(result1), result3);
            result1.splice(std::begin(result1), result4);

            return result1;
        }
        else if (c1_curvature <= max_curvature && max_curvature < c2_curvature) {
            Handle_Geom_BSplineCurve c21 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2->FirstParameter(), curve2MidParm);
            Handle_Geom_BSplineCurve c22 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2MidParm, curve2->LastParameter());

            auto result1 = getRangesOfIntersection(curve1, c21, tolerance);
            auto result2 = getRangesOfIntersection(curve1, c22, tolerance);

            result1.splice(std::begin(result1), result2);
            return result1;
        }
        else if (c2_curvature <= max_curvature && max_curvature < c1_curvature) {
            Handle_Geom_BSplineCurve c11 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1->FirstParameter(), curve1MidParm);
            Handle_Geom_BSplineCurve c12 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1MidParm, curve1->LastParameter());

            auto result1 = getRangesOfIntersection(c11, curve2, tolerance);
            auto result2 = getRangesOfIntersection(c12, curve2, tolerance);

            result1.splice(std::begin(result1), result2);
            return result1;
        }

        return {};
    }

    class CurveCurveDistanceObjective : public math_MultipleVarFunctionWithGradient
    {
    public:
        CurveCurveDistanceObjective(const Handle(Geom_Curve)& c1, const Handle(Geom_Curve)& c2)
            : m_c1(c1), m_c2(c2)
        {}

        virtual Standard_Integer NbVariables()  const override
        {
            return 2;
        }

        Standard_Boolean Value (const math_Vector& X, Standard_Real& F) override
        {
            math_Vector G(1, 2);
            return Values(X, F, G);
        }

        Standard_Boolean Gradient (const math_Vector& X, math_Vector& G) override
        {
            Standard_Real F = 0.;
            return Values(X, F, G);
        }

        virtual  Standard_Boolean Values (const math_Vector& X, Standard_Real& F, math_Vector& G) override
        {
            double u = X.Value(1);
            double v = X.Value(2);

            gp_Pnt p1, p2;
            gp_Vec d1, d2;
            m_c1->D1(u, p1, d1);
            m_c2->D1(v, p2, d2);

            gp_Vec diff = p1.XYZ() - p2.XYZ();
            F = diff.SquareMagnitude();
            G(1) = 2. * diff.Dot(d1);
            G(2) = -2. * diff.Dot(d2);

#if OCC_VERSION_HEX < VERSION_HEX_CODE(7,2,0)
            // OCCT < 7.2.0 does not support bound bfgs optimization. Hence, we add soft constraints instead by
            // adding a large penalty of 1e7, if the variables go outside the valid range
            double u_penalty = sqr(maxval(0., m_c1->FirstParameter() - u)) + sqr(maxval(0., u - m_c1->LastParameter()));
            double v_penalty = sqr(maxval(0., m_c2->FirstParameter() - v)) + sqr(maxval(0., v - m_c2->LastParameter()));

            double d_u_penalty = -2. * maxval(0., m_c1->FirstParameter() - u) + 2.*maxval(0., u - m_c1->LastParameter());
            double d_v_penalty = -2. * maxval(0., m_c2->FirstParameter() - v) + 2.*maxval(0., v - m_c2->LastParameter());

            double fac = 1e7;
            F += fac*(u_penalty + v_penalty);
            G(1) += fac*d_u_penalty;
            G(2) += fac*d_v_penalty;
#endif
            return true;
        }

    private:
        const Handle(Geom_Curve) m_c1, m_c2;
    };

} // namespace

namespace tigl
{

std::vector<tigl::CurveIntersectionResult> IntersectBSplines(const Handle(Geom_BSplineCurve) curve1, const Handle(Geom_BSplineCurve) curve2, double tolerance)
{
    auto hulls = getRangesOfIntersection(curve1, curve2, tolerance);

    std::list<BoundingBox> curve1_ints, curve2_ints;
    for (const auto& hull : hulls) {
        curve1_ints.push_back(hull.b1);
        curve2_ints.push_back(hull.b2);
    }

    auto compare = [](const BoundingBox& b1, const BoundingBox& b2) {
        return b1.range.min < b2.range.min;
    };

    // sort ascending parameter
    curve1_ints.sort(compare);
    curve2_ints.sort(compare);

    // Remove duplicates
    curve1_ints.unique();
    curve2_ints.unique();

    auto is_adjacent = [](const BoundingBox& b1, const BoundingBox& b2) {
        const double EPS = 1e-15;
        return fabs(b1.range.max - b2.range.min) < EPS;
    };

    auto merge_boxes = [](const BoundingBox& b1, const BoundingBox& b2) {
        BoundingBox result(b1);
        return result.Merge(b2);
    };

    // merge neighboring intervals
    ReplaceAdjacentWithMerged(curve1_ints, is_adjacent, merge_boxes);
    ReplaceAdjacentWithMerged(curve2_ints, is_adjacent, merge_boxes);

    // combine intersection intervals
    std::vector<BoundingBoxPair> intersectionCandidates;
    for (const BoundingBox& b1 : curve1_ints) {
        for (const BoundingBox& b2 : curve2_ints) {
            if (b1.Intersects(b2, tolerance)) {
                intersectionCandidates.push_back(BoundingBoxPair(b1, b2));
            }
        }
    }

    CurveCurveDistanceObjective obj(curve1, curve2);

    std::vector<tigl::CurveIntersectionResult> results;

    for (const BoundingBoxPair& boxes : intersectionCandidates) {
        math_Vector guess(1, 2);
        guess(1) = 0.5*(boxes.b1.range.min + boxes.b1.range.max);
        guess(2) = 0.5*(boxes.b2.range.min + boxes.b2.range.max);

#if OCC_VERSION_HEX >= VERSION_HEX_CODE(6,9,1)
        math_BFGS optimizer(obj.NbVariables(), 1e-8, 50, 1e-8);

  #if OCC_VERSION_HEX >= VERSION_HEX_CODE(7,2,0)
        // OCCT Added bound optimization with 7.2.0
        math_Vector lb(1, 2), ub(1, 2);
        lb(1) = boxes.b1.range.min;
        lb(2) = boxes.b2.range.min;
        ub(1) = boxes.b1.range.max;
        ub(2) = boxes.b2.range.max;
        optimizer.SetBoundary(lb, ub);
  #endif
        optimizer.Perform(obj, guess);
#else
        math_BFGS optimizer(obj, guess, 1e-12);
#endif

        double u = optimizer.Location().Value(1);
        double v = optimizer.Location().Value(2);

#if OCC_VERSION_HEX < VERSION_HEX_CODE(7,2,0)
        u = Clamp(u, curve1->FirstParameter(), curve1->LastParameter());
        v = Clamp(v, curve2->FirstParameter(), curve2->LastParameter());

#else
        assert(lb(1) <= u && u <= ub(1));
        assert(lb(2) <= v && v <= ub(2));
#endif

        CurveIntersectionResult result;
        result.parmOnCurve1 = u;
        result.parmOnCurve2 = v;
        result.point = (CTiglPoint(curve1->Value(u).XYZ()) + CTiglPoint(curve2->Value(v).XYZ()))*0.5;

        results.push_back(result);
    }

    return results;
}

} // namespace tigl
<commit_msg>Added comments to bspline intersection<commit_after>/*
* Copyright (C) 2019 German Aerospace Center (DLR/SC)
*
* Created: 2019-09-30 Martin Siggel <Martin.Siggel@dlr.de>
*
* Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include "CTiglIntersectBSplines.h"
#include "CTiglBSplineAlgorithms.h"
#include "CTiglLineSegment.h"
#include "tiglcommonfunctions.h"

#include <math_MultipleVarFunctionWithGradient.hxx>
#include <math_BFGS.hxx>

#include <limits>
#include <list>
#include <algorithm>

#include <cassert>

#include <Standard_Version.hxx>

namespace
{
    template <typename T>
    T maxval(const T& v1, const T& v2)
    {
        return v1 > v2 ? v1 : v2;
    }

    template <typename T>
    T sqr(const T& v)
    {
        return v*v;
    }

    tigl::CTiglPoint minCoords(const tigl::CTiglPoint& p1, const tigl::CTiglPoint& p2)
    {
        tigl::CTiglPoint result = p1;
        result.x = p1.x < p2.x? p1.x : p2.x;
        result.y = p1.y < p2.y? p1.y : p2.y;
        result.z = p1.z < p2.z? p1.z : p2.z;
        return result;

    }

    tigl::CTiglPoint maxCoords(const tigl::CTiglPoint& p1, const tigl::CTiglPoint& p2)
    {
        tigl::CTiglPoint result = p1;
        result.x = p1.x > p2.x? p1.x : p2.x;
        result.y = p1.y > p2.y? p1.y : p2.y;
        result.z = p1.z > p2.z? p1.z : p2.z;
        return result;

    }

    class Intervall
    {
    public:
        Intervall(double mmin, double mmax)
            : min(mmin), max(mmax)
        {}
        double min;
        double max;

        bool operator==(const Intervall& other) const
        {
            const double EPS = 1e-15;
            return fabs(min - other.min) < EPS && fabs(max - other.max) < EPS;
        }
    };

    class BoundingBox
    {
    public:
        BoundingBox(const Handle(Geom_BSplineCurve) curve)
            : range(curve->FirstParameter(), curve->LastParameter())
        {
            low.x = low.y = low.z = std::numeric_limits<double>::max();
            high.x = high.y = high.z = -std::numeric_limits<double>::max();
            // compute min / max from control points
            for (Standard_Integer i = 1; i <= curve->NbPoles(); ++i) {
                gp_XYZ p = curve->Pole(i).XYZ();
                low = minCoords(low, p);
                high = maxCoords(high, p);
            }
        }

        bool Intersects(const BoundingBox& other, double eps) const
        {
            tigl::CTiglPoint min = maxCoords(low, other.low);
            tigl::CTiglPoint max = minCoords(high, other.high);
            return (min.x < max.x + eps) && (min.y < max.y + eps) && (min.z < max.z + eps);
        }

        BoundingBox& Merge(const BoundingBox& other)
        {
            assert(range.max == other.range.min);
            range.max = other.range.max;
            high = maxCoords(high, other.high);
            low = minCoords(low, other.low);

            return *this;
        }

        bool operator==(const BoundingBox& other) const
        {
            return range == other.range;
        }

        tigl::CTiglPoint low, high;
        Intervall range;
    };

    // Computes the total curvature of the curve
    // A curvature of 1 is equivalent to a straight line
    double curvature(const Handle(Geom_BSplineCurve)& curve)
    {
        double len = curve->Pole(1).Distance(curve->Pole(curve->NbPoles()));
        double total = 0.;
        for (Standard_Integer i = 1; i < curve->NbPoles(); ++i) {
            gp_Pnt p1 = curve->Pole(i);
            gp_Pnt p2 = curve->Pole(i+1);
            double dist = p1.Distance(p2);
            total += dist;
        }

        return total / len;
    }

    struct BoundingBoxPair
    {
        BoundingBoxPair (const BoundingBox& i1, const BoundingBox& i2)
            : b1(i1), b2(i2)
        {}
        BoundingBox b1;
        BoundingBox b2;
    };

    /// Computes possible ranges of intersections by a bracketing approach
    std::list<BoundingBoxPair> getRangesOfIntersection(const Handle(Geom_BSplineCurve) curve1, const Handle(Geom_BSplineCurve) curve2, double tolerance)
    {
        BoundingBox h1(curve1);
        BoundingBox h2(curve2);

        if (!h1.Intersects(h2, tolerance)) {
            // Bounding boxes do not intersect. No intersection possible
            return {};
        }

        double c1_curvature = curvature(curve1);
        double c2_curvature = curvature(curve2);
        double max_curvature = 1.0005;

        // If both curves are linear enough, we can stop refining
        if (c1_curvature <= max_curvature && c2_curvature <= max_curvature) {
            // both curves are now almost linear. check approximate distance between line segments
            tigl::CTiglLineSegment l1(curve1->Pole(1).XYZ(), curve1->Pole(curve1->NbPoles()).XYZ());
            tigl::CTiglLineSegment l2(curve2->Pole(1).XYZ(), curve2->Pole(curve2->NbPoles()).XYZ());

            if (l1.distance(l2) < tolerance){
                return {BoundingBoxPair(h1, h2)};
            }
            else {
                return {};
            }
        }

        double curve1MidParm = 0.5*(curve1->FirstParameter() + curve1->LastParameter());
        double curve2MidParm = 0.5*(curve2->FirstParameter() + curve2->LastParameter());

        if (c1_curvature > max_curvature && c2_curvature > max_curvature) {
            // Refine both curves by splitting them in the parametric center
            Handle_Geom_BSplineCurve c11 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1->FirstParameter(), curve1MidParm);
            Handle_Geom_BSplineCurve c12 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1MidParm, curve1->LastParameter());

            Handle_Geom_BSplineCurve c21 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2->FirstParameter(), curve2MidParm);
            Handle_Geom_BSplineCurve c22 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2MidParm, curve2->LastParameter());

            auto result1 = getRangesOfIntersection(c11, c21, tolerance);
            auto result2 = getRangesOfIntersection(c11, c22, tolerance);
            auto result3 = getRangesOfIntersection(c12, c21, tolerance);
            auto result4 = getRangesOfIntersection(c12, c22, tolerance);

            // append all results
            result1.splice(std::begin(result1), result2);
            result1.splice(std::begin(result1), result3);
            result1.splice(std::begin(result1), result4);

            return result1;
        }
        else if (c1_curvature <= max_curvature && max_curvature < c2_curvature) {
            // Refine only curve 2
            Handle_Geom_BSplineCurve c21 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2->FirstParameter(), curve2MidParm);
            Handle_Geom_BSplineCurve c22 = tigl::CTiglBSplineAlgorithms::trimCurve(curve2, curve2MidParm, curve2->LastParameter());

            auto result1 = getRangesOfIntersection(curve1, c21, tolerance);
            auto result2 = getRangesOfIntersection(curve1, c22, tolerance);

            result1.splice(std::begin(result1), result2);
            return result1;
        }
        else if (c2_curvature <= max_curvature && max_curvature < c1_curvature) {
            // Refine only curve 1
            Handle_Geom_BSplineCurve c11 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1->FirstParameter(), curve1MidParm);
            Handle_Geom_BSplineCurve c12 = tigl::CTiglBSplineAlgorithms::trimCurve(curve1, curve1MidParm, curve1->LastParameter());

            auto result1 = getRangesOfIntersection(c11, curve2, tolerance);
            auto result2 = getRangesOfIntersection(c12, curve2, tolerance);

            result1.splice(std::begin(result1), result2);
            return result1;
        }

        return {};
    }

    class CurveCurveDistanceObjective : public math_MultipleVarFunctionWithGradient
    {
    public:
        CurveCurveDistanceObjective(const Handle(Geom_Curve)& c1, const Handle(Geom_Curve)& c2)
            : m_c1(c1), m_c2(c2)
        {}

        virtual Standard_Integer NbVariables()  const override
        {
            return 2;
        }

        Standard_Boolean Value (const math_Vector& X, Standard_Real& F) override
        {
            math_Vector G(1, 2);
            return Values(X, F, G);
        }

        Standard_Boolean Gradient (const math_Vector& X, math_Vector& G) override
        {
            Standard_Real F = 0.;
            return Values(X, F, G);
        }

        virtual  Standard_Boolean Values (const math_Vector& X, Standard_Real& F, math_Vector& G) override
        {
            double u = X.Value(1);
            double v = X.Value(2);

            gp_Pnt p1, p2;
            gp_Vec d1, d2;
            m_c1->D1(u, p1, d1);
            m_c2->D1(v, p2, d2);

            gp_Vec diff = p1.XYZ() - p2.XYZ();
            F = diff.SquareMagnitude();
            G(1) = 2. * diff.Dot(d1);
            G(2) = -2. * diff.Dot(d2);

#if OCC_VERSION_HEX < VERSION_HEX_CODE(7,2,0)
            // OCCT < 7.2.0 does not support bound bfgs optimization. Hence, we add soft constraints instead by
            // adding a large penalty of 1e7, if the variables go outside the valid range
            double u_penalty = sqr(maxval(0., m_c1->FirstParameter() - u)) + sqr(maxval(0., u - m_c1->LastParameter()));
            double v_penalty = sqr(maxval(0., m_c2->FirstParameter() - v)) + sqr(maxval(0., v - m_c2->LastParameter()));

            double d_u_penalty = -2. * maxval(0., m_c1->FirstParameter() - u) + 2.*maxval(0., u - m_c1->LastParameter());
            double d_v_penalty = -2. * maxval(0., m_c2->FirstParameter() - v) + 2.*maxval(0., v - m_c2->LastParameter());

            double fac = 1e7;
            F += fac*(u_penalty + v_penalty);
            G(1) += fac*d_u_penalty;
            G(2) += fac*d_v_penalty;
#endif
            return true;
        }

    private:
        const Handle(Geom_Curve) m_c1, m_c2;
    };

} // namespace

namespace tigl
{

std::vector<tigl::CurveIntersectionResult> IntersectBSplines(const Handle(Geom_BSplineCurve) curve1, const Handle(Geom_BSplineCurve) curve2, double tolerance)
{
    auto hulls = getRangesOfIntersection(curve1, curve2, tolerance);

    std::list<BoundingBox> curve1_ints, curve2_ints;
    for (const auto& hull : hulls) {
        curve1_ints.push_back(hull.b1);
        curve2_ints.push_back(hull.b2);
    }

    auto compare = [](const BoundingBox& b1, const BoundingBox& b2) {
        return b1.range.min < b2.range.min;
    };

    // sort ascending parameter
    curve1_ints.sort(compare);
    curve2_ints.sort(compare);

    // Remove duplicates
    curve1_ints.unique();
    curve2_ints.unique();

    auto is_adjacent = [](const BoundingBox& b1, const BoundingBox& b2) {
        const double EPS = 1e-15;
        return fabs(b1.range.max - b2.range.min) < EPS;
    };

    auto merge_boxes = [](const BoundingBox& b1, const BoundingBox& b2) {
        BoundingBox result(b1);
        return result.Merge(b2);
    };

    // merge neighboring intervals
    ReplaceAdjacentWithMerged(curve1_ints, is_adjacent, merge_boxes);
    ReplaceAdjacentWithMerged(curve2_ints, is_adjacent, merge_boxes);

    // combine intersection intervals
    std::vector<BoundingBoxPair> intersectionCandidates;
    for (const BoundingBox& b1 : curve1_ints) {
        for (const BoundingBox& b2 : curve2_ints) {
            if (b1.Intersects(b2, tolerance)) {
                intersectionCandidates.push_back(BoundingBoxPair(b1, b2));
            }
        }
    }

    CurveCurveDistanceObjective obj(curve1, curve2);

    std::vector<tigl::CurveIntersectionResult> results;

    for (const BoundingBoxPair& boxes : intersectionCandidates) {
        math_Vector guess(1, 2);
        guess(1) = 0.5*(boxes.b1.range.min + boxes.b1.range.max);
        guess(2) = 0.5*(boxes.b2.range.min + boxes.b2.range.max);

#if OCC_VERSION_HEX >= VERSION_HEX_CODE(6,9,1)
        math_BFGS optimizer(obj.NbVariables(), 1e-8, 50, 1e-8);

  #if OCC_VERSION_HEX >= VERSION_HEX_CODE(7,2,0)
        // OCCT Added bound optimization with 7.2.0
        math_Vector lb(1, 2), ub(1, 2);
        lb(1) = boxes.b1.range.min;
        lb(2) = boxes.b2.range.min;
        ub(1) = boxes.b1.range.max;
        ub(2) = boxes.b2.range.max;
        optimizer.SetBoundary(lb, ub);
  #endif
        optimizer.Perform(obj, guess);
#else
        math_BFGS optimizer(obj, guess, 1e-12);
#endif

        double u = optimizer.Location().Value(1);
        double v = optimizer.Location().Value(2);

#if OCC_VERSION_HEX < VERSION_HEX_CODE(7,2,0)
        u = Clamp(u, curve1->FirstParameter(), curve1->LastParameter());
        v = Clamp(v, curve2->FirstParameter(), curve2->LastParameter());

#else
        assert(lb(1) <= u && u <= ub(1));
        assert(lb(2) <= v && v <= ub(2));
#endif

        CurveIntersectionResult result;
        result.parmOnCurve1 = u;
        result.parmOnCurve2 = v;
        result.point = (CTiglPoint(curve1->Value(u).XYZ()) + CTiglPoint(curve2->Value(v).XYZ()))*0.5;

        results.push_back(result);
    }

    return results;
}

} // namespace tigl
<|endoftext|>


<commit_before>/*++
Copyright (c) 2013 Microsoft Corporation

Module Name:

    duality_dl_interface.cpp

Abstract:

    SMT2 interface for Duality

Author:

    Krystof Hoder (t-khoder) 2011-9-22.
    Modified by Ken McMIllan (kenmcmil) 2013-4-18.

Revision History:

--*/

#include "dl_cmds.h"
#include "dl_context.h"
#include "dl_mk_coi_filter.h"
#include "dl_mk_interp_tail_simplifier.h"
#include "dl_mk_subsumption_checker.h"
#include "dl_mk_rule_inliner.h"
#include "dl_rule.h"
#include "dl_rule_transformer.h"
#include "dl_mk_extract_quantifiers.h"
#include "smt2parser.h"
#include "duality_dl_interface.h"
#include "dl_rule_set.h"
#include "dl_mk_slice.h"
#include "dl_mk_unfold.h"
#include "dl_mk_coalesce.h"
#include "expr_abstract.h"
#include "model_smt2_pp.h"
#include "model_v2_pp.h"

// template class symbol_table<family_id>;

#include "duality.h"
#include "duality_profiling.h"

// using namespace Duality;

namespace Duality {

  enum DualityStatus {StatusModel, StatusRefutation, StatusUnknown, StatusNull};

  class duality_data {
  public:
    context ctx;
    RPFP::LogicSolver *ls;
    RPFP *rpfp;

    DualityStatus status;
    std::vector<expr> clauses;
    std::vector<std::vector<RPFP::label_struct> > clause_labels;
    hash_map<RPFP::Edge *,int> map;  // edges to clauses
    Solver::Counterexample cex;

    duality_data(ast_manager &_m) : ctx(_m,config(params_ref())) {
      ls = 0;
      rpfp = 0;
      status = StatusNull;
    }
    ~duality_data(){
      if(rpfp)
	dealloc(rpfp);
      if(ls)
	dealloc(ls);
      if(cex.tree)
	delete cex.tree;
    }
  };

dl_interface::dl_interface(datalog::context& dl_ctx) :  m_ctx(dl_ctx)

{
  _d = 0;
}

dl_interface::~dl_interface() {
  if(_d)
    dealloc(_d);
}

//
// Check if the new rules are weaker so that we can
// re-use existing context.
//
#if 0
void dl_interface::check_reset() {
  // TODO
    datalog::rule_ref_vector const& new_rules = m_ctx.get_rules().get_rules();
    datalog::rule_ref_vector const& old_rules = m_old_rules.get_rules();
    bool is_subsumed = !old_rules.empty();
    for (unsigned i = 0; is_subsumed && i < new_rules.size(); ++i) {
        is_subsumed = false;
        for (unsigned j = 0; !is_subsumed && j < old_rules.size(); ++j) {
            if (m_ctx.check_subsumes(*old_rules[j], *new_rules[i])) {
                is_subsumed = true;
            }
        }
        if (!is_subsumed) {
            TRACE("pdr", new_rules[i]->display(m_ctx, tout << "Fresh rule "););
            m_context->reset();
        }
    }
    m_old_rules.reset();
    m_old_rules.add_rules(new_rules.size(), new_rules.c_ptr());
}
#endif

lbool dl_interface::query(::expr * query) {

  // we restore the initial state in the datalog context
  m_ctx.ensure_opened();

  // if there is old data, get the cex and dispose (later)
  Solver::Counterexample old_cex;
  duality_data *old_data = _d;
  if(old_data)
    old_cex = old_data->cex;

  // make a new problem and solver
  _d = alloc(duality_data,m_ctx.get_manager());
  _d->ls = alloc(RPFP::iZ3LogicSolver,_d->ctx);
  _d->rpfp = alloc(RPFP,_d->ls);

  expr_ref_vector rules(m_ctx.get_manager());
  svector< ::symbol> names;
  // m_ctx.get_rules_as_formulas(rules, names);
   m_ctx.get_raw_rule_formulas(rules, names);

  // get all the rules as clauses
  std::vector<expr> &clauses = _d->clauses;
  clauses.clear();
  for (unsigned i = 0; i < rules.size(); ++i) {
    expr e(_d->ctx,rules[i].get());
    clauses.push_back(e);
  }

  // turn the query into a clause
  expr q(_d->ctx,m_ctx.bind_variables(query,false));

  std::vector<sort> b_sorts;
  std::vector<symbol> b_names;
  if (q.is_quantifier() && !q.is_quantifier_forall()) {
    int bound = q.get_quantifier_num_bound();
    for(int j = 0; j < bound; j++){
      b_sorts.push_back(q.get_quantifier_bound_sort(j));
      b_names.push_back(q.get_quantifier_bound_name(j));
    }
    q = q.arg(0);
  }

  expr qc = implies(q,_d->ctx.bool_val(false));
  qc = _d->ctx.make_quant(Forall,b_sorts,b_names,qc);
  clauses.push_back(qc);

  // get the background axioms
  unsigned num_asserts = m_ctx.get_num_assertions();
  for (unsigned i = 0; i < num_asserts; ++i) {
    expr e(_d->ctx,m_ctx.get_assertion(i));
    _d->rpfp->AssertAxiom(e);
  }

  // creates 1-1 map between clauses and rpfp edges
  _d->rpfp->FromClauses(clauses);

  // populate the edge-to-clause map
  for(unsigned i = 0; i < _d->rpfp->edges.size(); ++i)
    _d->map[_d->rpfp->edges[i]] = i;

  // create a solver object

  Solver *rs = Solver::Create("duality", _d->rpfp);

  rs->LearnFrom(old_cex); // new solver gets hints from old cex

  // set its options
  IF_VERBOSE(1, rs->SetOption("report","1"););
  rs->SetOption("full_expand",m_ctx.get_params().full_expand() ? "1" : "0");
  rs->SetOption("no_conj",m_ctx.get_params().no_conj() ? "1" : "0");
  rs->SetOption("feasible_edges",m_ctx.get_params().feasible_edges() ? "1" : "0");
  rs->SetOption("use_underapprox",m_ctx.get_params().use_underapprox() ? "1" : "0");
  rs->SetOption("stratified_inlining",m_ctx.get_params().stratified_inlining() ? "1" : "0");
  unsigned rb = m_ctx.get_params().recursion_bound();
  if(rb != UINT_MAX){
    std::ostringstream os; os << rb;
    rs->SetOption("recursion_bound", os.str());
  }

  // Solve!
  bool ans;
  try {
    ans = rs->Solve();
  }
  catch (Duality::solver::cancel_exception &exn){
    throw default_exception("duality canceled");
  }

  // profile!

  if(m_ctx.get_params().profile())
    print_profile(std::cout);

  // save the result and counterexample if there is one
  _d->status = ans ? StatusModel : StatusRefutation;
  _d->cex = rs->GetCounterexample();

  if(old_data){
    old_data->cex.tree = 0; // we own it now
    dealloc(old_data);
  }

  dealloc(rs);

  // true means the RPFP problem is SAT, so the query is UNSAT
  return ans ? l_false : l_true;
}

expr_ref dl_interface::get_cover_delta(int level, ::func_decl* pred_orig) {
    SASSERT(false);
    return expr_ref(m_ctx.get_manager());
}

  void dl_interface::add_cover(int level, ::func_decl* pred, ::expr* property) {
    SASSERT(false);
}

  unsigned dl_interface::get_num_levels(::func_decl* pred) {
    SASSERT(false);
    return 0;
}

  void dl_interface::collect_statistics(::statistics& st) const {
}

void dl_interface::reset_statistics() {
}

static hash_set<func_decl> *local_func_decls;

static void print_proof(dl_interface *d, std::ostream& out, Solver::Counterexample &cex) {
  context &ctx = d->dd()->ctx;
  RPFP::Node &node = *cex.root;
  RPFP::Edge &edge = *node.Outgoing;

  // first, prove the children (that are actually used)

  for(unsigned i = 0; i < edge.Children.size(); i++){
    if(!cex.tree->Empty(edge.Children[i])){
      Solver::Counterexample foo = cex;
      foo.root = edge.Children[i];
      print_proof(d,out,foo);
    }
  }

  // print the label and the proved fact

  out << "(step s!" << node.number;
  out << " (" << node.Name.name();
  for(unsigned i = 0; i < edge.F.IndParams.size(); i++)
    out << " " << cex.tree->Eval(&edge,edge.F.IndParams[i]);
  out << ")\n";

  // print the rule number

  out << " rule!" << node.Outgoing->map->number;

  // print the substitution

  out << "  (subst\n";
  RPFP::Edge *orig_edge = edge.map;
  int orig_clause = d->dd()->map[orig_edge];
  expr &t = d->dd()->clauses[orig_clause];
  if (t.is_quantifier() && t.is_quantifier_forall()) {
    int bound = t.get_quantifier_num_bound();
    std::vector<sort> sorts;
    std::vector<symbol> names;
    hash_map<int,expr> subst;
    for(int j = 0; j < bound; j++){
      sort the_sort = t.get_quantifier_bound_sort(j);
      symbol name = t.get_quantifier_bound_name(j);
      expr skolem = ctx.constant(symbol(ctx,name),sort(ctx,the_sort));
      out << "    (= " << skolem << " " << cex.tree->Eval(&edge,skolem) << ")\n";
      expr local_skolem = cex.tree->Localize(&edge,skolem);
      (*local_func_decls).insert(local_skolem.decl());
    }
  }
  out << "  )\n";

  out << "  (labels";
  std::vector<symbol> labels;
  cex.tree->GetLabels(&edge,labels);
  for(unsigned j = 0; j < labels.size(); j++){
    out << " " << labels[j];
  }

  out << "  )\n";

  // reference the proofs of all the children, in syntactic order
  // "true" means the child is not needed

  out << "  (ref ";
  for(unsigned i = 0; i < edge.Children.size(); i++){
    if(!cex.tree->Empty(edge.Children[i]))
      out << " s!" << edge.Children[i]->number;
    else
      out << " true";
  }
  out << "  )";
  out << ")\n";
}

void dl_interface::display_certificate(std::ostream& out) {
  if(_d->status == StatusModel){
    ast_manager &m = m_ctx.get_manager();
    model_ref md = get_model();
    model_smt2_pp(out, m, *md.get(), 0);
  }
  else if(_d->status == StatusRefutation){
    out << "(derivation\n";
    // negation of the query is the last clause -- prove it
    hash_set<func_decl> locals;
    local_func_decls = &locals;
    print_proof(this,out,_d->cex);
    out << ")\n";
    out << "(model \n\"";
    ::model mod(m_ctx.get_manager());
    model orig_model = _d->cex.tree->dualModel;
    for(unsigned i = 0; i < orig_model.num_consts(); i++){
      func_decl cnst = orig_model.get_const_decl(i);
      if(locals.find(cnst) == locals.end()){
	expr thing = orig_model.get_const_interp(cnst);
	mod.register_decl(to_func_decl(cnst.raw()),to_expr(thing.raw()));
      }
    }
    for(unsigned i = 0; i < orig_model.num_funcs(); i++){
      func_decl cnst = orig_model.get_func_decl(i);
      if(locals.find(cnst) == locals.end()){
	func_interp thing = orig_model.get_func_interp(cnst);
	::func_interp *thing_raw = thing;
	mod.register_decl(to_func_decl(cnst.raw()),thing_raw->copy());
      }
    }
    model_v2_pp(out,mod);
    out << "\")\n";
  }
}

expr_ref dl_interface::get_answer() {
    SASSERT(false);
    return expr_ref(m_ctx.get_manager());
}

void dl_interface::cancel() {
  if(_d && _d->ls)
    _d->ls->cancel();
}

void dl_interface::cleanup() {
}

void dl_interface::updt_params() {
}

model_ref dl_interface::get_model() {
  ast_manager &m = m_ctx.get_manager();
  model_ref md(alloc(::model, m));
  std::vector<RPFP::Node *> &nodes = _d->rpfp->nodes;
  expr_ref_vector conjs(m);
  for (unsigned i = 0; i < nodes.size(); ++i) {
    RPFP::Node *node = nodes[i];
    func_decl &pred = node->Name;
    expr_ref prop(m);
    prop = to_expr(node->Annotation.Formula);
    std::vector<expr> &params = node->Annotation.IndParams;
    expr_ref q(m);
    expr_ref_vector sig_vars(m);
    for (unsigned j = 0; j < params.size(); ++j)
      sig_vars.push_back(params[params.size()-j-1]); // TODO: why backwards?
    expr_abstract(m, 0, sig_vars.size(), sig_vars.c_ptr(), prop, q);
    if (params.empty()) {
      md->register_decl(pred, q);
    }
    else {
      ::func_interp* fi = alloc(::func_interp, m, params.size());
      fi->set_else(q);
      md->register_decl(pred, fi);
    }
  }
  return md;
}

proof_ref dl_interface::get_proof() {
  return proof_ref(m_ctx.get_manager());
}
}
<commit_msg>changed timeout behavior in duality<commit_after>/*++
Copyright (c) 2013 Microsoft Corporation

Module Name:

    duality_dl_interface.cpp

Abstract:

    SMT2 interface for Duality

Author:

    Krystof Hoder (t-khoder) 2011-9-22.
    Modified by Ken McMIllan (kenmcmil) 2013-4-18.

Revision History:

--*/

#include "dl_cmds.h"
#include "dl_context.h"
#include "dl_mk_coi_filter.h"
#include "dl_mk_interp_tail_simplifier.h"
#include "dl_mk_subsumption_checker.h"
#include "dl_mk_rule_inliner.h"
#include "dl_rule.h"
#include "dl_rule_transformer.h"
#include "dl_mk_extract_quantifiers.h"
#include "smt2parser.h"
#include "duality_dl_interface.h"
#include "dl_rule_set.h"
#include "dl_mk_slice.h"
#include "dl_mk_unfold.h"
#include "dl_mk_coalesce.h"
#include "expr_abstract.h"
#include "model_smt2_pp.h"
#include "model_v2_pp.h"

// template class symbol_table<family_id>;

#include "duality.h"
#include "duality_profiling.h"

// using namespace Duality;

namespace Duality {

  enum DualityStatus {StatusModel, StatusRefutation, StatusUnknown, StatusNull};

  class duality_data {
  public:
    context ctx;
    RPFP::LogicSolver *ls;
    RPFP *rpfp;

    DualityStatus status;
    std::vector<expr> clauses;
    std::vector<std::vector<RPFP::label_struct> > clause_labels;
    hash_map<RPFP::Edge *,int> map;  // edges to clauses
    Solver::Counterexample cex;

    duality_data(ast_manager &_m) : ctx(_m,config(params_ref())) {
      ls = 0;
      rpfp = 0;
      status = StatusNull;
    }
    ~duality_data(){
      if(rpfp)
	dealloc(rpfp);
      if(ls)
	dealloc(ls);
      if(cex.tree)
	delete cex.tree;
    }
  };

dl_interface::dl_interface(datalog::context& dl_ctx) :  m_ctx(dl_ctx)

{
  _d = 0;
}

dl_interface::~dl_interface() {
  if(_d)
    dealloc(_d);
}

//
// Check if the new rules are weaker so that we can
// re-use existing context.
//
#if 0
void dl_interface::check_reset() {
  // TODO
    datalog::rule_ref_vector const& new_rules = m_ctx.get_rules().get_rules();
    datalog::rule_ref_vector const& old_rules = m_old_rules.get_rules();
    bool is_subsumed = !old_rules.empty();
    for (unsigned i = 0; is_subsumed && i < new_rules.size(); ++i) {
        is_subsumed = false;
        for (unsigned j = 0; !is_subsumed && j < old_rules.size(); ++j) {
            if (m_ctx.check_subsumes(*old_rules[j], *new_rules[i])) {
                is_subsumed = true;
            }
        }
        if (!is_subsumed) {
            TRACE("pdr", new_rules[i]->display(m_ctx, tout << "Fresh rule "););
            m_context->reset();
        }
    }
    m_old_rules.reset();
    m_old_rules.add_rules(new_rules.size(), new_rules.c_ptr());
}
#endif

lbool dl_interface::query(::expr * query) {

  // we restore the initial state in the datalog context
  m_ctx.ensure_opened();

  // if there is old data, get the cex and dispose (later)
  Solver::Counterexample old_cex;
  duality_data *old_data = _d;
  if(old_data)
    old_cex = old_data->cex;

  // make a new problem and solver
  _d = alloc(duality_data,m_ctx.get_manager());
  _d->ls = alloc(RPFP::iZ3LogicSolver,_d->ctx);
  _d->rpfp = alloc(RPFP,_d->ls);

  expr_ref_vector rules(m_ctx.get_manager());
  svector< ::symbol> names;
  // m_ctx.get_rules_as_formulas(rules, names);
   m_ctx.get_raw_rule_formulas(rules, names);

  // get all the rules as clauses
  std::vector<expr> &clauses = _d->clauses;
  clauses.clear();
  for (unsigned i = 0; i < rules.size(); ++i) {
    expr e(_d->ctx,rules[i].get());
    clauses.push_back(e);
  }

  // turn the query into a clause
  expr q(_d->ctx,m_ctx.bind_variables(query,false));

  std::vector<sort> b_sorts;
  std::vector<symbol> b_names;
  if (q.is_quantifier() && !q.is_quantifier_forall()) {
    int bound = q.get_quantifier_num_bound();
    for(int j = 0; j < bound; j++){
      b_sorts.push_back(q.get_quantifier_bound_sort(j));
      b_names.push_back(q.get_quantifier_bound_name(j));
    }
    q = q.arg(0);
  }

  expr qc = implies(q,_d->ctx.bool_val(false));
  qc = _d->ctx.make_quant(Forall,b_sorts,b_names,qc);
  clauses.push_back(qc);

  // get the background axioms
  unsigned num_asserts = m_ctx.get_num_assertions();
  for (unsigned i = 0; i < num_asserts; ++i) {
    expr e(_d->ctx,m_ctx.get_assertion(i));
    _d->rpfp->AssertAxiom(e);
  }

  // creates 1-1 map between clauses and rpfp edges
  _d->rpfp->FromClauses(clauses);

  // populate the edge-to-clause map
  for(unsigned i = 0; i < _d->rpfp->edges.size(); ++i)
    _d->map[_d->rpfp->edges[i]] = i;

  // create a solver object

  Solver *rs = Solver::Create("duality", _d->rpfp);

  rs->LearnFrom(old_cex); // new solver gets hints from old cex

  // set its options
  IF_VERBOSE(1, rs->SetOption("report","1"););
  rs->SetOption("full_expand",m_ctx.get_params().full_expand() ? "1" : "0");
  rs->SetOption("no_conj",m_ctx.get_params().no_conj() ? "1" : "0");
  rs->SetOption("feasible_edges",m_ctx.get_params().feasible_edges() ? "1" : "0");
  rs->SetOption("use_underapprox",m_ctx.get_params().use_underapprox() ? "1" : "0");
  rs->SetOption("stratified_inlining",m_ctx.get_params().stratified_inlining() ? "1" : "0");
  unsigned rb = m_ctx.get_params().recursion_bound();
  if(rb != UINT_MAX){
    std::ostringstream os; os << rb;
    rs->SetOption("recursion_bound", os.str());
  }

  // Solve!
  bool ans;
  try {
    ans = rs->Solve();
  }
  catch (Duality::solver::cancel_exception &exn){
    throw default_exception("duality canceled");
  }

  // profile!

  if(m_ctx.get_params().profile())
    print_profile(std::cout);

  // save the result and counterexample if there is one
  _d->status = ans ? StatusModel : StatusRefutation;
  _d->cex = rs->GetCounterexample();

  if(old_data){
    old_data->cex.tree = 0; // we own it now
    dealloc(old_data);
  }

  dealloc(rs);

  // true means the RPFP problem is SAT, so the query is UNSAT
  return ans ? l_false : l_true;
}

expr_ref dl_interface::get_cover_delta(int level, ::func_decl* pred_orig) {
    SASSERT(false);
    return expr_ref(m_ctx.get_manager());
}

  void dl_interface::add_cover(int level, ::func_decl* pred, ::expr* property) {
    SASSERT(false);
}

  unsigned dl_interface::get_num_levels(::func_decl* pred) {
    SASSERT(false);
    return 0;
}

  void dl_interface::collect_statistics(::statistics& st) const {
}

void dl_interface::reset_statistics() {
}

static hash_set<func_decl> *local_func_decls;

static void print_proof(dl_interface *d, std::ostream& out, Solver::Counterexample &cex) {
  context &ctx = d->dd()->ctx;
  RPFP::Node &node = *cex.root;
  RPFP::Edge &edge = *node.Outgoing;

  // first, prove the children (that are actually used)

  for(unsigned i = 0; i < edge.Children.size(); i++){
    if(!cex.tree->Empty(edge.Children[i])){
      Solver::Counterexample foo = cex;
      foo.root = edge.Children[i];
      print_proof(d,out,foo);
    }
  }

  // print the label and the proved fact

  out << "(step s!" << node.number;
  out << " (" << node.Name.name();
  for(unsigned i = 0; i < edge.F.IndParams.size(); i++)
    out << " " << cex.tree->Eval(&edge,edge.F.IndParams[i]);
  out << ")\n";

  // print the rule number

  out << " rule!" << node.Outgoing->map->number;

  // print the substitution

  out << "  (subst\n";
  RPFP::Edge *orig_edge = edge.map;
  int orig_clause = d->dd()->map[orig_edge];
  expr &t = d->dd()->clauses[orig_clause];
  if (t.is_quantifier() && t.is_quantifier_forall()) {
    int bound = t.get_quantifier_num_bound();
    std::vector<sort> sorts;
    std::vector<symbol> names;
    hash_map<int,expr> subst;
    for(int j = 0; j < bound; j++){
      sort the_sort = t.get_quantifier_bound_sort(j);
      symbol name = t.get_quantifier_bound_name(j);
      expr skolem = ctx.constant(symbol(ctx,name),sort(ctx,the_sort));
      out << "    (= " << skolem << " " << cex.tree->Eval(&edge,skolem) << ")\n";
      expr local_skolem = cex.tree->Localize(&edge,skolem);
      (*local_func_decls).insert(local_skolem.decl());
    }
  }
  out << "  )\n";

  out << "  (labels";
  std::vector<symbol> labels;
  cex.tree->GetLabels(&edge,labels);
  for(unsigned j = 0; j < labels.size(); j++){
    out << " " << labels[j];
  }

  out << "  )\n";

  // reference the proofs of all the children, in syntactic order
  // "true" means the child is not needed

  out << "  (ref ";
  for(unsigned i = 0; i < edge.Children.size(); i++){
    if(!cex.tree->Empty(edge.Children[i]))
      out << " s!" << edge.Children[i]->number;
    else
      out << " true";
  }
  out << "  )";
  out << ")\n";
}

void dl_interface::display_certificate(std::ostream& out) {
  if(_d->status == StatusModel){
    ast_manager &m = m_ctx.get_manager();
    model_ref md = get_model();
    model_smt2_pp(out, m, *md.get(), 0);
  }
  else if(_d->status == StatusRefutation){
    out << "(derivation\n";
    // negation of the query is the last clause -- prove it
    hash_set<func_decl> locals;
    local_func_decls = &locals;
    print_proof(this,out,_d->cex);
    out << ")\n";
    out << "(model \n\"";
    ::model mod(m_ctx.get_manager());
    model orig_model = _d->cex.tree->dualModel;
    for(unsigned i = 0; i < orig_model.num_consts(); i++){
      func_decl cnst = orig_model.get_const_decl(i);
      if(locals.find(cnst) == locals.end()){
	expr thing = orig_model.get_const_interp(cnst);
	mod.register_decl(to_func_decl(cnst.raw()),to_expr(thing.raw()));
      }
    }
    for(unsigned i = 0; i < orig_model.num_funcs(); i++){
      func_decl cnst = orig_model.get_func_decl(i);
      if(locals.find(cnst) == locals.end()){
	func_interp thing = orig_model.get_func_interp(cnst);
	::func_interp *thing_raw = thing;
	mod.register_decl(to_func_decl(cnst.raw()),thing_raw->copy());
      }
    }
    model_v2_pp(out,mod);
    out << "\")\n";
  }
}

expr_ref dl_interface::get_answer() {
    SASSERT(false);
    return expr_ref(m_ctx.get_manager());
}

void dl_interface::cancel() {
#if 0
  if(_d && _d->ls)
    _d->ls->cancel();
#else
  // HACK: duality can't cancel at all times, we just exit here
  std::cout << "(error \"duality canceled\")\nunknown\n";
  exit(0);
#endif
}

void dl_interface::cleanup() {
}

void dl_interface::updt_params() {
}

model_ref dl_interface::get_model() {
  ast_manager &m = m_ctx.get_manager();
  model_ref md(alloc(::model, m));
  std::vector<RPFP::Node *> &nodes = _d->rpfp->nodes;
  expr_ref_vector conjs(m);
  for (unsigned i = 0; i < nodes.size(); ++i) {
    RPFP::Node *node = nodes[i];
    func_decl &pred = node->Name;
    expr_ref prop(m);
    prop = to_expr(node->Annotation.Formula);
    std::vector<expr> &params = node->Annotation.IndParams;
    expr_ref q(m);
    expr_ref_vector sig_vars(m);
    for (unsigned j = 0; j < params.size(); ++j)
      sig_vars.push_back(params[params.size()-j-1]); // TODO: why backwards?
    expr_abstract(m, 0, sig_vars.size(), sig_vars.c_ptr(), prop, q);
    if (params.empty()) {
      md->register_decl(pred, q);
    }
    else {
      ::func_interp* fi = alloc(::func_interp, m, params.size());
      fi->set_else(q);
      md->register_decl(pred, fi);
    }
  }
  return md;
}

proof_ref dl_interface::get_proof() {
  return proof_ref(m_ctx.get_manager());
}
}
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#define SOFA_COMPONENT_ENGINE_GROUPFILTERYOUNGMODULUS_CPP
#include <sofa/component/engine/GroupFilterYoungModulus.h>
#include <sofa/core/ObjectFactory.h>

namespace sofa
{

namespace component
{

namespace engine
{

using namespace sofa::core::componentmodel::loader;

template <class DataTypes>
GroupFilterYoungModulus<DataTypes>::GroupFilterYoungModulus()
    : f_groups( initData (&f_groups, "groups", "Groups") )
    , f_primitives( initData (&f_primitives, "primitives", "Vector of primitives (indices)") )
    , f_youngModulus( initData (&f_youngModulus, "youngModulus", "Vector of young modulus for each primitive") )
    , p_mapGroupModulus( initData (&p_mapGroupModulus, "mapGroupModulus", "Mapping between groups and modulus") )
    , p_defaultModulus( initData (&p_defaultModulus, (Real) 10000.0, "defaultYoungModulus", "Default value if the primitive is not in a group") )
{
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::init()
{
    addInput(&f_groups);
    addInput(&f_primitives);
    addOutput(&f_youngModulus);
    setDirtyValue();
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::reinit()
{
    update();
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::update()
{
    cleanDirty();

    //Input
    const std::string& strMap = p_mapGroupModulus.getValue();
    const helper::vector<unsigned int>& primitives = f_primitives.getValue();
    const helper::vector<PrimitiveGroup >& groups = f_groups.getValue();
    const Real& defaultModulus =  p_defaultModulus.getValue();
    //Output
    helper::vector<Real>& youngModulusVector = *f_youngModulus.beginEdit();

    std::map<PrimitiveGroup, Real> mapMG;

    //does not matter if primitives is empty
    int maxSize = primitives.size();

    size_t begin = 0, end = strMap.find(";");
    std::string groupName;
    Real youngModulus;

    //read string and tokenize
    while(end != std::string::npos )
    {
        std::string tempStr = strMap.substr(begin, end-1);
        std::istringstream iss(tempStr);
        iss >> groupName >> youngModulus ;
        begin = end+1;
        end = strMap.find(";", begin);

        if (!groupName.empty() && youngModulus > 0)
        {
            //find group according to name
            bool found = false;
            unsigned int gid = 0;
            for (unsigned int i=0 ; i<groups.size() && !found; i++)
            {
                if (groups[i].groupName.compare(groupName) == 0)
                {
                    found = true;
                    gid = i;
                }
            }

            if (!found)
                serr << "Group " << groupName << " not found" << sendl;
            else
            {
                mapMG[groups[gid]] = youngModulus;

                if (maxSize < groups[gid].p0 + groups[gid].nbp)
                    maxSize = groups[gid].p0+ groups[gid].nbp;
            }
        }
        else serr << "Error while parsing mapping" << sendl;
    }

    //build YM vector
    youngModulusVector.clear();
    youngModulusVector.resize(maxSize);
    std::fill(youngModulusVector.begin(), youngModulusVector.end(), defaultModulus);

    typename std::map<PrimitiveGroup, Real>::const_iterator itMapMG;
    for (itMapMG = mapMG.begin() ; itMapMG != mapMG.end() ; itMapMG++)
    {
        PrimitiveGroup pg = (*itMapMG).first;
        Real ym = (*itMapMG).second;

        for (int i=pg.p0 ; i<pg.p0+pg.nbp ; i++)
            youngModulusVector[i] = ym;
    }

    //std::cout << youngModulusVector.size() << std::endl;
    //std::cout << youngModulusVector << std::endl;

    f_youngModulus.endEdit();
}

} // namespace engine

} // namespace component

} // namespace sofa

<commit_msg>r6566/sofa-dev : FIX: tiny bug<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#define SOFA_COMPONENT_ENGINE_GROUPFILTERYOUNGMODULUS_CPP
#include <sofa/component/engine/GroupFilterYoungModulus.h>
#include <sofa/core/ObjectFactory.h>

namespace sofa
{

namespace component
{

namespace engine
{

using namespace sofa::core::componentmodel::loader;

template <class DataTypes>
GroupFilterYoungModulus<DataTypes>::GroupFilterYoungModulus()
    : f_groups( initData (&f_groups, "groups", "Groups") )
    , f_primitives( initData (&f_primitives, "primitives", "Vector of primitives (indices)") )
    , f_youngModulus( initData (&f_youngModulus, "youngModulus", "Vector of young modulus for each primitive") )
    , p_mapGroupModulus( initData (&p_mapGroupModulus, "mapGroupModulus", "Mapping between groups and modulus") )
    , p_defaultModulus( initData (&p_defaultModulus, (Real) 10000.0, "defaultYoungModulus", "Default value if the primitive is not in a group") )
{
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::init()
{
    addInput(&f_groups);
    addInput(&f_primitives);
    addOutput(&f_youngModulus);
    setDirtyValue();
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::reinit()
{
    update();
}

template <class DataTypes>
void GroupFilterYoungModulus<DataTypes>::update()
{
    cleanDirty();

    //Input
    const std::string& strMap = p_mapGroupModulus.getValue();
    const helper::vector<unsigned int>& primitives = f_primitives.getValue();
    const helper::vector<PrimitiveGroup >& groups = f_groups.getValue();
    const Real& defaultModulus =  p_defaultModulus.getValue();
    //Output
    helper::vector<Real>& youngModulusVector = *f_youngModulus.beginEdit();

    std::map<PrimitiveGroup, Real> mapMG;

    //does not matter if primitives is empty
    int maxSize = primitives.size();

    size_t begin = 0, end = strMap.find(";");
    std::string groupName;
    Real youngModulus;

    //read string and tokenize
    while(end != std::string::npos )
    {
        std::string tempStr = strMap.substr(begin, end);
        std::istringstream iss(tempStr);
        iss >> groupName >> youngModulus ;
        begin = end+1;
        end = strMap.find(";", begin);

        if (!groupName.empty() && youngModulus > 0)
        {
            //find group according to name
            bool found = false;
            unsigned int gid = 0;
            for (unsigned int i=0 ; i<groups.size() && !found; i++)
            {
                if (groups[i].groupName.compare(groupName) == 0)
                {
                    found = true;
                    gid = i;
                }
            }

            if (!found)
                serr << "Group " << groupName << " not found" << sendl;
            else
            {
                mapMG[groups[gid]] = youngModulus;

                if (maxSize < groups[gid].p0 + groups[gid].nbp)
                    maxSize = groups[gid].p0+ groups[gid].nbp;
            }
        }
        else serr << "Error while parsing mapping" << sendl;
    }

    //build YM vector
    youngModulusVector.clear();
    youngModulusVector.resize(maxSize);
    std::fill(youngModulusVector.begin(), youngModulusVector.end(), defaultModulus);

    typename std::map<PrimitiveGroup, Real>::const_iterator itMapMG;
    for (itMapMG = mapMG.begin() ; itMapMG != mapMG.end() ; itMapMG++)
    {
        PrimitiveGroup pg = (*itMapMG).first;
        Real ym = (*itMapMG).second;

        for (int i=pg.p0 ; i<pg.p0+pg.nbp ; i++)
            youngModulusVector[i] = ym;
    }

    //std::cout << youngModulusVector.size() << std::endl;
    //std::cout << youngModulusVector << std::endl;

    f_youngModulus.endEdit();
}

} // namespace engine

} // namespace component

} // namespace sofa

<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: EventListenerHelper.hxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: vg $ $Date: 2007-05-22 18:16:13 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef CHART2_EVENTLISTENERHELPER_HXX
#define CHART2_EVENTLISTENERHELPER_HXX

#ifndef _COM_SUN_STAR_LANG_XEVENTLISTENER_HPP_
#include <com/sun/star/lang/XEventListener.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XCOMPONENT_HPP_
#include <com/sun/star/lang/XComponent.hpp>
#endif

#include <list>
#include <algorithm>
#include <functional>
#include <utility>

namespace chart
{
namespace EventListenerHelper
{

namespace impl
{

template< class InterfaceRef >
struct addListenerFunctor : public ::std::unary_function< InterfaceRef, void >
{
    explicit addListenerFunctor( const ::com::sun::star::uno::Reference<
                                     ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const InterfaceRef & xObject )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( xObject, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->addEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class InterfaceRef >
struct removeListenerFunctor : public ::std::unary_function< InterfaceRef, void >
{
    explicit removeListenerFunctor( const ::com::sun::star::uno::Reference<
                                        ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const InterfaceRef & xObject )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( xObject, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->removeEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class Pair >
struct addListenerToMappedElementFunctor : public ::std::unary_function< Pair, void >
{
    explicit addListenerToMappedElementFunctor( const ::com::sun::star::uno::Reference<
                                                    ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const Pair & aPair )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( aPair.second, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->addEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class Pair >
struct removeListenerFromMappedElementFunctor : public ::std::unary_function< Pair, void >
{
    explicit removeListenerFromMappedElementFunctor( const ::com::sun::star::uno::Reference<
                                                         ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const Pair & aPair )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( aPair.second, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->removeEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

} //  namespace impl

// --------------------------------------------------------------------------------

template< class InterfaceRef >
void addListener(
    const InterfaceRef & xObject,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
    {
        impl::addListenerFunctor< InterfaceRef > aFunctor( xListener );
        aFunctor( xObject );
    }
}

template< class Container >
void addListenerToAllElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::addListenerFunctor< typename Container::value_type >( xListener ));
}

template< class Container >
void addListenerToAllMapElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::addListenerToMappedElementFunctor< typename Container::value_type >( xListener ));
}

template< typename T >
void addListenerToAllSequenceElements(
    const ::com::sun::star::uno::Sequence< T > & rSequence,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rSequence.getConstArray(), rSequence.getConstArray() + rSequence.getLength(),
                         impl::addListenerFunctor< T >( xListener ));
}

template< class InterfaceRef >
void removeListener(
    const InterfaceRef & xObject,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
    {
        impl::removeListenerFunctor< InterfaceRef > aFunctor( xListener );
        aFunctor( xObject );
    }
}

template< class Container >
void removeListenerFromAllElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::removeListenerFunctor< typename Container::value_type >( xListener ));
}

template< class Container >
void removeListenerFromAllMapElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::removeListenerFromMappedElementFunctor< typename Container::value_type >( xListener ));
}

template< typename T >
void removeListenerFromAllSequenceElements(
    const ::com::sun::star::uno::Sequence< T > & rSequence,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rSequence.getConstArray(), rSequence.getConstArray() + rSequence.getLength(),
                         impl::removeListenerFunctor< T >( xListener ));
}

} //  namespace EventListenerHelper
} //  namespace chart

// CHART2_EVENTLISTENERHELPER_HXX
#endif
<commit_msg>INTEGRATION: CWS changefileheader (1.2.126); FILE MERGED 2008/04/01 10:50:28 thb 1.2.126.2: #i85898# Stripping all external header guards 2008/03/28 16:43:53 rt 1.2.126.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: EventListenerHelper.hxx,v $
 * $Revision: 1.3 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/
#ifndef CHART2_EVENTLISTENERHELPER_HXX
#define CHART2_EVENTLISTENERHELPER_HXX

#include <com/sun/star/lang/XEventListener.hpp>
#include <com/sun/star/lang/XComponent.hpp>

#include <list>
#include <algorithm>
#include <functional>
#include <utility>

namespace chart
{
namespace EventListenerHelper
{

namespace impl
{

template< class InterfaceRef >
struct addListenerFunctor : public ::std::unary_function< InterfaceRef, void >
{
    explicit addListenerFunctor( const ::com::sun::star::uno::Reference<
                                     ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const InterfaceRef & xObject )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( xObject, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->addEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class InterfaceRef >
struct removeListenerFunctor : public ::std::unary_function< InterfaceRef, void >
{
    explicit removeListenerFunctor( const ::com::sun::star::uno::Reference<
                                        ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const InterfaceRef & xObject )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( xObject, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->removeEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class Pair >
struct addListenerToMappedElementFunctor : public ::std::unary_function< Pair, void >
{
    explicit addListenerToMappedElementFunctor( const ::com::sun::star::uno::Reference<
                                                    ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const Pair & aPair )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( aPair.second, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->addEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

template< class Pair >
struct removeListenerFromMappedElementFunctor : public ::std::unary_function< Pair, void >
{
    explicit removeListenerFromMappedElementFunctor( const ::com::sun::star::uno::Reference<
                                                         ::com::sun::star::lang::XEventListener > & xListener ) :
            m_xListener( xListener )
    {}

    void operator() ( const Pair & aPair )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
              xBroadcaster( aPair.second, ::com::sun::star::uno::UNO_QUERY );
        if( xBroadcaster.is() && m_xListener.is())
            xBroadcaster->removeEventListener( m_xListener );
    }
private:
    ::com::sun::star::uno::Reference< ::com::sun::star::lang::XEventListener > m_xListener;
};

} //  namespace impl

// --------------------------------------------------------------------------------

template< class InterfaceRef >
void addListener(
    const InterfaceRef & xObject,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
    {
        impl::addListenerFunctor< InterfaceRef > aFunctor( xListener );
        aFunctor( xObject );
    }
}

template< class Container >
void addListenerToAllElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::addListenerFunctor< typename Container::value_type >( xListener ));
}

template< class Container >
void addListenerToAllMapElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::addListenerToMappedElementFunctor< typename Container::value_type >( xListener ));
}

template< typename T >
void addListenerToAllSequenceElements(
    const ::com::sun::star::uno::Sequence< T > & rSequence,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rSequence.getConstArray(), rSequence.getConstArray() + rSequence.getLength(),
                         impl::addListenerFunctor< T >( xListener ));
}

template< class InterfaceRef >
void removeListener(
    const InterfaceRef & xObject,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
    {
        impl::removeListenerFunctor< InterfaceRef > aFunctor( xListener );
        aFunctor( xObject );
    }
}

template< class Container >
void removeListenerFromAllElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::removeListenerFunctor< typename Container::value_type >( xListener ));
}

template< class Container >
void removeListenerFromAllMapElements(
    const Container & rContainer,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rContainer.begin(), rContainer.end(),
                         impl::removeListenerFromMappedElementFunctor< typename Container::value_type >( xListener ));
}

template< typename T >
void removeListenerFromAllSequenceElements(
    const ::com::sun::star::uno::Sequence< T > & rSequence,
    const ::com::sun::star::uno::Reference<
        ::com::sun::star::lang::XEventListener > & xListener )
{
    if( xListener.is())
        ::std::for_each( rSequence.getConstArray(), rSequence.getConstArray() + rSequence.getLength(),
                         impl::removeListenerFunctor< T >( xListener ));
}

} //  namespace EventListenerHelper
} //  namespace chart

// CHART2_EVENTLISTENERHELPER_HXX
#endif
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Luke San Antonio
 * All rights reserved.
 */
#include "driver.h"
#include "mesh.h"
#include "texture.h"
#include "shader.h"

#include "common.h"

#include "../../common/color.h"
#include "../../common/log.h"

#include "../../common/debugging.h"
#ifdef GAME_DEBUGGING_ENABLED
#define CAST_PTR dynamic_cast
#else
#define CAST_PTR reinterpret_cast
#endif

namespace redc
{
  namespace gfx
  {
    namespace gl
    {
      Driver::Driver(Vec<int> size) noexcept : IDriver(size)
      {
        glBlendEquationSeparate(GL_FUNC_ADD, GL_FUNC_ADD);
        glBlendFuncSeparate(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA, GL_ONE,
                            GL_ZERO);

        glEnable(GL_FRAMEBUFFER_SRGB);

        depth_test(true);
        write_depth(true);
      }
      Driver::~Driver() noexcept
      {
        glUseProgram(0);
      }

      std::unique_ptr<Shader> Driver::make_shader_repr() noexcept
      {
        return std::make_unique<GL_Shader>(*this);
      }
      void Driver::use_shader(Shader& s) noexcept
      {
        if(&s == cur_shader_) return;

        auto shader_ptr = CAST_PTR<GL_Shader*>(&s);
        REDC_ASSERT_MSG(shader_ptr, "Shader not made with this driver;"
                        " something is very wrong.");

        shader_ptr->use();
        cur_shader_ = shader_ptr;
      }
      Shader* Driver::active_shader() const noexcept
      {
        return cur_shader_;
      }

      std::unique_ptr<Mesh> Driver::make_mesh_repr() noexcept
      {
        // Make it and forget about it, we'll just use a dynamic cast for
        // simplicity.
        return std::make_unique<GL_Mesh>(*this);
      }
      void Driver::bind_mesh(Mesh& mesh) noexcept
      {
        // This kind of design can be more cache friendly. But the good thing
        // is when we are efficiently storing mesh data in a single mesh we
        // don't even have to dereference the pointer we just do a comparison.
        // In other words, this will not thrash the cache *too* often, despite
        // the fact that it would indeed be more efficient to instead literally
        // keep a list of integers representing each vao to switch to in that
        // order. I don't know, I guess this isn't the most efficient, but will
        // get faster as we optimize our mesh use so it's not bad!
        if(&mesh == cur_mesh_) return;

        auto gl_mesh = CAST_PTR<GL_Mesh*>(&mesh);
        REDC_ASSERT_MSG(gl_mesh, "Mesh not made with this driver; something is"
                        " very wrong.");

        gl_mesh->bind();
        cur_mesh_ = gl_mesh;
      }

      std::unique_ptr<Texture> Driver::make_texture_repr() noexcept
      {
        // build n' forget.
        return std::make_unique<GL_Texture>();
      }
      void Driver::bind_texture(Texture& tex, unsigned int loc) noexcept
      {
        auto gl_tex = CAST_PTR<GL_Texture*>(&tex);
        REDC_ASSERT_MSG(gl_tex, "Texture not made with this driver; something"
                        " is very wrong.");
        gl_tex->bind(loc);
      }

      void Driver::set_clear_color(Color const& c) noexcept
      {
        glClearColor(c.r / (float) 0xff, c.g / (float) 0xff,
                     c.b / (float) 0xff, 1.0);
      }
      void Driver::set_clear_depth(float f) noexcept
      {
        glClearDepth(f);
      }

      void Driver::clear() noexcept
      {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      }
      void Driver::clear_color() noexcept
      {
        glClear(GL_COLOR_BUFFER_BIT);
      }
      void Driver::clear_depth() noexcept
      {
        glClear(GL_DEPTH_BUFFER_BIT);
      }
      void Driver::depth_test(bool enable) noexcept
      {
        if(enable) glEnable(GL_DEPTH_TEST);
        else glDisable(GL_DEPTH_TEST);
      }
      void Driver::write_depth(bool enable) noexcept
      {
        glDepthMask(enable);
      }
      void Driver::blending(bool enable) noexcept
      {
        if(enable) glEnable(GL_BLEND);
        else glDisable(GL_BLEND);
      }
      void Driver::face_culling(bool enable) noexcept
      {
        if(enable) glEnable(GL_CULL_FACE);
        else glDisable(GL_CULL_FACE);
      }
      void Driver::cull_side(Cull_Side side) noexcept
      {
        switch(side)
        {
        case Cull_Side::Front:
          glCullFace(GL_FRONT);
          break;
        case Cull_Side::Back:
          glCullFace(GL_BACK);
          break;
        default:
          break;
        }
      }
      float Driver::read_pixel(Framebuffer fb, Vec<int> pt) noexcept
      {
        auto f = get_gl_pixel_format(fb);

        float ret;
        glReadPixels(pt.x, window_extents().y - pt.y, 1, 1, f, GL_FLOAT, &ret);
        return ret;
      }
      void Driver::check_error() noexcept
      {
        if(glGetError() == GL_INVALID_OPERATION)
        {
          log_e("OpenGL: Invalid operation");
        }
      }
    }
  }
}
<commit_msg>Set the OpenGL viewport to the window size<commit_after>/*
 * Copyright (C) 2015 Luke San Antonio
 * All rights reserved.
 */
#include "driver.h"
#include "mesh.h"
#include "texture.h"
#include "shader.h"

#include "common.h"

#include "../../common/color.h"
#include "../../common/log.h"

#include "../../common/debugging.h"
#ifdef GAME_DEBUGGING_ENABLED
#define CAST_PTR dynamic_cast
#else
#define CAST_PTR reinterpret_cast
#endif

namespace redc
{
  namespace gfx
  {
    namespace gl
    {
      Driver::Driver(Vec<int> size) noexcept : IDriver(size)
      {
        glViewport(0, 0, size.x, size.y);
        glBlendEquationSeparate(GL_FUNC_ADD, GL_FUNC_ADD);
        glBlendFuncSeparate(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA, GL_ONE,
                            GL_ZERO);

        glEnable(GL_FRAMEBUFFER_SRGB);

        depth_test(true);
        write_depth(true);
      }
      Driver::~Driver() noexcept
      {
        glUseProgram(0);
      }

      std::unique_ptr<Shader> Driver::make_shader_repr() noexcept
      {
        return std::make_unique<GL_Shader>(*this);
      }
      void Driver::use_shader(Shader& s) noexcept
      {
        if(&s == cur_shader_) return;

        auto shader_ptr = CAST_PTR<GL_Shader*>(&s);
        REDC_ASSERT_MSG(shader_ptr, "Shader not made with this driver;"
                        " something is very wrong.");

        shader_ptr->use();
        cur_shader_ = shader_ptr;
      }
      Shader* Driver::active_shader() const noexcept
      {
        return cur_shader_;
      }

      std::unique_ptr<Mesh> Driver::make_mesh_repr() noexcept
      {
        // Make it and forget about it, we'll just use a dynamic cast for
        // simplicity.
        return std::make_unique<GL_Mesh>(*this);
      }
      void Driver::bind_mesh(Mesh& mesh) noexcept
      {
        // This kind of design can be more cache friendly. But the good thing
        // is when we are efficiently storing mesh data in a single mesh we
        // don't even have to dereference the pointer we just do a comparison.
        // In other words, this will not thrash the cache *too* often, despite
        // the fact that it would indeed be more efficient to instead literally
        // keep a list of integers representing each vao to switch to in that
        // order. I don't know, I guess this isn't the most efficient, but will
        // get faster as we optimize our mesh use so it's not bad!
        if(&mesh == cur_mesh_) return;

        auto gl_mesh = CAST_PTR<GL_Mesh*>(&mesh);
        REDC_ASSERT_MSG(gl_mesh, "Mesh not made with this driver; something is"
                        " very wrong.");

        gl_mesh->bind();
        cur_mesh_ = gl_mesh;
      }

      std::unique_ptr<Texture> Driver::make_texture_repr() noexcept
      {
        // build n' forget.
        return std::make_unique<GL_Texture>();
      }
      void Driver::bind_texture(Texture& tex, unsigned int loc) noexcept
      {
        auto gl_tex = CAST_PTR<GL_Texture*>(&tex);
        REDC_ASSERT_MSG(gl_tex, "Texture not made with this driver; something"
                        " is very wrong.");
        gl_tex->bind(loc);
      }

      void Driver::set_clear_color(Color const& c) noexcept
      {
        glClearColor(c.r / (float) 0xff, c.g / (float) 0xff,
                     c.b / (float) 0xff, 1.0);
      }
      void Driver::set_clear_depth(float f) noexcept
      {
        glClearDepth(f);
      }

      void Driver::clear() noexcept
      {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      }
      void Driver::clear_color() noexcept
      {
        glClear(GL_COLOR_BUFFER_BIT);
      }
      void Driver::clear_depth() noexcept
      {
        glClear(GL_DEPTH_BUFFER_BIT);
      }
      void Driver::depth_test(bool enable) noexcept
      {
        if(enable) glEnable(GL_DEPTH_TEST);
        else glDisable(GL_DEPTH_TEST);
      }
      void Driver::write_depth(bool enable) noexcept
      {
        glDepthMask(enable);
      }
      void Driver::blending(bool enable) noexcept
      {
        if(enable) glEnable(GL_BLEND);
        else glDisable(GL_BLEND);
      }
      void Driver::face_culling(bool enable) noexcept
      {
        if(enable) glEnable(GL_CULL_FACE);
        else glDisable(GL_CULL_FACE);
      }
      void Driver::cull_side(Cull_Side side) noexcept
      {
        switch(side)
        {
        case Cull_Side::Front:
          glCullFace(GL_FRONT);
          break;
        case Cull_Side::Back:
          glCullFace(GL_BACK);
          break;
        default:
          break;
        }
      }
      float Driver::read_pixel(Framebuffer fb, Vec<int> pt) noexcept
      {
        auto f = get_gl_pixel_format(fb);

        float ret;
        glReadPixels(pt.x, window_extents().y - pt.y, 1, 1, f, GL_FLOAT, &ret);
        return ret;
      }
      void Driver::check_error() noexcept
      {
        if(glGetError() == GL_INVALID_OPERATION)
        {
          log_e("OpenGL: Invalid operation");
        }
      }
    }
  }
}
<|endoftext|>


<commit_before>

#include <assert.h>
#include <sys/time.h>           // gettimeofday()

#include "LockDebugger.h"		// LockDebugger
#include "logMsg/logMsg.h"		// LM_M()
#include "au/string.h"

#include "au/mutex/Token.h"				// Own interface

//#define DEBUG_AU_TOKEN

NAMESPACE_BEGIN(au)

std::string get_thread_id( pthread_t t )
{
    std::ostringstream output;

    unsigned char *base = (unsigned char*) &t;
    size_t i;
    for (i = 0; i < sizeof(t); i ++)
    {
        output << au::str("%02x", base[i]);
    }
    return output.str();
}

Token::Token( const char * _name )
{

    // Take the name for debuggin
    name = _name;

    LM_LM(("Token %s created" , name ));

    int r_init = pthread_mutex_init(&_lock, 0);

    if( r_init != 0 )
        LM_X(1, ("pthread_mutex_init for '%s' returned %d" , name , r_init ));

    int t_init_cond = pthread_cond_init(&_block, NULL);

    if( t_init_cond != 0 )
        LM_X(1, ("pthread_cond_init for '%s' returned %d" , name , r_init ));

    locked = false;
}

Token::~Token()
{
    LM_LM(("Token %s was destroyed" , name ));

    //LM_M(("Destroying token '%s' (%p)", name, this));
    pthread_mutex_destroy(&_lock);
    pthread_cond_destroy(&_block);
}

void Token::retain(  )
{
    LM_LM(("Thread [%s] trying to token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    if( locked && pthread_equal(pthread_self(), t ) )
    {
        counter++;
        LM_LM(("token %s was retained by me ( thread [%s] ) . Just updating counter to %d " , name , get_thread_id( pthread_self() ).c_str() ,  counter ));
        return;
    }

#ifdef DEBUG_AU_TOKEN
    LockDebugger::shared()->add_lock( this );
#endif
    // LOCK the mutex
    int ans = pthread_mutex_lock(&_lock);
    if( ans )
    {
        LM_LE(("Error %d getting mutex  (EINVAL:%d, EFAULT:%d, EDEADLK:%d", ans, EINVAL, EFAULT, EDEADLK));
        //assert(false);
        if ((name != NULL) && (name != (char *)0xffffffff) && (name > (char *)0x10000000) && ((name[0] > 'a') && (name[0] < 'Z')) && ((name[1] > 'a') && (name[1] < 'Z')))
        {
            LM_LE(("Token %s: pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
        else
        {
            LM_LE(("Token (wrong name:%p): pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
    }

    if( locked )
        LM_X(1,("Internal error: Thread [%s] has retained au::Token (%s) previously locked by thread [%s]" , get_thread_id( pthread_self() ).c_str() , name , get_thread_id( t ).c_str() ));

    t = pthread_self();
    counter = 1;
    locked = true;

    LM_LM(("Thread [%s] has retained token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

}

void Token::release( )
{
    LM_LM(("Thread [%s] trying to releases token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    if( !locked )
        LM_E(("Internal error: Releasing a non-locked au::Token"));

    if( ! pthread_equal(pthread_self(), t) )
        LM_E(("Internal error: Releasing an au::Token not locked by me"));

    counter--;
    if( counter > 0 )
    {
        LM_LM(("Token %s is still locked by thread [%s] with counter %d" , name , get_thread_id( pthread_self() ).c_str() , counter ));
        return;
    }

    // Flag this as unlocked
    locked = false;

#ifdef DEBUG_AU_TOKEN
    LockDebugger::shared()->remove_lock( this );
#endif
    // UNLOCK the mutex
    int ans = pthread_mutex_unlock(&_lock);
    if( ans )
    {
        LM_E(("Error %d releasing mutex (EINVAL:%d, EFAULT:%d, EPERM:%d", ans, EINVAL, EFAULT, EPERM));
        // Goyo. The segmentation fault when quitting delilah seems to be related to a corruption in name (SAMSON-314)
        if (name != NULL)
        {
            LM_E(("Token %p: pthread_mutex_unlock returned error %d (%p)", name, ans, this));
        }
        else
        {
            LM_E(("Token (NULL name:%p): pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
    }

    LM_LM(("Thread [%s] completelly releases by token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

}

void Token::stop()
{
    LM_LM(("Thread [%s] is being stopeed at token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    // You are suppoused to be retaining this lock
    if( !locked )
        LM_E(("Internal error: Releasing a non-locked au::Token"));

    if( !pthread_equal(pthread_self(), t) )
        LM_E(("Internal error: Releasing an au::Token not locked by me"));

    int tmp_counter = counter; // Keep counter of retains
    locked = false;            // We are temporally releasing this token

    // This unlock the mutex and froze the process in the condition
    if( pthread_cond_wait(&_block, &_lock) != 0 )
        LM_X(1, ("Internal error at au::TokenTaker"));

    LM_LM(("Thread [%s] is back from stopeed at token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    // Now you are retaining again
    locked = true;
    t = pthread_self();
    counter = tmp_counter;

}

NAMESPACE_END
<commit_msg>Removing traces for au::Token debug<commit_after>

#include <assert.h>
#include <sys/time.h>           // gettimeofday()

#include "LockDebugger.h"		// LockDebugger
#include "logMsg/logMsg.h"		// LM_M()
#include "au/string.h"

#include "au/mutex/Token.h"				// Own interface

//#define DEBUG_AU_TOKEN

NAMESPACE_BEGIN(au)

std::string get_thread_id( pthread_t t )
{
    std::ostringstream output;

    unsigned char *base = (unsigned char*) &t;
    size_t i;
    for (i = 0; i < sizeof(t); i ++)
    {
        output << au::str("%02x", base[i]);
    }
    return output.str();
}

Token::Token( const char * _name )
{

    // Take the name for debuggin
    name = _name;

    //LM_LM(("Token %s created" , name ));

    int r_init = pthread_mutex_init(&_lock, 0);

    if( r_init != 0 )
        LM_X(1, ("pthread_mutex_init for '%s' returned %d" , name , r_init ));

    int t_init_cond = pthread_cond_init(&_block, NULL);

    if( t_init_cond != 0 )
        LM_X(1, ("pthread_cond_init for '%s' returned %d" , name , r_init ));

    locked = false;
}

Token::~Token()
{
    //LM_LM(("Token %s was destroyed" , name ));

    //LM_M(("Destroying token '%s' (%p)", name, this));
    pthread_mutex_destroy(&_lock);
    pthread_cond_destroy(&_block);
}

void Token::retain(  )
{
    //LM_LM(("Thread [%s] trying to token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    if( locked && pthread_equal(pthread_self(), t ) )
    {
        counter++;
        //LM_LM(("token %s was retained by me ( thread [%s] ) . Just updating counter to %d " , name , get_thread_id( pthread_self() ).c_str() ,  counter ));
        return;
    }

#ifdef DEBUG_AU_TOKEN
    LockDebugger::shared()->add_lock( this );
#endif
    // LOCK the mutex
    int ans = pthread_mutex_lock(&_lock);
    if( ans )
    {
        LM_LE(("Error %d getting mutex  (EINVAL:%d, EFAULT:%d, EDEADLK:%d", ans, EINVAL, EFAULT, EDEADLK));
        //assert(false);
        if ((name != NULL) && (name != (char *)0xffffffff) && (name > (char *)0x10000000) && ((name[0] > 'a') && (name[0] < 'Z')) && ((name[1] > 'a') && (name[1] < 'Z')))
        {
            LM_LE(("Token %s: pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
        else
        {
            LM_LE(("Token (wrong name:%p): pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
    }

    if( locked )
        LM_X(1,("Internal error: Thread [%s] has retained au::Token (%s) previously locked by thread [%s]" , get_thread_id( pthread_self() ).c_str() , name , get_thread_id( t ).c_str() ));

    t = pthread_self();
    counter = 1;
    locked = true;

    //LM_LM(("Thread [%s] has retained token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

}

void Token::release( )
{
    //LM_LM(("Thread [%s] trying to releases token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    if( !locked )
        LM_E(("Internal error: Releasing a non-locked au::Token"));

    if( ! pthread_equal(pthread_self(), t) )
        LM_E(("Internal error: Releasing an au::Token not locked by me"));

    counter--;
    if( counter > 0 )
    {
        //LM_LM(("Token %s is still locked by thread [%s] with counter %d" , name , get_thread_id( pthread_self() ).c_str() , counter ));
        return;
    }

    // Flag this as unlocked
    locked = false;

#ifdef DEBUG_AU_TOKEN
    LockDebugger::shared()->remove_lock( this );
#endif
    // UNLOCK the mutex
    int ans = pthread_mutex_unlock(&_lock);
    if( ans )
    {
        LM_E(("Error %d releasing mutex (EINVAL:%d, EFAULT:%d, EPERM:%d", ans, EINVAL, EFAULT, EPERM));
        // Goyo. The segmentation fault when quitting delilah seems to be related to a corruption in name (SAMSON-314)
        if (name != NULL)
        {
            LM_E(("Token %p: pthread_mutex_unlock returned error %d (%p)", name, ans, this));
        }
        else
        {
            LM_E(("Token (NULL name:%p): pthread_mutex_lock returned error %d (%p)", name, ans, this));
        }
    }

    //LM_LM(("Thread [%s] completelly releases by token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

}

void Token::stop()
{
    //LM_LM(("Thread [%s] is being stopeed at token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    // You are suppoused to be retaining this lock
    if( !locked )
        LM_E(("Internal error: Releasing a non-locked au::Token"));

    if( !pthread_equal(pthread_self(), t) )
        LM_E(("Internal error: Releasing an au::Token not locked by me"));

    int tmp_counter = counter; // Keep counter of retains
    locked = false;            // We are temporally releasing this token

    // This unlock the mutex and froze the process in the condition
    if( pthread_cond_wait(&_block, &_lock) != 0 )
        LM_X(1, ("Internal error at au::TokenTaker"));

    //LM_LM(("Thread [%s] is back from stopeed at token %s..." , get_thread_id( pthread_self() ).c_str() ,  name ));

    // Now you are retaining again
    locked = true;
    t = pthread_self();
    counter = tmp_counter;

}

NAMESPACE_END
<|endoftext|>