<commit_before>/***************************************************************************
 *   Copyright (C) 1998-2010 by authors (see AUTHORS.txt )                 *
 *                                                                         *
 *   This file is part of LuxRays.                                         *
 *                                                                         *
 *   LuxRays is free software; you can redistribute it and/or modify       *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 3 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   LuxRays 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 Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
 *                                                                         *
 *   LuxRays website: http://www.luxrender.net                             *
 ***************************************************************************/

#include "luxrays/core/intersectiondevice.h"
#include "luxrays/core/pixeldevice.h"
#include "luxrays/core/context.h"
#include "luxrays/kernels/kernels.h"

namespace luxrays {

//------------------------------------------------------------------------------
// DeviceDescription
//------------------------------------------------------------------------------

void DeviceDescription::FilterOne(std::vector<DeviceDescription *> &deviceDescriptions) {
	int gpuIndex = -1;
	int cpuIndex = -1;
	for (size_t i = 0; i < deviceDescriptions.size(); ++i) {
		if ((cpuIndex == -1) && (deviceDescriptions[i]->GetType() == DEVICE_TYPE_NATIVE_THREAD))
			cpuIndex = i;
#if !defined(LUXRAYS_DISABLE_OPENCL)
		else if ((gpuIndex == -1) && (deviceDescriptions[i]->GetType() == DEVICE_TYPE_OPENCL) &&
				(((OpenCLDeviceDescription *)deviceDescriptions[i])->GetOpenCLType() == OCL_DEVICE_TYPE_GPU)) {
			gpuIndex = i;
			break;
		}
#endif
	}

	if (gpuIndex != -1) {
		std::vector<DeviceDescription *> selectedDev;
		selectedDev.push_back(deviceDescriptions[gpuIndex]);
		deviceDescriptions = selectedDev;
	} else if (gpuIndex != -1) {
		std::vector<DeviceDescription *> selectedDev;
		selectedDev.push_back(deviceDescriptions[cpuIndex]);
		deviceDescriptions = selectedDev;
	} else
		deviceDescriptions.clear();
}

void DeviceDescription::Filter(const DeviceType type,
		std::vector<DeviceDescription *> &deviceDescriptions) {
	size_t i = 0;
	while (i < deviceDescriptions.size()) {
		if ((type != DEVICE_TYPE_ALL) && (deviceDescriptions[i]->GetType() != type)) {
			// Remove the device from the list
			deviceDescriptions.erase(deviceDescriptions.begin() + i);
		} else
			++i;
	}
}

std::string DeviceDescription::GetDeviceType(const DeviceType type) {
	switch (type) {
		case DEVICE_TYPE_ALL:
			return "ALL";
		case DEVICE_TYPE_NATIVE_THREAD:
			return "NATIVE_THREAD";
		case DEVICE_TYPE_OPENCL:
			return "OPENCL";
		case DEVICE_TYPE_VIRTUAL:
			return "VIRTUAL";
		default:
			return "UNKNOWN";
	}
}

//------------------------------------------------------------------------------
// Device
//------------------------------------------------------------------------------

Device::Device(const Context *context, const DeviceType type, const unsigned int index) :
	deviceContext(context), deviceType(type) {
	deviceIndex = index;
	started = false;
}

Device::~Device() {
}

void Device::Start() {
	assert (!started);

	started = true;
}

void Device::Stop() {
	assert (started);
	started = false;
}

//------------------------------------------------------------------------------
// Native Device Description
//------------------------------------------------------------------------------

void NativeThreadDeviceDescription::AddDeviceDescs(std::vector<DeviceDescription *> &descriptions) {
	unsigned int count = boost::thread::hardware_concurrency();

	// Build the descriptions
	char buf[64];
	for (size_t i = 0; i < count; ++i) {
		sprintf(buf, "NativeThread-%03d", (int)i);
		std::string deviceName = std::string(buf);

		descriptions.push_back(new NativeThreadDeviceDescription(deviceName, i));
	}
}

//------------------------------------------------------------------------------
// OpenCL Device Description
//------------------------------------------------------------------------------

#if !defined(LUXRAYS_DISABLE_OPENCL)

std::string OpenCLDeviceDescription::GetDeviceType(const cl_int type) {
	switch (type) {
		case CL_DEVICE_TYPE_ALL:
			return "TYPE_ALL";
		case CL_DEVICE_TYPE_DEFAULT:
			return "TYPE_DEFAULT";
		case CL_DEVICE_TYPE_CPU:
			return "TYPE_CPU";
		case CL_DEVICE_TYPE_GPU:
			return "TYPE_GPU";
		default:
			return "TYPE_UNKNOWN";
	}
}

std::string OpenCLDeviceDescription::GetDeviceType(const OpenCLDeviceType type) {
	switch (type) {
		case OCL_DEVICE_TYPE_ALL:
			return "ALL";
		case OCL_DEVICE_TYPE_DEFAULT:
			return "DEFAULT";
		case OCL_DEVICE_TYPE_CPU:
			return "CPU";
		case OCL_DEVICE_TYPE_GPU:
			return "GPU";
		default:
			return "UNKNOWN";
	}
}

OpenCLDeviceType OpenCLDeviceDescription::GetOCLDeviceType(const cl_int type) {
	switch (type) {
		case CL_DEVICE_TYPE_ALL:
			return OCL_DEVICE_TYPE_ALL;
		case CL_DEVICE_TYPE_DEFAULT:
			return OCL_DEVICE_TYPE_DEFAULT;
		case CL_DEVICE_TYPE_CPU:
			return OCL_DEVICE_TYPE_CPU;
		case CL_DEVICE_TYPE_GPU:
			return OCL_DEVICE_TYPE_GPU;
		default:
			return OCL_DEVICE_TYPE_UNKNOWN;
	}
}

void OpenCLDeviceDescription::AddDeviceDescs(
	const cl::Platform &oclPlatform, const OpenCLDeviceType filter,
	std::vector<DeviceDescription *> &descriptions) {
	// Get the list of devices available on the platform
	VECTOR_CLASS<cl::Device> oclDevices;
	oclPlatform.getDevices(CL_DEVICE_TYPE_ALL, &oclDevices);

	// Build the descriptions
	for (size_t i = 0; i < oclDevices.size(); ++i) {
		OpenCLDeviceType type = GetOCLDeviceType(oclDevices[i].getInfo<CL_DEVICE_TYPE>());

		if ((filter == OCL_DEVICE_TYPE_ALL) || (filter == type)) {
			OpenCLDeviceDescription *desc = new OpenCLDeviceDescription(oclDevices[i], i);
			descriptions.push_back(desc);
		}
	}
}

void OpenCLDeviceDescription::Filter(const OpenCLDeviceType type,
		std::vector<DeviceDescription *> &deviceDescriptions) {
	if (type == OCL_DEVICE_TYPE_ALL)
		return;

	size_t i = 0;
	while (i < deviceDescriptions.size()) {
		if ((deviceDescriptions[i]->GetType() == DEVICE_TYPE_OPENCL) &&
				(((OpenCLDeviceDescription *)deviceDescriptions[i])->GetOpenCLType() != type)) {
			// Remove the device from the list
			deviceDescriptions.erase(deviceDescriptions.begin() + i);
		} else
			++i;
	}
}

#endif

//------------------------------------------------------------------------------
// IntersectionDevice
//------------------------------------------------------------------------------

IntersectionDevice::IntersectionDevice(const Context *context, const DeviceType type, const unsigned int index) :
	Device(context, type, index) {
	dataSet = NULL;
}

IntersectionDevice::~IntersectionDevice() {
}

void IntersectionDevice::SetDataSet(const DataSet *newDataSet) {
	assert (!started);
	assert (newDataSet != NULL);
	assert (newDataSet->IsPreprocessed());

	dataSet = newDataSet;
}

void IntersectionDevice::Start() {
	assert (dataSet != NULL);

	Device::Start();

	statsStartTime = WallClockTime();
	statsTotalRayCount = 0.0;
	statsDeviceIdleTime = 0.0;
	statsDeviceTotalTime = 0.0;
}

//------------------------------------------------------------------------------
// PixelDevice
//------------------------------------------------------------------------------

PixelDevice::PixelDevice(const Context *context, const DeviceType type, const unsigned int index) :
	Device(context, type, index) {
}

PixelDevice::~PixelDevice() {
}

void PixelDevice::Init(const unsigned int w, const unsigned int h) {
	assert (!started);

	width = w;
	height = h;
}

void PixelDevice::Start() {
	statsTotalSampleTime = 0.0;
	statsTotalSamplesCount = 0.0;
}

}
<commit_msg>Fixed a missing call to Device::Start() in the PixelDevice class<commit_after>/***************************************************************************
 *   Copyright (C) 1998-2010 by authors (see AUTHORS.txt )                 *
 *                                                                         *
 *   This file is part of LuxRays.                                         *
 *                                                                         *
 *   LuxRays is free software; you can redistribute it and/or modify       *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 3 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   LuxRays 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 Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
 *                                                                         *
 *   LuxRays website: http://www.luxrender.net                             *
 ***************************************************************************/

#include "luxrays/core/intersectiondevice.h"
#include "luxrays/core/pixeldevice.h"
#include "luxrays/core/context.h"
#include "luxrays/kernels/kernels.h"

namespace luxrays {

//------------------------------------------------------------------------------
// DeviceDescription
//------------------------------------------------------------------------------

void DeviceDescription::FilterOne(std::vector<DeviceDescription *> &deviceDescriptions) {
	int gpuIndex = -1;
	int cpuIndex = -1;
	for (size_t i = 0; i < deviceDescriptions.size(); ++i) {
		if ((cpuIndex == -1) && (deviceDescriptions[i]->GetType() == DEVICE_TYPE_NATIVE_THREAD))
			cpuIndex = i;
#if !defined(LUXRAYS_DISABLE_OPENCL)
		else if ((gpuIndex == -1) && (deviceDescriptions[i]->GetType() == DEVICE_TYPE_OPENCL) &&
				(((OpenCLDeviceDescription *)deviceDescriptions[i])->GetOpenCLType() == OCL_DEVICE_TYPE_GPU)) {
			gpuIndex = i;
			break;
		}
#endif
	}

	if (gpuIndex != -1) {
		std::vector<DeviceDescription *> selectedDev;
		selectedDev.push_back(deviceDescriptions[gpuIndex]);
		deviceDescriptions = selectedDev;
	} else if (gpuIndex != -1) {
		std::vector<DeviceDescription *> selectedDev;
		selectedDev.push_back(deviceDescriptions[cpuIndex]);
		deviceDescriptions = selectedDev;
	} else
		deviceDescriptions.clear();
}

void DeviceDescription::Filter(const DeviceType type,
		std::vector<DeviceDescription *> &deviceDescriptions) {
	size_t i = 0;
	while (i < deviceDescriptions.size()) {
		if ((type != DEVICE_TYPE_ALL) && (deviceDescriptions[i]->GetType() != type)) {
			// Remove the device from the list
			deviceDescriptions.erase(deviceDescriptions.begin() + i);
		} else
			++i;
	}
}

std::string DeviceDescription::GetDeviceType(const DeviceType type) {
	switch (type) {
		case DEVICE_TYPE_ALL:
			return "ALL";
		case DEVICE_TYPE_NATIVE_THREAD:
			return "NATIVE_THREAD";
		case DEVICE_TYPE_OPENCL:
			return "OPENCL";
		case DEVICE_TYPE_VIRTUAL:
			return "VIRTUAL";
		default:
			return "UNKNOWN";
	}
}

//------------------------------------------------------------------------------
// Device
//------------------------------------------------------------------------------

Device::Device(const Context *context, const DeviceType type, const unsigned int index) :
	deviceContext(context), deviceType(type) {
	deviceIndex = index;
	started = false;
}

Device::~Device() {
}

void Device::Start() {
	assert (!started);
	started = true;
}

void Device::Stop() {
	assert (started);
	started = false;
}

//------------------------------------------------------------------------------
// Native Device Description
//------------------------------------------------------------------------------

void NativeThreadDeviceDescription::AddDeviceDescs(std::vector<DeviceDescription *> &descriptions) {
	unsigned int count = boost::thread::hardware_concurrency();

	// Build the descriptions
	char buf[64];
	for (size_t i = 0; i < count; ++i) {
		sprintf(buf, "NativeThread-%03d", (int)i);
		std::string deviceName = std::string(buf);

		descriptions.push_back(new NativeThreadDeviceDescription(deviceName, i));
	}
}

//------------------------------------------------------------------------------
// OpenCL Device Description
//------------------------------------------------------------------------------

#if !defined(LUXRAYS_DISABLE_OPENCL)

std::string OpenCLDeviceDescription::GetDeviceType(const cl_int type) {
	switch (type) {
		case CL_DEVICE_TYPE_ALL:
			return "TYPE_ALL";
		case CL_DEVICE_TYPE_DEFAULT:
			return "TYPE_DEFAULT";
		case CL_DEVICE_TYPE_CPU:
			return "TYPE_CPU";
		case CL_DEVICE_TYPE_GPU:
			return "TYPE_GPU";
		default:
			return "TYPE_UNKNOWN";
	}
}

std::string OpenCLDeviceDescription::GetDeviceType(const OpenCLDeviceType type) {
	switch (type) {
		case OCL_DEVICE_TYPE_ALL:
			return "ALL";
		case OCL_DEVICE_TYPE_DEFAULT:
			return "DEFAULT";
		case OCL_DEVICE_TYPE_CPU:
			return "CPU";
		case OCL_DEVICE_TYPE_GPU:
			return "GPU";
		default:
			return "UNKNOWN";
	}
}

OpenCLDeviceType OpenCLDeviceDescription::GetOCLDeviceType(const cl_int type) {
	switch (type) {
		case CL_DEVICE_TYPE_ALL:
			return OCL_DEVICE_TYPE_ALL;
		case CL_DEVICE_TYPE_DEFAULT:
			return OCL_DEVICE_TYPE_DEFAULT;
		case CL_DEVICE_TYPE_CPU:
			return OCL_DEVICE_TYPE_CPU;
		case CL_DEVICE_TYPE_GPU:
			return OCL_DEVICE_TYPE_GPU;
		default:
			return OCL_DEVICE_TYPE_UNKNOWN;
	}
}

void OpenCLDeviceDescription::AddDeviceDescs(
	const cl::Platform &oclPlatform, const OpenCLDeviceType filter,
	std::vector<DeviceDescription *> &descriptions) {
	// Get the list of devices available on the platform
	VECTOR_CLASS<cl::Device> oclDevices;
	oclPlatform.getDevices(CL_DEVICE_TYPE_ALL, &oclDevices);

	// Build the descriptions
	for (size_t i = 0; i < oclDevices.size(); ++i) {
		OpenCLDeviceType type = GetOCLDeviceType(oclDevices[i].getInfo<CL_DEVICE_TYPE>());

		if ((filter == OCL_DEVICE_TYPE_ALL) || (filter == type)) {
			OpenCLDeviceDescription *desc = new OpenCLDeviceDescription(oclDevices[i], i);
			descriptions.push_back(desc);
		}
	}
}

void OpenCLDeviceDescription::Filter(const OpenCLDeviceType type,
		std::vector<DeviceDescription *> &deviceDescriptions) {
	if (type == OCL_DEVICE_TYPE_ALL)
		return;

	size_t i = 0;
	while (i < deviceDescriptions.size()) {
		if ((deviceDescriptions[i]->GetType() == DEVICE_TYPE_OPENCL) &&
				(((OpenCLDeviceDescription *)deviceDescriptions[i])->GetOpenCLType() != type)) {
			// Remove the device from the list
			deviceDescriptions.erase(deviceDescriptions.begin() + i);
		} else
			++i;
	}
}

#endif

//------------------------------------------------------------------------------
// IntersectionDevice
//------------------------------------------------------------------------------

IntersectionDevice::IntersectionDevice(const Context *context, const DeviceType type, const unsigned int index) :
	Device(context, type, index) {
	dataSet = NULL;
}

IntersectionDevice::~IntersectionDevice() {
}

void IntersectionDevice::SetDataSet(const DataSet *newDataSet) {
	assert (!started);
	assert (newDataSet != NULL);
	assert (newDataSet->IsPreprocessed());

	dataSet = newDataSet;
}

void IntersectionDevice::Start() {
	assert (dataSet != NULL);

	Device::Start();

	statsStartTime = WallClockTime();
	statsTotalRayCount = 0.0;
	statsDeviceIdleTime = 0.0;
	statsDeviceTotalTime = 0.0;
}

//------------------------------------------------------------------------------
// PixelDevice
//------------------------------------------------------------------------------

PixelDevice::PixelDevice(const Context *context, const DeviceType type, const unsigned int index) :
	Device(context, type, index) {
}

PixelDevice::~PixelDevice() {
}

void PixelDevice::Init(const unsigned int w, const unsigned int h) {
	assert (!started);

	width = w;
	height = h;
}

void PixelDevice::Start() {
	Device::Start();

	statsTotalSampleTime = 0.0;
	statsTotalSamplesCount = 0.0;
}

}
<|endoftext|>


<commit_before>#include <vector>
#include "toml.h"
#include <random>
#include <iostream>
#include <cmath>
#include "xoroshiro128plus.h"
#include "statistics.h"
#include "properties.h"

inline void metropolis_sweep(std::vector<std::vector<double> >& lattice,
                             xoroshiro128plus& gen,
                             std::uniform_real_distribution<double>& delta_dist,
                             std::uniform_real_distribution<double>& real_dist,
                             double beta){
    int L = lattice.size();
    int i_plus, j_plus, i_minus, j_minus;
    std::vector<double> nb_angles = {0.0, 0.0, 0.0, 0.0};
    double sigma_old, sigma_new;
    double delta_E, p_accept;
    N_ATTEMPTED_FLIPS += L*L;

    for (int i=0; i<L; i++){
        for (int j=0; j<L; j++){
            delta_E = 0.0;
            i_plus  = (i != L-1) ? i+1: 0;
            i_minus = (i != 0  ) ? i-1: L-1;
            j_plus  = (j != L-1) ? j+1: 0;
            j_minus = (j != 0  ) ? j-1: L-1;

            //neighboring angles
            nb_angles[0] = lattice[i_plus][j];
            nb_angles[1] = lattice[i_minus][j];
            nb_angles[2] = lattice[i][j_plus];
            nb_angles[3] = lattice[i][j_minus];

            //delta_E = E(sigma_new) - E(sigma)
            sigma_old = lattice[i][j];
            sigma_new = sigma_old - delta_dist(gen);

            for (int nb=0; nb<4; nb++){
                delta_E += cos(sigma_old - nb_angles[nb]) - cos(sigma_new - nb_angles[nb]);
            }

            if (delta_E <= 0){
                //good change
                lattice[i][j] = sigma_new;
                N_ACCEPTED_FLIPS++;
            }else{
                p_accept = exp(-beta*delta_E);
                if (p_accept > real_dist(gen)){
                    lattice[i][j] = sigma_new;
                    N_ACCEPTED_FLIPS++;
                }
            }
        }
    }
};

inline void microcanonical_sweep(std::vector<std::vector<double> >& lattice){
    int L = lattice.size();
    int i_plus, j_plus, i_minus, j_minus;
    std::vector<double> nb_angles = {0.0, 0.0, 0.0, 0.0};
    double Vxx, Vxy, Vx_sq;
    double x, y, x_new, y_new;
    double sigma;

    for (int i=0; i<L; i++){
        for (int j=0; j<L; j++){
            Vxx = 0.0;
            Vxy = 0.0;
            sigma = lattice[i][j];
            x = cos(sigma);
            y = sin(sigma);

            i_plus  = (i != L-1) ? i+1: 0;
            i_minus = (i != 0  ) ? i-1: L-1;
            j_plus  = (j != L-1) ? j+1: 0;
            j_minus = (j != 0  ) ? j-1: L-1;

            //neighboring angles
            nb_angles[0] = lattice[i_plus][j];
            nb_angles[1] = lattice[i_minus][j];
            nb_angles[2] = lattice[i][j_plus];
            nb_angles[3] = lattice[i][j_minus];

            for (int nb=0; nb<4; nb++){
                Vxx += cos(nb_angles[nb]);
                Vxy += sin(nb_angles[nb]);
            }
            Vx_sq = Vxx*Vxx + Vxy*Vxy;
            x_new = 2 * (x * Vxx + y * Vxy) / Vx_sq * Vxx - x;
            y_new = 2 * (x * Vxx + y * Vxy) / Vx_sq * Vxy - y;
            lattice[i][j] = atan2(y_new, x_new);
        }
    }

};
inline void cluster_sweep(){};

////////////////////////////////////////
//Standard metropolis + microcanonical//
////////////////////////////////////////
void metropolis(int n_steps_therm, int n_steps_prod, int side_length, double beta, std::string outfile, int outfreq, int conf_outfreq, double delta, int n_mc_sweep){
    //store lattice as 2d array
    std::vector<std::vector<double> > lattice;

    //Initialize 128 bits of random state
    std::random_device rd;
    std::array<uint32_t,4> seed;
    seed[0] = rd();
    seed[1] = rd();
    seed[2] = rd();
    seed[3] = rd();
    xoroshiro128plus gen(seed);

    std::uniform_real_distribution<double> angle_dist(0.0, 2.0*M_PI);
    std::uniform_real_distribution<double> delta_dist(-delta/2.0, delta/2.0);
    std::uniform_real_distribution<double> real_dist(0.0, 1.0);
    std::vector<double> energies;

    //setup lattice (hot start)
    for (int i=0; i<side_length; i++){
        std::vector<double> line;
        for (int j=0; j<side_length; j++){
            line.push_back(angle_dist(gen));
        }
        line.shrink_to_fit();
        lattice.push_back(line);
    }
    lattice.shrink_to_fit();

    //thermalize
    std::cout << lattice[0][0] << std::endl;
    for (int n=0; n<n_steps_therm; n++){
        metropolis_sweep(lattice, gen, delta_dist, real_dist, beta);
        //do n_mc_sweep of microcanonical sweeps
        for (int mc=0; mc<n_mc_sweep; mc++){microcanonical_sweep(lattice);}
        if (n%100 == 0){
            std::cout << "\rThermalizing, " << std::setprecision(3) << 100 * (double)n / (double)n_steps_therm << "%" << std::flush;
        }
    }
    std::cout << "\rThermalizing, ...Done!";
    std::cout << std::endl;
    //production run
    for (int n=0; n<n_steps_prod; n++){
        metropolis_sweep(lattice, gen, delta_dist, real_dist, beta);
        for (int mc=0; mc<n_mc_sweep; mc++){microcanonical_sweep(lattice);}
        if (n%outfreq == 0){
            energies.push_back(compute_energy(lattice));
        }
        if (n%conf_outfreq == 0){
            write_configuration(lattice, outfile + ".conf" + std::to_string(n));
        }
        if (n%100 == 0){
            std::cout << "\rRunning production, " << std::setprecision(3) << 100 * (double)n / (double)n_steps_prod << "%" << std::flush;
        }
    }
    std::cout << "\rRunning production, ...Done!";
    std::cout << std::endl;

    std::cout << "Beta: " << beta << " Acceptance ratio: " << (double) N_ACCEPTED_FLIPS / (double) N_ATTEMPTED_FLIPS << std::endl;
    //write properties and final configuration to file
    write_properties(energies, outfile + ".autocorr");
    write_energies(energies, outfile + ".energies");
    write_configuration(lattice, outfile + ".conf");

};

/////////////////////
//Cluster algorithm//
/////////////////////

<commit_msg>First steps towards cluster algorithm<commit_after>#include <vector>
#include "toml.h"
#include <random>
#include <iostream>
#include <cmath>
#include "xoroshiro128plus.h"
#include "statistics.h"
#include "properties.h"
#include <stack>
#include <set>

inline void metropolis_sweep(std::vector<std::vector<double> >& lattice,
                             xoroshiro128plus& gen,
                             std::uniform_real_distribution<double>& delta_dist,
                             std::uniform_real_distribution<double>& real_dist,
                             double beta){
    int L = lattice.size();
    int i_plus, j_plus, i_minus, j_minus;
    std::vector<double> nb_angles = {0.0, 0.0, 0.0, 0.0};
    double sigma_old, sigma_new;
    double delta_E, p_accept;
    N_ATTEMPTED_FLIPS += L*L;

    for (int i=0; i<L; i++){
        for (int j=0; j<L; j++){
            delta_E = 0.0;
            i_plus  = (i != L-1) ? i+1: 0;
            i_minus = (i != 0  ) ? i-1: L-1;
            j_plus  = (j != L-1) ? j+1: 0;
            j_minus = (j != 0  ) ? j-1: L-1;

            //neighboring angles
            nb_angles[0] = lattice[i_plus][j];
            nb_angles[1] = lattice[i_minus][j];
            nb_angles[2] = lattice[i][j_plus];
            nb_angles[3] = lattice[i][j_minus];

            //delta_E = E(sigma_new) - E(sigma)
            sigma_old = lattice[i][j];
            sigma_new = sigma_old - delta_dist(gen);

            for (int nb=0; nb<4; nb++){
                delta_E += cos(sigma_old - nb_angles[nb]) - cos(sigma_new - nb_angles[nb]);
            }

            if (delta_E <= 0){
                //good change
                lattice[i][j] = sigma_new;
                N_ACCEPTED_FLIPS++;
            }else{
                p_accept = exp(-beta*delta_E);
                if (p_accept > real_dist(gen)){
                    lattice[i][j] = sigma_new;
                    N_ACCEPTED_FLIPS++;
                }
            }
        }
    }
};

inline void microcanonical_sweep(std::vector<std::vector<double> >& lattice){
    int L = lattice.size();
    int i_plus, j_plus, i_minus, j_minus;
    std::vector<double> nb_angles = {0.0, 0.0, 0.0, 0.0};
    double Vxx, Vxy, Vx_sq;
    double x, y, x_new, y_new;
    double sigma;

    for (int i=0; i<L; i++){
        for (int j=0; j<L; j++){
            Vxx = 0.0;
            Vxy = 0.0;
            sigma = lattice[i][j];
            x = cos(sigma);
            y = sin(sigma);

            i_plus  = (i != L-1) ? i+1: 0;
            i_minus = (i != 0  ) ? i-1: L-1;
            j_plus  = (j != L-1) ? j+1: 0;
            j_minus = (j != 0  ) ? j-1: L-1;

            //neighboring angles
            nb_angles[0] = lattice[i_plus][j];
            nb_angles[1] = lattice[i_minus][j];
            nb_angles[2] = lattice[i][j_plus];
            nb_angles[3] = lattice[i][j_minus];

            for (int nb=0; nb<4; nb++){
                Vxx += cos(nb_angles[nb]);
                Vxy += sin(nb_angles[nb]);
            }
            Vx_sq = Vxx*Vxx + Vxy*Vxy;
            x_new = 2 * (x * Vxx + y * Vxy) / Vx_sq * Vxx - x;
            y_new = 2 * (x * Vxx + y * Vxy) / Vx_sq * Vxy - y;
            lattice[i][j] = atan2(y_new, x_new);
        }
    }

};

inline void cluster_sweep(std::vector<std::vector<int> >& lattice,
                          xoroshiro128plus& gen,
                          std::uniform_real_distribution<double>& angle_dist,
                          std::uniform_real_distribution<double>& double_dist,
                          std::uniform_int_distribution<int>& L_dist,
                          double beta){
    int i_plus, j_plus, i_minus, j_minus;
    int L = lattice.size();
    double p_add, sx, sy;
    N_ATTEMPTED_FLIPS += L*L;

    //make stack of neighbor spins
    std::stack<std::pair<int,int> > cluster_buffer;
    std::pair<int,int> current_site;

    //make set of sites currently in the cluster
    std::set<std::pair<int,int> > cluster_set;
    std::set<std::pair<int,int> >::iterator cluster_set_iterator;

    //pick random vector r
    double theta = angle_dist(gen);
    double rx = cos(theta);
    double ry = sin(theta);

    //pick starting seed
    int i = L_dist(gen);
    int j = L_dist(gen);

    //add seed site to accepted spins
    N_ACCEPTED_FLIPS++;
    cluster_set.insert(std::make_pair(i,j));
    cluster_buffer.push(std::make_pair(i, j));

    while(!cluster_buffer.empty()){
        std::vector<std::pair<int,int> > neighbors;
        //look at the top of the stack, and find the neighbors
        current_site = cluster_buffer.top();
        cluster_buffer.pop();

        i = current_site.first;
        j = current_site.second;

        sx = cos(lattice[i][j])*rx + sin(lattice[i][j])*ry;

        i_plus  = (i != L-1) ? i+1: 0;
        i_minus = (i != 0  ) ? i-1: L-1;
        j_plus  = (j != L-1) ? j+1: 0;
        j_minus = (j != 0  ) ? j-1: L-1;

        neighbors.push_back(std::make_pair(i_minus, j));
        neighbors.push_back(std::make_pair(i_plus, j));
        neighbors.push_back(std::make_pair(i, j_minus));
        neighbors.push_back(std::make_pair(i, j_plus));

        for (int n=0; n<(int)neighbors.size(); n++){
            i = neighbors[n].first;
            j = neighbors[n].second;
            if (cluster_set.count(neighbors[n]) == 0){
                // neighbor is *not* in cluster already
                // check if it should be added
                sy = cos(lattice[i][j]*rx) + sin(lattice[i][j])*ry;
                p_add = 1.0 - exp(-2*beta*sx*sy);
                if (double_dist(gen) < p_add){
                    N_ACCEPTED_FLIPS++;
                    cluster_set.insert(neighbors[n]);
                    cluster_buffer.push(neighbors[n]);
                }
           }
        }
    }
    for (auto site : cluster_set){
        //flip all spins in the cluster
        i = site.first;
        j = site.second;
        sx = cos(lattice[i][j])*rx + sin(lattice[i][j])*ry;
        lattice[i][j] = atan2(lattice[i][j] - 2*sx*ry, lattice[i][j] - 2*sx*rx);
    }
}

inline void cluster_sweep(){};

////////////////////////////////////////
//Standard metropolis + microcanonical//
////////////////////////////////////////
void metropolis(int n_steps_therm, int n_steps_prod, int side_length, double beta, std::string outfile, int outfreq, int conf_outfreq, double delta, int n_mc_sweep){
    //store lattice as 2d array
    std::vector<std::vector<double> > lattice;

    //Initialize 128 bits of random state
    std::random_device rd;
    std::array<uint32_t,4> seed;
    seed[0] = rd();
    seed[1] = rd();
    seed[2] = rd();
    seed[3] = rd();
    xoroshiro128plus gen(seed);

    std::uniform_real_distribution<double> angle_dist(0.0, 2.0*M_PI);
    std::uniform_real_distribution<double> delta_dist(-delta/2.0, delta/2.0);
    std::uniform_real_distribution<double> real_dist(0.0, 1.0);
    std::vector<double> energies;

    //setup lattice (hot start)
    for (int i=0; i<side_length; i++){
        std::vector<double> line;
        for (int j=0; j<side_length; j++){
            line.push_back(angle_dist(gen));
        }
        line.shrink_to_fit();
        lattice.push_back(line);
    }
    lattice.shrink_to_fit();

    //thermalize
    std::cout << lattice[0][0] << std::endl;
    for (int n=0; n<n_steps_therm; n++){
        metropolis_sweep(lattice, gen, delta_dist, real_dist, beta);
        //do n_mc_sweep of microcanonical sweeps
        for (int mc=0; mc<n_mc_sweep; mc++){microcanonical_sweep(lattice);}
        if (n%100 == 0){
            std::cout << "\rThermalizing, " << std::setprecision(3) << 100 * (double)n / (double)n_steps_therm << "%" << std::flush;
        }
    }
    std::cout << "\rThermalizing, ...Done!";
    std::cout << std::endl;
    //production run
    for (int n=0; n<n_steps_prod; n++){
        metropolis_sweep(lattice, gen, delta_dist, real_dist, beta);
        for (int mc=0; mc<n_mc_sweep; mc++){microcanonical_sweep(lattice);}
        if (n%outfreq == 0){
            energies.push_back(compute_energy(lattice));
        }
        if (n%conf_outfreq == 0){
            write_configuration(lattice, outfile + ".conf" + std::to_string(n));
        }
        if (n%100 == 0){
            std::cout << "\rRunning production, " << std::setprecision(3) << 100 * (double)n / (double)n_steps_prod << "%" << std::flush;
        }
    }
    std::cout << "\rRunning production, ...Done!";
    std::cout << std::endl;

    std::cout << "Beta: " << beta << " Acceptance ratio: " << (double) N_ACCEPTED_FLIPS / (double) N_ATTEMPTED_FLIPS << std::endl;
    //write properties and final configuration to file
    write_properties(energies, outfile + ".autocorr");
    write_energies(energies, outfile + ".energies");
    write_configuration(lattice, outfile + ".conf");

};

/////////////////////
//Cluster algorithm//
/////////////////////

<|endoftext|>


<commit_before>#include "TestScene.h"
#include "Material.h"
#include "Mesh.h"
#include "ObjLoader.h"
#include "SceneObject.h"
#include "PrimitiveTypes.h"

namespace BasicRenderer
{
	// TODO move to file
	World* TestScene()
	{
		World* scene = new World();

		scene->sun.SetDirection({ 0.5f, -1.0f, -1.0f });
		scene->sun.intensity = 1.f;
		scene->ambientLightIntensity = 0.1f;
		scene->ambientLightColor = { 0.529f, 0.808f, 0.922f };

		Material* emissive = new Material({ 1.0f, 1.0f, 1.0f });
		emissive->emissive = 10.f;

		Material* white = new Material({ 1.0f, 1.0f, 1.0f });
		Material* red = new Material({ 1.0f, 0.0f, 0.0f });
		Material* green = new Material({ 0.0f, 1.0f, 0.0f });
		Material* blue = new Material({ 0.0f, 0.1f, 1.0f });

		Material* silver = new Material({ 0.972f, 0.960f, 0.915f }, Material::Type::METALLIC);
		silver->metallic = 1.0f;
		Material* copper = new Material({ 0.955f, 0.637f, 0.538f });
		Material* gold = new Material({ 1.0f, 0.766f, 0.336f }, Material::Type::METALLIC);
		gold->metallic = 0.5f;
		Material* chromium = new Material({ .550f, 0.556f, 0.554f }, Material::Type::DIELECTRIC);

		std::shared_ptr<Mesh> bunnyMesh(ObjLoader::Load("../../../assets/bunny.obj"));
		std::shared_ptr<Mesh> cubeMesh(ObjLoader::Load("../../../assets/cube.obj"));
		std::shared_ptr<Mesh> quadMesh(ObjLoader::Load("../../../assets/quad.obj"));

		// TODO move to transform hierarchy when ready
		const float primitive_scale = 4.f;
		const float half_primitive_scale = primitive_scale * 0.5f;
		const Vector3 primitive_scale_vector = { primitive_scale, primitive_scale, primitive_scale };
		const Vector3 box_position = { 0.f, 0.f, -half_primitive_scale };

		SceneObject* bunny = new SceneObject(bunnyMesh, gold);
		bunny->GetTransform().SetScale(primitive_scale * 3.f, primitive_scale * 3.f, primitive_scale * 3.f);
		bunny->GetTransform().SetPosition(box_position + Vector3(primitive_scale * 0.25f, primitive_scale * -0.6f, primitive_scale * 0.15f));

		Sphere* sp = new Sphere(box_position + Vector3(primitive_scale * -0.25f, primitive_scale * -0.25f, primitive_scale * 0.2f), primitive_scale * 0.2f, silver);

		Quad* floor = new Quad(quadMesh, white);
		floor->GetTransform().SetPosition(box_position + Vector3(0.f, -half_primitive_scale, 0.f));
		floor->GetTransform().RotateDeg(-90.f, 0.f, 0.f);
		floor->GetTransform().SetScale(primitive_scale_vector);

		Quad* ceiling = new Quad(quadMesh, white);
		ceiling->GetTransform().SetPosition(box_position + Vector3(0.f, half_primitive_scale, 0.f));
		ceiling->GetTransform().RotateDeg(90.f, 0.f, 0.f);
		ceiling->GetTransform().SetScale(primitive_scale_vector);

		Quad* back = new Quad(quadMesh, white);
		back->GetTransform().SetPosition(box_position + Vector3(0.f, 0.f, -half_primitive_scale ));
		back->GetTransform().RotateDeg(0.f, 0.f, 0.f);
		back->GetTransform().SetScale(primitive_scale_vector);

		Quad* front = new Quad(quadMesh, white);
		front->GetTransform().SetPosition(box_position + Vector3(0.f, 0.f, half_primitive_scale));
		front->GetTransform().RotateDeg(0.f, 180.f, 0.f);
		front->GetTransform().SetScale(primitive_scale_vector);

		Quad* left = new Quad(quadMesh, green);
		left->GetTransform().SetPosition(box_position + Vector3(-half_primitive_scale, 0.f, 0.f));
		left->GetTransform().RotateDeg(0.f, 90.f, 0.f);
		left->GetTransform().SetScale(primitive_scale_vector);

		Quad* right = new Quad(quadMesh, red);
		right->GetTransform().SetPosition(box_position + Vector3(half_primitive_scale, 0.f, 0.f));
		right->GetTransform().RotateDeg(0.f, -90.f, 0.f);
		right->GetTransform().SetScale(primitive_scale_vector);

		Quad* light = new Quad(quadMesh, emissive);
		light->GetTransform().SetPosition(box_position + Vector3(0.f, half_primitive_scale - 0.05f, 0.f));
		light->GetTransform().RotateDeg(90.f, 0.f, 0.f);
		light->GetTransform().SetScale(primitive_scale_vector * 0.3f);

		Cube* cube = new Cube(cubeMesh, white);
		cube->GetTransform().SetPosition(box_position + Vector3(primitive_scale * -0.18f, primitive_scale * -0.15f, primitive_scale * -0.2f ));
		cube->GetTransform().RotateDeg(0.f, 120.f, 0.f);
		cube->GetTransform().SetScale(primitive_scale * 0.3f, primitive_scale * 0.7f, primitive_scale * 0.3f);

		Cube* cube2 = new Cube(cubeMesh, white);
		cube2->GetTransform().SetPosition(box_position + Vector3(primitive_scale * 0.25f, primitive_scale * -0.375f, primitive_scale * -0.25f ));
		cube2->GetTransform().RotateDeg(0.f, -25.f, 0.f);
		cube2->GetTransform().SetScale(primitive_scale_vector * 0.25f);

		//scene->Add(bunny);

		scene->Add(sp);

		scene->Add(floor);
		scene->Add(ceiling);
		scene->Add(back);
		scene->Add(front);
		scene->Add(left);
		scene->Add(right);

		scene->Add(light);

		scene->Add(cube);
		scene->Add(cube2);

		return scene;
	}

}<commit_msg>fixed cornell box colours<commit_after>#include "TestScene.h"
#include "Material.h"
#include "Mesh.h"
#include "ObjLoader.h"
#include "SceneObject.h"
#include "PrimitiveTypes.h"

namespace BasicRenderer
{
	// TODO move to file
	World* TestScene()
	{
		World* scene = new World();

		scene->sun.SetDirection({ 0.5f, -1.0f, -1.0f });
		scene->sun.intensity = 1.f;
		scene->ambientLightIntensity = 0.1f;
		scene->ambientLightColor = { 0.529f, 0.808f, 0.922f };

		Material* emissive = new Material({ 1.0f, 1.0f, 1.0f });
		emissive->emissive = 10.f;

		Material* white = new Material({ 1.0f, 1.0f, 1.0f });
		Material* red = new Material({ 1.0f, 0.0f, 0.0f });
		Material* green = new Material({ 0.0f, 1.0f, 0.0f });
		Material* blue = new Material({ 0.0f, 0.1f, 1.0f });

		Material* silver = new Material({ 0.972f, 0.960f, 0.915f }, Material::Type::METALLIC);
		silver->metallic = 1.0f;
		Material* copper = new Material({ 0.955f, 0.637f, 0.538f });
		Material* gold = new Material({ 1.0f, 0.766f, 0.336f }, Material::Type::METALLIC);
		gold->metallic = 0.5f;
		Material* chromium = new Material({ .550f, 0.556f, 0.554f }, Material::Type::DIELECTRIC);

		std::shared_ptr<Mesh> bunnyMesh(ObjLoader::Load("../../../assets/bunny.obj"));
		std::shared_ptr<Mesh> cubeMesh(ObjLoader::Load("../../../assets/cube.obj"));
		std::shared_ptr<Mesh> quadMesh(ObjLoader::Load("../../../assets/quad.obj"));

		// TODO move to transform hierarchy when ready
		const float primitive_scale = 4.f;
		const float half_primitive_scale = primitive_scale * 0.5f;
		const Vector3 primitive_scale_vector = { primitive_scale, primitive_scale, primitive_scale };
		const Vector3 box_position = { 0.f, 0.f, -half_primitive_scale };

		SceneObject* bunny = new SceneObject(bunnyMesh, gold);
		bunny->GetTransform().SetScale(primitive_scale * 3.f, primitive_scale * 3.f, primitive_scale * 3.f);
		bunny->GetTransform().SetPosition(box_position + Vector3(primitive_scale * 0.25f, primitive_scale * -0.6f, primitive_scale * 0.15f));

		Sphere* sp = new Sphere(box_position + Vector3(primitive_scale * -0.25f, primitive_scale * -0.25f, primitive_scale * 0.2f), primitive_scale * 0.2f, silver);

		Quad* floor = new Quad(quadMesh, white);
		floor->GetTransform().SetPosition(box_position + Vector3(0.f, -half_primitive_scale, 0.f));
		floor->GetTransform().RotateDeg(-90.f, 0.f, 0.f);
		floor->GetTransform().SetScale(primitive_scale_vector);

		Quad* ceiling = new Quad(quadMesh, white);
		ceiling->GetTransform().SetPosition(box_position + Vector3(0.f, half_primitive_scale, 0.f));
		ceiling->GetTransform().RotateDeg(90.f, 0.f, 0.f);
		ceiling->GetTransform().SetScale(primitive_scale_vector);

		Quad* back = new Quad(quadMesh, white);
		back->GetTransform().SetPosition(box_position + Vector3(0.f, 0.f, -half_primitive_scale ));
		back->GetTransform().RotateDeg(0.f, 0.f, 0.f);
		back->GetTransform().SetScale(primitive_scale_vector);

		Quad* front = new Quad(quadMesh, white);
		front->GetTransform().SetPosition(box_position + Vector3(0.f, 0.f, half_primitive_scale));
		front->GetTransform().RotateDeg(0.f, 180.f, 0.f);
		front->GetTransform().SetScale(primitive_scale_vector);

		Quad* left = new Quad(quadMesh, red);
		left->GetTransform().SetPosition(box_position + Vector3(-half_primitive_scale, 0.f, 0.f));
		left->GetTransform().RotateDeg(0.f, 90.f, 0.f);
		left->GetTransform().SetScale(primitive_scale_vector);

		Quad* right = new Quad(quadMesh, green);
		right->GetTransform().SetPosition(box_position + Vector3(half_primitive_scale, 0.f, 0.f));
		right->GetTransform().RotateDeg(0.f, -90.f, 0.f);
		right->GetTransform().SetScale(primitive_scale_vector);

		Quad* light = new Quad(quadMesh, emissive);
		light->GetTransform().SetPosition(box_position + Vector3(0.f, half_primitive_scale - 0.05f, 0.f));
		light->GetTransform().RotateDeg(90.f, 0.f, 0.f);
		light->GetTransform().SetScale(primitive_scale_vector * 0.3f);

		Cube* cube = new Cube(cubeMesh, white);
		cube->GetTransform().SetPosition(box_position + Vector3(primitive_scale * -0.18f, primitive_scale * -0.15f, primitive_scale * -0.2f ));
		cube->GetTransform().RotateDeg(0.f, 120.f, 0.f);
		cube->GetTransform().SetScale(primitive_scale * 0.3f, primitive_scale * 0.7f, primitive_scale * 0.3f);

		Cube* cube2 = new Cube(cubeMesh, white);
		cube2->GetTransform().SetPosition(box_position + Vector3(primitive_scale * 0.25f, primitive_scale * -0.375f, primitive_scale * -0.25f ));
		cube2->GetTransform().RotateDeg(0.f, -25.f, 0.f);
		cube2->GetTransform().SetScale(primitive_scale_vector * 0.25f);

		//scene->Add(bunny);

		scene->Add(sp);

		scene->Add(floor);
		scene->Add(ceiling);
		scene->Add(back);
		scene->Add(front);
		scene->Add(left);
		scene->Add(right);

		scene->Add(light);

		scene->Add(cube);
		scene->Add(cube2);

		return scene;
	}

}<|endoftext|>


<commit_before>/*********************************************************************
 * jacobian_cpp.cpp
 *
 * Copyright (c) 2017, ISCI / National Chiao Tung University (NCTU)
 *
 * Author: Howard Chen (s880367@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 **********************************************************************/

#include <iostream>
#include <Eigen/Dense>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <math.h>
#include <vector>
#include "tm_kinematics/tm_kin.h"
#include <unistd.h>
#include <sys/time.h>

#define D1 0.1451
#define D2 0
#define D3 0
#define D4 -0.1222
#define D5 0.106
#define D6 0.1144

#define A1 0
#define A2 0.329
#define A3 0.3115
#define A4 0
#define A5 0
#define A6 0

#define ALPHA1 -90
#define ALPHA2 0
#define ALPHA3 0
#define ALPHA4 90
#define ALPHA5 90
#define ALPHA6 0

#define PI 3.141592654
#define DEG2RAD 0.01745329252
#define RAD2DEG 57.29577951

using namespace std;

Eigen::Matrix<float, 6, 6> Jacobian_gen(Eigen::Matrix<float, 6,1> q)
{
	Eigen::Matrix<float, 6, 6> jacobian = Eigen::Matrix<float, 6, 6>::Zero();
	jacobian << D6*(cos(q(0))*cos(q(4)) + sin(q(4))*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))))) - D4*cos(q(0)) - D5*(cos(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))) - sin(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0)))) - A2*cos(q(1))*sin(q(0)) - A3*cos(q(1))*cos(q(2))*sin(q(0)) + A3*sin(q(0))*sin(q(1))*sin(q(2)),   D5*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1)))) - A2*cos(q(0))*sin(q(1)) - D6*sin(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2)))) - A3*cos(q(0))*cos(q(1))*sin(q(2)) - A3*cos(q(0))*cos(q(2))*sin(q(1)),   D5*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2)))) - A3*cos(q(0))*cos(q(1))*sin(q(2)) - A3*cos(q(0))*cos(q(2))*sin(q(1)),   D5*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2)))), -D6*(sin(q(0))*sin(q(4)) - cos(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))))),                                                                                0.,
 				D5*(cos(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2)))) - D4*sin(q(0)) + D6*(cos(q(4))*sin(q(0)) + sin(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))))) + A2*cos(q(0))*cos(q(1)) + A3*cos(q(0))*cos(q(1))*cos(q(2)) - A3*cos(q(0))*sin(q(1))*sin(q(2)), - D5*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))) - sin(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0)))) - A2*sin(q(0))*sin(q(1)) - A3*cos(q(1))*sin(q(0))*sin(q(2)) - A3*cos(q(2))*sin(q(0))*sin(q(1)), - D5*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))) - sin(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0)))) - A3*cos(q(1))*sin(q(0))*sin(q(2)) - A3*cos(q(2))*sin(q(0))*sin(q(1)), - D5*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))) - sin(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0)))),  D6*(cos(q(0))*sin(q(4)) - cos(q(4))*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))))),                                                                                0.,
																				                                                            0.,                                                                                A3*sin(q(1))*sin(q(2)) - D5*(cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2)))) - A3*cos(q(1))*cos(q(2)) - A2*cos(q(1)) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1)))),                                                                                A3*sin(q(1))*sin(q(2)) - A3*cos(q(1))*cos(q(2)) - D5*(cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1)))),                                                                 				- D5*(cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2)))) - D6*sin(q(4))*(cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1)))),                                                     			 -D6*cos(q(4))*(cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2)))),                                                                                0.,
																				                     	                                      0.,                                                                                -sin(q(0)),                                                                                -sin(q(0)),                                                                                -sin(q(0)),                                   		 cos(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))),   cos(q(4))*sin(q(0)) + sin(q(4))*(cos(q(3))*(cos(q(0))*cos(q(1))*cos(q(2)) - cos(q(0))*sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(0))*cos(q(1))*sin(q(2)) + cos(q(0))*cos(q(2))*sin(q(1)))),
																				                                                            0.,                                                                                cos(q(0)),                                                                                cos(q(0)),                                                                                cos(q(0)),                                   		 cos(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1))) - sin(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))), - cos(q(0))*cos(q(4)) - sin(q(4))*(cos(q(3))*(sin(q(0))*sin(q(1))*sin(q(2)) - cos(q(1))*cos(q(2))*sin(q(0))) + sin(q(3))*(cos(q(1))*sin(q(0))*sin(q(2)) + cos(q(2))*sin(q(0))*sin(q(1)))),
																				                                                            1.,                                                                                0.,                                                                                0.,                                                                                0.,                                                                   				 cos(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))) - sin(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))),                                                    			   -sin(q(4))*(cos(q(3))*(cos(q(1))*sin(q(2)) + cos(q(2))*sin(q(1))) + sin(q(3))*(cos(q(1))*cos(q(2)) - sin(q(1))*sin(q(2))));
	return jacobian;
}

void PrintMatrix_eigen(Eigen::MatrixXf InputMatrix)
{
	Eigen::MatrixXf InputTranspose = InputMatrix.transpose();
	short count = 0;
	int row = InputMatrix.rows();
	int col = InputMatrix.cols();

	for (int i = 0; i < row*col; ++i)
	{
		printf("%10.4f ", InputTranspose(i));
		if (count == row-1)
		{
			count = 0;
			printf("\n");
		}
		else
			count++;
	}
}

void PrintMatrix_std(double *InputMatrix)
{
	short count = 0;

	for (int i = 0; i < 16; ++i)
	{
		printf("%10.4f ", InputMatrix[i]);
		if (count == 3)
		{
			count = 0;
			printf("\n");
		}
		else
			count++;
	}
	printf("\n");
}

void Matrix2DoubleArray(Eigen::MatrixXf InputMatrix, double *T)
{
	Eigen::MatrixXf InputTranspose = InputMatrix.transpose();
	short row = InputMatrix.rows();
	short col = InputMatrix.cols();

	for (int i = 0; i < row*col; ++i)
		T[i] = InputTranspose(i);
}

int main(int argc, char const *argv[])
{
	double *q_inv, *q_forward, *T;

	q_inv = new double[48];
	q_forward = new double[6];
	T = new double[16];

	Eigen::Matrix<float, 6, 1> jointspd = Eigen::Matrix<float, 6, 1>::Zero();
	Eigen::Matrix<float, 6, 1> effspd = Eigen::Matrix<float, 6, 1>::Zero();
	Eigen::Matrix<float, 6, 1> q_AfterHomeOffset, q_BeforeHomeOffset;
	Eigen::Matrix<float, 6, 1> home;

	home << 0, -PI*0.5, 0, PI*0.5, 0, 0;

	q_BeforeHomeOffset << 	strtod(argv[1],NULL),
					strtod(argv[2],NULL),
					strtod(argv[3],NULL),
					strtod(argv[4],NULL),
					strtod(argv[5],NULL),
					strtod(argv[6],NULL);
	q_BeforeHomeOffset *= DEG2RAD;
	q_AfterHomeOffset = q_BeforeHomeOffset + home;

	//Eigen::Matrix<float, 6, 6> jacobian = Jacobian_gen(q_AfterHomeOffset);
	//cout << ">>>> jacobian" << endl;
	//PrintMatrix_eigen(jacobian);

	Matrix2DoubleArray(q_BeforeHomeOffset, q_forward);
	cout << ">>>> Input q : " << endl;
	for (int i = 0; i < 6; ++i)
	{
		printf("%10.4lf ",q_forward[i]*RAD2DEG );
	}
	printf("\n");

	tm_kinematics::forward(q_forward, T);

	int num_sol = tm_kinematics::inverse(T, q_inv, q_forward);

	cout << ">>>> forward T" << endl;
	PrintMatrix_std(T);

	cout << ">>>> inverse q number of sols : " << num_sol << endl;
	for (int j = 0; j < num_sol; ++j)
	{
		for(int i = 0; i < 6; i++)
			printf("%10.4lf ",q_inv[ i + j*6 ]*RAD2DEG);
		printf("\n");
	}

	return 0;
}
<commit_msg>already copy to main.cpp<commit_after><|endoftext|>


<commit_before>// Copyright (c) 2011-2013 Zeex
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 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.

#include <cassert>
#include <cstdarg>
#include <cstdlib>
#include <list>
#include <map>
#include <stack>
#include <string>

#include <amx/amx.h>
#include <amx/amxaux.h>

#include "amxdebuginfo.h"
#include "amxerror.h"
#include "amxpathfinder.h"
#include "amxscript.h"
#include "amxstacktrace.h"
#include "compiler.h"
#include "configreader.h"
#include "crashdetect.h"
#include "fileutils.h"
#include "logprintf.h"
#include "npcall.h"
#include "os.h"
#include "stacktrace.h"

#define AMX_EXEC_GDK (-10)

bool CrashDetect::error_detected_ = false;
std::stack<NPCall*> CrashDetect::np_calls_;;

// static
void CrashDetect::OnException(void *context) {
	if (!np_calls_.empty()) {
		CrashDetect::Get(np_calls_.top()->amx())->HandleException();
	} else {
		logprintf("Server crashed due to an unknown error");
	}
	PrintSystemBacktrace(context);
}

// static
void CrashDetect::OnInterrupt(void *context) {
	if (!np_calls_.empty()) {
		CrashDetect::Get(np_calls_.top()->amx())->HandleInterrupt();
	} else {
		logprintf("Server received interrupt signal");
	}
	PrintSystemBacktrace(context);
}

// static
void CrashDetect::DieOrContinue() {
	if (server_cfg_.GetOption("die_on_error", false)) {
		logprintf("Aborting...");
		std::exit(EXIT_FAILURE);
	}
}

CrashDetect::CrashDetect(AMX *amx)
	: AMXService<CrashDetect>(amx)
	, amx_(amx)
	, prev_callback_(0)
	, server_cfg_("server.cfg")
{
}

int CrashDetect::Load() {
	AMXPathFinder pathFinder;
	pathFinder.AddSearchPath("gamemodes");
	pathFinder.AddSearchPath("filterscripts");

	// Read a list of additional search paths from AMX_PATH.
	const char *AMX_PATH = getenv("AMX_PATH");
	if (AMX_PATH != 0) {
		std::string var(AMX_PATH);
		std::string path;
		std::string::size_type begin = 0;
		while (begin < var.length()) {
			std::string::size_type end = var.find(fileutils::kNativePathListSepChar, begin);
			if (end == std::string::npos) {
				end = var.length();
			}
			path.assign(var.begin() + begin, var.begin() + end);
			if (!path.empty()) {
				pathFinder.AddSearchPath(path);
			}
			begin = end + 1;
		}
	}

	amx_path_ = pathFinder.FindAMX(this->amx());
	amx_name_ = fileutils::GetFileName(amx_path_);

	if (!amx_path_.empty() && AMXDebugInfo::IsPresent(this->amx())) {
		debug_info_.Load(amx_path_);
	}

	amx_.DisableSysreqD();
	prev_callback_ = amx_.GetCallback();

	return AMX_ERR_NONE;
}

int CrashDetect::Unload() {
	return AMX_ERR_NONE;
}

int CrashDetect::DoAmxCallback(cell index, cell *result, cell *params) {
	NPCall call = NPCall::Native(amx(), index);
	np_calls_.push(&call);
	int error = prev_callback_(amx(), index, result, params);
	np_calls_.pop();
	return error;
}

int CrashDetect::DoAmxExec(cell *retval, int index) {
	NPCall call = NPCall::Public(amx(), index);
	np_calls_.push(&call);

	int error = ::amx_Exec(amx(), retval, index);
	if (error != AMX_ERR_NONE && !error_detected_) {
		HandleExecError(index, error);
	} else {
		error_detected_ = false;
	}

	np_calls_.pop();
	return error;
}

void CrashDetect::HandleExecError(int index, const AMXError &error) {
	CrashDetect::error_detected_ = true;

	if (error.code() == AMX_ERR_INDEX && index == AMX_EXEC_GDK) {
		return;
	}

	logprintf("Run time error %d: \"%s\"", error, error.string());

	switch (error.code()) {
		case AMX_ERR_BOUNDS: {
			static const int bounds = 121;
			cell *ip = reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip());
			cell opcode = *ip;
			if (opcode == bounds) {
				cell bound = *(ip + 1);
				cell index = amx_.GetPri();
				if (index < 0) {
					logprintf(" Accessing element at negative index %d", index);
				} else {
					logprintf(" Accessing element at index %d past array upper bound %d", index, bound);
				}
			}
			break;
		}
		case AMX_ERR_NOTFOUND: {
			AMX_FUNCSTUBNT *natives = amx_.GetNatives();
			int num_natives = amx_.GetNumNatives();
			for (int i = 0; i < num_natives; ++i) {
				if (natives[i].address == 0) {
					logprintf(" %s", amx_.GetName(natives[i].nameofs));
				}
			}
			break;
		}
		case AMX_ERR_STACKERR:
			logprintf(" Stack pointer (STK) is 0x%X, heap pointer (HEA) is 0x%X", amx_.GetStk(), amx_.GetHea());
			break;
		case AMX_ERR_STACKLOW:
			logprintf(" Stack pointer (STK) is 0x%X, stack top (STP) is 0x%X", amx_.GetStk(), amx_.GetStp());
			break;
		case AMX_ERR_HEAPLOW:
			logprintf(" Heap pointer (HEA) is 0x%X, heap bottom (HLW) is 0x%X", amx_.GetHea(), amx_.GetHlw());
			break;
		case AMX_ERR_INVINSTR: {
			cell opcode = *(reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip()));
			logprintf(" Unknown opcode 0x%x at address 0x%08X", opcode , amx_.GetCip());
			break;
		}
		case AMX_ERR_NATIVE: {
			static const int op_sysreq_c = 123;
			cell *ip = reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip());
			cell opcode = *(ip - 2);
			if (opcode == op_sysreq_c) {
				cell index = *(ip - 1);
				logprintf(" %s", amx_.GetNativeName(index));
			}
			break;
		}
	}

	if (error.code() != AMX_ERR_NOTFOUND &&
		error.code() != AMX_ERR_INDEX &&
		error.code() != AMX_ERR_CALLBACK &&
		error.code() != AMX_ERR_INIT)
	{
		PrintAmxBacktrace();
	}

	std::string command = server_cfg_.GetOption("run_on_error", std::string());
	if (!command.empty()) {
		std::system(command.c_str());
	}

	DieOrContinue();
}

void CrashDetect::HandleException() {
	logprintf("Server crashed while executing %s", amx_name_.c_str());
	PrintAmxBacktrace();
}

void CrashDetect::HandleInterrupt() {
	logprintf("Server received interrupt signal while executing %s", amx_name_.c_str());
	PrintAmxBacktrace();
}

// static
void CrashDetect::PrintAmxBacktrace() {
	if (np_calls_.empty()) {
		return;
	}

	AMXScript topAmx = np_calls_.top()->amx();

	cell frm = topAmx.GetFrm();
	cell cip = topAmx.GetCip();
	int level = 0;

	if (cip == 0) {
		return;
	}

	logprintf("AMX backtrace:");

	std::stack<NPCall*> np_calls = np_calls_;

	while (!np_calls.empty() && cip != 0) {
		const NPCall *call = np_calls.top();
		AMXScript amx = call->amx();

		// We don't trace calls across AMX bounds i.e. outside of top-level
		// function's AMX instance.
		if (amx != topAmx) {
			assert(level != 0);
			break;
		}

		if (call->IsNative()) {
			AMX_NATIVE address = reinterpret_cast<AMX_NATIVE>(amx.GetNativeAddr(call->index()));
			if (address != 0) {
				std::string module = fileutils::GetFileName(os::GetModulePathFromAddr((void*)address));
				std::string from = " from " + module;
				if (module.empty()) {
					from.clear();
				}
				const char *name = amx.GetNativeName(call->index());
				if (name != 0) {
					logprintf("#%d native %s () [%08x]%s", level++, name, address, from.c_str());
				}
			}
		} else if (call->IsPublic()) {
			AMXDebugInfo &debug_info = CrashDetect::Get(amx)->debug_info_;

			amx.PushStack(cip); // push return address
			amx.PushStack(frm); // push frame pointer

			std::list<AMXStackFrame> frames;
			{
				AMXStackTrace trace(amx, amx.GetStk(), &debug_info);
				do {
					AMXStackFrame frame = trace.GetFrame();
					if (frame) {
						frames.push_back(frame);
					} else {
						break;
					}
				} while (trace.Next());
			}

			frm = amx.PopStack(); // pop frame pointer
			cip = amx.PopStack(); // pop return address

			if (frames.empty()) {
				ucell ep_addr = amx.GetPublicAddr(call->index());
				frames.push_front(AMXStackFrame(amx, frm, 0, ep_addr, &debug_info));
			} else {
				if (!debug_info.IsLoaded()) {
					AMXStackFrame &bottom = frames.back();
					bottom = AMXStackFrame(amx,
						bottom.GetFrameAddr(),
						bottom.GetRetAddr(),
						amx.GetPublicAddr(call->index()),
						&debug_info);
				}
			}

			std::string &amx_name = CrashDetect::Get(amx)->amx_name_;
			for (std::list<AMXStackFrame>::const_iterator iterator = frames.begin();
					iterator != frames.end(); ++iterator)
			{
				std::string from = " from " + amx_name;
				if (amx_name.empty() || debug_info.IsLoaded()) {
					from.clear();
				}
				logprintf("#%d %s%s", level++, iterator->AsString().c_str(), from.c_str());
			}

			frm = call->frm();
			cip = call->cip();
		}

		np_calls.pop();
	}
}

// static
void CrashDetect::PrintSystemBacktrace(void *context) {
	logprintf("System backtrace:");

	int level = 0;

	StackTrace trace(context);
	std::deque<StackFrame> frames = trace.GetFrames();

	for (std::deque<StackFrame>::const_iterator iterator = frames.begin();
			iterator != frames.end(); ++iterator) {
		const StackFrame &frame = *iterator;

		std::string module = os::GetModulePathFromAddr(frame.GetRetAddr());
		std::string from = " from " + module;
		if (module.empty()) {
			from.clear();
		}

		logprintf("#%d %s%s", level++, frame.AsString().c_str(), from.c_str());
	}
}

// static
void CrashDetect::logprintf(const char *format, ...) {
	std::va_list va;
	va_start(va, format);

	std::string new_format;
	new_format.append("[debug] ");
	new_format.append(format);

	vlogprintf(new_format.c_str(), va);
	va_end(va);
}
<commit_msg>Move opcode constants to the top<commit_after>// Copyright (c) 2011-2013 Zeex
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 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.

#include <cassert>
#include <cstdarg>
#include <cstdlib>
#include <list>
#include <map>
#include <stack>
#include <string>

#include <amx/amx.h>
#include <amx/amxaux.h>

#include "amxdebuginfo.h"
#include "amxerror.h"
#include "amxpathfinder.h"
#include "amxscript.h"
#include "amxstacktrace.h"
#include "compiler.h"
#include "configreader.h"
#include "crashdetect.h"
#include "fileutils.h"
#include "logprintf.h"
#include "npcall.h"
#include "os.h"
#include "stacktrace.h"

#define AMX_EXEC_GDK (-10)

static const int kOpBounds  = 121;
static const int kOpSysreqC = 123;

bool CrashDetect::error_detected_ = false;
std::stack<NPCall*> CrashDetect::np_calls_;;

// static
void CrashDetect::OnException(void *context) {
	if (!np_calls_.empty()) {
		CrashDetect::Get(np_calls_.top()->amx())->HandleException();
	} else {
		logprintf("Server crashed due to an unknown error");
	}
	PrintSystemBacktrace(context);
}

// static
void CrashDetect::OnInterrupt(void *context) {
	if (!np_calls_.empty()) {
		CrashDetect::Get(np_calls_.top()->amx())->HandleInterrupt();
	} else {
		logprintf("Server received interrupt signal");
	}
	PrintSystemBacktrace(context);
}

// static
void CrashDetect::DieOrContinue() {
	if (server_cfg_.GetOption("die_on_error", false)) {
		logprintf("Aborting...");
		std::exit(EXIT_FAILURE);
	}
}

CrashDetect::CrashDetect(AMX *amx)
	: AMXService<CrashDetect>(amx)
	, amx_(amx)
	, prev_callback_(0)
	, server_cfg_("server.cfg")
{
}

int CrashDetect::Load() {
	AMXPathFinder pathFinder;
	pathFinder.AddSearchPath("gamemodes");
	pathFinder.AddSearchPath("filterscripts");

	// Read a list of additional search paths from AMX_PATH.
	const char *AMX_PATH = getenv("AMX_PATH");
	if (AMX_PATH != 0) {
		std::string var(AMX_PATH);
		std::string path;
		std::string::size_type begin = 0;
		while (begin < var.length()) {
			std::string::size_type end = var.find(fileutils::kNativePathListSepChar, begin);
			if (end == std::string::npos) {
				end = var.length();
			}
			path.assign(var.begin() + begin, var.begin() + end);
			if (!path.empty()) {
				pathFinder.AddSearchPath(path);
			}
			begin = end + 1;
		}
	}

	amx_path_ = pathFinder.FindAMX(this->amx());
	amx_name_ = fileutils::GetFileName(amx_path_);

	if (!amx_path_.empty() && AMXDebugInfo::IsPresent(this->amx())) {
		debug_info_.Load(amx_path_);
	}

	amx_.DisableSysreqD();
	prev_callback_ = amx_.GetCallback();

	return AMX_ERR_NONE;
}

int CrashDetect::Unload() {
	return AMX_ERR_NONE;
}

int CrashDetect::DoAmxCallback(cell index, cell *result, cell *params) {
	NPCall call = NPCall::Native(amx(), index);
	np_calls_.push(&call);
	int error = prev_callback_(amx(), index, result, params);
	np_calls_.pop();
	return error;
}

int CrashDetect::DoAmxExec(cell *retval, int index) {
	NPCall call = NPCall::Public(amx(), index);
	np_calls_.push(&call);

	int error = ::amx_Exec(amx(), retval, index);
	if (error != AMX_ERR_NONE && !error_detected_) {
		HandleExecError(index, error);
	} else {
		error_detected_ = false;
	}

	np_calls_.pop();
	return error;
}

void CrashDetect::HandleExecError(int index, const AMXError &error) {
	CrashDetect::error_detected_ = true;

	if (error.code() == AMX_ERR_INDEX && index == AMX_EXEC_GDK) {
		return;
	}

	logprintf("Run time error %d: \"%s\"", error, error.string());

	switch (error.code()) {
		case AMX_ERR_BOUNDS: {
			cell *ip = reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip());
			cell opcode = *ip;
			if (opcode == kOpBounds) {
				cell bound = *(ip + 1);
				cell index = amx_.GetPri();
				if (index < 0) {
					logprintf(" Accessing element at negative index %d", index);
				} else {
					logprintf(" Accessing element at index %d past array upper bound %d", index, bound);
				}
			}
			break;
		}
		case AMX_ERR_NOTFOUND: {
			AMX_FUNCSTUBNT *natives = amx_.GetNatives();
			int num_natives = amx_.GetNumNatives();
			for (int i = 0; i < num_natives; ++i) {
				if (natives[i].address == 0) {
					logprintf(" %s", amx_.GetName(natives[i].nameofs));
				}
			}
			break;
		}
		case AMX_ERR_STACKERR:
			logprintf(" Stack pointer (STK) is 0x%X, heap pointer (HEA) is 0x%X", amx_.GetStk(), amx_.GetHea());
			break;
		case AMX_ERR_STACKLOW:
			logprintf(" Stack pointer (STK) is 0x%X, stack top (STP) is 0x%X", amx_.GetStk(), amx_.GetStp());
			break;
		case AMX_ERR_HEAPLOW:
			logprintf(" Heap pointer (HEA) is 0x%X, heap bottom (HLW) is 0x%X", amx_.GetHea(), amx_.GetHlw());
			break;
		case AMX_ERR_INVINSTR: {
			cell opcode = *(reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip()));
			logprintf(" Unknown opcode 0x%x at address 0x%08X", opcode , amx_.GetCip());
			break;
		}
		case AMX_ERR_NATIVE: {
			cell *ip = reinterpret_cast<cell*>(amx_.GetCode() + amx_.GetCip());
			cell opcode = *(ip - 2);
			if (opcode == kOpSysreqC) {
				cell index = *(ip - 1);
				logprintf(" %s", amx_.GetNativeName(index));
			}
			break;
		}
	}

	if (error.code() != AMX_ERR_NOTFOUND &&
		error.code() != AMX_ERR_INDEX &&
		error.code() != AMX_ERR_CALLBACK &&
		error.code() != AMX_ERR_INIT)
	{
		PrintAmxBacktrace();
	}

	std::string command = server_cfg_.GetOption("run_on_error", std::string());
	if (!command.empty()) {
		std::system(command.c_str());
	}

	DieOrContinue();
}

void CrashDetect::HandleException() {
	logprintf("Server crashed while executing %s", amx_name_.c_str());
	PrintAmxBacktrace();
}

void CrashDetect::HandleInterrupt() {
	logprintf("Server received interrupt signal while executing %s", amx_name_.c_str());
	PrintAmxBacktrace();
}

// static
void CrashDetect::PrintAmxBacktrace() {
	if (np_calls_.empty()) {
		return;
	}

	AMXScript topAmx = np_calls_.top()->amx();

	cell frm = topAmx.GetFrm();
	cell cip = topAmx.GetCip();
	int level = 0;

	if (cip == 0) {
		return;
	}

	logprintf("AMX backtrace:");

	std::stack<NPCall*> np_calls = np_calls_;

	while (!np_calls.empty() && cip != 0) {
		const NPCall *call = np_calls.top();
		AMXScript amx = call->amx();

		// We don't trace calls across AMX bounds i.e. outside of top-level
		// function's AMX instance.
		if (amx != topAmx) {
			assert(level != 0);
			break;
		}

		if (call->IsNative()) {
			AMX_NATIVE address = reinterpret_cast<AMX_NATIVE>(amx.GetNativeAddr(call->index()));
			if (address != 0) {
				std::string module = fileutils::GetFileName(os::GetModulePathFromAddr((void*)address));
				std::string from = " from " + module;
				if (module.empty()) {
					from.clear();
				}
				const char *name = amx.GetNativeName(call->index());
				if (name != 0) {
					logprintf("#%d native %s () [%08x]%s", level++, name, address, from.c_str());
				}
			}
		} else if (call->IsPublic()) {
			AMXDebugInfo &debug_info = CrashDetect::Get(amx)->debug_info_;

			amx.PushStack(cip); // push return address
			amx.PushStack(frm); // push frame pointer

			std::list<AMXStackFrame> frames;
			{
				AMXStackTrace trace(amx, amx.GetStk(), &debug_info);
				do {
					AMXStackFrame frame = trace.GetFrame();
					if (frame) {
						frames.push_back(frame);
					} else {
						break;
					}
				} while (trace.Next());
			}

			frm = amx.PopStack(); // pop frame pointer
			cip = amx.PopStack(); // pop return address

			if (frames.empty()) {
				ucell ep_addr = amx.GetPublicAddr(call->index());
				frames.push_front(AMXStackFrame(amx, frm, 0, ep_addr, &debug_info));
			} else {
				if (!debug_info.IsLoaded()) {
					AMXStackFrame &bottom = frames.back();
					bottom = AMXStackFrame(amx,
						bottom.GetFrameAddr(),
						bottom.GetRetAddr(),
						amx.GetPublicAddr(call->index()),
						&debug_info);
				}
			}

			std::string &amx_name = CrashDetect::Get(amx)->amx_name_;
			for (std::list<AMXStackFrame>::const_iterator iterator = frames.begin();
					iterator != frames.end(); ++iterator)
			{
				std::string from = " from " + amx_name;
				if (amx_name.empty() || debug_info.IsLoaded()) {
					from.clear();
				}
				logprintf("#%d %s%s", level++, iterator->AsString().c_str(), from.c_str());
			}

			frm = call->frm();
			cip = call->cip();
		}

		np_calls.pop();
	}
}

// static
void CrashDetect::PrintSystemBacktrace(void *context) {
	logprintf("System backtrace:");

	int level = 0;

	StackTrace trace(context);
	std::deque<StackFrame> frames = trace.GetFrames();

	for (std::deque<StackFrame>::const_iterator iterator = frames.begin();
			iterator != frames.end(); ++iterator) {
		const StackFrame &frame = *iterator;

		std::string module = os::GetModulePathFromAddr(frame.GetRetAddr());
		std::string from = " from " + module;
		if (module.empty()) {
			from.clear();
		}

		logprintf("#%d %s%s", level++, frame.AsString().c_str(), from.c_str());
	}
}

// static
void CrashDetect::logprintf(const char *format, ...) {
	std::va_list va;
	va_start(va, format);

	std::string new_format;
	new_format.append("[debug] ");
	new_format.append(format);

	vlogprintf(new_format.c_str(), va);
	va_end(va);
}
<|endoftext|>


<commit_before>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <cstdio>
#include <stdexcept>
#include <kernel/syscalls.hpp>

/**
 * This header is for instantiating and implementing
 * missing functionality gluing libc++ to the kernel
 *
 **/

extern "C"
{
  /// Linux standard base (locale)
  size_t __ctype_get_mb_cur_max(void)
  {
    return (size_t) 2; // ???
  }
  size_t __mbrlen (const char*, size_t, mbstate_t*)
  {
    printf("WARNING: mbrlen was called - which we don't currently support - returning bogus value\n");
    return (size_t) 1;
  }

  using nl_catd = int;

  nl_catd catopen (const char* name, int flag)
  {
    printf("catopen: %s, flag=0x%x\n", name, flag);
    return (nl_catd) -1;
  }
  //char* catgets (nl_catd catalog_desc, int set, int message, const char *string)
  char* catgets (nl_catd, int, int, const char*)
  {
    return nullptr;
  }
  //int catclose (nl_catd catalog_desc)
  int catclose (nl_catd)
  {
    return (nl_catd) 0;
  }

  char _IO_getc()
  {
    /// NOTE: IMPLEMENT ME
    printf("_IO_getc(): returning bogus character as input from stdin\n");
    return 'x';
  }

  using ub_error = std::runtime_error;
  void undefined_throw(const char*) {

  }

  /// Undefined-behavior sanitizer
  void __ubsan_handle_out_of_bounds()
  {
    undefined_throw("Out-of-bounds access");
  }
  void __ubsan_handle_missing_return()
  {
    undefined_throw("Missing return");
  }

  void __ubsan_handle_add_overflow()
  {
    undefined_throw("Overflow on addition");
  }
  void __ubsan_handle_sub_overflow()
  {
    undefined_throw("Overflow on subtraction");
  }
  void __ubsan_handle_mul_overflow()
  {
    undefined_throw("Overflow on multiplication");
  }
  void __ubsan_handle_negate_overflow()
  {
    undefined_throw("Overflow on negation");
  }
  void __ubsan_handle_pointer_overflow()
  {
    undefined_throw("Pointer overflow");
  }
  void __ubsan_handle_divrem_overflow()
  {
    undefined_throw("Division remainder overflow");
  }
  void __ubsan_handle_float_cast_overflow()
  {
    undefined_throw("Float-cast overflow");
  }
  void __ubsan_handle_shift_out_of_bounds()
  {
    undefined_throw("Shift out-of-bounds");
  }

  void __ubsan_handle_type_mismatch_v1()
  {
    undefined_throw("Type mismatch");
  }
  void __ubsan_handle_function_type_mismatch()
  {
    undefined_throw("Function type mismatch");
  }
  void __ubsan_handle_load_invalid_value()
  {
    undefined_throw("Load of invalid value");
  }
  void __ubsan_handle_builtin_unreachable()
  {
    panic("Unreachable code reached");
  }
}
<commit_msg>cxxabi: More complete ubsan output<commit_after>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <cstdio>
#include <stdexcept>
#include <kernel/syscalls.hpp>

/**
 * This header is for instantiating and implementing
 * missing functionality gluing libc++ to the kernel
 *
 **/

extern "C"
{
  /// Linux standard base (locale)
  size_t __ctype_get_mb_cur_max(void)
  {
    return (size_t) 2; // ???
  }
  size_t __mbrlen (const char*, size_t, mbstate_t*)
  {
    printf("WARNING: mbrlen was called - which we don't currently support - returning bogus value\n");
    return (size_t) 1;
  }

  using nl_catd = int;

  nl_catd catopen (const char* name, int flag)
  {
    printf("catopen: %s, flag=0x%x\n", name, flag);
    return (nl_catd) -1;
  }
  //char* catgets (nl_catd catalog_desc, int set, int message, const char *string)
  char* catgets (nl_catd, int, int, const char*)
  {
    return nullptr;
  }
  //int catclose (nl_catd catalog_desc)
  int catclose (nl_catd)
  {
    return (nl_catd) 0;
  }

  char _IO_getc()
  {
    /// NOTE: IMPLEMENT ME
    printf("_IO_getc(): returning bogus character as input from stdin\n");
    return 'x';
  }

  struct source_location {
  	const char *file_name;
		struct {
			uint32_t line;
			uint32_t column;
		};
  };
  struct out_of_bounds {
	   struct source_location src;
	   //struct type_descriptor *array_type;
	   //struct type_descriptor *index_type;
  };
  struct overflow {
    source_location src;
  };
  struct mismatch {
    source_location src;
  };
  struct nonnull_return {
    source_location src;
    source_location attr;
  };
  struct unreachable {
    source_location src;
  };
  using ub_error = std::runtime_error;
  void print_src_location(const source_location& src) {
    printf("ubsan: %s at line %u col %u\n",
            src.file_name, src.line, src.column);
  }
  void undefined_throw(const char* error) {
    printf("ubsan: %s", error);
    print_backtrace();
    printf("\n");
  }

  /// Undefined-behavior sanitizer
  void __ubsan_handle_out_of_bounds(struct out_of_bounds* data)
  {
    print_src_location(data->src);
    undefined_throw("Out-of-bounds access");
  }
  void __ubsan_handle_missing_return()
  {
    undefined_throw("Missing return");
  }
  void __ubsan_handle_nonnull_return(struct nonnull_return* data)
  {
    print_src_location(data->src);
    undefined_throw("Non-null return");
  }

  void __ubsan_handle_add_overflow()
  {
    undefined_throw("Overflow on addition");
  }
  void __ubsan_handle_sub_overflow()
  {
    undefined_throw("Overflow on subtraction");
  }
  void __ubsan_handle_mul_overflow()
  {
    undefined_throw("Overflow on multiplication");
  }
  void __ubsan_handle_negate_overflow()
  {
    undefined_throw("Overflow on negation");
  }
  void __ubsan_handle_pointer_overflow()
  {
    undefined_throw("Pointer overflow");
  }
  void __ubsan_handle_divrem_overflow(struct overflow* data,
                                      unsigned long lhs,
                                      unsigned long rhs)
  {
    print_src_location(data->src);
    printf("ubsan: LHS %lu / RHS %lu\n", lhs, rhs);
    undefined_throw("Division remainder overflow");
  }
  void __ubsan_handle_float_cast_overflow()
  {
    undefined_throw("Float-cast overflow");
  }
  void __ubsan_handle_shift_out_of_bounds()
  {
    undefined_throw("Shift out-of-bounds");
  }

  void __ubsan_handle_type_mismatch_v1(struct mismatch* data, unsigned long ptr)
  {
    print_src_location(data->src);
    char buffer[1024];
    snprintf(buffer, sizeof(buffer),
            "Type mismatch on ptr %p, %s",
            (void*) ptr,
            (ptr < 1024) ? "was nullptr deref" : "normal");
    undefined_throw(buffer);
  }
  void __ubsan_handle_function_type_mismatch()
  {
    undefined_throw("Function type mismatch");
  }
  void __ubsan_handle_invalid_builtin()
  {
    undefined_throw("Invalid built-in function");
  }
  void __ubsan_handle_load_invalid_value()
  {
    undefined_throw("Load of invalid value");
  }
  [[noreturn]]
  void __ubsan_handle_builtin_unreachable(struct unreachable* data)
  {
    print_src_location(data->src);
    panic("Unreachable code reached");
  }
}
<|endoftext|>


<commit_before>/**************************************************************************
 *
 * Copyright 2011 Jose Fonseca
 * All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 **************************************************************************/

/*
 * Trace writing functions.
 */

#ifndef _JSON_HPP_
#define _JSON_HPP_

#include <assert.h>
#include <stddef.h>
#include <wchar.h>

#include <ostream>
#include <iomanip>

class JSONWriter
{
private:
    std::ostream &os;

    int level;
    bool value;
    char space;

    void newline(void) {
        os << "\n";
        for (int i = 0; i < level; ++i)
            os << "  ";
    }

    void separator(void) {
        if (value) {
            os << ",";
            switch (space) {
            case '\0':
                break;
            case '\n':
                newline();
                break;
            default:
                os << space;
                break;
            }
        } else {
            if (space == '\n') {
                newline();
            }
        }
    }

    void escapeAsciiString(const char *str) {
        os << "\"";

        const unsigned char *src = (const unsigned char *)str;
        unsigned char c;
        while ((c = *src++)) {
            if ((c == '\"') ||
                (c == '\\')) {
                // escape character
                os << '\\' << (unsigned char)c;
            } else if ((c >= 0x20 && c <= 0x7e) ||
                        c == '\t' ||
                        c == '\r' ||
                        c == '\n') {
                // pass-through character
                os << (unsigned char)c;
            } else {
                assert(0);
                os << "?";
            }
        }

        os << "\"";
    }

    void escapeUnicodeString(const char *str) {
        os << "\"";

        const char *locale = setlocale(LC_CTYPE, "");
        const char *src = str;
        mbstate_t state;

        memset(&state, 0, sizeof state);

        do {
            // Convert characters one at a time in order to recover from
            // conversion errors
            wchar_t c;
            size_t written = mbsrtowcs(&c, &src, 1, &state);
            if (written == 0) {
                // completed
                break;
            } if (written == (size_t)-1) {
                // conversion error -- skip
                os << "?";
                do {
                    ++src;
                } while (*src & 0x80);
            } else if ((c == '\"') ||
                       (c == '\\')) {
                // escape character
                os << '\\' << (unsigned char)c;
            } else if ((c >= 0x20 && c <= 0x7e) ||
                        c == '\t' ||
                        c == '\r' ||
                        c == '\n') {
                // pass-through character
                os << (unsigned char)c;
            } else {
                // unicode
                os << "\\u" << std::setfill('0') << std::hex << std::setw(4) << (unsigned)c;
                os << std::dec;
            }
        } while (src);

        setlocale(LC_CTYPE, locale);

        os << "\"";
    }

    void encodeBase64String(const unsigned char *bytes, size_t size) {
        const char *table64 = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
        unsigned char c0, c1, c2, c3;
        char buf[4];
        unsigned written;

        os << "\"";

        written = 0;
        while (size >= 3) {
            c0 = bytes[0] >> 2;
            c1 = ((bytes[0] & 0x03) << 4) | ((bytes[1] & 0xf0) >> 4);
            c2 = ((bytes[1] & 0x0f) << 2) | ((bytes[2] & 0xc0) >> 6);
            c3 = bytes[2] & 0x3f;

            buf[0] = table64[c0];
            buf[1] = table64[c1];
            buf[2] = table64[c2];
            buf[3] = table64[c3];

            os.write(buf, 4);

            bytes += 3;
            size -= 3;
            ++written;

            if (written >= 76/4) {
                os << "\n";
                written = 0;
            }
        }

        if (size > 0) {
            c0 = bytes[0] >> 2;
            c1 = ((bytes[0] & 0x03) << 4);

            if (size > 1) {
                c1 |= ((bytes[1] & 0xf0) >> 4);
                c2 = ((bytes[1] & 0x0f) << 2);
                if (size > 2) {
                    c2 |= ((bytes[2] & 0xc0) >> 6);
                    c3 = bytes[2] & 0x3f;
                    buf[3] = table64[c3];
                } else {
                    buf[3] = '=';
                }
                buf[2] = table64[c2];
            } else {
                buf[3] = '=';
                buf[2] = '=';
            }
            buf[1] = table64[c1];
            buf[0] = table64[c0];

            os.write(buf, 4);
        }

        os << "\"";
    }

public:
    JSONWriter(std::ostream &_os) :
        os(_os),
        level(0),
        value(false),
        space(0)
    {
        beginObject();
    }

    ~JSONWriter() {
        endObject();
        newline();
    }

    inline void beginObject() {
        separator();
        os << "{";
        ++level;
        value = false;
    }

    inline void endObject() {
        --level;
        if (value)
            newline();
        os << "}";
        value = true;
        space = '\n';
    }

    inline void beginMember(const char * name) {
        space = 0;
        separator();
        newline();
        escapeAsciiString(name);
        os << ": ";
        value = false;
    }

    inline void endMember(void) {
        assert(value);
        value = true;
        space = 0;
    }

    inline void beginArray() {
        separator();
        os << "[";
        ++level;
        value = false;
        space = 0;
    }

    inline void endArray(void) {
        --level;
        if (space == '\n') {
            newline();
        }
        os << "]";
        value = true;
        space = '\n';
    }

    inline void writeString(const char *s) {
        separator();
        escapeUnicodeString(s);
        value = true;
        space = ' ';
    }

    inline void writeBase64(const void *bytes, size_t size) {
        separator();
        encodeBase64String((const unsigned char *)bytes, size);
        value = true;
        space = ' ';
    }

    inline void writeNull(void) {
        separator();
        os << "null";
        value = true;
        space = ' ';
    }

    inline void writeBool(bool b) {
        separator();
        os << (b ? "true" : "false");
        value = true;
        space = ' ';
    }

    template<class T>
    inline void writeNumber(T n) {
        separator();
        os << std::dec << n;
        value = true;
        space = ' ';
    }

    inline void writeStringMember(const char *name, const char *s) {
        beginMember(name);
        writeString(s);
        endMember();
    }

    inline void writeBoolMember(const char *name, bool b) {
        beginMember(name);
        writeBool(b);
        endMember();
    }

    template<class T>
    inline void writeNumberMember(const char *name, T n) {
        beginMember(name);
        writeNumber(n);
        endMember();
    }
};

#endif /* _JSON_HPP_ */
<commit_msg>Minor cleanups to base64 encoding.<commit_after>/**************************************************************************
 *
 * Copyright 2011 Jose Fonseca
 * All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 **************************************************************************/

/*
 * Trace writing functions.
 */

#ifndef _JSON_HPP_
#define _JSON_HPP_

#include <assert.h>
#include <stddef.h>
#include <wchar.h>

#include <ostream>
#include <iomanip>

class JSONWriter
{
private:
    std::ostream &os;

    int level;
    bool value;
    char space;

    void newline(void) {
        os << "\n";
        for (int i = 0; i < level; ++i)
            os << "  ";
    }

    void separator(void) {
        if (value) {
            os << ",";
            switch (space) {
            case '\0':
                break;
            case '\n':
                newline();
                break;
            default:
                os << space;
                break;
            }
        } else {
            if (space == '\n') {
                newline();
            }
        }
    }

    void escapeAsciiString(const char *str) {
        os << "\"";

        const unsigned char *src = (const unsigned char *)str;
        unsigned char c;
        while ((c = *src++)) {
            if ((c == '\"') ||
                (c == '\\')) {
                // escape character
                os << '\\' << (unsigned char)c;
            } else if ((c >= 0x20 && c <= 0x7e) ||
                        c == '\t' ||
                        c == '\r' ||
                        c == '\n') {
                // pass-through character
                os << (unsigned char)c;
            } else {
                assert(0);
                os << "?";
            }
        }

        os << "\"";
    }

    void escapeUnicodeString(const char *str) {
        os << "\"";

        const char *locale = setlocale(LC_CTYPE, "");
        const char *src = str;
        mbstate_t state;

        memset(&state, 0, sizeof state);

        do {
            // Convert characters one at a time in order to recover from
            // conversion errors
            wchar_t c;
            size_t written = mbsrtowcs(&c, &src, 1, &state);
            if (written == 0) {
                // completed
                break;
            } if (written == (size_t)-1) {
                // conversion error -- skip
                os << "?";
                do {
                    ++src;
                } while (*src & 0x80);
            } else if ((c == '\"') ||
                       (c == '\\')) {
                // escape character
                os << '\\' << (unsigned char)c;
            } else if ((c >= 0x20 && c <= 0x7e) ||
                        c == '\t' ||
                        c == '\r' ||
                        c == '\n') {
                // pass-through character
                os << (unsigned char)c;
            } else {
                // unicode
                os << "\\u" << std::setfill('0') << std::hex << std::setw(4) << (unsigned)c;
                os << std::dec;
            }
        } while (src);

        setlocale(LC_CTYPE, locale);

        os << "\"";
    }

    void encodeBase64String(const unsigned char *bytes, size_t size) {
        const char *table64 = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
        unsigned char c0, c1, c2, c3;
        char buf[4];
        unsigned written;

        os << "\"";

        written = 0;
        while (size >= 3) {
            c0 = bytes[0] >> 2;
            c1 = ((bytes[0] & 0x03) << 4) | ((bytes[1] & 0xf0) >> 4);
            c2 = ((bytes[1] & 0x0f) << 2) | ((bytes[2] & 0xc0) >> 6);
            c3 = bytes[2] & 0x3f;

            buf[0] = table64[c0];
            buf[1] = table64[c1];
            buf[2] = table64[c2];
            buf[3] = table64[c3];

            os.write(buf, 4);

            bytes += 3;
            size -= 3;
            ++written;

            if (written >= 76/4 && size) {
                os << "\n";
                written = 0;
            }
        }

        if (size > 0) {
            c0 = bytes[0] >> 2;
            c1 = ((bytes[0] & 0x03) << 4);
            buf[2] = '=';
            buf[3] = '=';

            if (size > 1) {
                c1 |= ((bytes[1] & 0xf0) >> 4);
                c2 = ((bytes[1] & 0x0f) << 2);
                if (size > 2) {
                    c2 |= ((bytes[2] & 0xc0) >> 6);
                    c3 = bytes[2] & 0x3f;
                    buf[3] = table64[c3];
                }
                buf[2] = table64[c2];
            }
            buf[1] = table64[c1];
            buf[0] = table64[c0];

            os.write(buf, 4);
        }

        os << "\"";
    }

public:
    JSONWriter(std::ostream &_os) :
        os(_os),
        level(0),
        value(false),
        space(0)
    {
        beginObject();
    }

    ~JSONWriter() {
        endObject();
        newline();
    }

    inline void beginObject() {
        separator();
        os << "{";
        ++level;
        value = false;
    }

    inline void endObject() {
        --level;
        if (value)
            newline();
        os << "}";
        value = true;
        space = '\n';
    }

    inline void beginMember(const char * name) {
        space = 0;
        separator();
        newline();
        escapeAsciiString(name);
        os << ": ";
        value = false;
    }

    inline void endMember(void) {
        assert(value);
        value = true;
        space = 0;
    }

    inline void beginArray() {
        separator();
        os << "[";
        ++level;
        value = false;
        space = 0;
    }

    inline void endArray(void) {
        --level;
        if (space == '\n') {
            newline();
        }
        os << "]";
        value = true;
        space = '\n';
    }

    inline void writeString(const char *s) {
        separator();
        escapeUnicodeString(s);
        value = true;
        space = ' ';
    }

    inline void writeBase64(const void *bytes, size_t size) {
        separator();
        encodeBase64String((const unsigned char *)bytes, size);
        value = true;
        space = ' ';
    }

    inline void writeNull(void) {
        separator();
        os << "null";
        value = true;
        space = ' ';
    }

    inline void writeBool(bool b) {
        separator();
        os << (b ? "true" : "false");
        value = true;
        space = ' ';
    }

    template<class T>
    inline void writeNumber(T n) {
        separator();
        os << std::dec << n;
        value = true;
        space = ' ';
    }

    inline void writeStringMember(const char *name, const char *s) {
        beginMember(name);
        writeString(s);
        endMember();
    }

    inline void writeBoolMember(const char *name, bool b) {
        beginMember(name);
        writeBool(b);
        endMember();
    }

    template<class T>
    inline void writeNumberMember(const char *name, T n) {
        beginMember(name);
        writeNumber(n);
        endMember();
    }
};

#endif /* _JSON_HPP_ */
<|endoftext|>