<commit_before>//  (C) Copyright Gennadiy Rozental 2005.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : plain report formatter definition
// ***************************************************************************

#ifndef BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER
#define BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER

// Boost.Test
#include <boost/test/output/plain_report_formatter.hpp>
#include <boost/test/utils/custom_manip.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test_suite.hpp>

#include <boost/test/utils/basic_cstring/io.hpp>

// STL
#include <iomanip>
#include <cmath>
#include <iostream>

#include <boost/test/detail/suppress_warnings.hpp>

# ifdef BOOST_NO_STDC_NAMESPACE
namespace std { using ::log10; }
# endif

//____________________________________________________________________________//

namespace boost {

namespace unit_test {

namespace output {

namespace {

typedef custom_manip<struct quote_t> quote;

template<typename T>
inline std::ostream&
operator<<( custom_printer<quote> const& p, T const& value )
{
    *p << '"' << value << '"';

    return *p;
}

//____________________________________________________________________________//

void
print_stat_value( std::ostream& ostr, counter_t v, counter_t indent, counter_t total,
                  const_string name, const_string res )
{
    if( v > 0 ) {
        ostr << std::setw( indent ) << ""
             << v << ' ' << name << ( v != 1 ? "s" : "" );
        if( total > 0 )
            ostr << " out of " << total;

        ostr << ' ' << res << '\n';
    }
}

//____________________________________________________________________________//

} // local namespace

// ************************************************************************** //
// **************             plain_report_formatter           ************** //
// ************************************************************************** //

void
plain_report_formatter::results_report_start( std::ostream& ostr )
{
    m_indent = 0;
    ostr << '\n';
}

//____________________________________________________________________________//

void
plain_report_formatter::results_report_finish( std::ostream& ostr )
{
    ostr.flush();
}

//____________________________________________________________________________//

void
plain_report_formatter::test_unit_report_start( test_unit const& tu, std::ostream& ostr )
{
    test_results const& tr = results_collector.results( tu.p_id );

    const_string descr;

    if( tr.passed() )
        descr = "passed";
    else if( tr.p_skipped )
        descr = "skipped";
    else if( tr.p_aborted )
        descr = "aborted";
    else
        descr = "failed";

    ostr << std::setw( m_indent ) << ""
         << "Test " << (tu.p_type == tut_case ? "case " : "suite " ) << quote() << tu.p_name << ' ' << descr;

    if( tr.p_skipped ) {
        ostr << '\n';
        m_indent += 2;
        return;
    }

    counter_t total_assertions  = tr.p_assertions_passed + tr.p_assertions_failed;
    counter_t total_tc          = tr.p_test_cases_passed + tr.p_test_cases_failed + tr.p_test_cases_skipped;

    if( total_assertions > 0 || total_tc > 0 )
        ostr << " with:";

    ostr << '\n';
    m_indent += 2;

    print_stat_value( ostr, tr.p_assertions_passed, m_indent, total_assertions, "assertion", "passed" );
    print_stat_value( ostr, tr.p_assertions_failed, m_indent, total_assertions, "assertion", "failed" );
    print_stat_value( ostr, tr.p_expected_failures, m_indent, 0               , "failure"  , "expected" );
    print_stat_value( ostr, tr.p_test_cases_passed, m_indent, total_tc        , "test case", "passed" );
    print_stat_value( ostr, tr.p_test_cases_failed, m_indent, total_tc        , "test case", "failed" );
    print_stat_value( ostr, tr.p_test_cases_skipped, m_indent, total_tc       , "test case", "skipped" );

    ostr << '\n';
}

//____________________________________________________________________________//

void
plain_report_formatter::test_unit_report_finish( test_unit const&, std::ostream& )
{
    m_indent -= 2;
}

//____________________________________________________________________________//

void
plain_report_formatter::do_confirmation_report( test_unit const& tu, std::ostream& ostr )
{
    test_results const& tr = results_collector.results( tu.p_id );

    if( tr.passed() ) {
        ostr << "*** No errors detected\n";
        return;
    }

    if( tr.p_skipped ) {
        ostr << "*** Test " << tu.p_type_name << " skipped \n";
        return;
    }

    if( tr.p_assertions_failed == 0 ) {
        ostr << "*** errors detected in test " << tu.p_type_name << " " << quote() << tu.p_name
             << "; see standard output for details\n";
        return;
    }

    ostr << "*** " << tr.p_assertions_failed << " failure" << ( tr.p_assertions_failed != 1 ? "s" : "" ) << " detected";

    if( tr.p_expected_failures > 0 )
        ostr << " (" << tr.p_expected_failures << " failure" << ( tr.p_expected_failures != 1 ? "s" : "" ) << " expected)";

    ostr << " in test " << tu.p_type_name << " " << quote() << tu.p_name << "\n";
}

//____________________________________________________________________________//

} // namespace output

} // namespace unit_test

} // namespace boost

//____________________________________________________________________________//

#include <boost/test/detail/enable_warnings.hpp>

// ***************************************************************************
//  Revision History :
//
//  $Log$
//  Revision 1.2  2005/12/08 03:19:01  dgregor
//  Merged from Version_1_33_1
//
//  Revision 1.1.2.2  2005/11/28 11:57:07  dgregor
//  Make that.... iostream
//
//  Revision 1.1.2.1  2005/11/28 00:02:36  dgregor
//  Include ostream
//
//  Revision 1.1  2005/02/20 08:27:07  rogeeff
//  This a major update for Boost.Test framework. See release docs for complete list of fixes/updates
//
// ***************************************************************************

#endif // BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER
<commit_msg>report all aborted test units<commit_after>//  (C) Copyright Gennadiy Rozental 2005.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : plain report formatter definition
// ***************************************************************************

#ifndef BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER
#define BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER

// Boost.Test
#include <boost/test/output/plain_report_formatter.hpp>
#include <boost/test/utils/custom_manip.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test_suite_impl.hpp>

#include <boost/test/utils/basic_cstring/io.hpp>

// STL
#include <iomanip>
#include <cmath>
#include <iostream>

#include <boost/test/detail/suppress_warnings.hpp>

# ifdef BOOST_NO_STDC_NAMESPACE
namespace std { using ::log10; }
# endif

//____________________________________________________________________________//

namespace boost {

namespace unit_test {

namespace output {

namespace {

typedef custom_manip<struct quote_t> quote;

template<typename T>
inline std::ostream&
operator<<( custom_printer<quote> const& p, T const& value )
{
    *p << '"' << value << '"';

    return *p;
}

//____________________________________________________________________________//

void
print_stat_value( std::ostream& ostr, counter_t v, counter_t indent, counter_t total,
                  const_string name, const_string res )
{
    if( v > 0 ) {
        ostr << std::setw( indent ) << ""
             << v << ' ' << name << ( v != 1 ? "s" : "" );
        if( total > 0 )
            ostr << " out of " << total;

        ostr << ' ' << res << '\n';
    }
}

//____________________________________________________________________________//

} // local namespace

// ************************************************************************** //
// **************             plain_report_formatter           ************** //
// ************************************************************************** //

void
plain_report_formatter::results_report_start( std::ostream& ostr )
{
    m_indent = 0;
    ostr << '\n';
}

//____________________________________________________________________________//

void
plain_report_formatter::results_report_finish( std::ostream& ostr )
{
    ostr.flush();
}

//____________________________________________________________________________//

void
plain_report_formatter::test_unit_report_start( test_unit const& tu, std::ostream& ostr )
{
    test_results const& tr = results_collector.results( tu.p_id );

    const_string descr;

    if( tr.passed() )
        descr = "passed";
    else if( tr.p_skipped )
        descr = "skipped";
    else if( tr.p_aborted )
        descr = "aborted";
    else
        descr = "failed";

    ostr << std::setw( m_indent ) << ""
         << "Test " << (tu.p_type == tut_case ? "case " : "suite " ) << quote() << tu.p_name << ' ' << descr;

    if( tr.p_skipped ) {
        ostr << " due to " << (tu.check_dependencies() ? "test aborting\n" : "failed dependancy\n" );
        m_indent += 2;
        return;
    }

    counter_t total_assertions  = tr.p_assertions_passed + tr.p_assertions_failed;
    counter_t total_tc          = tr.p_test_cases_passed + tr.p_test_cases_failed + tr.p_test_cases_skipped;

    if( total_assertions > 0 || total_tc > 0 )
        ostr << " with:";

    ostr << '\n';
    m_indent += 2;

    print_stat_value( ostr, tr.p_assertions_passed, m_indent, total_assertions, "assertion", "passed" );
    print_stat_value( ostr, tr.p_assertions_failed, m_indent, total_assertions, "assertion", "failed" );
    print_stat_value( ostr, tr.p_expected_failures, m_indent, 0               , "failure"  , "expected" );
    print_stat_value( ostr, tr.p_test_cases_passed, m_indent, total_tc        , "test case", "passed" );
    print_stat_value( ostr, tr.p_test_cases_failed, m_indent, total_tc        , "test case", "failed" );
    print_stat_value( ostr, tr.p_test_cases_skipped, m_indent, total_tc       , "test case", "skipped" );
    print_stat_value( ostr, tr.p_test_cases_aborted, m_indent, total_tc       , "test case", "aborted" );

    ostr << '\n';
}

//____________________________________________________________________________//

void
plain_report_formatter::test_unit_report_finish( test_unit const&, std::ostream& )
{
    m_indent -= 2;
}

//____________________________________________________________________________//

void
plain_report_formatter::do_confirmation_report( test_unit const& tu, std::ostream& ostr )
{
    test_results const& tr = results_collector.results( tu.p_id );

    if( tr.passed() ) {
        ostr << "*** No errors detected\n";
        return;
    }

    if( tr.p_skipped ) {
        ostr << "*** Test " << tu.p_type_name << " skipped due to "
             << (tu.check_dependencies() ? "test aborting\n" : "failed dependancy\n" );
        return;
    }

    if( tr.p_assertions_failed == 0 ) {
        ostr << "*** errors detected in test " << tu.p_type_name << " " << quote() << tu.p_name
             << "; see standard output for details\n";
        return;
    }

    counter_t num_failures = tr.p_assertions_failed + tr.p_test_cases_aborted;

    if( !tr.p_test_cases_aborted && tr.p_aborted )
        ++num_failures;

    ostr << "*** " << num_failures << " failure" << ( num_failures != 1 ? "s" : "" ) << " detected";

    if( tr.p_expected_failures > 0 )
        ostr << " (" << tr.p_expected_failures << " failure" << ( tr.p_expected_failures != 1 ? "s" : "" ) << " expected)";

    ostr << " in test " << tu.p_type_name << " " << quote() << tu.p_name << "\n";
}

//____________________________________________________________________________//

} // namespace output

} // namespace unit_test

} // namespace boost

//____________________________________________________________________________//

#include <boost/test/detail/enable_warnings.hpp>

// ***************************************************************************
//  Revision History :
//
//  $Log$
//  Revision 1.3  2005/12/14 05:31:06  rogeeff
//  report all aborted test units
//
//  Revision 1.2  2005/12/08 03:19:01  dgregor
//  Merged from Version_1_33_1
//
//  Revision 1.1.2.2  2005/11/28 11:57:07  dgregor
//  Make that.... iostream
//
//  Revision 1.1.2.1  2005/11/28 00:02:36  dgregor
//  Include ostream
//
//  Revision 1.1  2005/02/20 08:27:07  rogeeff
//  This a major update for Boost.Test framework. See release docs for complete list of fixes/updates
//
// ***************************************************************************

#endif // BOOST_TEST_PLAIN_REPORT_FORMATTER_IPP_020105GER
<|endoftext|>


<commit_before>#include "drake/systems/LCMSystem.h"
#include "drake/systems/LinearSystem.h"
#include "drake/systems/pd_control_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/util/drakeAppUtil.h"
#include "lcmtypes/drake/lcmt_driving_control_cmd_t.hpp"

using namespace std;
using namespace Eigen;
using namespace Drake;

template <typename ScalarType = double>
class DrivingCommand {
 public:
  typedef drake::lcmt_driving_control_cmd_t LCMMessageType;
  static std::string channel() { return "DRIVING_COMMAND"; };

  DrivingCommand(void) : throttle(0), brake(0), steering_angle(0){};
  template <typename Derived>
  DrivingCommand(const Eigen::MatrixBase<Derived>& x)
      : steering_angle(x(0)), throttle(x(1)), brake(x(2)){};

  template <typename Derived>
  DrivingCommand& operator=(const Eigen::MatrixBase<Derived>& x) {
    steering_angle = x(0);
    throttle = x(1);
    brake = x(2);
    return *this;
  }

  friend Eigen::Vector3d toEigen(const DrivingCommand<ScalarType>& vec) {
    Eigen::Vector3d x;
    x << vec.steering_angle, vec.throttle, vec.brake;
    return x;
  }

  friend std::string getCoordinateName(const DrivingCommand<ScalarType>& vec,
                                       unsigned int index) {
    switch (index) {
      case 0:
        return "steering_angle";
      case 1:
        return "throttle";
      case 2:
        return "brake";
    }
    return "error";
  }
  const static int RowsAtCompileTime = 3;

  ScalarType steering_angle;
  ScalarType throttle;
  ScalarType brake;
};

bool decode(const drake::lcmt_driving_control_cmd_t& msg, double& t,
            DrivingCommand<double>& x) {
  t = double(msg.timestamp) / 1000.0;
  x.steering_angle = msg.steering_angle;
  x.throttle = msg.throttle_value;
  x.brake = msg.brake_value;
  return true;
}

/** Driving Simulator
 * Usage:  simulateLCM vehicle_urdf [world_urdf files ...]
 */

int main(int argc, char* argv[]) {
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " vehicle_urdf [world sdf files ...]"
              << std::endl;
    return 1;
  }

  // todo: consider moving this logic into the RigidBodySystem class so it can
  // be reused
  DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;

  auto rigid_body_sys = make_shared<RigidBodySystem>();
  rigid_body_sys->addRobotFromFile(argv[1], floating_base_type);
  auto const & tree = rigid_body_sys->getRigidBodyTree();
  for (int i=2; i<argc; i++)
    tree->addRobotFromSDF(argv[i], DrakeJoint::FIXED);  // add environment

  if (argc < 3) {  // add flat terrain
    double box_width = 1000;
    double box_depth = 10;
    DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
    Isometry3d T_element_to_link = Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2;  // top of the box is at z=0
    auto& world = tree->bodies[0];
    Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world->addVisualElement(
        DrakeShapes::VisualElement(geom, T_element_to_link, color));
    tree->addCollisionElement(
        RigidBody::CollisionElement(geom, T_element_to_link, world), *world,
        "terrain");
    tree->updateStaticCollisionElements();
  }

  shared_ptr<lcm::LCM> lcm = make_shared<lcm::LCM>();

  MatrixXd Kp(getNumInputs(*rigid_body_sys), tree->num_positions),
      Kd(getNumInputs(*rigid_body_sys), tree->num_velocities);
  Matrix<double, Eigen::Dynamic, 3> map_driving_cmd_to_x_d(
      tree->num_positions + tree->num_velocities, 3);
  {  // setup PD controller for throttle and steering
    double kpSteering = 400, kdSteering = 80, kThrottle = 100;
    Kp.setZero();
    Kd.setZero();
    map_driving_cmd_to_x_d.setZero();

    for (int actuator_idx = 0; actuator_idx < tree->actuators.size();
         actuator_idx++) {
      if (strcmp(tree->actuators[actuator_idx].name.c_str(),
                 "steering_angle") == 0) {
        auto const& b = tree->actuators[actuator_idx].body;
        Kp(actuator_idx, b->position_num_start) = kpSteering;  // steering
        Kd(actuator_idx, b->velocity_num_start) = kdSteering;  // steeringdot
        map_driving_cmd_to_x_d(b->position_num_start, 0) =
            1;  // steering command
      } else if (strncmp(tree->actuators[actuator_idx].name.c_str(), "throttle",
                         8) == 0) {  // intentionally match all throttle_ inputs
        auto const& b = tree->actuators[actuator_idx].body;
        Kd(actuator_idx, b->velocity_num_start) = kThrottle;  // throttle
        map_driving_cmd_to_x_d(tree->num_positions + b->velocity_num_start, 1) =
            20;  // throttle (velocity) command
        map_driving_cmd_to_x_d(tree->num_positions + b->velocity_num_start, 2) =
            -20;  // braking (velocity) command
      }
    }
  }
  auto vehicle_with_pd =
      make_shared<PDControlSystem<RigidBodySystem>>(rigid_body_sys, Kp, Kd);
  auto vehicle_sys = cascade(
      make_shared<
          Gain<DrivingCommand, PDControlSystem<RigidBodySystem>::InputVector>>(
          map_driving_cmd_to_x_d),
      vehicle_with_pd);
  auto visualizer =
      make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm, tree);
  auto sys = cascade(vehicle_sys, visualizer);

  SimulationOptions options = default_simulation_options;
  rigid_body_sys->penetration_stiffness = 5000.0;
  rigid_body_sys->penetration_damping =
      rigid_body_sys->penetration_stiffness / 10.0;
  rigid_body_sys->friction_coefficient = 10.0;  // essentially infinite friction
  options.initial_step_size = 5e-3;
  options.timeout_seconds = numeric_limits<double>::infinity();

  VectorXd x0 = VectorXd::Zero(rigid_body_sys->getNumStates());
  x0.head(tree->num_positions) = tree->getZeroConfiguration();
  // todo:  call getInitialState instead?  (but currently, that would require
  // snopt).  needs #1627
  // I'm getting away without it, but might be generating large internal forces
  // initially as the ackerman constraint (hopefully) gets enforced by the
  // stabilization terms.

  runLCM(sys, lcm, 0, std::numeric_limits<double>::infinity(), x0, options);
  //  simulate(*sys, 0, std::numeric_limits<double>::infinity(), x0, options);

  return 0;
}
<commit_msg>Added toString to DrivingCommand and print statement each time a driving command is received.<commit_after>#include "drake/systems/LCMSystem.h"
#include "drake/systems/LinearSystem.h"
#include "drake/systems/pd_control_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/util/drakeAppUtil.h"
#include "lcmtypes/drake/lcmt_driving_control_cmd_t.hpp"

using namespace std;
using namespace Eigen;
using namespace Drake;

template <typename ScalarType = double>
class DrivingCommand {
 public:
  typedef drake::lcmt_driving_control_cmd_t LCMMessageType;
  static std::string channel() { return "DRIVING_COMMAND"; };

  DrivingCommand(void) : throttle(0), brake(0), steering_angle(0){};
  template <typename Derived>
  DrivingCommand(const Eigen::MatrixBase<Derived>& x)
      : steering_angle(x(0)), throttle(x(1)), brake(x(2)){};

  template <typename Derived>
  DrivingCommand& operator=(const Eigen::MatrixBase<Derived>& x) {
    steering_angle = x(0);
    throttle = x(1);
    brake = x(2);
    return *this;
  }

  friend Eigen::Vector3d toEigen(const DrivingCommand<ScalarType>& vec) {
    Eigen::Vector3d x;
    x << vec.steering_angle, vec.throttle, vec.brake;
    return x;
  }

  friend std::string getCoordinateName(const DrivingCommand<ScalarType>& vec,
                                       unsigned int index) {
    switch (index) {
      case 0:
        return "steering_angle";
      case 1:
        return "throttle";
      case 2:
        return "brake";
    }
    return "error";
  }
  const static int RowsAtCompileTime = 3;

  ScalarType steering_angle;
  ScalarType throttle;
  ScalarType brake;
};

template <typename ScalarType = double>
std::ostream& operator<<(std::ostream &os, const DrivingCommand<ScalarType> &dc) {
  return os << "[steering_angle = " << dc.steering_angle << ", throttle = " << dc.throttle << ", brake = " << dc.brake << "]";
}

bool decode(const drake::lcmt_driving_control_cmd_t& msg, double& t,
            DrivingCommand<double>& x) {
  t = double(msg.timestamp) / 1000.0;
  x.steering_angle = msg.steering_angle;
  x.throttle = msg.throttle_value;
  x.brake = msg.brake_value;
  std::cout << "simualteLCM.cpp: decode(): Decoded driving command: " << x << std::endl;
  return true;
}

/** Driving Simulator
 * Usage:  simulateLCM vehicle_urdf [world_urdf files ...]
 */

int main(int argc, char* argv[]) {
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " vehicle_urdf [world sdf files ...]"
              << std::endl;
    return 1;
  }

  // todo: consider moving this logic into the RigidBodySystem class so it can
  // be reused
  DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;

  auto rigid_body_sys = make_shared<RigidBodySystem>();
  rigid_body_sys->addRobotFromFile(argv[1], floating_base_type);
  auto const & tree = rigid_body_sys->getRigidBodyTree();
  for (int i=2; i<argc; i++)
    tree->addRobotFromSDF(argv[i], DrakeJoint::FIXED);  // add environment

  if (argc < 3) {  // add flat terrain
    double box_width = 1000;
    double box_depth = 10;
    DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
    Isometry3d T_element_to_link = Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2;  // top of the box is at z=0
    auto& world = tree->bodies[0];
    Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world->addVisualElement(
        DrakeShapes::VisualElement(geom, T_element_to_link, color));
    tree->addCollisionElement(
        RigidBody::CollisionElement(geom, T_element_to_link, world), *world,
        "terrain");
    tree->updateStaticCollisionElements();
  }

  shared_ptr<lcm::LCM> lcm = make_shared<lcm::LCM>();

  MatrixXd Kp(getNumInputs(*rigid_body_sys), tree->num_positions),
      Kd(getNumInputs(*rigid_body_sys), tree->num_velocities);
  Matrix<double, Eigen::Dynamic, 3> map_driving_cmd_to_x_d(
      tree->num_positions + tree->num_velocities, 3);
  {  // setup PD controller for throttle and steering
    double kpSteering = 400, kdSteering = 80, kThrottle = 100;
    Kp.setZero();
    Kd.setZero();
    map_driving_cmd_to_x_d.setZero();

    for (int actuator_idx = 0; actuator_idx < tree->actuators.size();
         actuator_idx++) {
      if (strcmp(tree->actuators[actuator_idx].name.c_str(),
                 "steering_angle") == 0) {
        auto const& b = tree->actuators[actuator_idx].body;
        Kp(actuator_idx, b->position_num_start) = kpSteering;  // steering
        Kd(actuator_idx, b->velocity_num_start) = kdSteering;  // steeringdot
        map_driving_cmd_to_x_d(b->position_num_start, 0) =
            1;  // steering command
      } else if (strncmp(tree->actuators[actuator_idx].name.c_str(), "throttle",
                         8) == 0) {  // intentionally match all throttle_ inputs
        auto const& b = tree->actuators[actuator_idx].body;
        Kd(actuator_idx, b->velocity_num_start) = kThrottle;  // throttle
        map_driving_cmd_to_x_d(tree->num_positions + b->velocity_num_start, 1) =
            20;  // throttle (velocity) command
        map_driving_cmd_to_x_d(tree->num_positions + b->velocity_num_start, 2) =
            -20;  // braking (velocity) command
      }
    }
  }
  auto vehicle_with_pd =
      make_shared<PDControlSystem<RigidBodySystem>>(rigid_body_sys, Kp, Kd);
  auto vehicle_sys = cascade(
      make_shared<
          Gain<DrivingCommand, PDControlSystem<RigidBodySystem>::InputVector>>(
          map_driving_cmd_to_x_d),
      vehicle_with_pd);
  auto visualizer =
      make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm, tree);
  auto sys = cascade(vehicle_sys, visualizer);

  SimulationOptions options = default_simulation_options;
  rigid_body_sys->penetration_stiffness = 5000.0;
  rigid_body_sys->penetration_damping =
      rigid_body_sys->penetration_stiffness / 10.0;
  rigid_body_sys->friction_coefficient = 10.0;  // essentially infinite friction
  options.initial_step_size = 5e-3;
  options.timeout_seconds = numeric_limits<double>::infinity();

  VectorXd x0 = VectorXd::Zero(rigid_body_sys->getNumStates());
  x0.head(tree->num_positions) = tree->getZeroConfiguration();
  // todo:  call getInitialState instead?  (but currently, that would require
  // snopt).  needs #1627
  // I'm getting away without it, but might be generating large internal forces
  // initially as the ackerman constraint (hopefully) gets enforced by the
  // stabilization terms.

  runLCM(sys, lcm, 0, std::numeric_limits<double>::infinity(), x0, options);
  //  simulate(*sys, 0, std::numeric_limits<double>::infinity(), x0, options);

  return 0;
}
<|endoftext|>


<commit_before>#include "kinetic_scroller.hpp"
#include "visual_params.hpp"

#include "indexer/scales.hpp"

#include "base/logging.hpp"

namespace df
{

double const kKineticDuration = 0.6;
double const kKineticFadeoff = 1.5;
double const kKineticThreshold = 50.0;
double const kKineticAcceleration = 0.4;
double const kKineticMaxSpeedStart = 1000.0; // pixels per second
double const kKineticMaxSpeedEnd = 10000.0; // pixels per second

double CalculateKineticMaxSpeed(ScreenBase const & modelView)
{
  double const kMinZoom = 1.0;
  double const kMaxZoom = scales::UPPER_STYLE_SCALE + 1.0;
  double const zoomLevel = my::clamp(fabs(log(modelView.GetScale()) / log(2.0)), kMinZoom, kMaxZoom);
  double const lerpCoef = 1.0 - ((zoomLevel - kMinZoom) / (kMaxZoom - kMinZoom));

  return (kKineticMaxSpeedStart * lerpCoef + kKineticMaxSpeedEnd * (1.0 - lerpCoef)) * VisualParams::Instance().GetVisualScale();
}

class KineticScrollAnimation : public BaseModelViewAnimation
{
public:
  // startRect - mercator visible on screen rect in moment when user release fingers.
  // direction - mercator space direction of moving. length(direction) - mercator distance on wich map will be offset.
  KineticScrollAnimation(m2::AnyRectD const & startRect, m2::PointD const & direction, double duration)
    : BaseModelViewAnimation(duration)
    , m_targetCenter(startRect.GlobalCenter() + direction)
    , m_angle(startRect.Angle())
    , m_localRect(startRect.GetLocalRect())
    , m_direction(direction)
  {
  }

  ModelViewAnimationType GetType() const override { return ModelViewAnimationType::KineticScroll; }

  m2::AnyRectD GetCurrentRect(ScreenBase const & screen) const override
  {
    // Current position = target position - amplutide * e ^ (elapsed / duration).
    // We calculate current position not based on start position, but based on target position.
    return m2::AnyRectD(m_targetCenter - m_direction * exp(-kKineticFadeoff * GetT()), m_angle, m_localRect);
  }

  m2::AnyRectD GetTargetRect(ScreenBase const & screen) const override
  {
    return GetCurrentRect(screen);
  }

private:
  m2::PointD m_targetCenter;
  ang::AngleD m_angle;
  m2::RectD m_localRect;
  m2::PointD m_direction;
};

KineticScroller::KineticScroller()
  : m_lastTimestamp(-1.0)
  , m_direction(m2::PointD::Zero())
{
}

void KineticScroller::InitGrab(ScreenBase const & modelView, double timeStamp)
{
  ASSERT_LESS(m_lastTimestamp, 0.0, ());
  m_lastTimestamp = timeStamp;
  m_lastRect = modelView.GlobalRect();
}

bool KineticScroller::IsActive() const
{
  return m_lastTimestamp > 0.0;
}

void KineticScroller::GrabViewRect(ScreenBase const & modelView, double timeStamp)
{
  // In KineitcScroller we store m_direction in mixed state.
  // Direction in mercator space, and length(m_direction) in pixel space.
  // We need same reaction on different zoom levels, and should calculate velocity on pixel space.
  ASSERT_GREATER(m_lastTimestamp, 0.0, ());
  ASSERT_GREATER(timeStamp, m_lastTimestamp, ());
  double elapsed = timeStamp - m_lastTimestamp;

  m2::PointD lastCenter = m_lastRect.GlobalCenter();
  m2::PointD currentCenter = modelView.GlobalRect().GlobalCenter();
  double pxDeltaLength = (modelView.GtoP(currentCenter) - modelView.GtoP(lastCenter)).Length();
  m2::PointD delta = (currentCenter - lastCenter);
  if (!delta.IsAlmostZero())
  {
    delta = delta.Normalize();

    // Velocity on pixels.
    double const v = min(pxDeltaLength / elapsed, CalculateKineticMaxSpeed(modelView));

    // At this point length(m_direction) already in pixel space, and delta normalized.
    m_direction = delta * v;
  }
  else
  {
    m_direction = m2::PointD::Zero();
  }

  m_lastTimestamp = timeStamp;
  m_lastRect = modelView.GlobalRect();
}

void KineticScroller::CancelGrab()
{
  m_lastTimestamp = -1;
  m_direction = m2::PointD::Zero();
}

unique_ptr<BaseModelViewAnimation> KineticScroller::CreateKineticAnimation(ScreenBase const & modelView)
{
  static double kVelocityThreshold = kKineticThreshold * VisualParams::Instance().GetVisualScale();
  if (m_direction.Length() < kVelocityThreshold)
    return unique_ptr<BaseModelViewAnimation>();

  // Before we start animation we have to convert length(m_direction) from pixel space to mercator space.
  m2::PointD center = m_lastRect.GlobalCenter();
  double const offset = (modelView.PtoG(modelView.GtoP(center) + m_direction) - center).Length();
  double const glbLength = kKineticAcceleration * offset;
  m2::PointD const glbDirection = m_direction.Normalize() * glbLength;
  m2::PointD const targetCenter = center + glbDirection;
  if (!df::GetWorldRect().IsPointInside(targetCenter))
    return unique_ptr<BaseModelViewAnimation>();

  return unique_ptr<BaseModelViewAnimation>(new KineticScrollAnimation(m_lastRect, glbDirection, kKineticDuration));
}

} // namespace df
<commit_msg>Tuned kinetic scroll braking<commit_after>#include "kinetic_scroller.hpp"
#include "visual_params.hpp"

#include "indexer/scales.hpp"

#include "base/logging.hpp"

namespace df
{

double const kKineticDuration = 1.5;
double const kKineticFadeoff = 4.0;
double const kKineticThreshold = 50.0;
double const kKineticAcceleration = 0.4;
double const kKineticMaxSpeedStart = 1000.0; // pixels per second
double const kKineticMaxSpeedEnd = 10000.0; // pixels per second

double CalculateKineticMaxSpeed(ScreenBase const & modelView)
{
  double const kMinZoom = 1.0;
  double const kMaxZoom = scales::UPPER_STYLE_SCALE + 1.0;
  double const zoomLevel = my::clamp(fabs(log(modelView.GetScale()) / log(2.0)), kMinZoom, kMaxZoom);
  double const lerpCoef = 1.0 - ((zoomLevel - kMinZoom) / (kMaxZoom - kMinZoom));

  return (kKineticMaxSpeedStart * lerpCoef + kKineticMaxSpeedEnd * (1.0 - lerpCoef)) * VisualParams::Instance().GetVisualScale();
}

class KineticScrollAnimation : public BaseModelViewAnimation
{
public:
  // startRect - mercator visible on screen rect in moment when user release fingers.
  // direction - mercator space direction of moving. length(direction) - mercator distance on wich map will be offset.
  KineticScrollAnimation(m2::AnyRectD const & startRect, m2::PointD const & direction, double duration)
    : BaseModelViewAnimation(duration)
    , m_targetCenter(startRect.GlobalCenter() + direction)
    , m_angle(startRect.Angle())
    , m_localRect(startRect.GetLocalRect())
    , m_direction(direction)
  {
  }

  ModelViewAnimationType GetType() const override { return ModelViewAnimationType::KineticScroll; }

  m2::AnyRectD GetCurrentRect(ScreenBase const & screen) const override
  {
    // Current position = target position - amplutide * e ^ (elapsed / duration).
    // We calculate current position not based on start position, but based on target position.
    return m2::AnyRectD(m_targetCenter - m_direction * exp(-kKineticFadeoff * GetT()), m_angle, m_localRect);
  }

  m2::AnyRectD GetTargetRect(ScreenBase const & screen) const override
  {
    return GetCurrentRect(screen);
  }

private:
  m2::PointD m_targetCenter;
  ang::AngleD m_angle;
  m2::RectD m_localRect;
  m2::PointD m_direction;
};

KineticScroller::KineticScroller()
  : m_lastTimestamp(-1.0)
  , m_direction(m2::PointD::Zero())
{
}

void KineticScroller::InitGrab(ScreenBase const & modelView, double timeStamp)
{
  ASSERT_LESS(m_lastTimestamp, 0.0, ());
  m_lastTimestamp = timeStamp;
  m_lastRect = modelView.GlobalRect();
}

bool KineticScroller::IsActive() const
{
  return m_lastTimestamp > 0.0;
}

void KineticScroller::GrabViewRect(ScreenBase const & modelView, double timeStamp)
{
  // In KineitcScroller we store m_direction in mixed state.
  // Direction in mercator space, and length(m_direction) in pixel space.
  // We need same reaction on different zoom levels, and should calculate velocity on pixel space.
  ASSERT_GREATER(m_lastTimestamp, 0.0, ());
  ASSERT_GREATER(timeStamp, m_lastTimestamp, ());
  double elapsed = timeStamp - m_lastTimestamp;

  m2::PointD lastCenter = m_lastRect.GlobalCenter();
  m2::PointD currentCenter = modelView.GlobalRect().GlobalCenter();
  double pxDeltaLength = (modelView.GtoP(currentCenter) - modelView.GtoP(lastCenter)).Length();
  m2::PointD delta = (currentCenter - lastCenter);
  if (!delta.IsAlmostZero())
  {
    delta = delta.Normalize();

    // Velocity on pixels.
    double const v = min(pxDeltaLength / elapsed, CalculateKineticMaxSpeed(modelView));

    // At this point length(m_direction) already in pixel space, and delta normalized.
    m_direction = delta * v;
  }
  else
  {
    m_direction = m2::PointD::Zero();
  }

  m_lastTimestamp = timeStamp;
  m_lastRect = modelView.GlobalRect();
}

void KineticScroller::CancelGrab()
{
  m_lastTimestamp = -1;
  m_direction = m2::PointD::Zero();
}

unique_ptr<BaseModelViewAnimation> KineticScroller::CreateKineticAnimation(ScreenBase const & modelView)
{
  static double kVelocityThreshold = kKineticThreshold * VisualParams::Instance().GetVisualScale();
  if (m_direction.Length() < kVelocityThreshold)
    return unique_ptr<BaseModelViewAnimation>();

  // Before we start animation we have to convert length(m_direction) from pixel space to mercator space.
  m2::PointD center = m_lastRect.GlobalCenter();
  double const offset = (modelView.PtoG(modelView.GtoP(center) + m_direction) - center).Length();
  double const glbLength = kKineticAcceleration * offset;
  m2::PointD const glbDirection = m_direction.Normalize() * glbLength;
  m2::PointD const targetCenter = center + glbDirection;
  if (!df::GetWorldRect().IsPointInside(targetCenter))
    return unique_ptr<BaseModelViewAnimation>();

  return unique_ptr<BaseModelViewAnimation>(new KineticScrollAnimation(m_lastRect, glbDirection, kKineticDuration));
}

} // namespace df
<|endoftext|>


<commit_before>#include <iostream>
#include "mtao/optimization/line_search.hpp"

#include <Eigen/Dense>

struct QuadraticFunc {
    using Scalar = double;
    using Vector = Eigen::VectorXd;

    Scalar objective(const Vector& p) const {
        return p.transpose() * A * p + b.dot(p) + c;
    }
    Vector gradient(const Vector& p) const {
        Vector ret = 2 * A * p + b;
        return ret;
    }

    //Vector descent_direction(const Vector& p) const { return -gradient(p).normalized(); }
    Vector descent_direction(const Vector& p) const {

        if(trinary) {
        return (-gradient(p)).unaryExpr([](double v) -> double {
                if(v > 0) { return 1; }
                else if(v < 0) { return -1; }
                else { return 0; }
                });
        } else {
            return -gradient(p).normalized();
        }

    }
    bool trinary = false;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Scalar c;

};

using namespace mtao::optimization;

int main(int argc, char * argv[]) {
    std::cout << "Eigen threads: " << Eigen::nbThreads() << std::endl;
    QuadraticFunc func;
    int N = 50;
    Eigen::MatrixXd A(N,N);
    Eigen::VectorXd b(N);

    if constexpr(true) {
        A.setIdentity();
        b.setOnes(N);

        func.A = A.transpose() * A;
        func.b = -2 * A.transpose() * b;
        func.c = b.dot(b);
    } else {

        A.setRandom();
        b.setRandom();
        func.A = A.transpose() * A;
        func.b = b;
        func.c = 5;
    }

    auto run = [&](auto&& opt) {
        opt.set_position(b);
        std::cout << "Optimal energy: " << opt.objective() << std::endl;
        std::cout << "Optimal gradient norm: " << opt.gradient().norm() << std::endl;
        opt.set_position(Eigen::VectorXd::Random(N));
        std::cout << "Initial energy: " << opt.objective() << std::endl;
        int iters = opt.run();
        std::cout << "Final energy: " << opt.objective() << " in " << iters << " iterations."<< std::endl;
        std::cout << std::endl;
        //std::cout << "position: " << opt.position().transpose() << std::endl;
    };
    auto run_run = [&]() {
    {
        std::cout << "Backtracking / armijo conditions" << std::endl;
        auto opt = make_backtracking_line_search(func);
        run(opt);
    }
    {
        std::cout << "weak wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_weak();
        run(opt);
    }
    {
        std::cout << "Strong wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_strong();
        run(opt);
    }
    };
    run_run();
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
    func.trinary = true;
    std::cout << "Using a `trinary` which is lazy for something in the roughly right direction" << std::endl;
    run_run();
}
<commit_msg>slight flop reduction on a useless test<commit_after>#include <iostream>
#include "mtao/optimization/line_search.hpp"

#include <Eigen/Dense>

struct QuadraticFunc {
    using Scalar = double;
    using Vector = Eigen::VectorXd;

    Scalar objective(const Vector& p) const {
        return p.transpose() * (A * p + b) + c;
    }
    Vector gradient(const Vector& p) const {
        Vector ret = 2 * A * p + b;
        return ret;
    }

    //Vector descent_direction(const Vector& p) const { return -gradient(p).normalized(); }
    Vector descent_direction(const Vector& p) const {

        if(trinary) {
        return (-gradient(p)).unaryExpr([](double v) -> double {
                if(v > 0) { return 1; }
                else if(v < 0) { return -1; }
                else { return 0; }
                });
        } else {
            return -gradient(p).normalized();
        }

    }
    bool trinary = false;
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Scalar c;

};

using namespace mtao::optimization;

int main(int argc, char * argv[]) {
    std::cout << "Eigen threads: " << Eigen::nbThreads() << std::endl;
    QuadraticFunc func;
    int N = 50;
    Eigen::MatrixXd A(N,N);
    Eigen::VectorXd b(N);

    if constexpr(true) {
        A.setIdentity();
        b.setOnes(N);

        func.A = A.transpose() * A;
        func.b = -2 * A.transpose() * b;
        func.c = b.dot(b);
    } else {

        A.setRandom();
        b.setRandom();
        func.A = A.transpose() * A;
        func.b = b;
        func.c = 5;
    }

    auto run = [&](auto&& opt) {
        opt.set_position(b);
        std::cout << "Optimal energy: " << opt.objective() << std::endl;
        std::cout << "Optimal gradient norm: " << opt.gradient().norm() << std::endl;
        opt.set_position(Eigen::VectorXd::Random(N));
        std::cout << "Initial energy: " << opt.objective() << std::endl;
        int iters = opt.run();
        std::cout << "Final energy: " << opt.objective() << " in " << iters << " iterations."<< std::endl;
        std::cout << std::endl;
        //std::cout << "position: " << opt.position().transpose() << std::endl;
    };
    auto run_run = [&]() {
    {
        std::cout << "Backtracking / armijo conditions" << std::endl;
        auto opt = make_backtracking_line_search(func);
        run(opt);
    }
    {
        std::cout << "weak wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_weak();
        run(opt);
    }
    {
        std::cout << "Strong wolfe conditions" << std::endl;
        auto opt = make_wolfe_line_search(func);
        opt.set_strong();
        run(opt);
    }
    };
    run_run();
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
    func.trinary = true;
    std::cout << "Using a `trinary` which is lazy for something in the roughly right direction" << std::endl;
    run_run();
}
<|endoftext|>


<commit_before>//! @Yue Wang
//!
//! Exercise 6.22:
//! Write a function to swap two int pointers.
//!
#include <iostream>

void swap(int*& lft, int*& rht)
{
    auto tmp = lft;
    lft = rht;
    rht = tmp;
}

int main()
{
    int i = 42, j = 99;
    auto lft = &i;
    auto rht = &j;
    swap(lft, rht);
    std::cout << *lft << " " << *rht << std::endl;

    return 0;
}
<commit_msg>Update ex6_22.cpp<commit_after>//! @Yue Wang
//!
//! Exercise 6.22:
//! Write a function to swap two int pointers.
//!
#include <iostream>

void swap(int*(&lft), int*(&rht))
{
    auto tmp = lft;
    lft = rht;
    rht = tmp;
}

int main()
{
    int i = 42, j = 99;
    auto lft = &i;
    auto rht = &j;
    swap(lft, rht);
    std::cout << *lft << " " << *rht << std::endl;

    return 0;
}
<|endoftext|>


<commit_before>//! @Alan
//!
//! Exercise 6.44: Rewrite the isShorter function from § 6.2.2 (p. 211) to be inline.
//!

#include <iostream>
#include <string>

using std::string;

inline bool isShorter(const string &s1, const string &s2) // defining in the header more better.
{
    return s1.size() < s2.size();
}

int main()
{
    std::cout << isShorter("pezy", "mooophy") << std::endl;
}
<commit_msg>Update ex6_44.cpp<commit_after>//! @Yue Wang
//!
//! Exercise 6.44: Rewrite the isShorter function from § 6.2.2 (p. 211) to be inline.
//!
#include <iostream>
#include <string>
using std::string; using std::cout; using std::endl;

inline bool is_shorter(const string &lft, const string &rht) // defining in the header more better.
{
    return lft.size() < rht.size();
}

int main()
{
    cout << is_shorter("pezy", "mooophy") << endl;
    return 0;
}
<|endoftext|>


<commit_before>//
//  ex9_18.cpp
//  Exercise 9.18
//
//  Created by pezy on 12/3/14.
//  Copyright (c) 2014 pezy. All rights reserved.
//
//  @Brief  Write a program to read a sequence of strings from the standard
//  input into
//          a deque. Use iterators to write a loop to print the elements in the
//          deque.

#include <iostream>
#include <string>
#include <deque>

using std::string;
using std::deque;
using std::cout;
using std::cin;
using std::endl;

int main()
{
    deque<string> input;
    for (string str; cin >> str; input.push_back(str))
        ;
    for (auto iter = input.cbegin(); iter != input.cend(); ++iter)
        cout << *iter << endl;

    return 0;
}
<commit_msg>Update ex9_18.cpp<commit_after>//
//  ex9_18.cpp
//  Exercise 9.18
//
//  Created by pezy on 12/3/14.
//  Copyright (c) 2014 pezy. All rights reserved.
//
//  @Brief  Write a program to read a sequence of strings from the standard input into
//          a deque. Use iterators to write a loop to print the elements in the deque.

#include <iostream>
#include <deque>
#include <string>

using std::cin;
using std::cout;
using std::endl;
using std::deque;
using std::string;

int main()
{
    deque<string> input;
    for(string str; cin>>str; input.push_back(str))
        ;
    for(auto iter=input.cbegin(); iter!=input.cend(); ++iter)
    {
        cout<<*iter<<" ";
    }
    cout<<endl;
}
<|endoftext|>


<commit_before>//
// Created by dar on 11/27/15.
//

#include <stdio.h>
#include <stdlib.h>
#include <gui/GuiText.h>
#include <gui/GuiButton.h>
#include "Game.h"
#include "../core/Core.h"
#include "../core/map/TiledTxtMapLoader.h"
#include "render/RenderManager.h"
#include "InputManager.h"
#include <core/map/entity/EntityBullet.h>
#include <gui/GuiTextBubble.h>
#include <core/map/entity/EntityDoor.h>
#include "../logging.h"

Game::Game(const std::function<bool(Window *window)> &switchWindow) : Window(switchWindow) {
    MapLoader *mapLoader = new TiledTxtMapLoader("test_map");
    Map *bmap = mapLoader->loadMap();
    this->core = new Core(bmap);
    delete mapLoader;

    double ringScale = 2 * this->core->getBlockSize();
    this->entityRotationRing = new GuiElement(GUI_TOP_LEFT, 0, 0, ringScale, ringScale, 6);
    this->entityRotationRing->setVisible(false);
    this->entityRotationRing->setAngle(2);
    this->guiElements.push_back(this->entityRotationRing);

    GuiElement *character = new GuiElement(GUI_TOP_RIGHT, 0, 50, 150, 150, 7);
    this->guiElements.push_back(character);
    GuiElement *window = new GuiTextBubble(GUI_TOP_RIGHT, 160, 60, 400, 170);
    this->guiElements.push_back(window);
    GuiText *text = new GuiText(string("Hey, I am Willy. I will \nguide you around this \nplace. I am the \nghost from the blah \nblah blah blah..."), -500, 73, GUI_TOP_LEFT, 24, 0x666666FF, 0);
    this->guiElements.push_back(text);
    this->popup[0] = character;
    this->popup[1] = window;
    this->popup[2] = text;

    GuiText *t = new GuiText(string("Dev Build: ") + __DATE__ + " " + __TIME__, 15, 15, GUI_BOTTOM_LEFT, 32, 0xFFFFFFFF, 0);
    this->guiElements.push_back(t);
}

void Game::reload(unsigned int windowWidth, unsigned int windowHeight) {
    for (GuiElement *e : this->guiElements) {
        e->reinit(windowWidth, windowHeight);
    }

    this->windowWidth = windowWidth;
    this->windowHeight = windowHeight;
    SDL_StartTextInput();
}

void Game::tick(double deltaTime) {
    this->core->getMap()->update();
    this->core->getMap()->getWorld()->Step(deltaTime, 8, 3);
    for (int i = 0; i < this->core->getMap()->getEntities().size(); i++) {
        Entity *entity = this->core->getMap()->getEntities().at(i);
        if (entity->isToBeDeleted()) {
            this->core->getMap()->removeEntity(entity);
            i--;
        }
    }

    bool popupVisible = ((GuiText *) this->popup[2])->getString().size() > 0;
    static float alpha = 0.0f;
    alpha += deltaTime * 0.75;
    if (alpha > 1) alpha = 1;
    for (int i = 0; i < 3; i++) {
        this->popup[i]->setVisible(popupVisible);
        int color = this->popup[i]->getColor() & 0xFFFFFF00;
        color |= (int) (alpha * 255);
        this->popup[i]->setColor(color);
    }
    this->popup[2]->setX(this->popup[1]->getX() + 10);

    double dx = (this->core->getPlayer()->getX() + this->core->getPlayer()->getWidth() / 2 - 1) * this->core->getBlockSize() * this->core->getGeneralScale() + this->core->getCamX();
    double dy = (this->core->getPlayer()->getY() + this->core->getPlayer()->getHeight() / 2 - 1) * this->core->getBlockSize() * this->core->getGeneralScale() + this->core->getCamY();
    if (abs(dx) > 2) this->core->setCamX(-this->core->getCamX() + (dx) * 0.05);
    if (abs(dy) > 2) this->core->setCamY(-this->core->getCamY() + (dy) * 0.05);

    double camX = this->core->getCamX(), camY = this->core->getCamY();
    if (-camX < this->windowWidth / 2 - this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamX(this->windowWidth / 2 - this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camY < this->windowHeight / 2 - this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamY(this->windowHeight / 2 - this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camX > -(signed) this->windowWidth / 2 + (this->core->getMap()->getWidth() - 1) * this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamX(-(signed) this->windowWidth / 2 + (this->core->getMap()->getWidth() - 1) * this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camY > -(signed) this->windowHeight / 2 + (this->core->getMap()->getHeight() - 1) * this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamY(-(signed) this->windowHeight / 2 + (this->core->getMap()->getHeight() - 1) * this->core->getBlockSize() * this->core->getGeneralScale());
    }
}

void Game::handleKeyboard(const Keypress *const keypress) {
    if (keypress[SDLK_c].isPressed()) {
        double angle = atan2(0, 0) + M_PI; //TODO
        EntityBullet *p = new EntityBullet(this->core->getMap(), angle, 1);
        p->setX(this->core->getPlayer()->getX() - (p->getWidth()) + 0.5 * cos(angle));
        p->setY(this->core->getPlayer()->getY() - (p->getHeight()) + 0.5 * sin(angle));
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_n].isPressed()) {
        EntityDoor *p = new EntityDoor(this->core->getMap());
        p->setX(this->core->getPlayer()->getX() - this->core->getPlayer()->getWidth() / 2);
        p->setY(this->core->getPlayer()->getY() - this->core->getPlayer()->getHeight() / 2);
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_m].isPressed()) {
        SimpleShape *p = new SimpleShape(this->core->getMap(), (unsigned int) (rand() % 3));
        p->setX(this->core->getPlayer()->getX() - this->core->getPlayer()->getWidth() / 2);
        p->setY(this->core->getPlayer()->getY() - this->core->getPlayer()->getHeight() / 2);
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_k].isPressed()) {
        if (this->core->getPlayer()->getToy() == nullptr) {
            this->core->getPlayer()->setToy();
        } else {
            this->core->getPlayer()->eject();
        }
    }
    double playerSpeed = this->core->getPlayer()->getSpeed();
    if (keypress[SDLK_w].isDown()) {
        this->core->getPlayer()->applyImpulse(0, -playerSpeed);
    }
    if (keypress[SDLK_s].isDown()) {
        this->core->getPlayer()->applyImpulse(0, playerSpeed);
    }
    if (keypress[SDLK_a].isDown()) {
        this->core->getPlayer()->applyImpulse(-playerSpeed, 0);
    }
    if (keypress[SDLK_d].isDown()) {
        this->core->getPlayer()->applyImpulse(playerSpeed, 0);
    }
    if (keypress[SDLK_q].isPressed()) {
        this->core->stop();
    }
    if (keypress[SDLK_p].isPressed()) {
        this->core->getMap()->saveEntities();
    }
    if (keypress[SDLK_MINUS].isDown()) {
        this->core->setBlockSize(this->core->getBlockSize() - 0.15);
    }
    if (keypress[SDLK_EQUALS].isDown()) {
        this->core->setBlockSize(this->core->getBlockSize() + 0.15);
    }
}

void Game::handleClick(const TouchPoint *const p) {
    float x = (float) ((-this->core->getCamX() - (double) this->windowWidth / 2 + p->x) / this->core->getGeneralScale() / this->core->getBlockSize() + 0.5);
    float y = (float) ((-this->core->getCamY() - (double) this->windowHeight / 2 + p->y) / this->core->getGeneralScale() / this->core->getBlockSize() + 0.5);
    if (p->id == SDL_BUTTON_LEFT) {
        if (p->state == 0) {
            this->heldEntity = this->getEntityAt(x, y);
            int i = 0;
        } else if (p->state == 2) {
            if (this->heldEntity != nullptr && !this->entityRotationRing->isVisible()) {
                Entity *e = this->heldEntity;
                e->setX(x + e->getWidth() / 2);
                e->setY(y + e->getHeight() / 2);
            }
        } else if (p->state == 1) {
            if (this->heldEntity == nullptr) {
                SimpleShape *s = new SimpleShape(this->core->getMap(), (unsigned int) (rand() % 3));
                s->setX(x + s->getWidth() / 2);
                s->setY(y + s->getWidth() / 2);
                this->core->getMap()->addEntity(s);
            } else {
                this->heldEntity = nullptr;
                if (this->entityRotationRing->isVisible()) {
                    this->entityRotationRing->setVisible(false);
                    SDL_ShowCursor(true);
                    SDL_WarpMouseGlobal(this->mouseLockX, this->mouseLockY);
                }
            }
        }
    } else if (p->id == SDL_BUTTON_MIDDLE) {
        if (p->state == 0) {
            this->heldEntity = this->getEntityAt(x, y);
        } else if (p->state == 1) {
            if (this->heldEntity != nullptr && this->getEntityAt(x, y) == this->heldEntity) {
                this->heldEntity->remove();
            }
        }
    } else if (p->id == SDL_BUTTON_RIGHT) {
        if (p->state == 0) {
            if (this->heldEntity != nullptr) {
                this->entityRotationRing->setX(p->x - this->entityRotationRing->getWidth() / 2);
                this->entityRotationRing->setY(p->y - this->entityRotationRing->getHeight() / 2);
                this->entityRotationRing->setAngle(this->heldEntity->getAngle());
                this->entityRotationRing->setVisible(true);
                SDL_GetGlobalMouseState(&this->mouseLockX, &this->mouseLockY);
                SDL_ShowCursor(false);
            }
        } else if (p->state == 2) {
            if (this->heldEntity != nullptr) {
                int x, y;
                SDL_GetGlobalMouseState(&x, &y);
                double angle = 10.0 * (x - this->mouseLockX) / this->windowWidth;
                this->entityRotationRing->setAngle(angle);
                this->heldEntity->setAngle(angle);
            }
        } else if (p->state == 1) {
            if (this->entityRotationRing->isVisible()) {
                this->entityRotationRing->setVisible(false);
                SDL_ShowCursor(true);
                SDL_WarpMouseGlobal(this->mouseLockX, this->mouseLockY);
            }
        }
    }
}

Entity *Game::getEntityAt(float x, float y) {
    for (Entity *e : this->core->getMap()->getEntities()) {
        if (x >= e->getX() - e->getWidth() && x <= e->getX() &&
            y >= e->getY() - e->getHeight() && y <= e->getY()) {
            return e;
        }
    }
    return nullptr;
}

Game::~Game() {
    SDL_StopTextInput();
    delete this->core;
}
<commit_msg>Added radius to entity select math<commit_after>//
// Created by dar on 11/27/15.
//

#include <stdio.h>
#include <stdlib.h>
#include <gui/GuiText.h>
#include <gui/GuiButton.h>
#include "Game.h"
#include "../core/Core.h"
#include "../core/map/TiledTxtMapLoader.h"
#include "render/RenderManager.h"
#include "InputManager.h"
#include <core/map/entity/EntityBullet.h>
#include <gui/GuiTextBubble.h>
#include <core/map/entity/EntityDoor.h>
#include "../logging.h"

Game::Game(const std::function<bool(Window *window)> &switchWindow) : Window(switchWindow) {
    MapLoader *mapLoader = new TiledTxtMapLoader("test_map");
    Map *bmap = mapLoader->loadMap();
    this->core = new Core(bmap);
    delete mapLoader;

    double ringScale = 2 * this->core->getBlockSize();
    this->entityRotationRing = new GuiElement(GUI_TOP_LEFT, 0, 0, ringScale, ringScale, 6);
    this->entityRotationRing->setVisible(false);
    this->entityRotationRing->setAngle(2);
    this->guiElements.push_back(this->entityRotationRing);

    GuiElement *character = new GuiElement(GUI_TOP_RIGHT, 0, 50, 150, 150, 7);
    this->guiElements.push_back(character);
    GuiElement *window = new GuiTextBubble(GUI_TOP_RIGHT, 160, 60, 400, 170);
    this->guiElements.push_back(window);
    GuiText *text = new GuiText(string("Hey, I am Willy. I will \nguide you around this \nplace. I am the \nghost from the blah \nblah blah blah..."), -500, 73, GUI_TOP_LEFT, 24, 0x666666FF, 0);
    this->guiElements.push_back(text);
    this->popup[0] = character;
    this->popup[1] = window;
    this->popup[2] = text;

    GuiText *t = new GuiText(string("Dev Build: ") + __DATE__ + " " + __TIME__, 15, 15, GUI_BOTTOM_LEFT, 32, 0xFFFFFFFF, 0);
    this->guiElements.push_back(t);
}

void Game::reload(unsigned int windowWidth, unsigned int windowHeight) {
    for (GuiElement *e : this->guiElements) {
        e->reinit(windowWidth, windowHeight);
    }

    this->windowWidth = windowWidth;
    this->windowHeight = windowHeight;
    SDL_StartTextInput();
}

void Game::tick(double deltaTime) {
    this->core->getMap()->update();
    this->core->getMap()->getWorld()->Step(deltaTime, 8, 3);
    for (int i = 0; i < this->core->getMap()->getEntities().size(); i++) {
        Entity *entity = this->core->getMap()->getEntities().at(i);
        if (entity->isToBeDeleted()) {
            this->core->getMap()->removeEntity(entity);
            i--;
        }
    }

    bool popupVisible = ((GuiText *) this->popup[2])->getString().size() > 0;
    static float alpha = 0.0f;
    alpha += deltaTime * 0.75;
    if (alpha > 1) alpha = 1;
    for (int i = 0; i < 3; i++) {
        this->popup[i]->setVisible(popupVisible);
        int color = this->popup[i]->getColor() & 0xFFFFFF00;
        color |= (int) (alpha * 255);
        this->popup[i]->setColor(color);
    }
    this->popup[2]->setX(this->popup[1]->getX() + 10);

    double dx = (this->core->getPlayer()->getX() + this->core->getPlayer()->getWidth() / 2 - 1) * this->core->getBlockSize() * this->core->getGeneralScale() + this->core->getCamX();
    double dy = (this->core->getPlayer()->getY() + this->core->getPlayer()->getHeight() / 2 - 1) * this->core->getBlockSize() * this->core->getGeneralScale() + this->core->getCamY();
    if (abs(dx) > 2) this->core->setCamX(-this->core->getCamX() + (dx) * 0.05);
    if (abs(dy) > 2) this->core->setCamY(-this->core->getCamY() + (dy) * 0.05);

    double camX = this->core->getCamX(), camY = this->core->getCamY();
    if (-camX < this->windowWidth / 2 - this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamX(this->windowWidth / 2 - this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camY < this->windowHeight / 2 - this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamY(this->windowHeight / 2 - this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camX > -(signed) this->windowWidth / 2 + (this->core->getMap()->getWidth() - 1) * this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamX(-(signed) this->windowWidth / 2 + (this->core->getMap()->getWidth() - 1) * this->core->getBlockSize() * this->core->getGeneralScale());
    }
    if (-camY > -(signed) this->windowHeight / 2 + (this->core->getMap()->getHeight() - 1) * this->core->getBlockSize() * this->core->getGeneralScale()) {
        this->core->setCamY(-(signed) this->windowHeight / 2 + (this->core->getMap()->getHeight() - 1) * this->core->getBlockSize() * this->core->getGeneralScale());
    }
}

void Game::handleKeyboard(const Keypress *const keypress) {
    if (keypress[SDLK_c].isPressed()) {
        double angle = atan2(0, 0) + M_PI; //TODO
        EntityBullet *p = new EntityBullet(this->core->getMap(), angle, 1);
        p->setX(this->core->getPlayer()->getX() - (p->getWidth()) + 0.5 * cos(angle));
        p->setY(this->core->getPlayer()->getY() - (p->getHeight()) + 0.5 * sin(angle));
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_n].isPressed()) {
        EntityDoor *p = new EntityDoor(this->core->getMap());
        p->setX(this->core->getPlayer()->getX() - this->core->getPlayer()->getWidth() / 2);
        p->setY(this->core->getPlayer()->getY() - this->core->getPlayer()->getHeight() / 2);
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_m].isPressed()) {
        SimpleShape *p = new SimpleShape(this->core->getMap(), (unsigned int) (rand() % 3));
        p->setX(this->core->getPlayer()->getX() - this->core->getPlayer()->getWidth() / 2);
        p->setY(this->core->getPlayer()->getY() - this->core->getPlayer()->getHeight() / 2);
        this->core->getMap()->addEntity(p);
    }
    if (keypress[SDLK_k].isPressed()) {
        if (this->core->getPlayer()->getToy() == nullptr) {
            this->core->getPlayer()->setToy();
        } else {
            this->core->getPlayer()->eject();
        }
    }
    double playerSpeed = this->core->getPlayer()->getSpeed();
    if (keypress[SDLK_w].isDown()) {
        this->core->getPlayer()->applyImpulse(0, -playerSpeed);
    }
    if (keypress[SDLK_s].isDown()) {
        this->core->getPlayer()->applyImpulse(0, playerSpeed);
    }
    if (keypress[SDLK_a].isDown()) {
        this->core->getPlayer()->applyImpulse(-playerSpeed, 0);
    }
    if (keypress[SDLK_d].isDown()) {
        this->core->getPlayer()->applyImpulse(playerSpeed, 0);
    }
    if (keypress[SDLK_q].isPressed()) {
        this->core->stop();
    }
    if (keypress[SDLK_p].isPressed()) {
        this->core->getMap()->saveEntities();
    }
    if (keypress[SDLK_MINUS].isDown()) {
        this->core->setBlockSize(this->core->getBlockSize() - 0.15);
    }
    if (keypress[SDLK_EQUALS].isDown()) {
        this->core->setBlockSize(this->core->getBlockSize() + 0.15);
    }
}

void Game::handleClick(const TouchPoint *const p) {
    float x = (float) ((-this->core->getCamX() - (double) this->windowWidth / 2 + p->x) / this->core->getGeneralScale() / this->core->getBlockSize() + 0.5);
    float y = (float) ((-this->core->getCamY() - (double) this->windowHeight / 2 + p->y) / this->core->getGeneralScale() / this->core->getBlockSize() + 0.5);
    if (p->id == SDL_BUTTON_LEFT) {
        if (p->state == 0) {
            this->heldEntity = this->getEntityAt(x, y);
            int i = 0;
        } else if (p->state == 2) {
            if (this->heldEntity != nullptr && !this->entityRotationRing->isVisible()) {
                Entity *e = this->heldEntity;
                e->setX(x + e->getWidth() / 2);
                e->setY(y + e->getHeight() / 2);
            }
        } else if (p->state == 1) {
            if (this->heldEntity == nullptr) {
                SimpleShape *s = new SimpleShape(this->core->getMap(), (unsigned int) (rand() % 3));
                s->setX(x + s->getWidth() / 2);
                s->setY(y + s->getWidth() / 2);
                this->core->getMap()->addEntity(s);
            } else {
                this->heldEntity = nullptr;
                if (this->entityRotationRing->isVisible()) {
                    this->entityRotationRing->setVisible(false);
                    SDL_ShowCursor(true);
                    SDL_WarpMouseGlobal(this->mouseLockX, this->mouseLockY);
                }
            }
        }
    } else if (p->id == SDL_BUTTON_MIDDLE) {
        if (p->state == 0) {
            this->heldEntity = this->getEntityAt(x, y);
        } else if (p->state == 1) {
            if (this->heldEntity != nullptr && this->getEntityAt(x, y) == this->heldEntity) {
                this->heldEntity->remove();
            }
        }
    } else if (p->id == SDL_BUTTON_RIGHT) {
        if (p->state == 0) {
            if (this->heldEntity != nullptr) {
                this->entityRotationRing->setX(p->x - this->entityRotationRing->getWidth() / 2);
                this->entityRotationRing->setY(p->y - this->entityRotationRing->getHeight() / 2);
                this->entityRotationRing->setAngle(this->heldEntity->getAngle());
                this->entityRotationRing->setVisible(true);
                SDL_GetGlobalMouseState(&this->mouseLockX, &this->mouseLockY);
                SDL_ShowCursor(false);
            }
        } else if (p->state == 2) {
            if (this->heldEntity != nullptr) {
                int x, y;
                SDL_GetGlobalMouseState(&x, &y);
                double angle = 10.0 * (x - this->mouseLockX) / this->windowWidth;
                this->entityRotationRing->setAngle(angle);
                this->heldEntity->setAngle(angle);
            }
        } else if (p->state == 1) {
            if (this->entityRotationRing->isVisible()) {
                this->entityRotationRing->setVisible(false);
                SDL_ShowCursor(true);
                SDL_WarpMouseGlobal(this->mouseLockX, this->mouseLockY);
            }
        }
    }
}

Entity *Game::getEntityAt(float x, float y) {
    for (Entity *e : this->core->getMap()->getEntities()) {
        if (x >= e->getX() - e->getWidth() - 0.25 && x <= e->getX() + 0.25 &&
            y >= e->getY() - e->getHeight() - 0.25 && y <= e->getY() + 0.25) {
            return e;
        }
    }
    return nullptr;
}

Game::~Game() {
    SDL_StopTextInput();
    delete this->core;
}
<|endoftext|>


<commit_before>/**
 * These codes are licensed under the Unlicense.
 * http://unlicense.org
 */

#ifdef _MSC_VER
#pragma warning(disable:4996)
#endif

#include <memory>
#include <string>

#include <gtest/gtest.h>

#include <commata/char_input.hpp>

#include "BaseTest.hpp"

using namespace std::literals::string_literals;

using namespace commata;
using namespace commata::test;

namespace {

struct TestCharInput : BaseTest
{};

TEST_F(TestCharInput, OwnedStringCopy)
{
    auto i = std::make_unique<owned_string_input<char>>("ABC"s);
    auto j = *i;

    std::vector<char> b(6);

    {
        const auto len = (*i)(b.data(), 4);
        ASSERT_EQ(3U, len);
        ASSERT_EQ("ABC", std::string(b.data(), 3));
    }
    i.reset();

    {
        const auto len = j(b.data() + 3, 4);
        ASSERT_EQ(3U, len);
        ASSERT_EQ("ABCABC", std::string(b.data(), 6));
    }
}

}
<commit_msg>Add tests for make_char_input<commit_after>/**
 * These codes are licensed under the Unlicense.
 * http://unlicense.org
 */

#ifdef _MSC_VER
#pragma warning(disable:4996)
#endif

#include <memory>
#include <string>

#include <gtest/gtest.h>

#include <commata/char_input.hpp>

#include "BaseTest.hpp"

using namespace std::literals::string_literals;
using namespace std::literals::string_view_literals;

using namespace commata;
using namespace commata::test;

namespace {

struct TestCharInput : BaseTest
{};

TEST_F(TestCharInput, OwnedStringCopy)
{
    auto i = std::make_unique<owned_string_input<char>>("ABC"s);
    auto j = *i;

    std::vector<char> b(6);

    {
        const auto len = (*i)(b.data(), 4);
        ASSERT_EQ(3U, len);
        ASSERT_EQ("ABC", std::string(b.data(), 3));
    }
    i.reset();

    {
        const auto len = j(b.data() + 3, 4);
        ASSERT_EQ(3U, len);
        ASSERT_EQ("ABCABC", std::string(b.data(), 6));
    }
}

TEST_F(TestCharInput, MakeFromStreambufPtr)
{
    std::wstringbuf buf(L"XYZ");
    streambuf_input<wchar_t> in = make_char_input(&buf);
    std::wstring out(5, L' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ(L"XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromIStreamLvalueRef)
{
    std::istringstream buf("XYZ");
    streambuf_input<char> in = make_char_input(buf);
    std::string out(5, ' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ("XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromStreambufRvalueRef)
{
    std::stringbuf buf("XYZ");
    owned_streambuf_input<std::stringbuf> in = make_char_input(std::move(buf));
    std::string out(5, ' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ("XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromIStreamRvalueRef)
{
    std::istringstream buf("XYZ");
    owned_istream_input<std::istringstream> in =
        make_char_input(std::move(buf));
    std::string out(5, ' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ("XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromCharPtr)
{
    const wchar_t* const str= L"XYZ";
    string_input<wchar_t> in = make_char_input(str);
    std::wstring out(5, L' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ(L"XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromCharPtrAndSize)
{
    const wchar_t* const str= L"XYZABC";
    string_input<wchar_t> in = make_char_input(str, 4);
    std::wstring out(5, L' ');
    ASSERT_EQ(4, in(out.data(), 4));
    ASSERT_EQ(L"XYZA ", out);
}

TEST_F(TestCharInput, MakeFromStringView)
{
    const auto str= L"XYZABC"sv;
    string_input<wchar_t> in = make_char_input(str.substr(2));
    std::wstring out(5, L' ');
    ASSERT_EQ(4, in(out.data(), 4));
    ASSERT_EQ(L"ZABC ", out);
}

TEST_F(TestCharInput, MakeFromStringLvalueRef)
{
    std::wstring str(L"XYZ");
    string_input<wchar_t> in = make_char_input(str);
    std::wstring out(5, L' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ(L"XYZ  ", out);
}

TEST_F(TestCharInput, MakeFromStringRvalueRef)
{
    std::string str("XYZ");
    owned_string_input<char> in = make_char_input(std::move(str));
    std::string out(5, ' ');
    ASSERT_EQ(3, in(out.data(), 5));
    ASSERT_EQ("XYZ  ", out);
}

}
<|endoftext|>


<commit_before>/*
 * Box2D QML plugin
 * Copyright (C) 2010 Nokia Corporation
 *
 * This file is part of the Box2D QML plugin.
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library;  If not, see <http://www.gnu.org/licenses/>.
 */

#include "box2dworld.h"

#include "box2dbody.h"
#include "box2dfixture.h"
#include "box2djoint.h"
#include "box2ddestructionlistener.h"

#include <QTimerEvent>

#include <Box2D.h>

#define FPS_REPORT_MARGIN 2
#define MONITOR_FPS

class ContactEvent
{
public:
    enum Type {
        BeginContact,
        EndContact
    };

    Type type;
    Box2DFixture *fixtureA;
    Box2DFixture *fixtureB;
};

class ContactListener : public b2ContactListener
{
public:
    void BeginContact(b2Contact *contact);
    void EndContact(b2Contact *contact);

    void removeEvent(int index) { mEvents.removeAt(index); }
    void clearEvents() { mEvents.clear(); }
    const QList<ContactEvent> &events() { return mEvents; }

private:
    QList<ContactEvent> mEvents;
};

void ContactListener::BeginContact(b2Contact *contact)
{
    ContactEvent event;
    event.type = ContactEvent::BeginContact;
    event.fixtureA = toBox2DFixture(contact->GetFixtureA());
    event.fixtureB = toBox2DFixture(contact->GetFixtureB());
    mEvents.append(event);
}

void ContactListener::EndContact(b2Contact *contact)
{
    ContactEvent event;
    event.type = ContactEvent::EndContact;
    event.fixtureA = toBox2DFixture(contact->GetFixtureA());
    event.fixtureB = toBox2DFixture(contact->GetFixtureB());
    mEvents.append(event);
}

Box2DWorld::Box2DWorld(QDeclarativeItem *parent) :
    QDeclarativeItem(parent),
    mWorld(0),
    mContactListener(new ContactListener),
    mDestructionListener(new Box2DDestructionListener),
    mTimeStep(1.0f / 60.0f),
    mVelocityIterations(10),
    mPositionIterations(10),
    mFrameTime(1000 / 60),
    mGravity(qreal(0), qreal(-10)),
    mIsRunning(true),
    mAllowSleeping(true),
    mFps(0),
    mReportFps(false)
{
    connect(mDestructionListener, SIGNAL(fixtureDestroyed(Box2DFixture*)),
            this, SLOT(fixtureDestroyed(Box2DFixture*)));
    time = QTime();
}

Box2DWorld::~Box2DWorld()
{
    // Bodies must be deleted before the world
    foreach (Box2DBody *body, mBodies)
        delete body;
    mBodies.clear();

    delete mWorld;
    delete mContactListener;
    delete mDestructionListener;
}

void Box2DWorld::setAllowSleeping(bool allowSleeping)
{
    if (mAllowSleeping == allowSleeping)
        return;

    mAllowSleeping=allowSleeping;

    if (mWorld)
        mWorld->SetAllowSleeping(allowSleeping);

    emit allowSleepingChanged();
}

void Box2DWorld::setRunning(bool running)
{
    if (mIsRunning == running)
        return;

    mIsRunning = running;
    emit runningChanged();

    if (isComponentComplete()) {
        if (running)
            mTimer.start(mFrameTime, this);
        else
            mTimer.stop();
    }
}

void Box2DWorld::setGravity(const QPointF &gravity)
{
    if (mGravity == gravity)
        return;

    mGravity = gravity;
    if (mWorld)
        mWorld->SetGravity(b2Vec2(gravity.x(), gravity.y()));

    emit gravityChanged();
}

void Box2DWorld::setReportFps(bool reportfps)
{
    if (mReportFps == reportfps )
        return;

    mReportFps = reportfps;
    if (mReportFps) {
        time.start();
        lastTime = time.elapsed();
    }

    emit reportFpsChanged();
}

void Box2DWorld::componentComplete()
{
    QDeclarativeItem::componentComplete();

    const b2Vec2 gravity(mGravity.x(), mGravity.y());

    mWorld = new b2World(gravity);
    mWorld->SetContactListener(mContactListener);
    mWorld->SetDestructionListener(mDestructionListener);

    foreach (QGraphicsItem *child, childItems())
        if (Box2DBody *body = dynamic_cast<Box2DBody*>(child)) {
            registerBody(body);
            connect(body, SIGNAL(destroyed()), this, SLOT(unregisterBody()));
        } else if (Box2DJoint *joint = dynamic_cast<Box2DJoint*>(child)) {
            registerJoint(joint);
        }

    if (mIsRunning)
        mTimer.start(mFrameTime, this);
}

/**
 * Registers a Box2D body with this world. When the world component is
 * complete, it will initialize the body.
 */
void Box2DWorld::registerBody(Box2DBody *body)
{
    mBodies.append(body);
    body->initialize(mWorld);
}

/**
 * Unregisters a Box2D body from this world. Called when a dynamically
 * created Box2D body has been deleted.
 */
void Box2DWorld::unregisterBody()
{
    Box2DBody *body = static_cast<Box2DBody*>(sender());
    mBodies.removeOne(body);
}

void Box2DWorld::registerJoint(Box2DJoint *joint)
{
    joint->initialize(this);
}

void Box2DWorld::fixtureDestroyed(Box2DFixture *fixture)
{
    QList<ContactEvent> events = mContactListener->events();
    for (int i = events.count() - 1; i >= 0; i--) {
        if (events.at(i).fixtureA == fixture
                || events.at(i).fixtureB == fixture)
            mContactListener->removeEvent(i);
    }
}

void Box2DWorld::timerEvent(QTimerEvent *event)
{
    if (event->timerId() == mTimer.timerId()) {
#ifdef MONITOR_FPS
        if (mReportFps) {
        	float tempTime = time.elapsed();
	        float timeStep = (tempTime - lastTime);
        	lastTime = tempTime;
	        int tempFps = 1000.0f/timeStep;
        	if (tempFps < mFps-FPS_REPORT_MARGIN || tempFps > mFps+FPS_REPORT_MARGIN) {
        	    mFps = tempFps;
	            emit fpsChanged();
        	}
        }
#endif
        mWorld->Step(mTimeStep, mVelocityIterations, mPositionIterations);
        foreach (Box2DBody *body, mBodies)
            body->synchronize();

        // Emit contact signals
        foreach (const ContactEvent &event, mContactListener->events()) {
            switch (event.type) {
            case ContactEvent::BeginContact:
                event.fixtureA->emitBeginContact(event.fixtureB);
                event.fixtureB->emitBeginContact(event.fixtureA);
                break;
            case ContactEvent::EndContact:
                event.fixtureA->emitEndContact(event.fixtureB);
                event.fixtureB->emitEndContact(event.fixtureA);
                break;
            }
        }
        mContactListener->clearEvents();

        // Emit signals for the current state of the contacts
        b2Contact *contact = mWorld->GetContactList();
        while (contact) {
            Box2DFixture *fixtureA = toBox2DFixture(contact->GetFixtureA());
            Box2DFixture *fixtureB = toBox2DFixture(contact->GetFixtureB());

            fixtureA->emitContactChanged(fixtureB);
            fixtureB->emitContactChanged(fixtureA);

            contact = contact->GetNext();
        }

        emit stepped();
    }

    QDeclarativeItem::timerEvent(event);
}

QVariant Box2DWorld::itemChange(GraphicsItemChange change,
                                const QVariant &value)
{
    if (isComponentComplete()) {
        if (change == ItemChildAddedChange) {
            QGraphicsItem *child = value.value<QGraphicsItem*>();
            if (Box2DBody *body = dynamic_cast<Box2DBody*>(child)) {
                registerBody(body);
                connect(body, SIGNAL(destroyed()), this, SLOT(unregisterBody()));
            } else if (Box2DJoint *joint = dynamic_cast<Box2DJoint*>(child)) {
                registerJoint(joint);
            }
        }
    }

    return QDeclarativeItem::itemChange(change, value);
}

void Box2DWorld::stop()
{
    setRunning(false);
}

void Box2DWorld::start()
{
    setRunning(true);
}

void Box2DWorld::resetVelocities()
{
    foreach (Box2DBody *body, mBodies)
        body->resetVelocities();
}

<commit_msg>Add optional define to sync physics world with the item world every second frame only<commit_after>/*
 * Box2D QML plugin
 * Copyright (C) 2010 Nokia Corporation
 *
 * This file is part of the Box2D QML plugin.
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library;  If not, see <http://www.gnu.org/licenses/>.
 */

#include "box2dworld.h"

#include "box2dbody.h"
#include "box2dfixture.h"
#include "box2djoint.h"
#include "box2ddestructionlistener.h"

#include <QTimerEvent>

#include <Box2D.h>

#define FPS_REPORT_MARGIN 2
#define MONITOR_FPS
// #define HALVE_FPS

class ContactEvent
{
public:
    enum Type {
        BeginContact,
        EndContact
    };

    Type type;
    Box2DFixture *fixtureA;
    Box2DFixture *fixtureB;
};

class ContactListener : public b2ContactListener
{
public:
    void BeginContact(b2Contact *contact);
    void EndContact(b2Contact *contact);

    void removeEvent(int index) { mEvents.removeAt(index); }
    void clearEvents() { mEvents.clear(); }
    const QList<ContactEvent> &events() { return mEvents; }

private:
    QList<ContactEvent> mEvents;
};

void ContactListener::BeginContact(b2Contact *contact)
{
    ContactEvent event;
    event.type = ContactEvent::BeginContact;
    event.fixtureA = toBox2DFixture(contact->GetFixtureA());
    event.fixtureB = toBox2DFixture(contact->GetFixtureB());
    mEvents.append(event);
}

void ContactListener::EndContact(b2Contact *contact)
{
    ContactEvent event;
    event.type = ContactEvent::EndContact;
    event.fixtureA = toBox2DFixture(contact->GetFixtureA());
    event.fixtureB = toBox2DFixture(contact->GetFixtureB());
    mEvents.append(event);
}

Box2DWorld::Box2DWorld(QDeclarativeItem *parent) :
    QDeclarativeItem(parent),
    mWorld(0),
    mContactListener(new ContactListener),
    mDestructionListener(new Box2DDestructionListener),
    mTimeStep(1.0f / 60.0f),
    mVelocityIterations(10),
    mPositionIterations(10),
    mFrameTime(1000 / 60),
    mGravity(qreal(0), qreal(-10)),
    mIsRunning(true),
    mAllowSleeping(true),
    mFps(0),
    mReportFps(false)
{
    connect(mDestructionListener, SIGNAL(fixtureDestroyed(Box2DFixture*)),
            this, SLOT(fixtureDestroyed(Box2DFixture*)));
    time = QTime();
}

Box2DWorld::~Box2DWorld()
{
    // Bodies must be deleted before the world
    foreach (Box2DBody *body, mBodies)
        delete body;
    mBodies.clear();

    delete mWorld;
    delete mContactListener;
    delete mDestructionListener;
}

void Box2DWorld::setAllowSleeping(bool allowSleeping)
{
    if (mAllowSleeping == allowSleeping)
        return;

    mAllowSleeping=allowSleeping;

    if (mWorld)
        mWorld->SetAllowSleeping(allowSleeping);

    emit allowSleepingChanged();
}

void Box2DWorld::setRunning(bool running)
{
    if (mIsRunning == running)
        return;

    mIsRunning = running;
    emit runningChanged();

    if (isComponentComplete()) {
        if (running)
            mTimer.start(mFrameTime, this);
        else
            mTimer.stop();
    }
}

void Box2DWorld::setGravity(const QPointF &gravity)
{
    if (mGravity == gravity)
        return;

    mGravity = gravity;
    if (mWorld)
        mWorld->SetGravity(b2Vec2(gravity.x(), gravity.y()));

    emit gravityChanged();
}

void Box2DWorld::setReportFps(bool reportfps)
{
    if (mReportFps == reportfps )
        return;

    mReportFps = reportfps;
    if (mReportFps) {
        time.start();
        lastTime = time.elapsed();
    }

    emit reportFpsChanged();
}

void Box2DWorld::componentComplete()
{
    QDeclarativeItem::componentComplete();

    const b2Vec2 gravity(mGravity.x(), mGravity.y());

    mWorld = new b2World(gravity);
    mWorld->SetContactListener(mContactListener);
    mWorld->SetDestructionListener(mDestructionListener);

    foreach (QGraphicsItem *child, childItems())
        if (Box2DBody *body = dynamic_cast<Box2DBody*>(child)) {
            registerBody(body);
            connect(body, SIGNAL(destroyed()), this, SLOT(unregisterBody()));
        } else if (Box2DJoint *joint = dynamic_cast<Box2DJoint*>(child)) {
            registerJoint(joint);
        }

    if (mIsRunning)
        mTimer.start(mFrameTime, this);
}

/**
 * Registers a Box2D body with this world. When the world component is
 * complete, it will initialize the body.
 */
void Box2DWorld::registerBody(Box2DBody *body)
{
    mBodies.append(body);
    body->initialize(mWorld);
}

/**
 * Unregisters a Box2D body from this world. Called when a dynamically
 * created Box2D body has been deleted.
 */
void Box2DWorld::unregisterBody()
{
    Box2DBody *body = static_cast<Box2DBody*>(sender());
    mBodies.removeOne(body);
}

void Box2DWorld::registerJoint(Box2DJoint *joint)
{
    joint->initialize(this);
}

void Box2DWorld::fixtureDestroyed(Box2DFixture *fixture)
{
    QList<ContactEvent> events = mContactListener->events();
    for (int i = events.count() - 1; i >= 0; i--) {
        if (events.at(i).fixtureA == fixture
                || events.at(i).fixtureB == fixture)
            mContactListener->removeEvent(i);
    }
}

void Box2DWorld::timerEvent(QTimerEvent *event)
{
    static bool flipflop=true;
    if (event->timerId() == mTimer.timerId()) {
#ifdef MONITOR_FPS
        if (mReportFps) {
        	float tempTime = time.elapsed();
	        float timeStep = (tempTime - lastTime);
        	lastTime = tempTime;
	        int tempFps = 1000.0f/timeStep;
        	if (tempFps < mFps-FPS_REPORT_MARGIN || tempFps > mFps+FPS_REPORT_MARGIN) {
        	    mFps = tempFps;
	            emit fpsChanged();
        	}
        }
#endif
        mWorld->Step(mTimeStep, mVelocityIterations, mPositionIterations);

#ifdef HALVE_FPS
	if (flipflop) {
	        foreach (Box2DBody *body, mBodies)
        	    body->synchronize();
	}
	flipflop=!flipflop;
#endif

        // Emit contact signals
        foreach (const ContactEvent &event, mContactListener->events()) {
            switch (event.type) {
            case ContactEvent::BeginContact:
                event.fixtureA->emitBeginContact(event.fixtureB);
                event.fixtureB->emitBeginContact(event.fixtureA);
                break;
            case ContactEvent::EndContact:
                event.fixtureA->emitEndContact(event.fixtureB);
                event.fixtureB->emitEndContact(event.fixtureA);
                break;
            }
        }
        mContactListener->clearEvents();

        // Emit signals for the current state of the contacts
        b2Contact *contact = mWorld->GetContactList();
        while (contact) {
            Box2DFixture *fixtureA = toBox2DFixture(contact->GetFixtureA());
            Box2DFixture *fixtureB = toBox2DFixture(contact->GetFixtureB());

            fixtureA->emitContactChanged(fixtureB);
            fixtureB->emitContactChanged(fixtureA);

            contact = contact->GetNext();
        }

        emit stepped();
    }

    QDeclarativeItem::timerEvent(event);
}

QVariant Box2DWorld::itemChange(GraphicsItemChange change,
                                const QVariant &value)
{
    if (isComponentComplete()) {
        if (change == ItemChildAddedChange) {
            QGraphicsItem *child = value.value<QGraphicsItem*>();
            if (Box2DBody *body = dynamic_cast<Box2DBody*>(child)) {
                registerBody(body);
                connect(body, SIGNAL(destroyed()), this, SLOT(unregisterBody()));
            } else if (Box2DJoint *joint = dynamic_cast<Box2DJoint*>(child)) {
                registerJoint(joint);
            }
        }
    }

    return QDeclarativeItem::itemChange(change, value);
}

void Box2DWorld::stop()
{
    setRunning(false);
}

void Box2DWorld::start()
{
    setRunning(true);
}

void Box2DWorld::resetVelocities()
{
    foreach (Box2DBody *body, mBodies)
        body->resetVelocities();
}

<|endoftext|>