C++ std::shared_ptr类代码示例

 Leg(std::shared_ptr<geogo::Mesh> _geometry, const glm::vec3& foot_pos) : geometry(_geometry), default_foot_pos_local(foot_pos), default_hip_pos_local(0.1f * foot_pos)
 {
     foot_vertex = geometry->create_vertex();
     hip_vertex = geometry->create_vertex();
     geometry->create_edge(hip_vertex, foot_vertex);
 }

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChTrackShoeBandBushing::Connect(std::shared_ptr<ChTrackShoe> next) {
    // Bushings are inherited from ChLoad, so they require a 'load container'
    auto loadcontainer = std::make_shared<ChLoadContainer>();
    m_shoe->GetSystem()->Add(loadcontainer);

    // Stiffness and Damping matrix values
    ChMatrixNM<double, 6, 6> K_matrix;
    ChMatrixNM<double, 6, 6> R_matrix;

    K_matrix(0, 0) = m_Klin;
    K_matrix(1, 1) = m_Klin;
    K_matrix(2, 2) = m_Klin;
    K_matrix(3, 3) = m_Krot_other;
    K_matrix(4, 4) = m_Krot_dof;
    K_matrix(5, 5) = m_Krot_other;

    R_matrix(0, 0) = m_Dlin;
    R_matrix(1, 1) = m_Dlin;
    R_matrix(2, 2) = m_Dlin;
    R_matrix(3, 3) = m_Drot_other;
    R_matrix(4, 4) = m_Drot_dof;
    R_matrix(5, 5) = m_Drot_other;

    int index = 0;

    // Connect tread body to the first web segment.
    {
        ChVector<> loc = m_shoe->TransformPointLocalToParent(ChVector<>(GetToothBaseLength() / 2, 0, 0));
        ChQuaternion<>& rot = m_shoe->GetRot();
        auto loadbushing = std::make_shared<ChLoadBodyBodyBushingGeneric>(
            m_shoe,               // body A
            m_web_segments[0],    // body B
            ChFrame<>(loc, rot),  // initial frame of bushing in abs space
            K_matrix,             // the 6x6 (translation+rotation) K matrix in local frame
            R_matrix              // the 6x6 (translation+rotation) R matrix in local frame
        );
        loadbushing->SetNameString(m_name + "_bushing_" + std::to_string(index++));
        loadbushing->SetApplicationFrameA(ChFrame<>(ChVector<>(GetToothBaseLength() / 2, 0, 0)));
        loadbushing->SetApplicationFrameB(ChFrame<>(ChVector<>(-m_seg_length / 2, 0, 0)));
        loadcontainer->Add(loadbushing);
        m_web_bushings.push_back(loadbushing);
    }

    // Connect the web segments to each other.
    for (size_t is = 0; is < GetNumWebSegments() - 1; is++) {
        ChVector<> loc = m_web_segments[is]->TransformPointLocalToParent(ChVector<>(m_seg_length / 2, 0, 0));
        ChQuaternion<>& rot = m_web_segments[is]->GetRot();
        auto loadbushing = std::make_shared<ChLoadBodyBodyBushingGeneric>(
            m_web_segments[is],      // body A
            m_web_segments[is + 1],  // body B
            ChFrame<>(loc, rot),     // initial frame of bushing in abs space
            K_matrix,                // the 6x6 (translation+rotation) K matrix in local frame
            R_matrix                 // the 6x6 (translation+rotation) R matrix in local frame
        );
        loadbushing->SetNameString(m_name + "_bushing_" + std::to_string(index++));
        loadbushing->SetApplicationFrameA(ChFrame<>(ChVector<>(m_seg_length / 2, 0, 0)));
        loadbushing->SetApplicationFrameB(ChFrame<>(ChVector<>(-m_seg_length / 2, 0, 0)));
        loadcontainer->Add(loadbushing);
        m_web_bushings.push_back(loadbushing);
    }

    {
        // Connect the last web segment to the tread body from the next track shoe.
        int is = GetNumWebSegments() - 1;
        ChVector<> loc = m_web_segments[is]->TransformPointLocalToParent(ChVector<>(m_seg_length / 2, 0, 0));
        ChQuaternion<>& rot = m_web_segments[is]->GetRot();
        auto loadbushing = std::make_shared<ChLoadBodyBodyBushingGeneric>(
            m_web_segments[is],   // body A
            next->GetShoeBody(),  // body B
            ChFrame<>(loc, rot),  // initial frame of bushing in abs space
            K_matrix,             // the 6x6 (translation+rotation) K matrix in local frame
            R_matrix              // the 6x6 (translation+rotation) R matrix in local frame
        );
        loadbushing->SetNameString(m_name + "_bushing_" + std::to_string(index++));
        loadbushing->SetApplicationFrameA(ChFrame<>(ChVector<>(m_seg_length / 2, 0, 0)));
        loadbushing->SetApplicationFrameB(ChFrame<>(ChVector<>(-GetToothBaseLength() / 2, 0, 0)));
        loadcontainer->Add(loadbushing);
        m_web_bushings.push_back(loadbushing);
    }
}

shared_ptr<SwitchState> ThriftConfigApplier::run() {
    auto newState = orig_->clone();
    bool changed = false;

    processVlanPorts();

    {
        auto newPorts = updatePorts();
        if (newPorts) {
            newState->resetPorts(std::move(newPorts));
            changed = true;
        }
    }

    {
        auto newIntfs = updateInterfaces();
        if (newIntfs) {
            newState->resetIntfs(std::move(newIntfs));
            changed = true;
        }
    }

    // Note: updateInterfaces() must be called before updateVlans(),
    // as updateInterfaces() populates the vlanInterfaces_ data structure.
    {
        auto newVlans = updateVlans();
        if (newVlans) {
            newState->resetVlans(std::move(newVlans));
            changed = true;
        }
    }

    // Note: updateInterfaces() must be called before updateRouteTables(),
    // as updateInterfaces() populates the intfRouteTables_ data structure.
    {
        auto newTables = updateRouteTables();
        if (newTables) {
            newState->resetRouteTables(std::move(newTables));
            changed = true;
        }
    }

    // Make sure all interfaces refer to valid VLANs.
    auto newVlans = newState->getVlans();
    for (const auto& vlanInfo : vlanInterfaces_) {
        if (newVlans->getVlanIf(vlanInfo.first) == nullptr) {
            throw FbossError("Interface ",
                             *(vlanInfo.second.interfaces.begin()),
                             " refers to non-existent VLAN ", vlanInfo.first);
        }
    }

    VlanID dfltVlan(cfg_->defaultVlan);
    if (orig_->getDefaultVlan() != dfltVlan) {
        if (newVlans->getVlanIf(dfltVlan) == nullptr) {
            throw FbossError("Default VLAN ", dfltVlan, " does not exist");
        }
        newState->setDefaultVlan(dfltVlan);
        changed = true;
    }

    std::chrono::seconds arpAgerInterval(cfg_->arpAgerInterval);
    if (orig_->getArpAgerInterval() != arpAgerInterval) {
        newState->setArpAgerInterval(arpAgerInterval);
        changed = true;
    }

    if (!changed) {
        return nullptr;
    }
    return newState;
}

void WorldLogic::setForce(std::shared_ptr<Box> &box, std::shared_ptr<Paddle> &paddle)
{
  if (!paddle) {
    std::uniform_real_distribution<scalar_type> x_dist(-20.f, 20.f), y_dist(-5.f, 25.f);
    box->externalForce() = vec3_type(x_dist(rng), y_dist(rng), 0.f);

    return;
  }

  // I like how this function completely disregards the size of the box in
  // regard to how box and paddle overlap

  vec3_type pll = paddle->position() - paddle->size() / 2.f; // lower left
  vec3_type pur = paddle->position() + paddle->size() / 2.f; // upper right
  pll.y() = -HUGE_VALF;
  pur.y() =  HUGE_VALF;

  vec3_type force;

  if (in_bounding_box(box->position(), pll, pur)) {
    force = vec3_type(0.f, 1.f, 0.f);
  } else if (box->position().y() - box->size() / 2.f > paddle->position().y()) { // "oberhalb", not "oberhalb oder auf gleicher Höhe"
    vec3_type min_vector(HUGE_VALF, HUGE_VALF, HUGE_VALF);

    // Only consider vertices, no edges (the tasks requires this)
    // (2edgy4u)
    for (int xm: {-1, 1}) {
      for (int ym: {-1, 1}) {
        vec3_type paddle_vertex =
            paddle->position() + vec3_type(xm * paddle->size().x(), ym * paddle->size().y(), 0.f);

        vec3_type vector = box->position() - paddle_vertex;
        if (vector.length() < min_vector.length()) {
          min_vector = vector;
        }
      }
    }

    min_vector.normalize();

    // If a box is bigger than the paddle, it might happen that the box is
    // off-center, but its closest edge ie on the other side of the paddle
    // center than most of the box; this will lead to the box being pushed in
    // the wrong direction. Fix this here.
    if (((box->position().x() > paddle->position().x()) && (min_vector.x() < 0.f)) ||
        ((box->position().x() < paddle->position().x()) && (min_vector.x() > 0.f)))
    {
      min_vector.x() = 0.f;
    }

    force = off_force_mult * powf(vec3_type(0.f, 1.f, 0.f).dot(min_vector), off_force_exp) * min_vector;
  }

  box->externalForce() = paddle->relativeFanPower() * force * 10.f * box->size() * box->size();
}

void Flow::calculateConstraints(const std::shared_ptr<AnimSkeleton>& skeleton, 
                                AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses) {
    cleanUp();
    if (!skeleton) {
        return;
    }
    auto flowPrefix = FLOW_JOINT_PREFIX.toUpper();
    auto simPrefix = SIM_JOINT_PREFIX.toUpper();
    std::vector<int> handsIndices;
    _groupSettings.clear();

    for (int i = 0; i < skeleton->getNumJoints(); i++) {
        auto name = skeleton->getJointName(i);
        if (std::find(HAND_COLLISION_JOINTS.begin(), HAND_COLLISION_JOINTS.end(), name) != HAND_COLLISION_JOINTS.end()) {
            handsIndices.push_back(i);
        }
        auto parentIndex = skeleton->getParentIndex(i);
        if (parentIndex == -1) {
            continue;
        }
        auto jointChildren = skeleton->getChildrenOfJoint(i);
        // auto childIndex = jointChildren.size() > 0 ? jointChildren[0] : -1;
        auto group = QStringRef(&name, 0, 3).toString().toUpper();
        auto split = name.split("_");
        bool isSimJoint = (group == simPrefix);
        bool isFlowJoint = split.size() > 2 && split[0].toUpper() == flowPrefix;
        if (isFlowJoint || isSimJoint) {
            group = "";
            if (isSimJoint) {
                for (int j = 1; j < name.size() - 1; j++) {
                    bool toFloatSuccess;
                    QStringRef(&name, (int)(name.size() - j), 1).toString().toFloat(&toFloatSuccess);
                    if (!toFloatSuccess && (name.size() - j) > (int)simPrefix.size()) {
                        group = QStringRef(&name, (int)simPrefix.size(), (int)(name.size() - j + 1) - (int)simPrefix.size()).toString();
                        break;
                    }
                }
                if (group.isEmpty()) {
                    group = QStringRef(&name, (int)simPrefix.size(), name.size() - (int)simPrefix.size()).toString();
                }
                qCDebug(animation) << "Sim joint added to flow: " << name;
            } else {
                group = split[1];
            }
            if (!group.isEmpty()) {
                _flowJointKeywords.push_back(group);
                FlowPhysicsSettings jointSettings;
                if (PRESET_FLOW_DATA.find(group) != PRESET_FLOW_DATA.end()) {
                    jointSettings = PRESET_FLOW_DATA.at(group);
                } else {
                    jointSettings = DEFAULT_JOINT_SETTINGS;
                }
                if (_flowJointData.find(i) ==  _flowJointData.end()) {
                    auto flowJoint = FlowJoint(i, parentIndex, -1, name, group, jointSettings);
                    _flowJointData.insert(std::pair<int, FlowJoint>(i, flowJoint));
                }
                updateGroupSettings(group, jointSettings);
            }
        } else {
            if (PRESET_COLLISION_DATA.find(name) != PRESET_COLLISION_DATA.end()) {
                _collisionSystem.addCollisionSphere(i, PRESET_COLLISION_DATA.at(name));
            }
        }
    }

    for (auto &jointData : _flowJointData) {
        int jointIndex = jointData.first;
        glm::vec3 jointPosition, parentPosition, jointTranslation;
        glm::quat jointRotation;
        getJointPositionInWorldFrame(absolutePoses, jointIndex, jointPosition, _entityPosition, _entityRotation);
        getJointTranslation(relativePoses, jointIndex, jointTranslation);
        getJointRotation(relativePoses, jointIndex, jointRotation);
        getJointPositionInWorldFrame(absolutePoses, jointData.second.getParentIndex(), parentPosition, _entityPosition, _entityRotation);

        jointData.second.setInitialData(jointPosition, jointTranslation, jointRotation, parentPosition);
    }

    std::vector<int> roots;

    for (auto &joint :_flowJointData) {
        if (_flowJointData.find(joint.second.getParentIndex()) == _flowJointData.end()) {
            joint.second.setAnchored(true);
            roots.push_back(joint.first);
        } else {
            _flowJointData[joint.second.getParentIndex()].setChildIndex(joint.first);
        }
    }
    int extraIndex = -1;
    for (size_t i = 0; i < roots.size(); i++) {
        FlowThread thread = FlowThread(roots[i], &_flowJointData);
        // add threads with at least 2 joints
        if (thread._joints.size() > 0) {
            if (thread._joints.size() == 1) {
                int jointIndex = roots[i];
                auto &joint = _flowJointData[jointIndex];
                auto &jointPosition = joint.getUpdatedPosition();
                auto newSettings = joint.getSettings();
                extraIndex = extraIndex > -1 ? extraIndex + 1 : skeleton->getNumJoints();
                joint.setChildIndex(extraIndex);
                auto newJoint = FlowJoint(extraIndex, jointIndex, -1, joint.getName(), joint.getGroup(), newSettings);
//.........这里部分代码省略.........

std::unique_ptr<RenderQueue> BREW::CreateFrameDrawable( std::shared_ptr<const Frame> frame ) const {
	auto padding = GetProperty<float>( "Padding", frame );
	auto border_color = GetProperty<sf::Color>( "BorderColor", frame );
	auto color = GetProperty<sf::Color>( "Color", frame );
	auto border_width = GetProperty<float>( "BorderWidth", frame );
	const auto& font_name = GetProperty<std::string>( "FontName", frame );
	auto font_size = GetProperty<unsigned int>( "FontSize", frame );
	const auto& font = GetResourceManager().GetFont( font_name );
	auto label_padding = GetProperty<float>( "LabelPadding", frame );
	auto line_height = GetFontLineHeight( *font, font_size );

	std::unique_ptr<RenderQueue> queue( new RenderQueue );

	// Right
	queue->Add(
		Renderer::Get().CreateLine(
			sf::Vector2f( frame->GetAllocation().width - border_width / 2.f, line_height / 2.f + border_width / 2.f ),
			sf::Vector2f( frame->GetAllocation().width - border_width / 2.f, frame->GetAllocation().height - border_width ),
			border_color,
			border_width
		)
	);

	// Bottom
	queue->Add(
		Renderer::Get().CreateLine(
			sf::Vector2f( frame->GetAllocation().width - border_width / 2.f, frame->GetAllocation().height - border_width ),
			sf::Vector2f( border_width / 2.f, frame->GetAllocation().height - border_width ),
			border_color,
			border_width
		)
	);

	// Left
	queue->Add(
		Renderer::Get().CreateLine(
			sf::Vector2f( border_width / 2.f, frame->GetAllocation().height - border_width ),
			sf::Vector2f( border_width / 2.f, line_height / 2.f + border_width / 2.f ),
			border_color,
			border_width
		)
	);

	auto label_start_x = 0.f;
	auto label_end_x = 0.f;

	auto alignment = frame->GetAlignment().x;

	if( frame->GetLabel().getSize() > 0 ) {
		auto metrics = GetTextStringMetrics( frame->GetLabel(), *font, font_size );
		metrics.x += 2.f * label_padding;

		label_start_x = padding + ( alignment * ( frame->GetAllocation().width - 2.f * padding - metrics.x ) );
		label_end_x = label_start_x + metrics.x;

		sf::Text text( frame->GetLabel(), *font, font_size );
		text.setPosition( label_start_x + label_padding, border_width / 2.f );
		text.setFillColor( color );
		queue->Add( Renderer::Get().CreateText( text ) );
	}

	// Top Left
	queue->Add(
		Renderer::Get().CreateLine(
			sf::Vector2f( border_width / 2.f, line_height / 2.f + border_width / 2.f ),
			sf::Vector2f( label_start_x - .5f * border_width, line_height / 2.f + border_width / 2.f ),
			border_color,
			border_width
		)
	);

	// Top Right
	queue->Add(
		Renderer::Get().CreateLine(
			sf::Vector2f( label_end_x + .5f * border_width, line_height / 2.f + border_width / 2.f ),
			sf::Vector2f( frame->GetAllocation().width - border_width / 2.f, line_height / 2.f + border_width / 2.f ),
			border_color,
			border_width
		)
	);

	return queue;
}

  void mcts_two_players<Game>::think(const std::shared_ptr<Game>& game)
  {
    using namespace std;
    const chrono::steady_clock::time_point start = chrono::steady_clock::now();
    chrono::steady_clock::time_point now;
    mt19937& generator = mcts<Game>::generators[util::omp_util::get_thread_num()];
    auto state = game->get_state();
    vector<node*> visited(200);
    vector<uint16_t> moves(200);
    unsigned int nb_iter = 0;
    do
      {
        int size = 1;
        node* current = this->root;
        visited[0] = current;
        while (!game->end_of_game() && !current->is_leaf() && !current->is_proven())
	  {
            current = select(game, generator, current);
            visited[size++] = current;
	  }
        int game_value = 0;
        if (current->is_proven())
	  {
            if (current->is_won()) game_value = 1;
            else
	      {
                game_value = -1;
	      }
	  }
        else if (game->end_of_game())
	  {
            int v = game->value_for_current_player();
            if (v > 0)
	      {
                game_value = 1;
                if (new_version_) current->set_won();
	      }
            else if (v < 0)
	      {
                game_value = -1;
                if (new_version_)
		  {
                    current->set_lost();
                    if (size > 1) visited[size - 2]->set_won();
		  }
	      }
	  }
        else
	  {
            uint8_t player = game->current_player();
            expand(game, current);
            game->playout(generator);
            int v = game->value(player);
            if (v > 0) game_value = 1;
            else if (v < 0) game_value = -1;
	  }
        for (int i = size - 1; i >= 0; --i)
	  {
            visited[i]->update(game_value);
            game_value = -game_value;
	  }
        game->set_state(state);
        ++nb_iter;
        if ((nb_iter & 0x3F) == 0) now = chrono::steady_clock::now();
      }
    while ((nb_iter & 0x3F) != 0 || now < start + this->milliseconds);
  }

    void MakeBid(
        std::shared_ptr<Packet_ServerBeginRound> roundInfo,   // Information about this particular round
        const std::shared_ptr<Packet_ServerRequestBid> request,     // The specific request we received
        double period,                                                                          // How long this bidding period will last
        double skewEstimate,                                                                // An estimate of the time difference between us and the server (positive -> we are ahead)
        std::vector<uint32_t> &solution,                                                // Our vector of indices describing the solution
        uint32_t *pProof                                                                        // Will contain the "proof", which is just the value
    )
    {
        double tSafetyMargin = 0.5; // accounts for uncertainty in network conditions
        /* This is when the server has said all bids must be produced by, plus the
            adjustment for clock skew, and the safety margin
        */
        double tFinish = request->timeStampReceiveBids * 1e-9 + skewEstimate - tSafetyMargin;

        Log(Log_Verbose, "MakeBid - start, total period=%lg.", period);

        /*
            We will use this to track the best solution we have created so far.
        */
        roundInfo->maxIndices = 4;
        std::vector<uint32_t> bestSolution(roundInfo->maxIndices);
        std::vector<uint32_t> gpuBestSolution(roundInfo->maxIndices);
        bigint_t bestProof, gpuBestProof;

        wide_ones(BIGINT_WORDS, bestProof.limbs);

        // Incorporate the existing block chain data - in a real system this is the
        // list of transactions we are signing. This is the FNV hash:
        // http://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function
        hash::fnv<64> hasher;
        uint64_t chainHash = hasher((const char *)&roundInfo->chainData[0], roundInfo->chainData.size());

        bigint_t x;
        wide_x_init(&x.limbs[0], uint32_t(0), roundInfo->roundId, roundInfo->roundSalt, chainHash);

        std::vector<uint32_t> indices(roundInfo->maxIndices);

        //Define TBB arrays
        uint32_t *parallel_Indices = (uint32_t *)malloc(sizeof(uint32_t) * TBB_PARALLEL_COUNT);
        uint32_t *parallel_BestSolutions = (uint32_t *)malloc(sizeof(uint32_t) * TBB_PARALLEL_COUNT * roundInfo->maxIndices);
        uint32_t *parallel_Proofs = (uint32_t *)malloc(sizeof(uint32_t) * 8 * TBB_PARALLEL_COUNT);
        uint32_t *parallel_BestProofs = (uint32_t *)malloc(sizeof(uint32_t) * 8 * TBB_PARALLEL_COUNT);

        //Define GPU arrays
        uint32_t *d_ParallelBestSolutions;

        checkCudaErrors(cudaMalloc((void **)&d_ParallelBestSolutions, sizeof(uint32_t) * CUDA_DIM * CUDA_DIM * roundInfo->maxIndices));

        checkCudaErrors(cudaMemcpy(d_hashConstant, &roundInfo->c[0], sizeof(uint32_t) * 4, cudaMemcpyHostToDevice));

        unsigned gpuTrials = 0;
        unsigned cpuTrials = 0;

        unsigned maxNum = uint32_t(0xFFFFFFFF);

        auto runGPU = [ = , &gpuTrials]
        {
            cudaInit(CUDA_DIM, d_ParallelBestProofs);

            do
            {
                cudaIteration(d_ParallelIndices, d_ParallelProofs, d_ParallelBestProofs, d_ParallelBestSolutions, x, d_hashConstant, roundInfo->hashSteps, CUDA_DIM, gpuTrials, CUDA_TRIALS, roundInfo->maxIndices);

                gpuTrials += CUDA_TRIALS;
            }
            while ((tFinish - now() * 1e-9) > 0);
        };

        std::thread runGPUThread(runGPU);

        auto tbbInitial = [ = ](unsigned i)
        {
            bigint_t ones;
            wide_ones(8, ones.limbs);
            wide_copy(8, &parallel_BestProofs[i * 8], ones.limbs);
        };

        tbb::parallel_for<unsigned>(0, TBB_PARALLEL_COUNT, tbbInitial);

        do
        {
            auto tbbIteration = [ = ](unsigned i)
            {
                uint32_t index = maxNum - (TBB_PARALLEL_COUNT<<2) - cpuTrials + (i<<1);

                bigint_t proof = tbbHash(roundInfo.get(),
                                         index,
                                         x);

                wide_copy(8, &parallel_Proofs[i * 8], proof.limbs);
                parallel_Indices[i] = index;
            };

            tbb::parallel_for<unsigned>(0, TBB_PARALLEL_COUNT, tbbIteration);

            auto tbbCrossHash = [ = ](unsigned i)
            {
                for (unsigned xorStride = 1; xorStride < TBB_PARALLEL_COUNT >> 2; xorStride++)
                {
//.........这里部分代码省略.........

void CameraControl::applyInteraction(glm::mat4 & camera) {
#ifdef HAVE_SIXENSE
  static bool hydraInitialized = false;
  if (!hydraInitialized) {
    int init = sixenseInit();
    sixenseSetActiveBase(0);
    sixenseUtils::getTheControllerManager()->setGameType(sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER);
    sixenseUtils::getTheControllerManager()->registerSetupCallback(controller_manager_setup_callback);
    hydraInitialized = true;
  }
  if (hydraEnabled) {
    sixenseSetActiveBase(0);
    static sixenseAllControllerData acd;
    sixenseGetAllNewestData(&acd);
    sixenseUtils::getTheControllerManager()->update(&acd);
    int i = sixenseUtils::getTheControllerManager()->getIndex(
        sixenseUtils::IControllerManager::P1L);
    const sixenseControllerData & left = acd.controllers[i];
    i = sixenseUtils::getTheControllerManager()->getIndex(
        sixenseUtils::IControllerManager::P1R);
    const sixenseControllerData & right = acd.controllers[i];
    translateCamera(camera,
        glm::vec3(left.joystick_x, right.joystick_y, -left.joystick_y)
            / 100.0f);
    rotateCamera(camera,
        glm::angleAxis(-right.joystick_x /100.0f, glm::vec3(0, 1, 0)));
  }
#endif

#ifdef HAVE_SPNAV
  static int spnav = -2;
  static spnav_event event;
  if (-2 == spnav) {
    spnav = spnav_open();
  }
  if (spnav >= 0) {
    int eventType;
    while (0 != (eventType = spnav_poll_event(&event))) {
      SAY("event type %d", eventType);
      if (SPNAV_EVENT_MOTION == eventType) {
        spnav_event_motion & m = event.motion;
        glm::vec3 spaceTranslation = getTranslation(m);
        translateCamera(camera, spaceTranslation);
//        camera = glm::rotate(camera, (float)m.rx / 200.0f, GlUtils::X_AXIS);

        // We take the world Y axis and put it into the camera reference frame
        glm::vec3 yawAxis = glm::inverse(glm::quat(camera)) * GlUtils::Y_AXIS;
        camera = glm::rotate(camera, (float)m.ry / 200.0f, yawAxis);

//        if (abs(m.ry) >= 3 || abs(m.rx) >= 3) {
//          if (m.rx > m.ry) {
//          } else {
//            camera = glm::rotate(camera, (float)m.ry / 200.0f, GlUtils::Y_AXIS);
//          }
//
//        }
//        camera = glm::rotate(camera, (float)m.rx / 500.0f, GlUtils::X_AXIS);
//        glm::quat currentRotation(camera);
//        glm::quat inverse = glm::inverse(currentRotation);
//        camera = glm::mat4_cast(inverse) * camera;
//        rot = glm::quat(euler);
//        camera = glm::mat4_cast(rot) * camera;
//        camera = glm::mat4_cast(spaceRotation * currentRotation) * camera;
//        rotateCamera(camera, glm::quat(rotation));
      }
    }
  }
//  int spnav_sensitivity(double sens);

#endif

  if (glfwJoystickPresent(0)) {
    static const char * joyName = glfwGetJoystickName(0);
    static bool x52present =
        std::string(joyName).find("X52") !=
            std::string::npos;

    static std::shared_ptr<GlfwJoystick> joystick(
        x52present ?
            (GlfwJoystick*)new SaitekX52Pro::Controller(0) :
            (GlfwJoystick*)new Xbox::Controller(0)
        );
    joystick->read();
    glm::vec3 translation;
    glm::quat rotation;
    float scale = 500.0f;

    if (x52present) {
      using namespace SaitekX52Pro::Axis;
      // 0 - 9
      float scaleMod = joystick->getCalibratedAxisValue(SaitekX52Pro::Axis::THROTTLE_SLIDER);
      scaleMod *= 0.5f;
      scaleMod += 0.5f;
      scaleMod *= 9.0f;
      scaleMod += 1.0f;
      scale /= scaleMod;

      translation = joystick->getCalibratedVector(
          STICK_POV_X,
          STICK_POV_Y,
//.........这里部分代码省略.........

void print_myobject3_2(std::shared_ptr<MyObject3> obj) { py::print(obj->toString()); }

void print_myobject3_3(const std::shared_ptr<MyObject3> &obj) { py::print(obj->toString()); }

void Enemy::render(std::shared_ptr<sf::RenderTarget> screen) {
	screen->draw(sprite);
}

bool OsmAnd::MapPrimitivesMetricsLayerProvider_P::obtainData(
    const IMapDataProvider::Request& request_,
    std::shared_ptr<IMapDataProvider::Data>& outData,
    std::shared_ptr<Metric>* const pOutMetric)
{
    const auto& request = MapDataProviderHelpers::castRequest<MapPrimitivesMetricsLayerProvider::Request>(request_);
    if (pOutMetric)
        pOutMetric->reset();

    MapPrimitivesProvider_Metrics::Metric_obtainData obtainDataMetric;

    // Obtain offline map primitives tile
    std::shared_ptr<MapPrimitivesProvider::Data> primitivesTile;
    owner->primitivesProvider->obtainTiledPrimitives(request, primitivesTile, &obtainDataMetric);
    if (!primitivesTile)
    {
        outData.reset();
        return true;
    }

    // Prepare drawing canvas
    const std::shared_ptr<SkBitmap> bitmap(new SkBitmap());
    if (!bitmap->tryAllocPixels(SkImageInfo::MakeN32Premul(owner->tileSize, owner->tileSize)))
    {
        LogPrintf(LogSeverityLevel::Error,
            "Failed to allocate buffer for rasterization surface %dx%d",
            owner->tileSize,
            owner->tileSize);
        return false;
    }
    SkBitmapDevice target(*bitmap);
    SkCanvas canvas(&target);
    canvas.clear(SK_ColorDKGRAY);

    QString text;
    text += QString(QLatin1String("TILE   %1x%[email protected]%3\n"))
        .arg(request.tileId.x)
        .arg(request.tileId.y)
        .arg(request.zoom);
    QString obtainBinaryMapObjectsElapsedTime(QLatin1String("?"));
    if (const auto obtainBinaryMapObjectsMetric = obtainDataMetric.findSubmetricOfType<ObfMapObjectsProvider_Metrics::Metric_obtainData>(true))
    {
        obtainBinaryMapObjectsElapsedTime = QString::number(obtainBinaryMapObjectsMetric->elapsedTime, 'f', 2);
    }
    QString primitiviseElapsedTime(QLatin1String("?"));
    if (const auto primitiviseMetric = obtainDataMetric.findSubmetricOfType<MapPrimitiviser_Metrics::Metric_primitiviseWithSurface>(true))
    {
        text += QString(QLatin1String("order %1/-%2 %3s ~%4us/e\n"))
            .arg(primitiviseMetric->orderEvaluations)
            .arg(primitiviseMetric->orderRejects)
            .arg(QString::number(primitiviseMetric->elapsedTimeForOrderEvaluation, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForOrderEvaluation * 1000000.0f / primitiviseMetric->orderEvaluations));
        text += QString(QLatin1String("polyg %1/-%2(-%3) %4s ~%5us/e\n"))
            .arg(primitiviseMetric->polygonEvaluations)
            .arg(primitiviseMetric->polygonRejects)
            .arg(primitiviseMetric->polygonsRejectedByArea)
            .arg(QString::number(primitiviseMetric->elapsedTimeForPolygonEvaluation, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPolygonEvaluation * 1000000.0f / primitiviseMetric->polygonEvaluations));
        text += QString(QLatin1String("%1s ~%2us/p\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForPolygonProcessing, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPolygonProcessing * 1000000.0f / primitiviseMetric->polygonPrimitives));
        text += QString(QLatin1String("polyl %1/-%2(-%3) %4s ~%5us/e\n"))
            .arg(primitiviseMetric->polylineEvaluations)
            .arg(primitiviseMetric->polylineRejects)
            .arg(primitiviseMetric->polylineRejectedByDensity)
            .arg(QString::number(primitiviseMetric->elapsedTimeForPolylineEvaluation, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPolylineEvaluation * 1000000.0f / primitiviseMetric->polylineEvaluations));
        text += QString(QLatin1String("%1s ~%2us/p\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForPolylineProcessing, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPolylineProcessing * 1000000.0f / primitiviseMetric->polylinePrimitives));
        text += QString(QLatin1String("point %1/-%2 %3s ~%4us/e\n"))
            .arg(primitiviseMetric->pointEvaluations)
            .arg(primitiviseMetric->pointRejects)
            .arg(QString::number(primitiviseMetric->elapsedTimeForPointEvaluation, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPointEvaluation * 1000000.0f / primitiviseMetric->pointEvaluations));
        text += QString(QLatin1String("%1s ~%2us/p\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForPointProcessing, 'f', 2))
            .arg(static_cast<int>(primitiviseMetric->elapsedTimeForPointProcessing * 1000000.0f / primitiviseMetric->pointPrimitives));
        const auto deltaGroups =
            primitiviseMetric->elapsedTimeForObtainingPrimitivesGroups -
            primitiviseMetric->elapsedTimeForOrderEvaluation -
            primitiviseMetric->elapsedTimeForOrderProcessing -
            primitiviseMetric->elapsedTimeForPolygonEvaluation -
            primitiviseMetric->elapsedTimeForPolygonProcessing -
            primitiviseMetric->elapsedTimeForPolylineEvaluation -
            primitiviseMetric->elapsedTimeForPolylineProcessing -
            primitiviseMetric->elapsedTimeForPointEvaluation -
            primitiviseMetric->elapsedTimeForPointProcessing;
        text += QString(QLatin1String("grp %1s (-^=%2s)\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForObtainingPrimitivesGroups, 'f', 2))
            .arg(QString::number(deltaGroups, 'f', 2));
        text += QString(QLatin1String("prim %1s+s%2s+?=%3s\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForObtainingPrimitivesGroups, 'f', 2))
            .arg(QString::number(primitiviseMetric->elapsedTimeForFutureSharedPrimitivesGroups, 'f', 2))
            .arg(QString::number(primitiviseMetric->elapsedTimeForPrimitives, 'f', 2));
        text += QString(QLatin1String("d/b/c %1s/%2s/%3s\n"))
            .arg(QString::number(primitiviseMetric->elapsedTimeForObtainingPrimitivesFromDetailedmap, 'f', 2))
            .arg(QString::number(primitiviseMetric->elapsedTimeForObtainingPrimitivesFromBasemap, 'f', 2))
            .arg(QString::number(primitiviseMetric->elapsedTimeForObtainingPrimitivesFromCoastlines, 'f', 2));
        text += QString(QLatin1String("txt %1(-%2) %3s ~%4us/e ~%5us/p\n"))
//.........这里部分代码省略.........

// Publish Various Representations of the Depth Image:
void Pass::sendSweepDepthImage(){
  bool write_raw = false;
  bool publish_raw = true;
  bool publish_range_image = false;
  
  // a. Write raw depths to file
  if (write_raw){
    cout << "Writing Raw Range Image Points to /tmp\n";
    std::ofstream ofs("/tmp/sweep_depths.txt");
    for (int i = 0; i < camera_params_.height; ++i) {
      for (int j = 0; j < camera_params_.width; ++j) {
        ofs << depth_buf_[i*camera_params_.width + j] << " ";
      }
      ofs << std::endl;
    }
    ofs.close();
  }


  // b. Reproject the depth image into xyz, colourize, apply a mask and publish
  if (publish_raw){
    cout << "Publishing Raw Range Image Points\n";
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4 (new pcl::PointCloud<pcl::PointXYZRGB> ());
    int decimate=4;
    uint8_t* mask_buf = NULL;
    if (mask_init_){
      mask_buf = imgutils_->unzipImage( &last_mask_ );
    }

    for (int v = 0; v < camera_params_.height ; v=v+decimate) { // rows t2b //height
      for (int u = 0; u < camera_params_.width; u=u+decimate) { // cols l2r
        pcl::PointXYZRGB pt;
        int pixel = v*camera_params_.width +u;
        pt.z = 1/ depth_buf_[pixel]; /// inversion currently required due to Matt's interperation of depth as 1/depth ;)
        pt.x = ( pt.z * (u  - camera_params_.cx ))/  camera_params_.fx ;
        pt.y = ( pt.z * (v  - camera_params_.cy ))/  camera_params_.fy ;
        if (img_.pixelformat == bot_core::image_t::PIXEL_FORMAT_RGB){
          pt.r = (float) img_.data[pixel*3];
          pt.g = (float) img_.data[pixel*3+1];
          pt.b = (float) img_.data[pixel*3+2];
        }else if (img_.pixelformat == bot_core::image_t::PIXEL_FORMAT_GRAY){
          pt.r = (float) img_.data[pixel];
          pt.g = (float) img_.data[pixel];
          pt.b = (float) img_.data[pixel];
        }

        if (mask_init_){ // if we have a mask color the points by it
          if (mask_buf[pixel] > 0){ // if the mask is not 0 (black), apply it as red
            pt.r = 255;
            pt.g = 0;
            pt.b = 0;
          }
        }

        cloud4->points.push_back(pt);
      }
    }
    cloud4->width = cloud4->points.size();
    cloud4->height =1;
    Isometry3dTime camera_pose_T = Isometry3dTime(current_utime_, camera_pose_);
    pc_vis_->pose_to_lcm_from_list(91004, camera_pose_T);  
    pc_vis_->ptcld_to_lcm_from_list(91005, *cloud4, current_utime_, current_utime_);  
    // pc_vis_->pointcloud2_to_lcm(*cloud4,"RANGE_IMAGE_POINTS",current_utime_);
  }
  

  if (publish_range_image){
    std::cout << "Publishing Range image to LIDARSWEEP\n";
    // c. publish in Depth Image mode:
    int n_bytes=2; // 2 bytes per value // different from before in driver
    int isize = n_bytes*camera_params_.width * camera_params_.height;
    disparity_.utime =img_.utime;
    disparity_.width = camera_params_.width;
    disparity_.height = camera_params_.height;
    disparity_.pixelformat =bot_core::image_t::PIXEL_FORMAT_GRAY; //PIXEL_FORMAT_GRAY;
    disparity_.nmetadata =0;
    disparity_.row_stride=n_bytes* camera_params_.width ;
    disparity_.size =isize;
    disparity_.data.resize(isize);
    for (size_t i=0; i < camera_params_.width * camera_params_.height; i++){
      // convert to MM - the same as kinect mm openni format
      disparity_data_[i] = (uint16_t) 1000* 1/(depth_buf_[i]); // need 1/depth for now until Matt fixes this
    }
    memcpy(&disparity_.data[0], disparity_data_, isize);
    
    bot_core::images_t images;
    images.utime = img_.utime;
    images.n_images =2;
    images.image_types.push_back( 0 ); // multisense::images_t::LEFT ); for some reason enums won't work
    images.image_types.push_back( 4 ); // multisense::images_t::DEPTH_MM );
    images.images.push_back( img_ );
    images.images.push_back(disparity_);
    lcm_->publish("LIDARSWEEP", &images); 
  }
}

static void ddRenderOBBs(const mat4& projview)
{
	const ShaderInfo* shader = g_dbgdrawShader.get();
	GLint posLoc = shader->m_attrs[GEOM_Pos];
	GLint colorLoc = shader->m_attrs[GEOM_Color];
	GLint mvpLoc = shader->m_uniforms[BIND_Mvp];
	glUniformMatrix4fv(mvpLoc, 1, 0, projview.m);

	static const float s_coords[][3] = {
		{ -1.f, -1.f, -1.f },
		{ 1.f, -1.f, -1.f },
		{ 1.f, 1.f, -1.f },
		{ -1.f, 1.f, -1.f },
		{ -1.f, -1.f, 1.f },
		{ 1.f, -1.f, 1.f },
		{ 1.f, 1.f, 1.f },
		{ -1.f, 1.f, 1.f },
	};

	const ddOBB* cur = g_lists.m_obbs;
	if(cur) 
	{	
		glBegin(GL_LINES);
		while(cur)
		{
			OBB obb = OBBTransform(cur->m_xfm, cur->m_obb);

			glVertexAttrib3fv(colorLoc, &cur->m_color.r);
			vec3 pt[8];
			for(int i = 0; i < 8; ++i)
			{
				pt[i] = obb.m_center + 
					obb.m_b[0] * s_coords[i][0] +
					obb.m_b[1] * s_coords[i][1] +
					obb.m_b[2] * s_coords[i][2];
			}

			// Bottom half
			glVertexAttrib3fv(posLoc, &pt[0].x);
			glVertexAttrib3fv(posLoc, &pt[1].x);

			glVertexAttrib3fv(posLoc, &pt[1].x);
			glVertexAttrib3fv(posLoc, &pt[2].x);

			glVertexAttrib3fv(posLoc, &pt[2].x);
			glVertexAttrib3fv(posLoc, &pt[3].x);

			glVertexAttrib3fv(posLoc, &pt[3].x);
			glVertexAttrib3fv(posLoc, &pt[0].x);

			// Top half
			glVertexAttrib3fv(posLoc, &pt[4].x);
			glVertexAttrib3fv(posLoc, &pt[5].x);

			glVertexAttrib3fv(posLoc, &pt[5].x);
			glVertexAttrib3fv(posLoc, &pt[6].x);

			glVertexAttrib3fv(posLoc, &pt[6].x);
			glVertexAttrib3fv(posLoc, &pt[7].x);

			glVertexAttrib3fv(posLoc, &pt[7].x);
			glVertexAttrib3fv(posLoc, &pt[4].x);

			// Connecting lines
			glVertexAttrib3fv(posLoc, &pt[0].x);
			glVertexAttrib3fv(posLoc, &pt[4].x);

			glVertexAttrib3fv(posLoc, &pt[1].x);
			glVertexAttrib3fv(posLoc, &pt[5].x);

			glVertexAttrib3fv(posLoc, &pt[2].x);
			glVertexAttrib3fv(posLoc, &pt[6].x);

			glVertexAttrib3fv(posLoc, &pt[3].x);
			glVertexAttrib3fv(posLoc, &pt[7].x);

			cur = cur->m_next;
		}
		glEnd();
	}
}