text:
#include "testerMain.h"

#include <iostream>
#include <time.h>
#include "MovementLib.h"
#include <TypeIRML.h>
#include <exception>

#include <boost/thread/thread.hpp>

#ifndef PI
#define PI			3.1415926535897932384626433832795
#endif

#ifndef RAD
#define RAD(A)	((A) * PI / 180.0 )
#endif

#ifndef DEG
#define DEG(A)	((A) * 180.0 / PI )
#endif

//using Eigen::Matrix4d;

//*******************************************************************************************
// main()

int main(int argc, char *argv[])
{
	int		i=0
			,	ResultValue
			;

    std::cout << "########################################\n"
                 << "              Welcome\n########################################\n"
                 << std::endl;

    FRIBasicDriverCPP * fd = NULL;

	// connect
    try {
        fd = new FRIBasicDriverCPP();
    }
    catch (int e)
    {
        printf("An Exception occured while connecting to the FRIDriver: %d\n", e);
        return 0;
    }
    printf("\n\n");

	// ******************************************************************************************
	// Calling test methods to receive and print the available robot informations
	// remove comments to run

	//testGetterCompleteInformation(&fd);
	//testGetterMethods(&fd);

	// set new velocity and acceleration:
//	fd->setMaxAcceleration(90.0);
//	fd->setMaxVelocity(90.0);

#if 1
    // print system status
    //
    printf("Reached Position with Cartesian Velocity Control\n%s\n",fd->GetCompleteRobotStateAndInformation());

    // Check for communication quality and if Robot is reachable
    //
    ResultValue = fd->IsCommunicationOk();
    if(!ResultValue) {
        printf("ERROR: bad communication! FRIMode is turned OFF!");
        return 0;
    } else {
        printf("Robot is turned on and reachable.");
    }
#endif

    MovementLib mv = MovementLib(fd);

#if 0
    // get cart-position and print as e6pos
    //

    float measured_position[12];
    e6pos pose;
    fd->GetMeasuredCartPose(measured_position);
    pose = mv.calcPoseFromFloatArray(measured_position, 12);
    printf("Measured Current Position:\n");
    mv.printE6POS(pose);
    printf("*****************************\n");
#endif

#if 0
	// ******************************************************************************************
    // Move to some position using joint ptp movement
    // STARTING POSITION!!
    //

    printf("Moving to pos2\n");

//	double pos2[] = {90.0, 0.0, 0.0, -90.0, 0.0, 90.0, 0.0};
// some other pose, close to pose2 of the cartesian trajectory tests
//    double pos2[] = {90.0, 35.0, 0.0, -90.0, -0.0, 55.0, 45.0, 0.0};
    double pos2[] = {-78.812, 39.331, 5.999, -84.519, -1.021, 51.064, 32.190};
    fd->mvJoint(pos2);
    fd->Stop();
    printf("Reached Position with Cartesian Velocity Control\n%s\n",fd->GetCompleteRobotStateAndInformation());

#endif

#if 0
    // ******************************************************************************************
    // Move to some position
    // Control Scheme 10 = JOINT_POSITION_CONTROL
    //
    fd->StartInControlScheme(10);

    if(!fd->IsMachineOK())
    {
        printf("Error: machine is not OK!");
    }
    else
    {
        float jointValuesRad[NUMBER_OF_JOINTS];
        fd->getMeasuredJointPositions(jointValuesRad);

        for(int i=0; i<100; ++i)
        {
            jointValuesRad[0] -= 0.015;
            fd->cmdVelJoint(jointValuesRad);
        }
        printf(".\n");
    }
#endif

#if 0
    // ******************************************************************************************
    // test very simple Cartesian Velocity Control
    //
    printf("testerMain::\tperform movement with cartesian impedance control\n");
    if(!testCmdVelCart(fd)) {
        printf("testerMain::\tCould not finish movement. Stopping execution.\n");
        fd->Stop();
        return 0;
    }

    //printf("Reached Position with Cartesian Velocity Control\n%s\n",fd->GetCompleteRobotStateAndInformation());
#endif

#if 0
    // ******************************************************************************************
    // Move to Test-Position 2 (cartesian velocity control)
    //
    MovementLib mv = MovementLib(fd);

    e6pos p;
    p.x = 0.0;
    p.y = -0.439;
    p.z = 0.311;
    p.a = 91.000;
    p.b = -1.000;
    p.c = 91.000;

    ResultValue = mv.movePTPCart(p, 5);
    if(!ResultValue) {
        printf("\tTestFindHole::runTest01() \tERROR: could not finish movement!\n");
        return false;
    }
    mv.~MovementLib();

#endif

    while(true) {

#if 1
        // ******************************************************************************************
        // move to starting position using cartesian position
        //
        printf("\ttesterMain::\tMoving to starting position");
        e6pos startPose;
        startPose.x = -0.084326;
        startPose.y =  0.601255;
        startPose.z =  0.304220;
        startPose.a = -60.178525;
        startPose.b =   6.514957;
        startPose.c =  88.356525;
        ResultValue = mv.movePTPCart(startPose, 5);
        if(!ResultValue) {
            printf("testerMain::\tCould not finish movement. Stopping execution.\n");
    //        fd->Stop();
            return 0;
        }
        boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
#endif

#if 1
        // ******************************************************************************************
        // tester
        // find a hole using a zig-zag path and test pressure
        //
        printf("testerMain::\ttest ''FindHole functions''\n");
        ResultValue = findHoleTester(fd);
        if(!ResultValue) {
            printf("testerMain::\tCould not finish movement. Stopping execution.\n");
    //        fd->Stop();
            return 0;
        }
        printf("testerMain::\tpausing....\n");
        boost::this_thread::sleep(boost::posix_time::milliseconds(5000));
#endif

#if 1
        // move upwards to get out of critical positions
        //
        e6pos pose2;
        pose2.x =   0.143531;
        pose2.y =   0.657165;
        pose2.z =   0.143250;
        pose2.a = -60.549;
        pose2.b =   0.89;
        pose2.c =  88.0;

    //    MovementLib mv = MovementLib(fd);

        e6pos estimatedCenter = pose2;
        e6pos startPos = estimatedCenter;
        startPos.z = startPos.z + 0.1;
        ResultValue = mv.movePTPCart(startPos, 3);
        if(!ResultValue) {
            printf("\tTestFindHole::runTest01() \tERROR: could not finish movement!\n");
            return false;
        }
        printf("\tTestFindHole::runTest01() \tmoved out of tool-zone.\n");
#endif

    }

#if 0
	// ******************************************************************************************
	// Move back to home position
    //
	// set new velocity and acceleration:
    fd->setMaxAcceleration(50.0);
    fd->setMaxVelocity(50.0);

	double homePosition[] = {100.0, 45, 0, -90, 0.0, -45, 0.0};
    printf("\nMoving to home position\n");
    fd->mvJoint(homePosition);
    fd->Stop();
#endif

#if 0
    // ******************************************************************************************
    // loop X seconds and print forces and torques
    //
    printf("testerMain::\tPrint Forces and Torques: \n");
    float forceAndTorqeValues[6];

    for(int i=0;i<turns;i++) {

        getForces(forcesAndTorques);
        printf("\r%d:\t%f\t%f\t%f", i, forcesAndTorques[0], forcesAndTorques[1], forcesAndTorques[2]);
        boost::this_thread::sleep(boost::posix_time::milliseconds(100));
    }
#endif

    //printf("%s\n",fd->GetCompleteRobotStateAndInformation());

    mv.~MovementLib();

    fd->Stop();

    fd->~FRIBasicDriverCPP();

    return 0;
}

/*============================================================================================
 * Tester Methods
 *============================================================================================
 */

bool findHoleTester(FRIBasicDriverCPP *_fd)
{
    MovementLib mv = MovementLib(_fd);

    // estimated hole-position
    //
    e6pos pose;
    pose.x =   0.146531;
    pose.y =   0.657165;
    pose.z =   0.143250;
    pose.a = -60.549;
    pose.b =   0.0;
    pose.c =  90.0;

    double errorTolerance   = 1.0,  // cm
           speed            = 1.5,    // cm/s
           angle            = 85;   // degree

    bool ret = mv.findHole(pose, errorTolerance, speed, angle);

    mv.~MovementLib();

    return ret;
}

// tester method for cartesian impedance control
// moving 20cm in one direction
bool testCmdVelCart(FRIBasicDriverCPP *_fd)
{
    // declare variables
    int     i = 0,
            counter = 0;
    bool    endPositionReached  = false,
            resultValue         = true,
            negDist             = false;
    float   MeasuredCartPose[NUMBER_OF_FRAME_ELEMENTS],
            StartPose[NUMBER_OF_FRAME_ELEMENTS],
            EndPose[NUMBER_OF_FRAME_ELEMENTS],
            CommandedPose[NUMBER_OF_FRAME_ELEMENTS],
            tolerance           = 0.01,
            dist                = 0.0,
            xVal                = 0.0;

    printf("NumberOfFrameElements: %d\n", NUMBER_OF_FRAME_ELEMENTS);

    // check and restart robot in proper control scheme
    //
    _fd->StartInControlScheme(20); // 20 == Cartesian Impedance Control

    // get current cartesian position
    //
    _fd->GetMeasuredCartPose(StartPose);
    printFloatArray(StartPose, 12);

    // copy starting-values to end-pose
    //
    for(i=0;i<NUMBER_OF_FRAME_ELEMENTS;i++)
    {
        EndPose[i] = StartPose[i];
        CommandedPose[i] = StartPose[i];
    }

    // set new end-position (only x-value will be changed)
    //
    EndPose[3] = EndPose[3] + 0.2;
    xVal = StartPose[3];

    // loop
    //
    counter = 0;
    while(!endPositionReached)
    {
        // check if machine is ok
        if(!_fd->IsMachineOK())
        {
            printf("\ttestMain::testCmdVelCart() \tMachine is not ready!\n");
            return false;
        }

        // DEBUG: stop after X loops
//        if(counter >= 50) return true;

        // check if final position is reached
        //
        _fd->GetMeasuredCartPose(MeasuredCartPose);

        // range of 0.002 m (2 milimeter)
        tolerance = 0.002;
        dist = EndPose[3] - MeasuredCartPose[3];
        negDist = false;
        if(dist < 0.0) {
            dist*(-1.0);
            negDist = true;
        }
        if((counter%10) == 0) {
            printf("\ttesterMain::testCmdVelCart() \tremaining distance: %f\n", dist);
        }
        if(dist < tolerance)
        {
            printf("\ttesterMain::testCmdVelCart() \tEnd-position reached.\n");
            endPositionReached = true;
            break;
        }

        // add 0.5 milimeter in one direction (e.g. x) until start-position + 20cm is reached
        //
        if(negDist){
            CommandedPose[3] = xVal - (((float) counter / 2.0) / 1000.0);
        } else {
            CommandedPose[3] = xVal + (((float) counter / 2.0) / 1000.0);
        }

        // move
        resultValue = _fd->cmdVelCart(CommandedPose);
        if(!resultValue) {
            printf("\ttesterMain::testCmdVelCart() \tSome unknown error occured, stopped movement.\n");
            return false;
        }

        // increase counter
        counter++;
    }
    return true;
}

// make a Velocity Joint Movement to a set position
void testCmdVelJoint(FRIBasicDriverCPP *fd, double *velPos)
{
	// for velocity control, YOU have to make sure, that the robot is in the correct control scheme and to start the Robot.
    fd->StartInControlScheme(10);

	if(!fd->IsMachineOK())
	{
        printf("Error: machine is not OK!");
	}

	// ********************************************************
	// It depends on you, how you calculate the single steps.
	// in this Tester-Method I'm using the TypeIRML Library for the calculations. Any other calculation will do fine too.

	bool						MachineOK					=	true	;
	int							i							=	0
							,	ResultValue					=	0		;
	double						CycleTime					=	0.002	;
	float						JointValuesInRad[NUMBER_OF_JOINTS]		;

    TypeIRML					*RML						=	NULL	;
	TypeIRMLInputParameters		*IP							=	NULL	;
	TypeIRMLOutputParameters	*OP							=	NULL	;
	RML					=	new TypeIRML(		NUMBER_OF_JOINTS
											,	CycleTime			);
	IP					=	new TypeIRMLInputParameters(NUMBER_OF_JOINTS);
	OP					=	new TypeIRMLOutputParameters(NUMBER_OF_JOINTS);

	memset(JointValuesInRad, 0x0, NUMBER_OF_JOINTS * sizeof(float));

	// print target position
    printf("Target Joint Values: \n");
    for(i=0;i<NUMBER_OF_JOINTS;i++) {printf("     %f", velPos[i]);}
    printf("\n");

	// check if robot is in correct control scheme and OK
	if(fd->getCurrentControlScheme() != 10 || !fd->IsMachineOK())
	{
        printf("Robot is not ready for movement.\n");
		delete	RML;
		delete	IP;
		delete	OP;

		return;
	}

	// get current position
	fd->getMeasuredJointPositions(JointValuesInRad);

	// print current position
    printf("Current Joint Positions: \n");
    for(i=0;i<NUMBER_OF_JOINTS;i++) {printf("     %f", JointValuesInRad[i]);}
    printf("\n");

	// set current position as commanded position.

	// setting parameter for TypeIRML calculations
	for ( i = 0; i < NUMBER_OF_JOINTS; i++)
	{
		IP->CurrentPosition->VecData		[i] =	(double)DEG(JointValuesInRad[i]);
		IP->TargetPosition->VecData			[i] =	(double)velPos[i];
		IP->MaxVelocity->VecData			[i] =	(double)50.0;
		IP->MaxAcceleration->VecData		[i] =	(double)50.0;
		IP->SelectionVector->VecData		[i] =	true;
	}

	ResultValue = TypeIRML::RML_WORKING;

	// starting loop
    printf("Start movement Loop");

	while (ResultValue != TypeIRML::RML_FINAL_STATE_REACHED)
	{
		ResultValue = RML->GetNextMotionState_Position(*IP, OP);

		// check RML-Status
		if((ResultValue != TypeIRML::RML_WORKING) && (ResultValue != TypeIRML::RML_FINAL_STATE_REACHED))
		{
			printf("MoveToHome(): ERROR during trajectory generation (%d).", ResultValue);
		}

		for(i=0;i<NUMBER_OF_JOINTS;i++)
		{
			JointValuesInRad[i] = RAD((double)(OP->NewPosition->VecData[i]));
		}

		fd->cmdVelJoint(JointValuesInRad);

		*IP->CurrentPosition = *OP->NewPosition;
		*IP->CurrentVelocity = *OP->NewVelocity;
	}

    printf("New position reached.\n");

	if(!fd->IsMachineOK())
	{
		printf("Moving: ERROR, machine is not ready.");

		delete	RML;
		delete	IP;
		delete	OP;

		return;
	}

	fd->Stop();

	delete RML;
	delete IP;
	delete OP;

	return;
}

void testGetterMethods(FRIBasicDriverCPP *fd)
{
	bool					MachineOK;
	float					JointStiffnessValues[LBR_MNJ]
						,	JointTorques[LBR_MNJ]
						,	JointValuesInRad[NUMBER_OF_JOINTS]
						,	MeasuredPose[NUMBER_OF_FRAME_ELEMENTS]
						,	EstimatedCartForcesAndTorques[NUMBER_OF_JOINTS]
						,	CycleTime = 0;

	// get Current Control Scheme:
    printf("\nCurrent Control Scheme: %d\n\n", fd->getCurrentControlScheme());
    usleep(500);

	// get Measured Joint Positions
	fd->getMeasuredJointPositions(JointValuesInRad);
    printf("\nMeasured Joint Positions (in Radiant): \n");
	printFloatArray(JointValuesInRad, NUMBER_OF_JOINTS);

    usleep(500);

	// getMeasured Joint Torques
	fd->GetMeasuredJointTorques(JointTorques);
    printf("\nMeasured Joint Torques (in Radiant): \n");
	printFloatArray(JointTorques, NUMBER_OF_JOINTS);
    printf("==========================================\n");

    usleep(500);

	// get Measured Cartesian Pose
	fd->GetMeasuredCartPose(MeasuredPose);
    printf("\nMeasured Cartesian Pose:\n");
	printFloatArray(MeasuredPose, NUMBER_OF_FRAME_ELEMENTS);
    printf("==========================================\n");

    usleep(500);

	// get Drive Tempereatures
	fd->GetDriveTemperatures(JointStiffnessValues);
    printf("\nMeasured Drive Temperatures:\n");
	printFloatArray(JointStiffnessValues, NUMBER_OF_JOINTS);
    printf("==========================================\n");

    usleep(500);

	// get Estimated External Cart Forces and Torques
	fd->GetEstimatedExternalCartForcesAndTorques(EstimatedCartForcesAndTorques);
    printf("\nEstimated Cartesian Forces And Torques (in N):\n");
	printFloatArray(EstimatedCartForcesAndTorques, NUMBER_OF_JOINTS);
    printf("==========================================\n");

    usleep(500);

	// Get Estimated External Joint Torques
	fd->GetEstimatedExternalJointTorques(JointTorques);
    printf("\nGet Estimated External Joint Torques: \n");
	printFloatArray(JointTorques, NUMBER_OF_JOINTS);
    printf("==========================================\n");

    usleep(500);

	// Get FRI Cycle Time
	CycleTime = fd->GetFRICycleTime();
    printf("\nRobot Cycle Time: %f\n", CycleTime);
    printf("==========================================\n");

    usleep(500);

	// is Machine OK
	MachineOK = fd->IsMachineOK();
    printf("\nCheck if Machine is OK: \n");
	if(MachineOK) {
        printf("Yes, Machine is OK.\n");
	} else {
        printf("NO, Machine is NOT OK!\n");
	}
    printf("==========================================\n");

	return;
}

void testGetterCompleteInformation(FRIBasicDriverCPP *fd)
{
    printf("Complete Robot and State information:\n%s\n\n", fd->GetCompleteRobotStateAndInformation());
}

void printFloatArray(float *ar, int size)
{
	for(int i=0; i<size; i++) {
        printf("%f    ", (float) ar[i]);
	}
    printf("\n");
}

void printDoubleArray(double *ar, int size)
{
	for(int i=0; i<size; i++) {
        printf("%f    ", (double)ar[i]);
	}
    printf("\n");
}


text:
// Copyright © 2012 Jacob Snyder, Billy O'Neal III
// This is under the 2 clause BSD license.
// See the included LICENSE.TXT file for more details.

#pragma once
#include <cstdint>
#include <string>
#include <vector>
#include <stack>
#include <boost/noncopyable.hpp>
#include <windows.h>
#include "Expected.hpp"

namespace Instalog { namespace SystemFacades {

    /// @brief Represents a plain Win32 file handle.
    class File : boost::noncopyable
    {
        HANDLE hFile;
    public:
        /// @brief    Default constructor.
        File();

        ///
        /// @param    filename              Filename of the file.
        /// @param    desiredAccess          (optional) the desired access.
        /// @param    shareMode              (optional) the share mode.
        /// @param    securityAttributes    (optional) the security attributes.
        /// @param    createdDisposition    (optional) the created disposition.
        /// @param    flags                  (optional) the flags.
        ///
        /// @throw    Win32Exception on failure
        File(
            std::wstring const& filename,
            DWORD desiredAccess = GENERIC_READ,
            DWORD shareMode = FILE_SHARE_READ | FILE_SHARE_WRITE,
            LPSECURITY_ATTRIBUTES securityAttributes = nullptr,
            DWORD createdDisposition = OPEN_EXISTING,
            DWORD flags = FILE_ATTRIBUTE_NORMAL
        );

        /// @brief    Move constructor
        ///
        /// @param [in,out]    other    The other.
        File(File && other);

        /// @brief    Assignment operator.
        ///
        /// @param    other    The other.
        ///
        /// @return    A shallow copy of this instance.
        File& operator=(File other);

        /// @brief    Destructor.
        ~File();

        /// @brief    Gets the size of the file in bytes
        ///
        /// @return    The size.
        ///
        /// @throw    Win32Exception on failure
        std::uint64_t GetSize() const;

        /// @brief    Gets the attributes of the file as a DWORD
        ///
        /// @return    The attributes.
        ///
        /// @throw    Win32Exception on error
        DWORD GetAttributes() const;

        /// @brief    Gets the extended attributes of the file
        ///
        /// @return    The extended attributes.
        ///
        /// @throw    Win32Exception on error
        BY_HANDLE_FILE_INFORMATION GetExtendedAttributes() const;

        /// @brief    Reads the the specified amount of bytes into a vector
        ///
        /// @param    bytesToRead    The number of bytes to read.
        ///
        /// @return    The bytes as a vector of chars.
        ///
        /// @throw    Win32Exception on error
        std::vector<char> ReadBytes(unsigned int bytesToRead) const;

        /// @brief    Writes the specified bytes to the file
        ///
        /// @param    bytes    The bytes.
        ///
        /// @return    true if all of the bytes were written
        ///
        /// @throw    Win32Exception on error
        bool WriteBytes(std::vector<char> const& bytes);

        /// @brief    Gets the size of the specified file in bytes without opening a file handle
        ///
        /// @return    The size.
        ///
        /// @throw    Win32Exception on error
        static std::uint64_t GetSize(std::wstring const& filename);

        /// @brief    Gets the attributes of the specified file as a DWORD without opening a file handle
        ///
        /// @return    The attributes.
        ///
        /// @throw    Win32Exception on error
        static DWORD GetAttributes(std::wstring const& filename);

        /// @brief    Gets the extended attributes of the specified file without opening a file handle
        ///
        /// @return    The extended attributes.
        ///
        /// @throw    Win32Exception on error
        static WIN32_FILE_ATTRIBUTE_DATA GetExtendedAttributes(std::wstring const& filename);

        /// @brief    Deletes the given file.
        ///
        /// @param    filename    Filename of the file.
        ///
        /// @throw    Win32Exception on error
        static void Delete(std::wstring const& filename);

        /// @brief    Determines if the given file exists
        ///
        /// @param    filename    Filename of the file.
        ///
        /// @return    true if the file exists
        static bool Exists(std::wstring const& filename);

        /// @brief    Query if a path is a directory.
        ///
        /// @param    filename    Path of the file.
        ///
        /// @return    true if the path is directory, false if not.
        static bool IsDirectory(std::wstring const& filename);

        /// @brief  Query if a path is a file. (That is, exists, and is not a directory)
        ///
        /// @param  filename    Path of the file to check.
        ///
        /// @remarks Equivalent to Exists() && !IsDirectory()
        ///
        /// @return true if the path exists and is a file, false otherwise.
        static bool IsExclusiveFile(std::wstring const& fileName);

        /// @brief    Query if filename is an executable.
        ///
        /// @param    filename    Filename of the file.
        ///
        /// @return    true if it is an executable, false if not.
        static bool IsExecutable(std::wstring const& filename);

        /// @brief    Gets the company of the file
        ///
        /// @param    target    Target file
        ///
        /// @return    The company.
        ///
        /// @throw    Win32Exception on error
        static std::wstring GetCompany(std::wstring const& target);
    };

	/// Find files record.
	class FindFilesRecord
	{
		std::wstring cFileName;
		std::uint64_t ftCreationTime;
		std::uint64_t ftLastAccessTime;
		std::uint64_t ftLastWriteTime;
		std::uint64_t nFileSize;
		DWORD dwFileAttributes;
	public:
		/// Initializes a new instance of the FindFilesRecord class.
		/// @param prefix    The prefix path.
		/// @param winSource The windows data record source.
		FindFilesRecord(std::wstring prefix, WIN32_FIND_DATAW const& winSource);

		/// Initializes a new instance of the File class.
		/// @param other The copied record.
		FindFilesRecord(FindFilesRecord const& other);

		/// Initializes a moved instance of the File class.
		/// @param other The moved record.
		FindFilesRecord(FindFilesRecord&& other) throw();

		/// Assignment operator.
		/// @param other The copied item.
		/// @return A shallow copy of this object.
		FindFilesRecord& operator=(FindFilesRecord other);

		/// Gets file name.
		/// @return The file name.
		std::wstring const& GetFileName() const throw();

		/// Gets creation time.
		/// @return The creation time.
		std::uint64_t GetCreationTime() const throw();

		/// Gets the last access time.
		/// @return The last access time.
		std::uint64_t GetLastAccessTime() const throw();

		/// Gets the last write time.
		/// @return The last write time.
		std::uint64_t GetLastWriteTime() const throw();

		/// Gets the size.
		/// @return The size.
		std::uint64_t GetSize() const throw();

		/// Gets the attributes.
		/// @return The attributes.
		DWORD GetAttributes() const throw();

		/// Swaps the given record.
		/// @param [in,out] other The other record with which to swap.
		void swap(FindFilesRecord &other) throw();
	};

    /**
     * Tests whether or not a FindFilesRecord is a directory with the name . or ...
     */
    inline bool IsDotDirectory(FindFilesRecord const& test) throw()
    {
        auto const& str = test.GetFileName();
        return (test.GetAttributes() & FILE_ATTRIBUTE_DIRECTORY) && (str == L"." || str == L"..");
    }

	/// Swaps a pair of FindFilesRecords.
	/// @param [in,out] lhs The left hand side.
	/// @param [in,out] rhs The right hand side.
	inline void swap(FindFilesRecord &lhs, FindFilesRecord &rhs) throw()
	{
		lhs.swap(rhs);
	}

    /// @brief    Finds files in directories.  Wrapper around FindFirstFile and FindNextFile
    class FindFiles : boost::noncopyable
    {
        std::stack<HANDLE, std::vector<HANDLE>> handles;
        const bool skipDotDirectories;
        const bool recursive;
        std::wstring pattern;
        std::stack<const std::wstring> subPaths;

        /// @summary    The data of the next file found.
        expected<FindFilesRecord> data;

        /**
         * Gets the next file spec for enumerating recurisively.
         */
        std::wstring GetNextSpec() const;
    public:
		/// Gets the data record for the current index.
		/// @return The data record for the current index.
		expected<FindFilesRecord> const& GetData() const throw()
		{
			return data;
		}

        /// @brief    Constructor
        ///
        /// @param    pattern            A pattern specifying the files of interest. Supports everything that
        ///                              FindFirstFile supports (check MSDN docs for this)
        /// @param    recursive          (optional) whether to recurse deeper into directories or not
        /// @param    skipDotDirectories (optional) if this is true, the implied directories . and .. will be skipped
        ///
        /// @detail This will swallow invalid path and invalid file exceptions and instead just set
        ///         IsValid to false.
        FindFiles(std::wstring const& patternSpec, bool recursive = false, bool skipDotDirectories = true);

        /// @brief    Destructor.
        ~FindFiles() throw();

        /// @brief    Populates data with the next file (if there is one)
        void Next() throw();

        /**
         * Checks whether or not this instance has more entries to enumerate.
         */
        bool IsValid() throw()
        {
            return !handles.empty();
        }
    };

}}

ext: hpp

max_stars_repo_name: BillyONeal/pevFind

max_issues_repo_name: BillyONeal/pevFind

max_forks_repo_name: BillyONeal/pevFind


text:
/*
 * KDataTempCsvDriver.cpp
 *
 *  Created on: 2017年7月30日
 *      Author: fasiondog
 */

#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include "KDataTempCsvDriver.h"

#include "../../../utilities/util.h"
#include "../../../Log.h"

namespace hku {

KDataTempCsvDriver::~KDataTempCsvDriver() {}

KDataTempCsvDriver::KDataTempCsvDriver(const string& day_filename, const string& min_filename)
: m_day_filename(day_filename), m_min_filename(min_filename) {
    for (int i = 0; i < LAST; ++i) {
        m_column[i] = Null<size_t>();
    }

    m_token_buf.reserve(LAST);
}

void KDataTempCsvDriver::_get_token(const string& line) {
    m_token_buf.clear();

    string token;
    size_t pos = 0, prepos = 0;
    pos = line.find(',', prepos);
    while (pos != line.npos) {
        token.assign(line, prepos, pos - prepos);
        boost::trim(token);
        m_token_buf.push_back(token);
        prepos = pos + 1;
        pos = line.find(',', prepos);
    }

    if (prepos != pos) {
        token.assign(line, prepos, pos);
        boost::trim(token);
        m_token_buf.push_back(token);
    }
}

void KDataTempCsvDriver::_get_title_column(const string& line) {
    _get_token(line);

    int total = (int)m_token_buf.size();
    for (int i = 0; i < total; ++i) {
        string token = m_token_buf[i];
        to_upper(token);

        if ("DATE" == token || "DATETIME" == token || "日期" == token) {
            m_column[DATE] = i;

        } else if ("OPEN" == token || "开盘价" == token) {
            m_column[OPEN] = i;

        } else if ("HIGH" == token || "最高价" == token) {
            m_column[HIGH] = i;

        } else if ("LOW" == token || "最低价" == token) {
            m_column[LOW] = i;

        } else if ("CLOSE" == token || "收盘价" == token) {
            m_column[CLOSE] = i;

        } else if ("AMOUNT" == token || "成交金额" == token) {
            m_column[AMOUNT] = i;

        } else if ("VOLUME" == token || "COUNT" == token || "VOL" == token || "成交量" == token) {
            m_column[VOLUME] = i;
        }
    }
}

size_t KDataTempCsvDriver::getCount(const string& market, const string& code, KQuery::KType kType) {
    return getKRecordList(market, code, KQuery(0, Null<int64_t>(), kType)).size();
}

bool KDataTempCsvDriver::getIndexRangeByDate(const string& market, const string& code,
                                             const KQuery& query, size_t& out_start,
                                             size_t& out_end) {
    out_start = 0;
    out_end = 0;

    Datetime start_date = query.startDatetime();
    Datetime end_date = query.endDatetime();
    HKU_IF_RETURN(start_date >= end_date, false);

    auto klist = getKRecordList(market, code, KQuery(0, Null<int64_t>(), query.kType()));
    size_t total = klist.size();
    HKU_IF_RETURN(total == 0, false);

    for (size_t i = total - 1; i == 0; --i) {
        if (klist[i].datetime < end_date) {
            out_end = i + 1;
            break;
        }
    }

    if (out_end == 0) {
        return false;
    }

    for (size_t i = out_end - 1; i == 0; --i) {
        if (klist[i].datetime <= start_date) {
            out_start = i;
            break;
        }
    }

    if (out_start >= out_end) {
        out_start = 0;
        out_end = 0;
        return false;
    }

    return true;
}

KRecordList KDataTempCsvDriver::getKRecordList(const string& market, const string& code,
                                               const KQuery& query) {
    KRecordList result;
    if (query.queryType() == KQuery::INDEX) {
        result = _getKRecordListByIndex(market, code, query.start(), query.end(), query.kType());
    } else {
        HKU_INFO("Query by date are not supported!");
    }
    return result;
}

KRecordList KDataTempCsvDriver::_getKRecordListByIndex(const string& market, const string& code,
                                                       int64_t start_ix, int64_t end_ix,
                                                       KQuery::KType kType) {
    KRecordList result;

    string filename;
    if (kType == KQuery::DAY) {
        filename = m_day_filename;
    } else if (kType == KQuery::MIN) {
        filename = m_min_filename;
    } else {
        HKU_INFO("Only support DAY and MIN!");
        return result;
    }

    std::ifstream infile(filename.c_str());
    HKU_ERROR_IF_RETURN(!infile, result, "Can't open this file: {}", filename);
    string line;
    if (!std::getline(infile, line)) {
        infile.close();
        return result;
    }

    _get_title_column(line);

    int64_t line_no = 0;
    while (std::getline(infile, line)) {
        if (line_no++ < start_ix)
            continue;

        if (line_no >= end_ix)
            break;

        _get_token(line);
        size_t token_count = m_token_buf.size();

        KRecord record;
        string action;
        try {
            action = "DATE";
            if (token_count >= m_column[DATE])
                record.datetime = Datetime(m_token_buf[m_column[DATE]]);

            action = "OPEN";
            if (token_count >= m_column[OPEN])
                record.openPrice = boost::lexical_cast<price_t>(m_token_buf[m_column[OPEN]]);

            action = "HIGH";
            if (token_count >= m_column[HIGH])
                record.highPrice = boost::lexical_cast<price_t>(m_token_buf[m_column[HIGH]]);

            action = "LOW";
            if (token_count >= m_column[LOW])
                record.lowPrice = boost::lexical_cast<price_t>(m_token_buf[m_column[LOW]]);

            action = "CLOSE";
            if (token_count >= m_column[CLOSE])
                record.closePrice = boost::lexical_cast<price_t>(m_token_buf[m_column[CLOSE]]);

            action = "VOLUME";
            if (token_count >= m_column[VOLUME])
                record.transCount = boost::lexical_cast<price_t>(m_token_buf[m_column[VOLUME]]);

            action = "AMOUNT";
            if (token_count >= m_column[AMOUNT])
                record.transAmount = boost::lexical_cast<price_t>(m_token_buf[m_column[AMOUNT]]);

            result.push_back(record);

        } catch (...) {
            HKU_WARN("Invalid data in line {}! at trans {}", line_no, action);
        }

        line_no++;
    }

    infile.close();

    return result;
}

} /* namespace hku */

ext: cpp

max_stars_repo_name: scanfyu/hikyuu

max_issues_repo_name: scanfyu/hikyuu

max_forks_repo_name: scanfyu/hikyuu


text:
//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

// clang-format off

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <ImathVec.h>
#include "PyImath.h"
#include "PyImathMathExc.h"
#include "PyImathEuler.h"
#include "PyImathDecorators.h"
#include "PyImathExport.h"
#include "PyImathOperators.h"

// XXX incomplete array wrapping, docstrings missing

namespace PyImath {
template<> const char *PyImath::EulerfArray::name() { return "EulerfArray"; }
template<> const char *PyImath::EulerdArray::name() { return "EulerdArray"; }
}

namespace PyImath {
using namespace boost::python;
using namespace IMATH_NAMESPACE;

template <class T> struct EulerName { static const char *value; };
template<> const char *EulerName<float>::value  = "Eulerf";
template<> const char *EulerName<double>::value = "Eulerd";

template <class T>
static std::string nameOfOrder(typename IMATH_NAMESPACE::Euler<T>::Order order)
{
    switch(order)
    {
        case IMATH_NAMESPACE::Euler<T>::XYZ:
            return "EULER_XYZ";
        case IMATH_NAMESPACE::Euler<T>::XZY:
            return "EULER_XZY";
        case IMATH_NAMESPACE::Euler<T>::YZX:
            return "EULER_YZX";
        case IMATH_NAMESPACE::Euler<T>::YXZ:
            return "EULER_YXZ";
        case IMATH_NAMESPACE::Euler<T>::ZXY:
            return "EULER_ZXY";
        case IMATH_NAMESPACE::Euler<T>::ZYX:
            return "EULER_ZYX";
        case IMATH_NAMESPACE::Euler<T>::XZX:
            return "EULER_XZX";
        case IMATH_NAMESPACE::Euler<T>::XYX:
            return "EULER_XYX";
        case IMATH_NAMESPACE::Euler<T>::YXY:
            return "EULER_YXY";
        case IMATH_NAMESPACE::Euler<T>::YZY:
            return "EULER_YZY";
        case IMATH_NAMESPACE::Euler<T>::ZYZ:
            return "EULER_ZYZ";
        case IMATH_NAMESPACE::Euler<T>::ZXZ:
            return "EULER_ZXZ";
        case IMATH_NAMESPACE::Euler<T>::XYZr:
            return "EULER_XYZr";
        case IMATH_NAMESPACE::Euler<T>::XZYr:
            return "EULER_XZYr";
        case IMATH_NAMESPACE::Euler<T>::YZXr:
            return "EULER_YZXr";
        case IMATH_NAMESPACE::Euler<T>::YXZr:
            return "EULER_YXZr";
        case IMATH_NAMESPACE::Euler<T>::ZXYr:
            return "EULER_ZXYr";
        case IMATH_NAMESPACE::Euler<T>::ZYXr:
            return "EULER_ZYXr";
        case IMATH_NAMESPACE::Euler<T>::XZXr:
            return "EULER_XZXr";
        case IMATH_NAMESPACE::Euler<T>::XYXr:
            return "EULER_XYXr";
        case IMATH_NAMESPACE::Euler<T>::YXYr:
            return "EULER_YXYr";
        case IMATH_NAMESPACE::Euler<T>::YZYr:
            return "EULER_YZYr";
        case IMATH_NAMESPACE::Euler<T>::ZYZr:
            return "EULER_ZYZr";
        case IMATH_NAMESPACE::Euler<T>::ZXZr:
            return "EULER_ZXZr";
        default:
            break;
    }

    return "";
}

template <class T>
static std::string Euler_str(const Euler<T> &e)
{
    std::stringstream stream;
    stream << EulerName<T>::value << "(" << e.x << ", " << e.y << ", " << e.z << ", "
           << nameOfOrder<T> (e.order()) << ")";
    return stream.str();
}

// Non-specialized repr is same as str
template <class T>
static std::string Euler_repr(const Euler<T> &e)
{
    return Euler_str(e);
}

// Specialization for float to full precision
template <>
std::string Euler_repr(const Euler<float> &e)
{
    return (boost::format("%s(%.9g, %.9g, %.9g, %s)")
                        % EulerName<float>::value
                        % e.x % e.y % e.z
                        % nameOfOrder<float>(e.order()).c_str()).str();
}

// Specialization for double to full precision
template <>
std::string Euler_repr(const Euler<double> &e)
{
    return (boost::format("%s(%.17g, %.17g, %.17g, %s)")
                        % EulerName<double>::value
                        % e.x % e.y % e.z
                        % nameOfOrder<double>(e.order()).c_str()).str();
}

template <class T>
static bool
equal(const Euler<T> &e0, const Euler<T> &e1)
{
    if(e0.x == e1.x && e0.y == e1.y && e0.z == e1.z && (e0.order())==(e1.order()))
        return true;
    else
        return false;
}

template <class T>
static bool
notequal(const Euler<T> &e0, const Euler<T> &e1)
{
    if(e0.x != e1.x || e0.y != e1.y || e0.z != e1.z || (e0.order()) != (e1.order()))
    {
        return true;
    }
    else
        return false;
}

template <class T>
static IMATH_NAMESPACE::Vec3 <int> getAngleOrder(Euler <T> &euler)
{
    int i, j, k;
    euler.angleOrder(i, j, k);
    return IMATH_NAMESPACE::Vec3 <int> (i, j, k);
}

template <class T>
static void
setXYZTuple(Euler<T> &euler, const tuple &t)
{
    MATH_EXC_ON;
    Vec3<T> v;
    if(t.attr("__len__")() == 3)
    {
        v.x = extract<T>(t[0]);
        v.y = extract<T>(t[1]);
        v.z = extract<T>(t[2]);

        euler.setXYZVector(v);
    }
    else
        throw std::invalid_argument ("Color3 expects tuple of length 3");
}

// needed to convert Eulerf::Order to Euler<T>::Order
template <class T>
static typename Euler<T>::Order interpretOrder(typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = Euler<T>::XYZ;
    switch(order)
    {
        case IMATH_NAMESPACE::Eulerf::XYZ:
        {
            o = Euler<T>::XYZ;
        }break;
        case IMATH_NAMESPACE::Eulerf::XZY:
        {
            o = Euler<T>::XZY;
        }break;
        case IMATH_NAMESPACE::Eulerf::YZX:
        {
            o = Euler<T>::YZX;
        }break;
        case IMATH_NAMESPACE::Eulerf::YXZ:
        {
            o = Euler<T>::YXZ;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZXY:
        {
            o = Euler<T>::ZXY;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZYX:
        {
            o = Euler<T>::ZYX;
        }break;
        case IMATH_NAMESPACE::Eulerf::XZX:
        {
            o = Euler<T>::XZX;
        }break;
        case IMATH_NAMESPACE::Eulerf::XYX:
        {
            o = Euler<T>::XYX;
        }break;
        case IMATH_NAMESPACE::Eulerf::YXY:
        {
            o = Euler<T>::YXY;
        }break;
        case IMATH_NAMESPACE::Eulerf::YZY:
        {
            o = Euler<T>::YZY;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZYZ:
        {
            o = Euler<T>::ZYZ;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZXZ:
        {
            o = Euler<T>::ZXZ;
        }break;
        case IMATH_NAMESPACE::Eulerf::XYZr:
        {
            o = Euler<T>::XYZr;
        }break;
        case IMATH_NAMESPACE::Eulerf::XZYr:
        {
            o = Euler<T>::XZYr;
        }break;
        case IMATH_NAMESPACE::Eulerf::YZXr:
        {
            o = Euler<T>::YZXr;
        }break;
        case IMATH_NAMESPACE::Eulerf::YXZr:
        {
            o = Euler<T>::YXZr;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZXYr:
        {
            o = Euler<T>::ZXYr;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZYXr:
        {
            o = Euler<T>::ZYXr;
        }break;
        case IMATH_NAMESPACE::Eulerf::XZXr:
        {
            o = Euler<T>::XZXr;
        }break;
        case IMATH_NAMESPACE::Eulerf::XYXr:
        {
            o = Euler<T>::XYXr;
        }break;
        case IMATH_NAMESPACE::Eulerf::YXYr:
        {
            o = Euler<T>::YXYr;
        }break;
        case IMATH_NAMESPACE::Eulerf::YZYr:
        {
            o = Euler<T>::YZYr;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZYZr:
        {
            o = Euler<T>::ZYZr;
        }break;
        case IMATH_NAMESPACE::Eulerf::ZXZr:
        {
            o = Euler<T>::ZXZr;
        }break;
        default:
            break;
    }

    return o;
}

// needed to convert Eulerf::Axis to Euler<T>::Axis
template <class T>
static typename Euler<T>::Axis interpretAxis(typename IMATH_NAMESPACE::Eulerf::Axis axis)
{
    if (axis == IMATH_NAMESPACE::Eulerf::X)
        return Euler<T>::X;
    else if (axis == IMATH_NAMESPACE::Eulerf::Y)
        return Euler<T>::Y;
    else
        return Euler<T>::Z;
}

template <class T>
static Euler<T> *
eulerConstructor1(const Vec3<T> &v, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    return new Euler<T>(v, o);
}

template <class T>
static Euler<T> *
eulerConstructor1a(const Vec3<T> &v)
{
    return eulerConstructor1 (v, IMATH_NAMESPACE::Eulerf::Default);
}

template <class T>
static Euler<T> *
eulerConstructor1b(const Vec3<T> &v, int iorder)
{
    typename Euler<T>::Order o = typename Euler<T>::Order (iorder);
    return new Euler<T>(v, o);
}

template <class T>
static Euler<T> *
eulerConstructor2(T i, T j, T k, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    return new Euler<T>(i, j, k, o);
}

template <class T>
static Euler<T> *
eulerConstructor2a(T i, T j, T k)
{
    return eulerConstructor2 (i, j, k, IMATH_NAMESPACE::Eulerf::Default);
}

template <class T>
static Euler<T> *
eulerConstructor2b(T i, T j, T k, int iorder)
{
    typename Euler<T>::Order o = typename Euler<T>::Order (iorder);
    return new Euler<T>(i, j, k, o);
}

template <class T>
static Euler<T> *
eulerConstructor3(const Matrix33<T> &mat, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    return new Euler<T>(mat, o);
}

template <class T>
static Euler<T> *
eulerConstructor3a(const Matrix33<T> &mat)
{
    return eulerConstructor3 (mat, IMATH_NAMESPACE::Eulerf::Default);
}

template <class T>
static Euler<T> *
eulerConstructor3b(const Matrix33<T> &mat, int iorder)
{
    typename Euler<T>::Order o = typename Euler<T>::Order (iorder);
    return new Euler<T>(mat, o);
}

template <class T>
static Euler<T> *
eulerConstructor4(const Matrix44<T> &mat, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    return new Euler<T>(mat, o);
}

template <class T>
static Euler<T> *
eulerConstructor4a(const Matrix44<T> &mat)
{
    return eulerConstructor4 (mat, IMATH_NAMESPACE::Eulerf::Default);
}

template <class T>
static Euler<T> *
eulerConstructor4b(const Matrix44<T> &mat, int iorder)
{
    typename Euler<T>::Order o = typename Euler<T>::Order (iorder);
    return new Euler<T>(mat, o);
}

template <class T>
static Euler<T> *
eulerConstructor5(typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    return new Euler<T>(o);
}

template <class T>
static Euler<T> *
eulerConstructor5a()
{
    typename Euler<T>::Order o = interpretOrder<T>(IMATH_NAMESPACE::Eulerf::Default);
    return new Euler<T>(o);
}

template <class T>
static Euler<T> *
eulerConstructor5b(int iorder)
{
    typename Euler<T>::Order o = typename Euler<T>::Order (iorder);
    return new Euler<T>(o);
}

template <class T>
static Euler<T> *
eulerConstructor6(T x, T y, T z)
{
    return new Euler<T>(Vec3<T>(x,y,z));
}

template <class T>
static Euler<T> *
eulerConstructor7(const Quat<T> &quat, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    Euler<T> *e = eulerConstructor5<T>(order);
    e->extract(quat);
    return e;
}

template <class T>
static Euler<T> *
eulerConstructor7a(const Quat<T> &quat)
{
    return eulerConstructor7(quat, IMATH_NAMESPACE::Eulerf::Default);
}

template <class T>
static Euler<T> *
eulerConstructor7b(const Quat<T> &quat, int iorder)
{
    Euler<T> *e = eulerConstructor5b<T>(iorder);
    e->extract(quat);
    return e;
}

template <class T, class S>
static Euler<T> *
eulerConversionConstructor(const Euler<S> &euler)
{
    MATH_EXC_ON;
    Euler<T> *e = new Euler<T>;
    *e = euler;
    return e;
}

template <class T>
static void
eulerMakeNear(Euler<T> &euler, Euler<T> &target)
{
    MATH_EXC_ON;
    euler.makeNear (target);
}

template <class T>
static void
eulerSetOrder(Euler<T> &euler, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    typename Euler<T>::Order o = interpretOrder<T>(order);
    euler.setOrder (o);
}

template <class T>
static void
eulerSet(Euler<T> &euler, IMATH_NAMESPACE::Eulerf::Axis axis, int relative, int parityEven, int firstRepeats)
{
    MATH_EXC_ON;
    typename Euler<T>::Axis a = interpretAxis<T>(axis);
    euler.set (a, relative, parityEven, firstRepeats);
}

template <class T>
static void
extract1(Euler<T> &euler, const Matrix33<T> &m)
{
    MATH_EXC_ON;
    euler.extract(m);
}

template <class T>
static void
extract2(Euler<T> &euler, const Matrix44<T> &m)
{
    MATH_EXC_ON;
    euler.extract(m);
}

template <class T>
static void
extract3(Euler<T> &euler, const Quat<T> &q)
{
    MATH_EXC_ON;
    euler.extract(q);
}

template <class T>
static Matrix33<T>
toMatrix33(Euler<T> &euler)
{
    MATH_EXC_ON;
    return euler.toMatrix33();
}

template <class T>
static Matrix44<T>
toMatrix44(Euler<T> &euler)
{
    MATH_EXC_ON;
    return euler.toMatrix44();
}

template <class T>
static Quat<T>
toQuat(Euler<T> &euler)
{
    MATH_EXC_ON;
    return euler.toQuat();
}

template <class T>
static Vec3<T>
toXYZVector(Euler<T> &euler)
{
    MATH_EXC_ON;
    return euler.toXYZVector();
}

template <class T>
class_<Euler<T>,bases<IMATH_NAMESPACE::Vec3<T> > >
register_Euler()
{
    class_<Euler<T>,bases<Vec3<T> > > euler_class(EulerName<T>::value,EulerName<T>::value,init<Euler<T> >("copy construction"));
    euler_class
        .def(init<>("imath Euler default construction"))
        .def("__init__", make_constructor(eulerConstructor1<T>))
        .def("__init__", make_constructor(eulerConstructor1a<T>))
        .def("__init__", make_constructor(eulerConstructor1b<T>))
        .def("__init__", make_constructor(eulerConstructor2<T>))
        .def("__init__", make_constructor(eulerConstructor2a<T>))
        .def("__init__", make_constructor(eulerConstructor2b<T>))
        .def("__init__", make_constructor(eulerConstructor3<T>),
             "Euler-from-matrix construction assumes, but does\n"
             "not verify, that the matrix includes no shear or\n"
             "non-uniform scaling.  If necessary, you can fix\n"
             "the matrix by calling the removeScalingAndShear()\n"
             "function.\n")
        .def("__init__", make_constructor(eulerConstructor3a<T>))
        .def("__init__", make_constructor(eulerConstructor3b<T>))
        .def("__init__", make_constructor(eulerConstructor4<T>))
        .def("__init__", make_constructor(eulerConstructor4a<T>))
        .def("__init__", make_constructor(eulerConstructor4b<T>))
        .def("__init__", make_constructor(eulerConstructor5<T>))
        .def("__init__", make_constructor(eulerConstructor5a<T>))
        .def("__init__", make_constructor(eulerConstructor5b<T>))
        .def("__init__", make_constructor(eulerConstructor6<T>))
        .def("__init__", make_constructor(eulerConstructor7<T>))
        .def("__init__", make_constructor(eulerConstructor7a<T>))
        .def("__init__", make_constructor(eulerConstructor7b<T>))
        .def("__init__", make_constructor(eulerConversionConstructor<T, float>))
        .def("__init__", make_constructor(eulerConversionConstructor<T, double>))

        .def("angleOrder", &getAngleOrder<T>, "angleOrder() set the angle order")

        .def("frameStatic", &Euler<T>::frameStatic,
             "e.frameStatic() -- returns true if the angles of e\n"
             "are measured relative to a set of fixed axes,\n"
             "or false if the angles of e are measured relative to\n"
             "each other\n")

        .def("initialAxis", &Euler<T>::initialAxis,
             "e.initialAxis() -- returns the initial rotation\n"
             "axis of e (EULER_X_AXIS, EULER_Y_AXIS, EULER_Z_AXIS)")

        .def("initialRepeated", &Euler<T>::initialRepeated,
             "e.initialRepeated() -- returns 1 if the initial\n"
             "rotation axis of e is repeated (for example,\n"
             "e.order() == EULER_XYX); returns 0 if the initial\n"
             "rotation axis is not repeated.\n")

        .def("makeNear", &eulerMakeNear<T>,
             "e.makeNear(t) -- adjusts Euler e so that it\n"
             "represents the same rotation as before, but the\n"
             "individual angles of e differ from the angles of\n"
             "t by as little as possible.\n"
             "This method might not make sense if e.order()\n"
             "and t.order() are different\n")

        .def("order", &Euler<T>::order,
             "e.order() -- returns the rotation order in e\n"
             "(EULER_XYZ, EULER_XZY, ...)")

        .def("parityEven", &Euler<T>::parityEven,
             "e.parityEven() -- returns the parity of the\n"
             "axis permutation of e\n")

        .def("set", &eulerSet<T>,
             "e.set(i,r,p,f) -- sets the rotation order in e\n"
             "according to the following flags:\n"
             "\n"
             "   i   initial axis (EULER_X_AXIS,\n"
             "       EULER_Y_AXIS or EULER_Z_AXIS)\n"
             "\n"
             "   r   rotation angles are measured relative\n"
             "       to each other (r == 1), or relative to a\n"
             "       set of fixed axes (r == 0)\n"
             "\n"
             "   p   parity of axis permutation is even (r == 1)\n"
             "       or odd (r == 0)\n"
             "\n"
             "   f   first rotation axis is repeated (f == 1)\n"
             "	or not repeated (f == 0)\n")

        .def("setOrder", &eulerSetOrder<T>,
             "e.setOrder(o) -- sets the rotation order in e\n"
             "to o (EULER_XYZ, EULER_XZY, ...)")

        .def("setXYZVector", &Euler<T>::setXYZVector,
             "e.setXYZVector(v) -- sets the three rotation\n"
             "angles in e to v[0], v[1], v[2]")
        .def("setXYZVector", &setXYZTuple<T>)

        .def("extract", &extract1<T>,
             "e.extract(m) -- extracts the rotation component\n"
             "from 3x3 matrix m and stores the result in e.\n"
             "Assumes that m does not contain shear or non-\n"
             "uniform scaling.  If necessary, you can fix m\n"
             "by calling m.removeScalingAndShear().")

        .def("extract", &extract2<T>,
             "e.extract(m) -- extracts the rotation component\n"
             "from 4x4 matrix m and stores the result in e.\n"
             "Assumes that m does not contain shear or non-\n"
             "uniform scaling.  If necessary, you can fix m\n"
             "by calling m.removeScalingAndShear().")

        .def("extract", &extract3<T>,
             "e.extract(q) -- extracts the rotation component\n"
             "from quaternion q and stores the result in e")

        .def("toMatrix33", &toMatrix33<T>, "e.toMatrix33() -- converts e into a 3x3 matrix\n")

        .def("toMatrix44", &toMatrix44<T>, "e.toMatrix44() -- converts e into a 4x4 matrix\n")

        .def("toQuat", &toQuat<T>, "e.toQuat() -- converts e into a quaternion\n")

        .def("toXYZVector", &toXYZVector<T>,
             "e.toXYZVector() -- converts e into an XYZ\n"
             "rotation vector")
        .def("__str__", &Euler_str<T>)
        .def("__repr__", &Euler_repr<T>)

        .def("__eq__", &equal<T>)
        .def("__ne__", &notequal<T>)
        ;

    // fill in the Euler scope
    {
        scope euler_scope(euler_class);
        enum_<typename Euler<T>::Order> euler_order("Order");
        euler_order
            .value("XYZ",Euler<T>::XYZ)
            .value("XZY",Euler<T>::XZY)
            .value("YZX",Euler<T>::YZX)
            .value("YXZ",Euler<T>::YXZ)
            .value("ZXY",Euler<T>::ZXY)
            .value("ZYX",Euler<T>::ZYX)
            .value("XZX",Euler<T>::XZX)
            .value("XYX",Euler<T>::XYX)
            .value("YXY",Euler<T>::YXY)
            .value("YZY",Euler<T>::YZY)
            .value("ZYZ",Euler<T>::ZYZ)
            .value("ZXZ",Euler<T>::ZXZ)
            .value("XYZr",Euler<T>::XYZr)
            .value("XZYr",Euler<T>::XZYr)
            .value("YZXr",Euler<T>::YZXr)
            .value("YXZr",Euler<T>::YXZr)
            .value("ZXYr",Euler<T>::ZXYr)
            .value("ZYXr",Euler<T>::ZYXr)
            .value("XZXr",Euler<T>::XZXr)
            .value("XYXr",Euler<T>::XYXr)
            .value("YXYr",Euler<T>::YXYr)
            .value("YZYr",Euler<T>::YZYr)
            .value("ZYZr",Euler<T>::ZYZr)
            .value("ZXZr",Euler<T>::ZXZr)

            // don't export these, they're not really part of the public interface
            //.value("Legal",Euler<T>::Legal)
            //.value("Min",Euler<T>::Min)
            //.value("Max",Euler<T>::Max)

            // handle Default seperately since boost sets up a 1-1 mapping for enum values
            //.value("Default",Euler<T>::Default)
            .export_values()
            ;
        // just set it to the XYZ value manually
        euler_scope.attr("Default") = euler_scope.attr("XYZ");

        enum_<typename Euler<T>::Axis>("Axis")
            .value("X",Euler<T>::X)
            .value("Y",Euler<T>::Y)
            .value("Z",Euler<T>::Z)
            .export_values()
            ;

        enum_<typename Euler<T>::InputLayout>("InputLayout")
            .value("XYZLayout",Euler<T>::XYZLayout)
            .value("IJKLayout",Euler<T>::IJKLayout)
            .export_values()
            ;
    }

    decoratecopy(euler_class);

    return euler_class;
}

// XXX fixme - template this
// really this should get generated automatically...

/*
template <class T,int index>
static FixedArray<T>
EulerArray_get(FixedArray<IMATH_NAMESPACE::Euler<T> > &qa)
{
    return FixedArray<T>( &(qa[0].r)+index, qa.len(), 4*qa.stride());
}
*/

template <class T>
static FixedArray<IMATH_NAMESPACE::Euler<T> > *
EulerArray_eulerConstructor7a(const FixedArray<IMATH_NAMESPACE::Quat<T> > &q)
{
    MATH_EXC_ON;
    size_t len = q.len();
    FixedArray<IMATH_NAMESPACE::Euler<T> >* result = new FixedArray<IMATH_NAMESPACE::Euler<T> >(len);
    for (size_t i = 0; i < len; ++i) {
        (*result)[i].extract(q[i]);
    }
    return result;
}

template <class T>
static FixedArray<IMATH_NAMESPACE::Euler<T> > *
EulerArray_eulerConstructor8a(const FixedArray<IMATH_NAMESPACE::Vec3<T> >& v)
{
    MATH_EXC_ON;
    size_t len = v.len();
    FixedArray<IMATH_NAMESPACE::Euler<T> >* result = new FixedArray<IMATH_NAMESPACE::Euler<T> >(len);

    for (size_t i = 0; i < len; ++i)
        (*result)[i] = Euler<T>(v[i]);

    return result;
}

template <class T>
static FixedArray<IMATH_NAMESPACE::Euler<T> > *
EulerArray_eulerConstructor9a(const FixedArray<IMATH_NAMESPACE::Vec3<T> >& v, typename IMATH_NAMESPACE::Eulerf::Order order)
{
    MATH_EXC_ON;
    size_t len = v.len();
    FixedArray<IMATH_NAMESPACE::Euler<T> >* result = new FixedArray<IMATH_NAMESPACE::Euler<T> >(len);

    typename Euler<T>::Order o = interpretOrder<T>(order);
    for (size_t i = 0; i < len; ++i)
        (*result)[i] = Euler<T>(v[i], o);

    return result;
}

template <class T>
static FixedArray<IMATH_NAMESPACE::Vec3<T> >
EulerArray_toXYZVector(const FixedArray<IMATH_NAMESPACE::Euler<T> >& e)
{
    MATH_EXC_ON;
    size_t len = e.len();
    FixedArray<IMATH_NAMESPACE::Vec3<T> > result(len, UNINITIALIZED);
    for (size_t i = 0; i < len; ++i)
        result[i] = e[i].toXYZVector();
    return result;
}

template <class T>
static FixedArray<IMATH_NAMESPACE::Quat<T> >
EulerArray_toQuat(const FixedArray<IMATH_NAMESPACE::Euler<T> >& e)
{
    MATH_EXC_ON;
    size_t len = e.len();
    FixedArray<IMATH_NAMESPACE::Quat<T> > result(len, UNINITIALIZED);
    for (size_t i = 0; i < len; ++i)
        result[i] = e[i].toQuat();
    return result;
}

template <class T>
class_<FixedArray<IMATH_NAMESPACE::Euler<T> > >
register_EulerArray()
{
    class_<FixedArray<IMATH_NAMESPACE::Euler<T> > > eulerArray_class = FixedArray<IMATH_NAMESPACE::Euler<T> >::register_("Fixed length array of IMATH_NAMESPACE::Euler");
    eulerArray_class
        //.add_property("x",&EulerArray_get<T,1>)
        //.add_property("y",&EulerArray_get<T,2>)
        //.add_property("z",&EulerArray_get<T,3>)
        .def("__init__", make_constructor(EulerArray_eulerConstructor7a<T>))
        .def("__init__", make_constructor(EulerArray_eulerConstructor8a<T>))
        .def("__init__", make_constructor(EulerArray_eulerConstructor9a<T>))
        .def("toXYZVector", EulerArray_toXYZVector<T>)
        .def("toQuat", EulerArray_toQuat<T>)
        ;

    add_comparison_functions(eulerArray_class);
    PyImath::add_explicit_construction_from_type<IMATH_NAMESPACE::Matrix33<T> >(eulerArray_class);
    PyImath::add_explicit_construction_from_type<IMATH_NAMESPACE::Matrix44<T> >(eulerArray_class);
    return eulerArray_class;
}

template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Euler<float>,bases<IMATH_NAMESPACE::Vec3<float> > > register_Euler<float>();
template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Euler<double>,bases<IMATH_NAMESPACE::Vec3<double> > > register_Euler<double>();

template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Euler<float> > > register_EulerArray<float>();
template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Euler<double> > > register_EulerArray<double>();

template<> PYIMATH_EXPORT IMATH_NAMESPACE::Euler<float> FixedArrayDefaultValue<IMATH_NAMESPACE::Euler<float> >::value() { return IMATH_NAMESPACE::Euler<float>(); }
template<> PYIMATH_EXPORT IMATH_NAMESPACE::Euler<double> FixedArrayDefaultValue<IMATH_NAMESPACE::Euler<double> >::value() { return IMATH_NAMESPACE::Euler<double>(); }

} // namespace PyImath

max_stars_repo_name: JenusL/Imath

max_issues_repo_name: JenusL/Imath

max_forks_repo_name: JenusL/Imath


text:
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2015, Tel Aviv University
*  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 the Tel Aviv University 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 THE
*  COPYRIGHT OWNER OR CONTRIBUTORS 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.
*********************************************************************/

/* Author: Oren Salzman, Aditya Mandalika, Sertac Karaman, Ioan Sucan, Mark Moll */

#include "ompl/geometric/planners/rrt/LBTRRT.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include <limits>
#include <cmath>
#include <boost/math/constants/constants.hpp>

ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si)
  : base::Planner(si, "LBTRRT")
{
    specs_.approximateSolutions = true;
    specs_.directed = true;

    Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
    Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor,
                                  "0.:.1:10.");

    addPlannerProgressProperty("iterations INTEGER", [this]
                               {
                                   return getIterationCount();
                               });
    addPlannerProgressProperty("best cost REAL", [this]
                               {
                                   return getBestCost();
                               });
}

ompl::geometric::LBTRRT::~LBTRRT()
{
    freeMemory();
}

void ompl::geometric::LBTRRT::clear()
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (nn_)
        nn_->clear();
    lowerBoundGraph_.clear();
    lastGoalMotion_ = nullptr;

    iterations_ = 0;
    bestCost_ = std::numeric_limits<double>::infinity();
}

void ompl::geometric::LBTRRT::setup()
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (!nn_)
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
    nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
                             {
                                 return distanceFunction(a, b);
                             });
}

void ompl::geometric::LBTRRT::freeMemory()
{
    if (!idToMotionMap_.empty())
    {
        for (auto &i : idToMotionMap_)
        {
            if (i->state_ != nullptr)
                si_->freeState(i->state_);
            delete i;
        }
    }
    idToMotionMap_.clear();
}

ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    // update goal and check validity
    base::Goal *goal = pdef_->getGoal().get();
    auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);

    if (goal == nullptr)
    {
        OMPL_ERROR("%s: Goal undefined", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    // update start and check validity
    while (const base::State *st = pis_.nextStart())
    {
        auto *motion = new Motion(si_);
        si_->copyState(motion->state_, st);
        motion->id_ = nn_->size();
        idToMotionMap_.push_back(motion);
        nn_->add(motion);
        lowerBoundGraph_.addVertex(motion->id_);
    }

    if (nn_->size() == 0)
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (nn_->size() > 1)
    {
        OMPL_ERROR("%s: There are multiple start states - currently not supported!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    Motion *solution = lastGoalMotion_;
    Motion *approxSol = nullptr;
    double approxdif = std::numeric_limits<double>::infinity();
    // e*(1+1/d)  K-nearest constant, as used in RRT*
    double k_rrg =
        boost::math::constants::e<double>() + boost::math::constants::e<double>() / (double)si_->getStateDimension();

    auto *rmotion = new Motion(si_);
    base::State *rstate = rmotion->state_;
    base::State *xstate = si_->allocState();
    unsigned int statesGenerated = 0;

    bestCost_ = lastGoalMotion_ != nullptr ? lastGoalMotion_->costApx_ : std::numeric_limits<double>::infinity();
    while (!ptc())
    {
        iterations_++;
        /* sample random state (with goal biasing) */
        if ((goal_s != nullptr) && rng_.uniform01() < goalBias_ && goal_s->canSample())
            goal_s->sampleGoal(rstate);
        else
            sampler_->sampleUniform(rstate);

        /* find closest state in the tree */
        Motion *nmotion = nn_->nearest(rmotion);
        base::State *dstate = rstate;

        /* find state to add */
        double d = si_->distance(nmotion->state_, rstate);
        if (d == 0)  // this takes care of the case that the goal is a single point and we re-sample it multiple times
            continue;
        if (d > maxDistance_)
        {
            si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
            dstate = xstate;
        }

        if (checkMotion(nmotion->state_, dstate))
        {
            statesGenerated++;
            /* create a motion */
            auto *motion = new Motion(si_);
            si_->copyState(motion->state_, dstate);

            /* update fields */
            double distN = distanceFunction(nmotion, motion);

            motion->id_ = nn_->size();
            idToMotionMap_.push_back(motion);
            lowerBoundGraph_.addVertex(motion->id_);
            motion->parentApx_ = nmotion;

            std::list<std::size_t> dummy;
            lowerBoundGraph_.addEdge(nmotion->id_, motion->id_, distN, false, dummy);

            motion->costLb_ = nmotion->costLb_ + distN;
            motion->costApx_ = nmotion->costApx_ + distN;
            nmotion->childrenApx_.push_back(motion);

            std::vector<Motion *> nnVec;
            unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
            nn_->nearestK(motion, k, nnVec);
            nn_->add(motion);  // if we add the motion before the nearestK call, we will get ourselves...

            IsLessThan isLessThan(this, motion);
            std::sort(nnVec.begin(), nnVec.end(), isLessThan);

            //-------------------------------------------------//
            //  Rewiring Part (i) - find best parent of motion //
            //-------------------------------------------------//
            if (motion->parentApx_ != nnVec.front())
            {
                for (auto potentialParent : nnVec)
                {
                    double dist = distanceFunction(potentialParent, motion);
                    considerEdge(potentialParent, motion, dist);
                }
            }

            //------------------------------------------------------------------//
            //  Rewiring Part (ii)                                              //
            //  check if motion may be a better parent to one of its neighbors  //
            //------------------------------------------------------------------//
            for (auto child : nnVec)
            {
                double dist = distanceFunction(motion, child);
                considerEdge(motion, child, dist);
            }

            double dist = 0.0;
            bool sat = goal->isSatisfied(motion->state_, &dist);

            if (sat)
            {
                approxdif = dist;
                solution = motion;
            }
            if (dist < approxdif)
            {
                approxdif = dist;
                approxSol = motion;
            }

            if (solution != nullptr && bestCost_ != solution->costApx_)
            {
                OMPL_INFORM("%s: approximation cost = %g", getName().c_str(), solution->costApx_);
                bestCost_ = solution->costApx_;
            }
        }
    }

    bool solved = false;
    bool approximate = false;

    if (solution == nullptr)
    {
        solution = approxSol;
        approximate = true;
    }

    if (solution != nullptr)
    {
        lastGoalMotion_ = solution;

        /* construct the solution path */
        std::vector<Motion *> mpath;
        while (solution != nullptr)
        {
            mpath.push_back(solution);
            solution = solution->parentApx_;
        }

        /* set the solution path */
        auto path(std::make_shared<PathGeometric>(si_));
        for (int i = mpath.size() - 1; i >= 0; --i)
            path->append(mpath[i]->state_);
        // Add the solution path.
        base::PlannerSolution psol(path);
        psol.setPlannerName(getName());
        if (approximate)
            psol.setApproximate(approxdif);
        pdef_->addSolutionPath(psol);
        solved = true;
    }

    si_->freeState(xstate);
    if (rmotion->state_ != nullptr)
        si_->freeState(rmotion->state_);
    delete rmotion;

    OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated);

    return base::PlannerStatus(solved, approximate);
}

void ompl::geometric::LBTRRT::considerEdge(Motion *parent, Motion *child, double c)
{
    // optimization - check if the bounded approximation invariant
    // will be violated after the edge insertion (at least for the child node)
    // if this is the case - perform the local planning
    // this prevents the update of the graph due to the edge insertion and then the re-update as it is removed
    double potential_cost = parent->costLb_ + c;
    if (child->costApx_ > (1 + epsilon_) * potential_cost)
        if (!checkMotion(parent, child))
            return;

    // update lowerBoundGraph_
    std::list<std::size_t> affected;

    lowerBoundGraph_.addEdge(parent->id_, child->id_, c, true, affected);

    // now, check if the bounded apprimation invariant has been violated for each affected vertex
    // insert them into a priority queue ordered according to the lb cost
    std::list<std::size_t>::iterator iter;
    IsLessThanLB isLessThanLB(this);
    Lb_queue queue(isLessThanLB);

    for (iter = affected.begin(); iter != affected.end(); ++iter)
    {
        Motion *m = getMotion(*iter);
        m->costLb_ = lowerBoundGraph_.getShortestPathCost(*iter);
        if (m->costApx_ > (1 + epsilon_) * m->costLb_)
            queue.insert(m);
    }

    while (!queue.empty())
    {
        Motion *motion = *(queue.begin());
        queue.erase(queue.begin());

        if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
        {
            Motion *potential_parent = getMotion(lowerBoundGraph_.getShortestPathParent(motion->id_));
            if (checkMotion(potential_parent, motion))
            {
                double delta = lazilyUpdateApxParent(motion, potential_parent);
                updateChildCostsApx(motion, delta);
            }
            else
            {
                affected.clear();

                lowerBoundGraph_.removeEdge(potential_parent->id_, motion->id_, true, affected);

                for (iter = affected.begin(); iter != affected.end(); ++iter)
                {
                    Motion *affected = getMotion(*iter);
                    auto lb_queue_iter = queue.find(affected);
                    if (lb_queue_iter != queue.end())
                    {
                        queue.erase(lb_queue_iter);
                        affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
                        if (affected->costApx_ > (1 + epsilon_) * affected->costLb_)
                            queue.insert(affected);
                    }
                    else
                    {
                        affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
                    }
                }

                motion->costLb_ = lowerBoundGraph_.getShortestPathCost(motion->id_);
                if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
                    queue.insert(motion);

                // optimization - we can remove the opposite edge
                lowerBoundGraph_.removeEdge(motion->id_, potential_parent->id_, false, affected);
            }
        }
    }

    }

void ompl::geometric::LBTRRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion *> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_ != nullptr)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state_));

    for (auto &motion : motions)
    {
        if (motion->parentApx_ == nullptr)
            data.addStartVertex(base::PlannerDataVertex(motion->state_));
        else
            data.addEdge(base::PlannerDataVertex(motion->parentApx_->state_), base::PlannerDataVertex(motion->state_));
    }
}

void ompl::geometric::LBTRRT::updateChildCostsApx(Motion *m, double delta)
{
    for (auto child : m->childrenApx_)
    {
        child->costApx_ += delta;
        updateChildCostsApx(child, delta);
    }
}

double ompl::geometric::LBTRRT::lazilyUpdateApxParent(Motion *child, Motion *parent)
{
    double dist = distanceFunction(parent, child);
    removeFromParentApx(child);
    double deltaApx = parent->costApx_ + dist - child->costApx_;
    child->parentApx_ = parent;
    parent->childrenApx_.push_back(child);
    child->costApx_ = parent->costApx_ + dist;

    return deltaApx;
}

void ompl::geometric::LBTRRT::removeFromParentApx(Motion *m)
{
    std::vector<Motion *> &vec = m->parentApx_->childrenApx_;
    for (auto it = vec.begin(); it != vec.end(); ++it)
        if (*it == m)
        {
            vec.erase(it);
            break;
        }
}

max_stars_repo_name: jingxixu/ompl

max_issues_repo_name: jingxixu/ompl

max_forks_repo_name: jingxixu/ompl


text:
/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2014      John Fletcher

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#ifndef PHOENIX_BIND_BIND_MEMBER_VARIABLE_HPP
#define PHOENIX_BIND_BIND_MEMBER_VARIABLE_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_member_function_pointer.hpp>
#include <boost/phoenix/core/expression.hpp>
#include <boost/phoenix/core/detail/function_eval.hpp>
#include <boost/phoenix/bind/detail/member_variable.hpp>

namespace boost { namespace phoenix
{
    template <typename RT, typename ClassT, typename ClassA>
    inline
    typename boost::lazy_disable_if<
      boost::is_member_function_pointer<RT (ClassT::*)>,
        typename detail::expression::function_eval<
            detail::member_variable<RT, RT ClassT::*>
          , ClassA >//::type
      >::type const
    bind(RT ClassT::*mp, ClassA const& obj)
    {
        typedef detail::member_variable<RT, RT ClassT::*> mp_type;
        return
            detail::expression::function_eval<mp_type, ClassA>
                ::make(mp_type(mp), obj);
    }

    template <typename RT, typename ClassT>
    inline
    typename boost::lazy_disable_if<
      boost::is_member_function_pointer<RT (ClassT::*)>,
        typename detail::expression::function_eval<
            detail::member_variable<RT, RT ClassT::*>
          , ClassT >//::type
        >::type const
    bind(RT ClassT::*mp, ClassT& obj)
    {
        typedef detail::member_variable<RT, RT ClassT::*> mp_type;
        return
            detail::expression::function_eval<
                mp_type
              , ClassT
            >::make(mp_type(mp), obj);
    }

}}

#endif

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
// ======================================================================
// \title  FlatOutputAdapterComponentImpl.cpp
// \author decoy
// \brief  cpp file for FlatOutputAdapter component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged.
//
// ======================================================================

#include <Gnc/Utils/FlatOutputAdapter/FlatOutputAdapterComponentImpl.hpp>
#include "Fw/Types/BasicTypes.hpp"
#include <Eigen/Geometry>

namespace Gnc {

  // ----------------------------------------------------------------------
  // Construction, initialization, and destruction
  // ----------------------------------------------------------------------

  FlatOutputAdapterComponentImpl ::
#if FW_OBJECT_NAMES == 1
    FlatOutputAdapterComponentImpl(
        const char *const compName
    ) :
      FlatOutputAdapterComponentBase(compName)
#else
    FlatOutputAdapterComponentImpl(void)
#endif
  {

  }

  void FlatOutputAdapterComponentImpl ::
    init(
        const NATIVE_INT_TYPE instance
    )
  {
    FlatOutputAdapterComponentBase::init(instance);
  }

  FlatOutputAdapterComponentImpl ::
    ~FlatOutputAdapterComponentImpl(void)
  {

  }

  // ----------------------------------------------------------------------
  // Handler implementations for user-defined typed input ports
  // ----------------------------------------------------------------------

  void FlatOutputAdapterComponentImpl ::
    se3Cmd_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::mav_msgs::Se3FeedForward &Se3FeedForward
    )
  {
      using namespace Eigen;

      ROS::geometry_msgs::Quaternion quat1(Se3FeedForward.getattitude());

      Eigen::Quaterniond quat2(quat1.getw(),quat1.getx(),quat1.gety(),quat1.getz());

      quat2.normalize();

      Eigen::Vector3d euler = quat2.toRotationMatrix().eulerAngles(0,1,2);

      printf("after euler in the function\n");
      ROS::mav_msgs::FlatOutput flatoutput(Se3FeedForward.getheader(),
                                           Se3FeedForward.getposition(),
                                           Se3FeedForward.getvelocity(),
                                           Se3FeedForward.getacceleration(),
                                           ROS::geometry_msgs::Vector3(0.0, 0.0, 0.0), // jerk
                                           ROS::geometry_msgs::Vector3(0.0, 0.0, 0.0), // snap
                                           euler(2),
                                           0.0); //yawdot
      this->flatOutput_out(0,flatoutput);
  }

} // end namespace Gnc

max_stars_repo_name: genemerewether/fprime

max_issues_repo_name: genemerewether/fprime

max_forks_repo_name: genemerewether/fprime


text:
#include <boost/filesystem.hpp>

#include "TestTypedPropertyGraph.h"
#include "katana/Logging.h"
#include "katana/PropertyGraph.h"
#include "katana/SharedMemSys.h"
#include "katana/analytics/Utils.h"
#include "llvm/Support/CommandLine.h"
#include "stdio.h"
#include "tsuba/RDG.h"

namespace cll = llvm::cl;
namespace fs = boost::filesystem;

static cll::opt<std::string> ldbc_003InputFile(
    cll::Positional, cll::desc("<ldbc_003 input file>"), cll::Required);

katana::PropertyGraph
LoadGraph(const std::string& rdg_file) {
  KATANA_LOG_ASSERT(!rdg_file.empty());
  auto g_res = katana::PropertyGraph::Make(rdg_file, tsuba::RDGLoadOptions());

  if (!g_res) {
    KATANA_LOG_FATAL("making result: {}", g_res.error());
  }
  katana::PropertyGraph g = std::move(*g_res.value());
  return g;
}

std::string
StoreGraph(katana::PropertyGraph* g) {
  auto uri_res = katana::Uri::MakeRand("/tmp/propertyfilegraph");
  KATANA_LOG_ASSERT(uri_res);
  std::string tmp_rdg_dir(uri_res.value().path());  // path() because local
  std::string command_line;

  // Store graph. If there is a new storage format then storing it is enough to bump the version up.
  KATANA_LOG_WARN("writing graph at temp file {}", tmp_rdg_dir);
  auto write_result = g->Write(tmp_rdg_dir, command_line);
  if (!write_result) {
    KATANA_LOG_FATAL("writing result failed: {}", write_result.error());
  }
  return tmp_rdg_dir;
}

void
TestConvertGraphStorageFormat() {
  // Load existing "old" graph, which converts all uint8/bool properties into types
  // store it as a new file
  // load the new file
  // ensure the converted old graph, and the loaded new graph match
  katana::PropertyGraph g = LoadGraph(ldbc_003InputFile);
  std::string g2_rdg_file = StoreGraph(&g);
  katana::PropertyGraph g2 = LoadGraph(g2_rdg_file);

  KATANA_LOG_WARN("{}", g.ReportDiff(&g2));
  KATANA_LOG_ASSERT(g.Equals(&g2));
}

void
TestRoundTripNewStorageFormat() {
  // Test store/load cycle of a graph with the new storage format
  // To do this, we first must first convert an old graph.
  // Steps:
  // Load existing "old" graph, which converts all uint8/bool properties into types
  // Store it as a new file
  // Load the new file
  // Ensure the converted old graph, and the loaded new graph match.
  // this should be trivially true if TestLoadGraphWithoutExternalTypes() passed
  // Now store the new graph
  // Noad the new graph

  // first cycle converts old->new
  katana::PropertyGraph g = LoadGraph(ldbc_003InputFile);
  std::string g2_rdg_file = StoreGraph(&g);
  katana::PropertyGraph g2 = LoadGraph(g2_rdg_file);

  KATANA_LOG_WARN("{}", g.ReportDiff(&g2));
  KATANA_LOG_ASSERT(g.Equals(&g2));

  // second cycle doesn't do any conversion, but tests storing/loading a "new format" graph
  std::string g3_rdg_file = StoreGraph(&g2);
  katana::PropertyGraph g3 = LoadGraph(g3_rdg_file);

  KATANA_LOG_WARN("{}", g2.ReportDiff(&g3));
  KATANA_LOG_ASSERT(g2.Equals(&g3));
}

int
main(int argc, char** argv) {
  katana::SharedMemSys sys;
  cll::ParseCommandLineOptions(argc, argv);

  TestConvertGraphStorageFormat();
  TestRoundTripNewStorageFormat();

  return 0;
}

max_stars_repo_name: chakpongchung/katana

max_issues_repo_name: chakpongchung/katana

max_forks_repo_name: chakpongchung/katana


text:
/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/algorithm/string.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <ored/configuration/basecorrelationcurveconfig.hpp>
#include <ored/configuration/capfloorvolcurveconfig.hpp>
#include <ored/configuration/cdsvolcurveconfig.hpp>
#include <ored/configuration/curveconfigurations.hpp>
#include <ored/configuration/defaultcurveconfig.hpp>
#include <ored/configuration/equitycurveconfig.hpp>
#include <ored/configuration/equityvolcurveconfig.hpp>
#include <ored/configuration/fxspotconfig.hpp>
#include <ored/configuration/fxvolcurveconfig.hpp>
#include <ored/configuration/inflationcapfloorpricesurfaceconfig.hpp>
#include <ored/configuration/inflationcurveconfig.hpp>
#include <ored/configuration/securityconfig.hpp>
#include <ored/configuration/swaptionvolcurveconfig.hpp>
#include <ored/configuration/yieldcurveconfig.hpp>
#include <ored/utilities/csvfilereader.hpp>
#include <ored/utilities/parsers.hpp>
#include <oret/datapaths.hpp>
#include <oret/fileutilities.hpp>
#include <oret/toplevelfixture.hpp>

#include <ql/time/calendar.hpp>
#include <ql/time/daycounter.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore;
using namespace ore::data;

using ore::test::TopLevelFixture;
using std::ostream;

namespace bdata = boost::unit_test::data;

namespace {

// Suite level fixture that:
// - loads curve configurations
// - cleans up output files
class F : public TopLevelFixture {
public:
    CurveConfigurations curveConfigs;

    F() {
        // Read curve configurations from file
        curveConfigs.fromFile(TEST_INPUT_FILE("curve_config.xml"));
    }

    ~F() { clearOutput(TEST_OUTPUT_PATH); }
};

set<string> readQuotes(const string& filename) {
    // Read the quotes from the file
    CSVFileReader reader(filename, false, ",");

    // Insert the quotes in a set
    set<string> quotes;
    while (reader.next()) {
        quotes.insert(reader.get(0));
    }

    return quotes;
}

// Todays market input and expected quotes output file
vector<pair<string, string>> files = {
    make_pair("todays_market_only_ir.xml", "expected_quotes_only_ir.csv"),
    make_pair("todays_market_with_fx_vol_smile.xml", "expected_quotes_with_fx_vol_smile.csv"),
    make_pair("todays_market_with_fx_vol_atm.xml", "expected_quotes_with_fx_vol_atm.csv"),
    make_pair("todays_market_single_config_gbp.xml", "expected_quotes_tmp_single_gbp.csv")};

} // namespace

// Needed for BOOST_DATA_TEST_CASE below as it writes out the string pair
// https://stackoverflow.com/a/33965517/1771882
namespace boost {
namespace test_tools {
namespace tt_detail {
template <> struct print_log_value<pair<string, string>> {
    void operator()(std::ostream& os, const pair<string, string>& p) { os << "[" << p.first << "," << p.second << "]"; }
};
} // namespace tt_detail
} // namespace test_tools
} // namespace boost

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, TopLevelFixture)

BOOST_FIXTURE_TEST_SUITE(CurveConfigTest, F)

BOOST_AUTO_TEST_CASE(testFromToXml) {

    // Write the curve configurations to file
    string outputFile_1 = TEST_OUTPUT_FILE("curve_config_out_1.xml");
    curveConfigs.toFile(outputFile_1);

    // Read curve configurations from output file
    CurveConfigurations curveConfigsNew;
    curveConfigsNew.fromFile(outputFile_1);

    // Write curve configurations to file again
    string outputFile_2 = TEST_OUTPUT_FILE("curve_config_out_2.xml");
    curveConfigsNew.toFile(outputFile_2);

    // Compare contents of the output files
    BOOST_CHECK(compareFiles(outputFile_1, outputFile_2));
}

// Testing curve config quotes method with no restrictions
BOOST_AUTO_TEST_CASE(testCurveConfigQuotesAll) {

    // Ask the curve configurations object for all of its quotes
    set<string> quotes = curveConfigs.quotes();

    // Read the expected set of quotes from the file
    set<string> expectedQuotes = readQuotes(TEST_INPUT_FILE("expected_quotes_all.csv"));

    // Check that the quotes match the expected quotes
    BOOST_REQUIRE_EQUAL(quotes.size(), expectedQuotes.size());
    BOOST_CHECK_EQUAL_COLLECTIONS(quotes.begin(), quotes.end(), expectedQuotes.begin(), expectedQuotes.end());
}

// Testing curve config quotes method for various TodaysMarketParameters
BOOST_DATA_TEST_CASE(testCurveConfigQuotesSimpleTodaysMarket, bdata::make(files), filePair) {

    BOOST_TEST_MESSAGE("Testing with todaysmarket file: " << filePair.first);

    // Read the simple, single default configuration, TodaysMarketParameters instance from file
    boost::shared_ptr<TodaysMarketParameters> tmp = boost::make_shared<TodaysMarketParameters>();
    tmp->fromFile(TEST_INPUT_FILE(filePair.first));

    // Ask the curve configurations object for its quotes, restricted by the TodaysMarketParameters instance
    set<string> quotes = curveConfigs.quotes(tmp, {Market::defaultConfiguration});

    // Read the expected set of quotes from the file
    set<string> expectedQuotes = readQuotes(TEST_INPUT_FILE(filePair.second));

    // Check that the quotes match the expected quotes
    BOOST_REQUIRE_EQUAL(quotes.size(), expectedQuotes.size());
    BOOST_CHECK_EQUAL_COLLECTIONS(quotes.begin(), quotes.end(), expectedQuotes.begin(), expectedQuotes.end());
}

// Testing curve config quotes method for a TodaysMarketParameters with multiple configurations
BOOST_AUTO_TEST_CASE(testCurveConfigQuotesTodaysMarketMultipleConfigs) {

    // Read the TodaysMarketParameters instance, containing multiple configurations, from file
    boost::shared_ptr<TodaysMarketParameters> tmp = boost::make_shared<TodaysMarketParameters>();
    tmp->fromFile(TEST_INPUT_FILE("todays_market_multiple_configs.xml"));

    BOOST_REQUIRE_EQUAL(tmp->configurations().size(), 4);

    // Check the quotes for each configuration in turn
    for (const auto& kv : tmp->configurations()) {

        BOOST_TEST_MESSAGE("Checking quotes for configuration: " << kv.first);

        // Ask the curve configurations object for its quotes, restricted by the TodaysMarketParameters
        // instance and the configuration
        set<string> quotes = curveConfigs.quotes(tmp, {kv.first});

        // Read the expected set of quotes from the file
        set<string> expectedQuotes;
        if (kv.first == "collateral_inccy") {
            expectedQuotes = readQuotes(TEST_INPUT_FILE("expected_quotes_tmp_multiple_collateral_inccy.csv"));
        } else {
            expectedQuotes = readQuotes(TEST_INPUT_FILE("expected_quotes_tmp_multiple.csv"));
        }

        // Check that the quotes match the expected quotes
        BOOST_REQUIRE_EQUAL(quotes.size(), expectedQuotes.size());
        BOOST_CHECK_EQUAL_COLLECTIONS(quotes.begin(), quotes.end(), expectedQuotes.begin(), expectedQuotes.end());
    }
}

// Test fromXML for DiscountRatioYieldCurveSegment
BOOST_AUTO_TEST_CASE(testDiscountRatioSegmentFromXml) {

    // XML string
    string xml;
    xml.append("<DiscountRatio>");
    xml.append("  <Type>Discount Ratio</Type>");
    xml.append("  <BaseCurve currency=\"EUR\">EUR1D</BaseCurve>");
    xml.append("  <NumeratorCurve currency=\"BRL\">BRL-IN-USD</NumeratorCurve>");
    xml.append("  <DenominatorCurve currency=\"EUR\">EUR-IN-USD</DenominatorCurve>");
    xml.append("</DiscountRatio>");

    // XMLDocument from string
    XMLDocument doc;
    doc.fromXMLString(xml);

    // Populate empty segment from XML node
    DiscountRatioYieldCurveSegment seg;
    seg.fromXML(doc.getFirstNode(""));

    // Perform the checks
    BOOST_CHECK(seg.type() == YieldCurveSegment::Type::DiscountRatio);
    BOOST_CHECK_EQUAL(seg.typeID(), "Discount Ratio");
    BOOST_CHECK_EQUAL(seg.conventionsID(), "");
    BOOST_CHECK(seg.quotes().empty());

    BOOST_CHECK_EQUAL(seg.baseCurveId(), "EUR1D");
    BOOST_CHECK_EQUAL(seg.baseCurveCurrency(), "EUR");
    BOOST_CHECK_EQUAL(seg.numeratorCurveId(), "BRL-IN-USD");
    BOOST_CHECK_EQUAL(seg.numeratorCurveCurrency(), "BRL");
    BOOST_CHECK_EQUAL(seg.denominatorCurveId(), "EUR-IN-USD");
    BOOST_CHECK_EQUAL(seg.denominatorCurveCurrency(), "EUR");
}

// Test toXML for DiscountRatioYieldCurveSegment
BOOST_AUTO_TEST_CASE(testDiscountRatioSegmentToXml) {
    // Create a discount ratio segment
    DiscountRatioYieldCurveSegment seg("Discount Ratio", "EUR1D", "EUR", "BRL-IN-USD", "BRL", "EUR-IN-USD", "EUR");

    // Create an XML document from the segment using toXML
    XMLDocument doc;
    doc.appendNode(seg.toXML(doc));

    // Create new segment using fromXML and check entries
    DiscountRatioYieldCurveSegment newSeg;
    BOOST_CHECK_NO_THROW(newSeg.fromXML(doc.getFirstNode("")));
    BOOST_CHECK(newSeg.type() == YieldCurveSegment::Type::DiscountRatio);
    BOOST_CHECK_EQUAL(newSeg.typeID(), "Discount Ratio");
    BOOST_CHECK_EQUAL(newSeg.conventionsID(), "");
    BOOST_CHECK(newSeg.quotes().empty());

    BOOST_CHECK_EQUAL(newSeg.baseCurveId(), "EUR1D");
    BOOST_CHECK_EQUAL(newSeg.baseCurveCurrency(), "EUR");
    BOOST_CHECK_EQUAL(newSeg.numeratorCurveId(), "BRL-IN-USD");
    BOOST_CHECK_EQUAL(newSeg.numeratorCurveCurrency(), "BRL");
    BOOST_CHECK_EQUAL(newSeg.denominatorCurveId(), "EUR-IN-USD");
    BOOST_CHECK_EQUAL(newSeg.denominatorCurveCurrency(), "EUR");
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: PiotrSiejda/Engine

max_issues_repo_name: PiotrSiejda/Engine

max_forks_repo_name: PiotrSiejda/Engine


text:
#pragma once

#include <bunsan/curl/detail/string_list.hpp>
#include <bunsan/curl/options/traits.hpp>

#include <curl/curl.h>

#include <boost/range/iterator_range.hpp>

#include <initializer_list>

namespace bunsan::curl::options::wrapper {

class string_list {
 public:
  using retention_policy = retention_policy::by_wrapper;

  string_list() = default;

  template <typename Range>
  explicit string_list(const Range &range)
      : m_data(boost::copy_range<curl::detail::string_list>(range)) {}

  template <typename T>
  explicit string_list(const std::initializer_list<T> &container)
      : m_data(begin(container), end(container)) {}

  const ::curl_slist *data() const { return m_data.data(); }

 private:
  curl::detail::string_list m_data;
};

}  // namespace bunsan::curl::options::wrapper

max_stars_repo_name: bacsorg/bacs

max_issues_repo_name: bacsorg/bacs

max_forks_repo_name: bacsorg/bacs


text:
// Copyright (C) 2010-2015 Joshua Boyce
// See the file COPYING for copying permission.

#include <hadesmem/region.hpp>
#include <hadesmem/region.hpp>

#include <locale>
#include <sstream>
#include <utility>

#include <hadesmem/detail/warning_disable_prefix.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <hadesmem/detail/warning_disable_suffix.hpp>

#include <hadesmem/config.hpp>
#include <hadesmem/error.hpp>
#include <hadesmem/process.hpp>

void TestRegion()
{
  hadesmem::Process const process(::GetCurrentProcessId());

  hadesmem::Region const first_region(process, nullptr);
  hadesmem::Region first_region_2(first_region);
  hadesmem::Region first_region_3(std::move(first_region_2));
  first_region_2 = std::move(first_region_3);
  BOOST_TEST_EQ(first_region, first_region_2);

  if (first_region.GetState() != MEM_FREE)
  {
    BOOST_TEST_NE(first_region.GetBase(), static_cast<void*>(nullptr));
    BOOST_TEST_NE(first_region.GetAllocBase(), static_cast<void*>(nullptr));
    BOOST_TEST_NE(first_region.GetAllocProtect(), 0U);
    BOOST_TEST_NE(first_region.GetType(), 0U);
  }

  first_region.GetProtect();

  BOOST_TEST_NE(first_region.GetSize(), 0U);
  BOOST_TEST_NE(first_region.GetState(), 0U);

  hadesmem::Region const first_region_other(
    process, static_cast<PBYTE>(first_region.GetBase()) + 1);
  BOOST_TEST_EQ(first_region.GetBase(), first_region_other.GetBase());
  BOOST_TEST_EQ(first_region.GetAllocBase(), first_region_other.GetAllocBase());
  BOOST_TEST_EQ(first_region.GetAllocProtect(),
                first_region_other.GetAllocProtect());
  BOOST_TEST_EQ(first_region.GetSize(), first_region_other.GetSize());
  BOOST_TEST_EQ(first_region.GetState(), first_region_other.GetState());
  BOOST_TEST_EQ(first_region.GetProtect(), first_region_other.GetProtect());
  BOOST_TEST_EQ(first_region.GetType(), first_region_other.GetType());

  hadesmem::Region const second_region(
    process,
    static_cast<char const* const>(first_region.GetBase()) +
      first_region.GetSize());
  BOOST_TEST_NE(first_region, second_region);
  BOOST_TEST(first_region < second_region);
  BOOST_TEST(first_region <= second_region);
  BOOST_TEST(second_region > first_region);
  BOOST_TEST(second_region >= first_region);
  BOOST_TEST(!(first_region > second_region));
  BOOST_TEST(!(first_region >= second_region));
  BOOST_TEST(!(second_region < first_region));
  BOOST_TEST(!(second_region <= first_region));
  BOOST_TEST(first_region >= first_region);
  BOOST_TEST(first_region <= first_region);
  BOOST_TEST_EQ(first_region, first_region);

  std::stringstream second_region_str_1;
  second_region_str_1.imbue(std::locale::classic());
  second_region_str_1 << second_region.GetBase();
  std::stringstream second_region_str_2;
  second_region_str_2.imbue(std::locale::classic());
  second_region_str_2 << second_region;
  BOOST_TEST(second_region_str_1.str() == second_region_str_2.str());

  std::wstringstream second_region_str_3;
  second_region_str_3.imbue(std::locale::classic());
  second_region_str_3 << second_region.GetBase();
  std::wstringstream second_region_str_4;
  second_region_str_4.imbue(std::locale::classic());
  second_region_str_4 << second_region;
  BOOST_TEST(second_region_str_3.str() == second_region_str_4.str());
}

int main()
{
  TestRegion();
  return boost::report_errors();
}


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

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"

#include <iostream>

#include "pinocchio/tools/timer.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_variant )
{
	using namespace Eigen;
  using namespace se3;;

  JointModelVariant jmodel = JointModelRX();
  const JointDataVariant & jdata = CreateJointData::run(jmodel);

  JointDataGeneric jdatagen(jdata);
  JointModelGeneric jmodelgen(jmodel);

  se3::Model model;
}

BOOST_AUTO_TEST_SUITE_END ()

max_stars_repo_name: matthieuvigne/pinocchio

max_issues_repo_name: matthieuvigne/pinocchio

max_forks_repo_name: matthieuvigne/pinocchio