<commit_before>/**
 *      _____
 *     /  _  \
 *    / _/ \  \
 *   / / \_/   \
 *  /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
 *  \  / \_/ \  / /  _\| |  | __| / _ \ | ++ \ | ++ \ / _ \ |_   _|| | | |
 *   \ \_/ \_/ /  | |  | |  | ++ | |_| || ++ / | ++_/| |_| |  | |  | +-+ |
 *    \  \_/  /   | |_ | |_ | ++ |  _  || |\ \ | |   |  _  |  | |  | +-+ |
 *     \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
 *             ROBOTICS™
 *
 *  File: kinova_ros_types.cpp
 *  Desc: Wrappers around Kinova structs to facilitate easier conversion to ROS
 *		  types.
 *  Auth: Alex Bencz
 *
 *  Copyright (c) 2013, Clearpath Robotics, Inc.
 *  All Rights Reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Clearpath Robotics, Inc. nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Please send comments, questions, or patches to skynet@clearpathrobotics.com
 *
 */

#include <math.h>
#include <angles/angles.h>
#include <tf/tf.h>
#include <kinova_driver/kinova_ros_types.h>
#include <string>

namespace kinova
{

// %Tag(converntion functions)%

// rads 0 to 2*M_PI
float normalizePositiveInRads(float rads)
{
    return static_cast<float>(angles::normalize_angle_positive(rads));
}

// rads -M_PI to M_PI
float normalizeInRads(float rads)
{
    return static_cast<float>(angles::normalize_angle(rads));
}

// degrees 0 to 360
float normalizePositiveInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle_positive(angles::from_degrees(degrees)));
}

// degrees -180 to 180
float normalizeInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle(angles::from_degrees(degrees)));
}

// %EndTag(converntion functions)%

bool areValuesClose(float first, float second, float tolerance)
{
    return ((first <= second + tolerance) && (first >= second - tolerance));
}

// Exceptions
// ----------
KinovaCommException::KinovaCommException(const std::string& message, const int error_code)
{
    std::stringstream ss;
        ss << "KinovaCommException: " << message << " (return code: " << error_code << ")" << std::endl;
    desc_ = ss.str();
}

const char* KinovaCommException::what() const throw()
{
    return desc_.c_str();
}

// Class definitions
// -----------------
/**
 * @brief obtain KinovaPosefrom geometry_msgs::Pose.
 *
 * obtain KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ] from geometry_msgs::Pose[x,y,z,qx,qy,qz,qw].
 * Euler angles[] are extracted from tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx), where EulerYPR is generated by Quaterion q.
 * Units are in meters and radians
 * @param geometry_msgs ROS standard pose[x,y,z,qx,qy,qz,qw], in meters and quaterion.
 */
KinovaPose::KinovaPose(const geometry_msgs::Pose &pose)
{
    double tx, ty, tz;
    tf::Quaternion q;
    tf::quaternionMsgToTF(pose.orientation, q);

    tf::Matrix3x3 bt_q(q);

    // tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)
    bt_q.getEulerYPR(tz, ty, tx);

    X = static_cast<float>(pose.position.x);
    Y = static_cast<float>(pose.position.y);
    Z = static_cast<float>(pose.position.z);

    ThetaX = normalizePositiveInRads(tx);
    ThetaY = normalizePositiveInRads(ty);
    ThetaZ = normalizePositiveInRads(tz);
}

/**
 * @brief obtain KinovaPosefrom CartesianInfo &pose.
 *
 * KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ]. position are in meters, and orientation is Euler-ZYX = Rz(ThetaZ)*Ry(ThetaY)*Rx(ThetaX).
 *
 * @param pose CartesianInfo units are in meters and degrees
 */
KinovaPose::KinovaPose(const CartesianInfo &pose)
{
    X = pose.X;
    Y = pose.Y;
    Z = pose.Z;

    ThetaX = normalizePositiveInRads(pose.ThetaX);
    ThetaY = normalizePositiveInRads(pose.ThetaY);
    ThetaZ = normalizePositiveInRads(pose.ThetaZ);
}

/**
 * @brief construct geometric::Pose message from KinovaPose
 * @return geometry_msgs::Pose[x,y,z,qx,qy,qz,qw] position in meters, orientation is in Quaternion.
 */
geometry_msgs::Pose KinovaPose::constructPoseMsg()
{
    geometry_msgs::Pose pose;
    tf::Quaternion position_quaternion;

   // setEulerZYX(tz, ty, tx) == setRPY(tx, ty, tz) == = mz * my * mx. the previous is Euler angle rotating with temp axis, while latter rotates with fixed axis.
//  position_quaternion.setEulerZYX(ThetaZ, ThetaY, ThetaX);
    position_quaternion.setRPY(ThetaX, ThetaY, ThetaZ);

    tf::quaternionTFToMsg(position_quaternion, pose.orientation);

    pose.position.x = X;
    pose.position.y = Y;
    pose.position.z = Z;

    return pose;
}

/**
 * @brief construct geometric::Wrench message from KinovaPose data structure
 *
 * KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ] doesn't store pose information in this case. Instead, it stores force and torque correspondingly.
 *
 * @return geometry_msgs [Fx,Fy,Fz,Tx,Ty,Tz] in Newton and Nm.
 */
geometry_msgs::Wrench KinovaPose::constructWrenchMsg()
{
    geometry_msgs::Wrench wrench;

    wrench.force.x  = X;
    wrench.force.y  = Y;
    wrench.force.z  = Z;
    wrench.torque.x = ThetaX;
    wrench.torque.y = ThetaY;
    wrench.torque.z = ThetaZ;

    return wrench;
}

bool KinovaPose::isCloseToOther(const KinovaPose &other, float position_tolerance, float EulerAngle_tolerance) const
{
    bool status = true;
    status = status && areValuesClose(X, other.X, position_tolerance);
    status = status && areValuesClose(Y, other.Y, position_tolerance);
    status = status && areValuesClose(Z, other.Z, position_tolerance);
    status = status && areValuesClose(ThetaX, other.ThetaX, EulerAngle_tolerance);
    status = status && areValuesClose(ThetaY, other.ThetaY, EulerAngle_tolerance);
    status = status && areValuesClose(ThetaZ, other.ThetaZ, EulerAngle_tolerance);
    return status;
}

KinovaAngles::KinovaAngles(const kinova_msgs::JointAngles &angles, double j6o)
{
    Actuator1 = normalizePositiveInDegrees(180.0 - (angles.joint1 * (180.0 / M_PI)));
    Actuator2 = normalizePositiveInDegrees((angles.joint2 * (180.0 / M_PI)) + 270.0);
    Actuator3 = normalizePositiveInDegrees(90.0 - (angles.joint3 * (180.0 / M_PI)));
    Actuator4 = normalizePositiveInDegrees(180.0 - (angles.joint4 * (180.0 / M_PI)));
    Actuator5 = normalizePositiveInDegrees(180.0 - (angles.joint5 * (180.0 / M_PI)));
    Actuator6 = normalizePositiveInDegrees(j6o   - (angles.joint6 * (180.0 / M_PI)));
}

KinovaAngles::KinovaAngles(const AngularInfo &angles)
{
    Actuator1 = normalizePositiveInDegrees(angles.Actuator1);
    Actuator2 = normalizePositiveInDegrees(angles.Actuator2);
    Actuator3 = normalizePositiveInDegrees(angles.Actuator3);
    Actuator4 = normalizePositiveInDegrees(angles.Actuator4);
    Actuator5 = normalizePositiveInDegrees(angles.Actuator5);
    Actuator6 = normalizePositiveInDegrees(angles.Actuator6);
}

kinova_msgs::JointAngles KinovaAngles::constructAnglesMsg(double j6o)
{
    kinova_msgs::JointAngles angles;
    angles.joint1 = (180.0 - Actuator1) / (180.0 / M_PI);
    angles.joint2 = (Actuator2 - 270.0) / (180.0 / M_PI);
    angles.joint3 = (90.0 - Actuator3) / (180.0 / M_PI);
    angles.joint4 = (180.0 - Actuator4) / (180.0 / M_PI);
    angles.joint5 = (180.0 - Actuator5) / (180.0 / M_PI);
    angles.joint6 = (j6o   - Actuator6) / (180.0 / M_PI);
    return angles;
}

bool KinovaAngles::isCloseToOther(const KinovaAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Actuator1, other.Actuator1, tolerance);
    status = status && areValuesClose(Actuator2, other.Actuator2, tolerance);
    status = status && areValuesClose(Actuator3, other.Actuator3, tolerance);
    status = status && areValuesClose(Actuator4, other.Actuator4, tolerance);
    status = status && areValuesClose(Actuator5, other.Actuator5, tolerance);
    status = status && areValuesClose(Actuator6, other.Actuator6, tolerance);
    return status;
}

FingerAngles::FingerAngles(const kinova_msgs::FingerPosition &position)
{
    Finger1 = position.finger1;
    Finger2 = position.finger2;
    Finger3 = position.finger3;
}

FingerAngles::FingerAngles(const FingersPosition &angle)
{
    Finger1 = angle.Finger1;
    Finger2 = angle.Finger2;
    Finger3 = angle.Finger3;
}

kinova_msgs::FingerPosition FingerAngles::constructFingersMsg()
{
    kinova_msgs::FingerPosition angles;
    angles.finger1 = Finger1;
    angles.finger2 = Finger2;
    angles.finger3 = Finger3;
    return angles;
}

bool FingerAngles::isCloseToOther(const FingerAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Finger1, other.Finger1, tolerance);
    status = status && areValuesClose(Finger2, other.Finger2, tolerance);
    status = status && areValuesClose(Finger3, other.Finger3, tolerance);
    return status;
}

}  // namespace kinova
<commit_msg>Regulate the output range for KinovaPose<commit_after>/**
 *      _____
 *     /  _  \
 *    / _/ \  \
 *   / / \_/   \
 *  /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
 *  \  / \_/ \  / /  _\| |  | __| / _ \ | ++ \ | ++ \ / _ \ |_   _|| | | |
 *   \ \_/ \_/ /  | |  | |  | ++ | |_| || ++ / | ++_/| |_| |  | |  | +-+ |
 *    \  \_/  /   | |_ | |_ | ++ |  _  || |\ \ | |   |  _  |  | |  | +-+ |
 *     \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
 *             ROBOTICS™
 *
 *  File: kinova_ros_types.cpp
 *  Desc: Wrappers around Kinova structs to facilitate easier conversion to ROS
 *		  types.
 *  Auth: Alex Bencz
 *
 *  Copyright (c) 2013, Clearpath Robotics, Inc.
 *  All Rights Reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Clearpath Robotics, Inc. nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Please send comments, questions, or patches to skynet@clearpathrobotics.com
 *
 */

#include <math.h>
#include <angles/angles.h>
#include <tf/tf.h>
#include <kinova_driver/kinova_ros_types.h>
#include <string>

namespace kinova
{

// %Tag(converntion functions)%

// rads 0 to 2*M_PI
float normalizePositiveInRads(float rads)
{
    return static_cast<float>(angles::normalize_angle_positive(rads));
}

// rads -M_PI to M_PI
float normalizeInRads(float rads)
{
    return static_cast<float>(angles::normalize_angle(rads));
}

// degrees 0 to 360
float normalizePositiveInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle_positive(angles::from_degrees(degrees)));
}

// degrees -180 to 180
float normalizeInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle(angles::from_degrees(degrees)));
}

// %EndTag(converntion functions)%

bool areValuesClose(float first, float second, float tolerance)
{
    return ((first <= second + tolerance) && (first >= second - tolerance));
}

// Exceptions
// ----------
KinovaCommException::KinovaCommException(const std::string& message, const int error_code)
{
    std::stringstream ss;
        ss << "KinovaCommException: " << message << " (return code: " << error_code << ")" << std::endl;
    desc_ = ss.str();
}

const char* KinovaCommException::what() const throw()
{
    return desc_.c_str();
}

// Class definitions
// -----------------
/**
 * @brief obtain KinovaPosefrom geometry_msgs::Pose.
 *
 * obtain KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ] from geometry_msgs::Pose[x,y,z,qx,qy,qz,qw].
 * Euler angles[] are extracted from tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx), where EulerYPR is generated by Quaterion.
 * Units are in meters and radians, The range of Euler angles are [-M_PI, M_PI)
 *
 * @param geometry_msgs ROS standard pose[x,y,z,qx,qy,qz,qw], in meters and quaterion.
 */
KinovaPose::KinovaPose(const geometry_msgs::Pose &pose)
{
    double tx, ty, tz;
    tf::Quaternion q;
    tf::quaternionMsgToTF(pose.orientation, q);

    tf::Matrix3x3 bt_q(q);

    // tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)
    bt_q.getEulerYPR(tz, ty, tx);

    X = static_cast<float>(pose.position.x);
    Y = static_cast<float>(pose.position.y);
    Z = static_cast<float>(pose.position.z);

    ThetaX = normalizeInRads(tx);
    ThetaY = normalizeInRads(ty);
    ThetaZ = normalizeInRads(tz);
}

/**
 * @brief obtain KinovaPosefrom CartesianInfo &pose.
 *
 * KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ]. position are in meters, and orientation is Euler-ZYX = Rz(ThetaZ)*Ry(ThetaY)*Rx(ThetaX). The range of Euler angles are [-M_PI, M_PI)
 *
 * @param pose CartesianInfo units are in meters and degrees
 */
KinovaPose::KinovaPose(const CartesianInfo &pose)
{
    X = pose.X;
    Y = pose.Y;
    Z = pose.Z;

    ThetaX = normalizeInRads(pose.ThetaX);
    ThetaY = normalizeInRads(pose.ThetaY);
    ThetaZ = normalizeInRads(pose.ThetaZ);
}

/**
 * @brief construct geometric::Pose message from KinovaPose
 * @return geometry_msgs::Pose[x,y,z,qx,qy,qz,qw] position in meters, orientation is in Quaternion.
 */
geometry_msgs::Pose KinovaPose::constructPoseMsg()
{
    geometry_msgs::Pose pose;
    tf::Quaternion position_quaternion;

   // setEulerZYX(tz, ty, tx) == setRPY(tx, ty, tz) == = mz * my * mx. the previous is Euler angle rotating with temp axis, while latter rotates with fixed axis.
//  position_quaternion.setEulerZYX(ThetaZ, ThetaY, ThetaX);
    position_quaternion.setRPY(ThetaX, ThetaY, ThetaZ);

    tf::quaternionTFToMsg(position_quaternion, pose.orientation);

    pose.position.x = X;
    pose.position.y = Y;
    pose.position.z = Z;

    return pose;
}

/**
 * @brief construct geometric::Wrench message from KinovaPose data structure
 *
 * KinovaPose[X,Y,Z,ThetaX,ThetaY,ThetaZ] doesn't store pose information in this case. Instead, it stores force and torque correspondingly.
 *
 * @return geometry_msgs [Fx,Fy,Fz,Tx,Ty,Tz] in Newton and Nm.
 */
geometry_msgs::Wrench KinovaPose::constructWrenchMsg()
{
    geometry_msgs::Wrench wrench;

    wrench.force.x  = X;
    wrench.force.y  = Y;
    wrench.force.z  = Z;
    wrench.torque.x = ThetaX;
    wrench.torque.y = ThetaY;
    wrench.torque.z = ThetaZ;

    return wrench;
}

bool KinovaPose::isCloseToOther(const KinovaPose &other, float position_tolerance, float EulerAngle_tolerance) const
{
    bool status = true;
    status = status && areValuesClose(X, other.X, position_tolerance);
    status = status && areValuesClose(Y, other.Y, position_tolerance);
    status = status && areValuesClose(Z, other.Z, position_tolerance);
    status = status && areValuesClose(ThetaX, other.ThetaX, EulerAngle_tolerance);
    status = status && areValuesClose(ThetaY, other.ThetaY, EulerAngle_tolerance);
    status = status && areValuesClose(ThetaZ, other.ThetaZ, EulerAngle_tolerance);
    return status;
}

/**
 * @brief KinovaAngles::KinovaAngles
 *
 *
 *
 * @param angles in radians, input has no limit, but output KinovaAngles are limited to [0,360)
 * @param j6o is 270 for all the robot model, except the 1st generation of jaco robot (260)
 */
KinovaAngles::KinovaAngles(const kinova_msgs::JointAngles &angles, double j6o)
{
    Actuator1 = normalizePositiveInDegrees(180.0 - (angles.joint1 * (180.0 / M_PI)));
    Actuator2 = normalizePositiveInDegrees((angles.joint2 * (180.0 / M_PI)) + 270.0);
    Actuator3 = normalizePositiveInDegrees(90.0 - (angles.joint3 * (180.0 / M_PI)));
    Actuator4 = normalizePositiveInDegrees(180.0 - (angles.joint4 * (180.0 / M_PI)));
    Actuator5 = normalizePositiveInDegrees(180.0 - (angles.joint5 * (180.0 / M_PI)));
    Actuator6 = normalizePositiveInDegrees(j6o   - (angles.joint6 * (180.0 / M_PI)));
}

KinovaAngles::KinovaAngles(const AngularInfo &angles)
{
    Actuator1 = normalizePositiveInDegrees(angles.Actuator1);
    Actuator2 = normalizePositiveInDegrees(angles.Actuator2);
    Actuator3 = normalizePositiveInDegrees(angles.Actuator3);
    Actuator4 = normalizePositiveInDegrees(angles.Actuator4);
    Actuator5 = normalizePositiveInDegrees(angles.Actuator5);
    Actuator6 = normalizePositiveInDegrees(angles.Actuator6);
}

kinova_msgs::JointAngles KinovaAngles::constructAnglesMsg(double j6o)
{
    kinova_msgs::JointAngles angles;
    angles.joint1 = (180.0 - Actuator1) / (180.0 / M_PI);
    angles.joint2 = (Actuator2 - 270.0) / (180.0 / M_PI);
    angles.joint3 = (90.0 - Actuator3) / (180.0 / M_PI);
    angles.joint4 = (180.0 - Actuator4) / (180.0 / M_PI);
    angles.joint5 = (180.0 - Actuator5) / (180.0 / M_PI);
    angles.joint6 = (j6o   - Actuator6) / (180.0 / M_PI);
    return angles;
}

bool KinovaAngles::isCloseToOther(const KinovaAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Actuator1, other.Actuator1, tolerance);
    status = status && areValuesClose(Actuator2, other.Actuator2, tolerance);
    status = status && areValuesClose(Actuator3, other.Actuator3, tolerance);
    status = status && areValuesClose(Actuator4, other.Actuator4, tolerance);
    status = status && areValuesClose(Actuator5, other.Actuator5, tolerance);
    status = status && areValuesClose(Actuator6, other.Actuator6, tolerance);
    return status;
}

FingerAngles::FingerAngles(const kinova_msgs::FingerPosition &position)
{
    Finger1 = position.finger1;
    Finger2 = position.finger2;
    Finger3 = position.finger3;
}

FingerAngles::FingerAngles(const FingersPosition &angle)
{
    Finger1 = angle.Finger1;
    Finger2 = angle.Finger2;
    Finger3 = angle.Finger3;
}

kinova_msgs::FingerPosition FingerAngles::constructFingersMsg()
{
    kinova_msgs::FingerPosition angles;
    angles.finger1 = Finger1;
    angles.finger2 = Finger2;
    angles.finger3 = Finger3;
    return angles;
}

bool FingerAngles::isCloseToOther(const FingerAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Finger1, other.Finger1, tolerance);
    status = status && areValuesClose(Finger2, other.Finger2, tolerance);
    status = status && areValuesClose(Finger3, other.Finger3, tolerance);
    return status;
}

}  // namespace kinova
<|endoftext|>


<commit_before>//
// Copyright (c) 2010 Advanced Micro Devices, Inc. All rights reserved.
//

#ifndef VERSIONS_HPP_
#define VERSIONS_HPP_

#include "utils/macros.hpp"

#ifndef AMD_PLATFORM_NAME
# define AMD_PLATFORM_NAME "AMD Accelerated Parallel Processing"
#endif // AMD_PLATFORM_NAME

#ifndef AMD_PLATFORM_BUILD_NUMBER
# define AMD_PLATFORM_BUILD_NUMBER 1858
#endif // AMD_PLATFORM_BUILD_NUMBER

#ifndef AMD_PLATFORM_REVISION_NUMBER
# define AMD_PLATFORM_REVISION_NUMBER 0
#endif // AMD_PLATFORM_REVISION_NUMBER

#ifndef AMD_PLATFORM_RELEASE_INFO
# define AMD_PLATFORM_RELEASE_INFO
#endif // AMD_PLATFORM_RELEASE_INFO

#define AMD_BUILD_STRING XSTR(AMD_PLATFORM_BUILD_NUMBER) \
    "." XSTR(AMD_PLATFORM_REVISION_NUMBER)

#ifndef AMD_PLATFORM_INFO
# define AMD_PLATFORM_INFO "AMD-APP" AMD_PLATFORM_RELEASE_INFO \
    DEBUG_ONLY("." IF(IS_OPTIMIZED,"opt","dbg")) " (" AMD_BUILD_STRING ")"
#endif // ATI_PLATFORM_INFO

#endif // VERSIONS_HPP_
<commit_msg>P4 to Git Change 1175426 by johtaylo@johtaylo-JTBUILDER03-increment on 2015/07/30 03:00:13<commit_after>//
// Copyright (c) 2010 Advanced Micro Devices, Inc. All rights reserved.
//

#ifndef VERSIONS_HPP_
#define VERSIONS_HPP_

#include "utils/macros.hpp"

#ifndef AMD_PLATFORM_NAME
# define AMD_PLATFORM_NAME "AMD Accelerated Parallel Processing"
#endif // AMD_PLATFORM_NAME

#ifndef AMD_PLATFORM_BUILD_NUMBER
# define AMD_PLATFORM_BUILD_NUMBER 1859
#endif // AMD_PLATFORM_BUILD_NUMBER

#ifndef AMD_PLATFORM_REVISION_NUMBER
# define AMD_PLATFORM_REVISION_NUMBER 0
#endif // AMD_PLATFORM_REVISION_NUMBER

#ifndef AMD_PLATFORM_RELEASE_INFO
# define AMD_PLATFORM_RELEASE_INFO
#endif // AMD_PLATFORM_RELEASE_INFO

#define AMD_BUILD_STRING XSTR(AMD_PLATFORM_BUILD_NUMBER) \
    "." XSTR(AMD_PLATFORM_REVISION_NUMBER)

#ifndef AMD_PLATFORM_INFO
# define AMD_PLATFORM_INFO "AMD-APP" AMD_PLATFORM_RELEASE_INFO \
    DEBUG_ONLY("." IF(IS_OPTIMIZED,"opt","dbg")) " (" AMD_BUILD_STRING ")"
#endif // ATI_PLATFORM_INFO

#endif // VERSIONS_HPP_
<|endoftext|>


<commit_before>#pragma once
#include <tuple>
#include "common.hpp"
#include "check_macro.hpp"

DEF_HASTYPE_T(type)
namespace spn {
	//! std::result_ofの関数対応版
	/*! result_ofのシグニチャと意味が紛らわしいが、気にしない */
	template <class, class>
	struct _ResultOf;
	template <class RT, class... Ts>
	struct _ResultOf<RT (Ts...), std::false_type> { using type = RT; };
	template <class RT, class... Ts>
	struct _ResultOf<RT (&)(Ts...), std::false_type> { using type = RT; };
	template <class RT, class... Ts>
	struct _ResultOf<RT (*)(Ts...), std::false_type> { using type = RT; };
	template <class T>
	struct _ResultOf<T, std::true_type> : std::result_of<T> {};
	template <class T>
	struct ResultOf : _ResultOf<T, HasTypeT_type_t<std::result_of<T>>> {};
	template <class T>
	using ResultOf_t = typename ResultOf<T>::type;

	//! bool値による型の選択
	template <int N, class T, class F>
	struct SelectType {
		using type = F;
	};
	template <class T, class F>
	struct SelectType<1,T,F> {
		using type = T;
	};
	template <int N, template<class> class T, template<class> class F>
	struct SelectTypeT {
		template <class I>
		using type = F<I>;
	};
	template <template<class> class T, template<class> class F>
	struct SelectTypeT<1,T,F> {
		template <class I>
		using type = T<I>;
	};

	//! 型変換判定
	template <class T, class U>
	class Conversion {
		public:
			typedef char Small;
			class Big { char d[2]; };
			static Small Test(U);
			static Big Test(...);
			static T MakeT();

		public:
			enum { exists =
				sizeof(Test(MakeT())) == sizeof(Small),
				sameType = 0};
	};
	template <class T>
	class Conversion<T,T> {
		public:
			enum { exists=1, sameType=1 };
	};
	template <class T>
	class Conversion<void, T> {
		public:
			enum { exists=0, sameType=0 };
	};
	template <class T>
	class Conversion<T, void> {
		public:
			enum { exists=0, sameType=0 };
	};
	template <>
	class Conversion<void, void> {
		public:
			enum { exists=1, sameType=1 };
	};

	template <class T, class U>
	constexpr bool IsDerived() { return Conversion<const U*, const T*>::exists && !Conversion<const T*, const void*>::sameType; }

	//! 特定の型を取り出す
	template <int N, class... TS>
	struct TypeAt;
	template <int N, class T, class... TS>
	struct TypeAt<N, T, TS...> {
		typedef typename TypeAt<N-1, TS...>::type	type;
	};
	template <class T, class... TS>
	struct TypeAt<0,T,TS...> {
		typedef T type;
	};

	//! タイプリストが指定のs型を含んでいるか
	template <class T, int N, class... TS>
	struct TypeFind;
	template <class T, int N, class T0, class... TS>
	struct TypeFind<T,N,T0,TS...> {
		enum { result=TypeFind<T,N+1,TS...>::result };
	};
	template <class T, int N, class... TS>
	struct TypeFind<T,N,T,TS...> {
		enum { result=N };
	};
	template <class T, int N>
	struct TypeFind<T,N> {
		enum { result=-1 };
	};

	//! ビットフィールド用: 必要なビット数を計算
	template <class... TS>
	struct MaxBit {
		enum { result=0 };
	};
	template <class T, class... TS>
	struct MaxBit<T,TS...> {
		enum { result=TValue<T::offset+T::length, MaxBit<TS...>::result>::great };
	};
	//! タイプリストの変数を並べた時に消費するメモリ量を計算
	template <class... TS>
	struct TypeSum {
		enum { result=0 };
	};
	template <class T, class... TS>
	struct TypeSum<T, TS...> {
		enum { result=sizeof(T)+TypeSum<TS...>::result };
	};
	//! インデックス指定による先頭からのサイズ累計
	template <int N, template <class> class Getter, class... Ts2>
	struct _SumN {
		constexpr static int result = 0; };
	template <int N, template <class> class Getter, class T0, class... Ts2>
	struct _SumN<N, Getter, T0, Ts2...> {
		constexpr static int result = Getter<T0>::get() * (N>0 ? 1 : 0) + _SumN<N-1, Getter, Ts2...>::result; };
	//! タイプ指定による先頭からのサイズ累計
	template <class T, template <class> class Getter, class... Ts2>
	struct _SumT;
	template <class T, template <class> class Getter, class T0, class... Ts2>
	struct _SumT<T, Getter, T0, Ts2...> {
		constexpr static int result = Getter<T0>::get() + _SumT<T, Getter, Ts2...>::result; };
	template <class T, template <class> class Getter, class... Ts2>
	struct _SumT<T, Getter, T, Ts2...> {
		constexpr static int result = 0; };
	//! 通常の型サイズ取得クラス
	template <class T>
	struct GetSize_Normal {
		static constexpr int get() { return sizeof(T); }
	};

	template <class... TS>
	class CType;

	//! タイプリストを逆順にした物を返す（ただしCType<CType<A,B>, C> の形になる）
	/*! CPlainと組み合わせて使う */
	template <class... Ts>
	struct CReverse;
	template <class T>
	struct CReverse<T> {
		using type = CType<T>;
	};
	template <class T, class... Ts>
	struct CReverse<T,Ts...> {
		using type = CType<typename CReverse<Ts...>::type, T>;
	};
	//! CType<CType<...>, ...> の入れ子を展開する
	template <class... Ts>
	struct CPlain {
		using type = CType<Ts...>;
	};
	template <class T>
	struct CPlain<T> {
		using type = T;
	};
	template <class... Ts0, class... Ts1>
	struct CPlain<CType<Ts0...>, Ts1...> {
		using type = typename CPlain<Ts0..., Ts1...>::type;
	};
	template <class... Ts0, class... Ts1>
	struct CPlain<CType<CType<Ts0...>, Ts1...>> {
		using type = typename CPlain<Ts0..., Ts1...>::type;
	};

	template <class... TS>
	class CType {
		private:
			template <class T>
			struct _Find {
				enum { result= TypeFind<T,0,TS...>::result };
			};

		public:
			//! インデックス指定で型を取り出す
			template <int N>
			struct At {
				static_assert(N<sizeof...(TS),"At: out of index");
				using type = typename TypeAt<N,TS...>::type;
			};
			//! 末尾に型を追加
			template <class... TS2>
			using Append = CType<TS..., TS2...>;
			//! 先頭に型を追加
			template <class... TS2>
			using Prepend = CType<TS2..., TS...>;
			//! 型のインデックス検索
			template <class T>
			struct Find : _Find<T> {
				static_assert(_Find<T>::result>=0, "Find: not found");
			};
			//! 型を持っていればintegral_constant<bool,true>
			template <class T>
			struct Has : std::integral_constant<bool, _Find<T>::result>=0> {};
			//! std::tupleに変換
			using AsTuple = std::tuple<TS...>;
			//! 要素のそれぞれについてoperator()をした結果の型リストを生成
			template <class Dummy=void>
			struct Another {
				using result = CType<decltype(TS()())...>;
			};
			//! 型リストを逆順にする
			template <class Dummy=void>
			struct Reversed {
				using type = typename CPlain<typename CReverse<TS...>::type>::type;
			};
			//! 自身の型
			using type = CType<TS...>;
			//! 別の型のテンプレート引数に使う
			template <template<class...> class T>
			struct As {
				using type = T<TS...>;
			};

			constexpr static int size = sizeof...(TS),					//!< 型リストの要素数
								sum = TypeSum<TS...>::result,			//!< 要素のサイズ合計
								maxsize = TMax<sizeof(TS)...>::result;	//!< 要素の最大サイズ
			//! タイプリストの位置に対する比較(LessThan)
			template <class T0, class T1>
			using Less = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::lesser>;
			//! タイプリストの位置に対する比較(GreaterThan)
			template <class T0, class T1>
			using Greater = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::greater>;
			//! タイプリストの位置に対する比較(Equal)
			template <class T0, class T1>
			using Equal = std::is_same<T0,T1>;
			//! タイプリストの位置に対する比較(LessThan or Equal)
			template <class T0, class T1>
			using LessEq = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::less_eq>;
			//! タイプリストの位置に対する比較(GreaterThan or Equal)
			template <class T0, class T1>
			using GreatEq = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::great_eq>;

			// 型がimcompleteなケースを考えてテンプレートクラスとしてある
			//! ビットフィールド用: 必要なビット数を計算
			//! 各タイプの::offset, ::lengthフィールドを使って計算
			template <class Dummy=void>
			struct Size {
				constexpr static int maxbit = MaxBit<TS...>::result; };
			//! インデックス指定による先頭からのサイズ累計
			template <int N, template <class> class Getter=GetSize_Normal>
			struct SumN {
				static_assert(N<=sizeof...(TS),"Sum: out of index");
				constexpr static int result = _SumN<N, Getter, TS...>::result; };
			//! タイプ指定による先頭からのサイズ累計
			template <class T, template <class> class Getter=GetSize_Normal>
			struct SumT {
				constexpr static int result = _SumT<T, Getter, TS...>::result; };

		private:
			template <bool flag, int N, class DEFAULT>
			struct __At {
				using type = DEFAULT;
			};
			template <int N, class DEFAULT>
			struct __At<true,N,DEFAULT> {
				using type = typename At<N>::type;
			};
			template <int N, class DEFAULT=void>
			struct _At {
				using type = typename __At<TValue<N,sizeof...(TS)>::lesser, N,DEFAULT>::type;
			};
	};
}

<commit_msg>operator * ()で参照可能ならばその型を取り出すヘルパー関数<commit_after>#pragma once
#include <tuple>
#include "common.hpp"
#include "check_macro.hpp"

DEF_HASTYPE_T(type)
namespace spn {
	//! std::result_ofの関数対応版
	/*! result_ofのシグニチャと意味が紛らわしいが、気にしない */
	template <class, class>
	struct _ResultOf;
	template <class RT, class... Ts>
	struct _ResultOf<RT (Ts...), std::false_type> { using type = RT; };
	template <class RT, class... Ts>
	struct _ResultOf<RT (&)(Ts...), std::false_type> { using type = RT; };
	template <class RT, class... Ts>
	struct _ResultOf<RT (*)(Ts...), std::false_type> { using type = RT; };
	template <class T>
	struct _ResultOf<T, std::true_type> : std::result_of<T> {};
	template <class T>
	struct ResultOf : _ResultOf<T, HasTypeT_type_t<std::result_of<T>>> {};
	template <class T>
	using ResultOf_t = typename ResultOf<T>::type;

	//! bool値による型の選択
	template <int N, class T, class F>
	struct SelectType {
		using type = F;
	};
	template <class T, class F>
	struct SelectType<1,T,F> {
		using type = T;
	};
	template <int N, template<class> class T, template<class> class F>
	struct SelectTypeT {
		template <class I>
		using type = F<I>;
	};
	template <template<class> class T, template<class> class F>
	struct SelectTypeT<1,T,F> {
		template <class I>
		using type = T<I>;
	};

	namespace detail {
		template <class T>
		T dereference_ptr(...);
		template <class T>
		auto dereference_ptr(typename std::remove_reference<decltype(*std::declval<T>())>::type* = nullptr)
			-> typename std::remove_reference<decltype(*std::declval<T>())>::type;

		template <class T>
		auto dereference(T& t, typename std::decay<decltype(*std::declval<T>())>::type*) -> decltype(*std::declval<T>()) {
			return *t;
		}
		template <class T>
		T& dereference(T& t, ...) {
			return t;
		}
	}
	//! *で参照出来る型を取り出す
	template <class T>
	using dereference_ptr_t = decltype(detail::dereference_ptr<T>(nullptr));
	//! *で参照出来る値を取り出す (参照できない場合はそのまま)
	template <class T>
	auto dereference(T& t) -> decltype(detail::dereference(t, nullptr)) {
		return detail::dereference(t, nullptr);
	}

	//! 型変換判定
	template <class T, class U>
	class Conversion {
		public:
			typedef char Small;
			class Big { char d[2]; };
			static Small Test(U);
			static Big Test(...);
			static T MakeT();

		public:
			enum { exists =
				sizeof(Test(MakeT())) == sizeof(Small),
				sameType = 0};
	};
	template <class T>
	class Conversion<T,T> {
		public:
			enum { exists=1, sameType=1 };
	};
	template <class T>
	class Conversion<void, T> {
		public:
			enum { exists=0, sameType=0 };
	};
	template <class T>
	class Conversion<T, void> {
		public:
			enum { exists=0, sameType=0 };
	};
	template <>
	class Conversion<void, void> {
		public:
			enum { exists=1, sameType=1 };
	};

	template <class T, class U>
	constexpr bool IsDerived() { return Conversion<const U*, const T*>::exists && !Conversion<const T*, const void*>::sameType; }

	//! 特定の型を取り出す
	template <int N, class... TS>
	struct TypeAt;
	template <int N, class T, class... TS>
	struct TypeAt<N, T, TS...> {
		typedef typename TypeAt<N-1, TS...>::type	type;
	};
	template <class T, class... TS>
	struct TypeAt<0,T,TS...> {
		typedef T type;
	};

	//! タイプリストが指定のs型を含んでいるか
	template <class T, int N, class... TS>
	struct TypeFind;
	template <class T, int N, class T0, class... TS>
	struct TypeFind<T,N,T0,TS...> {
		enum { result=TypeFind<T,N+1,TS...>::result };
	};
	template <class T, int N, class... TS>
	struct TypeFind<T,N,T,TS...> {
		enum { result=N };
	};
	template <class T, int N>
	struct TypeFind<T,N> {
		enum { result=-1 };
	};

	//! ビットフィールド用: 必要なビット数を計算
	template <class... TS>
	struct MaxBit {
		enum { result=0 };
	};
	template <class T, class... TS>
	struct MaxBit<T,TS...> {
		enum { result=TValue<T::offset+T::length, MaxBit<TS...>::result>::great };
	};
	//! タイプリストの変数を並べた時に消費するメモリ量を計算
	template <class... TS>
	struct TypeSum {
		enum { result=0 };
	};
	template <class T, class... TS>
	struct TypeSum<T, TS...> {
		enum { result=sizeof(T)+TypeSum<TS...>::result };
	};
	//! インデックス指定による先頭からのサイズ累計
	template <int N, template <class> class Getter, class... Ts2>
	struct _SumN {
		constexpr static int result = 0; };
	template <int N, template <class> class Getter, class T0, class... Ts2>
	struct _SumN<N, Getter, T0, Ts2...> {
		constexpr static int result = Getter<T0>::get() * (N>0 ? 1 : 0) + _SumN<N-1, Getter, Ts2...>::result; };
	//! タイプ指定による先頭からのサイズ累計
	template <class T, template <class> class Getter, class... Ts2>
	struct _SumT;
	template <class T, template <class> class Getter, class T0, class... Ts2>
	struct _SumT<T, Getter, T0, Ts2...> {
		constexpr static int result = Getter<T0>::get() + _SumT<T, Getter, Ts2...>::result; };
	template <class T, template <class> class Getter, class... Ts2>
	struct _SumT<T, Getter, T, Ts2...> {
		constexpr static int result = 0; };
	//! 通常の型サイズ取得クラス
	template <class T>
	struct GetSize_Normal {
		static constexpr int get() { return sizeof(T); }
	};

	template <class... TS>
	class CType;

	//! タイプリストを逆順にした物を返す（ただしCType<CType<A,B>, C> の形になる）
	/*! CPlainと組み合わせて使う */
	template <class... Ts>
	struct CReverse;
	template <class T>
	struct CReverse<T> {
		using type = CType<T>;
	};
	template <class T, class... Ts>
	struct CReverse<T,Ts...> {
		using type = CType<typename CReverse<Ts...>::type, T>;
	};
	//! CType<CType<...>, ...> の入れ子を展開する
	template <class... Ts>
	struct CPlain {
		using type = CType<Ts...>;
	};
	template <class T>
	struct CPlain<T> {
		using type = T;
	};
	template <class... Ts0, class... Ts1>
	struct CPlain<CType<Ts0...>, Ts1...> {
		using type = typename CPlain<Ts0..., Ts1...>::type;
	};
	template <class... Ts0, class... Ts1>
	struct CPlain<CType<CType<Ts0...>, Ts1...>> {
		using type = typename CPlain<Ts0..., Ts1...>::type;
	};

	template <class... TS>
	class CType {
		private:
			template <class T>
			struct _Find {
				enum { result= TypeFind<T,0,TS...>::result };
			};

		public:
			//! インデックス指定で型を取り出す
			template <int N>
			struct At {
				static_assert(N<sizeof...(TS),"At: out of index");
				using type = typename TypeAt<N,TS...>::type;
			};
			//! 末尾に型を追加
			template <class... TS2>
			using Append = CType<TS..., TS2...>;
			//! 先頭に型を追加
			template <class... TS2>
			using Prepend = CType<TS2..., TS...>;
			//! 型のインデックス検索
			template <class T>
			struct Find : _Find<T> {
				static_assert(_Find<T>::result>=0, "Find: not found");
			};
			//! 型を持っていればintegral_constant<bool,true>
			template <class T>
			struct Has : std::integral_constant<bool, _Find<T>::result>=0> {};
			//! std::tupleに変換
			using AsTuple = std::tuple<TS...>;
			//! 要素のそれぞれについてoperator()をした結果の型リストを生成
			template <class Dummy=void>
			struct Another {
				using result = CType<decltype(TS()())...>;
			};
			//! 型リストを逆順にする
			template <class Dummy=void>
			struct Reversed {
				using type = typename CPlain<typename CReverse<TS...>::type>::type;
			};
			//! 自身の型
			using type = CType<TS...>;
			//! 別の型のテンプレート引数に使う
			template <template<class...> class T>
			struct As {
				using type = T<TS...>;
			};

			constexpr static int size = sizeof...(TS),					//!< 型リストの要素数
								sum = TypeSum<TS...>::result,			//!< 要素のサイズ合計
								maxsize = TMax<sizeof(TS)...>::result;	//!< 要素の最大サイズ
			//! タイプリストの位置に対する比較(LessThan)
			template <class T0, class T1>
			using Less = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::lesser>;
			//! タイプリストの位置に対する比較(GreaterThan)
			template <class T0, class T1>
			using Greater = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::greater>;
			//! タイプリストの位置に対する比較(Equal)
			template <class T0, class T1>
			using Equal = std::is_same<T0,T1>;
			//! タイプリストの位置に対する比較(LessThan or Equal)
			template <class T0, class T1>
			using LessEq = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::less_eq>;
			//! タイプリストの位置に対する比較(GreaterThan or Equal)
			template <class T0, class T1>
			using GreatEq = std::integral_constant<bool, TValue<Find<T0>::result, Find<T1>::result>::great_eq>;

			// 型がimcompleteなケースを考えてテンプレートクラスとしてある
			//! ビットフィールド用: 必要なビット数を計算
			//! 各タイプの::offset, ::lengthフィールドを使って計算
			template <class Dummy=void>
			struct Size {
				constexpr static int maxbit = MaxBit<TS...>::result; };
			//! インデックス指定による先頭からのサイズ累計
			template <int N, template <class> class Getter=GetSize_Normal>
			struct SumN {
				static_assert(N<=sizeof...(TS),"Sum: out of index");
				constexpr static int result = _SumN<N, Getter, TS...>::result; };
			//! タイプ指定による先頭からのサイズ累計
			template <class T, template <class> class Getter=GetSize_Normal>
			struct SumT {
				constexpr static int result = _SumT<T, Getter, TS...>::result; };

		private:
			template <bool flag, int N, class DEFAULT>
			struct __At {
				using type = DEFAULT;
			};
			template <int N, class DEFAULT>
			struct __At<true,N,DEFAULT> {
				using type = typename At<N>::type;
			};
			template <int N, class DEFAULT=void>
			struct _At {
				using type = typename __At<TValue<N,sizeof...(TS)>::lesser, N,DEFAULT>::type;
			};
	};
}

<|endoftext|>


<commit_before>/*
 * ucrc_t.cpp
 *
 *
 * version 1.0
 *
 *
 * Copyright (c) 2015, Koynov Stas - skojnov@yandex.ru
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  1 Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  2 Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  3 Neither the name of the <organization> nor the
 *    names of its contributors may be used to endorse or promote products
 *   derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "ucrc_t.h"
#include <cstdio>
#include <errno.h>

uCRC_t::uCRC_t(const std::string Name, uint8_t Bits, uint64_t Poly, uint64_t Init, bool RefIn, bool RefOut, uint64_t XorOut) :
    name    (Name),
    bits    (Bits),
    poly    (Poly),
    init    (Init),
    ref_in  (RefIn),
    ref_out (RefOut),
    xor_out (XorOut)
{

    init_class();
}

uCRC_t::uCRC_t(uint8_t Bits, uint64_t Poly, uint64_t Init, bool RefIn, bool RefOut, uint64_t XorOut) :
    bits    (Bits),
    poly    (Poly),
    init    (Init),
    ref_in  (RefIn),
    ref_out (RefOut),
    xor_out (XorOut)
{

    init_class();
}

int uCRC_t::set_bits(uint8_t new_bits)
{
    if( (new_bits < 1) || (new_bits > 64) )
        return -1; //error

    bits = new_bits;
    init_class();

    return 0; //good job
}

uint64_t uCRC_t::get_crc(const char* buf, size_t len) const
{
    uint64_t crc = get_raw_crc(buf, len, crc_init);

    return get_final_crc(crc);
}

int uCRC_t::get_crc(uint64_t &crc, const char* file_name) const
{
    char buf[4096];

    return get_crc(crc, file_name, buf, sizeof(buf));
}

int uCRC_t::get_crc(uint64_t &crc, FILE *pfile) const
{
    char buf[4096];

    return get_crc(crc, pfile, buf, sizeof(buf));
}

int uCRC_t::get_crc(uint64_t &crc, const char *file_name, void *buf, size_t size_buf) const
{
    if( !file_name )
    {
        errno = EINVAL;
        return -1;
    }

    FILE *stream = fopen(file_name, "rb");
    if( stream == NULL )
        return -1; //Cant open file

    int res = get_crc(crc, stream, buf, size_buf);

    fclose(stream);

    return res;
}

int uCRC_t::get_crc(uint64_t &crc, FILE *pfile, void *buf, size_t size_buf) const
{
    if( !pfile || !buf || (size_buf == 0) )
    {
        errno = EINVAL;
        return -1;
    }

    crc          = init;
    long cur_pos = ftell(pfile);
    rewind(pfile);

    while( !feof(pfile) )
    {
       size_t len = fread(buf, 1, size_buf, pfile);
       crc = get_raw_crc((char *)buf, len, crc);
    }

    fseek(pfile, cur_pos, SEEK_SET);

    crc = get_final_crc(crc);

    return 0; //good  job
}

uint64_t uCRC_t::get_raw_crc(const char* buf, size_t len, uint64_t crc) const
{
    if(bits > 8)
    {
        if(ref_in)
            while (len--)
                crc = (crc >> 8) ^ crc_table[ (crc ^ *buf++) & 0xff ];
        else
            while (len--)
                crc = (crc << 8) ^ crc_table[ ((crc >> shift) & 0xff) ^ *buf++ ];
    }
    else
    {
        if (ref_in)
            while (len--)
                crc = crc_table[ crc ^ *buf++ ];
        else
            while (len--)
                crc = crc_table[ (crc << shift) ^ *buf++ ];
    }

    return crc;
}

uint64_t uCRC_t::get_final_crc(uint64_t raw_crc) const
{
    if(ref_out^ref_in) raw_crc = reflect(raw_crc, bits);

    raw_crc ^= xor_out;
    raw_crc &= crc_mask; //for CRC not power 2

    return raw_crc;
}

uint64_t uCRC_t::reflect(uint64_t data, uint8_t num_bits) const
{
    uint64_t reflection = 0;
    uint64_t one = 1;

    for ( size_t i = 0; i < num_bits; ++i, data >>= 1 )
    {
        if ( data & one )
        {
            reflection |= ( one << (num_bits - one - i) );
        }
    }

    return reflection;
}

void uCRC_t::init_crc_table()
{
    int i;
    uint64_t crc;

    for(i = 0; i < 256; i++)
    {

        crc = 0;

        for(uint8_t mask = 0x80; mask; mask >>= 1)
        {

            if ( i & mask )
                crc ^= top_bit;

            if (crc & top_bit)
            {
                crc <<= 1;
                crc ^= poly;
            }
            else
                crc <<= 1;
        }

        crc &= crc_mask; //for CRC not power 2

        if(ref_in)
            crc_table[reflect(i, 8)] = reflect(crc, bits);
        else
            crc_table[i] = crc;
    }
}

void uCRC_t::init_class()
{
    top_bit  = (uint64_t)1 << (bits - 1);
    crc_mask = ( (top_bit - 1) << 1) | 1;

    if(bits > 8)
        shift = (bits - 8);
    else
        shift = (8 - bits);

    if(ref_in)
        crc_init = reflect(init, bits);
    else
        crc_init = init;

    init_crc_table();
}
<commit_msg>BugFix for Big Data SEGFAULT for Bits <= 8<commit_after>/*
 * ucrc_t.cpp
 *
 *
 * version 1.0
 *
 *
 * Copyright (c) 2015, Koynov Stas - skojnov@yandex.ru
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  1 Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  2 Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  3 Neither the name of the <organization> nor the
 *    names of its contributors may be used to endorse or promote products
 *   derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "ucrc_t.h"
#include <cstdio>
#include <errno.h>

uCRC_t::uCRC_t(const std::string Name, uint8_t Bits, uint64_t Poly, uint64_t Init, bool RefIn, bool RefOut, uint64_t XorOut) :
    name    (Name),
    bits    (Bits),
    poly    (Poly),
    init    (Init),
    ref_in  (RefIn),
    ref_out (RefOut),
    xor_out (XorOut)
{

    init_class();
}

uCRC_t::uCRC_t(uint8_t Bits, uint64_t Poly, uint64_t Init, bool RefIn, bool RefOut, uint64_t XorOut) :
    bits    (Bits),
    poly    (Poly),
    init    (Init),
    ref_in  (RefIn),
    ref_out (RefOut),
    xor_out (XorOut)
{

    init_class();
}

int uCRC_t::set_bits(uint8_t new_bits)
{
    if( (new_bits < 1) || (new_bits > 64) )
        return -1; //error

    bits = new_bits;
    init_class();

    return 0; //good job
}

uint64_t uCRC_t::get_crc(const char* buf, size_t len) const
{
    uint64_t crc = get_raw_crc(buf, len, crc_init);

    return get_final_crc(crc);
}

int uCRC_t::get_crc(uint64_t &crc, const char* file_name) const
{
    char buf[4096];

    return get_crc(crc, file_name, buf, sizeof(buf));
}

int uCRC_t::get_crc(uint64_t &crc, FILE *pfile) const
{
    char buf[4096];

    return get_crc(crc, pfile, buf, sizeof(buf));
}

int uCRC_t::get_crc(uint64_t &crc, const char *file_name, void *buf, size_t size_buf) const
{
    if( !file_name )
    {
        errno = EINVAL;
        return -1;
    }

    FILE *stream = fopen(file_name, "rb");
    if( stream == NULL )
        return -1; //Cant open file

    int res = get_crc(crc, stream, buf, size_buf);

    fclose(stream);

    return res;
}

int uCRC_t::get_crc(uint64_t &crc, FILE *pfile, void *buf, size_t size_buf) const
{
    if( !pfile || !buf || (size_buf == 0) )
    {
        errno = EINVAL;
        return -1;
    }

    crc          = init;
    long cur_pos = ftell(pfile);
    rewind(pfile);

    while( !feof(pfile) )
    {
       size_t len = fread(buf, 1, size_buf, pfile);
       crc = get_raw_crc((char *)buf, len, crc);
    }

    fseek(pfile, cur_pos, SEEK_SET);

    crc = get_final_crc(crc);

    return 0; //good  job
}

uint64_t uCRC_t::get_raw_crc(const char* buf, size_t len, uint64_t crc) const
{
    if(bits > 8)
    {
        if(ref_in)
            while (len--)
                crc = (crc >> 8) ^ crc_table[ (crc ^ *buf++) & 0xff ];
        else
            while (len--)
                crc = (crc << 8) ^ crc_table[ ((crc >> shift) ^ *buf++) & 0xff ];
    }
    else
    {
        if (ref_in)
            while (len--)
                crc = crc_table[ (crc ^ *buf++) & 0xff ];
        else
            while (len--)
                crc = crc_table[ ((crc << shift) ^ *buf++) & 0xff ];
    }

    return crc;
}

uint64_t uCRC_t::get_final_crc(uint64_t raw_crc) const
{
    if(ref_out^ref_in) raw_crc = reflect(raw_crc, bits);

    raw_crc ^= xor_out;
    raw_crc &= crc_mask; //for CRC not power 2

    return raw_crc;
}

uint64_t uCRC_t::reflect(uint64_t data, uint8_t num_bits) const
{
    uint64_t reflection = 0;
    uint64_t one = 1;

    for ( size_t i = 0; i < num_bits; ++i, data >>= 1 )
    {
        if ( data & one )
        {
            reflection |= ( one << (num_bits - one - i) );
        }
    }

    return reflection;
}

void uCRC_t::init_crc_table()
{
    int i;
    uint64_t crc;

    for(i = 0; i < 256; i++)
    {

        crc = 0;

        for(uint8_t mask = 0x80; mask; mask >>= 1)
        {

            if ( i & mask )
                crc ^= top_bit;

            if (crc & top_bit)
            {
                crc <<= 1;
                crc ^= poly;
            }
            else
                crc <<= 1;
        }

        crc &= crc_mask; //for CRC not power 2

        if(ref_in)
            crc_table[reflect(i, 8)] = reflect(crc, bits);
        else
            crc_table[i] = crc;
    }
}

void uCRC_t::init_class()
{
    top_bit  = (uint64_t)1 << (bits - 1);
    crc_mask = ( (top_bit - 1) << 1) | 1;

    if(bits > 8)
        shift = (bits - 8);
    else
        shift = (8 - bits);

    if(ref_in)
        crc_init = reflect(init, bits);
    else
        crc_init = init;

    init_crc_table();
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2012. The Regents of the University of California. All rights reserved.
 * Licensed pursuant to the terms and conditions available for viewing at:
 * http://opensource.org/licenses/BSD-3-Clause
 *
 * File: TestLocalizer.cpp
 * Author: Jonathan Ventura
 * Last Modified: 25.2.2013
 */

#include <MultiView/multiview.h>
#include <MultiView/multiview_io_xml.h>
#include <Localizer/localizer.h>
#include <Localizer/nnlocalizer.h>
#include <FeatureMatcher/bruteforce.h>
#include <FeatureExtraction/features.h>

#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <Eigen/Eigen>

#include <iostream>

using namespace vrlt;

void loadImages( std::string prefix, Node *node )
{
    if ( node->camera != NULL ) {
        std::stringstream path;
        path << prefix << "/" << node->camera->path;
        node->camera->image = cv::imread( path.str(), cv::IMREAD_GRAYSCALE );
        node->camera->pyramid = ImagePyramid( node->camera );
    }

    ElementList::iterator it;
    for ( it = node->children.begin(); it != node->children.end(); it++ )
    {
        Node *child = (Node *)it->second;
        loadImages( prefix, child );
    }
}

int main( int argc, char **argv )
{
    if ( argc != 4 && argc != 5 ) {
        fprintf( stderr, "usage: %s <reconstruction> <query> <step> [<test bounds>]\n", argv[0] );
        exit(1);
    }

    std::string pathin = std::string(argv[1]);
    std::string queryin = std::string(argv[2]);
    int step = atoi(argv[3]);
    bool test_bounds = ( argc == 5 );

    Reconstruction r;
    r.pathPrefix = pathin;
    std::stringstream mypath;
    mypath << pathin << "/reconstruction.xml";
    XML::read( r, mypath.str() );

    Node *root = (Node*)r.nodes["root"];
    XML::readDescriptors( r, root );

    loadImages( pathin, root );

    double minY = INFINITY;
    for ( ElementList::iterator it = root->children.begin(); it != root->children.end(); it++ ) {
        Node *node = (Node *)it->second;
        Eigen::Vector3d center = -( node->globalPose().so3().inverse() * node->globalPose().translation() );
        if ( center[1] < minY ) minY = center[1];
    }

    Reconstruction query;
    XML::read( query, queryin );

    Node *queryroot = new Node;
    queryroot->name = "root";

    query.nodes[ "root"] = queryroot;

    ElementList::iterator it;

    NN *index = NULL;

    index = new BruteForceNN;

    NNLocalizer *localizer = new NNLocalizer( root, index );
    localizer->verbose = true;
    localizer->tracker->verbose = false;
    ElementList::iterator camerait = query.cameras.begin();
    Camera *camera = (Camera*)camerait->second;
    Calibration *calibration = camera->calibration;
    cv::Mat image = cv::imread( camera->path, cv::IMREAD_GRAYSCALE );
    int width = image.size().width;
    localizer->thresh = 0.006 * width / camera->calibration->focal;
    localizer->tracker->minnumpoints = 100;

    Calibration *mycalibration = new Calibration;
    Camera *mycamera = new Camera;
    Node *mynode = new Node;
    mycamera->calibration = calibration;
    mycamera->node = mynode;
    mynode->camera = mycamera;
    mynode->parent = NULL;

    int count = 0;
    int goodCount = 0;
    int attemptedCount = 0;
    for ( it = query.cameras.begin(); it != query.cameras.end(); it++,count++ )
    {
        if ( count % step != 0 ) continue;

        Camera *querycamera = (Camera *)it->second;

        Node *querynode = (Node *)querycamera->node;
        if ( querynode == NULL )
        {
            querynode = new Node;
            char name[256];
            sprintf( name, "%s.node", querycamera->name.c_str() );
            querynode->name = name;
            query.nodes[ querynode->name ] = querynode;
            querynode->camera = querycamera;
            querycamera->node = querynode;
        }

        mycamera->name = querycamera->name;
        mynode->pose = querycamera->node->globalPose();
        mycamera->path = querycamera->path;
        mycamera->image = cv::imread( mycamera->path, cv::IMREAD_GRAYSCALE );
        mycamera->pyramid.resize( mycamera->image.size() );
        mycamera->pyramid.copy_from( mycamera->image );

        mycamera->features.clear();
        std::vector<Feature*> features;
		extractSIFT( mycamera->image, features );

        for ( int i = 0; i < features.size(); i++ )
        {
            char name[256];
            sprintf( name, "feature%d", i );
            features[i]->name = name;
            features[i]->camera = mycamera;
            mycamera->features[name] = features[i];
        }

        bool good = localizer->localize( mycamera );

        if ( good && test_bounds ) {
            Sophus::SE3d pose = mycamera->node->pose;
            Eigen::Vector3d center = - ( pose.so3().inverse() * pose.translation() );
            if ( center[1] > 0 || center[1] < minY ) {
                std::cout << "\trejected because of bad pose (Y = " << center[1] << " and minY is " << minY << ")\n";
                good = false;
            }
        }

        if ( good ) goodCount++;
        attemptedCount++;

        if ( querycamera->node->parent != NULL ) querycamera->node->parent->pose = Sophus::SE3d();
        querycamera->node->pose = mynode->pose;

        for ( int i = 0; i < features.size(); i++ )
        {
            delete features[i];
        }

        if ( good ) {
            query.nodes.erase( querycamera->node->name );
            querycamera->node->parent = queryroot;
            queryroot->children[ querycamera->node->name ] = querycamera->node;
        }
    }

    XML::write( query, "localized.xml" );

    return 0;
}
<commit_msg>Changed TestLocalizer to use approximate NN<commit_after>/*
 * Copyright (c) 2012. The Regents of the University of California. All rights reserved.
 * Licensed pursuant to the terms and conditions available for viewing at:
 * http://opensource.org/licenses/BSD-3-Clause
 *
 * File: TestLocalizer.cpp
 * Author: Jonathan Ventura
 * Last Modified: 25.2.2013
 */

#include <MultiView/multiview.h>
#include <MultiView/multiview_io_xml.h>
#include <Localizer/localizer.h>
#include <Localizer/nnlocalizer.h>
#include <FeatureMatcher/approxnn.h>
#include <FeatureExtraction/features.h>

#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <Eigen/Eigen>

#include <iostream>

using namespace vrlt;

void loadImages( std::string prefix, Node *node )
{
    if ( node->camera != NULL ) {
        std::stringstream path;
        path << prefix << "/" << node->camera->path;
        node->camera->image = cv::imread( path.str(), cv::IMREAD_GRAYSCALE );
        node->camera->pyramid = ImagePyramid( node->camera );
    }

    ElementList::iterator it;
    for ( it = node->children.begin(); it != node->children.end(); it++ )
    {
        Node *child = (Node *)it->second;
        loadImages( prefix, child );
    }
}

int main( int argc, char **argv )
{
    if ( argc != 4 && argc != 5 ) {
        fprintf( stderr, "usage: %s <reconstruction> <query> <step> [<test bounds>]\n", argv[0] );
        exit(1);
    }

    std::string pathin = std::string(argv[1]);
    std::string queryin = std::string(argv[2]);
    int step = atoi(argv[3]);
    bool test_bounds = ( argc == 5 );

    Reconstruction r;
    r.pathPrefix = pathin;
    std::stringstream mypath;
    mypath << pathin << "/reconstruction.xml";
    XML::read( r, mypath.str() );

    Node *root = (Node*)r.nodes["root"];
    XML::readDescriptors( r, root );

    loadImages( pathin, root );

    double minY = INFINITY;
    for ( ElementList::iterator it = root->children.begin(); it != root->children.end(); it++ ) {
        Node *node = (Node *)it->second;
        Eigen::Vector3d center = -( node->globalPose().so3().inverse() * node->globalPose().translation() );
        if ( center[1] < minY ) minY = center[1];
    }

    Reconstruction query;
    XML::read( query, queryin );

    Node *queryroot = new Node;
    queryroot->name = "root";

    query.nodes[ "root"] = queryroot;

    ElementList::iterator it;

    NN *index = NULL;

    index = new ApproxNN;

    NNLocalizer *localizer = new NNLocalizer( root, index );
    localizer->verbose = true;
    localizer->tracker->verbose = false;
    ElementList::iterator camerait = query.cameras.begin();
    Camera *camera = (Camera*)camerait->second;
    Calibration *calibration = camera->calibration;
    cv::Mat image = cv::imread( camera->path, cv::IMREAD_GRAYSCALE );
    int width = image.size().width;
    localizer->thresh = 0.006 * width / camera->calibration->focal;
    localizer->tracker->minnumpoints = 100;

    Camera *mycamera = new Camera;
    Node *mynode = new Node;
    mycamera->calibration = calibration;
    mycamera->node = mynode;
    mynode->camera = mycamera;
    mynode->parent = NULL;

    int count = 0;
    int goodCount = 0;
    int attemptedCount = 0;
    for ( it = query.cameras.begin(); it != query.cameras.end(); it++,count++ )
    {
        if ( count % step != 0 ) continue;

        Camera *querycamera = (Camera *)it->second;

        Node *querynode = (Node *)querycamera->node;
        if ( querynode == NULL )
        {
            querynode = new Node;
            char name[256];
            sprintf( name, "%s.node", querycamera->name.c_str() );
            querynode->name = name;
            query.nodes[ querynode->name ] = querynode;
            querynode->camera = querycamera;
            querycamera->node = querynode;
        }

        mycamera->name = querycamera->name;
        mynode->pose = querycamera->node->globalPose();
        mycamera->path = querycamera->path;
        mycamera->image = cv::imread( mycamera->path, cv::IMREAD_GRAYSCALE );
        mycamera->pyramid.resize( mycamera->image.size() );
        mycamera->pyramid.copy_from( mycamera->image );

        mycamera->features.clear();
        std::vector<Feature*> features;
		extractSIFT( mycamera->image, features );

        for ( int i = 0; i < features.size(); i++ )
        {
            char name[256];
            sprintf( name, "feature%d", i );
            features[i]->name = name;
            features[i]->camera = mycamera;
            mycamera->features[name] = features[i];
        }

        bool good = localizer->localize( mycamera );

        if ( good && test_bounds ) {
            Sophus::SE3d pose = mycamera->node->pose;
            Eigen::Vector3d center = - ( pose.so3().inverse() * pose.translation() );
            if ( center[1] > 0 || center[1] < minY ) {
                std::cout << "\trejected because of bad pose (Y = " << center[1] << " and minY is " << minY << ")\n";
                good = false;
            }
        }

        if ( good ) goodCount++;
        attemptedCount++;

        if ( querycamera->node->parent != NULL ) querycamera->node->parent->pose = Sophus::SE3d();
        querycamera->node->pose = mynode->pose;

        for ( int i = 0; i < features.size(); i++ )
        {
            delete features[i];
        }

        if ( good ) {
            query.nodes.erase( querycamera->node->name );
            querycamera->node->parent = queryroot;
            queryroot->children[ querycamera->node->name ] = querycamera->node;
        }
    }

    XML::write( query, "localized.xml" );

    return 0;
}
<|endoftext|>


<commit_before>#include "dictreader.hpp"

//----------------------------------------------------------
DictReader::DictReader()
{

}

//----------------------------------------------------------
DictReader::DictReader(const DictReader& that)
    :
      name_full(that.name_full),
      name_prefix(that.name_prefix),
      dict(that.dict),
      log(that.log),
      status(that.status),
      counter_loaded(that.counter_loaded),
      counter_invalid(that.counter_invalid),
      counter_doubled(that.counter_doubled),
      counter_all(that.counter_all)
{

}

//----------------------------------------------------------
DictReader& DictReader::operator=(const DictReader& that)
{
    name_full = that.name_full;
    name_prefix = that.name_prefix;
    dict = that.dict;
    log = that.log;
    status = that.status;
    counter_loaded = that.counter_loaded;
    counter_invalid = that.counter_invalid;
    counter_doubled = that.counter_doubled;
    counter_all = that.counter_all;
    return *this;
}

//----------------------------------------------------------
DictReader::~DictReader()
{

}

//----------------------------------------------------------
void DictReader::readFile(const std::string &path)
{
    std::ifstream file(path, std::ios::binary);
    if(file)
    {
        std::string content;
        char buffer[16384];
        size_t size = file.tellg();
        content.reserve(size);
        std::streamsize chars_read;
        while(file.read(buffer, sizeof(buffer)), chars_read = file.gcount())
        {
            content.append(buffer, chars_read);
        }
        parseDict(content, path);
    }
    else
    {
        std::cout << "--> Error while loading " + path + " (wrong path)!" << std::endl;
        status = false;
    }
}

//----------------------------------------------------------
void DictReader::setName(const std::string &path)
{
    name_full = path.substr(path.find_last_of("\\/") + 1);
    name_prefix = name_full.substr(0, name_full.find_last_of("."));
}

//----------------------------------------------------------
void DictReader::parseDict(const std::string &content,
                           const std::string &path)
{
    try
    {
        std::string friendly_text;
        size_t pos_beg;
        size_t pos_end;

        std::regex re("<id>(.*?)</id>\\s*<key>(.*?)</key>");
        std::smatch found;
        std::sregex_iterator next(content.begin(), content.end(), re);
        std::sregex_iterator end;
        while(next != end)
        {
            found = *next;

            // no multiline in regex :(
            pos_beg = content.find("<val>", found.position(2)) + 5;
            pos_end = content.find("</val>", pos_beg);
            friendly_text = content.substr(pos_beg, pos_end - pos_beg);

            validateRecord(found[1].str(), found[2].str(), friendly_text);
            counter_all++;
            next++;
        }
        std::cout << "--> Loading " + path + "..." << std::endl;
        printLog();
        setName(path);
        status = true;
    }
    catch(std::exception const& e)
    {
        std::cout << "--> Error in function parseDict() (possibly broken dictionary)!" << std::endl;
        std::cout << "--> Exception: " << e.what() << std::endl;
        status = false;
    }
}

//----------------------------------------------------------
void DictReader::validateRecord(const std::string &id,
                                const std::string &unique_text,
                                const std::string &friendly_text)
{
    if(id == "CELL")
    {
        if(friendly_text.size() > 63)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 63 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::CELL, unique_text, friendly_text);
        }
    }
    else if(id == "GMST")
    {
        insertRecord(yampt::rec_type::GMST, unique_text, friendly_text);
    }
    else if(id == "DESC")
    {
        insertRecord(yampt::rec_type::DESC, unique_text, friendly_text);
    }
    else if(id == "TEXT")
    {
        insertRecord(yampt::rec_type::TEXT, unique_text, friendly_text);
    }
    else if(id == "INDX")
    {
        insertRecord(yampt::rec_type::INDX, unique_text, friendly_text);
    }
    else if(id == "DIAL")
    {
        insertRecord(yampt::rec_type::DIAL, unique_text, friendly_text);
    }
    else if(id == "BNAM")
    {
        insertRecord(yampt::rec_type::BNAM, unique_text, friendly_text);
    }
    else if(id == "SCTX")
    {
        insertRecord(yampt::rec_type::SCTX, unique_text, friendly_text);
    }
    else if(id == "RNAM")
    {
        if(friendly_text.size() > 32)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 32 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::RNAM, unique_text, friendly_text);
        }
    }
    else if(id == "FNAM")
    {
        if(friendly_text.size() > 31)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 31 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::FNAM, unique_text, friendly_text);
        }
    }
    else if(id == "INFO")
    {
        if(friendly_text.size() > 512)
        {
            makeLog(id, unique_text, friendly_text, "Ok, but more than 512 bytes!");
            insertRecord(yampt::rec_type::INFO, unique_text, friendly_text);
        }
        else
        {
            insertRecord(yampt::rec_type::INFO, unique_text, friendly_text);
        }
    }
    else
    {
        makeLog(id, unique_text, friendly_text, "Invalid record!");
        counter_invalid++;
    }

}

//----------------------------------------------------------
void DictReader::insertRecord(yampt::rec_type type,
                              const std::string &unique_text,
                              const std::string &friendly_text)
{
    if(dict[type].insert({unique_text, friendly_text}).second == true)
    {
        counter_loaded++;
    }
    else
    {
        makeLog(yampt::type_name[type], unique_text, friendly_text, "Doubled record!");
        counter_doubled++;
    }
}

//----------------------------------------------------------
void DictReader::makeLog(const std::string &id,
                         const std::string &unique_text,
                         const std::string &friendly_text,
                         const std::string &comment)
{
    log += "<log>\r\n";
    log += "\t<file>" + name_full + "</file>\r\n";
    log += "\t<status>" + comment + "</status>\r\n";
    log += "\t<id>" + id + "</id>\r\n";
    log += "\t<key>" + unique_text + "</key>\r\n";
    log += "\t<val>" + friendly_text + "</val>\r\n";
    log += "<log>\r\n";
}

//----------------------------------------------------------
void DictReader::printLog()
{
    std::cout << "---------------------------------------" << std::endl
              << "    Loaded / Doubled / Invalid /    All" << std::endl
              << "---------------------------------------" << std::endl
              << std::setw(10) << std::to_string(counter_loaded) << " / "
              << std::setw(7) << std::to_string(counter_doubled) << " / "
              << std::setw(7) << std::to_string(counter_invalid) << " / "
              << std::setw(6) << std::to_string(counter_all) << std::endl
              << "---------------------------------------" << std::endl;
}
<commit_msg>cleaning<commit_after>#include "dictreader.hpp"

//----------------------------------------------------------
DictReader::DictReader()
{

}

//----------------------------------------------------------
DictReader::DictReader(const DictReader& that)
    : name_full(that.name_full),
      name_prefix(that.name_prefix),
      dict(that.dict),
      log(that.log),
      status(that.status),
      counter_loaded(that.counter_loaded),
      counter_invalid(that.counter_invalid),
      counter_doubled(that.counter_doubled),
      counter_all(that.counter_all)
{

}

//----------------------------------------------------------
DictReader& DictReader::operator=(const DictReader& that)
{
    name_full = that.name_full;
    name_prefix = that.name_prefix;
    dict = that.dict;
    log = that.log;
    status = that.status;
    counter_loaded = that.counter_loaded;
    counter_invalid = that.counter_invalid;
    counter_doubled = that.counter_doubled;
    counter_all = that.counter_all;
    return *this;
}

//----------------------------------------------------------
DictReader::~DictReader()
{

}

//----------------------------------------------------------
void DictReader::readFile(const std::string &path)
{
    std::ifstream file(path, std::ios::binary);
    if(file)
    {
        std::string content;
        char buffer[16384];
        size_t size = file.tellg();
        content.reserve(size);
        std::streamsize chars_read;
        while(file.read(buffer, sizeof(buffer)), chars_read = file.gcount())
        {
            content.append(buffer, chars_read);
        }
        parseDict(content, path);
    }
    else
    {
        std::cout << "--> Error while loading " + path + " (wrong path)!" << std::endl;
        status = false;
    }
}

//----------------------------------------------------------
void DictReader::setName(const std::string &path)
{
    name_full = path.substr(path.find_last_of("\\/") + 1);
    name_prefix = name_full.substr(0, name_full.find_last_of("."));
}

//----------------------------------------------------------
void DictReader::parseDict(const std::string &content,
                           const std::string &path)
{
    try
    {
        std::string friendly_text;
        size_t pos_beg;
        size_t pos_end;

        std::regex re("<id>(.*?)</id>\\s*<key>(.*?)</key>");
        std::smatch found;
        std::sregex_iterator next(content.begin(), content.end(), re);
        std::sregex_iterator end;
        while(next != end)
        {
            found = *next;

            // no multiline in regex :(
            pos_beg = content.find("<val>", found.position(2)) + 5;
            pos_end = content.find("</val>", pos_beg);
            friendly_text = content.substr(pos_beg, pos_end - pos_beg);

            validateRecord(found[1].str(), found[2].str(), friendly_text);
            counter_all++;
            next++;
        }
        std::cout << "--> Loading " + path + "..." << std::endl;
        printLog();
        setName(path);
        status = true;
    }
    catch(std::exception const& e)
    {
        std::cout << "--> Error in function parseDict() (possibly broken dictionary)!" << std::endl;
        std::cout << "--> Exception: " << e.what() << std::endl;
        status = false;
    }
}

//----------------------------------------------------------
void DictReader::validateRecord(const std::string &id,
                                const std::string &unique_text,
                                const std::string &friendly_text)
{
    if(id == "CELL")
    {
        if(friendly_text.size() > 63)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 63 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::CELL, unique_text, friendly_text);
        }
    }
    else if(id == "GMST")
    {
        insertRecord(yampt::rec_type::GMST, unique_text, friendly_text);
    }
    else if(id == "DESC")
    {
        insertRecord(yampt::rec_type::DESC, unique_text, friendly_text);
    }
    else if(id == "TEXT")
    {
        insertRecord(yampt::rec_type::TEXT, unique_text, friendly_text);
    }
    else if(id == "INDX")
    {
        insertRecord(yampt::rec_type::INDX, unique_text, friendly_text);
    }
    else if(id == "DIAL")
    {
        insertRecord(yampt::rec_type::DIAL, unique_text, friendly_text);
    }
    else if(id == "BNAM")
    {
        insertRecord(yampt::rec_type::BNAM, unique_text, friendly_text);
    }
    else if(id == "SCTX")
    {
        insertRecord(yampt::rec_type::SCTX, unique_text, friendly_text);
    }
    else if(id == "RNAM")
    {
        if(friendly_text.size() > 32)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 32 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::RNAM, unique_text, friendly_text);
        }
    }
    else if(id == "FNAM")
    {
        if(friendly_text.size() > 31)
        {
            makeLog(id, unique_text, friendly_text, "Too long, more than 31 bytes!");
            counter_invalid++;
        }
        else
        {
            insertRecord(yampt::rec_type::FNAM, unique_text, friendly_text);
        }
    }
    else if(id == "INFO")
    {
        if(friendly_text.size() > 512)
        {
            makeLog(id, unique_text, friendly_text, "Ok, but more than 512 bytes!");
            insertRecord(yampt::rec_type::INFO, unique_text, friendly_text);
        }
        else
        {
            insertRecord(yampt::rec_type::INFO, unique_text, friendly_text);
        }
    }
    else
    {
        makeLog(id, unique_text, friendly_text, "Invalid record!");
        counter_invalid++;
    }

}

//----------------------------------------------------------
void DictReader::insertRecord(yampt::rec_type type,
                              const std::string &unique_text,
                              const std::string &friendly_text)
{
    if(dict[type].insert({unique_text, friendly_text}).second == true)
    {
        counter_loaded++;
    }
    else
    {
        makeLog(yampt::type_name[type], unique_text, friendly_text, "Doubled record!");
        counter_doubled++;
    }
}

//----------------------------------------------------------
void DictReader::makeLog(const std::string &id,
                         const std::string &unique_text,
                         const std::string &friendly_text,
                         const std::string &comment)
{
    log += "<log>\r\n";
    log += "\t<file>" + name_full + "</file>\r\n";
    log += "\t<status>" + comment + "</status>\r\n";
    log += "\t<id>" + id + "</id>\r\n";
    log += "\t<key>" + unique_text + "</key>\r\n";
    log += "\t<val>" + friendly_text + "</val>\r\n";
    log += "<log>\r\n";
}

//----------------------------------------------------------
void DictReader::printLog()
{
    std::cout << "---------------------------------------" << std::endl
              << "    Loaded / Doubled / Invalid /    All" << std::endl
              << "---------------------------------------" << std::endl
              << std::setw(10) << std::to_string(counter_loaded) << " / "
              << std::setw(7) << std::to_string(counter_doubled) << " / "
              << std::setw(7) << std::to_string(counter_invalid) << " / "
              << std::setw(6) << std::to_string(counter_all) << std::endl
              << "---------------------------------------" << std::endl;
}
<|endoftext|>


<commit_before>#include "uart.h"
#include "utils.h"
#include "Register.h"
#include "Gpio.h"
#include "system.h"
#include <string.h>

static const uint32_t UART0_BASE = System::getPeripheralBase() + 0x201000;

static Register dr    (UART0_BASE + 0x00);
static Register fr    (UART0_BASE + 0x18);
static Register ibrd  (UART0_BASE + 0x24);
static Register fbrd  (UART0_BASE + 0x28);
static Register lcrh  (UART0_BASE + 0x2C);
static Register cr    (UART0_BASE + 0x30);
static Register imsc  (UART0_BASE + 0x38);
static Register icr   (UART0_BASE + 0x44);

enum {
    LCRH_SEND_BREAK,
    LCRH_ENABLE_PARITY,
    LCRH_EVEN_PARITY,
    LCRH_TWO_STOP_BITS,
    LCRH_ENABLE_FIFOS,
    LCRH_WORLD_LENGTH,
    LCRH_STICK_PARITY = 7
};

enum {
    CR_UART_ENABLE,
    CR_LOOPBACK_ENABLE = 7,
    CR_TRANSMIT_ENABLE,
    CR_RECEIVE_ENABLE,
    CR_RTS = 11,
    CR_RTS_ENABLE = 14,
    CR_CTS_ENABLE = 15
};

enum {
    IMSC_CTS_INT_MASK = 1,
    IMSC_RECEIVE_INT_MASK = 4,
    IMSC_TRANSMIT_INT_MASK,
    IMSC_TIMEOUT_INT_MASK,
    IMSC_FRAMING_INT_MASK,
    IMSC_PARITY_INT_MASK,
    IMSC_BREAK_INT_MASK,
    IMSC_OVERRUN_INT_MASK,
};

Uart::Uart() : mUseLock(false)
{
    // Disable UART0.
    cr.clear();
    // Setup the GPIO pin 14 && 15.
    Gpio::disablePullupAndPulldown(14,15);

    // Clear pending interrupts.
    icr.write(0x7FF);

    // Set integer & fractional part of baud rate.
    // Divider = UART_CLOCK/(16 * Baud)
    // Fraction part register = (Fractional part * 64) + 0.5
    // UART_CLOCK = 3000000; Baud = 115200.

    // Divider = 3000000 / (16 * 115200) = 1.627 = ~1.
    // Fractional part register = (.627 * 64) + 0.5 = 40.6 = ~40.
    ibrd.write(1);
    fbrd.write(40);

    // Enable FIFO & 8 bit data transmission (1 stop bit, no parity).
    lcrh.setOnly(LCRH_ENABLE_FIFOS);
    lcrh.write(3, LCRH_WORLD_LENGTH);

    // Mask all interrupts.
    imsc.setOnly(IMSC_CTS_INT_MASK, IMSC_RECEIVE_INT_MASK, IMSC_TRANSMIT_INT_MASK, IMSC_TIMEOUT_INT_MASK,
                 IMSC_FRAMING_INT_MASK, IMSC_PARITY_INT_MASK, IMSC_BREAK_INT_MASK, IMSC_OVERRUN_INT_MASK);

    // Enable UART0, receive & transfer part of UART.
    cr.setOnly(CR_UART_ENABLE, CR_TRANSMIT_ENABLE, CR_RECEIVE_ENABLE);
}

static void uart_putc(char byte)
{
    // Wait for UART to become ready to transmit.
    while ( fr.get(5) ) { }
    dr.write(byte);
}

#if 0
static unsigned char uart_getc()
{
    // Wait for UART to have recieved something.
    while (REG(UART0_FR) & UART0_RX_FIFO_EMPTY) {

    }
    return REG(UART0_DR);
}
#endif

void Uart::write(const char* buffer, size_t size) {
    if (mUseLock) {
        mLock.lock();
    }
    for (size_t i = 0; i < size; i++) {
        uart_putc(buffer[i]);
    }
    if(mUseLock) {
        mLock.unlock();
    }
}

void Uart::write(const char* buffer) {
    write(buffer, strlen(buffer));
}

void Uart::writeU32(uint32_t x) {
    static const char HEX[] = "0123456789ABCDEF";
    char buf[] = "0x00000000";
    char *p = &buf[2];
    for(int i = 28; i >= 0; i -= 4) {
        *p++ = HEX[(x >> i) % 16];
    }
    write(buf, 10);
}

void Uart::enableLocking() {
    mUseLock = true;
}
<commit_msg>Remove include<commit_after>#include "uart.h"
#include "Register.h"
#include "Gpio.h"
#include "system.h"
#include <string.h>

static const uint32_t UART0_BASE = System::getPeripheralBase() + 0x201000;

static Register dr    (UART0_BASE + 0x00);
static Register fr    (UART0_BASE + 0x18);
static Register ibrd  (UART0_BASE + 0x24);
static Register fbrd  (UART0_BASE + 0x28);
static Register lcrh  (UART0_BASE + 0x2C);
static Register cr    (UART0_BASE + 0x30);
static Register imsc  (UART0_BASE + 0x38);
static Register icr   (UART0_BASE + 0x44);

enum {
    LCRH_SEND_BREAK,
    LCRH_ENABLE_PARITY,
    LCRH_EVEN_PARITY,
    LCRH_TWO_STOP_BITS,
    LCRH_ENABLE_FIFOS,
    LCRH_WORLD_LENGTH,
    LCRH_STICK_PARITY = 7
};

enum {
    CR_UART_ENABLE,
    CR_LOOPBACK_ENABLE = 7,
    CR_TRANSMIT_ENABLE,
    CR_RECEIVE_ENABLE,
    CR_RTS = 11,
    CR_RTS_ENABLE = 14,
    CR_CTS_ENABLE = 15
};

enum {
    IMSC_CTS_INT_MASK = 1,
    IMSC_RECEIVE_INT_MASK = 4,
    IMSC_TRANSMIT_INT_MASK,
    IMSC_TIMEOUT_INT_MASK,
    IMSC_FRAMING_INT_MASK,
    IMSC_PARITY_INT_MASK,
    IMSC_BREAK_INT_MASK,
    IMSC_OVERRUN_INT_MASK,
};

Uart::Uart() : mUseLock(false)
{
    // Disable UART0.
    cr.clear();
    // Setup the GPIO pin 14 && 15.
    Gpio::disablePullupAndPulldown(14,15);

    // Clear pending interrupts.
    icr.write(0x7FF);

    // Set integer & fractional part of baud rate.
    // Divider = UART_CLOCK/(16 * Baud)
    // Fraction part register = (Fractional part * 64) + 0.5
    // UART_CLOCK = 3000000; Baud = 115200.

    // Divider = 3000000 / (16 * 115200) = 1.627 = ~1.
    // Fractional part register = (.627 * 64) + 0.5 = 40.6 = ~40.
    ibrd.write(1);
    fbrd.write(40);

    // Enable FIFO & 8 bit data transmission (1 stop bit, no parity).
    lcrh.setOnly(LCRH_ENABLE_FIFOS);
    lcrh.write(3, LCRH_WORLD_LENGTH);

    // Mask all interrupts.
    imsc.setOnly(IMSC_CTS_INT_MASK, IMSC_RECEIVE_INT_MASK, IMSC_TRANSMIT_INT_MASK, IMSC_TIMEOUT_INT_MASK,
                 IMSC_FRAMING_INT_MASK, IMSC_PARITY_INT_MASK, IMSC_BREAK_INT_MASK, IMSC_OVERRUN_INT_MASK);

    // Enable UART0, receive & transfer part of UART.
    cr.setOnly(CR_UART_ENABLE, CR_TRANSMIT_ENABLE, CR_RECEIVE_ENABLE);
}

static void uart_putc(char byte)
{
    // Wait for UART to become ready to transmit.
    while ( fr.get(5) ) { }
    dr.write(byte);
}

#if 0
static unsigned char uart_getc()
{
    // Wait for UART to have recieved something.
    while (REG(UART0_FR) & UART0_RX_FIFO_EMPTY) {

    }
    return REG(UART0_DR);
}
#endif

void Uart::write(const char* buffer, size_t size) {
    if (mUseLock) {
        mLock.lock();
    }
    for (size_t i = 0; i < size; i++) {
        uart_putc(buffer[i]);
    }
    if(mUseLock) {
        mLock.unlock();
    }
}

void Uart::write(const char* buffer) {
    write(buffer, strlen(buffer));
}

void Uart::writeU32(uint32_t x) {
    static const char HEX[] = "0123456789ABCDEF";
    char buf[] = "0x00000000";
    char *p = &buf[2];
    for(int i = 28; i >= 0; i -= 4) {
        *p++ = HEX[(x >> i) % 16];
    }
    write(buf, 10);
}

void Uart::enableLocking() {
    mUseLock = true;
}
<|endoftext|>


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

WARNING
WARNING
WARNING

GOT WEIRD PROBLEM HAPPENING WHERE IF TURNING_LEFT_GLOBAL, RIGHT_GLOBAL.. AND MOVING_FOWARDS...
if they are initially set to 0, they get set to 1. So setting to -1 for now.*/

/*WARNING WARNING

WARNING

WARNING*/
/************************************************************************************************/

#include <cstdio>
#include <cstdlib>
#include <string>
#include <cmath>
#include <SDL/SDL.h>
#include <SDL/SDL_opengl.h>
#include <SDL/SDL_mixer.h>
#include <ctime>

#include <sstream>
#include <iostream>

#include "main_game.h"
#include "game_state.h"
#include "render_world.h"
#include "defs.h"
#include "audio.h"
#include "texture.h"
#include "maze.h"

using namespace std;

/*Our maze*/ //creates new instance
//Maze maze;

Maze maze;

/* For move function */
 int turning_left_global = 0;
int turning_right_global = 0;
 int moving_fowards_global = 0;

/*Our player data structure - Where we are in the world */
int x_position = 0;
int y_position = 0;
int orientation;

/*Where we start*/
int start_x = 0;
int start_y = 0;

int end_x = 7;
int end_y = 1;

/*Music*/
Mix_Music *world_music;
bool music_on = true;

/*Modifies the players position/orientation based
  on input variables such as moving_fowards_global*/
static void move(){

  printf("Moving fowards: %d \n", moving_fowards_global);

  if(moving_fowards_global == 1){
    if(orientation == NORTH){
      if(maze.value_at(x_position, y_position, NORTH) != 0){
	printf("No");
      }
      else{
	if(y_position < 8){
	  y_position++;
	}

      }
    }
    if(orientation == SOUTH){
      if(maze.value_at(x_position, y_position, SOUTH) != 0){
	printf("No");
      }
      else{
	if(y_position > 0){
	  y_position--;
	}
      }
    }

    if(orientation == EAST){
      if(maze.value_at(x_position, y_position, WEST) != 0){
	printf("No");
      }
      else{
	if(x_position > 0){
	  x_position--;
	}
      }
    }

    if(orientation == WEST){
      if(maze.value_at(x_position, y_position, EAST) != 0){
	printf("No");
      }
      else{
	if(x_position < 8){
	  x_position++;
	}
      }
    }
  }

  else if(turning_right_global == 1){
    if(orientation == WEST){
      orientation = NORTH;
    }
    else{
      orientation++;
    }
  }
  else if(turning_left_global == 1){
    if(orientation == NORTH){
      orientation = WEST ;
    }
    else{
      orientation--;
    }
  }
  if(orientation == SOUTH){
    printf("%d %d SOUTH\n", x_position, y_position);
  }
  if(orientation == NORTH){
    printf("%d %d NORTH\n", x_position, y_position);
  }
  if(orientation == EAST){
    printf("%d %d EAST\n", x_position, y_position);
  }
  if(orientation == WEST){
    printf("%d %d WEST\n", x_position, y_position);
  }

  /*If these aren't reset the player races off at the speed of light*/
  moving_fowards_global = 0;
  turning_left_global = 0;
  turning_right_global = 0;

}

/*Called when a key is released*/
void mainGameKeyboardUp(SDLKey key)
{

  if (key == SDLK_LEFT){
    turning_left_global = 0;
  }
  else if(key == SDLK_RIGHT){
    turning_right_global = 0;
  }

  else if(key == SDLK_UP){
    moving_fowards_global = 0;
  }
  else if(key == SDLK_m){
    music_on = !music_on;
  }
}

/*Called when a key is held down*/
void mainGameKeyboardDown(SDLKey key){
  printf("KEY\n");

  if(key == SDLK_LEFT){

    turning_left_global = 1;
  }

  else if(key == SDLK_UP){
    moving_fowards_global = 1;
  }

  else if(key == SDLK_RIGHT){
    turning_right_global = 1;
  }
}

/*Our main game function.. checks for input, moves and redraws the screen.

  TODO: Needs to be changed to a timer function later so it dosen't hog 100%cpu */
void mainGameUpdate(void){

  if(turning_left_global == 1 || turning_right_global == 1
     || moving_fowards_global == 1){
    printf("Moving\n");
    move();
  }

  if(x_position == end_x && y_position == end_y){

    printf("You win!\n");
    exit(0);
  }
  //glutPostRedisplay(); don't think there is a SDL equivalent of this
  // so taking it out.
  //check if music should be playing
  if(!music_on && isAudioPlaying()){
    pauseAudio();
  }else if(music_on && !isAudioPlaying()){
    resumeAudio();
  }
}

/*Display function - called from main - This function is called as
 * often as possible - Whenever the idle loop finishes*/
void mainGameRender(void){
  renderWorld();
}

/*Leslies code*/
static void initTextures(){

  srand(time(NULL));

  int floor_tex_num = 0;
  int wall_tex_num = 0;

  floor_tex_num = rand() % 8; //SINCE THERE ARE 8 FLOOR TEXTURES ATM.
  wall_tex_num = rand() % 3; // 3 wall textures atm.

  string w_string = "data/images/textures/w";

  ostringstream oss_wall;
  oss_wall <<wall_tex_num;

  string w_num = oss_wall.str();

  w_string += w_num;
  w_string += ".bmp";

  string f_string = "data/images/textures/f";

  ostringstream oss_floor;
  oss_floor << floor_tex_num;

  string f_num = oss_floor.str();

  f_string += f_num;

  f_string += ".bmp";

  char *c_w_string = (char*)w_string.c_str();
  char *c_f_string = (char*)f_string.c_str();

  cout << c_w_string << endl;
  cout << c_f_string << endl;

  wall_texture = genTexture(c_w_string);
  floor_texture = genTexture(c_f_string);
  finish_texture = genTexture("data/images/textures/finish.bmp");

}

static void freeTextures(){
  glDeleteTextures( 1, &wall_texture);
  glDeleteTextures(1, &floor_texture);
}

static void initMusic(){
  world_music = loadAudioFile("data/audio/music/gamemusic.ogg");
  //might as well start it playing
  if(music_on)
    playAudio(world_music,-1);//-1 loops forever
}

static void freeMusic(){
  unloadAudioFile(world_music);
}

//This code will need to be moved to main_game later.
/*Initialization of the game*/
int mainGameInit(){

  x_position = start_x;
  y_position = start_y;
  orientation = WEST;

  initTextures();
  initMusic();

  set_maze(maze);

  reshape(SCREEN_WIDTH,SCREEN_HEIGHT);
  return 0;
}

int mainGameDeInit(){
  if(isAudioPlaying())
    stopAllAudio();
  freeTextures();
  freeMusic();
  return 0;
}

<commit_msg>Added a SDL_Delay  in the main game loop, so the game does not take 100% CPU now, and only around 10, 20 %<commit_after>/***********************************************
 ***********************************************

WARNING
WARNING
WARNING

GOT WEIRD PROBLEM HAPPENING WHERE IF TURNING_LEFT_GLOBAL, RIGHT_GLOBAL.. AND MOVING_FOWARDS...
if they are initially set to 0, they get set to 1. So setting to -1 for now.*/

/*WARNING WARNING

WARNING

WARNING*/
/************************************************************************************************/

#include <cstdio>
#include <cstdlib>
#include <string>
#include <cmath>
#include <SDL/SDL.h>
#include <SDL/SDL_opengl.h>
#include <SDL/SDL_mixer.h>
#include <ctime>

#include <sstream>
#include <iostream>

#include "main_game.h"
#include "game_state.h"
#include "render_world.h"
#include "defs.h"
#include "audio.h"
#include "texture.h"
#include "maze.h"

using namespace std;

/*Our maze*/ //creates new instance
//Maze maze;

Maze maze;

/* For move function */
 int turning_left_global = 0;
int turning_right_global = 0;
 int moving_fowards_global = 0;

/*Our player data structure - Where we are in the world */
int x_position = 0;
int y_position = 0;
int orientation;

/*Where we start*/
int start_x = 0;
int start_y = 0;

int end_x = 7;
int end_y = 1;

/*Music*/
Mix_Music *world_music;
bool music_on = true;

/*Modifies the players position/orientation based
  on input variables such as moving_fowards_global*/
static void move(){

  printf("Moving fowards: %d \n", moving_fowards_global);

  if(moving_fowards_global == 1){
    if(orientation == NORTH){
      if(maze.value_at(x_position, y_position, NORTH) != 0){
	printf("No");
      }
      else{
	if(y_position < 8){
	  y_position++;
	}

      }
    }
    if(orientation == SOUTH){
      if(maze.value_at(x_position, y_position, SOUTH) != 0){
	printf("No");
      }
      else{
	if(y_position > 0){
	  y_position--;
	}
      }
    }

    if(orientation == EAST){
      if(maze.value_at(x_position, y_position, WEST) != 0){
	printf("No");
      }
      else{
	if(x_position > 0){
	  x_position--;
	}
      }
    }

    if(orientation == WEST){
      if(maze.value_at(x_position, y_position, EAST) != 0){
	printf("No");
      }
      else{
	if(x_position < 8){
	  x_position++;
	}
      }
    }
  }

  else if(turning_right_global == 1){
    if(orientation == WEST){
      orientation = NORTH;
    }
    else{
      orientation++;
    }
  }
  else if(turning_left_global == 1){
    if(orientation == NORTH){
      orientation = WEST ;
    }
    else{
      orientation--;
    }
  }
  if(orientation == SOUTH){
    printf("%d %d SOUTH\n", x_position, y_position);
  }
  if(orientation == NORTH){
    printf("%d %d NORTH\n", x_position, y_position);
  }
  if(orientation == EAST){
    printf("%d %d EAST\n", x_position, y_position);
  }
  if(orientation == WEST){
    printf("%d %d WEST\n", x_position, y_position);
  }

  /*If these aren't reset the player races off at the speed of light*/
  moving_fowards_global = 0;
  turning_left_global = 0;
  turning_right_global = 0;

}

/*Called when a key is released*/
void mainGameKeyboardUp(SDLKey key)
{

  if (key == SDLK_LEFT){
    turning_left_global = 0;
  }
  else if(key == SDLK_RIGHT){
    turning_right_global = 0;
  }

  else if(key == SDLK_UP){
    moving_fowards_global = 0;
  }
  else if(key == SDLK_m){
    music_on = !music_on;
  }
}

/*Called when a key is held down*/
void mainGameKeyboardDown(SDLKey key){
  printf("KEY\n");

  if(key == SDLK_LEFT){

    turning_left_global = 1;
  }

  else if(key == SDLK_UP){
    moving_fowards_global = 1;
  }

  else if(key == SDLK_RIGHT){
    turning_right_global = 1;
  }
}

/*Our main game function.. checks for input, moves and redraws the screen.

  TODO: Needs to be changed to a timer function later so it dosen't hog 100%cpu */
void mainGameUpdate(void){

  if(turning_left_global == 1 || turning_right_global == 1
     || moving_fowards_global == 1){
    printf("Moving\n");
    move();
  }

  if(x_position == end_x && y_position == end_y){

    printf("You win!\n");
    exit(0);
  }
  //glutPostRedisplay(); don't think there is a SDL equivalent of this
  // so taking it out.
  //check if music should be playing
  if(!music_on && isAudioPlaying()){
    pauseAudio();
  }else if(music_on && !isAudioPlaying()){
    resumeAudio();
  }

  SDL_Delay(10);
}

/*Display function - called from main - This function is called as
 * often as possible - Whenever the idle loop finishes*/
void mainGameRender(void){
  renderWorld();
}

/*Leslies code*/
static void initTextures(){

  srand(time(NULL));

  int floor_tex_num = 0;
  int wall_tex_num = 0;

  floor_tex_num = rand() % 8; //SINCE THERE ARE 8 FLOOR TEXTURES ATM.
  wall_tex_num = rand() % 3; // 3 wall textures atm.

  string w_string = "data/images/textures/w";

  ostringstream oss_wall;
  oss_wall <<wall_tex_num;

  string w_num = oss_wall.str();

  w_string += w_num;
  w_string += ".bmp";

  string f_string = "data/images/textures/f";

  ostringstream oss_floor;
  oss_floor << floor_tex_num;

  string f_num = oss_floor.str();

  f_string += f_num;

  f_string += ".bmp";

  char *c_w_string = (char*)w_string.c_str();
  char *c_f_string = (char*)f_string.c_str();

  cout << c_w_string << endl;
  cout << c_f_string << endl;

  wall_texture = genTexture(c_w_string);
  floor_texture = genTexture(c_f_string);
  finish_texture = genTexture("data/images/textures/finish.bmp");

}

static void freeTextures(){
  glDeleteTextures( 1, &wall_texture);
  glDeleteTextures(1, &floor_texture);
}

static void initMusic(){
  world_music = loadAudioFile("data/audio/music/gamemusic.ogg");
  //might as well start it playing
  if(music_on)
    playAudio(world_music,-1);//-1 loops forever
}

static void freeMusic(){
  unloadAudioFile(world_music);
}

//This code will need to be moved to main_game later.
/*Initialization of the game*/
int mainGameInit(){

  x_position = start_x;
  y_position = start_y;
  orientation = WEST;

  initTextures();
  initMusic();

  set_maze(maze);

  reshape(SCREEN_WIDTH,SCREEN_HEIGHT);
  return 0;
}

int mainGameDeInit(){
  if(isAudioPlaying())
    stopAllAudio();
  freeTextures();
  freeMusic();
  return 0;
}

<|endoftext|>


<commit_before>//cci_shared_lib.cpp    william k. johnson 2017
//

#include <iostream>
#include <cci_shared_lib.h>

using namespace cci_daemon_impl;

//--------------------------------------------------------------------------------
cci_handle_t cci_shared_lib::load( const std::string& lib )
{
            std::ostringstream ostr;
            ostr << "lib"
                 << lib
                 << ".so"
                 << std::ends;
            std::string path_with_extension = ostr.str();
            cci_handle_t shared_object = ::dlopen( path_with_extension.c_str() ,
                                                   RTLD_NOW) ;
            if( shared_object == nullptr )
            {
                throw std::runtime_error( std::string( "could not load '")
                                          + path_with_extension + "'");
            }

            std::cerr << "...loaded library...."
                      << path_with_extension
                      << "...\n";

            return shared_object;
}

//--------------------------------------------------------------------------------
void  cci_shared_lib::unload( cci_handle_t shared_handle )
{
          if ( shared_handle ) { dlclose( shared_handle); }
}

<commit_msg>kernel<commit_after>//cci_shared_lib.cpp    chromatic universe william k. johnson 2017
//

#include <iostream>
#include <cci_shared_lib.h>

using namespace cci_daemon_impl;

//--------------------------------------------------------------------------------
cci_handle_t cci_shared_lib::load( const std::string& lib )
{
            std::ostringstream ostr;
            ostr << "lib"
                 << lib
                 << ".so"
                 << std::ends;
            std::string path_with_extension = ostr.str();
            cci_handle_t shared_object = ::dlopen( path_with_extension.c_str() ,
                                                   RTLD_NOW) ;
            if( shared_object == nullptr )
            {
                throw std::runtime_error( std::string( "could not load '")
                                          + path_with_extension + "'");
            }

            std::cerr << "...loaded library...."
                      << path_with_extension
                      << "...\n";

            return shared_object;
}

//--------------------------------------------------------------------------------
void  cci_shared_lib::unload( cci_handle_t shared_handle )
{
          if ( shared_handle ) { dlclose( shared_handle); }
}

<|endoftext|>


<commit_before><commit_msg>more clean-up<commit_after><|endoftext|>