text:
/** @file
    @brief Implementation

    @date 2015, 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015, 2016 Sensics, Inc.
//
// 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 "unifiedvideoinertial/Stride.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
static ::util::Stride debugStride{401};

#if 0
template <typename T>
inline void dumpKalmanDebugOuput(const char name[], const char expr[],
                                 T const &value) {
    if (debugStride) {
        std::cout << "\n(Kalman Debug Output) " << name << " [" << expr
                  << "]:\n" << value << std::endl;
    }
}
#define UVBI_KALMAN_DEBUG_OUTPUT(Name, Value)                                  \
    dumpKalmanDebugOuput(Name, #Value, Value)
#endif

// Internal Includes
#include "ImagePointMeasurement.h"
#include "LED.h"
#include "PinholeCameraFlip.h"
#include "PoseEstimator_SCAATKalman.h"
#include "UsefulQuaternions.h"
#include "videotrackershared/cvToEigen.h"

// Library/third-party includes
#include "FlexKalman/AugmentedProcessModel.h"
#include "FlexKalman/AugmentedState.h"
#include "FlexKalman/ConstantProcess.h"
#include "FlexKalman/FlexibleKalmanFilter.h"

#include "unifiedvideoinertial/Stride.h"

// Standard includes
#include <algorithm>
#include <iostream>
#include <iterator> // back_inserter
#include <random>

#undef UVBI_DEBUG_VELOCITY
#undef UVBI_VARIANCE_PENALTY_FOR_FIXED_BEACONS
#undef UVBI_CHECK_BOUNDING_BOXES
#undef UVBI_TRY_LIMITING_ANGULAR_VELOCITY_CHANGE
#undef UVBI_DEBUG_EMISSION_DIRECTION

namespace videotracker {
namespace uvbi {
    static const auto DIM_BEACON_CUTOFF_TO_SKIP_BRIGHTS = 4;

    /// If we have more that this number of supposedly misidentifieds per their
    /// residual, and more misidentifieds than used, then we don't actually mark
    /// them misidentified, just skip them and count them as bad (which will
    /// tend to cause us to reset the model)
    static const auto MISIDENTIFIED_BEACON_CUTOFF = 3;

    /// 4, so that if the residual itself is 2x the max residual, we reject the
    /// identification.
    static const auto SQUARED_MAX_RESIDUAL_FACTOR_FOR_ID_REJECT = 4;
    SCAATKalmanPoseEstimator::SCAATKalmanPoseEstimator(
        ConfigParams const &params)
        : m_shouldSkipBright(params.shouldSkipBrightLeds),
          m_maxResidual(params.maxResidual),
          m_maxSquaredResidual(params.maxResidual * params.maxResidual),
          m_maxZComponent(params.maxZComponent),
          m_highResidualVariancePenalty(params.highResidualVariancePenalty),
          m_beaconProcessNoise(params.beaconProcessNoise),
          m_noveltyPenaltyBase(params.tuning.noveltyPenaltyBase),
          m_noBeaconLinearVelocityDecayCoefficient(
              params.noBeaconLinearVelocityDecayCoefficient),
          m_brightLedVariancePenalty(params.brightLedVariancePenalty),
          m_measurementVarianceScaleFactor(
              params.measurementVarianceScaleFactor),
          m_distanceMeasVarianceBase(params.tuning.distanceMeasVarianceBase),
          m_distanceMeasVarianceIntercept(
              params.tuning.distanceMeasVarianceIntercept),
          m_extraVerbose(params.extraVerbose),
          m_randEngine(std::random_device()()) {
        std::tie(m_minBoxRatio, m_maxBoxRatio) =
            std::minmax({params.boundingBoxFilterRatio,
                         1.f / params.boundingBoxFilterRatio});

        const auto maxSquaredResidual = params.maxResidual * params.maxResidual;
    }

    inline double xyDistanceFromMetersToPixels(double xyDistance,
                                               double depthInMeters,
                                               CameraModel const &cam) {
        return (xyDistance / depthInMeters) * cam.focalLength;
    }
    inline double squaredXyDistanceFromMetersToPixels(double xyDistance,
                                                      double depthInMeters,
                                                      CameraModel const &cam) {
        return (xyDistance * xyDistance / (depthInMeters * depthInMeters)) *
               cam.focalLength * cam.focalLength;
    }

    bool SCAATKalmanPoseEstimator::
    operator()(EstimatorInOutParams const &p, LedPtrList const &leds,
               videotracker::util::TimeValue const &frameTime, double videoDt) {
        bool gotMeasurement = false;
        double varianceFactor = 1;

        const auto inBoundsID = leds.size();
        // Default to using all the measurements we can
        auto skipBright = false;

        auto skipAll = false;
        auto numBeaconsToUse = std::size_t{0};
        {
            auto inBoundsBright = std::size_t{0};
            auto inBoundsRound = std::size_t{0};

            /// Count up types of beacons
            for (auto const &ledPtr : leds) {
                auto &led = *ledPtr;
                if (led.isBright()) {
                    inBoundsBright++;
                }

                auto boundingBoxRatioResult = inBoundingBoxRatioRange(led);
                if (TriBool::True == boundingBoxRatioResult) {
                    inBoundsRound++;
                }
            }

            /// Decisions based on the counts.
            if (inBoundsID - inBoundsBright >
                    DIM_BEACON_CUTOFF_TO_SKIP_BRIGHTS &&
                m_shouldSkipBright) {
                skipBright = true;
            }
            if (0 == inBoundsID) {
                m_framesWithoutIdentifiedBlobs++;
            } else {
                m_framesWithoutIdentifiedBlobs = 0;
            }
            numBeaconsToUse =
                skipBright ? (inBoundsID - inBoundsBright) : inBoundsID;
            if (m_lastUsableBeaconsSeen > 0 && numBeaconsToUse == 1) {
                // If we're losing sight of beacons and we can only see one,
                // it's likely to give us nasty velocity, so in that case we
                // skip it too.
                skipAll = true;
                // Set this here, because goodLeds will be empty below.
                m_lastUsableBeaconsSeen = numBeaconsToUse;
            }
        }

        if (p.startingTime != frameTime) {
            /// Predict first if appropriate.
            auto dt = util::time::duration(frameTime, p.startingTime);
            // auto dt = uvbiTimeValueDurationSeconds(&frameTime,
            // &p.startingTime);
            flexkalman::predict(p.state, p.processModel, dt);
            p.state.externalizeRotation();
        }

        auto numBad = std::size_t{0};
        auto numGood = std::size_t{0};
        auto goodLeds = filterLeds(leds, skipBright, skipAll, numBad, p);

        if (!skipAll) {
            // if we were in skipAll mode, we set this above.
            m_lastUsableBeaconsSeen = goodLeds.size();
        }

        if (goodLeds.empty() && p.startingTime != frameTime) {
            /// If we have no good LEDs, let's attenuate the linear velocity
            /// quickly: avoid coasting/gliding.
            auto dt = util::time::duration(frameTime, p.startingTime);
            auto atten = std::pow(m_noBeaconLinearVelocityDecayCoefficient, dt);
            p.state.velocity() *= atten;
        }

        /// Shuffle the order of the good LEDS
        std::shuffle(begin(goodLeds), end(goodLeds), m_randEngine);

#if 0
        static ::util::Stride varianceStride{ 809 };
        if (++varianceStride) {
            std::cout << p.state.errorCovariance().diagonal().transpose()
                << std::endl;
        }
#endif

        CameraModel cam;
        cam.focalLength = p.camParams.focalLength();
        cam.principalPoint = p.camParams.eiPrincipalPoint();
        ImagePointMeasurement meas{cam, p.targetToBody};

        flexkalman::ConstantProcess<flexkalman::PureVectorState<>>
            beaconProcess;

        for (auto &ledPtr : goodLeds) {
            auto &led = *ledPtr;

            auto id = led.getID();
            auto index = asIndex(id);

            auto &debug = p.beaconDebug[index];

            auto localVarianceFactor = varianceFactor;
            auto newIdentificationVariancePenalty =
                std::pow(m_noveltyPenaltyBase, led.novelty());

            /// Stick a little bit of process model uncertainty in the beacon,
            /// if it's meant to have some
            if (p.beaconFixed[index]) {
                beaconProcess.setNoiseAutocorrelation(0);
#ifdef UVBI_VARIANCE_PENALTY_FOR_FIXED_BEACONS
                /// Add a bit of variance to the fixed ones, since the lack of
                /// beacon autocalib otherwise make them seem
                /// super-authoritative.
                localVarianceFactor *= 2;
#endif
            } else {
                beaconProcess.setNoiseAutocorrelation(m_beaconProcessNoise);
                flexkalman::predict(*(p.beacons[index]), beaconProcess,
                                    videoDt);
            }

            /// subtracting from image size to flip signs of x and y, aka 180
            /// degree rotation about z axis.
            meas.setMeasurement(
                cvToVector(led.getLocationForTracking()).cast<double>());

            auto state =
                flexkalman::makeAugmentedState(p.state, *(p.beacons[index]));
            meas.updateFromState(state);

            /// Investigate measurement residual here (difference in measurement
            /// space from expected measurement based on the model) to decide if
            ///
            /// - it's a reasonable measurement
            /// - it's a little unreasonable (and should get some extra variance
            ///   to indicate we think it's a poor-quality measurement)
            /// - it's totally unreasonable and should be de-identified and
            ///   skipped (because we've probably mis-identified something in
            ///   the environment as a beacon)
            Eigen::Vector2d residual = meas.getResidual(state);
            auto squaredResidual = residual.squaredNorm();
            // Compute what the squared, pixel-space residual would be for the
            // configured, max-tolerable residual in meters at the beacon depth
            // before applying the penalty
            auto depth = meas.getBeaconInCameraSpace().z();
            auto maxSquaredResidual =
                squaredXyDistanceFromMetersToPixels(m_maxResidual, depth, cam);
            if (squaredResidual > maxSquaredResidual) {
                // OK, it's bad, but is it really bad?

                /// Have to still count it as bad, in case it's our model, not
                /// the beacon, that's actually bad.
                numBad++;

                // Let's see if it's really bad and thus likely actually some
                // other object that we've mis-recognized as a beacon, like a
                // lighthouse base station.
                if (squaredResidual >
                    squaredXyDistanceFromMetersToPixels(
                        SQUARED_MAX_RESIDUAL_FACTOR_FOR_ID_REJECT *
                            m_maxResidual,
                        depth, cam)) {
                    // Yeah, it's really bad, throw it out!
                    markAsPossiblyMisidentified(led);
                    continue;
                }

                // OK, it's just probably a low-quality measurement but
                // not a measurement of something else.
                localVarianceFactor *= m_highResidualVariancePenalty;
            } else {
                // It's reasonable!
                numGood++;
            }

            /// That was the last place we'd reject an LED, so now we can say
            /// for sure we're using this one.
            markAsUsed(led);

            debug.residual.x = residual.x();
            debug.residual.y = residual.y();
#if 0
            /// ad-hoc estimated variance computation from experimentation,
            /// tuned with optimizer and m_measurementVarianceScaleFactor
            auto effectiveVariance =
                localVarianceFactor * m_measurementVarianceScaleFactor *
                newIdentificationVariancePenalty *
                (led.isBright() ? m_brightLedVariancePenalty : 1.) *
                p.beaconMeasurementVariance[index] / led.getMeasurement().area;
#else
            /// Typically, all of these will be 1 except for the variance from
            /// beacon depth.
            /// At some distance past 55cm, bright LEDs actually have lower than
            /// average variances instead of higher, but the overall mean
            /// variance follows an exponential decay trend with distance.
            auto effectiveVariance = localVarianceFactor *
                                     m_measurementVarianceScaleFactor *
                                     newIdentificationVariancePenalty *
                                     p.beaconMeasurementVariance[index] *
                                     getVarianceFromBeaconDepth(depth);
#endif
            debug.variance = effectiveVariance;
            meas.setVariance(effectiveVariance);

            /// Now, do the correction.
            auto model = flexkalman::makeAugmentedProcessModel(p.processModel,
                                                               beaconProcess);

            auto correction =
                flexkalman::beginExtendedCorrection(state, model, meas);
            if (!correction.stateCorrectionFinite) {
                std::cout << "Non-finite state correction processing beacon "
                          << led.getOneBasedID().value() << std::endl;
                continue;
            }
#ifdef UVBI_TRY_LIMITING_ANGULAR_VELOCITY_CHANGE
            /// this is the velocity correction:
            /// correction.stateCorrection.segment<3>(6)
            /// this is the angular velocity correction:
            /// correction.stateCorrection.segment<3>(9)

            /// These are, in practice, surprisingly high...
            /// as well as dependent on the variances...
            static const auto MaxAngVelChangeFromOneBeacon = 3 * EIGEN_PI;
            static const auto MaxAnglVelChangeSquared =
                MaxAngVelChangeFromOneBeacon * MaxAngVelChangeFromOneBeacon;
            auto angVelChangeSquared =
                correction.stateCorrection.segment<3>(9).squaredNorm();
            if (angVelChangeSquared > MaxAnglVelChangeSquared) {
                std::cout << "Got too high of a angular velocity correction "
                             "from report from "
                          << led.getOneBasedID().value() << ": magnitude "
                          << std::sqrt(angVelChangeSquared) << std::endl;
                continue;
            }
#endif
            correction.finishCorrection();

            gotMeasurement = true;
        }

        handlePossiblyMisidentifiedLeds();

        if (gotMeasurement) {
            // Re-symmetrize error covariance.
            using BodySquareMatrix = flexkalman::types::SquareMatrix<
                flexkalman::getDimension<BodyState>()>;
            BodySquareMatrix cov = 0.5 * p.state.errorCovariance() +
                                   0.5 * p.state.errorCovariance().transpose();
            p.state.errorCovariance() = cov;

#ifdef UVBI_DEBUG_VELOCITY
            {
                static ::util::Stride s(77);
                if (++s) {
                    if (p.state.velocity().squaredNorm() > 0.01) {
                        // fast enough to say something about
                        std::cout
                            << "Velocity: " << p.state.velocity().transpose()
                            << "\n";
                    }
                    double angSpeed = p.state.angularVelocity().norm();
                    if (angSpeed > 0.1) {
                        std::cout << "AngVel: " << angSpeed << " about "
                                  << (p.state.angularVelocity() / angSpeed)
                                         .transpose()
                                  << "\n";
                    }
                }
            }
#endif
        }

        /// Probation: Dealing with ratios of bad to good residuals
        bool incrementProbation = false;
        if (0 == m_framesInProbation) {
            // Let's try to keep a 3:2 ratio of good to bad when not "in
            // probation"
            incrementProbation = (numBad * 3 > numGood * 2);

        } else {
            // Already in trouble, add a bit of hysteresis and raising the bar
            // so we don't hop out easily.
            incrementProbation = numBad * 2 > numGood;
            if (!incrementProbation) {
                // OK, we're good again
                m_framesInProbation = 0;
            }
        }
        if (incrementProbation) {
            m_framesInProbation++;
        }

        /// Frames without measurements: dealing with getting in a bad state
        if (gotMeasurement) {
            m_framesWithoutUtilizedMeasurements = 0;
        } else {
            if (inBoundsID > 0) {
                /// We had a measurement, we rejected it. The problem may be the
                /// plank in our own eye, not the speck in our beacon's eye.
                m_framesWithoutUtilizedMeasurements++;
            }
        }

        if (gotMeasurement && p.state.position().z() < 0) {
            if (m_extraVerbose) {
                std::cout << "Kalman detected wrong side of the looking glass! "
                             "We can't be behind the camera and be seen, but "
                             "our position is apparently "
                          << p.state.position().transpose() << std::endl;
            }
            /// We're on the wrong side of the camera #fail
            /// This is a fix-up after the fact, rather than a prevention: if we
            /// get here, we goofed up earlier - but better late than never.
            Eigen::Quaterniond quat = p.state.getQuaternion();
            pinholeCameraFlipPose(p.state.position(), quat);
            p.state.setQuaternion(quat);

            pinholeCameraFlipVelocities(p.state.velocity(),
                                        p.state.angularVelocity());
#if 0
            /// invert position and velocity
            p.state.position() *= -1;
            p.state.velocity() *= -1;

            /// rotate orientation/angvel 180 about z.
            p.state.angularVelocity() =
                get180aboutZ() * p.state.angularVelocity();
            p.state.setQuaternion(get180aboutZ() * p.state.getQuaternion());
#endif
        }
        return true;
    }

    LedPtrList SCAATKalmanPoseEstimator::filterLeds(
        LedPtrList const &leds, const bool skipBright, const bool skipAll,
        std::size_t &numBad, EstimatorInOutParams const &p) {
        LedPtrList ret;

        /// @todo should we be recalculating this for each beacon after each
        /// correction step? The order we filter them in is rather arbitrary...
        const Eigen::Matrix3d rotate =
            p.state.getCombinedQuaternion().toRotationMatrix();
        /// @todo use this for more speed?
        // Eigen::RowVector3d zRotate = rotate.row(2);

        std::copy_if(
            begin(leds), end(leds), std::back_inserter(ret), [&](Led *ledPtr) {
                auto &led = *ledPtr;

                auto id = led.getID();
                auto index = asIndex(id);

                auto &debug = p.beaconDebug[index];
                debug.seen = true;
                debug.measurement = led.getLocationForTracking();
                if (skipAll) {
                    return false;
                }

                // Angle of emission checking
                // If we transform the emission vector into camera space, an LED
                // pointed right at the camera will be -Z. Anything with a 0 or
                // positive z component is clearly out, and realistically,
                // anything with a z component over -0.5 is probably fairly
                // oblique. We don't want to use such beacons since they can
                // easily introduce substantial error.
                double zComponent =
                    (rotate * cvToVector(p.beaconEmissionDirection[index])).z();
#if UVBI_DEBUG_EMISSION_DIRECTION

                /// Beacon 32 is right on the front, should be facing nice and
                /// forward.
                // static const auto beaconInspected = OneBasedBeaconId(32);
                /// Beacon 10 is on the right side
                // static const auto beaconInspected = OneBasedBeaconId(10);
                /// Beacon 5 on on the left side
                static const auto beaconInspected = OneBasedBeaconId(5);
                /// Beacon 9 is on the top

                if (makeOneBased(id) == beaconInspected) {
                    static ::util::Stride s(20);
                    if (s) {
                        auto altZ =
                            zRotate *
                            cvToVector(p.beaconEmissionDirection[index]);
                        std::cout
                            << "Beacon " << beaconInspected.value()
                            << ": alternate z component is " << altZ
                            << " and beacon emission direction is "
                            << (rotate *
                                cvToVector(p.beaconEmissionDirection[index]))
                                   .transpose()
                            << std::endl;
                    }
                    s.advance();
                }
#endif
                if (zComponent > 0.) {
                    if (m_extraVerbose) {
                        std::cout
                            << "Rejecting an LED at "
                            << led.getLocationForTracking() << " claiming ID "
                            << led.getOneBasedID().value()
                            << " thus emission vec "
                            << (rotate *
                                cvToVector(p.beaconEmissionDirection[index]))
                                   .transpose()
                            << std::endl;
                    }
                    /// This means the LED is pointed away from us - so we
                    /// shouldn't be able to see it.
                    markAsPossiblyMisidentified(led);

                    /// @todo This could be a mis-identification, or it could
                    /// mean we're in a totally messed up state. Do we count
                    /// this against ourselves?
                    numBad++;
                    return false;
                }
                if (zComponent > m_maxZComponent) {
                    /// LED is too askew of the camera to provide reliable data,
                    /// so skip it.
                    if (m_extraVerbose) {
                        std::cout << "Skipping " << led.getOneBasedID().value()
                                  << " with z component " << zComponent
                                  << std::endl;
                    }
                    return false;
                }

                if (skipBright && led.isBright()) {
                    return false;
                }

#ifdef UVBI_CHECK_BOUNDING_BOXES
                /// @todo For right now, if we don't have a bounding box, we're
                /// assuming it's square enough (and only testing for
                /// non-squareness on those who actually do have bounding
                /// boxes). This is very much a temporary situation.
                auto boundingBoxRatioResult = inBoundingBoxRatioRange(led);
                if (TriBool::False == boundingBoxRatioResult) {
                    /// skip non-circular blobs.
                    numBad++;
                    return false;
                }
#endif
                return true;
            });
        return ret;
    }

    SCAATKalmanPoseEstimator::TriBool
    SCAATKalmanPoseEstimator::inBoundingBoxRatioRange(Led const &led) {
        if (led.getMeasurement().knowBoundingBox()) {
            auto boundingBoxRatio =
                led.getMeasurement().boundingBoxSize().height /
                led.getMeasurement().boundingBoxSize().width;
            if (boundingBoxRatio > m_minBoxRatio &&
                boundingBoxRatio < m_maxBoxRatio) {
                return TriBool::True;
            }
            return TriBool::False;
        }
        return TriBool::Unknown;
    }

    void SCAATKalmanPoseEstimator::markAsPossiblyMisidentified(Led &led) {
        m_possiblyMisidentified.push_back(&led);
    }

    void SCAATKalmanPoseEstimator::markAsUsed(Led &led) {
        led.markAsUsed();
        m_ledsUsed++;
    }

    double SCAATKalmanPoseEstimator::getVarianceFromBeaconDepth(double depth) {
        return std::pow(m_distanceMeasVarianceBase, depth * 100.) *
               m_distanceMeasVarianceIntercept;
    }

    void SCAATKalmanPoseEstimator::handlePossiblyMisidentifiedLeds() {
        m_ledsConsideredMisidentifiedLastFrame = m_possiblyMisidentified.size();
        if (m_ledsConsideredMisidentifiedLastFrame > m_ledsUsed &&
            m_ledsConsideredMisidentifiedLastFrame >
                MISIDENTIFIED_BEACON_CUTOFF) {
            std::cout << "Think our model is wrong: considered "
                      << m_ledsConsideredMisidentifiedLastFrame
                      << " beacons misidentified, while only used "
                      << m_ledsUsed << std::endl;
            m_misIDConsideredOurFault = true;
        } else {
            m_misIDConsideredOurFault = false;
            for (auto &ledPtr : m_possiblyMisidentified) {
                ledPtr->markMisidentified();
            }
        }

        // Get ready for the next frame.
        m_ledsUsedLastFrame = m_ledsUsed;
        m_ledsUsed = 0;
        m_possiblyMisidentified.clear();
    }

    // The total number of frames that we can have dodgy Kalman tracking for
    // before RANSAC takes over again.
    static const std::size_t MAX_PROBATION_FRAMES = 10;

    // static const std::size_t MAX_FRAMES_WITHOUT_MEASUREMENTS = 100;

    static const std::size_t MAX_FRAMES_WITHOUT_ID_BLOBS = 25;

    SCAATKalmanPoseEstimator::TrackingHealth
    SCAATKalmanPoseEstimator::getTrackingHealth() {
        // Reset immediately if we have a large number of mis-identified beacons
        // that we decided were a model error, not a recognition error.
        if (m_misIDConsideredOurFault) {
            return TrackingHealth::NeedsHardResetNow;
        }

        auto needsReset = (m_framesInProbation > MAX_PROBATION_FRAMES);
#if 0
			||
                          (m_framesWithoutUtilizedMeasurements >
                           MAX_FRAMES_WITHOUT_MEASUREMENTS);
#endif
        if (needsReset) {
            return TrackingHealth::NeedsHardResetNow;
        }
        /// Additional case: We've been a while without blobs...
        if (m_framesWithoutIdentifiedBlobs > MAX_FRAMES_WITHOUT_ID_BLOBS) {
            // In this case, we force ransac once we get blobs once again.
            return TrackingHealth::SoftResetWhenBeaconsSeen;
        }
        /// Otherwise, we're doing OK!
        return TrackingHealth::Functioning;
    }
} // namespace uvbi
} // namespace videotracker


text:
#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

struct CollisionConstraint {
public:
    virtual ~CollisionConstraint() {}

    virtual std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const = 0;

    virtual double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const;

    virtual VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const;

    virtual MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const;

    double minimum_distance = 0;
};

///////////////////////////////////////////////////////////////////////////////

struct VertexVertexConstraint : VertexVertexCandidate, CollisionConstraint {
    VertexVertexConstraint(long vertex0_index, long vertex1_index);
    VertexVertexConstraint(const VertexVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex0_index, vertex1_index } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    unsigned int multiplicity = 1;
};

///////////////////////////////////////////////////////////////////////////////

struct EdgeVertexConstraint : EdgeVertexCandidate, CollisionConstraint {
    EdgeVertexConstraint(long edge_index, long vertex_index);
    EdgeVertexConstraint(const EdgeVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex_index, E(edge_index, 0), E(edge_index, 1) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    unsigned int multiplicity = 1;
};

///////////////////////////////////////////////////////////////////////////////

struct EdgeEdgeConstraint : EdgeEdgeCandidate, CollisionConstraint {
    EdgeEdgeConstraint(long edge0_index, long edge1_index, double eps_x);
    EdgeEdgeConstraint(const EdgeEdgeCandidate& candidate, double eps_x);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { E(edge0_index, 0), E(edge0_index, 1), //
                   E(edge1_index, 0), E(edge1_index, 1) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    double eps_x;
};

///////////////////////////////////////////////////////////////////////////////

struct FaceVertexConstraint : FaceVertexCandidate, CollisionConstraint {
    FaceVertexConstraint(long face_index, long vertex_index);
    FaceVertexConstraint(const FaceVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex_index, //
                   F(face_index, 0), F(face_index, 1), F(face_index, 2) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;
};

///////////////////////////////////////////////////////////////////////////////

struct PlaneVertexConstraint : CollisionConstraint {
    PlaneVertexConstraint(
        const VectorMax3d& plane_origin,
        const VectorMax3d& plane_normal,
        const long vertex_index);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { vertex_index };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax3d plane_origin;
    VectorMax3d plane_normal;
    long vertex_index;
};

///////////////////////////////////////////////////////////////////////////////

struct Constraints {
    std::vector<VertexVertexConstraint> vv_constraints;
    std::vector<EdgeVertexConstraint> ev_constraints;
    std::vector<EdgeEdgeConstraint> ee_constraints;
    std::vector<FaceVertexConstraint> fv_constraints;
    std::vector<PlaneVertexConstraint> pv_constraints;

    size_t size() const;

    size_t num_constraints() const;

    bool empty() const;

    void clear();

    CollisionConstraint& operator[](size_t idx);
    const CollisionConstraint& operator[](size_t idx) const;
};

} // namespace ipc

ext: hpp

max_stars_repo_name: ipc-sim/ipc-toolk

max_issues_repo_name: dbelgrod/ipc-toolkit

max_forks_repo_name: dbelgrod/ipc-toolkit


text:
//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_QVM_57E1C032B9F311DEB7D9BAFE55D89593
#define BOOST_QVM_57E1C032B9F311DEB7D9BAFE55D89593

#include <boost/qvm/quat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/inline.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        template <class Scalar>
        struct
        scalar_traits
            {
            static
            BOOST_QVM_INLINE_CRITICAL
            Scalar
            value( int v )
                {
                return Scalar(v);
                }
            };

        template <class T>
        struct
        is_scalar
            {
            static bool const value=false;
            };
        template <> struct is_scalar<char> { static bool const value=true; };
        template <> struct is_scalar<signed char> { static bool const value=true; };
        template <> struct is_scalar<unsigned char> { static bool const value=true; };
        template <> struct is_scalar<signed short> { static bool const value=true; };
        template <> struct is_scalar<unsigned short> { static bool const value=true; };
        template <> struct is_scalar<signed int> { static bool const value=true; };
        template <> struct is_scalar<unsigned int> { static bool const value=true; };
        template <> struct is_scalar<signed long> { static bool const value=true; };
        template <> struct is_scalar<unsigned long> { static bool const value=true; };
        template <> struct is_scalar<float> { static bool const value=true; };
        template <> struct is_scalar<double> { static bool const value=true; };
        template <> struct is_scalar<long double> { static bool const value=true; };

        namespace
        qvm_detail
            {
            template <class A,bool M=is_mat<A>::value,bool Q=is_quat<A>::value,bool V=is_vec<A>::value>
            struct
            scalar_impl
                {
                };

            template <class A>
            struct
            scalar_impl<A,true,false,false>
                {
                typedef typename mat_traits<A>::scalar_type type;
                };

            template <class A>
            struct
            scalar_impl<A,false,true,false>
                {
                typedef typename quat_traits<A>::scalar_type type;
                };

            template <class A>
            struct
            scalar_impl<A,false,false,true>
                {
                typedef typename vec_traits<A>::scalar_type type;
                };
            }

        template <class A>
        struct
        scalar
            {
            typedef typename qvm_detail::scalar_impl<A>::type type;
            };
        }
    }

#endif

max_stars_repo_name: mamil/demo

max_issues_repo_name: qingkouwei/mediaones

max_forks_repo_name: qingkouwei/mediaones


text:
/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef GEOMETRY_HH
#define GEOMETRY_HH

#include <deal.II/distributed/tria.h>

#include <boost/property_tree/ptree.hpp>

namespace adamantine
{
/**
 * This class generates and stores a Triangulation given a database.
 */
template <int dim>
class Geometry
{
public:
  /**
   * Constructor.
   */
  Geometry(MPI_Comm const &communicator,
           boost::property_tree::ptree const &database);

  /**
   * Return the underlying Triangulation.
   */
  dealii::parallel::distributed::Triangulation<dim> &get_triangulation();

private:
  /**
   * Shared pointer to the underlying Triangulation.
   */
  dealii::parallel::distributed::Triangulation<dim> _triangulation;

  /**
   * Assign the material state to the mesh.
   */
  void assign_material_state(boost::property_tree::ptree const &database);
};

template <int dim>
inline dealii::parallel::distributed::Triangulation<dim> &
Geometry<dim>::get_triangulation()
{
  return _triangulation;
}
} // namespace adamantine

#endif

ext: hh

max_stars_repo_name: Rombur/adamantine

max_issues_repo_name: Rombur/adamantine

max_forks_repo_name: Rombur/adamantine


text:
/*!
 * \file config_file.cpp
 *  The file interface to the config class.
*/
// Config Class
// Author: Charles Gruenwald III
#include "config_file.hpp"
#include "config_exceptions.hpp"
#include "itostr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <errno.h>

#include <boost/algorithm/string.hpp>
#include <boost/version.hpp>
#include <boost/lexical_cast.hpp>

namespace config
{

    ConfigFile::ConfigFile(bool case_sensitive)
        :
            Config(case_sensitive)
    {
    }

    ConfigFile::ConfigFile(const Section & root, bool case_sensitive)
        :
            Config(root, case_sensitive)
    {
    }

    //load a given filename into a string, taken from boost regexp replace example
    void ConfigFile::loadFileToString(String& s, const String& filename)
    {
        std::ifstream file_in(filename.c_str());

        s.erase();
        s.reserve(file_in.rdbuf()->in_avail());
        char c;
        while(file_in.get(c))
        {
            if(s.capacity() == s.size())
                s.reserve(s.capacity() * 3);
            s.append(1, c);
        }
        file_in.close();
    }

    void ConfigFile::loadConfig()
    {
        String filename = m_path;

        //Make sure the file exists
        if(access(filename.c_str(), R_OK) != 0)
        {
            throw FileNotFound(filename);
        }

        //Read the data in from the file
        String data;
        loadFileToString(data, filename);
        loadConfigFromString(data);

    }

    void ConfigFile::loadConfigFromString(const String & cfg)
    {
        parse(cfg, m_root);
    }

    //Build the in-memory representation by line-by-line matching boost::regex expressions
    void ConfigFile::parse(const String &source, Section & current)
    {
        //rules
        config_parser parser;
        parse_info_t info = ast_parse<config_parser::factory_t>(source.c_str(), source.c_str() + source.size(),parser,space_p | comment_p("#"));

        if(info.full)
        {
            // Uncomment the following lines for more verbose output
//            showParseTree(info.trees.begin());
            evalTree(m_root, info.trees.begin());
        }
        else
        {
            String error_str(info.stop);
            throw parserError(error_str);
        }
    }

    void ConfigFile::showParseTree(tree_iter_t const& node, int depth)
    {
        String tabs = "";
        for(int i=0;i<depth;i++)
            tabs = tabs.append("    ");

        RuleID rule = node->value.value().e;

        // get the string value from the iter
        String value(node->value.begin(), node->value.end() - node->value.begin());
        if(rule == sectionID)
            value = "s: ";

        if(rule == configID)
            value = "c: " + value;

        else if(rule == keyID)
            value = "k: " + value;

        else if(rule == keyValueID)
            value = "kv: " + value;

        else if(rule == keyNameID)
            value = "kn: " + value;

        else if(rule == sectionNameID)
            value = "sn: " + value;

        else if(rule == keyValueArrayID)
            value = "kva: " + value;

        else if(rule == keyValueSpanID)
            value = "kvs: " + value;

        // if(rule == sectionID || rule == keyID || rule == keyValueID || rule == keyNameID || rule == sectionNameID)
        // if(rule > 0)
            std::cout << tabs << (int)(rule) << ".[" << value << "]" << std::endl;

        for(tree_iter_t chi = node->children.begin(); chi != node->children.end(); chi++)
        {
            showParseTree(chi, depth + 1);
        }
    }

    void ConfigFile::unEscapeText(const String & source, String & dest)
    {
        bool backslash = false;
        unsigned int start = 0;
        unsigned int end = source.size();

        //remove surrounding parens if necessary
        if(source.size() >= 2 && source.c_str()[0] == '"' && source.c_str()[source.size() - 1] == '"')
        {
            ++start;
            --end;
        }

        for (unsigned int i = start; i < end; i++) {
            char c = source.c_str()[i];
            if (backslash)
            {
                switch (c)
                {
                    case '\\': dest += "\\"; break;
                    case 'b':  dest += "\b"; break;
                    case 'r':  dest += "\r"; break;
                    case 'f':  dest += "\f"; break;
                    case 't':  dest += "\t"; break;
                    case 'n':  dest += "\n"; break;
                    case '"':  dest += "\""; break;
                    case '\'': dest += "'";  break;
                    default:   dest += c;
                }
                backslash = false;
            }
            else
            {
                if (c == '\\')
                    backslash = true;
                else
                    dest += c;
            }
        }
    }

    String ConfigFile::getNodeValue(tree_iter_t const& node)
    {
        if(node->value.begin() == node->value.end())
            return "";
        return String(node->value.begin(), node->value.end());
    }

    RuleID ConfigFile::getNodeID(tree_iter_t const& node)
    {
        return node->value.value().e;
    }

    void ConfigFile::escapeText(const String & source, String & dest)
    {
        for (unsigned int i = 0; i < source.size(); i++) {
            char c = source.c_str()[i];
                switch (c)
                {
                    case '\\': dest += "\\\\"; break;
                    case '\"': dest += "\\\""; break;
                    default:   dest += c;
                }
        }
    }

    // This function recursively decends the tree built by the parser and populates the config
    // file with the appropriate entries
    void ConfigFile::evalTree(Section & current, tree_iter_t const& node, int depth)
    {
        String tabs = "";
        for(int i=0;i<depth;i++)
            tabs = tabs.append("    ");

        const RuleID rule (getNodeID(node));
        const String value(getNodeValue(node));

        if(rule == sectionNameID)
        {
            // Since this is a  flat file representation, whenever we get a section we only add it
            // to the root node, thus we break out if we are currently nested.
            if(!current.isRoot())
                return;

            //HACK: Strip off the '[' and ']', guaranteed to be matched.
            //This should be removed when we upgrade to Boost 1.35, as per config_file_grammar.hpp
            //note we should just be able to say section_name = value
            String section_name;
            section_name = value.substr(1, value.size() - 2);

            // std::cout << "Found section: [" << section_name << "]" << std::endl;

            // create the section
            Section &child = Config::getSection_unsafe(section_name);

            // add each of the children nodes to this section
            for(tree_iter_t chi_node = node->children.begin(); chi_node != node->children.end(); chi_node++)
            {
                evalTree(child, chi_node, depth + 1);
            }
        }
        else if(rule == keyID)
        {
            //assert(node->children.size() == 1 || node->children.size() == 2);

            String key_name = "";
            String key_value = "";

            // Allow for empty (non-existant) values
            if (node->children.size() != 1 && node->children.size() != 2)
            {
                throw parserError("Internal parser error: Expected one or two keyID children, not " + boost::lexical_cast<String>(node->children.size()));
            }

            if (getNodeID(node->children.begin()) != keyNameID)
            {
                throw parserError("Internal parser error: Expected the first entry to be a keyNameID");
            }

            unEscapeText(getNodeValue(node->children.begin()), key_name);

            if (node->children.size() == 1)
            {
                current.addKey(key_name, key_value);
            }
            else if (node->children.size() == 2)
            {
                UInt64 index = 0;
                tree_iter_t const& datanode = node->children.begin() + 1;
                switch(getNodeID(datanode))
                {
                    case keyValueID:
                        unEscapeText(getNodeValue(datanode), key_value);
                        current.addKey(key_name, key_value);
                        break;
                    case keyValueArrayID:
                        for(tree_iter_t chi = datanode->children.begin(); chi != datanode->children.end(); chi++)
                        {
                            switch(getNodeID(chi))
                            {
                                case keySeparatorID:
                                    ++index;
                                    break;
                                case keyValueID:
                                    key_value = "";
                                    unEscapeText(getNodeValue(chi), key_value);
                                    if (key_value != "")
                                        current.addKey(key_name, key_value, index);
                                    break;
                                default:
                                    throw parserError("Internal parser error: Expected separator or value");
                            }
                        }
                        break;
                    // FIXME: Span should properly create a span of numbers
                    case keyValueSpanID:
                    default:
                        throw parserError("Internal parser error: Unexpected node type");
                }
            }
            else
            {
                throw parserError("Internal parser error: Unexpacted number of keyID children found: " + boost::lexical_cast<String>(node->children.size()));
            }
        }
        else
        {
            for(tree_iter_t chi = node->children.begin(); chi != node->children.end(); chi++)
            {
                evalTree(current, chi, depth + 1);
            }
        }

    }

    void ConfigFile::saveAs(const String &path)
    {
        const String tmp_path(path + ".tmp");

        std::ofstream output_file;
        output_file.exceptions ( std::ofstream::eofbit | std::ofstream::failbit | std::ofstream::badbit );

        //Atomic write with error handling
        try
        {
            output_file.open(tmp_path.c_str());
            SaveTreeAs(output_file, m_root);
            output_file.close();
        }
        catch(const std::ofstream::failure & e)
        {
            throw SaveError(e.what());
        }

        unlink(path.c_str());
        int res = rename(tmp_path.c_str(), path.c_str());
        if (res == -1)
        {
            int errsv = errno;
            String error_str("Moving " + tmp_path + " to " + path + ": error(" + itostr(errsv) + ") " + strerror(errsv) + ".");
            throw SaveError(error_str);
        }
    }

    //Dump the tree in a format that is readable by our parser
    void ConfigFile::SaveTreeAs(std::ofstream &out, const Section &current)
    {

        if(!current.isRoot())
        {
            out << "[" << current.getFullPath() << "]" << std::endl;
        }

        // Write out all of the default values
        KeyList const & keys = current.getKeys();
        for(KeyList::const_iterator i = keys.begin(); i != keys.end();i++)
        {

            const String & name = i->second->getName();
            //Quote the name if it has spaces in it
            if(boost::find_first(name, " ") || boost::find_first(name, "\""))
            {
                String escaped_name;
                escapeText(name, escaped_name);
                out << "\"" << escaped_name << "\"";
            }
            else
                out << name;

            //Quote the value if it is a string
            if(i->second->getFloatValid() || i->second->getIntValid())
                out << " = " << i->second->getString() << std::endl;
            else
            {
                String escaped_value;
                escapeText(i->second->getString(), escaped_value);
                out << " = \"" << escaped_value << "\"" << std::endl;
            }

        }

        //Write out the array data that overrides the defaults
        KeyArrayList const & arr = current.getArrayKeys();
        for(KeyArrayList::const_iterator i = arr.begin(); i != arr.end(); ++i)
        {
            for (uint64_t index = 0; index < i->second.size(); ++index)
            {
                // Find a non-default configuration value where we can get the name from
                if (i->second[index])
                {
                    const String & name = i->second[index]->getName();
                    //Quote the name if it has spaces in it
                    if(boost::find_first(name, " ") || boost::find_first(name, "\""))
                    {
                        String escaped_name;
                        escapeText(name, escaped_name);
                        out << "\"" << escaped_name << "\"";
                    }
                    else
                        out << name;
                    break;
                }
            }

            out << "[] = ";

            for (uint64_t index = 0; index < i->second.size(); ++index)
            {
                if (i->second[index])
                {
                    //Quote the value if it is a string
                    if(i->second[index]->getFloatValid() || i->second[index]->getIntValid())
                        out << i->second[index]->getString();
                    else
                    {
                        String escaped_value;
                        escapeText(i->second[index]->getString(), escaped_value);
                        out << "\"" << escaped_value << "\"";
                    }
                }
                // else: default value, print nothing between the ","

                out << ",";
            }

            out << std::endl;
        }

        out << std::endl;

        SectionList const & subsections = current.getSubsections();
        for(SectionList::const_iterator i = subsections.begin(); i != subsections.end(); i++)
        {
            Section const & subsection = *(i->second);
            SaveTreeAs(out, subsection);
        }

        return;
    }

//namespaces
}

ext: cpp


text:
#include <boost/test/unit_test.hpp>

#include "fstest.h"
#include "fscore.h"
#include "fsmem.h"
#include "fsutil.h"

using namespace fs;

struct FlagFixture
{
    FlagFixture()
    {
    }

    ~FlagFixture()
    {
    }
};

FS_DECLARE_FLAGS(TestFlagsEnum, A, B, C);

using TestFlags = Flags<TestFlagsEnum>;

BOOST_AUTO_TEST_SUITE(core)
BOOST_FIXTURE_TEST_SUITE(flags, FlagFixture)

BOOST_AUTO_TEST_CASE(construct_empty_flags)
{
    TestFlags flags;
    BOOST_CHECK(!flags.isAnySet());
    BOOST_CHECK(!flags.areAllSet());
    BOOST_CHECK(!flags.isSet(TestFlags::A));
    BOOST_CHECK(!flags.isSet(TestFlags::B));
    BOOST_CHECK(!flags.isSet(TestFlags::C));
}

BOOST_AUTO_TEST_CASE(constuct_flags_with_operator_or)
{
    TestFlags flags = TestFlags::A | TestFlags::B | TestFlags::C;
    BOOST_CHECK(flags.isAnySet());
    BOOST_CHECK(flags.areAllSet());
    BOOST_CHECK(flags.isSet(TestFlags::A));
    BOOST_CHECK(flags.isSet(TestFlags::B));
    BOOST_CHECK(flags.isSet(TestFlags::C));
}

BOOST_AUTO_TEST_CASE(set_and_unset_flags)
{
    TestFlags flags;
    flags.set(TestFlags::A);
    BOOST_CHECK(flags.isSet(TestFlags::A));

    flags.remove(TestFlags::A);
    BOOST_CHECK(!flags.isSet(TestFlags::A));

    flags.set(TestFlags::A);
    flags.set(TestFlags::B);
    BOOST_CHECK(flags.isSet(TestFlags::A));
    BOOST_CHECK(flags.isSet(TestFlags::B));
}

BOOST_AUTO_TEST_CASE(combine_with_operator_or)
{
    TestFlags flagsA(TestFlags::A);
    TestFlags flagsB(TestFlags::B);

    TestFlags flagsAB = flagsA | flagsB;
    BOOST_CHECK(flagsAB.isSet(TestFlags::A));
    BOOST_CHECK(flagsAB.isSet(TestFlags::B));

    flagsAB.clear();
    flagsAB |= flagsA;
    flagsAB |= flagsB;
    BOOST_CHECK(flagsAB.isSet(TestFlags::A));
    BOOST_CHECK(flagsAB.isSet(TestFlags::B));

    flagsAB.clear();
    flagsAB = TestFlags::A | TestFlags::B;
    BOOST_CHECK(flagsAB.isSet(TestFlags::A));
    BOOST_CHECK(flagsAB.isSet(TestFlags::B));
}

BOOST_AUTO_TEST_CASE(clear_flags)
{
    TestFlags flags = TestFlags::A | TestFlags::B | TestFlags::C;
    BOOST_CHECK(flags.isAnySet());

    flags.clear();
    BOOST_CHECK(!flags.isAnySet());
}

BOOST_AUTO_TEST_CASE(valid_description)
{
    TestFlags flags;

    // Empty Flags
    TestFlags::Description desc;
    flags.toString(desc);
    auto length = std::strlen(desc);
    BOOST_CHECK_MESSAGE(length == 0, "length was " << length << " and desc was '" << desc << "'");

    // Single Flags
    flags.set(TestFlags::A);
    flags.toString(desc);
    BOOST_CHECK(std::strlen(desc) > 0);
    BOOST_CHECK_MESSAGE(std::strcmp("A", desc) == 0, "desc was '" << desc << "'");

    // Multiple Flags
    flags |= TestFlags::B | TestFlags::C;
    flags.toString(desc);
    BOOST_CHECK(std::strlen(desc) > 0);
    BOOST_CHECK_MESSAGE(std::strcmp("A, B, C", desc) == 0, "desc was '" << desc << "'");
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: hfarrow/Fissura

max_issues_repo_name: hfarrow/Fissura

max_forks_repo_name: hfarrow/Fissura


text:
// Copyright (c) 2009, Florian Wagner <florian@wagner-flo.net>.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#define BOOST_TEST_MODULE tests
#include <boost/test/included/unit_test.hpp>

#include <inipp.hh>

BOOST_AUTO_TEST_CASE( sunshine_inifile )
{
  std::ifstream cstream("tests-sunshine.conf");
  inipp::inifile cfile(cstream);

  // correct queries
  BOOST_REQUIRE_EQUAL(cfile.get("everything"),
                      "borked");
  BOOST_REQUIRE_EQUAL(cfile.dget("inipp", "is buggy as hell"),
                      "may not be borked");
  BOOST_REQUIRE_EQUAL(cfile.dget("inivisible entry", "is invisible"),
                      "is invisible");
  BOOST_REQUIRE_EQUAL(cfile.get("rule the world", "use lolcats"),
                      "en masse");
  BOOST_REQUIRE_EQUAL(cfile.get("rule the world", "but do not"),
                      "fall over laughing");
  BOOST_REQUIRE_EQUAL(cfile.get("sp3c14|_ c#4r4c73r2", "do"),
                      "work in inipp");
  BOOST_REQUIRE_EQUAL(cfile.dget("sp3c14|_ c#4r4c73r2", "do", "not work"),
                      "work in inipp");
  BOOST_REQUIRE_EQUAL(cfile.dget("sp3c14|_ c#4r4c73r2", "are", "funky"),
                      "funky");
  BOOST_REQUIRE_EQUAL(cfile.get("whitespace aplenty", "these are double"),
                      "= signs");
  BOOST_REQUIRE_EQUAL(cfile.dget("no section", "without an entry",
                                 "and no value"),
                      "and no value");

  // queries supposed to throw up
  BOOST_REQUIRE_THROW(cfile.get("nothing"), inipp::unknown_entry_error);
  BOOST_REQUIRE_THROW(cfile.get("rule the world", "use loldogs"),
                      inipp::unknown_entry_error);
  BOOST_REQUIRE_THROW(cfile.get("normal characters", "work in inipp"),
                      inipp::unknown_section_error);
  BOOST_REQUIRE_THROW(cfile.get("everything = borked"),
                      inipp::unknown_entry_error);
}

BOOST_AUTO_TEST_CASE( sunshine_inisection )
{
  std::ifstream cstream("tests-sunshine.conf");
  inipp::inifile cfile(cstream);
  inipp::inisection rule = cfile.section("rule the world");

  // correct queries
  BOOST_REQUIRE_EQUAL(rule.get("use lolcats"), "en masse");
  BOOST_REQUIRE_EQUAL(rule.get("but do not"), "fall over laughing");
  BOOST_REQUIRE_EQUAL(rule.dget("use of force", "is a bad idea"),
                      "is a bad idea");

  // queries supposed to throw up
  BOOST_REQUIRE_THROW(cfile.section("nosection"),
                      inipp::unknown_section_error);
  BOOST_REQUIRE_THROW(rule.get("use of force"),
                      inipp::unknown_entry_error);
}

BOOST_AUTO_TEST_CASE( malformed )
{
  // we use a single ifstream
  std::ifstream cstream("tests-malformed.conf");

  // The test config has two lines with distinct errors on them. So we
  // construct a inipp::inifile twice without rewinding the ifstream
  // in between. Both times an error should be thrown.
  BOOST_REQUIRE_THROW(inipp::inifile cfile(cstream), inipp::syntax_error);
  BOOST_REQUIRE_THROW(inipp::inifile cfile(cstream), inipp::syntax_error);

  // But no error afterwards.
  BOOST_REQUIRE_NO_THROW(inipp::inifile cfile(cstream));
}

ext: cc

max_stars_repo_name: th-yoo/inipp

max_issues_repo_name: th-yoo/inipp

max_forks_repo_name: th-yoo/inipp


text:
// $Id$
//
// Copyright 2009 Chris Purcell.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Taken from http://lists.boost.org/Archives/boost/2009/09/156509.php
// This file should be deleted if/when this makes it into boost.

#ifndef BOOST_FOREACH_FIELD

#include <boost/preprocessor/seq/for_each_i.hpp>
#include <boost/fusion/include/at_c.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/fusion/container/generation/ignore.hpp> // for boost::fusion::ignore

#define BOOST_FOREACH_ASSIGN_VAR(R, ROW, I, VAR) \
    for (VAR = boost::fusion::at_c<I>(ROW); \
         !BOOST_FOREACH_ID(_foreach_leave_outerloop); \
         BOOST_FOREACH_ID(_foreach_leave_outerloop) = true)

#define BOOST_FOREACH_FIELD(VARS, COL) \
    BOOST_FOREACH_PREAMBLE() \
    if (boost::foreach_detail_::auto_any_t BOOST_FOREACH_ID(_foreach_col) = BOOST_FOREACH_CONTAIN(COL)) {} \
    else if (boost::foreach_detail_::auto_any_t BOOST_FOREACH_ID(_foreach_cur) = BOOST_FOREACH_BEGIN(COL)) {} \
    else if (boost::foreach_detail_::auto_any_t BOOST_FOREACH_ID(_foreach_end) = BOOST_FOREACH_END(COL)) {} \
    else for (bool BOOST_FOREACH_ID(_foreach_continue) = true, BOOST_FOREACH_ID(_foreach_leave_outerloop) = true; \
              BOOST_FOREACH_ID(_foreach_continue) && !BOOST_FOREACH_DONE(COL); \
              BOOST_FOREACH_ID(_foreach_continue) ? BOOST_FOREACH_NEXT(COL) : (void)0) \
    if (boost::foreach_detail_::set_false(BOOST_FOREACH_ID(_foreach_continue))) {} \
    else if (boost::foreach_detail_::set_false(BOOST_FOREACH_ID(_foreach_leave_outerloop))) {} \
    else BOOST_PP_SEQ_FOR_EACH_I(BOOST_FOREACH_ASSIGN_VAR, BOOST_FOREACH_DEREF(COL), VARS) \
         for (;!BOOST_FOREACH_ID(_foreach_continue);BOOST_FOREACH_ID(_foreach_continue) = true)

#endif // BOOST_FOREACH_FIELD

max_stars_repo_name: austinkeller/pwiz

max_issues_repo_name: austinkeller/pwiz

max_forks_repo_name: austinkeller/pwiz


text:
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class Network {
  public:
    Network(vector<int> &sizes, vector<VectorXd> &training_inputs, vector<VectorXd> &training_outputs,
       vector<VectorXd> &test_inputs, vector<int> &test_labels,
       int mini_batch_size, double learning_rate);
    void feedforward(VectorXd &a);
    void backpropagate(VectorXd &y);
    void stochastic_gradient_descent(int epochs);
    double evaluate();
    int save_weights_and_biases(string filepath);
  private:
    void update_mini_batch(vector<int> &indices, int batch_num); /* helper for ``stochastic_gradient_descent``. */
    VectorXd sigmoid_prime(int &layer_num); /* helper for ``backpropogate``. */
    VectorXd relu_prime(int &layer_num); /* helper for ``backpropogate``. */
    VectorXd cost_derivative(VectorXd &y); /* helper for ``backpropogate``. */
    int num_layers_; /* Number of layers in the network (including input and output layers). */
    vector<int> sizes_; /* Vector containing the sizes of each layer. ``sizes_.size() == num_layers``. */
    vector<VectorXd> biases_; /* Bias vectors for each layer.
                               * 0th index contains a dummy vector since there is no bias for the input layer. */
    vector<MatrixXd> weights_; /* Weight vectors for each layer except the input.
                                * 0th index contains a dummy matrix since there is no weight for the input layer. */
    vector<VectorXd> as_; /* Stores the activations of each layer to avoid repeated computation. */
    vector<VectorXd> zs_; /* Stores the z-vectors of each layer to avoid repeated computation. */
    vector<VectorXd> nabla_b_; /* Stores the derivatives of the cost function with respect to the baises. */
    vector<MatrixXd> nabla_w_; /* Stores the derivatives of the cost function with respect to the weights. */
    vector<VectorXd> training_inputs_;
    vector<VectorXd> training_outputs_;
    vector<VectorXd> test_inputs_;
    vector<int> test_labels_;
    int mini_batch_size_;
    double learning_rate_;
};

max_stars_repo_name: oojiang/Neural-Net-From-Scratch

max_issues_repo_name: oojiang/Neural-Net-From-Scratch

max_forks_repo_name: oojiang/Neural-Net-From-Scratch


text:
#include "storage/storage.hpp"
#include "util/exception.hpp"
#include "util/simple_logger.hpp"
#include "util/typedefs.hpp"
#include "util/version.hpp"

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

using namespace osrm;

// generate boost::program_options object for the routing part
bool generateDataStoreOptions(const int argc,
                              const char *argv[],
                              boost::filesystem::path &base_path)
{
    // declare a group of options that will be allowed only on command line
    boost::program_options::options_description generic_options("Options");
    generic_options.add_options()("version,v", "Show version")("help,h", "Show this help message")(
        "springclean,s", "Remove all regions in shared memory");

    // declare a group of options that will be allowed both on command line
    // as well as in a config file
    boost::program_options::options_description config_options("Configuration");

    // hidden options, will be allowed on command line but will not be shown to the user
    boost::program_options::options_description hidden_options("Hidden options");
    hidden_options.add_options()("base,b",
                                 boost::program_options::value<boost::filesystem::path>(&base_path),
                                 "base path to .osrm file");

    // positional option
    boost::program_options::positional_options_description positional_options;
    positional_options.add("base", 1);

    // combine above options for parsing
    boost::program_options::options_description cmdline_options;
    cmdline_options.add(generic_options).add(config_options).add(hidden_options);

    const auto *executable = argv[0];
    boost::program_options::options_description visible_options(
        boost::filesystem::path(executable).filename().string() + " [<options>] <configuration>");
    visible_options.add(generic_options).add(config_options);

    // print help options if no infile is specified
    if (argc < 2)
    {
        util::SimpleLogger().Write() << visible_options;
        return false;
    }

    // parse command line options
    boost::program_options::variables_map option_variables;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv)
                                      .options(cmdline_options)
                                      .positional(positional_options)
                                      .run(),
                                  option_variables);

    if (option_variables.count("version"))
    {
        util::SimpleLogger().Write() << OSRM_VERSION;
        return false;
    }

    if (option_variables.count("help"))
    {
        util::SimpleLogger().Write() << visible_options;
        return false;
    }

    boost::program_options::notify(option_variables);

    return true;
}

int main(const int argc, const char *argv[]) try
{
    util::LogPolicy::GetInstance().Unmute();

    boost::filesystem::path base_path;
    if (!generateDataStoreOptions(argc, argv, base_path))
    {
        return EXIT_SUCCESS;
    }
    storage::StorageConfig config(base_path);
    if (!config.IsValid())
    {
        util::SimpleLogger().Write(logWARNING) << "Config contains invalid file paths. Exiting!";
        return EXIT_FAILURE;
    }
    storage::Storage storage(std::move(config));
    return storage.Run();
}
catch (const std::bad_alloc &e)
{
    util::SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
    util::SimpleLogger().Write(logWARNING)
        << "Please provide more memory or disable locking the virtual "
           "address space (note: this makes OSRM swap, i.e. slow)";
    return EXIT_FAILURE;
}
catch (const std::exception &e)
{
    util::SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
    return EXIT_FAILURE;
}

max_stars_repo_name: megatontech/osrm-backend

max_issues_repo_name: megatontech/osrm-backend

max_forks_repo_name: megatontech/osrm-backend


text:
#include <iostream>
#include <unordered_map>
#include <thread>

#include <boost/circular_buffer.hpp>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <gtkmm.h>
#include <GL/gl.h>

#include <bot_param/param_util.h>
#include <bot_frames/bot_frames.h>
#include <gtkmm-renderer/RendererBase.hpp>

#include <maps/Collector.hpp>
#include <maps/Surfelizer.hpp>
#include <maps/Utils.hpp>
#include <maps/BotWrapper.hpp>
#include "MeshRenderer.hpp"

using namespace maps;

struct LidarData {
  std::string mName;
  std::string mChannel;
  bool mActive;
  Eigen::Vector3f mColor;
  typedef std::shared_ptr<PointSet> ScanPtr;
  typedef boost::circular_buffer<ScanPtr> ScanBuffer;
  ScanBuffer mScans;
  Surfelizer mSurfelizer;
  typedef boost::circular_buffer<std::vector<Surfelizer::Surfel> > SurfelList;
  SurfelList mSurfels;

  typedef std::shared_ptr<LidarData> Ptr;

  LidarData() : mActive(false), mColor(0,0,0) {}
  ~LidarData() {}
};

class LidarTestRenderer :
  public gtkmm::RendererBase, public Collector::DataListener {
protected:
  enum DrawMode {
    DrawModePoints,
    DrawModeSurfelsWire,
    DrawModeSurfelsFilled
  };

  struct SurfelParams {
    int mSurfelMode;
    int mDecimation;
    bool operator==(const SurfelParams& iParams) {
      return ((iParams.mSurfelMode == mSurfelMode) &&
              (iParams.mDecimation == mDecimation));
    }
    bool operator!=(const SurfelParams& iParams) {
      return !(*this==iParams);
    }
  };

  typedef std::unordered_map<std::string, LidarData::Ptr> LidarDataMap;
  LidarDataMap mDataMap;
  maps::Collector mCollector;
  int mColorMode;
  SurfelParams mParams;
  int mDrawMode;
  SurfelParams mSurfelParams;
  SurfelParams mSurfelParamsPrev;
  double mPointSize;
  double mSurfelScale;
  double mMinZ;
  double mMaxZ;
  int mScanHistory;
  std::shared_ptr<maps::MeshRenderer> mMeshRenderer;
  std::vector<Eigen::Vector3f> mPointBuffer;
  std::mutex mSurfelMutex;

public:

  LidarTestRenderer(BotViewer* iViewer, const int iPriority,
                    const lcm_t* iLcm,
                    const BotParam* iParam, const BotFrames* iFrames)
    : gtkmm::RendererBase("Lidar Test", iViewer, iPriority, iLcm,
                          iParam, iFrames) {

    // set up ui widgets
    {
      std::vector<int> ids = {
        maps::MeshRenderer::ColorModeFlat,
        maps::MeshRenderer::ColorModeHeight,
        maps::MeshRenderer::ColorModeRange,
        maps::MeshRenderer::ColorModeCamera };
      std::vector<std::string> labels = { "Flat", "Height", "Range", "Camera"};
      mColorMode = maps::MeshRenderer::ColorModeHeight;
      addCombo("Color Mode", mColorMode, labels, ids);
    }

    {
      std::vector<int> ids = { DrawModePoints, DrawModeSurfelsWire,
                               DrawModeSurfelsFilled };
      std::vector<std::string> labels = { "Points", "Surfels Wire",
                                          "Surfels Filled"};
      mDrawMode = DrawModePoints;
      addCombo("Draw Mode", mDrawMode, labels, ids);
    }

    {
      std::vector<int> ids = {
        Surfelizer::SizeMethodFixed, Surfelizer::SizeMethodRange,
        Surfelizer::SizeMethodAngles, Surfelizer::SizeMethodNeighbors };
      std::vector<std::string> labels = {"Fixed", "Range", "Angles",
                                         "Neighbors"};
      mSurfelParams.mSurfelMode = Surfelizer::SizeMethodFixed;
      addCombo("Surfel Size", mSurfelParams.mSurfelMode, labels, ids);
    }

    mPointSize = 3;
    addSlider("Point Size", mPointSize, 0.1, 10, 0.1);

    mSurfelScale = 1;
    addSlider("Surfel Scale", mSurfelScale, 0.1, 2.0, 0.1);

    mMinZ = 0;
    addSlider("Z Scale Min", mMinZ, -1, 2, 0.01);

    mMaxZ = 1;
    addSlider("Z Scale Max", mMaxZ, -1, 2, 0.01);

    mScanHistory = 1000;
    addSpin("Scan History", mScanHistory, 0, 1000, 1);

    mSurfelParams.mDecimation = 1;
    addSlider("Decimation", mSurfelParams.mDecimation, 1, 100, 1);

    // set up internal variables
    mSurfelParamsPrev.mSurfelMode = -1;
    BotWrapper::Ptr botWrapper(new BotWrapper(getLcm(), getBotParam(),
                                              getBotFrames()));
    mMeshRenderer.reset(new MeshRenderer());
    mMeshRenderer->setBotObjects(getLcm(), getBotParam(), getBotFrames());
    mMeshRenderer->setCameraChannel("CAMERALEFT");
    mCollector.setBotWrapper(botWrapper);
    mCollector.addListener(*this);

    // subscribe to lcm channels and set up data structures
    BotParam* botParam = getBotParam();
    char** lidarNames = bot_param_get_all_planar_lidar_names(botParam);
    if (lidarNames != NULL) {
      for (int i = 0; lidarNames[i] != NULL; ++i) {
        char prefix[1024];
        bot_param_get_planar_lidar_prefix(botParam, lidarNames[i], prefix,
                                          sizeof(prefix));
        std::string key = std::string(prefix) + ".lcm_channel";
        char* str;
        bot_param_get_str(botParam, key.c_str(), &str);
        std::string channel(str);
        free(str);
        mCollector.getDataReceiver()->
          addChannel(channel, SensorDataReceiver::SensorTypePlanarLidar,
                     channel, "local");

        key = std::string(prefix) + ".viewer_color";
        double color[3];
        bot_param_get_double_array(botParam, key.c_str(), color, 3);

        LidarData::Ptr data(new LidarData());
        data->mName = lidarNames[i];
        data->mChannel = channel;
        data->mActive = false;
        data->mColor = Eigen::Vector3f(color[0], color[1], color[2]);;
        data->mScans.set_capacity(1000);
        data->mSurfels.set_capacity(1000);
        mDataMap[data->mChannel] = data;
      }
      g_strfreev(lidarNames);
    }

    mCollector.start();
  }

  ~LidarTestRenderer() {
  }

  void notify(const SensorDataReceiver::SensorData& iData) {
    LidarDataMap::const_iterator item = mDataMap.find(iData.mChannel);
    if (item == mDataMap.end()) return;
    {
      std::lock_guard<std::mutex> lock(mSurfelMutex);
      item->second->mScans.push_back(iData.mPointSet);
      item->second->mSurfels.
        push_back(item->second->mSurfelizer.addScan(*iData.mPointSet));
    }
    requestDraw();
  }

  void draw() {
    std::lock_guard<std::mutex> lock(mSurfelMutex);

    mPointBuffer.clear();

    // iterate over different laser sensors
    LidarDataMap::const_iterator dataIter = mDataMap.begin();
    for (; dataIter != mDataMap.end(); ++dataIter) {
      LidarData::Ptr data = dataIter->second;

      // set up mesh renderer parameters
      mMeshRenderer->setColor(data->mColor[0], data->mColor[1],
                              data->mColor[2]);
      mMeshRenderer->setPointSize(mPointSize);
      mMeshRenderer->setColorMode((maps::MeshRenderer::ColorMode)mColorMode);
      mMeshRenderer->setScaleRange(mMinZ, mMaxZ);
      BotTrans trans;
      bot_frames_get_trans(getBotFrames(), "head", "local", &trans);
      Eigen::Vector3f pos(trans.trans_vec[0], trans.trans_vec[1],
                          trans.trans_vec[2]);
      mMeshRenderer->setRangeOrigin(pos);

      // draw points only
      if (mDrawMode == DrawModePoints) {
        int counter = 0;
        LidarData::ScanBuffer::reverse_iterator iter = data->mScans.rbegin();
        for (; (iter != data->mScans.rend()) && (counter < mScanHistory);
             ++iter, ++counter) {
          LidarData::ScanPtr scan = *iter;
          Eigen::Isometry3f pose = Utils::getPose(*scan->mCloud);
          Eigen::Vector3f origin = pose.translation();

          // grab all valid points from this scan
          mPointBuffer.reserve(mPointBuffer.size() + scan->mCloud->size());
          for (size_t i = 0; i < scan->mCloud->size(); ++i) {
            // TODO: this is duplicate computation
            Eigen::Vector3f pt = pose * (*scan->mCloud)[i].getVector3fMap();
            float range = (pt - origin).norm();
            if ((range >= scan->mMinRange) && (range <= scan->mMaxRange)) {
              mPointBuffer.push_back(pt);
            }
          }
        }
        mMeshRenderer->setMeshMode(maps::MeshRenderer::MeshModePoints);
        mMeshRenderer->setData(mPointBuffer, std::vector<Eigen::Vector3i>());
      }

      // draw surfels (wireframe)
      else if ((mDrawMode == DrawModeSurfelsWire) ||
               (mDrawMode == DrawModeSurfelsFilled)) {
        std::vector<Eigen::Vector3f> vertices;
        std::vector<Eigen::Vector3i> faces;
        computeSurfels(data, vertices, faces);
        mMeshRenderer->setMeshMode(maps::MeshRenderer::MeshModeWireframe);
        if (mDrawMode == DrawModeSurfelsFilled) {
          mMeshRenderer->setMeshMode(maps::MeshRenderer::MeshModeFilled);
        }
        mMeshRenderer->setData(vertices, faces);
      }

      // execute gl drawing
      mMeshRenderer->draw();
    }
  }

  void computeSurfels(const LidarData::Ptr& iData,
                      std::vector<Eigen::Vector3f>& oVertices,
                      std::vector<Eigen::Vector3i>& oFaces) {

    if (iData->mScans.size() == 0) return;

    if (mSurfelParams != mSurfelParamsPrev) {
      // set up surfelizer
      iData->mSurfelizer.setScanRadius(2);
      iData->mSurfelizer.setPointRadius(2);
      iData->mSurfelizer.
        setSizeMethod((Surfelizer::SizeMethod)mSurfelParams.mSurfelMode);
      iData->mSurfelizer.setNominalSize(0.01);
      iData->mSurfelizer.setDecimation(mSurfelParams.mDecimation);
      iData->mSurfels.clear();

      // loop over appropriate scans
      const int numScans = iData->mScans.size();
      const int stopIndex = std::max(0, numScans - mScanHistory - 1);
      for (int i = numScans-1; i >= stopIndex; --i) {
        std::vector<Surfelizer::Surfel> surfels =
          iData->mSurfelizer.addScan(*iData->mScans[i]);
        iData->mSurfels.push_back(surfels);
      }
      mSurfelParamsPrev = mSurfelParams;
    }

    // allocate vectors
    const int numScans = iData->mSurfels.size();
    const int stopIndex = std::max(0, numScans - mScanHistory - 1);
    int totalSurfels = 0;
    for (int i = numScans-1; i >= stopIndex; --i) {
      totalSurfels += iData->mSurfels[i].size();
    }
    oVertices.resize(totalSurfels*4);
    oFaces.resize(totalSurfels*2);

    // form quads
    int cnt = 0;
    for (int i = numScans-1; i >= stopIndex; --i) {
      for (size_t j = 0; j < iData->mSurfels[i].size(); ++j, ++cnt) {

        // scale principal vectors
        Surfelizer::Surfel& s = iData->mSurfels[i][j];
        Eigen::Vector3f v1 = s.mOrientation.col(0)*s.mSize[0]*mSurfelScale/2;
        Eigen::Vector3f v2 = s.mOrientation.col(1)*s.mSize[1]*mSurfelScale/2;

        // create a quad (two triangles) for this point
        oVertices[cnt*4+0] = s.mCenter -v1 +v2;
        oVertices[cnt*4+1] = s.mCenter +v1 +v2;
        oVertices[cnt*4+2] = s.mCenter +v1 -v2;
        oVertices[cnt*4+3] = s.mCenter -v1 -v2;
        oFaces[cnt*2+0] = Eigen::Vector3i(4*cnt+0,4*cnt+3,4*cnt+1);
        oFaces[cnt*2+1] = Eigen::Vector3i(4*cnt+1,4*cnt+3,4*cnt+2);
      }
    }
  }
};

// this is the single setup method exposed for integration with the viewer
void lidartest_renderer_setup(BotViewer* iViewer, const int iPriority,
                              const lcm_t* iLcm,
                              const BotParam* iParam,
                              const BotFrames* iFrames) {
  new LidarTestRenderer(iViewer, iPriority, iLcm, iParam, iFrames);
}

max_stars_repo_name: liangfok/oh-distro

max_issues_repo_name: liangfok/oh-distro

max_forks_repo_name: liangfok/oh-distro


text:
#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>
#include "quadrature/qhermite.hpp"
#include "spectral/lagrange_polynomial.hpp"

using namespace std;
using namespace boltzmann;

TEST(spectral, LagrangePolySimple)
{
  std::vector<int> ks({10, 20, 30, 40, 80});
  for (int K : ks) {
    QHermite qh(1.0, K);

    lagrange_poly_simple lp(qh.points_data(), qh.size());

    for (int j = 0; j < qh.size(); ++j) {
      for (int i = 0; i < qh.size(); ++i) {
        double val = lp.eval(j, qh.pts(i));
        if (i == j)
          EXPECT_NEAR(val, 1, 1e-12);
        else
          EXPECT_NEAR(val, 0, 1e-12);
      }
    }
  }
}


text:
// Copyright (c) Members of the EGEE Collaboration. 2009.
// See http://www.eu-egee.org/partners/ for details on the copyright holders.

// 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 <string>

#include <boost/lexical_cast.hpp>

#include <user_log.c++.h>

#include "glite/jobid/JobId.h"

#include "EventInterface.h"
#include "MonitorData.h"
#include "SubmitReader.h"

using namespace std;

JOBCONTROL_NAMESPACE_BEGIN {

namespace logmonitor { namespace processer {

const string      EventInterface::ei_s_edgideq( "Job id = " ), EventInterface::ei_s_subnodeof( "Sub job of DAG: " );
const string      EventInterface::ei_s_notsub( "Job seems to be not submitted by the GRID." );
const string      EventInterface::ei_s_joberror( "Job got an error while in the CondorG queue." );
const string      EventInterface::ei_s_jobwrapfail( "Failure while executing job wrapper" );
const string      EventInterface::ei_s_errremcorr( "Error removing CondorId <-> JobId correspondance." );
const string      EventInterface::ei_s_failedinsertion( "Failed insertion of aborting job in aborted list." );

SubmitReader *EventInterface::createReader( const string &edgid )
{
  SubmitReader   *reader;
  reader = new SubmitReader( glite::jobid::JobId(edgid) );
  return reader;
}

EventInterface::EventInterface( ULogEvent *event, MonitorData *data ) : ei_data( data ),
									ei_condor( boost::lexical_cast<string>(event->cluster) )
{
  if( event->proc != 0 ) {
    this->ei_condor.append( 1, '.' );
    this->ei_condor.append( boost::lexical_cast<string>(event->proc) );
  }
}

EventInterface::~EventInterface( void )
{}

}} // namespace processer, logmonitor

} JOBCONTROL_NAMESPACE_END

max_stars_repo_name: italiangrid/wms

max_issues_repo_name: italiangrid/wms

max_forks_repo_name: italiangrid/wms