<commit_before>/*****************************************************************************

  Licensed to Accellera Systems Initiative Inc. (Accellera) under one or
  more contributor license agreements.  See the NOTICE file distributed
  with this work for additional information regarding copyright ownership.
  Accellera licenses this file to you under the Apache License, Version 2.0
  (the "License"); you may not use this file except in compliance with the
  License.  You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
  implied.  See the License for the specific language governing
  permissions and limitations under the License.

 *****************************************************************************/

/*****************************************************************************

  sc_nbdefs.h -- Top level header file for arbitrary precision signed/unsigned
                 arithmetic. This file defines all the constants needed.

  Original Author: Ali Dasdan, Synopsys, Inc.

 *****************************************************************************/

/*****************************************************************************

  MODIFICATION LOG - modifiers, enter your name, affiliation, date and
  changes you are making here.

      Name, Affiliation, Date: Torsten Maehne, Berner Fachhochschule,
                               2016-09-24
  Description of Modification: Move constant definitions to the header so that
                               that their value is known at compile time.

 *****************************************************************************/

// $Log: sc_nbdefs.h,v $
// Revision 1.7  2011/02/18 20:19:15  acg
//  Andy Goodrich: updating Copyright notice.
//
// Revision 1.6  2011/02/18 20:09:34  acg
//  Philipp A. Hartmann: added alternative #define for Windows to guard.
//
// Revision 1.5  2011/01/20 16:52:20  acg
//  Andy Goodrich: changes for IEEE 1666 2011.
//
// Revision 1.4  2010/02/08 18:35:55  acg
//  Andy Goodrich: Philipp Hartmann's changes for Solaris and Linux 64.
//
// Revision 1.2  2009/05/22 16:06:29  acg
//  Andy Goodrich: process control updates.
//
// Revision 1.1.1.1  2006/12/15 20:20:05  acg
// SystemC 2.3
//
// Revision 1.3  2006/01/13 18:49:32  acg
// Added $Log command so that CVS check in comments are reproduced in the
// source.
//

#ifndef __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__
#define __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__

#include <stdint.h>

#include <climits>

namespace sc_dt
{

// ----------------------------------------------------------------------------
//  ENUM : sc_numrep
//
//  Enumeration of number representations for character string conversion.
// ----------------------------------------------------------------------------

enum sc_numrep
{
    SC_NOBASE = 0,
    SC_BIN = 2,
    SC_OCT = 8,
    SC_DEC = 10,
    SC_HEX = 16,
    SC_BIN_US,
    SC_BIN_SM,
    SC_OCT_US,
    SC_OCT_SM,
    SC_HEX_US,
    SC_HEX_SM,
    SC_CSD
};

// Sign of a number:
#define SC_NEG -1 // Negative number
#define SC_ZERO 0 // Zero
#define SC_POS 1 // Positive number
#define SC_NOSIGN 2 // Uninitialized sc_signed number

typedef unsigned char uchar;

// A small_type number is at least a char. Defining an int is probably
// better for alignment.
typedef int small_type;

// Attributes of a byte.
#define BITS_PER_BYTE 8
#define BYTE_RADIX 256
#define BYTE_MASK 255

// LOG2_BITS_PER_BYTE = log2(BITS_PER_BYTE), assuming that
// BITS_PER_BYTE is a power of 2.
#define LOG2_BITS_PER_BYTE 3

// Attributes of the unsigned long. These definitions are used mainly in
// the functions that are aware of the internal representation of
// digits, e.g., get/set_packed_rep().
#define BYTES_PER_DIGIT_TYPE 4
#define BITS_PER_DIGIT_TYPE 32

// Attributes of a digit, i.e., unsigned long less the overflow bits.
#define BYTES_PER_DIGIT 4
#define BITS_PER_DIGIT 30
#define DIGIT_RADIX (1ul << BITS_PER_DIGIT)
#define DIGIT_MASK (DIGIT_RADIX - 1)
// Make sure that BYTES_PER_DIGIT = ceil(BITS_PER_DIGIT / BITS_PER_BYTE).

// Similar attributes for the half of a digit. Note that
// HALF_DIGIT_RADIX is equal to the square root of DIGIT_RADIX. These
// definitions are used mainly in the multiplication routines.
#define BITS_PER_HALF_DIGIT (BITS_PER_DIGIT / 2)
#define HALF_DIGIT_RADIX (1ul << BITS_PER_HALF_DIGIT)
#define HALF_DIGIT_MASK (HALF_DIGIT_RADIX - 1)

// DIV_CEIL2(x, y) = ceil(x / y). x and y are positive numbers.
#define DIV_CEIL2(x, y) (((x) - 1) / (y) + 1)

// DIV_CEIL(x) = ceil(x / BITS_PER_DIGIT) = the number of digits to
// store x bits. x is a positive number.
#define DIV_CEIL(x) DIV_CEIL2(x, BITS_PER_DIGIT)

#ifdef SC_MAX_NBITS
static const int MAX_NDIGITS = DIV_CEIL(SC_MAX_NBITS) + 2;
// Consider a number with x bits another with y bits. The maximum
// number of bits happens when we multiply them. The result will have
// (x + y) bits. Assume that x + y <= SC_MAX_NBITS. Then, DIV_CEIL(x) +
// DIV_CEIL(y) <= DIV_CEIL(SC_MAX_NBITS) + 2. This is the reason for +2
// above. With this change, MAX_NDIGITS must be enough to hold the
// result of any operation.
#endif

// Support for "digit" vectors used to hold the values of sc_signed,
// sc_unsigned, sc_bv_base,  and sc_lv_base data types. This type is also used
// in the concatenation support. An sc_digit is currently an unsigned 32-bit
// quantity. The typedef used is an unsigned int, rather than an unsigned long,
// since the unsigned long data type varies in size between 32-bit and 64-bit
// machines.

typedef unsigned int sc_digit; // 32-bit unsigned integer

// Support for the long long type. This type is not in the standard
// but is usually supported by compilers.
#if defined(__x86_64__) || defined(__aarch64__)
typedef long long int64;
typedef unsigned long long uint64;
#else
typedef int64_t int64;
typedef uint64_t uint64;
#endif

static const uint64 UINT64_ZERO = 0ULL;
static const uint64 UINT64_ONE = 1ULL;
static const uint64 UINT64_32ONES = 0x00000000ffffffffULL;

// Bits per ...
// will be deleted in the future. Use numeric_limits instead
#define BITS_PER_CHAR 8
#define BITS_PER_INT (sizeof(int) * BITS_PER_CHAR)
#define BITS_PER_LONG (sizeof(long) * BITS_PER_CHAR)
#define BITS_PER_INT64 (sizeof(::sc_dt::int64) * BITS_PER_CHAR)
#define BITS_PER_UINT (sizeof(unsigned int) * BITS_PER_CHAR)
#define BITS_PER_ULONG (sizeof(unsigned long) * BITS_PER_CHAR)
#define BITS_PER_UINT64 (sizeof(::sc_dt::uint64) * BITS_PER_CHAR)

// Digits per ...
#define DIGITS_PER_CHAR 1
#define DIGITS_PER_INT ((BITS_PER_INT + 29) / 30)
#define DIGITS_PER_LONG ((BITS_PER_LONG + 29) / 30)
#define DIGITS_PER_INT64 ((BITS_PER_INT64 + 29) / 30)
#define DIGITS_PER_UINT ((BITS_PER_UINT + 29) / 30)
#define DIGITS_PER_ULONG ((BITS_PER_ULONG + 29) / 30)
#define DIGITS_PER_UINT64 ((BITS_PER_UINT64 + 29) / 30)

// Above, BITS_PER_X is mainly used for sc_signed, and BITS_PER_UX is
// mainly used for sc_unsigned.

static const small_type NB_DEFAULT_BASE = SC_DEC;

// For sc_int code:

typedef int64 int_type;
typedef uint64 uint_type;
#define SC_INTWIDTH 64
static const uint64 UINT_ZERO = UINT64_ZERO;
static const uint64 UINT_ONE = UINT64_ONE;

} // namespace sc_dt

#endif // __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__
<commit_msg>systemc: Fix builds on Power systems<commit_after>/*****************************************************************************

  Licensed to Accellera Systems Initiative Inc. (Accellera) under one or
  more contributor license agreements.  See the NOTICE file distributed
  with this work for additional information regarding copyright ownership.
  Accellera licenses this file to you under the Apache License, Version 2.0
  (the "License"); you may not use this file except in compliance with the
  License.  You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
  implied.  See the License for the specific language governing
  permissions and limitations under the License.

 *****************************************************************************/

/*****************************************************************************

  sc_nbdefs.h -- Top level header file for arbitrary precision signed/unsigned
                 arithmetic. This file defines all the constants needed.

  Original Author: Ali Dasdan, Synopsys, Inc.

 *****************************************************************************/

/*****************************************************************************

  MODIFICATION LOG - modifiers, enter your name, affiliation, date and
  changes you are making here.

      Name, Affiliation, Date: Torsten Maehne, Berner Fachhochschule,
                               2016-09-24
  Description of Modification: Move constant definitions to the header so that
                               that their value is known at compile time.

 *****************************************************************************/

// $Log: sc_nbdefs.h,v $
// Revision 1.7  2011/02/18 20:19:15  acg
//  Andy Goodrich: updating Copyright notice.
//
// Revision 1.6  2011/02/18 20:09:34  acg
//  Philipp A. Hartmann: added alternative #define for Windows to guard.
//
// Revision 1.5  2011/01/20 16:52:20  acg
//  Andy Goodrich: changes for IEEE 1666 2011.
//
// Revision 1.4  2010/02/08 18:35:55  acg
//  Andy Goodrich: Philipp Hartmann's changes for Solaris and Linux 64.
//
// Revision 1.2  2009/05/22 16:06:29  acg
//  Andy Goodrich: process control updates.
//
// Revision 1.1.1.1  2006/12/15 20:20:05  acg
// SystemC 2.3
//
// Revision 1.3  2006/01/13 18:49:32  acg
// Added $Log command so that CVS check in comments are reproduced in the
// source.
//

#ifndef __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__
#define __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__

#include <stdint.h>

#include <climits>

namespace sc_dt
{

// ----------------------------------------------------------------------------
//  ENUM : sc_numrep
//
//  Enumeration of number representations for character string conversion.
// ----------------------------------------------------------------------------

enum sc_numrep
{
    SC_NOBASE = 0,
    SC_BIN = 2,
    SC_OCT = 8,
    SC_DEC = 10,
    SC_HEX = 16,
    SC_BIN_US,
    SC_BIN_SM,
    SC_OCT_US,
    SC_OCT_SM,
    SC_HEX_US,
    SC_HEX_SM,
    SC_CSD
};

// Sign of a number:
#define SC_NEG -1 // Negative number
#define SC_ZERO 0 // Zero
#define SC_POS 1 // Positive number
#define SC_NOSIGN 2 // Uninitialized sc_signed number

typedef unsigned char uchar;

// A small_type number is at least a char. Defining an int is probably
// better for alignment.
typedef int small_type;

// Attributes of a byte.
#define BITS_PER_BYTE 8
#define BYTE_RADIX 256
#define BYTE_MASK 255

// LOG2_BITS_PER_BYTE = log2(BITS_PER_BYTE), assuming that
// BITS_PER_BYTE is a power of 2.
#define LOG2_BITS_PER_BYTE 3

// Attributes of the unsigned long. These definitions are used mainly in
// the functions that are aware of the internal representation of
// digits, e.g., get/set_packed_rep().
#define BYTES_PER_DIGIT_TYPE 4
#define BITS_PER_DIGIT_TYPE 32

// Attributes of a digit, i.e., unsigned long less the overflow bits.
#define BYTES_PER_DIGIT 4
#define BITS_PER_DIGIT 30
#define DIGIT_RADIX (1ul << BITS_PER_DIGIT)
#define DIGIT_MASK (DIGIT_RADIX - 1)
// Make sure that BYTES_PER_DIGIT = ceil(BITS_PER_DIGIT / BITS_PER_BYTE).

// Similar attributes for the half of a digit. Note that
// HALF_DIGIT_RADIX is equal to the square root of DIGIT_RADIX. These
// definitions are used mainly in the multiplication routines.
#define BITS_PER_HALF_DIGIT (BITS_PER_DIGIT / 2)
#define HALF_DIGIT_RADIX (1ul << BITS_PER_HALF_DIGIT)
#define HALF_DIGIT_MASK (HALF_DIGIT_RADIX - 1)

// DIV_CEIL2(x, y) = ceil(x / y). x and y are positive numbers.
#define DIV_CEIL2(x, y) (((x) - 1) / (y) + 1)

// DIV_CEIL(x) = ceil(x / BITS_PER_DIGIT) = the number of digits to
// store x bits. x is a positive number.
#define DIV_CEIL(x) DIV_CEIL2(x, BITS_PER_DIGIT)

#ifdef SC_MAX_NBITS
static const int MAX_NDIGITS = DIV_CEIL(SC_MAX_NBITS) + 2;
// Consider a number with x bits another with y bits. The maximum
// number of bits happens when we multiply them. The result will have
// (x + y) bits. Assume that x + y <= SC_MAX_NBITS. Then, DIV_CEIL(x) +
// DIV_CEIL(y) <= DIV_CEIL(SC_MAX_NBITS) + 2. This is the reason for +2
// above. With this change, MAX_NDIGITS must be enough to hold the
// result of any operation.
#endif

// Support for "digit" vectors used to hold the values of sc_signed,
// sc_unsigned, sc_bv_base,  and sc_lv_base data types. This type is also used
// in the concatenation support. An sc_digit is currently an unsigned 32-bit
// quantity. The typedef used is an unsigned int, rather than an unsigned long,
// since the unsigned long data type varies in size between 32-bit and 64-bit
// machines.

typedef unsigned int sc_digit; // 32-bit unsigned integer

// Support for the long long type. This type is not in the standard
// but is usually supported by compilers.
#if defined(__x86_64__) || defined(__aarch64__) || defined(__powerpc64__)
typedef long long int64;
typedef unsigned long long uint64;
#else
typedef int64_t int64;
typedef uint64_t uint64;
#endif

static const uint64 UINT64_ZERO = 0ULL;
static const uint64 UINT64_ONE = 1ULL;
static const uint64 UINT64_32ONES = 0x00000000ffffffffULL;

// Bits per ...
// will be deleted in the future. Use numeric_limits instead
#define BITS_PER_CHAR 8
#define BITS_PER_INT (sizeof(int) * BITS_PER_CHAR)
#define BITS_PER_LONG (sizeof(long) * BITS_PER_CHAR)
#define BITS_PER_INT64 (sizeof(::sc_dt::int64) * BITS_PER_CHAR)
#define BITS_PER_UINT (sizeof(unsigned int) * BITS_PER_CHAR)
#define BITS_PER_ULONG (sizeof(unsigned long) * BITS_PER_CHAR)
#define BITS_PER_UINT64 (sizeof(::sc_dt::uint64) * BITS_PER_CHAR)

// Digits per ...
#define DIGITS_PER_CHAR 1
#define DIGITS_PER_INT ((BITS_PER_INT + 29) / 30)
#define DIGITS_PER_LONG ((BITS_PER_LONG + 29) / 30)
#define DIGITS_PER_INT64 ((BITS_PER_INT64 + 29) / 30)
#define DIGITS_PER_UINT ((BITS_PER_UINT + 29) / 30)
#define DIGITS_PER_ULONG ((BITS_PER_ULONG + 29) / 30)
#define DIGITS_PER_UINT64 ((BITS_PER_UINT64 + 29) / 30)

// Above, BITS_PER_X is mainly used for sc_signed, and BITS_PER_UX is
// mainly used for sc_unsigned.

static const small_type NB_DEFAULT_BASE = SC_DEC;

// For sc_int code:

typedef int64 int_type;
typedef uint64 uint_type;
#define SC_INTWIDTH 64
static const uint64 UINT_ZERO = UINT64_ZERO;
static const uint64 UINT_ONE = UINT64_ONE;

} // namespace sc_dt

#endif // __SYSTEMC_EXT_DT_INT_SC_NBDEFS_HH__
<|endoftext|>


<commit_before>#include <ros/ros.h>

#include <moveit/move_group_interface/move_group.h>

#include <moveit/warehouse/planning_scene_storage.h>

#include <boost/program_options.hpp>

#include <view_controller_msgs/CameraPlacement.h>

#include "moveit_recorder/TrajectoryRetimer.h"
#include "moveit_recorder/AnimationRecorder.h"

#include <moveit_recorder/AnimationRequest.h>

bool static ready;

void animationResponseCallback(const boost::shared_ptr<std_msgs::Bool const>& msg)
{
  ready = msg->data;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "playback");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  sleep(20); // to let RVIZ come up

  boost::program_options::options_description desc;
  desc.add_options()
    ("help", "Show help message")
    ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
    ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");

  boost::program_options::variables_map vm;
  boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
  boost::program_options::store(po, vm);
  boost::program_options::notify(vm);

  if (vm.count("help") || argc == 1) // show help if no parameters passed
  {
    std::cout << desc << std::endl;
    return 1;
  }
  try
  {
    //connect to the DB
    std::string host = vm.count("host") ? vm["host"].as<std::string>() : "";
    size_t port = vm.count("port") ? vm["port"].as<std::size_t>() : 0;
    moveit_warehouse::PlanningSceneStorage pss(host, port);

    ROS_INFO("Connected to Warehouse DB at host (%s) and port (%d)", host.c_str(), (int)port);

    // response sub
    ros::Subscriber animation_sub = node_handle.subscribe("animation_response", 1, animationResponseCallback);
    while(animation_sub.getNumPublishers() < 1)
    {
      ros::WallDuration sleep_t(0.5);
      ROS_INFO("[Playback] Not enough publishers to \"%s\" topic...", "animation_response");
      sleep_t.sleep();
    }

    // request pub
    ros::Publisher animation_pub = node_handle.advertise<moveit_recorder::AnimationRequest>("animation_request",1);
    while(animation_pub.getNumSubscribers() < 1)
    {
      ros::WallDuration sleep_t(0.5);
      ROS_INFO("[Playback] Not enough subscribers to \"%s\" topic... ", "animation_request");
      sleep_t.sleep();
    }

    // control the camera
    view_controller_msgs::CameraPlacement view_msg;
    view_msg.target_frame = "base_link";
    view_msg.interpolation_mode = view_controller_msgs::CameraPlacement::SPHERICAL;
    view_msg.time_from_start = ros::Duration(0.5);
    std_msgs::Header header;
    header.stamp = ros::Time::now();
    header.frame_id = "base_link";
    view_msg.eye.header = header;
    view_msg.eye.point.x = 2.5;
    view_msg.eye.point.y = -1;
    view_msg.eye.point.z = 2;
    view_msg.focus.header = header;
    view_msg.focus.point.x = -0.21941;
    view_msg.focus.point.y = 0.27017;
    view_msg.focus.point.z = 0.52922;
    view_msg.up.header = header;
    view_msg.up.vector.x = 0;
    view_msg.up.vector.y = 0;
    view_msg.up.vector.z = 1;
    view_msg.mouse_interaction_mode = view_controller_msgs::CameraPlacement::NO_CHANGE;
    view_msg.interaction_disabled = true;
    view_msg.allow_free_yaw_axis = false;

    // ask the warehouse for the scenes
    std::vector<std::string> ps_names;
    pss.getPlanningSceneNames( ps_names );
    ROS_INFO("%d available scenes to display", (int)ps_names.size());

    // iterate over scenes
    std::vector<std::string>::iterator scene_name = ps_names.begin();
    for(; scene_name!=ps_names.end(); ++scene_name)
    {
      ROS_INFO("Retrieving scene %s", scene_name->c_str());
      moveit_warehouse::PlanningSceneWithMetadata pswm;
      pss.getPlanningScene(pswm, *scene_name);
      moveit_msgs::PlanningScene ps_msg = static_cast<const moveit_msgs::PlanningScene&>(*pswm);

      // ask qarehosue for the queries
      std::vector<std::string> pq_names;
      pss.getPlanningQueriesNames( pq_names, *scene_name);
      ROS_INFO("%d available queries to display", (int)pq_names.size());

      // iterate over the queries
      std::vector<std::string>::iterator query_name = pq_names.begin();
      for(; query_name!=pq_names.end(); ++query_name)
      {
        ROS_INFO("Retrieving query %s", query_name->c_str());
        moveit_warehouse::MotionPlanRequestWithMetadata mprwm;
        pss.getPlanningQuery(mprwm, *scene_name, *query_name);
        moveit_msgs::MotionPlanRequest mpr_msg = static_cast<const moveit_msgs::MotionPlanRequest&>(*mprwm);

        // ask warehouse for stored trajectories
        std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
        pss.getPlanningResults(planning_results, *scene_name, *query_name);
        ROS_INFO("Loaded %d trajectories", (int)planning_results.size());

        // animate each trajectory
        std::vector<moveit_warehouse::RobotTrajectoryWithMetadata>::iterator traj_w_mdata = planning_results.begin();
        for(; traj_w_mdata!=planning_results.end(); ++traj_w_mdata)
        {

          moveit_msgs::RobotTrajectory rt_msg;
          rt_msg = static_cast<const moveit_msgs::RobotTrajectory&>(**traj_w_mdata);
          // retime it
          TrajectoryRetimer retimer( "robot_description", mpr_msg.group_name );
          retimer.configure(ps_msg, mpr_msg);
          bool result = retimer.retime(rt_msg);
          ROS_INFO("Retiming success? %s", result? "yes" : "no" );

          moveit_recorder::AnimationRequest req;
          req.camera_placement = view_msg;
          req.planning_scene = ps_msg;
          req.motion_plan_request = mpr_msg;
          req.robot_trajectory = rt_msg;
          req.filepath.data = "/tmp/video.ogv";

          animation_pub.publish(req);
          usleep(1000);
          ready = false;
          while(ros::ok() && !ready)
          {
            ros::spinOnce(); //updates the ready status
            usleep(1000);
          }
          ROS_ERROR("RECORDING DONE!");
        }//traj
      }//query
    }//scene
  }
  catch(mongo_ros::DbConnectException &ex)
  {
    ROS_ERROR_STREAM("Unable to connect to warehouse. If you just created the database, it could take a while for initial setup. Please try to run the benchmark again."
        << std::endl << ex.what());
  }

  ROS_ERROR("Successfully performed trajectory playback");
  ros::shutdown();
  return 0;
}
<commit_msg>read views from bag file; correctly loop over all views and visualize<commit_after>#include <ros/ros.h>

#include <moveit/move_group_interface/move_group.h>

#include <moveit/warehouse/planning_scene_storage.h>

#include <boost/program_options.hpp>

#include <view_controller_msgs/CameraPlacement.h>

#include "moveit_recorder/TrajectoryRetimer.h"
#include "moveit_recorder/AnimationRecorder.h"

#include <moveit_recorder/AnimationRequest.h>
#include <rosbag/bag.h>
#include <rosbag/query.h>
#include <rosbag/view.h>

bool static ready;

void animationResponseCallback(const boost::shared_ptr<std_msgs::Bool const>& msg)
{
  ready = msg->data;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "playback");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  sleep(20); // to let RVIZ come up

  boost::program_options::options_description desc;
  desc.add_options()
    ("help", "Show help message")
    ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
    ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.")
    ("views",boost::program_options::value<std::string>(), "Bag file for viewpoints");

  boost::program_options::variables_map vm;
  boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
  boost::program_options::store(po, vm);
  boost::program_options::notify(vm);

  if (vm.count("help") || argc == 1) // show help if no parameters passed
  {
    std::cout << desc << std::endl;
    return 1;
  }
  try
  {
    //connect to the DB
    std::string host = vm.count("host") ? vm["host"].as<std::string>() : "";
    size_t port = vm.count("port") ? vm["port"].as<std::size_t>() : 0;
    moveit_warehouse::PlanningSceneStorage pss(host, port);

    ROS_INFO("Connected to Warehouse DB at host (%s) and port (%d)", host.c_str(), (int)port);

    //load the viewpoints
    std::string bagfilename = vm.count("views") ? vm["views"].as<std::string>() : "";
    std::vector<view_controller_msgs::CameraPlacement> views;
    rosbag::Bag viewbag;
    viewbag.open(bagfilename, rosbag::bagmode::Read);
    std::vector<std::string> topics; topics.push_back("viewpoints");
    rosbag::View view_t(viewbag, rosbag::TopicQuery(topics));
    BOOST_FOREACH(rosbag::MessageInstance const m, view_t)
    {
      view_controller_msgs::CameraPlacement::ConstPtr i = m.instantiate<view_controller_msgs::CameraPlacement>();
      if (i != NULL)
        views.push_back(*i);
    }
    viewbag.close();
    ROS_INFO("%d views loaded",(int)views.size());

    // response sub
    ros::Subscriber animation_sub = node_handle.subscribe("animation_response", 1, animationResponseCallback);
    while(animation_sub.getNumPublishers() < 1)
    {
      ros::WallDuration sleep_t(0.5);
      ROS_INFO("[Playback] Not enough publishers to \"%s\" topic...", "animation_response");
      sleep_t.sleep();
    }

    // request pub
    ros::Publisher animation_pub = node_handle.advertise<moveit_recorder::AnimationRequest>("animation_request",1);
    while(animation_pub.getNumSubscribers() < 1)
    {
      ros::WallDuration sleep_t(0.5);
      ROS_INFO("[Playback] Not enough subscribers to \"%s\" topic... ", "animation_request");
      sleep_t.sleep();
    }

    // ask the warehouse for the scenes
    std::vector<std::string> ps_names;
    pss.getPlanningSceneNames( ps_names );
    ROS_INFO("%d available scenes to display", (int)ps_names.size());

    // iterate over scenes
    std::vector<std::string>::iterator scene_name = ps_names.begin();
    for(; scene_name!=ps_names.end(); ++scene_name)
    {
      ROS_INFO("Retrieving scene %s", scene_name->c_str());
      moveit_warehouse::PlanningSceneWithMetadata pswm;
      pss.getPlanningScene(pswm, *scene_name);
      moveit_msgs::PlanningScene ps_msg = static_cast<const moveit_msgs::PlanningScene&>(*pswm);

      // ask qarehosue for the queries
      std::vector<std::string> pq_names;
      pss.getPlanningQueriesNames( pq_names, *scene_name);
      ROS_INFO("%d available queries to display", (int)pq_names.size());

      // iterate over the queries
      std::vector<std::string>::iterator query_name = pq_names.begin();
      for(; query_name!=pq_names.end(); ++query_name)
      {
        ROS_INFO("Retrieving query %s", query_name->c_str());
        moveit_warehouse::MotionPlanRequestWithMetadata mprwm;
        pss.getPlanningQuery(mprwm, *scene_name, *query_name);
        moveit_msgs::MotionPlanRequest mpr_msg = static_cast<const moveit_msgs::MotionPlanRequest&>(*mprwm);

        // ask warehouse for stored trajectories
        std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
        pss.getPlanningResults(planning_results, *scene_name, *query_name);
        ROS_INFO("Loaded %d trajectories", (int)planning_results.size());

        // animate each trajectory
        std::vector<moveit_warehouse::RobotTrajectoryWithMetadata>::iterator traj_w_mdata = planning_results.begin();
        for(; traj_w_mdata!=planning_results.end(); ++traj_w_mdata)
        {
          moveit_msgs::RobotTrajectory rt_msg;
          rt_msg = static_cast<const moveit_msgs::RobotTrajectory&>(**traj_w_mdata);
          // retime it
          TrajectoryRetimer retimer( "robot_description", mpr_msg.group_name );
          retimer.configure(ps_msg, mpr_msg);
          bool result = retimer.retime(rt_msg);
          ROS_INFO("Retiming success? %s", result? "yes" : "no" );

          std::vector<view_controller_msgs::CameraPlacement>::iterator view_msg;
          for(view_msg=views.begin(); view_msg!=views.end(); ++view_msg)
          {
            moveit_recorder::AnimationRequest req;

            view_msg->time_from_start = ros::Duration(0.1);
            ros::Time t_now = ros::Time::now();
            view_msg->eye.header.stamp = t_now;
            view_msg->focus.header.stamp = t_now;
            view_msg->up.header.stamp = t_now;

            req.camera_placement = *view_msg;
            req.planning_scene = ps_msg;
            req.motion_plan_request = mpr_msg;
            req.robot_trajectory = rt_msg;
            req.filepath.data = "/tmp/video.ogv";

            animation_pub.publish(req);
            usleep(1000);
            ready = false;
            while(ros::ok() && !ready)
            {
              ros::spinOnce(); //updates the ready status
              usleep(1000);
            }
            ROS_INFO("RECORDING DONE!");
            sleep(2);
          }//view
        }//traj
      }//query
    }//scene
  }
  catch(mongo_ros::DbConnectException &ex)
  {
    ROS_ERROR_STREAM("Unable to connect to warehouse. If you just created the database, it could take a while for initial setup. Please try to run the benchmark again."
        << std::endl << ex.what());
  }

  ROS_ERROR("Successfully performed trajectory playback");
  ros::shutdown();
  return 0;
}
<|endoftext|>


<commit_before>#include "position.h"
#include <cstring>
#include <cstdlib>
#include <stdexcept>

namespace {

Piece translate_fen_piece(char c) {
    Color color = std::isupper(c) ? WHITE : BLACK;
    c = std::tolower(c);
    PieceKind piece;
    switch (c) {
        case 'p': piece = PAWN; break;
        case 'n': piece = KNIGHT; break;
        case 'b': piece = BISHOP; break;
        case 'r': piece = ROOK; break;
        case 'q': piece = QUEEN; break;
        case 'k': piece = KING; break;
        default: throw std::runtime_error{"Invalid piece"};
    }
    return Piece{color, piece};
}

} // ~anonymous namespace

Position::Position() noexcept
    : moves(1u)
    , halfmoves(0u)
    , wtm(1u)
    , epsq(Position::ENPASSANT_NONE)
    , castle(Position::CASTLE_NONE)
{
    boards.fill(0ull);
    sidemask.fill(0ull);
    sq2p.fill(NO_PIECE);
}

Position Position::from_fen(std::string_view fen) {
    Position position;
    auto it = fen.begin();
    const auto last = fen.end();

    auto expect = [last](std::string_view::iterator it, char c) {
        if (it == last || *it != c) {
            std::string msg = "Expected '";
            msg += c;
            msg += "', received '";
            if (it == last) {
                msg += "<eof>";
            } else {
                msg += *it;
            }
            msg += "'";
            throw std::runtime_error(msg.c_str());
        }
        ++it;
        return it;
    };

    for (int rank = 7; rank >= 0; --rank) {
        for (int file = 0; file < 8; ++file) {
            if (it == last) {
                throw std::runtime_error("Board specification too short");
            }
            char c = *it++;
            if (c >= '1' && c <= '8') {
                file += c - '0' - 1;
                if (file >= 8) {
                    throw std::runtime_error("Invalid board specification");
                }
            } else {
                Piece piece = translate_fen_piece(c);
                Square sq{static_cast<u8>(file), static_cast<u8>(rank)};
                position.sq2p[sq.value()] = piece;
                position.sidemask[piece.color()] |= sq.mask();
                if (piece.kind() == KING) {
                    // position.kings[piece.color()] = sq.value();
                    position.kings[piece.color()] = sq;
                } else {
                    position.boards[piece.value()] |= sq.mask();
                }
            }
        }
        if (it < last && *it == '/') {
            ++it;
        }
    }
    it = expect(it, ' ');

    // Active Color
    if (it == last) {
        throw std::runtime_error("Expected color specification");
    }
    switch (*it++) {
        case 'W':
        case 'w':
            position._set_white_to_move(true);
            break;
        case 'B':
        case 'b':
            position._set_white_to_move(false);
            break;
        default:
            throw std::runtime_error("Invalid character in color specification");
    }
    it = expect(it, ' ');

    // Castling Availability
    if (it == last) {
        throw std::runtime_error("Expected castling availability specification");
    } else if (*it == '-') {
        position._set_castle_flags(Position::CASTLE_NONE);
        ++it;
    } else {
        u8 flags = 0;
        while (it < last && *it != ' ') {
            char c = *it++;
            switch (c) {
            case 'K':
                flags |= Position::CASTLE_WHITE_KING_SIDE;
                break;
            case 'k':
                flags |= Position::CASTLE_BLACK_KING_SIDE;
                break;
            case 'Q':
                flags |= Position::CASTLE_WHITE_QUEEN_SIDE;
                break;
            case 'q':
                flags |= Position::CASTLE_BLACK_QUEEN_SIDE;
                break;
            default:
                throw std::runtime_error("Invalid character in castling specification");
            }
        }
        position._set_castle_flags(flags);
    }
    it = expect(it, ' ');

    // En passant Target Square
    if (it == last) {
        throw std::runtime_error("Expected en passant target square");
    } else if (*it == '-') {
        position._set_enpassant_square(Position::ENPASSANT_NONE);
        ++it;
    } else {
        char c = *it++;
        if (!(c >= 'a' && c <= 'h')) {
            throw std::runtime_error("Invalid file for enpassant target square");
        }
        u8 file = c - 'a';
        u8 rank = *it++ - '1';
        assert(file >= 0 && file <= 7);
        assert(rank >= 0 && rank <= 7);
        Square square{file, rank};
        if (rank == RANK_3) {
            assert(square.value() >= A3 && square.value() <= H3);
            position._set_enpassant_square(square.value() - A3);
        } else if (rank == RANK_6) {
            assert(square.value() >= A6 && square.value() <= H6);
            position._set_enpassant_square(square.value() - A6);
        } else {
            throw std::runtime_error("Invalid rank for enpassant target square");
        }
    }

    // may or may not have halfmove and move specifications
    bool has_halfmove = it != last && *it++ == ' ';
    if (has_halfmove && it != last) {
        // Halfmove spec (50-move rule)
        while (it != last && *it != ' ') {
            if (*it < '0' && *it > '9') {
                throw std::runtime_error("Invalid halfmove specification");
            }
            position.halfmoves *= 10;
            position.halfmoves += *it - '0';
            ++it;
        }

        // assuming that if halfmove is there then move spec is also there
        it = expect(it, ' ');
        position.moves = 0;
        while (it != last && *it != ' ') {
            if (*it < '0' && *it > '9') {
                throw std::runtime_error("Invalid move specification");
            }
            position.moves *= 10;
            position.moves += *it - '0';
            ++it;
        }
    }

    return position;
}

[[nodiscard]]
constexpr u8 rook_square_to_castle_flag(Square square) noexcept {
    switch (square.value()) {
        case A8: return Position::CASTLE_BLACK_QUEEN_SIDE;
        case H8: return Position::CASTLE_BLACK_KING_SIDE;
        case A1: return Position::CASTLE_WHITE_QUEEN_SIDE;
        case H1: return Position::CASTLE_WHITE_KING_SIDE;
        default: return 0;
    }
}

[[nodiscard]]
constexpr bool is_rank2(u8 side, Square square) noexcept {
    constexpr u64 SECOND_RANK{0xff00ull};
    constexpr u64 SEVENTH_RANK{0xff000000000000ull};
    u64 mask = side == WHITE ? SECOND_RANK : SEVENTH_RANK;
    return square.mask() & mask;
}

[[nodiscard]]
constexpr bool is_rank7(u8 side, Square square) noexcept {
    return is_rank2(side ^ 1, square);
}

[[nodiscard]]
constexpr bool is_enpassant_square(u8 side, Square square) noexcept {
    constexpr u64 WHITE_ENPASSANT_SQUARES{0x00000000ff000000ull};
    constexpr u64 BLACK_ENPASSANT_SQUARES{0x000000ff00000000ull};
    u64 mask = side == WHITE ? WHITE_ENPASSANT_SQUARES : BLACK_ENPASSANT_SQUARES;
    return square.mask() & mask;
}

void Position::make_move(Savepos& sp, Move move) noexcept {
    assert(wtm == WHITE || wtm == BLACK);
    const Color side = static_cast<Color>(wtm);
    const Color contra = static_cast<Color>(side ^ 1);
    const Square from = move.from();
    const Square to = move.to();
    const Piece piece = piece_on_square(from);
    const Piece captured = piece_on_square(to);
    const Move::Flags flags = move.flags();
    const PieceKind kind = piece.kind();
    u64* board = kind != KING ? &boards[piece.value()] : 0;

    assert(to != from);
    assert(captured.kind() != KING);
    assert(piece.color() == side);
    assert(captured.empty() || captured.color() == contra);

    sp.halfmoves = halfmoves;
    sp.epsq = epsq;
    sp.castle = castle;
    sp.capture = captured;

    if (flags == Move::Flags::NONE) {
        if (board) {
            *board &= ~from.mask();
            *board |= to.mask();
        } else {
            kings[side] = to; // to.value();
            if (side == WHITE) {
                castle &= ~Position::CASTLE_WHITE;
            } else {
                castle &= ~Position::CASTLE_BLACK;
            }
        }
        sq2p[from.value()] = NO_PIECE;
        sq2p[to.value()] = piece;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();

        if (!captured.empty()) {
            boards[captured.value()] &= ~to.mask();
            sidemask[contra] &= ~to.mask();
            castle &= ~rook_square_to_castle_flag(to);
        } else if (kind == PAWN && is_rank2(side, from) && is_enpassant_square(side, to)) {
            _set_enpassant_square(to.value());
            assert((to.value() >= A3 && to.value() <= H3) || (to.value() >= A6 && to.value() <= H6));
        }

        if (kind == ROOK) {
            castle &= ~rook_square_to_castle_flag(from);
        }
    } else if (flags == Move::Flags::ENPASSANT) {
        const Square target{epsq};
        assert(kind == PAWN);
        assert(captured.empty());
        *board &= ~from.mask();
        *board |= to.mask();
        board[Piece(contra, PAWN).value()] &= ~target.mask();
        sq2p[target.value()] = NO_PIECE;
        sq2p[from.value()] = NO_PIECE;
        sq2p[to.value()] = piece;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();
        sidemask[contra] &= ~target.mask();
    } else if (flags == Move::Flags::PROMOTION) {
        const PieceKind promotion_kind = move.promotion();
        const Piece promotion_piece = Piece(side, promotion_kind);
        assert(kind == PAWN);
        *board &= ~from.mask();
        boards[promotion_piece.value()] |= to.mask();
        sq2p[to.value()] = promotion_piece;
        sq2p[from.value()] = NO_PIECE;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();
        if (!captured.empty()) {
            boards[captured.value()] &= ~to.mask();
            sidemask[contra] &= ~to.mask();
            castle &= ~rook_square_to_castle_flag(to);
        }
     } else if (flags == Move::Flags::CASTLE) {
         assert(kind == KING);
         u64* rooks = &boards[Piece(side, ROOK).value()];
         Square ksq;
         Square rsq;
         switch (to.value()) {
             case H1:
                 ksq = Square(G1);
                 rsq = Square(F1);
             case A1:
                 ksq = Square(C1);
                 rsq = Square(D1);
             case H8:
                 ksq = Square(G8);
                 rsq = Square(F8);
             case A8:
                 ksq = Square(C8);
                 rsq = Square(D8);
             default:
                 assert(0);
                 __builtin_unreachable();
         }
         kings[side] = ksq;
         *rooks &= ~to.mask();
         *rooks |= rsq.mask();

         sq2p[from.value()] = NO_PIECE;
         sq2p[to.value()] = NO_PIECE;
         sq2p[ksq.value()] = Piece(side, KING);
         sq2p[rsq.value()] = Piece(side, ROOK);
         sidemask[side] &= ~(to.mask() | from.mask());
         sidemask[side] |= ksq.mask() | rsq.mask();
     } else {
         assert(0);
         __builtin_unreachable();
     }
}

void Position::undo_move(const Savepos& sp, Move move) noexcept {

}
<commit_msg>First cut at make_move()<commit_after>#include "position.h"
#include <cstring>
#include <cstdlib>
#include <stdexcept>

namespace {

Piece translate_fen_piece(char c) {
    Color color = std::isupper(c) ? WHITE : BLACK;
    c = std::tolower(c);
    PieceKind piece;
    switch (c) {
        case 'p': piece = PAWN; break;
        case 'n': piece = KNIGHT; break;
        case 'b': piece = BISHOP; break;
        case 'r': piece = ROOK; break;
        case 'q': piece = QUEEN; break;
        case 'k': piece = KING; break;
        default: throw std::runtime_error{"Invalid piece"};
    }
    return Piece{color, piece};
}

} // ~anonymous namespace

Position::Position() noexcept
    : moves(1u)
    , halfmoves(0u)
    , wtm(1u)
    , epsq(Position::ENPASSANT_NONE)
    , castle(Position::CASTLE_NONE)
{
    boards.fill(0ull);
    sidemask.fill(0ull);
    sq2p.fill(NO_PIECE);
}

Position Position::from_fen(std::string_view fen) {
    Position position;
    auto it = fen.begin();
    const auto last = fen.end();

    auto expect = [last](std::string_view::iterator it, char c) {
        if (it == last || *it != c) {
            std::string msg = "Expected '";
            msg += c;
            msg += "', received '";
            if (it == last) {
                msg += "<eof>";
            } else {
                msg += *it;
            }
            msg += "'";
            throw std::runtime_error(msg.c_str());
        }
        ++it;
        return it;
    };

    for (int rank = 7; rank >= 0; --rank) {
        for (int file = 0; file < 8; ++file) {
            if (it == last) {
                throw std::runtime_error("Board specification too short");
            }
            char c = *it++;
            if (c >= '1' && c <= '8') {
                file += c - '0' - 1;
                if (file >= 8) {
                    throw std::runtime_error("Invalid board specification");
                }
            } else {
                Piece piece = translate_fen_piece(c);
                Square sq{static_cast<u8>(file), static_cast<u8>(rank)};
                position.sq2p[sq.value()] = piece;
                position.sidemask[piece.color()] |= sq.mask();
                if (piece.kind() == KING) {
                    // position.kings[piece.color()] = sq.value();
                    position.kings[piece.color()] = sq;
                } else {
                    position.boards[piece.value()] |= sq.mask();
                }
            }
        }
        if (it < last && *it == '/') {
            ++it;
        }
    }
    it = expect(it, ' ');

    // Active Color
    if (it == last) {
        throw std::runtime_error("Expected color specification");
    }
    switch (*it++) {
        case 'W':
        case 'w':
            position._set_white_to_move(true);
            break;
        case 'B':
        case 'b':
            position._set_white_to_move(false);
            break;
        default:
            throw std::runtime_error("Invalid character in color specification");
    }
    it = expect(it, ' ');

    // Castling Availability
    if (it == last) {
        throw std::runtime_error("Expected castling availability specification");
    } else if (*it == '-') {
        position._set_castle_flags(Position::CASTLE_NONE);
        ++it;
    } else {
        u8 flags = 0;
        while (it < last && *it != ' ') {
            char c = *it++;
            switch (c) {
            case 'K':
                flags |= Position::CASTLE_WHITE_KING_SIDE;
                break;
            case 'k':
                flags |= Position::CASTLE_BLACK_KING_SIDE;
                break;
            case 'Q':
                flags |= Position::CASTLE_WHITE_QUEEN_SIDE;
                break;
            case 'q':
                flags |= Position::CASTLE_BLACK_QUEEN_SIDE;
                break;
            default:
                throw std::runtime_error("Invalid character in castling specification");
            }
        }
        position._set_castle_flags(flags);
    }
    it = expect(it, ' ');

    // En passant Target Square
    if (it == last) {
        throw std::runtime_error("Expected en passant target square");
    } else if (*it == '-') {
        position._set_enpassant_square(Position::ENPASSANT_NONE);
        ++it;
    } else {
        char c = *it++;
        if (!(c >= 'a' && c <= 'h')) {
            throw std::runtime_error("Invalid file for enpassant target square");
        }
        u8 file = c - 'a';
        u8 rank = *it++ - '1';
        assert(file >= 0 && file <= 7);
        assert(rank >= 0 && rank <= 7);
        Square square{file, rank};
        if (rank == RANK_3) {
            assert(square.value() >= A3 && square.value() <= H3);
            position._set_enpassant_square(square.value() - A3);
        } else if (rank == RANK_6) {
            assert(square.value() >= A6 && square.value() <= H6);
            position._set_enpassant_square(square.value() - A6);
        } else {
            throw std::runtime_error("Invalid rank for enpassant target square");
        }
    }

    // may or may not have halfmove and move specifications
    bool has_halfmove = it != last && *it++ == ' ';
    if (has_halfmove && it != last) {
        // Halfmove spec (50-move rule)
        while (it != last && *it != ' ') {
            if (*it < '0' && *it > '9') {
                throw std::runtime_error("Invalid halfmove specification");
            }
            position.halfmoves *= 10;
            position.halfmoves += *it - '0';
            ++it;
        }

        // assuming that if halfmove is there then move spec is also there
        it = expect(it, ' ');
        position.moves = 0;
        while (it != last && *it != ' ') {
            if (*it < '0' && *it > '9') {
                throw std::runtime_error("Invalid move specification");
            }
            position.moves *= 10;
            position.moves += *it - '0';
            ++it;
        }
    }

    return position;
}

[[nodiscard]]
constexpr u8 rook_square_to_castle_flag(Square square) noexcept {
    switch (square.value()) {
        case A8: return Position::CASTLE_BLACK_QUEEN_SIDE;
        case H8: return Position::CASTLE_BLACK_KING_SIDE;
        case A1: return Position::CASTLE_WHITE_QUEEN_SIDE;
        case H1: return Position::CASTLE_WHITE_KING_SIDE;
        default: return 0;
    }
}

[[nodiscard]]
constexpr bool is_rank2(u8 side, Square square) noexcept {
    constexpr u64 SECOND_RANK{0xff00ull};
    constexpr u64 SEVENTH_RANK{0xff000000000000ull};
    u64 mask = side == WHITE ? SECOND_RANK : SEVENTH_RANK;
    return square.mask() & mask;
}

[[nodiscard]]
constexpr bool is_rank7(u8 side, Square square) noexcept {
    return is_rank2(side ^ 1, square);
}

[[nodiscard]]
constexpr bool is_enpassant_square(u8 side, Square square) noexcept {
    constexpr u64 WHITE_ENPASSANT_SQUARES{0x00000000ff000000ull};
    constexpr u64 BLACK_ENPASSANT_SQUARES{0x000000ff00000000ull};
    u64 mask = side == WHITE ? WHITE_ENPASSANT_SQUARES : BLACK_ENPASSANT_SQUARES;
    return square.mask() & mask;
}

void Position::make_move(Savepos& sp, Move move) noexcept {
    assert(wtm == WHITE || wtm == BLACK);
    const Color side = static_cast<Color>(wtm);
    const Color contra = static_cast<Color>(side ^ 1);
    const Square from = move.from();
    const Square to = move.to();
    const Piece piece = piece_on_square(from);
    const Piece captured = piece_on_square(to);
    const Move::Flags flags = move.flags();
    const PieceKind kind = piece.kind();
    u64* board = kind != KING ? &boards[piece.value()] : 0;

    assert(to != from);
    assert(captured.kind() != KING);
    assert(piece.color() == side);
    assert(captured.empty() || captured.color() == contra);

    sp.halfmoves = halfmoves;
    sp.epsq = epsq;
    sp.castle = castle;
    sp.capture = captured;

    if (flags == Move::Flags::NONE) {
        if (board) {
            *board &= ~from.mask();
            *board |= to.mask();
        } else {
            kings[side] = to; // to.value();
            if (side == WHITE) {
                castle &= ~Position::CASTLE_WHITE;
            } else {
                castle &= ~Position::CASTLE_BLACK;
            }
        }
        sq2p[from.value()] = NO_PIECE;
        sq2p[to.value()] = piece;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();

        if (!captured.empty()) {
            boards[captured.value()] &= ~to.mask();
            sidemask[contra] &= ~to.mask();
            castle &= ~rook_square_to_castle_flag(to);
        } else if (kind == PAWN && is_rank2(side, from) && is_enpassant_square(side, to)) {
            _set_enpassant_square(to.value());
            assert((to.value() >= A3 && to.value() <= H3) || (to.value() >= A6 && to.value() <= H6));
        }

        if (kind == ROOK) {
            castle &= ~rook_square_to_castle_flag(from);
        }
    } else if (flags == Move::Flags::ENPASSANT) {
        const Square target{epsq};
        assert(kind == PAWN);
        assert(captured.empty());
        *board &= ~from.mask();
        *board |= to.mask();
        board[Piece(contra, PAWN).value()] &= ~target.mask();
        sq2p[target.value()] = NO_PIECE;
        sq2p[from.value()] = NO_PIECE;
        sq2p[to.value()] = piece;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();
        sidemask[contra] &= ~target.mask();
    } else if (flags == Move::Flags::PROMOTION) {
        const PieceKind promotion_kind = move.promotion();
        const Piece promotion_piece = Piece(side, promotion_kind);
        assert(kind == PAWN);
        *board &= ~from.mask();
        boards[promotion_piece.value()] |= to.mask();
        sq2p[to.value()] = promotion_piece;
        sq2p[from.value()] = NO_PIECE;
        sidemask[side] &= ~from.mask();
        sidemask[side] |= to.mask();
        if (!captured.empty()) {
            boards[captured.value()] &= ~to.mask();
            sidemask[contra] &= ~to.mask();
            castle &= ~rook_square_to_castle_flag(to);
        }
     } else if (flags == Move::Flags::CASTLE) {
         assert(kind == KING);
         u64* rooks = &boards[Piece(side, ROOK).value()];
         Square ksq;
         Square rsq;
         switch (to.value()) {
             case H1:
                 ksq = Square(G1);
                 rsq = Square(F1);
             case A1:
                 ksq = Square(C1);
                 rsq = Square(D1);
             case H8:
                 ksq = Square(G8);
                 rsq = Square(F8);
             case A8:
                 ksq = Square(C8);
                 rsq = Square(D8);
             default:
                 assert(0);
                 __builtin_unreachable();
         }
         kings[side] = ksq;
         *rooks &= ~to.mask();
         *rooks |= rsq.mask();

         sq2p[from.value()] = NO_PIECE;
         sq2p[to.value()] = NO_PIECE;
         sq2p[ksq.value()] = Piece(side, KING);
         sq2p[rsq.value()] = Piece(side, ROOK);
         sidemask[side] &= ~(to.mask() | from.mask());
         sidemask[side] |= ksq.mask() | rsq.mask();
        if (side == WHITE) {
            castle &= ~Position::CASTLE_WHITE;
        } else {
            castle &= ~Position::CASTLE_BLACK;
        }
     } else {
         assert(0);
         __builtin_unreachable();
     }

    wtm = contra;
    if (kind == PAWN || !captured.empty()) {
        halfmoves = 0;
    } else {
        ++halfmoves;
    }
    ++moves;
}

void Position::undo_move(const Savepos& sp, Move move) noexcept {

}
<|endoftext|>


<commit_before>// Copyright (c) 2019 The Gulden developers
// Authored by: Malcolm MacLeod (mmacleod@gmx.com)
// Distributed under the GULDEN software license, see the accompanying
// file COPYING

#include "android_wallet.h"
#include "util.h"

#include <google/protobuf/descriptor.h>
#include <google/protobuf/dynamic_message.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/descriptor.pb.h>
#include <crypto/scrypt/crypto_scrypt.h>

#include <cryptopp/config.h>
#include <cryptopp/aes.h>
#include <cryptopp/modes.h>

#include <codecvt>
#include <locale>

#include <boost/scope_exit.hpp>

using namespace google::protobuf;

// The bitcoinj authors opted to split the utf16 password characters into bytes as opposed to doing a utf8 conversion or similar
// This replicates that behaviour.
std::vector<uint8_t> convertToJavaByteArray(const std::u16string& u16_string)
{
    std::vector<uint8_t> ret;
    ret.resize(u16_string.length()*2);
    for(uint32_t i = 0; i < u16_string.length(); i++)
    {
        int bytePosition = i << 1;
        ret[bytePosition] = uint8_t((u16_string[i]&0xFF00) >> 8);
        ret[bytePosition + 1] = uint8_t(u16_string[i]&0x00FF);
    }
    return ret;
}

std::string codec_type = "EmptyMessage";
android_wallet ParseAndroidProtoWallet(std::string walletPath, std::string walletPassword)
{
    android_wallet walletRet;

    //Parse the file with a blank/empty proto
    DescriptorPool pool;
    FileDescriptorProto file;
    file.set_name("empty_message.proto");
    file.add_message_type()->set_name(codec_type);
    pool.BuildFile(file);
    const Descriptor* type = pool.FindMessageTypeByName(codec_type);

    DynamicMessageFactory dynamic_factory(&pool);
    std::unique_ptr<Message> message(dynamic_factory.GetPrototype(type)->New());
    std::unique_ptr<Message> nestedMessage(dynamic_factory.GetPrototype(type)->New());

    int fd = open(walletPath.c_str(), O_RDONLY);
    if (fd<=0)
    {
        walletRet.resultMessage = "Failed to open input file.";
        walletRet.fileOpenError = true;
        return walletRet;
    }
    // Parse binary input
    io::FileInputStream in(fd);
    BOOST_SCOPE_EXIT(&fd) { close(fd); } BOOST_SCOPE_EXIT_END

    if (!message->ParsePartialFromZeroCopyStream(&in))
    {
        walletRet.resultMessage = "Failed to parse input.";
        return walletRet;
    }

    if (!message->IsInitialized())
    {
        walletRet.resultMessage = "Failed to parse input, missing required fields.";
        return walletRet;
    }

    auto fieldSet = message->GetReflection()->MutableUnknownFields(message.get());

    walletRet.numWalletFields = fieldSet->field_count();

    //Variables for dealing with password hashing
    std::string scryptSalt;
    int64_t scryptN=16383;
    int32_t scryptR=8;
    int32_t scryptP=1;
    std::string aesIV;
    std::string aesCryptedData;

    // Navigate the field tree
    for (int i=0;i<fieldSet->field_count();++i)
    {
        auto field = fieldSet->field(i);
        switch (field.number())
        {
            case 1:
            {
                if (field.type() == 3)
                {
                    // All valid wallet files should have this field.
                    if (field.length_delimited() == "com.gulden.production") { walletRet.validWalletProto = true; }
                }
                break;
            }
            case 6:
            {
                if (!nestedMessage->ParsePartialFromString(field.length_delimited()))
                {
                    walletRet.resultMessage = "Failed to parse input for nested scrypt data.";
                    return walletRet;
                }
                auto nestedFieldSet =  nestedMessage->GetReflection()->MutableUnknownFields(nestedMessage.get());
                for (int j=0;j<nestedFieldSet->field_count();++j)
                {
                    auto nestedField = nestedFieldSet->field(j);
                    switch (nestedField.number())
                    {
                        case 1: scryptSalt = nestedField.length_delimited(); break;
                        case 2: scryptN = nestedField.varint(); break;
                        case 3: scryptR = nestedField.varint(); break;
                        case 4: scryptP = nestedField.varint(); break;
                        default: walletRet.resultMessage = "Unknown encyption paramater."; return walletRet;
                    }
                }
                break;
            }
            case 3:
            {
                if (!nestedMessage->ParsePartialFromString(field.length_delimited()))
                {
                    walletRet.resultMessage = "Failed to parse input for nested key.";
                    return walletRet;
                }
                auto nestedFieldSet =  nestedMessage->GetReflection()->MutableUnknownFields(nestedMessage.get());
                int keyType = -1;
                for (int j=0;j<nestedFieldSet->field_count();++j)
                {
                    auto nestedField = nestedFieldSet->field(j);
                    // Grab the lowest possible birth date from the proto.
                    if (nestedField.number() == 5 && nestedField.type() == 0)
                    {
                        //Birth dates were stored in milliseconds to convert to seconds
                        uint64_t timeStamp = nestedField.varint()/1000;
                        if (walletRet.walletBirth == 0 || walletRet.walletBirth > timeStamp)
                        {
                            walletRet.walletBirth = timeStamp;
                        }
                    }
                    if (keyType < 0)
                    {
                        if (nestedField.number() == 1 && nestedField.type() == 0)
                        {
                            keyType = nestedField.varint();
                            switch (keyType)
                            {
                                case 3: break;
                                case 5: break;
                                case 4: LogPrintf("ParseAndroidProtoWallet: Skipping HD key\n"); break;
                                default: LogPrintf("ParseAndroidProtoWallet: Unhandled key type [%d]\n", keyType); break;
                            }
                        }
                    }
                    else
                    {
                        switch (keyType)
                        {
                            case 3:
                            case 5:
                            {
                                if (nestedField.type()==3)
                                {
                                    switch (nestedField.number())
                                    {
                                        case 2:
                                        {
                                            walletRet.walletSeedMnemonic = nestedField.length_delimited();
                                            break;
                                        }
                                        case 6:
                                        {
                                            walletRet.encrypted = true;

                                            std::unique_ptr<Message> aesMessage(dynamic_factory.GetPrototype(type)->New());
                                            if (!aesMessage->ParsePartialFromString(nestedField.length_delimited()))
                                            {
                                                walletRet.resultMessage = "Failed to parse input for encrypted key.";
                                                return walletRet;
                                            }
                                            auto aesFieldSet = aesMessage->GetReflection()->MutableUnknownFields(aesMessage.get());
                                            for (int k=0;k<aesFieldSet->field_count();++k)
                                            {
                                                auto aesField = aesFieldSet->field(k);
                                                switch (aesField.number())
                                                {
                                                    case 1: aesIV = aesField.length_delimited(); break;
                                                    case 2: aesCryptedData = aesField.length_delimited(); break;
                                                    default: walletRet.resultMessage = "Invalid encryption data for encrypted key."; return walletRet;
                                                }
                                            }
                                            break;
                                        }
                                    }
                                }
                                break;
                            }
                            case 4: break;
                            default: LogPrintf("ParseAndroidProtoWallet: Unhandled field         [%d] [%d]\n", nestedField.number(), nestedField.type()); break;
                        }
                    }
                }
                break;
            }
            default: LogPrintf("ParseAndroidProtoWallet: Unhandled field [%d] [%d]\n", field.number(), field.type()); break;
        }
    }

    if (!scryptSalt.empty())
    {
        // Generate aes key data from password using the provided scrypt paramaters.
        std::vector<uint8_t> aesKeyData;
        aesKeyData.resize(32);
        std::vector<uint8_t> passwordData = convertToJavaByteArray(std::wstring_convert<std::codecvt_utf8_utf16<char16_t>, char16_t>{}.from_bytes(walletPassword));
        crypto_scrypt((uint8_t*)&passwordData[0], passwordData.size(), (uint8_t*)scryptSalt.c_str(), scryptSalt.length(), scryptN, scryptR, scryptP, &aesKeyData[0], 32);

        // Combine the AES key with the IV and the encrypted data to retrieve the original unencrypted data
        //fixme: (SIGMA)
        CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption d;
        //fixme: PAD
        d.SetKeyWithIV(&aesKeyData[0], aesKeyData.size(), (const unsigned char*)&aesIV[0]);
        int nLen = aesCryptedData.size(); // plaintext will always be equal to or lesser than length of ciphertext
        walletRet.walletSeedMnemonic.resize(nLen);
        d.ProcessData((unsigned char*)&walletRet.walletSeedMnemonic[0], (const unsigned char*)&aesCryptedData[0], aesCryptedData.size());
        //nLen = dec.Decrypt((const unsigned char*)&aesCryptedData[0], aesCryptedData.size(), (unsigned char*)&walletRet.walletSeedMnemonic[0]);
        if (nLen == 0)
        {
            walletRet.resultMessage = "Decryption failed.";
            return walletRet;
        }
        walletRet.walletSeedMnemonic.resize(nLen);
    }
    if (!walletRet.walletSeedMnemonic.empty())
    {
        walletRet.validWallet = true;
    }

    return walletRet;
}
<commit_msg>CORE: Get android wallet import working again with cryptopp changes.<commit_after>// Copyright (c) 2019 The Gulden developers
// Authored by: Malcolm MacLeod (mmacleod@gmx.com)
// Distributed under the GULDEN software license, see the accompanying
// file COPYING

#include "android_wallet.h"
#include "util.h"

#include <google/protobuf/descriptor.h>
#include <google/protobuf/dynamic_message.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/descriptor.pb.h>
#include <crypto/scrypt/crypto_scrypt.h>

#include <cryptopp/config.h>
#include <cryptopp/aes.h>
#include <cryptopp/modes.h>
#include <cryptopp/filters.h>

#include <codecvt>
#include <locale>

#include <boost/scope_exit.hpp>

using namespace google::protobuf;

// The bitcoinj authors opted to split the utf16 password characters into bytes as opposed to doing a utf8 conversion or similar
// This replicates that behaviour.
std::vector<uint8_t> convertToJavaByteArray(const std::u16string& u16_string)
{
    std::vector<uint8_t> ret;
    ret.resize(u16_string.length()*2);
    for(uint32_t i = 0; i < u16_string.length(); i++)
    {
        int bytePosition = i << 1;
        ret[bytePosition] = uint8_t((u16_string[i]&0xFF00) >> 8);
        ret[bytePosition + 1] = uint8_t(u16_string[i]&0x00FF);
    }
    return ret;
}

std::string codec_type = "EmptyMessage";
android_wallet ParseAndroidProtoWallet(std::string walletPath, std::string walletPassword)
{
    android_wallet walletRet;

    //Parse the file with a blank/empty proto
    DescriptorPool pool;
    FileDescriptorProto file;
    file.set_name("empty_message.proto");
    file.add_message_type()->set_name(codec_type);
    pool.BuildFile(file);
    const Descriptor* type = pool.FindMessageTypeByName(codec_type);

    DynamicMessageFactory dynamic_factory(&pool);
    std::unique_ptr<Message> message(dynamic_factory.GetPrototype(type)->New());
    std::unique_ptr<Message> nestedMessage(dynamic_factory.GetPrototype(type)->New());

    int fd = open(walletPath.c_str(), O_RDONLY);
    if (fd<=0)
    {
        walletRet.resultMessage = "Failed to open input file.";
        walletRet.fileOpenError = true;
        return walletRet;
    }
    // Parse binary input
    io::FileInputStream in(fd);
    BOOST_SCOPE_EXIT(&fd) { close(fd); } BOOST_SCOPE_EXIT_END

    if (!message->ParsePartialFromZeroCopyStream(&in))
    {
        walletRet.resultMessage = "Failed to parse input.";
        return walletRet;
    }

    if (!message->IsInitialized())
    {
        walletRet.resultMessage = "Failed to parse input, missing required fields.";
        return walletRet;
    }

    auto fieldSet = message->GetReflection()->MutableUnknownFields(message.get());

    walletRet.numWalletFields = fieldSet->field_count();

    //Variables for dealing with password hashing
    std::string scryptSalt;
    int64_t scryptN=16383;
    int32_t scryptR=8;
    int32_t scryptP=1;
    std::string aesIV;
    std::string aesCryptedData;

    // Navigate the field tree
    for (int i=0;i<fieldSet->field_count();++i)
    {
        auto field = fieldSet->field(i);
        switch (field.number())
        {
            case 1:
            {
                if (field.type() == 3)
                {
                    // All valid wallet files should have this field.
                    if (field.length_delimited() == "com.gulden.production") { walletRet.validWalletProto = true; }
                }
                break;
            }
            case 6:
            {
                if (!nestedMessage->ParsePartialFromString(field.length_delimited()))
                {
                    walletRet.resultMessage = "Failed to parse input for nested scrypt data.";
                    return walletRet;
                }
                auto nestedFieldSet =  nestedMessage->GetReflection()->MutableUnknownFields(nestedMessage.get());
                for (int j=0;j<nestedFieldSet->field_count();++j)
                {
                    auto nestedField = nestedFieldSet->field(j);
                    switch (nestedField.number())
                    {
                        case 1: scryptSalt = nestedField.length_delimited(); break;
                        case 2: scryptN = nestedField.varint(); break;
                        case 3: scryptR = nestedField.varint(); break;
                        case 4: scryptP = nestedField.varint(); break;
                        default: walletRet.resultMessage = "Unknown encyption paramater."; return walletRet;
                    }
                }
                break;
            }
            case 3:
            {
                if (!nestedMessage->ParsePartialFromString(field.length_delimited()))
                {
                    walletRet.resultMessage = "Failed to parse input for nested key.";
                    return walletRet;
                }
                auto nestedFieldSet =  nestedMessage->GetReflection()->MutableUnknownFields(nestedMessage.get());
                int keyType = -1;
                for (int j=0;j<nestedFieldSet->field_count();++j)
                {
                    auto nestedField = nestedFieldSet->field(j);
                    // Grab the lowest possible birth date from the proto.
                    if (nestedField.number() == 5 && nestedField.type() == 0)
                    {
                        //Birth dates were stored in milliseconds to convert to seconds
                        uint64_t timeStamp = nestedField.varint()/1000;
                        if (walletRet.walletBirth == 0 || walletRet.walletBirth > timeStamp)
                        {
                            walletRet.walletBirth = timeStamp;
                        }
                    }
                    if (keyType < 0)
                    {
                        if (nestedField.number() == 1 && nestedField.type() == 0)
                        {
                            keyType = nestedField.varint();
                            switch (keyType)
                            {
                                case 3: break;
                                case 5: break;
                                case 4: LogPrintf("ParseAndroidProtoWallet: Skipping HD key\n"); break;
                                default: LogPrintf("ParseAndroidProtoWallet: Unhandled key type [%d]\n", keyType); break;
                            }
                        }
                    }
                    else
                    {
                        switch (keyType)
                        {
                            case 3:
                            case 5:
                            {
                                if (nestedField.type()==3)
                                {
                                    switch (nestedField.number())
                                    {
                                        case 2:
                                        {
                                            walletRet.walletSeedMnemonic = nestedField.length_delimited();
                                            break;
                                        }
                                        case 6:
                                        {
                                            walletRet.encrypted = true;

                                            std::unique_ptr<Message> aesMessage(dynamic_factory.GetPrototype(type)->New());
                                            if (!aesMessage->ParsePartialFromString(nestedField.length_delimited()))
                                            {
                                                walletRet.resultMessage = "Failed to parse input for encrypted key.";
                                                return walletRet;
                                            }
                                            auto aesFieldSet = aesMessage->GetReflection()->MutableUnknownFields(aesMessage.get());
                                            for (int k=0;k<aesFieldSet->field_count();++k)
                                            {
                                                auto aesField = aesFieldSet->field(k);
                                                switch (aesField.number())
                                                {
                                                    case 1: aesIV = aesField.length_delimited(); break;
                                                    case 2: aesCryptedData = aesField.length_delimited(); break;
                                                    default: walletRet.resultMessage = "Invalid encryption data for encrypted key."; return walletRet;
                                                }
                                            }
                                            break;
                                        }
                                    }
                                }
                                break;
                            }
                            case 4: break;
                            default: LogPrintf("ParseAndroidProtoWallet: Unhandled field         [%d] [%d]\n", nestedField.number(), nestedField.type()); break;
                        }
                    }
                }
                break;
            }
            default: LogPrintf("ParseAndroidProtoWallet: Unhandled field [%d] [%d]\n", field.number(), field.type()); break;
        }
    }

    if (!scryptSalt.empty())
    {
        // Generate aes key data from password using the provided scrypt paramaters.
        std::vector<uint8_t> aesKeyData;
        aesKeyData.resize(32);
        std::vector<uint8_t> passwordData = convertToJavaByteArray(std::wstring_convert<std::codecvt_utf8_utf16<char16_t>, char16_t>{}.from_bytes(walletPassword));
        crypto_scrypt((uint8_t*)&passwordData[0], passwordData.size(), (uint8_t*)scryptSalt.c_str(), scryptSalt.length(), scryptN, scryptR, scryptP, &aesKeyData[0], 32);

        // Combine the AES key with the IV and the encrypted data to retrieve the original unencrypted data
        CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption dec;
        dec.SetKeyWithIV(&aesKeyData[0], aesKeyData.size(), (const unsigned char*)&aesIV[0]);
        CryptoPP::StringSource s(walletRet.walletSeedMnemonic, true, new CryptoPP::StreamTransformationFilter(dec, new CryptoPP::StringSink(aesCryptedData)));

        if (walletRet.walletSeedMnemonic.size() == 0)
        {
            walletRet.resultMessage = "Decryption failed.";
            return walletRet;
        }
    }
    if (!walletRet.walletSeedMnemonic.empty())
    {
        walletRet.validWallet = true;
    }

    return walletRet;
}
<|endoftext|>


<commit_before>/*
 * Car.cpp
 *
 *  Created on: 2009-09-08
 *      Author: chudy
 */

#include "race/Car.h"
#include "Properties.h"
#include "Message.h"
#include "race/Level.h"
#include "race/RacePlayer.h"

#include <ClanLib/core.h>
#include <ClanLib/display.h>

Car::Car(RacePlayer *p_player) :
	m_player(p_player),
	m_level(NULL),
	m_sprite(),
	m_locked(false),
	m_position(0.0f, 0.0f), //najnowsza pozycja autka
	m_rotation(0, cl_degrees), // kąt pod jakim stoi autko
	m_turn(0.0f), //na boki
	m_acceleration(false), //do przodu
	m_brake(false), //do tyłu
	m_speed(0.0f),
	m_angle(0.0f),
	m_inputChecksum(0),
	m_lap(0)
{

}

Car::~Car() {
}

void Car::draw(CL_GraphicContext &p_gc) {

	// TODO: move to load();
	if (m_sprite.is_null()) {
		m_sprite = CL_Sprite(p_gc, "race/car", Stage::getResourceManager());
	}

	if (m_nickDisplayFont.is_null()) {
		CL_FontDescription fontDesc;

		fontDesc.set_typeface_name("resources/DejaVuSansCondensed-BoldOblique.ttf");
		fontDesc.set_height(12);

		m_nickDisplayFont = CL_Font_Freetype(p_gc, fontDesc);
	}

	p_gc.push_modelview();

	p_gc.mult_translate(m_position.x, m_position.y);

	// display nickname
	const CL_Size nameLabelSize = m_nickDisplayFont.get_text_size(p_gc, m_player->getPlayer().getName());
	m_nickDisplayFont.draw_text(p_gc, -nameLabelSize.width / 2, -20, m_player->getPlayer().getName());

	p_gc.mult_rotate(m_rotation, 0, 0, 1);

	m_sprite.draw(p_gc, 0, 0);

#ifndef NDEBUG
	if (Properties::getPropertyAsBool("debug.draw_vectors")) {
		debugDrawLine(p_gc, 0, 0, forceVector.x, forceVector.y, CL_Colorf::red);
		debugDrawLine(p_gc, 0, 0, driftVector.x*10, driftVector.y*10, CL_Colorf::green);
		debugDrawLine(p_gc, 0, 0, accelerationVector.x/10, accelerationVector.y/10, CL_Colorf::blue);
		debugDrawLine(p_gc, 0, 0, m_moveVector.x/10, m_moveVector.y/10, CL_Colorf::black);
	}
#endif // NDEBUG

	p_gc.pop_modelview();

#ifndef NDEBUG

	if (Properties::getPropertyAsBool("debug.draw_checkpoints")) {

		// draw checkpoints
		const CL_Vec4f red_color(1.0f, 0.0f, 0.0f, 0.3f);
		const CL_Vec4f green_color(0.0f, 1.0f, 0.0f, 0.3f);

		for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
			const CL_Rectf &rect = itor->getRect();
			const CL_Vec4f color = itor->isPassed() ? green_color : red_color;

	//		CL_Console::write_line(CL_String8("rect: left=") + CL_StringHelp::float_to_local8(rect.left) + CL_String8(", top=") + CL_StringHelp::float_to_local8(rect.top));

			CL_Vec2f positions[] = {
					CL_Vec2f(rect.left, rect.top),
					CL_Vec2f(rect.left, rect.bottom),
					CL_Vec2f(rect.right, rect.bottom),
					CL_Vec2f(rect.right, rect.top)
			};

			CL_Vec4f colors[] = { color, color, color, color };

			CL_PrimitivesArray vertices(p_gc);
			vertices.set_attributes(0, positions);
			vertices.set_attributes(1, colors);

			p_gc.set_program_object(cl_program_color_only);
			p_gc.draw_primitives(cl_quads, 4, vertices);
		}

	}
#endif // NDEBUG
}

#ifndef NDEBUG
void Car::debugDrawLine(CL_GraphicContext &p_gc, float x1, float y1, float x2, float y2, const CL_Color& p_color) {

	CL_Vec4f color(p_color.get_red_f(), p_color.get_green_f(), p_color.get_blue_f(), p_color.get_alpha_f());

	CL_Vec2f positions[] = {
			CL_Vec2f(x1, y1),
			CL_Vec2f(x2, y2),
	};

	CL_Vec4f colors[] = { color, color };

	CL_PrimitivesArray vertices(p_gc);
	vertices.set_attributes(0, positions);
	vertices.set_attributes(1, colors);

	p_gc.set_program_object(cl_program_color_only);
	p_gc.draw_primitives(cl_lines, 2, vertices);
}
#endif // NDEBUG

void Car::load(CL_GraphicContext &p_gc) {

}

void Car::update(unsigned int elapsedTime) {

	// don't do anything if car is locked
	if (m_locked) {
		return;
	}

	static const float BRAKE_POWER = 400.0f;

	static const float ACCEL_SPEED = 300.0f;
	static const float MAX_SPEED = 500.0f;

	static const float AIR_RESIST = 0.2f;

	static const float MAX_ANGLE = 60.0f;

	const float delta = elapsedTime / 1000.0f;

	// calculate input checksum and if its different than last one, then
	// invoke the signal
	const int inputChecksum = calculateInputChecksum();

	if (inputChecksum != m_inputChecksum) {
		m_statusChangeSignal.invoke(*this);
		m_inputChecksum = inputChecksum;
	}

	// mark checkpoints
	for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {

		if (itor->isPassed()) {
			continue;
		}

		if (itor->getRect().contains(m_position)) {
			itor->setPassed(true);
		}
	}

	// check lap progress
	if (areAllCheckpointsPassed()) {
		// check for last lap checkpoint
		if (m_lapCheckpoint.getRect().contains(m_position)) {
			// got lap
			++m_lap;
			resetCheckpoints();
		}
	}

	//turning
	if( m_turn == -1.0f )
		m_angle = -MAX_ANGLE;
	else if( m_turn == 1.0f )
		m_angle = MAX_ANGLE;
	else
		m_angle = 0.0f;

	// acceleration

	if (m_acceleration) {
		const float speedChange = ACCEL_SPEED * delta;

		m_speed += speedChange;

		if (m_speed > MAX_SPEED) {
			m_speed = MAX_SPEED;
		}

	}

	// brake
	if (m_brake) {
		const float speedChange = BRAKE_POWER * delta;

		m_speed -= speedChange;

		if (m_speed < -MAX_SPEED / 2) {
			m_speed = -MAX_SPEED / 2;
		}
	}

	// air resistance
	m_speed -= delta * AIR_RESIST * m_speed;

	// ground resistance
	if (m_level != NULL) {
		const float groundResist = m_level->getResistance(m_position.x, m_position.y);
		m_speed -= delta * groundResist * m_speed;
	}

	// rotation

	const float rad = m_rotation.to_radians();

	accelerationVector.x = cos(rad);
	accelerationVector.y = sin(rad);

	accelerationVector.normalize();
	accelerationVector *= m_speed;

	if( m_angle < 0.0f ) {
 		forceVector.x = sin(rad);
		forceVector.y = -cos(rad);

		forceVector.normalize();

		forceVector *= 0.1f * m_speed/7.0f;
	}
	else if( m_angle > 0.0f ){
		forceVector.x = -sin(rad);
		forceVector.y = cos(rad);

		forceVector.normalize();

		forceVector *= 0.1f * m_speed/7.0f;
	}

	Message::out() << "wartość: " << (tan(60.0f)) << std::endl;

	if( m_angle != 0.0f )
		m_moveVector = accelerationVector + forceVector;
	else
		m_moveVector = accelerationVector;

	/*const float rad = m_rotation.to_radians();

	m_moveVector.x = cos(rad);
	m_moveVector.y = sin(rad);

	m_moveVector.normalize();
	m_moveVector *= m_speed;
	*/
	// movement
	if( m_angle != 0.0f )
		m_rotation.set_degrees( atan2( m_moveVector.y, m_moveVector.x ) * 180.0f / 3.14f );

	//m_rotation.normalize();

	CL_Vec2f currentMoveVector = m_moveVector * delta;
	m_position.x += currentMoveVector.x;
	m_position.y += currentMoveVector.y;

#ifndef NDEBUG
			Stage::getDebugLayer()->putMessage(CL_String8("force"),  CL_StringHelp::float_to_local8(forceVector.length()));
			Stage::getDebugLayer()->putMessage(CL_String8("speed"),  CL_StringHelp::float_to_local8(m_speed));
			if (m_level != NULL) {
				Stage::getDebugLayer()->putMessage(CL_String8("resist"), CL_StringHelp::float_to_local8(m_level->getResistance(m_position.x, m_position.y)));
			}
#endif
}

int Car::prepareStatusEvent(CL_NetGameEvent &p_event) {
	CL_NetGameEventValue posX(m_position.x);
	CL_NetGameEventValue posY(m_position.y);
	CL_NetGameEventValue rotation(m_rotation.to_degrees());
	CL_NetGameEventValue turn(m_turn);
	CL_NetGameEventValue accel(m_acceleration);
	CL_NetGameEventValue brake(m_brake);
	CL_NetGameEventValue moveX(m_moveVector.x);
	CL_NetGameEventValue moveY(m_moveVector.y);
	CL_NetGameEventValue speed(m_speed);

	int c = 0;

	p_event.add_argument(posX);     ++c;
	p_event.add_argument(posY);     ++c;
	p_event.add_argument(rotation); ++c;
	p_event.add_argument(turn);     ++c;
	p_event.add_argument(accel);    ++c;
	p_event.add_argument(brake);    ++c;
	p_event.add_argument(moveX);    ++c;
	p_event.add_argument(moveY);    ++c;
	p_event.add_argument(speed);    ++c;

	return c;
}

int Car::applyStatusEvent(const CL_NetGameEvent &p_event, int p_beginIndex) {
	int i = p_beginIndex;

	m_position.x =   (float) p_event.get_argument(i++);
	m_position.y =   (float) p_event.get_argument(i++);
	m_rotation =     CL_Angle((float) p_event.get_argument(i++), cl_degrees);
	m_turn =         (float) p_event.get_argument(i++);
	m_acceleration =  (bool) p_event.get_argument(i++);
	m_brake =         (bool) p_event.get_argument(i++);
	m_moveVector.x = (float) p_event.get_argument(i++);
	m_moveVector.y = (float) p_event.get_argument(i++);
	m_speed =        (float) p_event.get_argument(i++);

	return i;
}

int Car::calculateInputChecksum() const {
	int checksum = 0;
	checksum |= (int) m_acceleration;
	checksum |= ((int) m_brake) << 1;
	checksum += m_turn * 10000.0f;

	return checksum;
}

bool Car::areAllCheckpointsPassed() const {
	for (std::vector<Checkpoint>::const_iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
		if (!itor->isPassed()) {
			return false;
		}
	}

	return true;
}

bool Car::resetCheckpoints() {
	for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
		itor->setPassed(false);
	}
}

void Car::setStartPosition(int p_startPosition) {
	m_position = m_level->getStartPosition(p_startPosition);

	// stop the car!
	m_rotation = CL_Angle::from_degrees(-90);
	m_turn = 0;
	m_acceleration = false;
	m_brake = false;
	m_moveVector = CL_Vec2f();
	accelerationVector = CL_Vec2f();
	forceVector = CL_Vec2f();
	driftVector = CL_Vec2f();
	m_speed = 0.0f;
	m_angle = 0.0f;
	m_lap = 1;

	// send the status change to other players
	m_statusChangeSignal.invoke(*this);
}
<commit_msg>* Done new car physics<commit_after>/*
 * Car.cpp
 *
 *  Created on: 2009-09-08
 *      Author: chudy
 */

#include "race/Car.h"
#include "Properties.h"
#include "Message.h"
#include "race/Level.h"
#include "race/RacePlayer.h"

#include <ClanLib/core.h>
#include <ClanLib/display.h>

Car::Car(RacePlayer *p_player) :
	m_player(p_player),
	m_level(NULL),
	m_sprite(),
	m_locked(false),
	m_position(0.0f, 0.0f), //najnowsza pozycja autka
	m_rotation(0, cl_degrees), // kąt pod jakim stoi autko
	m_turn(0.0f), //na boki
	m_acceleration(false), //do przodu
	m_brake(false), //do tyłu
	m_speed(0.0f),
	m_angle(0.0f), // kąt zmiany kierunku jazdy
	m_inputChecksum(0),
	m_lap(0)
{

}

Car::~Car() {
}

void Car::draw(CL_GraphicContext &p_gc) {

	// TODO: move to load();
	if (m_sprite.is_null()) {
		m_sprite = CL_Sprite(p_gc, "race/car", Stage::getResourceManager());
	}

	if (m_nickDisplayFont.is_null()) {
		CL_FontDescription fontDesc;

		fontDesc.set_typeface_name("resources/DejaVuSansCondensed-BoldOblique.ttf");
		fontDesc.set_height(12);

		m_nickDisplayFont = CL_Font_Freetype(p_gc, fontDesc);
	}

	p_gc.push_modelview();

	p_gc.mult_translate(m_position.x, m_position.y);

	// display nickname
	const CL_Size nameLabelSize = m_nickDisplayFont.get_text_size(p_gc, m_player->getPlayer().getName());
	m_nickDisplayFont.draw_text(p_gc, -nameLabelSize.width / 2, -20, m_player->getPlayer().getName());

	p_gc.mult_rotate(m_rotation, 0, 0, 1);

	m_sprite.draw(p_gc, 0, 0);

#ifndef NDEBUG
	if (Properties::getPropertyAsBool("debug.draw_vectors")) {
		debugDrawLine(p_gc, 0, 0, forceVector.x, forceVector.y, CL_Colorf::red);
		debugDrawLine(p_gc, 0, 0, driftVector.x*10, driftVector.y*10, CL_Colorf::green);
		debugDrawLine(p_gc, 0, 0, accelerationVector.x/10, accelerationVector.y/10, CL_Colorf::blue);
		debugDrawLine(p_gc, 0, 0, m_moveVector.x/10, m_moveVector.y/10, CL_Colorf::black);
	}
#endif // NDEBUG

	p_gc.pop_modelview();

#ifndef NDEBUG

	if (Properties::getPropertyAsBool("debug.draw_checkpoints")) {

		// draw checkpoints
		const CL_Vec4f red_color(1.0f, 0.0f, 0.0f, 0.3f);
		const CL_Vec4f green_color(0.0f, 1.0f, 0.0f, 0.3f);

		for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
			const CL_Rectf &rect = itor->getRect();
			const CL_Vec4f color = itor->isPassed() ? green_color : red_color;

	//		CL_Console::write_line(CL_String8("rect: left=") + CL_StringHelp::float_to_local8(rect.left) + CL_String8(", top=") + CL_StringHelp::float_to_local8(rect.top));

			CL_Vec2f positions[] = {
					CL_Vec2f(rect.left, rect.top),
					CL_Vec2f(rect.left, rect.bottom),
					CL_Vec2f(rect.right, rect.bottom),
					CL_Vec2f(rect.right, rect.top)
			};

			CL_Vec4f colors[] = { color, color, color, color };

			CL_PrimitivesArray vertices(p_gc);
			vertices.set_attributes(0, positions);
			vertices.set_attributes(1, colors);

			p_gc.set_program_object(cl_program_color_only);
			p_gc.draw_primitives(cl_quads, 4, vertices);
		}

	}
#endif // NDEBUG
}

#ifndef NDEBUG
void Car::debugDrawLine(CL_GraphicContext &p_gc, float x1, float y1, float x2, float y2, const CL_Color& p_color) {

	CL_Vec4f color(p_color.get_red_f(), p_color.get_green_f(), p_color.get_blue_f(), p_color.get_alpha_f());

	CL_Vec2f positions[] = {
			CL_Vec2f(x1, y1),
			CL_Vec2f(x2, y2),
	};

	CL_Vec4f colors[] = { color, color };

	CL_PrimitivesArray vertices(p_gc);
	vertices.set_attributes(0, positions);
	vertices.set_attributes(1, colors);

	p_gc.set_program_object(cl_program_color_only);
	p_gc.draw_primitives(cl_lines, 2, vertices);
}
#endif // NDEBUG

void Car::load(CL_GraphicContext &p_gc) {

}

void Car::update(unsigned int elapsedTime) {

//	Message::out() << m_moveVector << std::endl;

	// don't do anything if car is locked
	if (m_locked) {
		return;
	}

	static const float BRAKE_POWER = 400.0f;

	static const float ACCEL_SPEED = 300.0f;
	static const float MAX_SPEED = 500.0f;

	static const float AIR_RESIST = 0.2f;

	static const float MAX_ANGLE = 60.0f;

	static const float TENACITY = 0.08f;

	const float delta = elapsedTime / 1000.0f;

	// calculate input checksum and if its different than last one, then
	// invoke the signal
	const int inputChecksum = calculateInputChecksum();

	if (inputChecksum != m_inputChecksum) {
		m_statusChangeSignal.invoke(*this);
		m_inputChecksum = inputChecksum;
	}

	// mark checkpoints
	for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {

		if (itor->isPassed()) {
			continue;
		}

		if (itor->getRect().contains(m_position)) {
			itor->setPassed(true);
		}
	}

	// check lap progress
	if (areAllCheckpointsPassed()) {
		// check for last lap checkpoint
		if (m_lapCheckpoint.getRect().contains(m_position)) {
			// got lap
			++m_lap;
			resetCheckpoints();
		}
	}

	//turning
	float angle = MAX_ANGLE * m_turn;

	// acceleration speed

	if (m_acceleration) {
		const float speedChange = ACCEL_SPEED * delta;

		m_speed += speedChange;

		if (m_speed > MAX_SPEED) {
			m_speed = MAX_SPEED;
		}

	}

	// brake
	if (m_brake) {
		const float speedChange = BRAKE_POWER * delta;

		m_speed -= speedChange;

		if (m_speed < -MAX_SPEED / 2) {
			m_speed = -MAX_SPEED / 2;
		}
	}

	// air resistance
	m_speed -= delta * AIR_RESIST * m_speed;

	// ground resistance
	if (m_level != NULL) {
		const float groundResist = m_level->getResistance(m_position.x, m_position.y);
		m_speed -= delta * groundResist * m_speed;
	}

	// rotation

	const float rad = m_rotation.to_radians(); // kąt autka w radianach

	// wektor prędkości bez uwzględnionej zmiany kierunku
	accelerationVector.x = cos(rad);
	accelerationVector.y = sin(rad);

	accelerationVector.normalize();
	accelerationVector *= m_speed;

	CL_Vec2f changeVector; // wektor zmiany kierunku

	if( angle < 0.0f ) { // skręt w jedną stronę
		// przygotowanie wektora skrętu (zmiany kierunku jazdy)
 		changeVector.x = sin(rad);
		changeVector.y = -cos(rad);
		changeVector.normalize();
		changeVector *= tan( -angle ) * m_speed / 7.0f; // modyfikacja siły skrętu
	}
	else if( angle > 0.0f ){ // skręt w drugą stronę, wszystko analogicznie
		changeVector.x = -sin(rad);
		changeVector.y = cos(rad);
		changeVector.normalize();
		changeVector *= tan( angle ) * m_speed / 7.0f;
	}

	CL_Vec2f newAccelVector; // nowy, świeżo wyliczony w następnym IFie,
	// wektor prędkości, zgodnie z którym pojechałoby autko bez poślizgu

	if( angle != 0.0f ) // sumowanie wektorow: skrętu i prostej jazdy
		newAccelVector = changeVector + accelerationVector;
	else // jak nie skręca to ma jechać przed siebie :)
		newAccelVector = accelerationVector;

	// wektor o który zostanie przesunięte autko (już z poślizgiem)
	// tip: m_moveVector ma jeszcze wartość z poprzedniej klatki, więc
	// niejako mieszamy starą prędkość z nowo wyliczoną prędkością co
	// daje nam efekt poślizgu (na ciało działa siła która działała na
	// nie przed chwilą tylko podwpływem nowych sił - nowego kierunku
	// jazdy, maleje)
	CL_Vec2f realVector = m_moveVector + ( newAccelVector * TENACITY );
	realVector.normalize();
	realVector *= m_speed;

	m_moveVector = realVector;

	// update position
	m_position.x += m_moveVector.x * delta;
	m_position.y += m_moveVector.y * delta;

	// update rotation of car when changing direction
	if( m_turn != 0.0f ) { // wykonywany jest skręt

		// tworzymy sobie osobne wektory dla rotacji
		CL_Vec2f rotVector; // wektor rotacji
		CL_Vec2f changeRotVector; // wektor zmiany rotacji (w danej chwili)

		rotVector.x = cos(rad);
		rotVector.y = sin(rad);
		rotVector.normalize();

		if( m_turn < 0.0f ) { // skręt w jedną stronę
			changeRotVector.x = sin(rad);
			changeRotVector.y = -cos(rad);
			changeRotVector.normalize();
			changeRotVector *= 0.04f;
		}
		else if ( m_turn > 0.0f ) { // skręt w drugą stronę
			changeRotVector.x = -sin(rad);
			changeRotVector.y = cos(rad);
			changeRotVector.normalize();
			changeRotVector *= 0.04f;
		}
		rotVector += changeRotVector;
		m_rotation.set_degrees( atan2( rotVector.y, rotVector.x ) * 180.0f / 3.14f );
	}

#ifndef NDEBUG
			Stage::getDebugLayer()->putMessage(CL_String8("force"),  CL_StringHelp::float_to_local8(forceVector.length()));
			Stage::getDebugLayer()->putMessage(CL_String8("speed"),  CL_StringHelp::float_to_local8(m_speed));
			if (m_level != NULL) {
				Stage::getDebugLayer()->putMessage(CL_String8("resist"), CL_StringHelp::float_to_local8(m_level->getResistance(m_position.x, m_position.y)));
			}
#endif
}

int Car::prepareStatusEvent(CL_NetGameEvent &p_event) {
	CL_NetGameEventValue posX(m_position.x);
	CL_NetGameEventValue posY(m_position.y);
	CL_NetGameEventValue rotation(m_rotation.to_degrees());
	CL_NetGameEventValue turn(m_turn);
	CL_NetGameEventValue accel(m_acceleration);
	CL_NetGameEventValue brake(m_brake);
	CL_NetGameEventValue moveX(m_moveVector.x);
	CL_NetGameEventValue moveY(m_moveVector.y);
	CL_NetGameEventValue speed(m_speed);

	int c = 0;

	p_event.add_argument(posX);     ++c;
	p_event.add_argument(posY);     ++c;
	p_event.add_argument(rotation); ++c;
	p_event.add_argument(turn);     ++c;
	p_event.add_argument(accel);    ++c;
	p_event.add_argument(brake);    ++c;
	p_event.add_argument(moveX);    ++c;
	p_event.add_argument(moveY);    ++c;
	p_event.add_argument(speed);    ++c;

	return c;
}

int Car::applyStatusEvent(const CL_NetGameEvent &p_event, int p_beginIndex) {
	int i = p_beginIndex;

	m_position.x =   (float) p_event.get_argument(i++);
	m_position.y =   (float) p_event.get_argument(i++);
	m_rotation =     CL_Angle((float) p_event.get_argument(i++), cl_degrees);
	m_turn =         (float) p_event.get_argument(i++);
	m_acceleration =  (bool) p_event.get_argument(i++);
	m_brake =         (bool) p_event.get_argument(i++);
	m_moveVector.x = (float) p_event.get_argument(i++);
	m_moveVector.y = (float) p_event.get_argument(i++);
	m_speed =        (float) p_event.get_argument(i++);

	return i;
}

int Car::calculateInputChecksum() const {
	int checksum = 0;
	checksum |= (int) m_acceleration;
	checksum |= ((int) m_brake) << 1;
	checksum += m_turn * 10000.0f;

	return checksum;
}

bool Car::areAllCheckpointsPassed() const {
	for (std::vector<Checkpoint>::const_iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
		if (!itor->isPassed()) {
			return false;
		}
	}

	return true;
}

bool Car::resetCheckpoints() {
	for (std::vector<Checkpoint>::iterator itor = m_checkpoints.begin(); itor != m_checkpoints.end(); ++itor) {
		itor->setPassed(false);
	}
}

void Car::setStartPosition(int p_startPosition) {
	m_position = m_level->getStartPosition(p_startPosition);

	// stop the car!
	m_rotation = CL_Angle::from_degrees(-90);
	m_turn = 0;
	m_acceleration = false;
	m_brake = false;
	m_moveVector = CL_Vec2f();
	accelerationVector = CL_Vec2f();
	forceVector = CL_Vec2f();
	driftVector = CL_Vec2f();
	m_speed = 0.0f;
	m_angle = 0.0f;
	m_lap = 1;

	// send the status change to other players
	m_statusChangeSignal.invoke(*this);
}
<|endoftext|>