<commit_before>// Copyright (c) 2020 by Apex.AI Inc. All rights reserved.
//
// 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 "iceoryx_posh/popo/typed_subscriber.hpp"
#include "iceoryx_posh/runtime/posh_runtime.hpp"
#include "topic_data.hpp"

#include <chrono>
#include <csignal>
#include <iostream>

bool killswitch = false;

static void sigHandler(int sig [[gnu::unused]])
{
    killswitch = true;
}

// the maximum number of samples the subscriber can hold before discarding the least
// recent sample (i.e. the capacity of the sample queue on subscriber side)
constexpr uint64_t MAX_NUMBER_OF_SAMPLES{4U};

constexpr uint64_t UNSUBSCRIBED_TIME_SECONDS{3U};

void receive()
{
    iox::popo::SubscriberOptions options;
    options.queueCapacity = MAX_NUMBER_OF_SAMPLES - 2U;
    iox::popo::TypedSubscriber<CounterTopic> subscriber({"Group", "Instance", "Counter"}, options);

    subscriber.subscribe();
    while (!killswitch)
    {
        // unsubscribe and resubscribe
        subscriber.unsubscribe();
        std::cout << "Unsubscribed ... Subscribe in " << UNSUBSCRIBED_TIME_SECONDS << " seconds" << std::endl;

        // we will probably miss some data while unsubscribed
        std::this_thread::sleep_for(std::chrono::seconds(UNSUBSCRIBED_TIME_SECONDS));

        // we (re)subscribe and should see at most the latest options.queueCapacity
        subscriber.subscribe();

        std::this_thread::sleep_for(std::chrono::seconds(1));

        while (subscriber.hasSamples())
        {
            subscriber.take()
                .and_then([](iox::popo::Sample<const CounterTopic>& sample) {
                    std::cout << "Received: " << *sample.get() << std::endl;
                })
                .or_else([](iox::popo::ChunkReceiveError) { std::cout << "Error while receiving." << std::endl; });
        };
        std::cout << "Waiting for data ... " << std::endl;
    }
    subscriber.unsubscribe();
}

int main()
{
    signal(SIGINT, sigHandler);
    iox::runtime::PoshRuntime::initRuntime("/iox-resubscriber");

    std::thread receiver(receive);
    receiver.join();

    return (EXIT_SUCCESS);
}
<commit_msg>iox-#408 update resubscribe example<commit_after>// Copyright (c) 2020 by Apex.AI Inc. All rights reserved.
//
// 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 "iceoryx_posh/popo/typed_subscriber.hpp"
#include "iceoryx_posh/runtime/posh_runtime.hpp"
#include "topic_data.hpp"

#include <chrono>
#include <csignal>
#include <iostream>

bool killswitch = false;

static void sigHandler(int sig [[gnu::unused]])
{
    killswitch = true;
}

constexpr uint64_t UNSUBSCRIBED_TIME_SECONDS{3U};

void receive()
{
    iox::popo::SubscriberOptions options;
    // the maximum number of samples the subscriber can hold before discarding the least
    // recent sample (i.e. the capacity of the sample queue on subscriber side)
    options.queueCapacity = 4U;
    iox::popo::TypedSubscriber<CounterTopic> subscriber({"Group", "Instance", "Counter"}, options);

    subscriber.subscribe();
    while (!killswitch)
    {
        // unsubscribe and resubscribe
        subscriber.unsubscribe();
        std::cout << "Unsubscribed ... Subscribe in " << UNSUBSCRIBED_TIME_SECONDS << " seconds" << std::endl;

        // we will probably miss some data while unsubscribed
        std::this_thread::sleep_for(std::chrono::seconds(UNSUBSCRIBED_TIME_SECONDS));

        // we (re)subscribe and should see at most the latest options.queueCapacity
        subscriber.subscribe();

        std::this_thread::sleep_for(std::chrono::seconds(1));

        while (subscriber.hasSamples())
        {
            subscriber.take()
                .and_then([](iox::popo::Sample<const CounterTopic>& sample) {
                    std::cout << "Received: " << *sample.get() << std::endl;
                })
                .or_else([](iox::popo::ChunkReceiveError) { std::cout << "Error while receiving." << std::endl; });
        };
        std::cout << "Waiting for data ... " << std::endl;
    }
    subscriber.unsubscribe();
}

int main()
{
    signal(SIGINT, sigHandler);
    iox::runtime::PoshRuntime::initRuntime("/iox-resubscriber");

    std::thread receiver(receive);
    receiver.join();

    return (EXIT_SUCCESS);
}
<|endoftext|>


<commit_before>#pragma once

#include <cilantro/icp_base.hpp>
#include <cilantro/transform_estimation.hpp>
#include <cilantro/correspondence_search_combined_metric_adaptor.hpp>
#include <cilantro/kd_tree.hpp>

namespace cilantro {
    template <class TransformT, class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>>
    class CombinedMetricSingleTransformICP : public IterativeClosestPointBase<CombinedMetricSingleTransformICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>,TransformT,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {

        typedef IterativeClosestPointBase<CombinedMetricSingleTransformICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>,TransformT,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;

        friend Base;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointToPointCorrWeightEvaluatorT PointToPointCorrespondenceWeightEvaluator;

        typedef PointToPlaneCorrWeightEvaluatorT PointToPlaneCorrespondenceWeightEvaluator;

        CombinedMetricSingleTransformICP(const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
                                         CorrespondenceSearchEngineT &corr_engine,
                                         PointToPointCorrWeightEvaluatorT &point_corr_eval,
                                         PointToPlaneCorrWeightEvaluatorT &plane_corr_eval)
                : Base(corr_engine),
                  dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p), src_normals_(0),
                  max_optimization_iterations_(1), optimization_convergence_tol_((typename TransformT::Scalar)1e-5),
                  point_to_point_weight_((typename TransformT::Scalar)0.0), point_to_plane_weight_((typename TransformT::Scalar)1.0),
                  point_corr_eval_(point_corr_eval), plane_corr_eval_(plane_corr_eval),
                  src_points_trans_(src_points_.rows(), src_points_.cols()),
                  dst_mean_((dst_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(dst_points_.rowwise().mean())),
                  src_mean_((src_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(src_points_.rowwise().mean())),
                  has_source_normals_(false)
        {
            this->transform_init_.setIdentity();
        }

        CombinedMetricSingleTransformICP(const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_n,
                                         CorrespondenceSearchEngineT &corr_engine,
                                         PointToPointCorrWeightEvaluatorT &point_corr_eval,
                                         PointToPlaneCorrWeightEvaluatorT &plane_corr_eval)
                : Base(corr_engine),
                  dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p), src_normals_(src_n),
                  max_optimization_iterations_(1), optimization_convergence_tol_((typename TransformT::Scalar)1e-5),
                  point_to_point_weight_((typename TransformT::Scalar)0.0), point_to_plane_weight_((typename TransformT::Scalar)1.0),
                  point_corr_eval_(point_corr_eval), plane_corr_eval_(plane_corr_eval),
                  src_points_trans_(src_points_.rows(), src_points_.cols()),
                  src_normals_trans_(src_points_.rows(), src_points_.cols()),
                  dst_mean_((dst_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(dst_points_.rowwise().mean())),
                  src_mean_((src_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(src_points_.rowwise().mean())),
                  has_source_normals_(true)
        {
            this->transform_init_.setIdentity();
        }

        inline PointToPointCorrespondenceWeightEvaluator& pointToPointCorrespondenceWeightEvaluator() {
            return point_corr_eval_;
        }

        inline PointToPlaneCorrespondenceWeightEvaluator& pointToPlaneCorrespondenceWeightEvaluator() {
            return plane_corr_eval_;
        }

        inline typename TransformT::Scalar getPointToPointMetricWeight() const { return point_to_point_weight_; }

        inline CombinedMetricSingleTransformICP& setPointToPointMetricWeight(typename TransformT::Scalar weight) {
            point_to_point_weight_ = weight;
            return *this;
        }

        inline typename TransformT::Scalar getPointToPlaneMetricWeight() const { return point_to_plane_weight_; }

        inline CombinedMetricSingleTransformICP& setPointToPlaneMetricWeight(typename TransformT::Scalar weight) {
            point_to_plane_weight_ = weight;
            return *this;
        }

        inline size_t getMaxNumberOfOptimizationStepIterations() const { return max_optimization_iterations_; }

        inline CombinedMetricSingleTransformICP& setMaxNumberOfOptimizationStepIterations(size_t max_iter) {
            max_optimization_iterations_ = max_iter;
            return *this;
        }

        inline typename TransformT::Scalar getOptimizationStepConvergenceTolerance() const { return optimization_convergence_tol_; }

        inline CombinedMetricSingleTransformICP& setOptimizationStepConvergenceTolerance(typename TransformT::Scalar conv_tol) {
            optimization_convergence_tol_ = conv_tol;
            return *this;
        }

    private:
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_points_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_normals_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_points_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_normals_;

        size_t max_optimization_iterations_;
        typename TransformT::Scalar optimization_convergence_tol_;
        typename TransformT::Scalar point_to_point_weight_;
        typename TransformT::Scalar point_to_plane_weight_;

        PointToPointCorrespondenceWeightEvaluator& point_corr_eval_;
        PointToPlaneCorrespondenceWeightEvaluator& plane_corr_eval_;

        VectorSet<typename TransformT::Scalar,TransformT::Dim> src_points_trans_;
        VectorSet<typename TransformT::Scalar,TransformT::Dim> src_normals_trans_;

        Vector<typename TransformT::Scalar,TransformT::Dim> dst_mean_;
        Vector<typename TransformT::Scalar,TransformT::Dim> src_mean_;

        bool has_source_normals_;

        // ICP interface
        inline void initializeComputation() {}

        // ICP interface
        inline void updateCorrespondences() {
            this->correspondence_search_engine_.findCorrespondences(this->transform_);
        }

        void updateEstimate() {
            transformPoints(this->transform_, src_points_, src_points_trans_);
            CorrespondenceSearchCombinedMetricAdaptor<CorrespondenceSearchEngineT> corr_getter_proxy(this->correspondence_search_engine_);
            TransformT tform_iter;

            if (has_source_normals_) {
                transformNormals(this->transform_, src_normals_, src_normals_trans_);
                estimateTransformSymmetricMetric(dst_points_, dst_normals_, src_points_trans_, src_normals_trans_, corr_getter_proxy.getPointToPointCorrespondences(), point_to_point_weight_, corr_getter_proxy.getPointToPlaneCorrespondences(), point_to_plane_weight_, tform_iter, max_optimization_iterations_, optimization_convergence_tol_, point_corr_eval_, plane_corr_eval_, dst_mean_, this->transform_*src_mean_);
            } else {
                estimateTransformCombinedMetric(dst_points_, dst_normals_, src_points_trans_, corr_getter_proxy.getPointToPointCorrespondences(), point_to_point_weight_, corr_getter_proxy.getPointToPlaneCorrespondences(), point_to_plane_weight_, tform_iter, max_optimization_iterations_, optimization_convergence_tol_, point_corr_eval_, plane_corr_eval_, dst_mean_, this->transform_*src_mean_);
            }

            this->transform_ = tform_iter*this->transform_;
            if (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
                this->transform_.linear() = this->transform_.rotation();
            }
            this->last_delta_norm_ = std::sqrt((tform_iter.linear() - TransformT::LinearMatrixType::Identity()).squaredNorm() + tform_iter.translation().squaredNorm());
        }

        // ICP interface
        VectorSet<typename TransformT::Scalar,1> computeResiduals() {
            if (dst_points_.cols() == 0) {
                return VectorSet<typename TransformT::Scalar,1>::Constant(1, src_points_.cols(), std::numeric_limits<typename TransformT::Scalar>::quiet_NaN());
            }
            VectorSet<typename TransformT::Scalar,1> res(1, src_points_.cols());
            KDTree<typename TransformT::Scalar,TransformT::Dim,KDTreeDistanceAdaptors::L2> dst_tree(dst_points_);
            Neighbor<typename TransformT::Scalar> nn;
            Vector<typename TransformT::Scalar,TransformT::Dim> src_p_trans;
            Vector<typename TransformT::Scalar,TransformT::Dim> normal;
#pragma omp parallel for shared (res) private (nn, src_p_trans)
            for (size_t i = 0; i < src_points_.cols(); i++) {
                src_p_trans.noalias() = this->transform_*src_points_.col(i);
                dst_tree.nearestNeighborSearch(src_p_trans, nn);
                normal = dst_normals_.col(nn.index);
                if (has_source_normals_) normal += src_normals_.col(i);
                typename TransformT::Scalar point_to_plane_dist = normal.dot(dst_points_.col(nn.index) - src_p_trans);
                res[i] = point_to_point_weight_*(dst_points_.col(nn.index) - src_p_trans).squaredNorm() + point_to_plane_weight_*point_to_plane_dist*point_to_plane_dist;
            }
            return res;
        }
    };

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricRigidTransformICP2f = CombinedMetricSingleTransformICP<RigidTransform<float,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricRigidTransformICP2d = CombinedMetricSingleTransformICP<RigidTransform<double,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricRigidTransformICP3f = CombinedMetricSingleTransformICP<RigidTransform<float,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricRigidTransformICP3d = CombinedMetricSingleTransformICP<RigidTransform<double,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricAffineTransformICP2f = CombinedMetricSingleTransformICP<AffineTransform<float,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricAffineTransformICP2d = CombinedMetricSingleTransformICP<AffineTransform<double,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricAffineTransformICP3f = CombinedMetricSingleTransformICP<AffineTransform<float,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricAffineTransformICP3d = CombinedMetricSingleTransformICP<AffineTransform<double,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;
}
<commit_msg>Combined metric ICP fix (would previously only compile for 3D rigid transforms)<commit_after>#pragma once

#include <cilantro/icp_base.hpp>
#include <cilantro/transform_estimation.hpp>
#include <cilantro/correspondence_search_combined_metric_adaptor.hpp>
#include <cilantro/kd_tree.hpp>

namespace cilantro {
    template <class TransformT, class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<typename TransformT::Scalar,typename TransformT::Scalar>>
    class CombinedMetricSingleTransformICP : public IterativeClosestPointBase<CombinedMetricSingleTransformICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>,TransformT,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> {

        typedef IterativeClosestPointBase<CombinedMetricSingleTransformICP<TransformT,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>,TransformT,CorrespondenceSearchEngineT,VectorSet<typename TransformT::Scalar,1>> Base;

        friend Base;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointToPointCorrWeightEvaluatorT PointToPointCorrespondenceWeightEvaluator;

        typedef PointToPlaneCorrWeightEvaluatorT PointToPlaneCorrespondenceWeightEvaluator;

        CombinedMetricSingleTransformICP(const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
                                         CorrespondenceSearchEngineT &corr_engine,
                                         PointToPointCorrWeightEvaluatorT &point_corr_eval,
                                         PointToPlaneCorrWeightEvaluatorT &plane_corr_eval)
                : Base(corr_engine),
                  dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p), src_normals_(0),
                  max_optimization_iterations_(1), optimization_convergence_tol_((typename TransformT::Scalar)1e-5),
                  point_to_point_weight_((typename TransformT::Scalar)0.0), point_to_plane_weight_((typename TransformT::Scalar)1.0),
                  point_corr_eval_(point_corr_eval), plane_corr_eval_(plane_corr_eval),
                  src_points_trans_(src_points_.rows(), src_points_.cols()),
                  dst_mean_((dst_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(dst_points_.rowwise().mean())),
                  src_mean_((src_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(src_points_.rowwise().mean())),
                  has_source_normals_(false)
        {
            this->transform_init_.setIdentity();
        }

        CombinedMetricSingleTransformICP(const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &dst_n,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_p,
                                         const ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> &src_n,
                                         CorrespondenceSearchEngineT &corr_engine,
                                         PointToPointCorrWeightEvaluatorT &point_corr_eval,
                                         PointToPlaneCorrWeightEvaluatorT &plane_corr_eval)
                : Base(corr_engine),
                  dst_points_(dst_p), dst_normals_(dst_n), src_points_(src_p), src_normals_(src_n),
                  max_optimization_iterations_(1), optimization_convergence_tol_((typename TransformT::Scalar)1e-5),
                  point_to_point_weight_((typename TransformT::Scalar)0.0), point_to_plane_weight_((typename TransformT::Scalar)1.0),
                  point_corr_eval_(point_corr_eval), plane_corr_eval_(plane_corr_eval),
                  src_points_trans_(src_points_.rows(), src_points_.cols()),
                  src_normals_trans_(src_points_.rows(), src_points_.cols()),
                  dst_mean_((dst_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(dst_points_.rowwise().mean())),
                  src_mean_((src_points_.cols() == 0) ? Vector<typename TransformT::Scalar,TransformT::Dim>::Zero() : Vector<typename TransformT::Scalar,TransformT::Dim>(src_points_.rowwise().mean())),
                  has_source_normals_(true)
        {
            this->transform_init_.setIdentity();
        }

        inline PointToPointCorrespondenceWeightEvaluator& pointToPointCorrespondenceWeightEvaluator() {
            return point_corr_eval_;
        }

        inline PointToPlaneCorrespondenceWeightEvaluator& pointToPlaneCorrespondenceWeightEvaluator() {
            return plane_corr_eval_;
        }

        inline typename TransformT::Scalar getPointToPointMetricWeight() const { return point_to_point_weight_; }

        inline CombinedMetricSingleTransformICP& setPointToPointMetricWeight(typename TransformT::Scalar weight) {
            point_to_point_weight_ = weight;
            return *this;
        }

        inline typename TransformT::Scalar getPointToPlaneMetricWeight() const { return point_to_plane_weight_; }

        inline CombinedMetricSingleTransformICP& setPointToPlaneMetricWeight(typename TransformT::Scalar weight) {
            point_to_plane_weight_ = weight;
            return *this;
        }

        inline size_t getMaxNumberOfOptimizationStepIterations() const { return max_optimization_iterations_; }

        inline CombinedMetricSingleTransformICP& setMaxNumberOfOptimizationStepIterations(size_t max_iter) {
            max_optimization_iterations_ = max_iter;
            return *this;
        }

        inline typename TransformT::Scalar getOptimizationStepConvergenceTolerance() const { return optimization_convergence_tol_; }

        inline CombinedMetricSingleTransformICP& setOptimizationStepConvergenceTolerance(typename TransformT::Scalar conv_tol) {
            optimization_convergence_tol_ = conv_tol;
            return *this;
        }

    private:
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_points_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> dst_normals_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_points_;
        ConstVectorSetMatrixMap<typename TransformT::Scalar,TransformT::Dim> src_normals_;

        size_t max_optimization_iterations_;
        typename TransformT::Scalar optimization_convergence_tol_;
        typename TransformT::Scalar point_to_point_weight_;
        typename TransformT::Scalar point_to_plane_weight_;

        PointToPointCorrespondenceWeightEvaluator& point_corr_eval_;
        PointToPlaneCorrespondenceWeightEvaluator& plane_corr_eval_;

        VectorSet<typename TransformT::Scalar,TransformT::Dim> src_points_trans_;
        VectorSet<typename TransformT::Scalar,TransformT::Dim> src_normals_trans_;

        Vector<typename TransformT::Scalar,TransformT::Dim> dst_mean_;
        Vector<typename TransformT::Scalar,TransformT::Dim> src_mean_;

        bool has_source_normals_;

        // ICP interface
        inline void initializeComputation() {}

        // ICP interface
        inline void updateCorrespondences() {
            this->correspondence_search_engine_.findCorrespondences(this->transform_);
        }

        // Symmetric metric is only implemented for the rigid 3D case
        template <typename TformT = TransformT>
        typename std::enable_if<int(TformT::Mode) == int(Eigen::Isometry) && TformT::Dim == 3,void>::type updateEstimate() {
            transformPoints(this->transform_, src_points_, src_points_trans_);
            CorrespondenceSearchCombinedMetricAdaptor<CorrespondenceSearchEngineT> corr_getter_proxy(this->correspondence_search_engine_);
            TransformT tform_iter;

            if (has_source_normals_) {
                transformNormals(this->transform_, src_normals_, src_normals_trans_);
                estimateTransformSymmetricMetric(dst_points_, dst_normals_, src_points_trans_, src_normals_trans_, corr_getter_proxy.getPointToPointCorrespondences(), point_to_point_weight_, corr_getter_proxy.getPointToPlaneCorrespondences(), point_to_plane_weight_, tform_iter, max_optimization_iterations_, optimization_convergence_tol_, point_corr_eval_, plane_corr_eval_, dst_mean_, this->transform_*src_mean_);
            } else {
                estimateTransformCombinedMetric(dst_points_, dst_normals_, src_points_trans_, corr_getter_proxy.getPointToPointCorrespondences(), point_to_point_weight_, corr_getter_proxy.getPointToPlaneCorrespondences(), point_to_plane_weight_, tform_iter, max_optimization_iterations_, optimization_convergence_tol_, point_corr_eval_, plane_corr_eval_, dst_mean_, this->transform_*src_mean_);
            }

            this->transform_ = tform_iter*this->transform_;
            if (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
                this->transform_.linear() = this->transform_.rotation();
            }
            this->last_delta_norm_ = std::sqrt((tform_iter.linear() - TransformT::LinearMatrixType::Identity()).squaredNorm() + tform_iter.translation().squaredNorm());
        }

        template <typename TformT = TransformT>
        typename std::enable_if<int(TformT::Mode) != int(Eigen::Isometry) || TformT::Dim != 3,void>::type updateEstimate() {
            transformPoints(this->transform_, src_points_, src_points_trans_);
            CorrespondenceSearchCombinedMetricAdaptor<CorrespondenceSearchEngineT> corr_getter_proxy(this->correspondence_search_engine_);
            TransformT tform_iter;

            estimateTransformCombinedMetric(dst_points_, dst_normals_, src_points_trans_, corr_getter_proxy.getPointToPointCorrespondences(), point_to_point_weight_, corr_getter_proxy.getPointToPlaneCorrespondences(), point_to_plane_weight_, tform_iter, max_optimization_iterations_, optimization_convergence_tol_, point_corr_eval_, plane_corr_eval_, dst_mean_, this->transform_*src_mean_);

            this->transform_ = tform_iter*this->transform_;
            if (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
                this->transform_.linear() = this->transform_.rotation();
            }
            this->last_delta_norm_ = std::sqrt((tform_iter.linear() - TransformT::LinearMatrixType::Identity()).squaredNorm() + tform_iter.translation().squaredNorm());
        }

        // ICP interface
        VectorSet<typename TransformT::Scalar,1> computeResiduals() {
            if (dst_points_.cols() == 0) {
                return VectorSet<typename TransformT::Scalar,1>::Constant(1, src_points_.cols(), std::numeric_limits<typename TransformT::Scalar>::quiet_NaN());
            }
            VectorSet<typename TransformT::Scalar,1> res(1, src_points_.cols());
            KDTree<typename TransformT::Scalar,TransformT::Dim,KDTreeDistanceAdaptors::L2> dst_tree(dst_points_);
            Neighbor<typename TransformT::Scalar> nn;
            Vector<typename TransformT::Scalar,TransformT::Dim> src_p_trans;
            Vector<typename TransformT::Scalar,TransformT::Dim> normal;
#pragma omp parallel for shared (res) private (nn, src_p_trans)
            for (size_t i = 0; i < src_points_.cols(); i++) {
                src_p_trans.noalias() = this->transform_*src_points_.col(i);
                dst_tree.nearestNeighborSearch(src_p_trans, nn);
                normal = dst_normals_.col(nn.index);
                if (has_source_normals_) normal += src_normals_.col(i);
                typename TransformT::Scalar point_to_plane_dist = normal.dot(dst_points_.col(nn.index) - src_p_trans);
                res[i] = point_to_point_weight_*(dst_points_.col(nn.index) - src_p_trans).squaredNorm() + point_to_plane_weight_*point_to_plane_dist*point_to_plane_dist;
            }
            return res;
        }
    };

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricRigidTransformICP2f = CombinedMetricSingleTransformICP<RigidTransform<float,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricRigidTransformICP2d = CombinedMetricSingleTransformICP<RigidTransform<double,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricRigidTransformICP3f = CombinedMetricSingleTransformICP<RigidTransform<float,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricRigidTransformICP3d = CombinedMetricSingleTransformICP<RigidTransform<double,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricAffineTransformICP2f = CombinedMetricSingleTransformICP<AffineTransform<float,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricAffineTransformICP2d = CombinedMetricSingleTransformICP<AffineTransform<double,2>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<float,float>>
    using CombinedMetricAffineTransformICP3f = CombinedMetricSingleTransformICP<AffineTransform<float,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;

    template <class CorrespondenceSearchEngineT, class PointToPointCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>, class PointToPlaneCorrWeightEvaluatorT = UnityWeightEvaluator<double,double>>
    using CombinedMetricAffineTransformICP3d = CombinedMetricSingleTransformICP<AffineTransform<double,3>,CorrespondenceSearchEngineT,PointToPointCorrWeightEvaluatorT,PointToPlaneCorrWeightEvaluatorT>;
}
<|endoftext|>


<commit_before>#include "ObjectStore.hpp"
#include "conf/conf.hpp"
#include <functional>
#include <iostream>
#include <sstream>

#define NUM_APP_ARGS (1)

int main(int argc, char** argv) {
    if((argc < (NUM_APP_ARGS + 1)) || ((argc > (NUM_APP_ARGS + 1)) && strcmp("--", argv[argc - NUM_APP_ARGS - 1]))) {
        std::cerr << "Usage: " << argv[0] << " [ derecho-config-list -- ] <aio|bio>" << std::endl;
        return -1;
    }

    bool use_aio = false;
    if(strcmp("aio", argv[argc - NUM_APP_ARGS]) == 0) {
        use_aio = true;
    } else if(strcmp("bio", argv[1]) != 0) {
        std::cerr << "unrecognized argument:" << argv[argc - NUM_APP_ARGS] << ". Using bio (blocking io) instead." << std::endl;
    }

    derecho::Conf::initialize(argc, argv);
    std::cout << "Starting object store service..." << std::endl;
    // oss - objectstore service
    auto& oss = objectstore::IObjectStoreService::getObjectStoreService(argc, argv,
                                                                        [&](const objectstore::OID& oid, const objectstore::Object& object) {
                                                                            std::cout << "watcher: " << oid << "->" << object << std::endl;
                                                                        });
    // print some message
    std::cout << "Object store service started. \n\tIs replica:" << std::boolalpha << oss.isReplica()
              << "\n\tUsing aio API:" << use_aio
              << std::noboolalpha << "." << std::endl;

    bool bNextCommand = true;  // waiting for the next command.

    // prepare the commandline tool:
    std::map<std::string, std::pair<std::string, std::function<bool(std::string&)>>> commands = {
            {"put",  // command
             {
                     "put <oid> <string>",  // help info
                     [&oss, use_aio](std::string args) -> bool {
                         std::istringstream ss(args);
                         std::string oid, odata;
                         ss >> oid >> odata;
                         objectstore::Object object(std::stol(oid), odata.c_str(), odata.length() + 1);
                         try {
                             if(use_aio) {
                                 // asynchronous api
                                 derecho::rpc::QueryResults<persistent::version_t> results = oss.aio_put(object);
                                 decltype(results)::ReplyMap& replies = results.get();
                                 for(auto& reply_pair : replies) {
                                     std::cout << reply_pair.first << reply_pair.second.get() << std::endl;
                                 }
                             } else {
                                 // synchronous api
                                 std::cout << "bio put:" << std::boolalpha << oss.bio_put(object)
                                           << std::noboolalpha << std::endl;
                             }
                         } catch(...) {
                             return false;
                         }
                         return true;
                     }}},  // put
            {
                    "get",  // command
                    {
                            "get <oid>",  // help info
                            [&oss](std::string& args) -> bool {
                                try {
                                    objectstore::Object obj = oss.bio_get(std::stol(args));
                                    std::cout << obj << std::endl;
                                } catch(...) {
                                    return false;
                                }
                                return true;
                            }}},
            {"remove",  // command
             {
                     "remove <oid>",  // help info
                     [&oss](std::string& args) -> bool {
                         try {
                             return oss.bio_remove(std::stol(args));
                         } catch(...) {
                             return false;
                         }
                     }}},
            {"leave",  // command
             {
                     "leave",  // help info
                     [&oss, &bNextCommand](std::string&) -> bool {
                         oss.leave();
                         bNextCommand = false;
                         return true;
                     }}}};

    std::function<void()> help = [commands]() {
        std::cout << "Commands:" << std::endl;
        for(auto& cmd_entry : commands) {
            std::cout << "\t" << cmd_entry.second.first << std::endl;
        }
    };

    help();
    // main command loop
    while(bNextCommand) {
        std::string line;
        std::cout << "cmd>";
        std::getline(std::cin, line, '\n');
        std::string::size_type first_space_pos = line.find(' ');
        std::string command;
        std::string arguments;
        if(first_space_pos == std::string::npos) {
            command = line;
        } else {
            command = line.substr(0, first_space_pos);
            arguments = line.substr(first_space_pos + 1);
        }
        if(commands.find(command) == commands.end()) {
            help();
        } else {
            bool bRet = commands[command].second(arguments);
            std::cout << "Result:" << std::boolalpha << bRet << std::noboolalpha
                      << std::endl;
        }
    }
    return 0;
}
<commit_msg>enable version in objectstore tester.<commit_after>#include "ObjectStore.hpp"
#include "conf/conf.hpp"
#include <functional>
#include <iostream>
#include <sstream>

#define NUM_APP_ARGS (1)

int main(int argc, char** argv) {
    if((argc < (NUM_APP_ARGS + 1)) || ((argc > (NUM_APP_ARGS + 1)) && strcmp("--", argv[argc - NUM_APP_ARGS - 1]))) {
        std::cerr << "Usage: " << argv[0] << " [ derecho-config-list -- ] <aio|bio>" << std::endl;
        return -1;
    }

    bool use_aio = false;
    if(strcmp("aio", argv[argc - NUM_APP_ARGS]) == 0) {
        use_aio = true;
    } else if(strcmp("bio", argv[1]) != 0) {
        std::cerr << "unrecognized argument:" << argv[argc - NUM_APP_ARGS] << ". Using bio (blocking io) instead." << std::endl;
    }

    derecho::Conf::initialize(argc, argv);
    std::cout << "Starting object store service..." << std::endl;
    // oss - objectstore service
    auto& oss = objectstore::IObjectStoreService::getObjectStoreService(argc, argv,
                                                                        [&](const objectstore::OID& oid, const objectstore::Object& object) {
                                                                            std::cout << "watcher: " << oid << "->" << object << std::endl;
                                                                        });
    // print some message
    std::cout << "Object store service started. \n\tIs replica:" << std::boolalpha << oss.isReplica()
              << "\n\tUsing aio API:" << use_aio
              << std::noboolalpha << "." << std::endl;

    bool bNextCommand = true;  // waiting for the next command.

    // prepare the commandline tool:
    std::map<std::string, std::pair<std::string, std::function<bool(std::string&)>>> commands = {
            {"put",  // command
             {
                     "put <oid> <string>",  // help info
                     [&oss, use_aio](std::string args) -> bool {
                         std::istringstream ss(args);
                         std::string oid, odata;
                         ss >> oid >> odata;
                         objectstore::Object object(std::stol(oid), odata.c_str(), odata.length() + 1);
                         try {
                             if(use_aio) {
                                 // asynchronous api
                                 derecho::rpc::QueryResults<persistent::version_t> results = oss.aio_put(object);
                                 decltype(results)::ReplyMap& replies = results.get();
                                 for(auto& reply_pair : replies) {
                                     std::cout << reply_pair.first << reply_pair.second.get() << std::endl;
                                 }
                             } else {
                                 // synchronous api
                                 std::cout << "bio put:" << std::boolalpha << oss.bio_put(object)
                                           << std::noboolalpha << std::endl;
                             }
                         } catch(...) {
                             return false;
                         }
                         return true;
                     }}},  // put
            {
                    "get",  // command
                    {
                            "get <oid> [version]",  // help info
                            [&oss](std::string& args) -> bool {
                                char *argcopy = strdup(args.c_str());
                                char *token = std::strtok(argcopy, " ");
                                uint64_t oid = std::stol(token);
                                version_t ver = INVALID_VERSION;
                                if (token = std::strtok(NULL, " ")) {
                                    ver = std::stol(token);
                                }
                                try {
                                    objectstore::Object obj = oss.bio_get(oid,ver);
                                    std::cout << obj << std::endl;
                                } catch(...) {
                                    free(argcopy);
                                    return false;
                                }
                                free(argcopy);
                                return true;
                            }}},
            {"remove",  // command
             {
                     "remove <oid>",  // help info
                     [&oss](std::string& args) -> bool {
                         try {
                             return oss.bio_remove(std::stol(args));
                         } catch(...) {
                             return false;
                         }
                     }}},
            {"leave",  // command
             {
                     "leave",  // help info
                     [&oss, &bNextCommand](std::string&) -> bool {
                         oss.leave();
                         bNextCommand = false;
                         return true;
                     }}}};

    std::function<void()> help = [commands]() {
        std::cout << "Commands:" << std::endl;
        for(auto& cmd_entry : commands) {
            std::cout << "\t" << cmd_entry.second.first << std::endl;
        }
    };

    help();
    // main command loop
    while(bNextCommand) {
        std::string line;
        std::cout << "cmd>";
        std::getline(std::cin, line, '\n');
        std::string::size_type first_space_pos = line.find(' ');
        std::string command;
        std::string arguments;
        if(first_space_pos == std::string::npos) {
            command = line;
        } else {
            command = line.substr(0, first_space_pos);
            arguments = line.substr(first_space_pos + 1);
        }
        if(commands.find(command) == commands.end()) {
            help();
        } else {
            bool bRet = commands[command].second(arguments);
            std::cout << "Result:" << std::boolalpha << bRet << std::noboolalpha
                      << std::endl;
        }
    }
    return 0;
}
<|endoftext|>


<commit_before>// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

#ifndef __ROBOT_DEVASTATION_HPP__
#define __ROBOT_DEVASTATION_HPP__

#include <yarp/os/RFModule.h>

#include "RdUtils.hpp"
#include "RateThreadOutput.hpp"
#include "RateThreadProcess.hpp"
#include "RdAudioManager.hpp"
#include "RdMentalMap.hpp"
#include "RdInputManager.hpp"
#include "RdInputEventListener.hpp"
#include "RdYarpNetworkManager.hpp"
#include "RdRobotManager.hpp"
#include "RdRd1RobotManager.hpp"

namespace rd
{

class RobotDevastation : public yarp::os::RFModule, public RdInputEventListener
{
    public:
        bool configure(yarp::os::ResourceFinder &rf);

        virtual bool onKeyUp(RdKey k);
        virtual bool onKeyDown(RdKey k);

    private:
        RateThreadOutput rateThreadOutput;
        RateThreadProcess rateThreadProcess;
        RdInputManager *  inputManager;
        RdAudioManager * audioManager;
        RdMentalMap * mentalMap;
        RdNetworkManager * networkManager;
        RdRobotManager * robotManager;

        yarp::os::BufferedPort< yarp::sig::ImageOf < yarp::sig::PixelRgb> > inImg;

        bool interruptModule();
        double getPeriod();
        bool updateModule();

        bool initSound();

        char* rdRoot;

};

}  // namespace rd

#endif  // __ROBOT_DEVASTATION_HPP__

<commit_msg>ptr for stateMachine in the parent class<commit_after>// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

#ifndef __ROBOT_DEVASTATION_HPP__
#define __ROBOT_DEVASTATION_HPP__

#include <yarp/os/RFModule.h>

#include "RdUtils.hpp"
#include "RateThreadOutput.hpp"
#include "RateThreadProcess.hpp"
#include "RdAudioManager.hpp"
#include "RdMentalMap.hpp"
#include "RdInputManager.hpp"
#include "RdInputEventListener.hpp"
#include "RdYarpNetworkManager.hpp"
#include "RdRobotManager.hpp"
#include "RdRd1RobotManager.hpp"
#include "RdStateMachine.hpp"

namespace rd
{

class RobotDevastation : public yarp::os::RFModule, public RdInputEventListener
{
    public:
        bool configure(yarp::os::ResourceFinder &rf);

        virtual bool onKeyUp(RdKey k);
        virtual bool onKeyDown(RdKey k);

    private:
        RateThreadOutput rateThreadOutput;
        RateThreadProcess rateThreadProcess;
        RdInputManager *  inputManager;
        RdAudioManager * audioManager;
        RdMentalMap * mentalMap;
        RdNetworkManager * networkManager;
        RdRobotManager * robotManager;
        RdStateMachine * stateMachine;

        yarp::os::BufferedPort< yarp::sig::ImageOf < yarp::sig::PixelRgb> > inImg;

        bool interruptModule();
        double getPeriod();
        bool updateModule();

        bool initSound();

        char* rdRoot;

};

}  // namespace rd

#endif  // __ROBOT_DEVASTATION_HPP__

<|endoftext|>


<commit_before>/* -*- mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- */
// vim: ft=cpp:expandtab:ts=8:sw=4:softtabstop=4:
#ident "Copyright (c) 2011 Tokutek Inc.  All rights reserved."
#ident "$Id$"

#include <zlib.h>
#include <lzma.h>

#include "compress.h"
#include "memory.h"
#include "quicklz.h"
#include "toku_assert.h"

static inline enum toku_compression_method
normalize_compression_method(enum toku_compression_method method)
// Effect: resolve "friendly" names like "fast" and "small" into their real values.
{
    switch (method) {
    case TOKU_DEFAULT_COMPRESSION_METHOD:
    case TOKU_FAST_COMPRESSION_METHOD:
        return TOKU_QUICKLZ_METHOD;
    case TOKU_SMALL_COMPRESSION_METHOD:
        return TOKU_LZMA_METHOD;
    default:
        return method; // everything else is fine
    }
}

size_t toku_compress_bound (enum toku_compression_method a, size_t size)
// See compress.h for the specification of this function.
{
    a = normalize_compression_method(a);
    switch (a) {
    case TOKU_NO_COMPRESSION:
        return size + 1;
    case TOKU_LZMA_METHOD:
	return 1+lzma_stream_buffer_bound(size); // We need one extra for the rfc1950-style header byte (bits -03 are TOKU_LZMA_METHOD (1), bits 4-7 are the compression level)
    case TOKU_QUICKLZ_METHOD:
        return size+400 + 1;  // quicklz manual says 400 bytes is enough.  We need one more byte for the rfc1950-style header byte.  bits 0-3 are 9, bits 4-7 are the QLZ_COMPRESSION_LEVEL.
    case TOKU_ZLIB_METHOD:
        return compressBound (size);
    default:
        break;
    }
    // fall through for bad enum (thus compiler can warn us if we didn't use all the enums
    assert(0); return 0;
}

static const int zlib_compression_level = 5;

void toku_compress (enum toku_compression_method a,
                    // the following types and naming conventions come from zlib.h
                    Bytef       *dest,   uLongf *destLen,
                    const Bytef *source, uLong   sourceLen)
// See compress.h for the specification of this function.
{
    a = normalize_compression_method(a);
    assert(sourceLen < (1LL << 32));
    switch (a) {
    case TOKU_NO_COMPRESSION:
        dest[0] = TOKU_NO_COMPRESSION;
        memcpy(dest + 1, source, sourceLen);
        *destLen = sourceLen + 1;
        return;
    case TOKU_ZLIB_METHOD: {
        int r = compress2(dest, destLen, source, sourceLen, zlib_compression_level);
        assert(r == Z_OK);
        assert((dest[0]&0xF) == TOKU_ZLIB_METHOD);
        return;
    }
    case  TOKU_QUICKLZ_METHOD: {
        if (sourceLen==0) {
            // quicklz requires at least one byte, so we handle this ourselves
            assert(1 <= *destLen);
            *destLen = 1;
        } else {
            qlz_state_compress *XMALLOC(qsc);
            size_t actual_destlen = qlz_compress(source, (char*)(dest+1), sourceLen, qsc);
            assert(actual_destlen +1 <= *destLen);
            *destLen = actual_destlen+1; // add one for the rfc1950-style header byte.
            toku_free(qsc);
        }
        // Fill in that first byte
        dest[0] = TOKU_QUICKLZ_METHOD + (QLZ_COMPRESSION_LEVEL << 4);
        return;
    }
    case TOKU_LZMA_METHOD: {
	const int lzma_compression_level = 2;
	if (sourceLen==0) {
	    // lzma version 4.999 requires at least one byte, so we'll do it ourselves.
	    assert(1<=*destLen);
	    *destLen = 1;
	} else {
	    size_t out_pos = 1;
	    lzma_ret r = lzma_easy_buffer_encode(lzma_compression_level, LZMA_CHECK_CRC32, NULL,
						 source, sourceLen,
						 dest, &out_pos, *destLen);
	    assert(out_pos < *destLen);
            if (r != LZMA_OK) {
                fprintf(stderr, "lzma_easy_buffer_encode() returned %d\n", (int) r);
            }
	    assert(r==LZMA_OK);
	    *destLen = out_pos;
	}
	dest[0] = TOKU_LZMA_METHOD + (lzma_compression_level << 4);

	return;
    }
    default:
        break;
    }
    // default fall through to error.
    assert(0);
}

void toku_decompress (Bytef       *dest,   uLongf destLen,
                      const Bytef *source, uLongf sourceLen)
// See compress.h for the specification of this function.
{
    assert(sourceLen>=1); // need at least one byte for the RFC header.
    switch (source[0] & 0xF) {
    case TOKU_NO_COMPRESSION:
        memcpy(dest, source + 1, sourceLen - 1);
        return;
    case TOKU_ZLIB_METHOD: {
        uLongf actual_destlen = destLen;
        int r = uncompress(dest, &actual_destlen, source, sourceLen);
        assert(r == Z_OK);
        assert(actual_destlen == destLen);
        return;
    }
    case TOKU_QUICKLZ_METHOD:
        if (sourceLen>1) {
            qlz_state_decompress *XMALLOC(qsd);
            uLongf actual_destlen = qlz_decompress((char*)source+1, dest, qsd);
            assert(actual_destlen == destLen);
            toku_free(qsd);
        } else {
            // length 1 means there is no data, so do nothing.
            assert(destLen==0);
        }
        return;
    case TOKU_LZMA_METHOD: {
	if (sourceLen>1) {
	    uint64_t memlimit = UINT64_MAX;
	    size_t out_pos = 0;
	    size_t in_pos  = 1;
	    lzma_ret r = lzma_stream_buffer_decode(&memlimit,  // memlimit, use UINT64_MAX to disable this check
						   0,          // flags
						   NULL,       // allocator
						   source, &in_pos, sourceLen,
						   dest,   &out_pos, destLen);
	    assert(r==LZMA_OK);
	    assert(out_pos == destLen);
	} else {
	    // length 1 means there is no data, so do nothing.
	    assert(destLen==0);
	}
	return;
    }
    }
    // default fall through to error.
    assert(0);
}
<commit_msg>refs #5280 add portability include to compress.cc<commit_after>/* -*- mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- */
// vim: ft=cpp:expandtab:ts=8:sw=4:softtabstop=4:
#ident "Copyright (c) 2011 Tokutek Inc.  All rights reserved."
#ident "$Id$"

#include <toku_portability.h>
#include <zlib.h>
#include <lzma.h>

#include "compress.h"
#include "memory.h"
#include "quicklz.h"
#include "toku_assert.h"

static inline enum toku_compression_method
normalize_compression_method(enum toku_compression_method method)
// Effect: resolve "friendly" names like "fast" and "small" into their real values.
{
    switch (method) {
    case TOKU_DEFAULT_COMPRESSION_METHOD:
    case TOKU_FAST_COMPRESSION_METHOD:
        return TOKU_QUICKLZ_METHOD;
    case TOKU_SMALL_COMPRESSION_METHOD:
        return TOKU_LZMA_METHOD;
    default:
        return method; // everything else is fine
    }
}

size_t toku_compress_bound (enum toku_compression_method a, size_t size)
// See compress.h for the specification of this function.
{
    a = normalize_compression_method(a);
    switch (a) {
    case TOKU_NO_COMPRESSION:
        return size + 1;
    case TOKU_LZMA_METHOD:
	return 1+lzma_stream_buffer_bound(size); // We need one extra for the rfc1950-style header byte (bits -03 are TOKU_LZMA_METHOD (1), bits 4-7 are the compression level)
    case TOKU_QUICKLZ_METHOD:
        return size+400 + 1;  // quicklz manual says 400 bytes is enough.  We need one more byte for the rfc1950-style header byte.  bits 0-3 are 9, bits 4-7 are the QLZ_COMPRESSION_LEVEL.
    case TOKU_ZLIB_METHOD:
        return compressBound (size);
    default:
        break;
    }
    // fall through for bad enum (thus compiler can warn us if we didn't use all the enums
    assert(0); return 0;
}

static const int zlib_compression_level = 5;

void toku_compress (enum toku_compression_method a,
                    // the following types and naming conventions come from zlib.h
                    Bytef       *dest,   uLongf *destLen,
                    const Bytef *source, uLong   sourceLen)
// See compress.h for the specification of this function.
{
    a = normalize_compression_method(a);
    assert(sourceLen < (1LL << 32));
    switch (a) {
    case TOKU_NO_COMPRESSION:
        dest[0] = TOKU_NO_COMPRESSION;
        memcpy(dest + 1, source, sourceLen);
        *destLen = sourceLen + 1;
        return;
    case TOKU_ZLIB_METHOD: {
        int r = compress2(dest, destLen, source, sourceLen, zlib_compression_level);
        assert(r == Z_OK);
        assert((dest[0]&0xF) == TOKU_ZLIB_METHOD);
        return;
    }
    case  TOKU_QUICKLZ_METHOD: {
        if (sourceLen==0) {
            // quicklz requires at least one byte, so we handle this ourselves
            assert(1 <= *destLen);
            *destLen = 1;
        } else {
            qlz_state_compress *XMALLOC(qsc);
            size_t actual_destlen = qlz_compress(source, (char*)(dest+1), sourceLen, qsc);
            assert(actual_destlen +1 <= *destLen);
            *destLen = actual_destlen+1; // add one for the rfc1950-style header byte.
            toku_free(qsc);
        }
        // Fill in that first byte
        dest[0] = TOKU_QUICKLZ_METHOD + (QLZ_COMPRESSION_LEVEL << 4);
        return;
    }
    case TOKU_LZMA_METHOD: {
	const int lzma_compression_level = 2;
	if (sourceLen==0) {
	    // lzma version 4.999 requires at least one byte, so we'll do it ourselves.
	    assert(1<=*destLen);
	    *destLen = 1;
	} else {
	    size_t out_pos = 1;
	    lzma_ret r = lzma_easy_buffer_encode(lzma_compression_level, LZMA_CHECK_CRC32, NULL,
						 source, sourceLen,
						 dest, &out_pos, *destLen);
	    assert(out_pos < *destLen);
            if (r != LZMA_OK) {
                fprintf(stderr, "lzma_easy_buffer_encode() returned %d\n", (int) r);
            }
	    assert(r==LZMA_OK);
	    *destLen = out_pos;
	}
	dest[0] = TOKU_LZMA_METHOD + (lzma_compression_level << 4);

	return;
    }
    default:
        break;
    }
    // default fall through to error.
    assert(0);
}

void toku_decompress (Bytef       *dest,   uLongf destLen,
                      const Bytef *source, uLongf sourceLen)
// See compress.h for the specification of this function.
{
    assert(sourceLen>=1); // need at least one byte for the RFC header.
    switch (source[0] & 0xF) {
    case TOKU_NO_COMPRESSION:
        memcpy(dest, source + 1, sourceLen - 1);
        return;
    case TOKU_ZLIB_METHOD: {
        uLongf actual_destlen = destLen;
        int r = uncompress(dest, &actual_destlen, source, sourceLen);
        assert(r == Z_OK);
        assert(actual_destlen == destLen);
        return;
    }
    case TOKU_QUICKLZ_METHOD:
        if (sourceLen>1) {
            qlz_state_decompress *XMALLOC(qsd);
            uLongf actual_destlen = qlz_decompress((char*)source+1, dest, qsd);
            assert(actual_destlen == destLen);
            toku_free(qsd);
        } else {
            // length 1 means there is no data, so do nothing.
            assert(destLen==0);
        }
        return;
    case TOKU_LZMA_METHOD: {
	if (sourceLen>1) {
	    uint64_t memlimit = UINT64_MAX;
	    size_t out_pos = 0;
	    size_t in_pos  = 1;
	    lzma_ret r = lzma_stream_buffer_decode(&memlimit,  // memlimit, use UINT64_MAX to disable this check
						   0,          // flags
						   NULL,       // allocator
						   source, &in_pos, sourceLen,
						   dest,   &out_pos, destLen);
	    assert(r==LZMA_OK);
	    assert(out_pos == destLen);
	} else {
	    // length 1 means there is no data, so do nothing.
	    assert(destLen==0);
	}
	return;
    }
    }
    // default fall through to error.
    assert(0);
}
<|endoftext|>


<commit_before>#include "schedulers/greedy_turbo_scheduler.h"

namespace schedulers {

GreedyTurboScheduler::GreedyTurboScheduler(const game::Race& race,
                      game::CarTracker& car_tracker)
  : race_(race), car_tracker_(car_tracker), should_fire_now_(false) {
  FindLongestStraights();
}

// Returns if should we use turbo
bool GreedyTurboScheduler::ShouldFireTurbo() {
  return should_fire_now_;
}

// Makes decision on turbo usage
void GreedyTurboScheduler::Schedule(const game::CarState& state) {
  if (!state.turbo_state().available() ||
       state.turbo_state().is_on()) {
    should_fire_now_ = false;
    return;
  }

  // TODO its kind of greedy,
  // first of all - we shouldnt count just straights
  // second, it has to make some kind of decisions based on turbo freq

  if (strategy_ == Strategy::kOptimizeRace) {
    // Longest overall
    if (state.position().piece() == straights_[0].from()) {
      should_fire_now_ = true;
    } else if (CanFireBeforeStraight(state, straights_[0])) {
      should_fire_now_ = true;
    }
  } else if (strategy_ == Strategy::kOptimizeCurrentLap) {
    // Longest in between now and lap end
    for (auto& s : straights_) {
      if (s.from() == state.position().piece()) {
        should_fire_now_ = true;
      } else if (CanFireBeforeStraight(state, s)) {
        should_fire_now_ = true;
      } if (s.from() > state.position().piece()) {
        return;
      }
    }
  } else if (strategy_ == Strategy::kOptimizeNextLap) {
    // Give him best speed for next lap
    // TODO not optimal, just taking last piece
    // TODO commenting this wins usa
    auto last = &straights_[0];
    for (auto& s : straights_)
      if (s.from() > last->from())
        last = &s;

    // If connected to 0
    if (last->to() == race_.track().pieces().size() - 1)
      if (state.position().piece() == last->from())
        should_fire_now_ = true;
  }
}

bool GreedyTurboScheduler::CanFireBeforeStraight(const game::CarState& state, const Straight& straight) {
  if (state.position().piece() == straight.from())
    return true;

  if (state.position().piece() > straight.from())
    return false;

  // W teoriinie trzeba samych jedynek...
  if (state.position().piece() <= straight.from() &&
      state.position().piece() >= straight.from() - 2) {
    auto s = car_tracker_.Predict(state, game::Command(game::TurboToggle::kToggleOn));
    int max_ticks = 80;
    while (max_ticks-- > 0 && s.position().piece() != straight.from()) {
      if (!car_tracker_.crash_model().IsSafe(s.position().angle()))
        return false;
      s = car_tracker_.Predict(s, game::Command(1));
    }
    return car_tracker_.IsSafe(s);
  }
  return false;
}

void GreedyTurboScheduler::FindLongestStraights() {
  const auto& pieces = race_.track().pieces();

  int from = -1;
  double length = 0;
  for (int i = 0; i < pieces.size(); i++) {
    if (pieces[i].type() == game::PieceType::kStraight) {
      if (from == -1)
        from = i;
      length += pieces[i].length();
    } else {
      if (from != -1) {
        straights_.push_back(Straight(length, from, i - 1));
        length = 0;
        from = -1;
      }
    }
  }
  if (from != -1)
    straights_.push_back(Straight(length, from, pieces.size() - 1));

  sort(straights_.begin(), straights_.end(), [](const Straight& a, const Straight& b) {
      if (a.length() - b.length() > 1e-9) return true;
      if (b.length() - a.length() > 1e-9) return false;
      return a.from() < b.from(); } );
}

// Prepare for overtake
void GreedyTurboScheduler::Overtake(const string& color) {
  printf("Feature not implemented.\n");
}

void GreedyTurboScheduler::TurboUsed() {
  should_fire_now_ = false;
}

}  // namespace schedulers
<commit_msg>Add quick turbo heuristics<commit_after>#include "schedulers/greedy_turbo_scheduler.h"

namespace schedulers {

GreedyTurboScheduler::GreedyTurboScheduler(const game::Race& race,
                      game::CarTracker& car_tracker)
  : race_(race), car_tracker_(car_tracker), should_fire_now_(false) {
  FindLongestStraights();
}

// Returns if should we use turbo
bool GreedyTurboScheduler::ShouldFireTurbo() {
  return should_fire_now_;
}

// Makes decision on turbo usage
void GreedyTurboScheduler::Schedule(const game::CarState& state) {
  if (!state.turbo_state().available() ||
       state.turbo_state().is_on()) {
    should_fire_now_ = false;
    return;
  }

  // TODO its kind of greedy,
  // first of all - we shouldnt count just straights
  // second, it has to make some kind of decisions based on turbo freq

  if (strategy_ == Strategy::kOptimizeRace) {
    // Longest overall
    if (state.position().piece() == straights_[0].from()) {
      should_fire_now_ = true;
    } else if (CanFireBeforeStraight(state, straights_[0])) {
      should_fire_now_ = true;
    }
  } else if (strategy_ == Strategy::kOptimizeCurrentLap) {
    // Longest in between now and lap end
    for (auto& s : straights_) {
      if (s.from() == state.position().piece()) {
        should_fire_now_ = true;
      } else if (CanFireBeforeStraight(state, s)) {
        should_fire_now_ = true;
      } if (s.from() > state.position().piece()) {
        return;
      }
    }
  } else if (strategy_ == Strategy::kOptimizeNextLap) {
    // Give him best speed for next lap
    // TODO not optimal, just taking last piece
    // TODO commenting this wins usa
    auto last = &straights_[0];
    for (auto& s : straights_)
      if (s.from() > last->from())
        last = &s;

    // If connected to 0
    if (last->to() == race_.track().pieces().size() - 1)
      if (state.position().piece() == last->from())
        should_fire_now_ = true;
  }
}

bool GreedyTurboScheduler::CanFireBeforeStraight(const game::CarState& state, const Straight& straight) {
  if (state.position().piece() == straight.from())
    return true;

  if (state.position().piece() > straight.from())
    return false;

  // W teoriinie trzeba samych jedynek...
  if (state.position().piece() <= straight.from() &&
      state.position().piece() >= straight.from() - 2) {
    auto s = car_tracker_.Predict(state, game::Command(game::TurboToggle::kToggleOn));
    int max_ticks = 80;
    while (max_ticks-- > 0 && s.position().piece() != straight.from()) {
      if (!car_tracker_.crash_model().IsSafe(s.position().angle()))
        return false;
      s = car_tracker_.Predict(s, game::Command(1));
    }
    return car_tracker_.IsSafe(s);
  }
  return false;
}

void GreedyTurboScheduler::FindLongestStraights() {
  const auto& pieces = race_.track().pieces();

  int from = -1;
  double length = 0;
  for (int i = 0; i < pieces.size(); i++) {
    if (pieces[i].type() == game::PieceType::kStraight ||
        (pieces[i].radius() >= 150 && from != -1)) {
      if (from == -1)
        from = i;
      length += pieces[i].length();
    } else {
      if (from != -1) {
        straights_.push_back(Straight(length, from, i - 1));
        length = 0;
        from = -1;
      }
    }
  }
  if (from != -1)
    straights_.push_back(Straight(length, from, pieces.size() - 1));

  sort(straights_.begin(), straights_.end(), [](const Straight& a, const Straight& b) {
      if (a.length() - b.length() > 1e-9) return true;
      if (b.length() - a.length() > 1e-9) return false;
      return a.from() < b.from(); } );
}

// Prepare for overtake
void GreedyTurboScheduler::Overtake(const string& color) {
  printf("Feature not implemented.\n");
}

void GreedyTurboScheduler::TurboUsed() {
  should_fire_now_ = false;
}

}  // namespace schedulers
<|endoftext|>


<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) 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. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * 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 files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// TriMesh code by Erwin de Vries.

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>

#if dTRIMESH_ENABLED

#include "collision_util.h"

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

#if dTRIMESH_OPCODE
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
	dIASSERT (Stride >= (int)sizeof(dContactGeom));
	dIASSERT (g1->type == dTriMeshClass);
	dIASSERT (RayGeom->type == dRayClass);
	dIASSERT ((Flags & NUMC_MASK) >= 1);

	dxTriMesh* TriMesh = (dxTriMesh*)g1;

	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);

	RayCollider& Collider = TriMesh->_RayCollider;

	dReal Length = dGeomRayGetLength(RayGeom);

	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);

	Collider.SetFirstContact(FirstContact != 0);
	Collider.SetClosestHit(ClosestHit != 0);
	Collider.SetCulling(BackfaceCull != 0);
	Collider.SetMaxDist(Length);

	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

	/* Make Ray */
	Ray WorldRay;
	WorldRay.mOrig.x = Origin[0];
	WorldRay.mOrig.y = Origin[1];
	WorldRay.mOrig.z = Origin[2];
	WorldRay.mDir.x = Direction[0];
	WorldRay.mDir.y = Direction[1];
	WorldRay.mDir.z = Direction[2];

	/* Intersect */
	Matrix4x4 amatrix;
        int TriCount = 0;
        if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
                TriCount = TriMesh->Faces.GetNbFaces();
        }

        if (TriCount == 0) {
                return 0;
        }

	const CollisionFace* Faces = TriMesh->Faces.GetFaces();

	int OutTriCount = 0;
	for (int i = 0; i < TriCount; i++) {
		if (TriMesh->RayCallback == null ||
                    TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
                                         Faces[i].mU, Faces[i].mV)) {
			const int& TriIndex = Faces[i].mFaceID;
			if (!Callback(TriMesh, RayGeom, TriIndex)) {
                                continue;
                        }

			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);

			dVector3 dv[3];
			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);

			float T = Faces[i].mDistance;
			Contact->pos[0] = Origin[0] + (Direction[0] * T);
			Contact->pos[1] = Origin[1] + (Direction[1] * T);
			Contact->pos[2] = Origin[2] + (Direction[2] * T);
			Contact->pos[3] = REAL(0.0);

			dVector3 vu;
			vu[0] = dv[1][0] - dv[0][0];
			vu[1] = dv[1][1] - dv[0][1];
			vu[2] = dv[1][2] - dv[0][2];
			vu[3] = REAL(0.0);

			dVector3 vv;
			vv[0] = dv[2][0] - dv[0][0];
			vv[1] = dv[2][1] - dv[0][1];
			vv[2] = dv[2][2] - dv[0][2];
			vv[3] = REAL(0.0);

			dCROSS(Contact->normal, =, vv, vu);	// Reversed

			dNormalize3(Contact->normal);

			Contact->depth = T;
			Contact->g1 = TriMesh;
			Contact->g2 = RayGeom;

			OutTriCount++;

			// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
			if (OutTriCount >= (Flags & NUMC_MASK)) {
				break;
			}
		}
	}
	return OutTriCount;
}
#endif // dTRIMESH_OPCODE

#if dTRIMESH_GIMPACT
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
{
	dIASSERT (Stride >= (int)sizeof(dContactGeom));
	dIASSERT (g1->type == dTriMeshClass);
	dIASSERT (RayGeom->type == dRayClass);
	dIASSERT ((Flags & NUMC_MASK) >= 1);

	dxTriMesh* TriMesh = (dxTriMesh*)g1;

    dReal Length = dGeomRayGetLength(RayGeom);
	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);
	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

    char intersect=0;
    GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;

	if(ClosestHit)
	{
		intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
	}
	else
	{
	    intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
	}

    if(intersect == 0)
	{
        return 0;
    }

	int OutTriCount = 0;

	if(!TriMesh->RayCallback ||
		TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
	{
	    dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride);
        VEC_COPY(Contact->pos,contact_data.m_point);
        VEC_COPY(Contact->normal,contact_data.m_normal);
        Contact->depth = contact_data.tparam;
        Contact->g1 = TriMesh;
        Contact->g2 = RayGeom;

		OutTriCount = 1;
	}

	return OutTriCount;
}
#endif  // dTRIMESH_GIMPACT

#endif // dTRIMESH_ENABLED

<commit_msg>Improvement: changed variable type from float to dReal to prevent possible loss of precision in future<commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) 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. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * 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 files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// TriMesh code by Erwin de Vries.

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>

#if dTRIMESH_ENABLED

#include "collision_util.h"

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

#if dTRIMESH_OPCODE
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
	dIASSERT (Stride >= (int)sizeof(dContactGeom));
	dIASSERT (g1->type == dTriMeshClass);
	dIASSERT (RayGeom->type == dRayClass);
	dIASSERT ((Flags & NUMC_MASK) >= 1);

	dxTriMesh* TriMesh = (dxTriMesh*)g1;

	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);

	RayCollider& Collider = TriMesh->_RayCollider;

	dReal Length = dGeomRayGetLength(RayGeom);

	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);

	Collider.SetFirstContact(FirstContact != 0);
	Collider.SetClosestHit(ClosestHit != 0);
	Collider.SetCulling(BackfaceCull != 0);
	Collider.SetMaxDist(Length);

	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

	/* Make Ray */
	Ray WorldRay;
	WorldRay.mOrig.x = Origin[0];
	WorldRay.mOrig.y = Origin[1];
	WorldRay.mOrig.z = Origin[2];
	WorldRay.mDir.x = Direction[0];
	WorldRay.mDir.y = Direction[1];
	WorldRay.mDir.z = Direction[2];

	/* Intersect */
	Matrix4x4 amatrix;
        int TriCount = 0;
        if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
                TriCount = TriMesh->Faces.GetNbFaces();
        }

        if (TriCount == 0) {
                return 0;
        }

	const CollisionFace* Faces = TriMesh->Faces.GetFaces();

	int OutTriCount = 0;
	for (int i = 0; i < TriCount; i++) {
		if (TriMesh->RayCallback == null ||
                    TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
                                         Faces[i].mU, Faces[i].mV)) {
			const int& TriIndex = Faces[i].mFaceID;
			if (!Callback(TriMesh, RayGeom, TriIndex)) {
                                continue;
                        }

			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);

			dVector3 dv[3];
			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);

			// No sense to save on single type conversion in algorithm of this size.
			// If there would be a custom typedef for distance type it could be used
			// instead of dReal. However using float directly is the loss of abstraction
			// and possible loss of precision in future.
			/*float*/ dReal T = Faces[i].mDistance;
			Contact->pos[0] = Origin[0] + (Direction[0] * T);
			Contact->pos[1] = Origin[1] + (Direction[1] * T);
			Contact->pos[2] = Origin[2] + (Direction[2] * T);
			Contact->pos[3] = REAL(0.0);

			dVector3 vu;
			vu[0] = dv[1][0] - dv[0][0];
			vu[1] = dv[1][1] - dv[0][1];
			vu[2] = dv[1][2] - dv[0][2];
			vu[3] = REAL(0.0);

			dVector3 vv;
			vv[0] = dv[2][0] - dv[0][0];
			vv[1] = dv[2][1] - dv[0][1];
			vv[2] = dv[2][2] - dv[0][2];
			vv[3] = REAL(0.0);

			dCROSS(Contact->normal, =, vv, vu);	// Reversed

			dNormalize3(Contact->normal);

			Contact->depth = T;
			Contact->g1 = TriMesh;
			Contact->g2 = RayGeom;

			OutTriCount++;

			// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
			if (OutTriCount >= (Flags & NUMC_MASK)) {
				break;
			}
		}
	}
	return OutTriCount;
}
#endif // dTRIMESH_OPCODE

#if dTRIMESH_GIMPACT
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
{
	dIASSERT (Stride >= (int)sizeof(dContactGeom));
	dIASSERT (g1->type == dTriMeshClass);
	dIASSERT (RayGeom->type == dRayClass);
	dIASSERT ((Flags & NUMC_MASK) >= 1);

	dxTriMesh* TriMesh = (dxTriMesh*)g1;

    dReal Length = dGeomRayGetLength(RayGeom);
	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);
	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

    char intersect=0;
    GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;

	if(ClosestHit)
	{
		intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
	}
	else
	{
	    intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
	}

    if(intersect == 0)
	{
        return 0;
    }

	int OutTriCount = 0;

	if(!TriMesh->RayCallback ||
		TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
	{
		dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride);
        VEC_COPY(Contact->pos,contact_data.m_point);
        VEC_COPY(Contact->normal,contact_data.m_normal);
        Contact->depth = contact_data.tparam;
        Contact->g1 = TriMesh;
        Contact->g2 = RayGeom;

		OutTriCount = 1;
	}

	return OutTriCount;
}
#endif  // dTRIMESH_GIMPACT

#endif // dTRIMESH_ENABLED

<|endoftext|>