<commit_before>#include "Angle.h"

#include <cmath>
#include <cassert>
#include <sstream>

#include "Matrix.h"
#include "Quaternion.h"

#define M_PI 3.14159265358979323846

#define DEG2RAD(X) (X*(M_PI/180.0))
#define RAD2DEG(X) (X*(180.0/M_PI))

//void inline sincos( double radians, float *sine, float *cosine ) {
//	#if defined( _WIN32 )
//	_asm
//	{
//		fld		DWORD PTR [radians]
//		fsincos
//
//		mov edx, DWORD PTR [cosine]
//		mov eax, DWORD PTR [sine]
//
//		fstp DWORD PTR [edx]
//		fstp DWORD PTR [eax]
//	}
//#elif defined( _LINUX )
//	register double __cosr, __sinr;
// 	__asm __volatile__
//    		("fsincos"
//     	: "=t" (__cosr), "=u" (__sinr) : "0" (radians));
//
//  	*sine = __sinr;
//  	*cosine = __cosr;
//#endif
//}


Angle::Angle(const std::string& str) : Vertex(str)
{
}

Angle::Angle(const Matrix3d & mat)
{
	this->fromMatrix(mat);
}

double Angle::pitch() const
{
	return x();
}

double Angle::yaw() const
{
	return y();
}

double Angle::roll() const
{
	return z();
}

Vertex Angle::toVertex() const
{
	Vertex ret;
	double yawrad = vertex_[YAW] * (M_PI / 180.0);
	double rollrad = vertex_[ROLL] * (M_PI / 180.0);

	ret.x(cos(yawrad) * cos(rollrad));
	ret.y(sin(yawrad) * cos(rollrad));
	ret.z(sin(rollrad));
	return ret;
}

Matrix3d Angle::angleMatrix() const
{
	double sinpitch = sin(DEG2RAD((*this)[PITCH])),
		cospitch = cos(DEG2RAD((*this)[PITCH])),
		sinyaw = sin(DEG2RAD((*this)[YAW])),
		cosyaw = cos(DEG2RAD((*this)[YAW])),
		sinroll = sin(DEG2RAD((*this)[ROLL])),
		cosroll = cos(DEG2RAD((*this)[ROLL]));

	Matrix3d matrix;

	matrix[0][0] = cospitch * cosyaw;
	matrix[1][0] = cospitch * sinyaw;
	matrix[2][0] = -sinpitch;

	double crcy = cosroll * cosyaw;
	double crsy = cosroll * sinyaw;
	double srcy = sinroll * cosyaw;
	double srsy = sinroll * sinyaw;

	matrix[0][1] = sinpitch * srcy - crsy;
	matrix[1][1] = sinpitch * srsy + crcy;
	matrix[2][1] = sinroll * cospitch;

	matrix[0][2] = (sinpitch * crcy + srsy);
	matrix[1][2] = (sinpitch * crsy - srcy);
	matrix[2][2] = cosroll * cospitch;

	return matrix;
}

Matrix3d Angle::calcRotation(const Angle& target) const
{
	calcRotation(Quaternion{ target });
}

Matrix3d Angle::calcRotation(Quaternion target) const
{
	Quaternion curQuat(*this);
	curQuat.normalize();
	target.normalize();
	Quaternion rot = target * curQuat.conjugate();
	rot.normalize();
	return rot.quaternionMatrix();
}

void Angle::fromMatrix(const Matrix3d& matrix)
{
	double forward[3];
	double left[3];
	double up[3];

	forward[0] = matrix[0][0];
	forward[1] = matrix[1][0];
	forward[2] = matrix[2][0];
	left[0] = matrix[0][1];
	left[1] = matrix[1][1];
	left[2] = matrix[2][1];
	up[2] = matrix[2][2];

	double xyDist = sqrt(forward[0] * forward[0] + forward[1] * forward[1]);<commit_msg>Fixed calcRotation not returning a value.<commit_after>#include "Angle.h"

#include <cmath>
#include <cassert>
#include <sstream>

#include "Matrix.h"
#include "Quaternion.h"

#define M_PI 3.14159265358979323846

#define DEG2RAD(X) (X*(M_PI/180.0))
#define RAD2DEG(X) (X*(180.0/M_PI))

//void inline sincos( double radians, float *sine, float *cosine ) {
//	#if defined( _WIN32 )
//	_asm
//	{
//		fld		DWORD PTR [radians]
//		fsincos
//
//		mov edx, DWORD PTR [cosine]
//		mov eax, DWORD PTR [sine]
//
//		fstp DWORD PTR [edx]
//		fstp DWORD PTR [eax]
//	}
//#elif defined( _LINUX )
//	register double __cosr, __sinr;
// 	__asm __volatile__
//    		("fsincos"
//     	: "=t" (__cosr), "=u" (__sinr) : "0" (radians));
//
//  	*sine = __sinr;
//  	*cosine = __cosr;
//#endif
//}


Angle::Angle(const std::string& str) : Vertex(str)
{
}

Angle::Angle(const Matrix3d & mat)
{
	this->fromMatrix(mat);
}

double Angle::pitch() const
{
	return x();
}

double Angle::yaw() const
{
	return y();
}

double Angle::roll() const
{
	return z();
}

Vertex Angle::toVertex() const
{
	Vertex ret;
	double yawrad = vertex_[YAW] * (M_PI / 180.0);
	double rollrad = vertex_[ROLL] * (M_PI / 180.0);

	ret.x(cos(yawrad) * cos(rollrad));
	ret.y(sin(yawrad) * cos(rollrad));
	ret.z(sin(rollrad));
	return ret;
}

Matrix3d Angle::angleMatrix() const
{
	double sinpitch = sin(DEG2RAD((*this)[PITCH])),
		cospitch = cos(DEG2RAD((*this)[PITCH])),
		sinyaw = sin(DEG2RAD((*this)[YAW])),
		cosyaw = cos(DEG2RAD((*this)[YAW])),
		sinroll = sin(DEG2RAD((*this)[ROLL])),
		cosroll = cos(DEG2RAD((*this)[ROLL]));

	Matrix3d matrix;

	matrix[0][0] = cospitch * cosyaw;
	matrix[1][0] = cospitch * sinyaw;
	matrix[2][0] = -sinpitch;

	double crcy = cosroll * cosyaw;
	double crsy = cosroll * sinyaw;
	double srcy = sinroll * cosyaw;
	double srsy = sinroll * sinyaw;

	matrix[0][1] = sinpitch * srcy - crsy;
	matrix[1][1] = sinpitch * srsy + crcy;
	matrix[2][1] = sinroll * cospitch;

	matrix[0][2] = (sinpitch * crcy + srsy);
	matrix[1][2] = (sinpitch * crsy - srcy);
	matrix[2][2] = cosroll * cospitch;

	return matrix;
}

Matrix3d Angle::calcRotation(const Angle& target) const
{
	return calcRotation(Quaternion{ target });
}

Matrix3d Angle::calcRotation(Quaternion target) const
{
	Quaternion curQuat(*this);
	curQuat.normalize();
	target.normalize();
	Quaternion rot = target * curQuat.conjugate();
	rot.normalize();
	return rot.quaternionMatrix();
}

void Angle::fromMatrix(const Matrix3d& matrix)
{
	double forward[3];
	double left[3];
	double up[3];

	forward[0] = matrix[0][0];
	forward[1] = matrix[1][0];
	forward[2] = matrix[2][0];
	left[0] = matrix[0][1];
	left[1] = matrix[1][1];
	left[2] = matrix[2][1];
	up[2] = matrix[2][2];

	double xyDist = sqrt(forward[0] * forward[0] + forward[1] * forward[1]);
{'fraction_non_alphanumeric': 0.19543560949103422, 'fraction_numerical': 0.03622532149972831, 'mean_word_length': 4.269083969465649, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4789859', 'n_tokens_mistral': 2526, 'n_tokens_neox': 2399, 'n_words': 487}